Skip to content

Rigid Body Dynamics on Lie Groups

October 11, 2024

Michael Bauer

Introduction

While one can often get a lot of value from a simple kinematic model of a system (e.g. the bicycle model for wheeled vehicles), there are many cases in robotics where a simulation must adopt an accurate Newtonian dynamics model in order to usefully test the system in question. The bicycle model, for instance, may lose fidelity in extreme maneuvers with high lateral acceleration. This is why many robotics simulation tools (e.g. Dartsim, Drake, Gazebo) provide rich rigid body dynamics solvers.

As we discuss and motivate in our open core documentation, ReSim uses Lie groups and their derivatives to describe the trajectories of simulated actors through the world. Given our extensive use of Lie groups for kinematic applications (example), one may wonder if Lie groups can be used for dynamics problems as well. We can describe the pose and velocity of a rigid body using elements of the Lie group \(\text{SE}(3)\) and the Lie algebra \(\mathfrak{se}(3)\). However, having chosen these coordinates, is it feasible to predict how the system's state will evolve over time under the influence of external forces and torques? The answer is, of course, yes.

Definitions

Lets start by defining some parameters. We begin by describing a rigid body's position and orientation. We do this by attaching a Cartesian coordinate frame to our body. For convenience, we place the origin of this coordinate frame at its center of mass and align its axes with the principal axes of rotation of our body. This coordinate frame is attached to our rigid body such that every part of the body is always stationary in this coordinate frame. We then associate a group element \(g\in \text{SE}(3)\) with the pose of the rigid body. Specifically, the group action of \(g\) on a point's coordinates in our body coordinate frame gives that same point's coordinates in an inertial reference frame. For example, applying \(g\) to \((0, 0, 0)\in \mathbb{R}^3\) (the origin in body coordinates) gives the position of the rigid body our inertial reference frame.

Now, lets define our generalized velocity \(\xi\in\mathfrak{se}(3)\) as the time derivative of \(g\) in right tangent space. Specifically, we define it as the real derivative:

\[ \xi(t) = \frac{d}{d\epsilon}\log\left[g(t)^{-1}g(t + \epsilon)\right] \]

Although we often write this in shorthand:

\[ \xi(t) = \frac{dg(t)}{dt} \]

See Lie Group Derivatives from our open core documentation for information on where the full expression comes from. The Chain Rule section of that document will be relevant as well. We choose a basis for our Lie algebra such that the first three entries of \(\xi\) correspond to the angular velocity of the body in its own coordinate frame, and the last three entries of \(\xi\) correspond to the linear velocity of the body in its own coordinate frame. In other words \(\xi^T=\begin{bmatrix}\omega^T & v^T\end{bmatrix}\) where \(\omega\in\mathbb{R}^3\) is the angular velocity and \(v\in\mathbb{R}^3\) is the linear velocity.

Finally, we define our generalized inertia tensor \(I\in\mathbb{R}^{6\times 6}\) like so:

\[ \mathcal{I} = \begin{bmatrix}I_{xx} \\ & I_{yy}\\ & & I_{zz} \\ & & & m \\ & & & & m \\ & & & & & m\end{bmatrix} \]

Where \(I_{xx}\), \(I_{yy}\), and \(I_{zz}\) are the moments of inertia about the principle axes and \(m\) is the mass of the body. The inertia tensor has this nice diagonal form due to our choice of body coordinate frame (origin at the center of mass and axes aligned with the principal axes of rotation), but it can in general be a dense symmetric matrix if we choose another coordinate frame attached to our body.

The important thing about these definitions is that we can now write the kinetic energy of our rigid body system like so:

\[ \begin{aligned}T = \frac{1}{2} \xi^T \mathcal{I} \xi &= \frac{1}{2}\omega^T I_R\omega + \frac{1}{2}m||v||^2 \\&= KE_R + KE_T\end{aligned} \]

Where we can identify the terms as the rotational kinetic energy \(KE_R\) and the translational kinetic energy \(KE_T\).

Forward Dynamics

Using this expression for kinetic energy, we can derive an ordinary differential equation for the forward dynamics of the system. This ordinary differential equation will relate the system's generalized acceleration \(\dot \xi\) to its generalized velocity \(\xi\). To do this, we will utilize a variational approach. In particular, we define the Lagrangian without any conservative forces:

\[ L(g, \xi, t) = T - V = \frac{1}{2} \xi^T \mathcal{I} \xi \]

and then apply the Lagrange-D'Alembert Principle which states:

\[ 0 = \delta\int_{t_1}^{t_2} L(g,\xi, t)dt + \int_{t_1}^{t_2} (F \cdot \delta g) dt \]

