def reached_desired_pose(self, max_trans_err=0.02, max_rpy_err=0.04): """ Checks whether controller has reached the desired pose. **Args:** *max_trans_err (float):* Max translational error (in meters) allow in each dimension. *max_rpy_err (float):* Max rotational angle (in radians) allow for roll, pitch, or yaw. **Returns:** reached_desired (boolean): True if arm has reached the desired pose, False otherwise. """ with self._state_lock: ps_des = copy.deepcopy(self._desired_pose_stamped) self._transform_listener.waitForTransform( CONTROLLER_FRAME, ps_des.header.frame_id, rospy.Time(0), rospy.Duration(4.0) ) ps_des.header.stamp = rospy.Time(0) ps_des = self._transform_listener.transformPose(CONTROLLER_FRAME, ps_des) torso_H_wrist_des = conversions.pose_to_mat(ps_des.pose) # really shouldn't need to do waitForTransform here, since rospy.Time(0) means "use the most # recent available transform". but without this, tf raises an ExtrapolationException. -jdb self._transform_listener.waitForTransform(CONTROLLER_FRAME, self._goal_link, rospy.Time(0), rospy.Duration(4.0)) trans, rot = self._transform_listener.lookupTransform(CONTROLLER_FRAME, self._goal_link, rospy.Time(0)) torso_H_wrist = geom.trans_rot_to_hmat(trans, rot) # compute error between actual and desired wrist pose wrist_H_wrist_des = np.dot(linalg.inv(torso_H_wrist), torso_H_wrist_des) trans_err, rot_err = geom.hmat_to_trans_rot(wrist_H_wrist_des) curr_trans_err = np.abs(trans_err).max() curr_rpy_err = np.abs(transformations.euler_from_quaternion(rot_err)).max() if curr_trans_err < max_trans_err and curr_rpy_err < max_rpy_err: self.cancel_desired_pose() return True else: return False
def _pose_sending_func(self): """ Continually sends the desired pose to the controller. """ r = rospy.Rate(100) while not rospy.is_shutdown(): try: r.sleep() except rospy.ROSInterruptException: break # get the desired pose as a transform matrix with self._state_lock: ps_des = copy.deepcopy(self._desired_pose_stamped) delta_dist = self._delta_dist if ps_des == None: # no desired pose to publish continue # convert desired pose to transform matrix from goal frame to CONTROLLER_FRAME self._transform_listener.waitForTransform( CONTROLLER_FRAME, ps_des.header.frame_id, rospy.Time(0), rospy.Duration(4.0) ) # trans, rot = self._transform_listener.lookupTransform(CONTROLLER_FRAME, ps_des.header.frame_id, rospy.Time(0)) # torso_H_wrist_des = geom.trans_rot_to_hmat(trans, rot) ps_des.header.stamp = rospy.Time(0) ps_des = self._transform_listener.transformPose(CONTROLLER_FRAME, ps_des) torso_H_wrist_des = conversions.pose_to_mat(ps_des.pose) if self.br is not None: p = ps_des.pose.position q = ps_des.pose.orientation self.br.sendTransform( (p.x, p.y, p.z), (q.x, q.y, q.z, q.w), rospy.Time.now(), "/cci_goal", CONTROLLER_FRAME ) # get the current pose as a transform matrix goal_link = self._hand_description.hand_frame # really shouldn't need to do waitForTransform here, since rospy.Time(0) means "use the most # recent available transform". but without this, tf sometimes raises an ExtrapolationException. -jdb self._transform_listener.waitForTransform(CONTROLLER_FRAME, goal_link, rospy.Time(0), rospy.Duration(4.0)) trans, rot = self._transform_listener.lookupTransform(CONTROLLER_FRAME, goal_link, rospy.Time(0)) torso_H_wrist = geom.trans_rot_to_hmat(trans, rot) # compute difference between actual and desired wrist pose wrist_H_wrist_des = np.dot(linalg.inv(torso_H_wrist), torso_H_wrist_des) trans_err, rot_err = geom.hmat_to_trans_rot(wrist_H_wrist_des) # scale the relative transform such that the translation is equal # to delta_dist. we're assuming that the scaled angular error will # be reasonable scale_factor = delta_dist / linalg.norm(trans_err) if scale_factor > 1.0: scale_factor = 1.0 # don't overshoot scaled_trans_err = scale_factor * trans_err angle_err, direc, point = transformations.rotation_from_matrix(wrist_H_wrist_des) scaled_angle_err = scale_factor * angle_err wrist_H_wrist_control = np.dot( transformations.translation_matrix(scaled_trans_err), transformations.rotation_matrix(scaled_angle_err, direc, point), ) # find incrementally moved pose to send to the controller torso_H_wrist_control = np.dot(torso_H_wrist, wrist_H_wrist_control) desired_pose = conversions.mat_to_pose(torso_H_wrist_control) desired_ps = PoseStamped() desired_ps.header.stamp = rospy.Time(0) desired_ps.header.frame_id = CONTROLLER_FRAME desired_ps.pose = desired_pose if self.br is not None: p = desired_ps.pose.position q = desired_ps.pose.orientation self.br.sendTransform( (p.x, p.y, p.z), (q.x, q.y, q.z, q.w), rospy.Time.now(), "/cci_imm_goal", CONTROLLER_FRAME ) # send incremental pose to controller self._desired_pose_pub.publish(desired_ps)