๐งฎ SO3 Minimum jerk trajectory
Introduction
Planning a minimum jerk trajectory in SO(3) can be a complex task. In this document, we aim to demonstrate the capabilities of the bipedal-locomotion-framework for generating minimum jerk trajectories in SO(3). The document is structured as follows: we first provide an overview of the mathematical foundations behind the planner, followed by a simple example illustrating its usage. Finally, we recommend some interesting readings for readers seeking more in-depth explanations and rigorous proofs.
๐ Math
Given a fixed initial rotation and a final rotation and the associated left trivialized angular velocities, and , we want to compute a trajectory such that , , , , , such that left trivialized angular jerk is minimized
Following the work of Zefran et al. "On the Generation of Smooth Three-Dimensional Rigid Body Motions" it is possible to prove that a trajectory that satisfies the following equation is a minimum jerk trajectory in SO(3).
From now on we call this condition: Minimum jerk trajectory condition.
It is worth noting that the above does not admit an analytic solution for arbitrary boundary conditions. However, in the case of zero boundary velocity and acceleration or specific structure of the boundary condition, it is possible to show that
satisfies condition the minimum jerk necessary condition. To prove the latest statement, we assume that and , and then we compute the left trivialized angular velocity. The assumption can be easily removed with a simple change of variables.
The angular velocity is given by
In other words, the angular velocity will be always proportional to .
This means that we can ask for an initial and final angular velocity that is linearly dependent on . Let us introduce and as
similarly for the angular acceleration
So combining the initial and the final boundary conditions we can write the following linear system.
Solving the above system we obtain
It is now easy to show that satisfies the minimum jerk condition indeed
and all the cross products vanish since the angular velocity and its derivatives are linearly dependent.
๐ป How to use the planner
The SO3 minimum jerk planner is provided in bipedal-locomotion-framework through a template class that allows the user to use the left or the right trivialized angular velocity
using namespace std::chrono_literals; manif::SO3d initialTransform = manif::SO3d::Random(); manif::SO3d finalTransform = manif::SO3d::Random(); constexpr std::chrono::nanoseconds T = 1s; BipedalLocomotion::Planner::SO3PlannerInertial planner; manif::SO3d::Tangent initialVelocity = (finalTransform * initialTransform.inverse()).log(); initialVelocity.coeffs() = initialVelocity.coeffs() * 2; planner.setInitialConditions(initialVelocity, manif::SO3d::Tangent::Zero()); planner.setFinalConditions(manif::SO3d::Tangent::Zero(), manif::SO3d::Tangent::Zero()); planner.setRotations(initialTransform, finalTransform, T); manif::SO3d rotation, predictedRotation; manif::SO3d::Tangent velocity, predictedVelocity; manif::SO3d::Tangent acceleration; planner.evaluatePoint(0.42s, rotation, velocity, acceleration);
๐ Interesting readings
The interested reader can refer to the extensive literature, among which it is worth mentioning:
- ลฝefran, M., Kumar, V., and Croke, C. (1998). On the generation of smooth threedimensional rigid body motions. IEEE Transactions on Robotics and Automation, 14(4):576โ589.
- Dubrovin, B. A., Fomenko, A. T., and Novikov, S. P. (1984). Modern Geometry โ Methods and Applications, volume 93. Springer New York, New York, NY.
- Needham, T. (2021). Visual Differential Geometry and Forms. Princeton University Press.
- Pressley, A. (2010). Elementary Differential Geometry. Springer London, London.
- Giulio Romualdi (2022) Online Control of Humanoid Robot Locomotion Ph.D. Thesis.