hands.py 文件源码

def determine_hand_move_time(self, sidename, position, orientation, cur_transform):
        move_time = self.HAND_TRAJECTORY_TIME
        desired_q = msg_q_to_q(orientation)
        cur_q = msg_q_to_q(cur_transform.rotation)
        rotation_amount = quaternion_multiply(desired_q, conjugate_q(cur_q))
        rot_w = rotation_amount[3]
        if rot_w < 0: rot_w = -rot_w
        add_time = 0
        if rot_w < 0.85:
            add_time = self.HAND_ROTATION_TIME
        # if rot_w < 0.92
        #    add_time = self.HAND_ROTATION_TIME
        # if rot_w < 0.75:
        #    add_time += self.HAND_ROTATION_TIME
        if add_time > 0:
            rospy.loginfo('Doing significant {} hand rotation, '
                          'adding rotation wait of {}.'.format(sidename, add_time))
            move_time += add_time
        return move_time

    # Moves the hand center to an absolute world position (Vector3) and orientation (Quaternion).