pelvis.py 文件源码

def turn_body(self, angle):
        """
            Turn the torso by a given angle

            Angles smaller than 110 or larger than 250 are unstable
        """
        log("Turn body {} degrees".format(angle))
        chest_position = self.zarj.transform.lookup_transform(
            'world', 'torso', rospy.Time()).transform

        msg = ChestTrajectoryRosMessage()
        msg.unique_id = self.zarj.uid

        msg.execution_mode = msg.OVERRIDE

        euler = euler_from_quaternion((chest_position.rotation.w,
                                       chest_position.rotation.x,
                                       chest_position.rotation.y,
                                       chest_position.rotation.z))

        point = SO3TrajectoryPointRosMessage()
        point.time = 1.0

        qua = quaternion_from_euler(euler[0] + radians(angle), euler[1],
                                    euler[2])
        point.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0])
        point.angular_velocity = Vector3(0.0, 0.0, 0.0)

        msg.taskspace_trajectory_points = [point]

        self.chest_publisher.publish(msg)