Rigid body dynamics simulation

Adithya Ramesh.
Published at Learning for Dynamics & Control Conference (L4DC), 2023.

Paper | Code

Abstract

We focus on the robotics domain, mainly systems undergoing rigid body motion such as robotic manipulators. For such systems, it is easier to derive the equations of motion using Lagrangian or Hamiltonian mechanics rather than Newtonian mechanics. Conventionally, the Lagrangian approach is taken.

Environments

We focus on robotic systems undergoing rigid body motion without contacts. We also assume that there is no friction. The environments considered are shown below. We implement our own simulations from first principles.

In future work, we plan to include both friction as well as contacts.

Lagrangian Mechanics

These systems obey Lagrangian mechanics. Their state consists of generalized coordinates $\textbf{q}$, which describe the configuration of the system, and generalized velocities $\dot{\textbf{q}}$, which are the time derivatives of $\textbf{q}$. Let the motor torques be $\boldsymbol\tau$. The Lagrangian equations of motion are given by, \[\textbf{M}(\textbf{q}) \, \ddot{\textbf{q}} + \textbf{C}(\textbf{q},\dot{\textbf{q}}) \, \dot{\textbf{q}} + \textbf{G}(\textbf{q}) = \boldsymbol\tau\]

where, $\textbf{M}(\textbf{q})$ is the mass matrix, $\textbf{C}(\textbf{q},\dot{\textbf{q}}) \, \dot{\textbf{q}} = \frac{\partial }{\partial \textbf{q}} \big(\textbf{M}(\textbf{q})\, \dot{\textbf{q}} \big) \, \dot{\textbf{q}} - \frac{\partial }{\partial \textbf{q}} \big( \frac{1}{2} \, \dot{\textbf{q}}^{T} \, \textbf{M}(\textbf{q})\, \dot{\textbf{q}} \big)$ is the centripetal / Coriolis term and $\textbf{G}(\textbf{q}) = \frac{\partial \mathcal{V}(\textbf{q})}{\partial \textbf{q}}$ is the gravitational term, where $\mathcal{V}(\textbf{q})$ is the potential energy.

Sympy kinematics model

Numerical simulation

RK1 / Euler integration

\(\begin{bmatrix}\boldsymbol{q}_{t+1} \\ \dot{\boldsymbol{q}}_{t+1}\end{bmatrix} = \begin{bmatrix}\boldsymbol{q}_{t} \\ \dot{\boldsymbol{q}}_{t}\end{bmatrix} + \Delta_{t} \, f(\boldsymbol{q}_{t}, \dot{\boldsymbol{q}}_{t}, \boldsymbol{\tau})\)

RK2

\(\boldsymbol{k1} = f(\boldsymbol{q}_{t}, \dot{\boldsymbol{q}}_{t}, \boldsymbol{\tau})\\ \boldsymbol{k2} = f((\boldsymbol{q}_{t}, \dot{\boldsymbol{q}}_{t}) + \Delta_{t} \, k1, \boldsymbol{\tau})\\ \begin{bmatrix}\boldsymbol{q}_{t+1} \\ \dot{\boldsymbol{q}}_{t+1}\end{bmatrix} = \begin{bmatrix}\boldsymbol{q}_{t} \\ \dot{\boldsymbol{q}}_{t}\end{bmatrix} + \Delta_{t} \, (k1+k2)\)

RK4

\(\boldsymbol{k1} = f(\boldsymbol{q}_{t}, \dot{\boldsymbol{q}}_{t}, \boldsymbol{\tau})\\ \boldsymbol{k2} = f((\boldsymbol{q}_{t}, \dot{\boldsymbol{q}}_{t}) + \frac{\Delta_{t}}{2} \, k1, \boldsymbol{\tau})\\ \boldsymbol{k3} = f((\boldsymbol{q}_{t}, \dot{\boldsymbol{q}}_{t}) + \frac{\Delta_{t}}{2} \, k2, \boldsymbol{\tau})\\\boldsymbol{k4} = f((\boldsymbol{q}_{t}, \dot{\boldsymbol{q}}_{t}) + \Delta_{t} \, k3, \boldsymbol{\tau})\\ \begin{bmatrix}\boldsymbol{q}_{t+1} \\ \dot{\boldsymbol{q}}_{t+1}\end{bmatrix} = \begin{bmatrix}\boldsymbol{q}_{t} \\ \dot{\boldsymbol{q}}_{t}\end{bmatrix} + \frac{\Delta_{t}}{6} \, (k1+2\,k2+2\,k3+k4)\)

Energy check

To verify the correctness of the simulation, we plot the total energy as a function of time.

Videos

We show the behaviours learnt by our physics-informed model-based RL approach below.