concepts.simulator.pybullet.components.robot_base.BulletArmRobotBase

class BulletArmRobotBase

Bases: BulletRobotBase

Base class for arm robots, such as UR5, Panda, etc.

In this class, we assume that the robot joints are composed of two parts: the arm joints and the gripper joints. All joint IDs and joint configurations functions are based on arm joints. To handle the “full” joint configuration, we provide an additional set of functions to get and set the full joint configuration, including the gripper joints.

Methods

attach_object(object_id, ee_to_object)
create_gripper_constraint(object_id[, verbose])
detach_object()
do(action_name, *args, **kwargs)	Execute an action primitive.
fk(qpos[, link_name_or_id])	Forward kinematics.
get_attached_object()
get_attached_object_pose_in_ee_frame()
get_body_id()	Get the pybullet body ID of the robot.
get_body_pose()	Get the pose of the robot body.
get_collision_free_problem_space([...])
get_configuration_space()
get_coriolis_torque([qpos, qvel])	Get the Coriolis torque.
get_dof()	Get the total degrees of freedom of the robot.
get_ee_default_quat()	Get the default orientation of the end effector.
get_ee_home_pos()	Get the home position of the end effector.
get_ee_home_quat()	Get the home orientation of the end effector.
get_ee_link_id()	Get the pybullet link ID of the robot end effector.
get_ee_pose([fk])	Get the pose of the end effector.
get_ee_quat_from_vectors([u, v])	Compute the quaternion from two directions (the "down" direction for the end effector and the "forward" direction for the end effector).
get_ee_to_tool(tool_id)	Get the pose of the tool frame relative to the end effector frame.
get_ee_velocity([fk])	Get the velocity of the end effector.
get_full_dof()	Get the total degrees of freedom of the robot, including the gripper joints.
get_full_home_qpos()	Get the home joint configuration.
get_full_joint_ids()	Get the pybullet joint IDs of the robot, including the gripper joints.
get_full_joint_limits()	Get the joint limits of the robot arm, including the gripper joints.
get_full_qpos()	Get the current joint configuration.
get_full_qvel()	Get the current joint velocity.
get_gripper_state()	Get the gripper state.
get_home_qpos()	Get the home joint configuration.
get_jacobian([qpos, link_id])	Get the Jacobian matrix.
get_joint_ids()	Get the pybullet joint IDs of the robot.
get_joint_limits()	Get the joint limits.
get_mass_matrix([qpos])	Get the mass matrix.
get_qpos()	Get the current joint configuration.
get_qvel()	Get the current joint velocity.
ik(pos, quat[, force, max_distance, ...])	Inverse kinematics.
ik_pybullet(pos, quat[, force, pos_tol, ...])	Inverse kinematics using pybullet.
ikfast(pos, quat[, last_qpos, max_attempts, ...])	Inverse kinematics using IKFast.
is_colliding([q, return_contacts, ...])	Check if the robot is colliding with other objects.
is_colliding_with_saved_state([q, ...])
move_home([timeout])	Move the robot to the home configuration.
move_home_cfree([speed, timeout])
move_pose(pos, quat[, speed, force, verbose])	Move the end effector to the given pose.
move_pose_smooth(pos, quat[, speed, ...])
move_qpos(target_qpos[, speed, timeout, ...])	Move the robot to the given joint configuration.
move_qpos_path(qpos_trajectory[, speed, ...])	Move the robot along the given joint configuration trajectory.
move_qpos_path_v2(qpos_trajectory[, timeout])	A customized version of move_qpos_trajectory.
move_qpos_path_v2_set_control(qpos_trajectory)
register_action(name, func[, interface])	Register an action primitive.
replay_qpos_trajectory(qpos_trajectory[, ...])	Replay a joint confifguartion trajectory.
reset_home_qpos()	Reset the home joint configuration.
rrt_collision_free(qpos1[, qpos2, ...])	RRT-based collision-free path planning.
set_arm_joint_position_control(target_qpos)	Set the arm joint position control.
set_ee_impedance_control(target_pos, target_quat)	Set the end-effector impedance control command.
set_ee_pose(pos, quat)	Set the pose of the end effector by inverse kinematics.
set_full_hold_position_control([qpos, gains])	Set the control parameter for holding the current arm joint positions, while the gripper holding the object.
set_full_qpos(qpos)	Set the joint configuration.
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])
suppress_warnings()