Where \(F\in \mathfrak{dse}(3)\) is the generalized force on our rigid body \(\delta g\in\mathfrak{se}(3)\) is an arbitrary infinitesimal variation of our system's six degree of freedom state. \(\mathfrak{dse}(3)\) is the dual space of \(\mathfrak{se}(3)\) and is also isomorphic to \(\mathbb{R}^6\). One might guess that the first three components of \(F\) represent the torque on the system in body coordinates and the last three represent the linear force on the system in body coordinates and this is in fact correct. \(F\cdot \delta g\) is the virtual work done by the forces on our system applied along the virtual displacements \(\delta g\). Now, let's start by plugging in the definition of \(\xi\):

\[ 0 = \delta\int_{t_1}^{t_2} \left[ \frac{1}{2}\left( \frac{dg}{dt}\right)^T \mathcal{I} \left(\frac{dg}{dt} \right) \right]dt + \int_{t_1}^{t_2} (F \cdot \delta g) dt \]

Now, let's take the functional derivative \(\delta\) on the first term. Translating this into more familiar terms, we define the derivative like so:

\[ 0 = \frac{d}{d\epsilon}\left[ \int_{t_1}^{t_2} \left[ \frac{1}{2}\left( \frac{dg_\epsilon}{dt} \right)^T \mathcal{I} \left( \frac{dg_\epsilon}{dt} \right) \right]dt \right]_{\epsilon = 0} + \int_{t_1}^{t_2} (F \cdot \delta g) dt \]

Where we have substituted in the varied version of \(g\):

\[ g_\epsilon := g\exp(\epsilon \delta g) \]

Using the chain rule in right tangent space, we have:

\[ \frac{dg_{\epsilon}}{dt} = \text{Ad}_{\exp(-\epsilon \delta g)} \frac{dg}{dt} + \text{Dexp}(\epsilon \delta g) \epsilon \frac{d\delta g}{dt} \]

Where \(\text{Dexp}\) is the differential of the exponential map in right tangent space and \(\text{Ad}_{\exp(-\epsilon \delta g)}\) is the adjoint representation of \(\exp(-\epsilon\delta g)\). Plugging this in and expanding the first term gives:

\[ \begin{aligned} \frac{1}{2}\left( \frac{dg_\epsilon}{dt} \right)^T \mathcal{I} \left( \frac{dg_\epsilon}{dt} \right) =& \frac{1}{2}\left( \text{Ad}_{\exp(-\epsilon \delta g)} \frac{dg}{dt} \right)^T \mathcal{I} \left( \text{Ad}_{\exp(-\epsilon \delta g)} \frac{dg}{dt} \right) \\ &+ \left( \text{Ad}_{\exp(-\epsilon \delta g)} \frac{dg}{dt} \right)^T \mathcal{I} \left( \text{Dexp}(\epsilon \delta g) \epsilon \frac{d\delta g}{dt} \right) + O(\epsilon^2) \end{aligned} \]

Differentiating this with respect to \(\epsilon\) and setting \(\epsilon\rightarrow0\) we get:

\[ \frac{d}{d\epsilon}\left[ \frac{1}{2}\left( \frac{dg_\epsilon}{dt} \right)^T \mathcal{I} \left( \frac{dg_\epsilon}{dt} \right) \right]_{\epsilon=0} = -\left( \text{ad}_{ \delta g} \frac{dg}{dt} \right)^T \mathcal{I} \frac{dg}{dt} + \left( \frac{dg}{dt} \right)^T \mathcal{I} \left( \frac{d \delta g}{dt} \right) \]

This step merits some explanation. Looking at the first term, we recognize it as a quadratic form and compute the derivative of \(\left(\text{Ad}_{\exp(-\epsilon \delta g)} \frac{dg}{dt}\right)\) with respect to \(\epsilon\) first:

\[ \frac{d}{d\epsilon}\left( \text{Ad}_{\exp(-\epsilon \delta g)} \frac{dg}{dt} \right) = -\text{Ad}_{\exp(-\epsilon \delta g)} \text{ad}_{\delta g}\frac{dg}{dt} \]

Then \(\text{Ad}_{\exp(-\epsilon \delta g)}=\text{Ad}_e = I_6\) when \(\epsilon\rightarrow 0\) so this goes away. \(\text{ad}\) is the adjoint representation of the Lie algebra which we get because we differentiated the group adjoint representation \(\text{Ad}\). In the second term, we see that the whole term is multiplied by \(\epsilon\) to begin with, so only one term resulting from the product rule will survive after \(\epsilon\rightarrow 0\). We then use the fact that the adjoints go to the identity and \(\text{Dexp}(0)=\text{Identity}\) also to arrive at the final expression above. Finally, one more manipulation is useful. Using the fact that \(\text{ad}_uv = -\text{ad}_vu\) we have:

\[ \frac{d}{d\epsilon}\left[ \frac{1}{2}\left( \frac{dg_\epsilon}{dt} \right)^T \mathcal{I} \left( \frac{dg_\epsilon}{dt} \right) \right]_{\epsilon=0} = \left( \text{ad}_{\frac{dg}{dt}}\delta g \right)^T \mathcal{I} \frac{dg}{dt} + \left( \frac{dg}{dt} \right)^T \mathcal{I} \left( \frac{d \delta g}{dt} \right) \]

Plugging this into the integral and grouping terms yields:

\[ \begin{aligned} 0 &= \int_{t_1}^{t_2} \left[ \left( \text{ad}_{\frac{dg}{dt}}\delta g \right)^T \mathcal{I} \frac{dg}{dt} + \left( \frac{dg}{dt} \right)^T \mathcal{I} \left( \frac{d \delta g}{dt} \right) \right]dt + \int_{t_1}^{t_2} (F \cdot \delta g) dt \\ 0&= \int_{t_1}^{t_2} \left[ \left( \frac{dg}{dt} \right)^T \mathcal{I} \left( \frac{d \delta g}{dt} \right) \right]dt + \int_{t_1}^{t_2} \left[ \left( F + \text{ad}_\frac{dg}{dt}^T\mathcal{I}\frac{dg}{dt} \right)\cdot \delta g \right] dt \end{aligned} \]

Integrating the first term by parts, we get:

\[ \begin{aligned} 0 &= -\int_{t_1}^{t_2} \left[ \left( \frac{d}{dt}\left[ \mathcal{I}\frac{dg}{dt} \right] \right)\cdot \delta g \right]dt + \int_{t_1}^{t_2} \left[ \left( F + \text{ad}_\frac{dg}{dt}^T\mathcal{I}\frac{dg}{dt} \right)\cdot \delta g \right] dt \\ 0 &= \int_{t_1}^{t_2} \left[ \left( F + \text{ad}_\frac{dg}{dt}^T\mathcal{I}\frac{dg}{dt} - \frac{d}{dt}\left[ \mathcal{I}\frac{dg}{dt} \right] \right)\cdot \delta g \right] dt \end{aligned} \]

Note that the boundary terms are zero when integrating by parts since \(\delta g\) is defined to vanish at \(t_1\) and \(t_2\). Using the fundamental theorem of variational calculus, we note that, because this must be true for any variations \(\delta g\), we have:

\[ \begin{aligned} 0 &= F + \text{ad}_\frac{dg}{dt}^T\mathcal{I}\frac{dg}{dt} - \frac{d}{dt}\left[ \mathcal{I}\frac{dg}{dt} \right] \\ 0&= F + \text{ad}_\xi^T\mathcal{I}\xi - \mathcal{I} \dot \xi \end{aligned} \]

So we have our forward dynamics:

\[ \boxed{\mathcal{I}\dot \xi = F + \text{ad}_\xi^T\mathcal{I}\xi} \]

As a sanity check let's look at what this expression looks like on \(\text{SE}(3)\). For this group we can express elements of the algebra in a basis such that:

\[ \xi = \begin{bmatrix} \omega \\ v \end{bmatrix} \]

Where \(\omega\) and \(v\) are the angular and linear velocity expressed in body coordinates. We know:

\[ \text{ad}_\xi = \begin{bmatrix} \omega_\times & \\ v_\times & \omega_\times \end{bmatrix} \]

Assuming that our inertia tensor \(I\) has the diagonal form shown above, plugging these in yields:

\[ \begin{bmatrix} I_\text{rot} \\ & m \end{bmatrix} \begin{bmatrix} \dot \omega \\ \dot v \end{bmatrix} = \begin{bmatrix} \tau \\ f \end{bmatrix} - \begin{bmatrix} \omega_\times & v_\times\\ & \omega_\times \end{bmatrix} \begin{bmatrix} I_\text{rot} \\ & m \end{bmatrix} \begin{bmatrix} \omega \\ v \end{bmatrix} \]
\[ \begin{aligned} I_\text{rot} \dot \omega &= \tau - \omega \times I_\text{rot}\omega \\ m\dot v &= f - m\omega\times v \end{aligned} \]

The first equation is identifiable as the vector form of Euler's equations for rotation, which is exactly what we expect! Now, if we look at the second equation we have for the linear velocity in body-attached coordinates:

