pelvis.py 文件源码

def turn_body_to_pose(self, pose):
        """
            Turn the torso to a given orientation

            Angles smaller than 110 or larger than 250 are unstable
        """
        log("Turn body to match pose {}".format(pose))
        msg = ChestTrajectoryRosMessage()
        msg.unique_id = self.zarj.uid
        msg.execution_mode = msg.OVERRIDE

        result = self.zarj.transform.tf_buffer.transform(pose, 'world')

        point = SO3TrajectoryPointRosMessage()
        point.time = 1.0
        point.orientation = result.pose.orientation
        point.angular_velocity = Vector3(0.0, 0.0, 0.0)

        msg.taskspace_trajectory_points = [point]

        self.chest_publisher.publish(msg)