concepts.simulator.pybullet.components.robot_base.Robot#
- class Robot[source]#
Bases:
BulletComponent
Methods
do
(action_name, *args, **kwargs)Execute an action primitive.
fk
(qpos[, link_name_or_id])Forward kinematics.
Get the home position of the end effector.
Get the home orientation of the end effector.
get_ee_pose
([fk])Get the pose of the end effector.
get_ee_velocity
([fk])Get the velocity of the end effector.
Get the gripper state.
Get the home joint configuration.
get_jacobian
([qpos, link_id])Get the Jacobian matrix.
get_qpos
()Get the current joint configuration.
Get the pybullet body ID of the robot.
Get the pybullet link ID of the robot end effector.
ik
(pos, quat[, force, max_distance, ...])Inverse kinematics.
ik_pybullet
(pos, quat[, force, verbose])Inverse kinematics using pybullet.
ikfast
(pos, quat[, last_qpos, max_attempts, ...])Inverse kinematics using IKFast.
move_home
([timeout])Move the robot to the home configuration.
move_pose
(pos, quat[, speed, force, verbose])Move the end effector to the given pose.
move_qpos
(target_qpos[, speed, timeout, ...])Move the robot to the given joint configuration.
move_qpos_trajectory
(qpos_trajectory[, ...])Move the robot along the given joint configuration trajectory.
move_qpos_trajectory_v2
(qpos_trajectory[, ...])A customized version of move_qpos_trajectory.
register_action
(name, func[, interface])Register an action primitive.
replay_qpos_trajectory
(qpos_trajectory[, ...])Replay a joint confifguartion trajectory.
Reset the home joint configuration.
rrt_collision_free
(pos1[, pos2, ...])RRT-based collision-free path planning.
set_ee_pose
(pos, quat)Set the pose of the end effector by inverse kinematics.
set_ik_method
(ik_method)set_qpos
(qpos)Set the joint configuration.
set_qpos_with_holding
(qpos)Set the joint configuration with the gripper holding the object.
set_suppress_warnings
([value])Attributes
- __init__(client, body_name=None, gripper_objects=None, current_interface='pybullet', ik_method='pybullet')[source]#
- __new__(**kwargs)#
- get_ee_pose(fk=True)[source]#
Get the pose of the end effector.
- Parameters:
fk (bool) – whether to run forward kinematics to re-compute the pose.
- Returns:
3D position and 4D quaternion of the end effector.
- Return type:
Tuple[np.ndarray, np.ndarray]
- get_ee_velocity(fk=True)[source]#
Get the velocity of the end effector.
- Returns:
3D linear velocity and 3D angular velocity of the end effector.
- Return type:
Tuple[np.ndarray, np.ndarray]
- Parameters:
fk (bool) –
- get_gripper_state()[source]#
Get the gripper state.
- Returns:
True if the gripper is activated.
- Return type:
Optional[bool]
- get_qpos()[source]#
Get the current joint configuration.
- Returns:
Current joint configuration.
- Return type:
np.ndarray
- get_robot_body_id()[source]#
Get the pybullet body ID of the robot.
- Returns:
Body ID of the robot.
- Return type:
- get_robot_ee_link_id()[source]#
Get the pybullet link ID of the robot end effector.
- Returns:
Link ID of the robot end effector.
- Return type:
- ik(pos, quat, force=False, max_distance=float('inf'), max_attempts=1000, verbose=False)[source]#
Inverse kinematics.
- Parameters:
pos (ndarray) – 3D position of the end effector.
quat (ndarray) – 4D quaternion of the end effector.
force (bool) – Whether to force the IK solver to return a solution. Defaults to False. If set, the IK solve may return a solution even if the end effector is not at the given pose. This function is useful for debugging and for moving towards a certain direction.
max_distance (float) – Maximum distance between the last qpos and the solution (Only used for IKFast). Defaults to float(‘inf’).
max_attempts (int) – Maximum number of attempts (only used for IKFast). Defaults to 1000.
verbose (bool) – Whether to print debug information.
- Returns:
Joint configuration.
- Return type:
np.ndarray
- ik_pybullet(pos, quat, force=False, verbose=False)[source]#
Inverse kinematics using pybullet.
- Parameters:
pos (ndarray) – 3D position of the end effector.
quat (ndarray) – 4D quaternion of the end effector.
force (bool) – Whether to force the IK solver to return a solution. Defaults to False. If set, the IK solve may return a solution even if the end effector is not at the given pose. This function is useful for debugging and for moving towards a certain direction.
verbose (bool) – Whether to print debug information.
- Returns:
Joint configuration.
- Return type:
np.ndarray
- ikfast(pos, quat, last_qpos=None, max_attempts=1000, max_distance=float('inf'), error_on_fail=True, verbose=False)[source]#
Inverse kinematics using IKFast.
- Parameters:
pos (ndarray) – 3D position of the end effector.
quat (ndarray) – 4D quaternion of the end effector.
last_qpos (ndarray | None) – Last joint configuration. Defaults to None. If None, the current joint configuration is used.
max_attempts (int) – Maximum number of IKFast attempts. Defaults to 1000.
max_distance (float) – Maximum distance between the target pose and the end effector. Defaults to float(‘inf’).
error_on_fail (bool) – Whether to raise an error if the IKFast solver fails. Defaults to True.
verbose (bool) – Whether to print debug information.
- Returns:
Joint configuration.
- Return type:
np.ndarray
- move_pose(pos, quat, speed=0.01, force=False, verbose=False)[source]#
Move the end effector to the given pose.
- Parameters:
pos – 3D position of the end effector.
quat – 4D quaternion of the end effector.
speed – Speed of the movement. Defaults to 0.01.
force (bool) – Whether to force the IK solver to return a solution. Defaults to False. If set, the IK solve may return a solution even if the end effector is not at the specified pose.
verbose (bool) – Whether to print debug information.
- Returns:
True if the movement is successful.
- Return type:
- move_qpos(target_qpos, speed=0.01, timeout=10.0, local_smoothing=True)[source]#
Move the robot to the given joint configuration.
- Parameters:
- Returns:
True if the movement is successful.
- Return type:
- move_qpos_trajectory(qpos_trajectory, speed=0.01, timeout=10, first_timeout=None, local_smoothing=True, verbose=False)[source]#
Move the robot along the given joint configuration trajectory.
- Parameters:
qpos_trajectory (Iterable[ndarray]) – Joint configuration trajectory.
speed (float) – Speed of the movement. Defaults to 0.01.
timeout (float) – Timeout of the movement. Defaults to 10.
first_timeout (float | None) – Timeout of the first movement. Defaults to None (same as timeout).
local_smoothing (bool) – Whether to use local smoothing. Defaults to True.
verbose (bool) – Whether to print debug information.
- Returns:
True if the movement is successful.
- Return type:
- move_qpos_trajectory_v2(qpos_trajectory, timeout=20)[source]#
A customized version of move_qpos_trajectory. It allows the user to specify path tracking mechanisms.
- register_action(name, func, interface=None)[source]#
Register an action primitive.
- Parameters:
- Raises:
ValueError – If the action primitive is already registered.
- Return type:
None
- replay_qpos_trajectory(qpos_trajectory, verbose=True)[source]#
Replay a joint confifguartion trajectory. This function is useful for debugging a generated joint trajectory.
- rrt_collision_free(pos1, pos2=None, smooth_fine_path=False, disable_renderer=True, **kwargs)[source]#
RRT-based collision-free path planning.
- Parameters:
- Returns:
True if the path is collision-free. List[np.ndarray]: Joint configuration trajectory.
- Return type:
- set_ee_pose(pos, quat)[source]#
Set the pose of the end effector by inverse kinematics. Return True if the IK solver succeeds.
- set_qpos(qpos)[source]#
Set the joint configuration.
- Parameters:
qpos (ndarray) – Joint configuration.
- Return type:
None
- set_qpos_with_holding(qpos)[source]#
Set the joint configuration with the gripper holding the object.
- Parameters:
qpos (ndarray) – Joint configuration.
- Return type:
None
- property p#
- property w#
- property world#