airobot.arm.dual_arm_pybullet

class airobot.arm.dual_arm_pybullet.DualArmPybullet(cfgs, pb_client, seed=None, self_collision=False, eetool_cfg=None)[source]

Bases: airobot.arm.arm.ARM

Class for the pybullet simulation environment of a dual arm robot.

Parameters:
  • cfgs (YACS CfgNode) – configurations for the arm.
  • pb_client (BulletClient) – pybullet client.
  • seed (int) – random seed.
  • self_collision (bool) – enable self_collision or not whiling loading URDF.
  • eetool_cfg (dict) – arguments to pass in the constructor of the end effector tool class.
Variables:
  • cfgs (YACS CfgNode) – configurations for the robot.
  • robot_id (int) – pybullet body unique id of the robot.
  • arms (dict) – internal dictionary keyed by the names of each single arm, with values as interfaces to the arms.
  • arm_jnt_names (list) – names of the arm joints.
  • right_arm_jnt_names (list) – names of the arm joints on the right arm.
  • left_arm_jnt_names (list) – names of the arm joints on the left arm.
  • arm_jnt_ids (list) – pybullet joint ids of the arm joints.
  • r_ee_link_jnt (str) – name of the end effector link on the right arm.
  • l_ee_link_jnt (str) – name of the end effector link on the left arm.
  • dual_arm_dof (int) – total number of arm joints in the robot.
  • single_arm_dof (int) – number of joints in a single arm of the robot.
  • jnt_to_id (dict) – dictionary with [joint name : pybullet joint] id [key : value] pairs.
  • non_fixed_jnt_names (list) – names of non-fixed joints in the arms, used for returning the correct inverse kinematics solution.
compute_ik(pos, ori=None, arm=None, ns=False, *args, **kwargs)[source]

Compute the inverse kinematics solution given the position and orientation of the end effector.

Parameters:
  • pos (list or np.ndarray) – position (shape: \([3,]\))
  • ori (list or np.ndarray) – orientation. It can be euler angles ([roll, pitch, yaw], shape: \([3,]\)), or quaternion ([qx, qy, qz, qw], shape: \([4,]\)), or rotation matrix (shape: \([3, 3]\)).
  • arm (str) – Which arm EE pose corresponds to, must match arm names in cfg file
  • ns (bool) – whether to use the nullspace options in pybullet, True if nullspace should be used. Defaults to False.
Returns:

list – solution to inverse kinematics, joint angles which achieve the specified EE pose (shape: \([DOF]\)).
disable_torque_control(joint_name=None)[source]

Disable the torque control mode in Pybullet.

Parameters:joint_name (str) – If it’s none, then all the six joints on the UR robot are disabled with torque control. Otherwise, only the specified joint is disabled with torque control. The joint(s) will enter velocity control mode.
enable_torque_control(joint_name=None)[source]

Enable the torque control mode in Pybullet.

Parameters:joint_name (str) – If it’s none, then all the six joints on the UR robot are enabled in torque control mode. Otherwise, only the specified joint is enabled in torque control mode.
get_ee_pose(arm=None)[source]

Return the end effector pose.

Parameters:arm (str) – Returned pose will be for specified arm, must match arm names in cfg file.
Returns:4-element tuple containing
  • np.ndarray: x, y, z position of the EE (shape: \([3,]\)).
  • np.ndarray: quaternion representation of the EE orientation (shape: \([4,]\)).
  • np.ndarray: rotation matrix representation of the EE orientation (shape: \([3, 3]\)).
  • np.ndarray: euler angle representation of the EE orientation (roll, pitch, yaw with static reference frame) (shape: \([3,]\)).
get_ee_vel(arm=None)[source]

Return the end effector’s velocity.

Parameters:arm (str) – Which arm to get velocity for, must match arm names in cfg file.
Returns:2-element tuple containing
  • np.ndarray: translational velocity (shape: \([3,]\)).
  • np.ndarray: rotational velocity (shape: \([3,]\)).
get_jpos(joint_name=None)[source]

Return the joint position(s) of the arm.

Parameters:joint_name (str, optional) – If it’s None, it will return joint positions of all the actuated joints. Otherwise, it will return the joint position of the specified joint.
Returns:One of the following
  • float: joint position given joint_name.
  • list: joint positions if joint_name is None (shape: \([DOF]\)).
get_jtorq(joint_name=None)[source]

If the robot is operated in VELOCITY_CONTROL or POSITION_CONTROL mode, return the joint torque(s) applied during the last simulation step. In TORQUE_CONTROL, the applied joint motor torque is exactly what you provide, so there is no need to report it separately. So don’t use this method to get the joint torque values when the robot is in TORQUE_CONTROL mode.

Parameters:joint_name (str, optional) – If it’s None, it will return joint torques of all the actuated joints. Otherwise, it will return the joint torque of the specified joint.
Returns:One of the following
  • float: joint torque given joint_name.
  • list: joint torques if joint_name is None (shape: \([DOF]\)).
get_jvel(joint_name=None)[source]

Return the joint velocity(ies) of the arm.

