pelvis.py 文件源码

def lean_body(self, angle):
        """ Lean forward or back a given angle """
        chest_position = self.zarj.transform.lookup_transform(
            'world', 'torso', rospy.Time()).transform

        log("Lean body {} degrees".format(angle))
        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
        point.orientation = chest_position.rotation

        qua = quaternion_from_euler(euler[0], euler[1] + radians(angle),
                                    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)