\[ m\dot v = f - m\omega\times v \]

Now, we make a substitution into inertial coordinates. The velocity in inertial coordinates is related to the body-attached velocity by the rotation: \(u=Rv\). From this, we can also derive:

\[ \begin{aligned} \dot u &= \frac{d}{dt}(Rv) = R(\omega \times v) + R\dot v \\ \dot v &= R^T\dot u - \omega\times v \end{aligned} \]

Substituting this into our translational dynamics (along with transforming the force into inertial coordinates \(\varphi=Rf\)) we get:

\[ \begin{aligned} mR^T\dot u - m\omega\times v &= R^T\varphi - m\omega \times v \\ mR^T\dot u &= R^T\varphi \\ m\dot u &= \varphi \end{aligned} \]

In other terms, \(F = ma\), which is Newton's Second Law. Therefore, we see that this single equation is in fact equivalent to the combination of Newton's Second Law and Euler's Equations of rigid body rotation. The expression \(I\dot \xi = F + \text{ad}_\xi^TI\xi\) is actually a generalization of both of these equations and is a more general statement of Newton's Second Law. By choosing \(\mathbb{R}^3\) under addition as a Lie group, one can derive \(F=ma\) immediately, and by choosing \(\text{SO}(3)\) as the Lie group one can derive \(I_\text{rot} \dot \omega = \tau - \omega \times I_\text{rot}\omega\) immediately.

Simulating

We can then express our system's equations of motion like so:

\[ \begin{aligned} \frac{dg}{dt}&=\xi \\ \frac{d\xi}{dt} &= \mathcal{I}^{-1}(F + \text{ad}_\xi^T \mathcal{I}\xi) \end{aligned} \]

This can be integrated using Forward Euler integration by using the Lie group exponential the \(\xi\Delta t\) step to \(g\):

\[ \begin{aligned} g(t_{i+1})&=g(t_i)\exp(\xi_i \Delta t) \\ \xi(t_{i+1})&=\xi(t_i) + \mathcal{I}^{-1}(F + \text{ad}_{\xi_i}^T \mathcal{I})\Delta t \end{aligned} \]

This will give first order accuracy as one would expect from Forward Euler. In the case of \(\text{SE}(3)\), it happens to be the case that integrating \(\xi\) in the above manner is not stable, although it's usually not a big deal if small enough timestamps are used and appropriate dissipative forces are present (e.g. friction or air resistance). Higher-order integration schemes (e.g. Ralston's Method, RK4) can also be employed to mitigate this, although we will leave the exploration of such integrators for another time as some care should be taken in applying them to Lie groups in general.

Handling Gravity

Gravity is a very common force that's relevant when modeling rigid body dynamics, so it's worth taking a quick look on how to handle it. Due to our choice of body coordinates (i.e. coordinates whose origin is the center of mass), this is relatively doable. We know that the force of gravity is a linear force applied to the center of mass. In reference coordinates:

\[ f_{g,\text{ref}} = \begin{bmatrix} 0 \\ 0 \\ -9.8\text{m/s}^2 \cdot m \\ \end{bmatrix} \]

However, to use this force in the equation above, we need the rotation \(R\) associated with \(g\). In particular \(R\) can be defined as the rotation that satisfies \(Rx=gx-g\begin{bmatrix} 0 & 0 & 0\end{bmatrix}^T\). Then we can write the generalized force due to gravity as:

\[ F= \begin{bmatrix} 0 \\ 0 \\ 0 \\ R^{-1}\begin{bmatrix} 0 \\ 0 \\ -9.8\text{m/s}^2\cdot m \\ \end{bmatrix} \end{bmatrix} \]

Where \(R^T = R^{-1}\) can also be used if a matrix representation is employed for \(R\). Note that the first three entries are zero since gravity does not apply a torque to the center of mass. The final three entries are simply the gravitational force rotated into the coordinates of the body.

Bringing it All Together

As a quick illustration of this formulation of rigid body dynamics, here's a quick sim that you can use to explore how such dynamics behave. We're using forward Euler to simulate the rotational degrees of freedom of the above equations of motion, and we've tweaked the translational dynamics integration to enhance stability.

Controls

  • W/S: Pitch
  • A/D: Yaw
  • Q/E: Roll
  • G: Thrust
  • Space: Reset
  • T: Hold to point the engine downwards
  • Arrow Keys: Camera

Acknowledgement

This work is heavily inspired by Lie Group Formulation of Articulated Rigid Body Dynamics by Junggon Kim. I highly recommend this work as a particularly lucid exposition of the topic.