hands.py 文件源码

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.