Attributes

p
w
world

__init__(client, body_name=None, gripper_objects=None, current_interface='pybullet', ik_method='pybullet', use_magic_gripper=True)

Parameters:

• client (BulletClient)

• body_name (str | None)

• gripper_objects (Dict[str, List[int]] | None)

• ik_method (str | IKMethod)

• use_magic_gripper (bool)

__new__(**kwargs)

attach_object(object_id, ee_to_object)

Parameters:

• object_id (int)

• ee_to_object (Tuple[ndarray, ndarray])

Return type:

None

create_gripper_constraint(object_id, verbose=False)

Parameters:

verbose (bool)

detach_object()

Return type:

None

do(action_name, *args, **kwargs)

Execute an action primitive.

Parameters:

action_name (str) – Name of the action primitive.

Returns:

True if the action primitive is successful.

Return type:

bool

fk(qpos, link_name_or_id=None)

Forward kinematics.

Parameters:

• qpos (ndarray) – Joint configuration.

• link_name_or_id (int | str | None) – name or id of the link. If not specified, the pose of the end effector is returned.

Returns:

3D position and 4D quaternion of the end effector.

Return type:

Tuple[np.ndarray, np.ndarray]

get_attached_object()

Return type:

int | None

get_attached_object_pose_in_ee_frame()

Return type:

Tuple[ndarray, ndarray] | None

get_body_id()

Get the pybullet body ID of the robot.

Returns:

Body ID of the robot.

Return type:

int

get_body_pose()

Get the pose of the robot body.

Returns:

3D position and 4D quaternion of the robot body.

Return type:

Tuple[np.ndarray, np.ndarray]

get_collision_free_problem_space(ignored_collision_bodies=None)

Parameters:

ignored_collision_bodies (Sequence[int] | None)

Return type:

CollisionFreeProblemSpace

get_configuration_space()

Return type:

BoxConfigurationSpace

get_coriolis_torque(qpos=None, qvel=None)

Get the Coriolis torque.

Parameters:

• qpos (ndarray | None) – Joint configuration.

• qvel (ndarray | None) – Joint velocity.

Returns:

Coriolis torque.

Return type:

np.ndarray

get_dof()

Get the total degrees of freedom of the robot.

Returns:

Total degrees of freedom.

Return type:

int

get_ee_default_quat()

Get the default orientation of the end effector.

Return type:

ndarray

get_ee_home_pos()

Get the home position of the end effector.

Return type:

ndarray

get_ee_home_quat()

Get the home orientation of the end effector.

Return type:

ndarray

get_ee_link_id()

Get the pybullet link ID of the robot end effector.

Returns:

Link ID of the robot end effector.

Return type:

int

get_ee_pose(fk=True)

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_quat_from_vectors(u=(-1., 0., 0.), v=(1., 0., 0.))

Compute the quaternion from two directions (the “down” direction for the end effector and the “forward” direction for the end effector).

Parameters:

• u (Tuple[float, float, float] | List[float] | ndarray) – the “down” direction for the end effector.

• v (Tuple[float, float, float] | List[float] | ndarray) – the “forward” direction for the end effector.

Return type:

Tuple[float, float, float, float] | List[float] | ndarray

get_ee_to_tool(tool_id)

Get the pose of the tool frame relative to the end effector frame.

Parameters:

tool_id (int) – ID of the tool frame.

Returns:

3D position and 4D quaternion of the tool frame.

Return type:

Tuple[np.ndarray, np.ndarray]

get_ee_velocity(fk=True)

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_full_dof()

Get the total degrees of freedom of the robot, including the gripper joints.

Returns:

Total degrees of freedom.

Return type:

int

get_full_home_qpos()

Get the home joint configuration.

Return type:

ndarray

get_full_joint_ids()

Get the pybullet joint IDs of the robot, including the gripper joints.

Returns:

Joint IDs of the robot.

Return type:

Sequence[int]

get_full_joint_limits()

Get the joint limits of the robot arm, including the gripper joints.

Returns:

Lower and upper joint limits.

Return type:

Tuple[np.ndarray, np.ndarray]

get_full_qpos()

Get the current joint configuration.

Returns:

Current joint configuration.

Return type:

np.ndarray

get_full_qvel()

Get the current joint velocity.

Returns:

Current joint velocity.

Return type:

np.ndarray

get_gripper_state()

Get the gripper state.

Returns:

True if the gripper is activated.

Return type:

Optional[bool]

get_home_qpos()

Get the home joint configuration.

Return type:

ndarray

get_jacobian(qpos=None, link_id=None)

Get the Jacobian matrix.

Parameters:

• qpos (ndarray | None) – Joint configuration.

• link_id (int | None) – id of the link. If not specified, the Jacobian of the end effector is returned.

Returns:

Jacobian matrix. The shape is (6, nr_moveable_joints).

Return type:

np.ndarray

get_joint_ids()

Get the pybullet joint IDs of the robot.

Returns:

Joint IDs of the robot.

Return type:

Sequence[int]

get_joint_limits()

Get the joint limits.

Returns:

Lower and upper joint limits.

Return type:

Tuple[np.ndarray, np.ndarray]

get_mass_matrix(qpos=None)

Get the mass matrix.

Parameters:

qpos (ndarray | None) – Joint configuration.

Returns:

Mass matrix.

Return type:

np.ndarray

get_qpos()

Get the current joint configuration.

Returns:

Current joint configuration.

Return type:

np.ndarray

get_qvel()

Get the current joint velocity.

Returns:

Current joint velocity.

Return type:

np.ndarray

ik(pos, quat, force=False, max_distance=float('inf'), max_attempts=1000, verbose=False)

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, pos_tol=1e-2, quat_tol=1e-2, verbose=False)

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.

• pos_tol (float) – tolerance of the position. Defaults to 1e-2.

• quat_tol (float) – tolerance of the quaternion. Defaults to 1e-2.

• 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)

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

is_colliding(q=None, return_contacts=False, ignored_collision_bodies=None)

Check if the robot is colliding with other objects. When the joint configuration (q) is provided, we will set the robot to that configuration before checking the collision. Note that this function will not restore the robot to the original configuration after the check. If you want to restore the robot to the original configuration, you should use is_colliding_with_saved_state() instead.

Parameters:

• q (ndarray | None) – Joint configuration. If None, the current joint configuration is used.

• return_contacts (bool) – whether to return the contact information. Defaults to False.

• ignored_collision_bodies (Sequence[int] | None) – IDs of the objects to be ignored in collision checking. Defaults to None.

is_colliding_with_saved_state(q=None, return_contacts=False, ignored_collision_bodies=None)

Parameters:

• q (ndarray | None)

• return_contacts (bool)

• ignored_collision_bodies (Sequence[int] | None)

move_home(timeout=10.0)

Move the robot to the home configuration.

Parameters:

timeout (float)

Return type:

bool

move_home_cfree(speed=0.01, timeout=10.0)

Parameters:

• speed (float)

• timeout (float)

move_pose(pos, quat, speed=0.01, force=False, verbose=False)

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:

bool

move_pose_smooth(pos, quat, speed=0.01, max_qpos_distance=1.0)

Parameters:

• pos (ndarray)

• quat (ndarray)

• speed (float)

• max_qpos_distance (float)

Return type:

bool

move_qpos(target_qpos, speed=0.01, timeout=10.0, local_smoothing=True)

Move the robot to the given joint configuration.

Parameters:

• target_qpos (ndarray) – Target joint configuration.

• speed (float) – Speed of the movement. Defaults to 0.01.

• timeout (float) – Timeout of the movement. Defaults to 10.

• local_smoothing (bool) – Whether to use local smoothing. Defaults to True.

Returns:

True if the movement is successful.

Return type:

bool

move_qpos_path(qpos_trajectory, speed=0.01, timeout=10, first_timeout=None, local_smoothing=True, verbose=False)

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) – 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:

bool

move_qpos_path_v2(qpos_trajectory, timeout=20)

A customized version of move_qpos_trajectory. It allows the user to specify path tracking mechanisms.

Parameters:

• qpos_trajectory (Iterable[ndarray]) – joint configuration trajectory.

• timeout (float) – timeout of the movement.

Returns:

True if the movement is successful.

Return type:

bool

move_qpos_path_v2_set_control(qpos_trajectory, step_size=1, gains=0.3, atol=0.03, timeout=20, verbose=False, return_world_states=False)

Parameters:

• qpos_trajectory (Iterable[ndarray])

• step_size (float)

• gains (float)

• atol (float)

• timeout (float)

• verbose (bool)

• return_world_states (bool)

register_action(name, func, interface=None)

Register an action primitive.

Parameters:

• name (str) – name of the action primitive.

• func (Callable) – function that implements the action primitive.

• interface (str | None) – interface of the action primitive. Defaults to None. If None, the action primitive is registered for the current interface.

Raises:

ValueError – If the action primitive is already registered.

Return type:

None

replay_qpos_trajectory(qpos_trajectory, verbose=True)

Replay a joint confifguartion trajectory. This function is useful for debugging a generated joint trajectory.

Parameters:

• qpos_trajectory (Iterable[ndarray]) – joint configuration trajectory.

• verbose (bool) – whether to print debug information.

reset_home_qpos()

Reset the home joint configuration.

rrt_collision_free(qpos1, qpos2=None, ignored_collision_bodies=None, smooth_fine_path=False, disable_renderer=True, **kwargs)

RRT-based collision-free path planning.

Parameters:

• qpos1 (ndarray) – Start position.

• qpos2 (ndarray | None) – End position. If None, the current position is used.

• ignored_collision_bodies (Sequence[int] | None) – IDs of the objects to ignore in collision checking. Defaults to None.

• smooth_fine_path (bool) – Whether to smooth the path. Defaults to False.

• disable_renderer (bool) – Whether to disable the renderer. Defaults to True.

• kwargs – Additional arguments.

Returns:

True if the path is collision-free. List[np.ndarray]: Joint configuration trajectory.

Return type:

bool

set_arm_joint_position_control(target_qpos, control_mode=pybullet.POSITION_CONTROL, gains=0.3, set_gripper_control=True)

Set the arm joint position control.

Parameters:

• target_qpos (ndarray) – target joint configuration.

• control_mode (int) – control mode.

• gains (float) – gains of the controller. Defaults to 0.3.

• set_gripper_control (bool) – whether to set the gripper control. Defaults to True.

set_ee_impedance_control(target_pos, target_quat, kp_pos=200, kp_ori=1, kd_pos=None, kd_ori=0.01, max_torque=100, damping_scale=2.0, simulate_with_position_pd=False, tau_to_qpos_ratio=0.0005, set_gripper_control=True, verbose=True)

Set the end-effector impedance control command.

Parameters:

• target_pos (ndarray) – the target position of the end-effector.

• target_quat (ndarray) – the target orientation of the end-effector.

• kp_pos (float | ndarray) – the proportional gains for the position control.

• kp_ori (float | ndarray) – the proportional gains for the orientation control.

• kd_pos (float | ndarray | None) – the derivative gains for the position control.

• kd_ori (float | ndarray | None) – the derivative gains for the orientation control. It is recommended to manual set this to a small value to avoid oscillation (instead of using the critical damping).

• max_torque (float) – the maximum torque allowed for the robot.

• damping_scale (float) – the scale for the damping term (as a multiplicative term to the critical damping).

• simulate_with_position_pd (bool) – if True, we will simulate the torque control using the position control. This is useful for debugging and it can be more stable.

• tau_to_qpos_ratio (float) – the ratio between the torque and the joint position. This is only used when simulate_with_position_pd is True. We will use qpos += tau * tau_to_qpos_ratio to simulate the position control.

• set_gripper_control (bool) – if True, the gripper control will be set.

• verbose (bool) – if True, the debug information will be printed.

set_ee_pose(pos, quat)

Set the pose of the end effector by inverse kinematics. Return True if the IK solver succeeds.

Parameters:

• pos (ndarray) – 3D position of the end effector.

• quat (ndarray) – 4D quaternion of the end effector.

Returns:

True if the IK solver succeeds.

Return type:

bool

set_full_hold_position_control(qpos=None, gains=0.3)

Set the control parameter for holding the current arm joint positions, while the gripper holding the object.

Parameters:

• qpos (ndarray | None)

• gains (float)

set_full_qpos(qpos)

Set the joint configuration.

Parameters:

qpos (ndarray) – Joint configuration.

Return type:

None

set_ik_method(ik_method)

Parameters:

ik_method (str | IKMethod)

set_qpos(qpos)

Set the joint configuration.

Parameters:

qpos (ndarray) – Joint configuration.

Return type:

None

set_qpos_with_holding(qpos)

Set the joint configuration with the gripper holding the object.

Parameters:

qpos (ndarray) – Joint configuration.

Return type:

None

set_suppress_warnings(value=True)

Parameters:

value (bool)

suppress_warnings()

property p

property w

property world