Arm

class pyrobot.core.Arm(configs, moveit_planner='ESTkConfigDefault', analytical_ik=None, use_moveit=True)

Bases: object

This is a parent class on which the robot specific Arm classes would be built.

__init__(configs, moveit_planner='ESTkConfigDefault', analytical_ik=None, use_moveit=True)

Constructor for Arm parent class.

Parameters:

• configs (YACS CfgNode) – configurations for arm
• moveit_planner (string) – moveit planner type
• analytical_ik (None or an Analytical ik class) – customized analytical ik class if you have one, None otherwise
• use_moveit (bool) – use moveit or not, default is True

compute_fk_position(joint_positions, des_frame)

Given joint angles, compute the pose of desired_frame with respect to the base frame (self.configs.ARM.ARM_BASE_FRAME). The desired frame must be in self.arm_link_names

Parameters:

• joint_positions (np.ndarray) – joint angles
• des_frame (string) – desired frame

Returns:

translational vector and rotational matrix

Return type:

np.ndarray, np.ndarray

compute_fk_velocity(joint_positions, joint_velocities, des_frame)

Given joint_positions and joint velocities, compute the velocities of des_frame with respect to the base frame

Parameters:

• joint_positions (np.ndarray) – joint positions
• joint_velocities (np.ndarray) – joint velocities
• des_frame (string) – end frame

Returns:

translational and rotational velocities (vx, vy, vz, wx, wy, wz)

Return type:

np.ndarray

compute_ik(position, orientation, qinit=None, numerical=True)

Inverse kinematics

Parameters:

• position (np.ndarray) – end effector position (shape: \([3,]\))
• orientation (np.ndarray) – end effector orientation. It can be quaternion ([x,y,z,w], shape: \([4,]\)), euler angles (yaw, pitch, roll angles (shape: \([3,]\))), or rotation matrix (shape: \([3, 3]\))
• qinit (list) – initial joint positions for numerical IK
• numerical (bool) – use numerical IK or analytical IK

Returns:

None or joint positions

Return type:

np.ndarray

get_ee_pose(base_frame)

Return the end effector pose with respect to the base_frame

Parameters:base_frame (string) – reference frame Returns:tuple (trans, rot_mat, quat)

trans: translational vector (shape: \([3, 1]\))

rot_mat: rotational matrix (shape: \([3, 3]\))

quat: rotational matrix in the form of quaternion (shape: \([4,]\))

Return type:tuple or ROS PoseStamped

get_jacobian(joint_angles)

Return the geometric jacobian on the given joint angles. Refer to P112 in “Robotics: Modeling, Planning, and Control”

Parameters:joint_angles (list or flattened np.ndarray) – joint_angles Returns:jacobian Return type:np.ndarray

get_joint_angle(joint)

Return the joint angle of the ‘joint’

Parameters:joint (string) – joint name Returns:joint angle Return type:float

get_joint_angles()

Return the joint angles

Returns:joint_angles Return type:np.ndarray

get_joint_torque(joint)

Return the joint torque of the ‘joint’

Parameters:joint (string) – joint name Returns:joint torque Return type:float

get_joint_torques()

Return the joint torques

Returns:joint_torques Return type:np.ndarray

get_joint_velocities()

Return the joint velocities

Returns:joint_vels Return type:np.ndarray

get_joint_velocity(joint)

Return the joint velocity of the ‘joint’

Parameters:joint (string) – joint name Returns:joint velocity Return type:float

get_transform(src_frame, dest_frame)

Return the transform from the src_frame to dest_frame

Parameters:

• src_frame (string) – source frame
• dest_frame (basestring) – destination frame

Returns:

tuple (trans, rot_mat, quat )

trans: translational vector (shape: \([3, 1]\))

rot_mat: rotational matrix (shape: \([3, 3]\))

quat: rotational matrix in the form of quaternion (shape: \([4,]\))

Return type:

tuple or ROS PoseStamped

go_home()

Reset robot to default home position

move_ee_xyz(displacement, eef_step=0.005, numerical=True, plan=True, **kwargs)

Keep the current orientation fixed, move the end effector in {xyz} directions

Parameters:

• displacement (np.ndarray) – (delta_x, delta_y, delta_z)
• eef_step (float) – resolution (m) of the interpolation on the cartesian path
• numerical (bool) – use numerical inverse kinematics solver or analytical inverse kinematics solver
• plan (bool) – use moveit the plan a path to move to the desired pose. If False, it will do linear interpolation along the path, and simply use IK solver to find the sequence of desired joint positions and then call set_joint_positions

Returns:

whether the movement is successful or not

Return type:

bool

pose_ee

Return the end effector pose w.r.t ‘ARM_BASE_FRAME’

Returns:trans: translational vector (shape: \([3, 1]\))

rot_mat: rotational matrix (shape: \([3, 3]\))

quat: rotational matrix in the form of quaternion (shape: \([4,]\))

Return type:tuple (trans, rot_mat, quat)

set_ee_pose(position, orientation, plan=True, wait=True, numerical=True, **kwargs)

Commands robot arm to desired end-effector pose (w.r.t. ‘ARM_BASE_FRAME’). Computes IK solution in joint space and calls set_joint_positions. Will wait for command to complete if wait is set to True.

Parameters:

• position (np.ndarray) – position of the end effector (shape: \([3,]\))
• orientation (np.ndarray) – orientation of the end effector (can be rotation matrix, euler angles (yaw, pitch, roll), or quaternion) (shape: \([3, 3]\), \([3,]\) or \([4,]\)) The convention of the Euler angles here is z-y’-x’ (intrinsic rotations), which is one type of Tait-Bryan angles.
• plan (bool) – use moveit the plan a path to move to the desired pose
• wait (bool) – wait until the desired pose is achieved
• numerical (bool) – use numerical inverse kinematics solver or analytical inverse kinematics solver

Returns:

Returns True if command succeeded, False otherwise

Return type:

bool

set_joint_positions(positions, plan=True, wait=True, **kwargs)

Sets the desired joint angles for all arm joints

Parameters:

• positions (list) – list of length #of joints, angles in radians
• plan (bool) – whether to use moveit to plan a path. Without planning, there is no collision checking and each joint will move to its target joint position directly.
• wait (bool) – if True, it will wait until the desired joint positions are achieved

Returns:

True if successful; False otherwise (timeout, etc.)

Return type:

bool

set_joint_torques(torques, **kwargs)

Sets the desired joint torques for all arm joints

Parameters:torques (list) – target joint torques

set_joint_velocities(velocities, **kwargs)

Sets the desired joint velocities for all arm joints

Parameters:velocities (list) – target joint velocities