concepts.simulator.pybullet.components.ur5.ur5_robot.UR5Robot

class UR5Robot

Bases: BulletArmRobotBase

Methods

activate_gripper()
do(action_name, *args, **kwargs)	Execute an action primitive.
fk(qpos[, link_name_or_id])	Forward kinematics.
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_qpos()	Get the current joint configuration.
get_qvel()	Get the current joint velocity.
ignore_physics([ignore_physics])
ik(pos, quat[, force, max_distance, ...])	Inverse kinematics.
ik_pybullet(pos, quat[, force, verbose])	Calculate joint configuration with inverse kinematics.
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 UR5 to target 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.
release_gripper()
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_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_ignore_physics([ignore_physics])
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

UR5_FILE
UR5_JOINT_HOMES
p
w
world

__init__(client, body_name='ur5', gripper_type='suction', gripper_objects=None, pos=None, quat=None)

Parameters:

• client (BulletClient)

• gripper_type (str)

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

• pos (ndarray | None)

• quat (ndarray | None)

__new__(**kwargs)

activate_gripper()

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_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(ignore_collision_objects=None)

Parameters:

ignore_collision_objects (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_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

ignore_physics(ignore_physics=True)

Parameters:

ignore_physics (bool)

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, verbose=False)

Calculate joint configuration with inverse kinematics.

Parameters:

• pos (ndarray)

• quat (ndarray)

• force (bool)

• verbose (bool)

Return type:

ndarray | None

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, ignore_collision_objects=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.

• ignore_collision_objects (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, ignore_collision_objects=None)

Parameters:

• q (ndarray | None)

• return_contacts (bool)

• ignore_collision_objects (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 UR5 to target joint configuration.

Parameters:

• target_qpos (ndarray)

• speed (float)

• timeout (float)

• local_smoothing (bool)

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

release_gripper()

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, ignore_collision_objects=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.

• ignore_collision_objects (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_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_ignore_physics(ignore_physics=True)

Parameters:

ignore_physics (bool)

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

UR5_FILE = 'assets://robots/ur5/ur5.urdf'

UR5_JOINT_HOMES = array([-3.14159, -1.5708 ,  1.5708 , -1.5708 , -1.5708 ,  0.     ])

property p

property w

property world