๐Ÿงฎ 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 $(t_0, R_0)$ and a final rotation $(t_f, R_f)$ and the associated left trivialized angular velocities, $(t_0, \omega_0)$ and $(t_f, \omega_f)$ , we want to compute a trajectory $R : \mathbb{R}_+ \rightarrow SO(3)$ such that $R(t_0) = R_0$ , $R(t_f) = R_f$ , $\omega(t_0) = \omega_0$ , $\omega(t_f) = \omega_f$ , $\dot{\omega}(t_0) = \dot{\omega}_0$ , $\dot{\omega}(t_f) = \dot{\omega}_f$ such that left trivialized angular jerk is minimized

\[ \mathfrak{G} = \int_{t_0}^{t_f} \left({}^\mathcal{F} \ddot{\omega} _ {\mathcal{I}, F}^\top {}^\mathcal{F} \ddot{\omega} _ {\mathcal{I}, F} \right)\text{d} t. \]

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 $R(t)$ that satisfies the following equation is a minimum jerk trajectory in SO(3).

\[ \begin{array}{ll} \omega ^{(5)} & + 2 \omega \times \omega ^{(4)} \\ & + \frac{4}{5} \omega \times (\omega \times \omega ^{(3)}) + \frac{5}{2} \dot{\omega} \times \omega^{(3)} \\ & + \frac{1}{4} \omega \times (\omega \times (\omega \times \ddot{\omega})) \\ & + \frac{3}{2} \omega \times (\dot{\omega} \times \ddot{\omega}) \\ & - (\omega \times \ddot{\omega}) \times \dot{\omega} \\ & - \frac{1}{4} (\omega \times \dot{\omega}) \times \ddot{\omega} \\ & - \frac{3}{8} \omega \times ((\omega \times \dot{\omega}) \times \dot{\omega}) \\ & - \frac{1}{8} (\omega \times (\omega \times \dot{\omega})) \times \dot{\omega} = 0. \end{array} \]

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

\[ R(t) = R_{0} \exp{\left(s(t-t_0) \log\left(R_0^\top R_f \right) \right)} \quad \quad s(\tau) = a_5 \tau^5 + a_4 \tau^4 + a_3 \tau^3 + a_2 \tau^2 + a_1 \tau + a_0 \]

satisfies condition the minimum jerk necessary condition. To prove the latest statement, we assume that $t_0 = 0$ and $t_1 = 1$ , 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

\[ \begin{array}{ll} \omega^\wedge &= R^\top \dot{R} \\ &= \dot{s} \exp{\left(-s\log\left( R _ 0^\top R _ f \right) \right)}R_{0} ^\top R _ {0} \log\left( R _ 0^\top R _ f \right) \exp{\left(s\log\left( R _ 0^\top R _ f \right) \right)} \\ &= \dot{s}\exp{\left(-s\log\left(R _ 0^\top R _ f \right) \right)} \log\left( R _ 0^\top R _ f \right) \exp{\left(s\log\left( R _ 0^\top R _ f \right) \right)} \\ &= \dot{s}\exp{\left(-s\log\left(R _ 0^\top R _ f \right) \right)} \exp{\left(s\log\left( R _ 0^\top R _ f \right) \right)} \log\left( R _ 0^\top R _ f \right) \\ &= \dot{s} \log\left( R _ 0^\top R _ f \right). \end{array} \]

In other words, the angular velocity will be always proportional to $\log\left( R_0^\top R_f \right)$ .

This means that we can ask for an initial and final angular velocity that is linearly dependent on $\log\left(R_0^\top R_f \right)$ . Let us introduce $\omega^\wedge(0)$ and $\omega^\wedge(1)$ as

\[ \omega^\wedge(0) = \lambda ^\omega _ 0 \log\left( R_0^\top R_f \right) \quad \omega^\wedge(1) = \lambda ^\omega _ 1 \log\left( R_0^\top R_f \right) \]

similarly for the angular acceleration

\[ \dot{\omega}^\wedge(0) = \lambda ^\alpha _ 0 \log\left( R_0^\top R_f \right) \quad \dot{\omega}^\wedge(1) = \lambda ^\alpha _ 1 \log\left( R_0^\top R_f \right) \]

So combining the initial and the final boundary conditions we can write the following linear system.

\[ \begin{array}{ll} s(0) &= 0 \\ s(1) &= 1 \\ \dot{s}(0) &= \lambda ^\omega _ 0 \\ \dot{s}(1) &= \lambda ^\omega _ 1 \\ \ddot{s}(0) &= \lambda ^\alpha _ 0 \\ \ddot{s}(1) &= \lambda ^\alpha _ 1 \end{array} \]

Solving the above system we obtain

\[ \begin{array}{ll} a_0 &= 0 \\ a_1 &= \lambda ^\omega _ 0 \\ a_2 &= \lambda ^\alpha _ 0/2 \\ a_3 &= \lambda ^\alpha _ 1/2 - (3 \lambda ^\alpha _ 0)/2 - 6 \lambda ^\omega _ 0 - 4 \lambda ^\omega _ 1 + 10 \\ a_4 &= (3 \lambda ^\alpha _ 0)/2 - \lambda ^\alpha _ 1 + 8 \lambda ^\omega _ 0 + 7 \lambda ^\omega _ 1 - 15 \\ a_5 &= \lambda ^\alpha _ 1/2 - \lambda ^\alpha _ 0/2 - 3 \lambda ^\omega _ 0 - 3 \lambda ^\omega _ 1 + 6 \end{array} \]

It is now easy to show that $\omega$ satisfies the minimum jerk condition indeed

\[ \omega ^{(5)} = 0 \]

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.