Parameters:joint_name (str, optional) – If it’s None, it will return joint velocities of all the actuated joints. Otherwise, it will return the joint velocity of the specified joint.
Returns:One of the following
  • float: joint velocity given joint_name.
  • list: joint velocities if joint_name is None (shape: \([DOF]\)).
go_home(arm=None, ignore_physics=False)[source]

Move the robot to a pre-defined home pose.

move_ee_xyz(delta_xyz, eef_step=0.005, arm=None, *args, **kwargs)[source]

Move the end-effector in a straight line without changing the orientation.

Parameters:
  • delta_xyz (list or np.ndarray) – movement in x, y, z directions (shape: \([3,]\)).
  • eef_step (float) – interpolation interval along delta_xyz. Interpolate a point every eef_step distance between the two end points.
  • arm (str) – Which arm to move when setting cartesian command, must match arm names in cfg file.
Returns:

bool – A boolean variable representing if the action is successful at the moment when the function exits.
reset()[source]

Reset the simulation environment.

reset_joint_state(jnt_name, jpos, jvel=0)[source]

Reset the state of the joint. It’s best only to do this at the start, while not running the simulation. It will overrides all physics simulation.

Parameters:
  • jnt_name (str) – joint name.
  • jpos (float) – target joint position.
  • jvel (float) – optional, target joint velocity.
set_ee_pose(pos=None, ori=None, wait=True, arm=None, *args, **kwargs)[source]

Move the end effector to the specifed pose.

Parameters:
  • pos (list or np.ndarray) – Desired x, y, z positions in the robot’s base frame to move to (shape: \([3,]\)).
  • ori (list or np.ndarray, optional) – It can be euler angles ([roll, pitch, yaw], shape: \([4,]\)), or quaternion ([qx, qy, qz, qw], shape: \([4,]\)), or rotation matrix (shape: \([3, 3]\)). If it’s None, the solver will use the current end effector orientation as the target orientation.
  • wait (bool) – Set to True if command should be blocking, otherwise the command can be overwritten before completion.
  • arm (str) – Which arm to move when setting cartesian command, must match arm names in cfg file.
Returns:

bool – A boolean variable representing if the action is successful at the moment when the function exits.
set_jpos(position, arm=None, joint_name=None, wait=True, ignore_physics=False, *args, **kwargs)[source]

Move the arm to the specified joint position(s).

Parameters:
  • position (float or list) – desired joint position(s).
  • arm (str) – If not provided, position should be a list and all actuated joints will be moved. If provided, only half the joints will move, corresponding to which arm was specified. Value should match arm names in cfg file.
  • joint_name (str) – If not provided, position should be a list and all the actuated joints will be moved to the specified positions. If provided, only the specified joint will be moved to the desired joint position. If joint_name is provided, then arm argument should also be provided, and specified joint name must correspond to joint names for the specified arm.
  • wait (bool) – whether to block the code and wait for the action to complete.
Returns:

bool – A boolean variable representing if the action is successful at the moment when the function exits.
set_jtorq(torque, arm=None, joint_name=None, wait=False, *args, **kwargs)[source]

Apply torque(s) to the joint(s), call enable_torque_control() or enable_torque_control(joint_name) before doing torque control.

Note

call to this function is only effective in this simulation step. you need to supply torque value for each simulation step to do the torque control. It’s easier to use torque control in step_simulation mode instead of realtime_simulation mode. If you are using realtime_simulation mode, the time interval between two set_jtorq() calls must be small enough (like 0.0002s)

Parameters:
  • torque (float or list) – torque value(s) for the joint(s).
  • arm (str) – If not provided, torque should be a list and all actuated joints will be moved. If provided, only half the joints will move, corresponding to which arm was specified. Value should match arm names in cfg file.
  • joint_name (str) – specify the joint on which the torque is applied. If it’s not provided(None), it will apply the torques on the six joints on the arm. Otherwise, only the specified joint will be applied with the given torque. If joint_name is provided, then arm argument should also be provided, and specified joint name must correspond to joint names for the specified arm.
  • wait (bool) – Not used in this method, just to keep the method signature consistent.
Returns:

bool – Always return True as the torque will be applied as specified in Pybullet.
set_jvel(velocity, arm=None, joint_name=None, wait=False, *args, **kwargs)[source]

Move the arm with the specified joint velocity(ies).

Parameters:
  • velocity (float or list) – desired joint velocity(ies)
  • arm (str) – If not provided, velocity should be a list and all actuated joints will be moved. If provided, only half the joints will move, corresponding to which arm was specified. Value should match arm names in cfg file.
  • joint_name (str) – If not provided, velocity should be a list and all the actuated joints will be moved in the specified velocities. If provided, only the specified joint will be moved in the desired joint velocity. If joint_name is provided, then arm argument should also be provided, and specified joint name must correspond to joint names for the specified arm.
  • wait (bool) – whether to block the code and wait for the action to complete
Returns:

bool – A boolean variable representing if the action is successful at the moment when the function exits
setup_single_arms(right_arm, left_arm)[source]

Function to setup the single arm instances, and maintain an internal dictionary interface to them.

Parameters: