def _init_moveit(self):
"""
Initialize moveit and setup move_group object
"""
self.moveit_group = MoveGroup(self.configs.ARM.MOVEGROUP_NAME,
self.configs.ARM.ARM_BASE_FRAME,
self.configs.ARM.EE_FRAME,
self.configs.ARM.ROSSRV_CART_PATH,
listener=self.tf_listener,
plan_only=False)
self.moveit_group.setPlannerId(self.moveit_planner)
self.moveit_group.setPlanningTime(self.planning_time)
class Arm(object):
"""
This is a parent class on which the robot
specific Arm classes would be built.
"""
__metaclass__ = ABCMeta
def __init__(
self,
configs,
moveit_planner="ESTkConfigDefault",
planning_time=30,
analytical_ik=None,
use_moveit=True,
):
"""
Constructor for Arm parent class.
:param configs: configurations for arm
:param moveit_planner: moveit planner type
:param analytical_ik: customized analytical ik class
if you have one, None otherwise
:param use_moveit: use moveit or not, default is True
:type configs: YACS CfgNode
:type moveit_planner: string
:type analytical_ik: None or an Analytical ik class
:type use_moveit: bool
"""
self.configs = configs
self.moveit_planner = moveit_planner
self.planning_time = planning_time
self.moveit_group = None
self.use_moveit = use_moveit
self.joint_state_lock = threading.RLock()
self.tf_listener = tf.TransformListener()
if self.use_moveit:
self._init_moveit()
if analytical_ik is not None:
self.ana_ik_solver = analytical_ik(
configs.ARM.ARM_BASE_FRAME, configs.ARM.EE_FRAME
)
self.arm_joint_names = self.configs.ARM.JOINT_NAMES
self.arm_dof = len(self.arm_joint_names)
# Subscribers
self._joint_angles = dict()
self._joint_velocities = dict()
self._joint_efforts = dict()
rospy.Subscriber(
configs.ARM.ROSTOPIC_JOINT_STATES, JointState, self._callback_joint_states
)
# Ros-Params
rospy.set_param("pyrobot/base_link", configs.ARM.ARM_BASE_FRAME)
rospy.set_param("pyrobot/gripper_link", configs.ARM.EE_FRAME)
rospy.set_param(
"pyrobot/robot_description", configs.ARM.ARM_ROBOT_DSP_PARAM_NAME
)
# Publishers
self.joint_pub = None
self._setup_joint_pub()
# Services
self._ik_service = rospy.ServiceProxy("pyrobot/ik", IkCommand)
try:
self._ik_service.wait_for_service(timeout=3)
except:
rospy.logerr("Timeout waiting for Inverse Kinematics Service!!")
self._fk_service = rospy.ServiceProxy("pyrobot/fk", FkCommand)
try:
self._fk_service.wait_for_service(timeout=3)
except:
rospy.logerr("Timeout waiting for Forward Kinematics Service!!")
@abstractmethod
def go_home(self):
"""
Reset robot to default home position
"""
pass
@property
def pose_ee(self):
"""
Return the end effector pose w.r.t 'ARM_BASE_FRAME'
:return:
trans: translational vector (shape: :math:`[3, 1]`)
rot_mat: rotational matrix (shape: :math:`[3, 3]`)
quat: rotational matrix in the form
of quaternion (shape: :math:`[4,]`)
:rtype: tuple (trans, rot_mat, quat)
"""
return self.get_ee_pose(base_frame=self.configs.ARM.ARM_BASE_FRAME)
def get_ee_pose(self, base_frame):
"""
Return the end effector pose with respect to the base_frame
:param base_frame: reference frame
:type base_frame: string
:return:
tuple (trans, rot_mat, quat)
trans: translational vector (shape: :math:`[3, 1]`)
rot_mat: rotational matrix (shape: :math:`[3, 3]`)
quat: rotational matrix in the form
of quaternion (shape: :math:`[4,]`)
:rtype: tuple or ROS PoseStamped
"""
return self.get_transform(base_frame, self.configs.ARM.EE_FRAME)
def get_transform(self, src_frame, dest_frame):
"""
Return the transform from the src_frame to dest_frame
:param src_frame: source frame
:param dest_frame: destination frame
:type src_frame: string
:type dest_frame: basestring
:return:
tuple (trans, rot_mat, quat )
trans: translational vector (shape: :math:`[3, 1]`)
rot_mat: rotational matrix (shape: :math:`[3, 3]`)
quat: rotational matrix in the form
of quaternion (shape: :math:`[4,]`)
:rtype: tuple or ROS PoseStamped
"""
trans, quat = prutil.get_tf_transform(self.tf_listener, src_frame, dest_frame)
rot_mat = prutil.quat_to_rot_mat(quat)
trans = np.array(trans).reshape(-1, 1)
rot_mat = np.array(rot_mat)
quat = np.array(quat)
return trans, rot_mat, quat
def get_joint_angles(self):
"""
Return the joint angles
:return: joint_angles
:rtype: np.ndarray
"""
self.joint_state_lock.acquire()
joint_angles = []
for joint in self.arm_joint_names:
joint_angles.append(self.get_joint_angle(joint))
joint_angles = np.array(joint_angles).flatten()
self.joint_state_lock.release()
return joint_angles
def get_joint_velocities(self):
"""
Return the joint velocities
:return: joint_vels
:rtype: np.ndarray
"""
self.joint_state_lock.acquire()
joint_vels = []
for joint in self.arm_joint_names:
joint_vels.append(self.get_joint_velocity(joint))
joint_vels = np.array(joint_vels).flatten()
self.joint_state_lock.release()
return joint_vels
def get_joint_torques(self):
"""
Return the joint torques
:return: joint_torques
:rtype: np.ndarray
"""
self.joint_state_lock.acquire()
joint_torques = []
for joint in self.arm_joint_names:
try:
joint_torques.append(self.get_joint_torque(joint))
except (ValueError, IndexError):
rospy.loginfo("Torque value for joint " "[%s] not available!" % joint)
joint_torques = np.array(joint_torques).flatten()
self.joint_state_lock.release()
return joint_torques
def get_joint_angle(self, joint):
"""
Return the joint angle of the 'joint'
:param joint: joint name
:type joint: string
:return: joint angle
:rtype: float
"""
if joint not in self.arm_joint_names:
raise ValueError("%s not in arm joint list!" % joint)
if joint not in self._joint_angles.keys():
raise ValueError("Joint angle for joint $s not available!" % joint)
return self._joint_angles[joint]
def get_joint_velocity(self, joint):
"""
Return the joint velocity of the 'joint'
:param joint: joint name
:type joint: string
:return: joint velocity
:rtype: float
"""
if joint not in self.arm_joint_names:
raise ValueError("%s not in arm joint list!" % joint)
if joint not in self._joint_velocities.keys():
raise ValueError("Joint velocity for joint" " $s not available!" % joint)
return self._joint_velocities[joint]
def get_joint_torque(self, joint):
"""
Return the joint torque of the 'joint'
:param joint: joint name
:type joint: string
:return: joint torque
:rtype: float
"""
if joint not in self.arm_joint_names:
raise ValueError("%s not in arm joint list!" % joint)
if joint not in self._joint_efforts.keys():
raise ValueError("Joint torque for joint $s" " not available!" % joint)
return self._joint_efforts[joint]
def set_joint_positions(self, positions, plan=True, wait=True, **kwargs):
"""
Sets the desired joint angles for all arm joints
:param positions: list of length #of joints, angles in radians
:param plan: 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.
:param wait: if True, it will wait until the desired
joint positions are achieved
:type positions: list
:type plan: bool
:type wait: bool
:return: True if successful; False otherwise (timeout, etc.)
:rtype: bool
"""
result = False
if isinstance(positions, np.ndarray):
positions = positions.flatten().tolist()
if plan:
if not self.use_moveit:
raise ValueError(
"Moveit is not initialized, " "did you pass in use_moveit=True?"
)
if isinstance(positions, np.ndarray):
positions = positions.tolist()
rospy.loginfo("Moveit Motion Planning...")
result = self.moveit_group.moveToJointPosition(
self.arm_joint_names, positions, wait=wait
)
else:
self._pub_joint_positions(positions)
if wait:
result = self._loop_angle_pub_cmd(self._pub_joint_positions, positions)
return result
def make_plan_joint_positions(self, positions, **kwargs):
result = None
if isinstance(positions, np.ndarray):
positions = positions.flatten().tolist()
if not self.use_moveit:
raise ValueError(
"Moveit is not initialized, " "did you pass in use_moveit=True?"
)
rospy.loginfo("Moveit Motion Planning...")
result = self.moveit_group.motionPlanToJointPosition(
self.arm_joint_names, positions
)
return [p.positions for p in result]
def set_joint_velocities(self, velocities, **kwargs):
"""
Sets the desired joint velocities for all arm joints
:param velocities: target joint velocities
:type velocities: list
"""
self._pub_joint_velocities(velocities)
def set_joint_torques(self, torques, **kwargs):
"""
Sets the desired joint torques for all arm joints
:param torques: target joint torques
:type torques: list
"""
self._pub_joint_torques(torques)
def set_ee_pose(
self, 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.
:param position: position of the end effector (shape: :math:`[3,]`)
:param orientation: orientation of the end effector
(can be rotation matrix, euler angles (yaw,
pitch, roll), or quaternion)
(shape: :math:`[3, 3]`, :math:`[3,]`
or :math:`[4,]`)
The convention of the Euler angles here
is z-y'-x' (intrinsic rotations),
which is one type of Tait-Bryan angles.
:param plan: use moveit the plan a path to move to the desired pose
:param wait: wait until the desired pose is achieved
:param numerical: use numerical inverse kinematics solver or
analytical inverse kinematics solver
:type position: np.ndarray
:type orientation: np.ndarray
:type plan: bool
:type wait: bool
:type numerical: bool
:return: Returns True if command succeeded, False otherwise
:rtype: bool
"""
if plan:
if not self.use_moveit:
raise ValueError(
"Using plan=True when moveit is not"
" initialized, did you pass "
"in use_moveit=True?"
)
pose = Pose()
position = position.flatten()
if orientation.size == 4:
orientation = orientation.flatten()
ori_x = orientation[0]
ori_y = orientation[1]
ori_z = orientation[2]
ori_w = orientation[3]
elif orientation.size == 3:
quat = prutil.euler_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
elif orientation.size == 9:
quat = prutil.rot_mat_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
else:
raise TypeError(
"Orientation must be in one "
"of the following forms:"
"rotation matrix, euler angles, or quaternion"
)
pose.position.x, pose.position.y, pose.position.z = (
position[0],
position[1],
position[2],
)
(
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w,
) = (ori_x, ori_y, ori_z, ori_w)
result = self.moveit_group.moveToPose(pose, wait=True)
else:
joint_positions = self.compute_ik(
position, orientation, numerical=numerical
)
result = False
if joint_positions is None:
rospy.logerr("No IK solution found; check if target_pose is valid")
else:
result = self.set_joint_positions(joint_positions, plan=plan, wait=wait)
return result
def make_plan_pose(
self, position, orientation, wait=True, numerical=True, **kwargs
):
if not self.use_moveit:
raise ValueError(
"Using plan=True when moveit is not"
" initialized, did you pass "
"in use_moveit=True?"
)
pose = Pose()
position = position.flatten()
if orientation.size == 4:
orientation = orientation.flatten()
ori_x = orientation[0]
ori_y = orientation[1]
ori_z = orientation[2]
ori_w = orientation[3]
elif orientation.size == 3:
quat = prutil.euler_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
elif orientation.size == 9:
quat = prutil.rot_mat_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
else:
raise TypeError(
"Orientation must be in one "
"of the following forms:"
"rotation matrix, euler angles, or quaternion"
)
pose.position.x, pose.position.y, pose.position.z = (
position[0],
position[1],
position[2],
)
(
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w,
) = (ori_x, ori_y, ori_z, ori_w)
result = self.moveit_group.motionPlanToPose(pose, wait=True)
return [p.positions for p in result]
def move_ee_xyz(
self, displacement, eef_step=0.005, numerical=True, plan=True, **kwargs
):
"""
Keep the current orientation fixed, move the end
effector in {xyz} directions
:param displacement: (delta_x, delta_y, delta_z)
:param eef_step: resolution (m) of the interpolation
on the cartesian path
:param numerical: use numerical inverse kinematics solver or
analytical inverse kinematics solver
:param plan: 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`
:type displacement: np.ndarray
:type eef_step: float
:type numerical: bool
:type plan: bool
:return: whether the movement is successful or not
:rtype: bool
"""
displacement = displacement.reshape(-1, 1)
path_len = np.linalg.norm(displacement)
num_pts = int(np.ceil(path_len / float(eef_step)))
if num_pts <= 1:
num_pts = 2
if (hasattr(self, "ana_ik_solver") and not numerical) or not plan:
ee_pose = self.get_ee_pose(self.configs.ARM.ARM_BASE_FRAME)
cur_pos, cur_ori, cur_quat = ee_pose
waypoints_sp = np.linspace(0, path_len, num_pts)
waypoints = cur_pos + waypoints_sp / float(path_len) * displacement
way_joint_positions = []
qinit = self.get_joint_angles().tolist()
for i in range(waypoints.shape[1]):
joint_positions = self.compute_ik(
waypoints[:, i].flatten(),
cur_quat,
qinit=qinit,
numerical=numerical,
)
if joint_positions is None:
rospy.logerr(
"No IK solution found; " "check if target_pose is valid"
)
return False
way_joint_positions.append(copy.deepcopy(joint_positions))
qinit = copy.deepcopy(joint_positions)
success = False
for joint_positions in way_joint_positions:
success = self.set_joint_positions(
joint_positions, plan=plan, wait=True
)
return success
else:
if not self.use_moveit:
raise ValueError(
"Using plan=True when moveit is not"
" initialized, did you pass "
"in use_moveit=True?"
)
ee_pose = self.get_ee_pose(self.configs.ARM.ARM_BASE_FRAME)
cur_pos, cur_ori, cur_quat = ee_pose
cur_pos = np.array(cur_pos).reshape(-1, 1)
cur_quat = np.array(cur_quat)
waypoints_sp = np.linspace(0, path_len, num_pts)
waypoints = cur_pos + waypoints_sp / path_len * displacement
moveit_waypoints = []
wpose = Pose()
for i in range(waypoints.shape[1]):
if i < 1:
continue
wpose.position.x = waypoints[0, i]
wpose.position.y = waypoints[1, i]
wpose.position.z = waypoints[2, i]
wpose.orientation.x = cur_quat[0]
wpose.orientation.y = cur_quat[1]
wpose.orientation.z = cur_quat[2]
wpose.orientation.w = cur_quat[3]
moveit_waypoints.append(copy.deepcopy(wpose))
result = self.moveit_group.followCartesian(
way_points=moveit_waypoints, # waypoints to follow
way_point_frame=self.configs.ARM.ARM_BASE_FRAME,
max_step=eef_step, # eef_step
jump_threshold=0.0,
) # jump_threshold
if result is False:
return False
ee_pose = self.get_ee_pose(self.configs.ARM.ARM_BASE_FRAME)
cur_pos, cur_ori, cur_quat = ee_pose
cur_pos = np.array(cur_pos).reshape(-1, 1)
success = True
diff = cur_pos.flatten() - waypoints[:, -1].flatten()
error = np.linalg.norm(diff)
if error > self.configs.ARM.MAX_EE_ERROR:
rospy.logerr("Move end effector along xyz failed!")
success = False
return success
def compute_fk_position(self, 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
:param joint_positions: joint angles
:param des_frame: desired frame
:type joint_positions: np.ndarray
:type des_frame: string
:return: translational vector and rotational matrix
:rtype: np.ndarray, np.ndarray
"""
joint_positions = joint_positions.flatten()
req = FkCommandRequest()
req.joint_angles = list(joint_positions)
req.end_frame = des_frame
try:
resp = self._fk_service(req)
except:
rospy.logerr("FK Service call failed")
resp = FkCommandResponse()
resp.success = False
if not resp.success:
return None
pos = np.asarray(resp.pos).reshape(3, 1)
rot = prutil.quat_to_rot_mat(resp.quat)
return pos, rot
def get_jacobian(self, joint_angles):
"""
Return the geometric jacobian on the given joint angles.
Refer to P112 in "Robotics: Modeling, Planning, and Control"
:param joint_angles: joint_angles
:type joint_angles: list or flattened np.ndarray
:return: jacobian
:rtype: np.ndarray
"""
raise NotImplementedError
def compute_fk_velocity(self, joint_positions, joint_velocities, des_frame):
"""
Given joint_positions and joint velocities,
compute the velocities of des_frame with respect
to the base frame
:param joint_positions: joint positions
:param joint_velocities: joint velocities
:param des_frame: end frame
:type joint_positions: np.ndarray
:type joint_velocities: np.ndarray
:type des_frame: string
:return: translational and rotational
velocities (vx, vy, vz, wx, wy, wz)
:rtype: np.ndarray
"""
raise NotImplementedError
def compute_ik(self, position, orientation, qinit=None, numerical=True):
"""
Inverse kinematics
:param position: end effector position (shape: :math:`[3,]`)
:param orientation: end effector orientation.
It can be quaternion ([x,y,z,w],
shape: :math:`[4,]`),
euler angles (yaw, pitch, roll
angles (shape: :math:`[3,]`)),
or rotation matrix (shape: :math:`[3, 3]`)
:param qinit: initial joint positions for numerical IK
:param numerical: use numerical IK or analytical IK
:type position: np.ndarray
:type orientation: np.ndarray
:type qinit: list
:type numerical: bool
:return: None or joint positions
:rtype: np.ndarray
"""
position = position.flatten()
if orientation.size == 4:
orientation = orientation.flatten()
ori_x = orientation[0]
ori_y = orientation[1]
ori_z = orientation[2]
ori_w = orientation[3]
elif orientation.size == 3:
quat = prutil.euler_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
elif orientation.size == 9:
quat = prutil.rot_mat_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
else:
raise TypeError(
"Orientation must be in one "
"of the following forms:"
"rotation matrix, euler angles, or quaternion"
)
if qinit is None:
qinit = self.get_joint_angles().tolist()
elif isinstance(qinit, np.ndarray):
qinit = qinit.flatten().tolist()
if numerical:
pos_tol = self.configs.ARM.IK_POSITION_TOLERANCE
ori_tol = self.configs.ARM.IK_ORIENTATION_TOLERANCE
req = IkCommandRequest()
req.init_joint_positions = qinit
req.pose = [
position[0],
position[1],
position[2],
ori_x,
ori_y,
ori_z,
ori_w,
]
req.tolerance = 3 * [pos_tol] + 3 * [ori_tol]
try:
resp = self._ik_service(req)
except:
rospy.logerr("IK Service call failed")
resp = IkCommandResponse()
resp.success = False
if not resp.success:
joint_positions = None
else:
joint_positions = resp.joint_positions
else:
if not hasattr(self, "ana_ik_solver"):
raise TypeError(
"Analytical solver not provided, "
"please use `numerical=True`"
"to use the numerical method "
"for inverse kinematics"
)
else:
joint_positions = self.ana_ik_solver.get_ik(
qinit,
position[0],
position[1],
position[2],
ori_x,
ori_y,
ori_z,
ori_w,
)
if joint_positions is None:
return None
print(joint_positions)
return np.array(joint_positions)
def _callback_joint_states(self, msg):
"""
ROS subscriber callback for arm joint state (position, velocity)
:param msg: Contains message published in topic
:type msg: sensor_msgs/JointState
"""
self.joint_state_lock.acquire()
for idx, name in enumerate(msg.name):
if name in self.arm_joint_names:
if idx < len(msg.position):
self._joint_angles[name] = msg.position[idx]
if idx < len(msg.velocity):
self._joint_velocities[name] = msg.velocity[idx]
if idx < len(msg.effort):
self._joint_efforts[name] = msg.effort[idx]
self.joint_state_lock.release()
def _pub_joint_positions(self, positions):
joint_state = JointState()
joint_state.position = tuple(positions)
self.joint_pub.publish(joint_state)
def _pub_joint_velocities(self, velocities):
joint_state = JointState()
joint_state.velocity = tuple(velocities)
self.joint_pub.publish(joint_state)
def _pub_joint_torques(self, torques):
joint_state = JointState()
joint_state.effort = tuple(torques)
self.joint_pub.publish(joint_state)
def _init_moveit(self):
"""
Initialize moveit and setup move_group object
"""
self.moveit_group = MoveGroup(
self.configs.ARM.MOVEGROUP_NAME,
self.configs.ARM.ARM_BASE_FRAME,
self.configs.ARM.EE_FRAME,
self.configs.ARM.ROSSRV_CART_PATH,
self.configs.ARM.ROSSRV_MP_PATH,
listener=self.tf_listener,
plan_only=False,
)
self.moveit_group.setPlannerId(self.moveit_planner)
self.moveit_group.setPlanningTime(self.planning_time)
def _angle_error_is_small(self, target_joints):
cur_joint_vals = self.get_joint_angles()
joint_diff = cur_joint_vals - np.array(target_joints)
error = np.max(np.abs(joint_diff))
if error <= self.configs.ARM.MAX_JOINT_ERROR:
return joint_diff, error, True
else:
return joint_diff, error, False
def _loop_angle_pub_cmd(self, cmd, joint_vals):
start_time = time.time()
vel_zero_times = 0
# wait 10s in worse case
success = False
for i in range(int(10 / self.configs.ARM.WAIT_MIN_TIME)):
cmd(joint_vals)
res = self._angle_error_is_small(joint_vals)
joint_diff, error, eflag = res
if eflag:
success = True
break
vels = self.get_joint_velocities()
es_time = time.time() - start_time
if es_time > 0.5 and np.max(np.abs(vels)) < 0.01:
vel_zero_times += 1
else:
vel_zero_times = 0
if vel_zero_times > 10:
success = False
break
rospy.sleep(self.configs.ARM.WAIT_MIN_TIME)
return success
def _setup_joint_pub(self):
self.joint_pub = rospy.Publisher(
self.configs.ARM.ROSTOPIC_SET_JOINT, JointState, queue_size=1
)