BaxterArmClient.py 文件源码

def position(self, target_position, trans, height):
        """
        Calculate simple position of the robot's arm.

        Args:
            target_position (Pose): Wanted coordinates of robot's tool
            trans: Calculated transformation
            height (float): Height offset, depends on the number of disks on the rod

        Returns:
            target_position (Pose): Modified coordinates and orientation of robot's tool
        """
        roll = -math.pi / 2
        pitch = 0
        yaw = -math.pi / 2
        quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
        target_position.orientation.x = quaternion[0]
        target_position.orientation.y = quaternion[1]
        target_position.orientation.z = quaternion[2]
        target_position.orientation.w = quaternion[3]
        target_position.position.x = trans[0]
        target_position.position.y = trans[1]
        target_position.position.z = trans[2] + height
        return target_position