concepts.simulator.pybullet.components.rby1a.rby1a_robot.RBY1ARobot#

class RBY1ARobot[source]#

Bases: BulletMultiChainRobotRobotBase

Methods

`add_ignore_collision_pair_by_id`(link_a_id, ...)
`add_ignore_collision_pair_by_name`(...)
`attach_object`(ee_id, object_id, ee_to_object)
`close_gripper_free`(hand[, timeout, force])
`create_gripper_constraint`(ee_id, object_id)
`define_joint_groups`(group_name, joint_indices)
`detach_object`(ee_id)
`differential_ik`(pos, quat, link_name_or_id, ...)	Differential inverse kinematics.
`do`(action_name, args, *kwargs)	Execute an action primitive.
`fk`(qpos[, link_name_or_id])	Forward kinematics.
`get_attached_object`(ee_id)
`get_attached_object_pose_in_ee_frame`(ee_id)
`get_body_id`()	Get the pybullet body ID of the robot.
`get_body_name`()	Get the name of the robot body.
`get_body_pose`()	Get the pose of the robot body.
`get_coriolis_torque`([qpos, qvel])	Get the Coriolis torque.
`get_dof`([group_name])	Get the total degrees of freedom of the robot.
`get_ee_default_quat`([group_name])	Get the default orientation of the end effector.
`get_ee_link_default_quat`(ee_link_id)
`get_ee_link_id`([group_name])	Get the pybullet link ID of the robot end effector.
`get_ee_pose`([group_name, fk])	Get the pose of the end effector.
`get_ee_quat_from_vectors`([u, v, group_name])	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`(hand, tool_id)
`get_ee_velocity`([group_name, fk])	Get the velocity of the end effector.
`get_home_qpos`()	Get the home joint configuration.
`get_jacobian`([qpos, link_id, group_name])	Get the Jacobian matrix.
`get_joint_ids`([group_name])	Get the pybullet joint IDs of the robot.
`get_joint_limits`([group_name])	Get the joint limits.
`get_joint_names`([group_name])	Get the joint names of the robot.
`get_link_index`(name)
`get_link_pose`(name_or_index)	Get the pose of one of the links.
`get_mass_matrix`([qpos])	Get the mass matrix.
`get_qpos`([group_name])	Get the current joint configuration.
`get_qpos_groupdict`()	Get the current joint configuration.
`get_qpos_nameddict`([group_name])	Get the current joint configuration.
`get_qvel`([group_name])	Get the current joint velocity.
`get_tracik_solver`()
`ik`(pos, quat, link_name_or_id[, last_qpos, ...])	Inverse kinematics.
`ik_pybullet`(pos, quat, link_id[, force, ...])
`ik_scipy`(pos, quat, link_id[, last_qpos, ...])
`ik_tracik`(pos, quat, ee_link_id[, ...])
`ikfast`(pos, quat, link_id[, last_qpos, ...])
`index_full_joint_state_by_name`(state, names)
`index_full_joint_state_group`(array, group_name)	Index the full joint state to the group joint state.
`init_joint_name_to_state_index`()
`internal_set_gripper_position`(hand, activate)
`internal_set_gripper_state`(hand, activate[, ...])
`is_qpos_valid`(qpos)	Check if the joint configuration is valid.
`is_self_collision`(qpos)	Check if the robot is in self collision
`is_valid_ik_solution`(ik_solution, last_state)
`open_gripper_free`(hand[, timeout, force])
`register_action_controller`(name, func[, ...])	Register an action primitive.
`reset_home_qpos`()	Reset the home joint configuration.
`set_body_pose`(pos, quat)	Set the pose of the robot body.
`set_ee_pose`(pos, quat[, group_name])	Set the pose of the end effector by inverse kinematics.
`set_ik_method`(ik_method)
`set_index_full_joint_state_by_name`(state, ...)
`set_index_full_joint_state_group_dict`(...)	Set the group joint state to the full joint state.
`set_qpos`(qpos[, group_name, ...])	Set the joint configuration.
`set_qpos_groupdict`(qpos_dict[, ...])	Set the joint configuration.
`set_qpos_nameddict`(qpos_dict[, group_name, ...])	Set the joint configuration.
`set_qpos_with_attached_objects`(qpos)	Set the joint configuration with the gripper holding the object.
`set_suppress_warnings`([value])
`suppress_warnings`()

Attributes

`IGNORED_COLLISION_PAIRS`
`RBY1A_FILE`
`RBY1A_TRACIK_FILE`
`joint_name_to_state_index`
`p`
`w`
`world`
`joint_groups`	Joint groups of the robot.
`joint_groups_names`	Joint names of each joint in the joint groups.
`joint_groups_ee_link_ids`	End effector link IDs of each joint groups.
`joint_groups_start_index`	Start index of each joint groups.

__init__(client, body_id=None, body_name='rby1a', pos=None, quat=None, use_magic_gripper=True, enforce_torso_safety=True)[source]#

Parameters:

client (BulletClient)
body_id (int | None)
body_name (str)
pos (Tuple[float, float, float] | List[float] | ndarray | None)
quat (Tuple[float, float, float, float] | List[float] | ndarray | None)
use_magic_gripper (bool)
enforce_torso_safety (bool)

__new__(**kwargs)#

add_ignore_collision_pair_by_id(link_a_id, link_b_id)[source]#

Parameters:

link_a_id (int)
link_b_id (int)

add_ignore_collision_pair_by_name(link_name_a, link_name_b)[source]#

attach_object(ee_id, object_id, ee_to_object)#

Parameters:

ee_id (int)
object_id (int)
ee_to_object (Tuple[ndarray, ndarray])

Return type:

None

close_gripper_free(hand, timeout=0.0, force=False)[source]#

Parameters:

hand (str)
timeout (float)
force (bool)

Return type:

bool

create_gripper_constraint(ee_id, object_id, verbose=False)#

Parameters:

ee_id (int)
object_id (int)
verbose (bool)

define_joint_groups(group_name, joint_indices, ee_link_id=None, start_index=0)#

Parameters:

group_name (str)
joint_indices (Sequence[int])
ee_link_id (int | None)
start_index (int | List[int])

Return type:

None

detach_object(ee_id)#

Parameters:: ee_id (int)
Return type:: None

differential_ik(pos, quat, link_name_or_id, last_qpos)#

Differential inverse kinematics.

Parameters:

pos (ndarray)
quat (ndarray)
link_name_or_id (str | int)
last_qpos (ndarray)

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)
link_name_or_id (int | str | None)

Return type:

Tuple[ndarray, ndarray]

get_attached_object(ee_id)#

Parameters:: ee_id (int)
Return type:: int | None

get_attached_object_pose_in_ee_frame(ee_id)#

Parameters:: ee_id (int)
Return type:: Tuple[ndarray, ndarray] | None

get_body_id()[source]#

Get the pybullet body ID of the robot.

Returns:: Body ID of the robot.
Return type:: int

get_body_name()#

Get the name of the robot body.

Return type:: str

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_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(group_name='__all__')#

Get the total degrees of freedom of the robot.

Returns:: Total degrees of freedom.
Return type:: int
Parameters:: group_name (str)

get_ee_default_quat(group_name=None)#

Get the default orientation of the end effector.

Parameters:: group_name (str | None)
Return type:: ndarray

get_ee_link_default_quat(ee_link_id)#

get_ee_link_id(group_name='__all__')#

Get the pybullet link ID of the robot end effector.

Parameters:: group_name (str)
Return type:: int

get_ee_pose(group_name='__all__', fk=True)#

Get the pose of the end effector.

Parameters:

group_name (str)
fk (bool)

Return type:

Tuple[ndarray, ndarray]

get_ee_quat_from_vectors(u=(-1., 0., 0.), v=(1., 0., 0.), group_name=None)#

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)
v (Tuple[float, float, float] | List[float] | ndarray)
group_name (str | None)

Return type:

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

get_ee_to_tool(hand, tool_id)[source]#

Parameters:

hand (str)
tool_id (int)

Return type:

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

get_ee_velocity(group_name='__all__', fk=True)#

Get the velocity of the end effector.

Parameters:

group_name (str)
fk (bool)

Return type:

Tuple[ndarray, ndarray]

get_home_qpos()#

Get the home joint configuration.

Return type:: ndarray

get_jacobian(qpos=None, link_id=None, group_name='__all__')#

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.
group_name (str) – name of the joint group.

Returns:

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

Return type:

np.ndarray

get_joint_ids(group_name='__all__')#

Get the pybullet joint IDs of the robot.

Returns:: Joint IDs of the robot.
Return type:: Sequence[int]
Parameters:: group_name (str)

get_joint_limits(group_name='__all__')[source]#

Get the joint limits.

Parameters:: group_name (str)
Return type:: Tuple[ndarray, ndarray]

get_joint_names(group_name='__all__')#

Get the joint names of the robot.

Returns:: Joint names of the robot.
Return type:: List[str]
Parameters:: group_name (str)

get_link_index(name)#

Parameters:: name (str)
Return type:: int

get_link_pose(name_or_index)#

Get the pose of one of the links.

Parameters:: name_or_index (str | int)
Return type:: Tuple[ndarray, 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(group_name='__all__')#

Get the current joint configuration.

Parameters:: group_name (str)
Return type:: ndarray

get_qpos_groupdict()#

Get the current joint configuration.

Return type:: Dict[str, ndarray]

get_qpos_nameddict(group_name='__all__')#

Get the current joint configuration.

Parameters:: group_name (str)
Return type:: Dict[str, ndarray]

get_qvel(group_name='__all__')#

Get the current joint velocity.

Parameters:: group_name (str)
Return type:: ndarray

get_tracik_solver()[source]#

ik(pos, quat, link_name_or_id, last_qpos=None, force=False, max_distance=float('inf'), max_attempts=50, verbose=False)#

Inverse kinematics.

Parameters:

pos (ndarray)
quat (ndarray)
link_name_or_id (str | int)
last_qpos (ndarray | None)
force (bool)
max_distance (float)
max_attempts (int)
verbose (bool)

Return type:

ndarray | Dict[str, ndarray] | None

ik_pybullet(pos, quat, link_id, force=False, pos_tol=1e-2, quat_tol=1e-2, verbose=False)[source]#

Parameters:

pos (ndarray)
quat (ndarray)
link_id (int)
force (bool)
pos_tol (float)
quat_tol (float)
verbose (bool)

Return type:

ndarray | Dict[str, ndarray] | None

ik_scipy(pos, quat, link_id, last_qpos=None, max_distance=float('inf'), max_attempts=20, verbose=False)[source]#

Parameters:

pos (ndarray)
quat (ndarray)
link_id (int)
last_qpos (ndarray | None)
max_distance (float)
max_attempts (int)
verbose (bool)

Return type:

ndarray | None

ik_tracik(pos, quat, ee_link_id, last_qpos=None, max_distance=float('inf'), max_attempts=50, verbose=False)[source]#

Parameters:

pos (ndarray)
quat (ndarray)
ee_link_id (int)
last_qpos (ndarray | None)
max_distance (float)
max_attempts (int)
verbose (bool)

Return type:

ndarray | None

ikfast(pos, quat, link_id, last_qpos=None, max_attempts=50, max_distance=float('inf'), error_on_fail=True, verbose=False)#

Parameters:

pos (ndarray)
quat (ndarray)
link_id (int)
last_qpos (ndarray | None)
max_attempts (int)
max_distance (float)
error_on_fail (bool)
verbose (bool)

Return type:

ndarray | None

index_full_joint_state_by_name(state, names)#

Parameters:

state (ndarray)
names (Sequence[str])

Return type:

ndarray

index_full_joint_state_group(array, group_name)#

Index the full joint state to the group joint state.

Parameters:

array (ndarray)
group_name (str)

Return type:

ndarray

init_joint_name_to_state_index()#

internal_set_gripper_position(hand, activate)[source]#

Parameters:

hand (str)
activate (bool)

internal_set_gripper_state(hand, activate, body_index=None)[source]#

Parameters:

hand (str)
activate (bool)
body_index (int | None)

Return type:

None

is_qpos_valid(qpos)[source]#

Check if the joint configuration is valid.

Parameters:: qpos (ndarray)

is_self_collision(qpos)[source]#

Check if the robot is in self collision

Parameters:: qpos (ndarray)
Return type:: bool

is_valid_ik_solution(ik_solution, last_state)[source]#

Parameters:

ik_solution (Dict[str, float])
last_state (ndarray)

open_gripper_free(hand, timeout=0.0, force=False)[source]#

Parameters:

hand (str)
timeout (float)
force (bool)

Return type:

bool

register_action_controller(name, func, interface=None)#

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

reset_home_qpos()#: Reset the home joint configuration.

set_body_pose(pos, quat)#

Set the pose of the robot body.

Parameters:

pos (ndarray) – 3D position of the robot body.
quat (ndarray) – 4D quaternion of the robot body.

Return type:

None

set_ee_pose(pos, quat, group_name=None)#

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

Parameters:

pos (ndarray)
quat (ndarray)
group_name (str | None)

Return type:

bool

set_ik_method(ik_method)#

Parameters:: ik_method (str | IKMethod)

set_index_full_joint_state_by_name(state, name2value)#

Parameters:

state (ndarray)
name2value (Dict[str, float])

Return type:

ndarray

set_index_full_joint_state_group_dict(full_array, group_values)#

Set the group joint state to the full joint state.

Parameters:

full_array (ndarray)
group_values (Dict[str, ndarray])

Return type:

ndarray

set_qpos(qpos, group_name='__all__', update_attached_objects=True)#

Set the joint configuration.

Parameters:

qpos (ndarray)
group_name (str)
update_attached_objects (bool)

Return type:

None

set_qpos_groupdict(qpos_dict, update_attached_objects=True)#

Set the joint configuration.

Parameters:

qpos_dict (Dict[str, ndarray])
update_attached_objects (bool)

Return type:

None

set_qpos_nameddict(qpos_dict, group_name='__all__', update_attached_objects=True)#

Set the joint configuration.

Parameters:

qpos_dict (Dict[str, ndarray])
group_name (str)
update_attached_objects (bool)

Return type:

None

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

IGNORED_COLLISION_PAIRS = [('link_torso_4', 'link_torso_2'), ('link_right_arm_3', 'link_right_arm_5'), ('link_left_arm_3', 'link_left_arm_5')]#

RBY1A_FILE = 'assets://robots/rby1a/model.urdf'#

RBY1A_TRACIK_FILE = 'assets://robots/rby1a/model_tracikpy.urdf'#

joint_groups: Dict[str, Tuple[int, ...]]#: Joint groups of the robot. Each entry is represented as a tuple of joint indices in PyBullet.

joint_groups_ee_link_ids: Dict[str, int]#: End effector link IDs of each joint groups.

joint_groups_names: Dict[str, List[str]]#: Joint names of each joint in the joint groups.

joint_groups_start_index: Dict[str, int | List[int]]#: Start index of each joint groups. This is the first joint index in a full qpos array (which contains only movable joints).

property joint_name_to_state_index#

property p#

property w#

property world#