# Robot Dynamics: Hamiltonian formalism & AutoDiff

07 Jul 2019As promised in this series, we dive into the last piece: how to use Hamiltonian mechanics^{1} and automatic differentiation to simulate any mechanical system given its description and assuming no loss of energy (no dissipation modeled).

We need to define three components for our system to be automagically solved using Hamiltonian equations and automatic differentiation:

- the dynamics in Cartesian space expressed as a function of a minimal generalized coordinates set,
- the inertia matrix in Cartesian space,
- the potential energy of the system as a function of the same generalized coordinates set used above.

# Dynamics

Let’s consider again our toy problem of describing the motion of a N-link planar pendulum in 2D Cartesian space. Its dynamics in Cartesian space are a function of the pivot link angle such that:

where $l_n$ is the link of that pendulum’s link and $\theta_i$ is the link’s pivot angle. So the set of angles $\theta_i$ gives us our set of generalized coordinates $q_i$.

# Inertia Matrix

For a perfect pendulum its mass is given by each link’s masses such that only the diagonal is non-zero:

where $m_i$ is the mass of a given link in the pendulum.

# Potential Energy

Assuming the pendulum rests at zero angles in the direction of gravity, the potential energy is given by:

where $g$ is the Earth’s gravity constant and y(q) is the position of the pendulum in the y-axis as described by its dynamics f(q) above.

# Combining it all together

Mixing it all together we end up with

- the jacobian of the dynamics by AD:

- and the associated Hessian by one more AD step:

- from that we can derive the evolution of the momentum of our system over time:

- and we can deduce the evolution of the system over time since:

by integrating those 1^{st} order differential equations, we can simulate our system given its initial conditions.

In Python, this can be expressed as follows:

This is exactly what I implemented for CVRA in a custom package^{2} to simulate the 3-link pendulum of our robot’s arm.

# Conclusion

So in this series, we’ve learned how to simulate any mechanical conservative system with minimal amount of math derived by hand. This was useful to simulate a 3-link pendulum in Python. This article also concludes my series until I have figured out the maths to simulate dissipative energies in a system with Hamiltonian mechanics.