Robot Dynamics: Hamiltonian formalism & AutoDiff

As promised in this series, we dive into the last piece: how to use Hamiltonian mechanics1 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:

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

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 package2 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.

References