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 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)
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)
