pelvis.py 文件源码

def turn_body_to(self, angle, wait=True):
        """
            Turn the torso to a given angle as related to the pelvis

            Angles smaller than 110 or larger than 250 are unstable
        """
        log("Turn body to {} degrees".format(angle))
        chest_position = self.zarj.transform.lookup_transform(
            'pelvis', '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))

        qua = quaternion_from_euler(radians(-angle-180), euler[1], euler[2])

        pose = PoseStamped()
        pose.pose.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0])
        pose.header.frame_id = 'pelvis'
        pose.header.stamp = rospy.Time()
        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)

        if wait:
            rospy.sleep(point.time + 0.1)