def move_hand_center_abs(self, sidename, position, orientation, move_time = None):
desired_q = msg_q_to_q(orientation)
# Use the hand trajectory message to move the hand
if move_time is None:
cur_transform = self.get_current_hand_center_transform(sidename)
move_time = self.determine_hand_move_time(sidename, position, orientation, cur_transform)
rospy.loginfo('Desired {} hand center orientation in world reference frame is {}.'.format(
sidename, fmt_q_as_ypr(desired_q)))
# Rotate from orientation based on x-axis being along hand to one where x-axis
# is perpendicular to palm.
if sidename == 'left':
to_palm_q = [0, 0, -self.SQRT_TWO / 2, self.SQRT_TWO/2]
elif sidename == 'right':
to_palm_q = [0, 0, self.SQRT_TWO / 2, self.SQRT_TWO/2]
else:
rospy.logerr("Unknown side {}".format(sidename))
return
perpendicular_to_palm_q = quaternion_multiply(desired_q, to_palm_q)
palm_orientation = q_to_msg_q(perpendicular_to_palm_q)
# rospy.loginfo('Palm quaternion desired: ' + str(perpendicular_to_palm_q))
msg = self.make_hand_trajectory(sidename, position, palm_orientation, self.HAND_TRAJECTORY_TIME)
self.hand_trajectory_publisher.publish(msg)
rospy.sleep(move_time + 0.5)
# Moves the hand by a relative displacement and to a named orientation. If no orientation
# is provided, then the orientation is not changed. The relative displacement is done
# within the reference frame of the torso so Vector3(1,0,0) will move the hand forward
# away from the front of the robot.
评论列表
文章目录