pelvis.py 文件源码

def get_lean_points(self, angle):
        """ Return a set of trajectory points to accomplish a lean """
        point = SO3TrajectoryPointRosMessage()

        point.time = 1.0  # Just give it a second to get there
        rotate = quaternion_about_axis(radians(angle), [0, 0, -1])
        point.orientation = Quaternion(rotate[1], rotate[2], rotate[3], rotate[0])
        point.angular_velocity = Vector3(0, 0, 0)
        log('Lean to: {}'.format(point))

        points = []
        points.append(point)
        return points