# Understanding Physics Engines: Dynamic Models of Manipulators

Physics engines simulate a carefully approximated and idealised version of the real world. In recent times, the popularity of physics simulators has only increased among machine learning and computer vision practicioners. Being able to forward simulate the evolution of a physical world as a function of time — under certain set of assumptions and approximations — allows for optimising a control policy to carry out tasks. In this post, we will dive into the underlying maths and derive the fundamental equation that all physics engines implement to simulate dynamic model of an articulated manipulator, *i.e.*
how does the applied torque relate to the joint velocity and acceleration given joint properties like mass, friction and gravity. It is formulated as:

where

- is the vector of generalized coordinates, for instance the vector of joint-angles for a manipulator,
- is the velocity of ,
- is the acceleration of ,
- is the generalised inertia matrix,
- is matrix and is vector of centripetal and coriolis terms,
- is vector of gravity terms, and
- is vector of joint torques.

This equation provides the relation between the applied forces/torques and the resulting motion of a manipulator. Similar to kinematics, it is also possible to define two dynamics “models”:

**Forward model:** once the forces/torques applied to the joints, as well as the joint positions and velocities are known, compute the joint accelerations:

and then

**Inverse model:** once the joint accelerations, velocities and positions are known, compute the corresponding forces/torques

This equation can be derived using either Newton-Euler method or the Lagrangian mechanics via Euler-Lagrange which is what we do here in this post. The Largrangian mechanics deals with energies of the system and from physics we know that it is possible to define:

- The kinetic energy of the system,
- Potential energy of the system,

The Lagrangian is .

One may think of a physical system, changing as time goes on from one state or configuration to another, as progressing along a particular evolutionary path, and ask, from this point of view, why it selects that particular path out of all the paths imaginable. The answer is that the physical system sums the values of its Lagrangian function for all the points along each imaginable path and then selects that path with the smallest result. The Lagrangian function measures something analogous to increments of distance, in which case one may say, in an abstract way, that physical systems always take the shortest paths. Source: https://www.britannica.com/science/Lagrangian-function

The Euler-Lagrange equations are defined as

Let us denote the joint by with being the non-conservative (external or dissipative) generalised forces performing any work on the joints . It can be decomposed into:

- , the joint actuator torque.
- , the term due to external forces.
- , joint friction torque.

Therefore, it can be written as . Since the potential energy does not depend on the velocity, the euler-lagrange equation can be further simplied as

Where .

## Computing Kinetic Energy

For any rigid body B, the *mass* can be computed by integrating the mass density as:

where the term denotes the mass density and in some cases can be assumed constant, . The *center of mass* (CoM) can be computed as:

The overall kinectic energy can be then written as:

We know that the velocity of a any point $\mathbf{p}$ on a body undergoing motion in 3D can be written as

Denoting by and writing the cross product as matrix vector product *i.e.* , the overall kinetic energy can be re-written as:

The expression sums to 0 *i.e.*

Further, using the identity , we can rewrite the second term in the kinectic energy as:

where the matrix is the *body inertia matrix*. The matrix is the *Euler matrix* and is:

The inertia matrix is:

Therefore, the kinetic energy can be compactly written as:

This is also known as *Konig Theorem*. Thus, the kinect energy of an n-dof manipulator is

where

- is the mass of the i-th link.
- is the linear velocity of the center of mass and is the rotatinal velocity of the link.
- is the inertial matrix computed in a fixed reference frame attached to the center of the mass.
- is the rotation matrix of the link with respect to the fixed base frame .

**Deriving the velocity of the center of mass**

Given the kinetic energy expression derived above we’d like to be able to obtain the velocities of center of masses of all the joints of the manipulator. Denoting as the position of the end effector with respect to the base frame we can obtain the expression for it as a function of forward dynamics and joint angles vector

Differentiating this we can get velocity of end-effector position as a function of angles vector

Denoting the velocity as , we can express it recursively for any link as

Assuming the end-effector frame is denoted by , the velocity of the end-effector can be re-written as

Let us denote to be the axis of rotation of joint . We can rewrite the angular velocity of joint wrt to as

Also, we know that

Therefore, the angular velocity of link can be written as

Plugging this expression back into the link velocity equation we get

Similarly, the velocity of any joint can be expressed as

Rewriting expression (22) as a vector dot product, we get

Therefore, the Kinectic energy can be rewritten as

The denotes the Jacobian of the link with respect to the base.

## Computing the Potential Energy

A rigid body under the influence of gravity has a potential energy. For any generic link of an n-dof manipulator, it can be expressed as:

The overall potential energy of the system is therefore

## Putting together

Remember that the Euler-Lagrange gives us the following:

The Lagrangian function is:

and

Recall that is a function of and therefore, the time-derivative of would require a total derivative *i.e.*

and therefore

Furthermore

If we define as

and as

We can then rewrite the above expression as

Rewriting this in matrix form, we get

## Some explanation of these terms

The terms are only a function of joint positions so they can be pre-computed once the joint manipulator configuration is known.

is the *moment of inertia* about the k-th joint *i.e.* inertia at joint when the joint accelerates. is the inertia coupling, which captures the effect of acceleration of joint on joint i.e. the inertia seen at joint when joint accelerates.

accounts for the *centrifugal effect* induced on joint by the velocity of joint . is the *coriolis effect* induced on joint by the velocity of joints and .

represents the torque generated on joint by the gravity force acting on the manipulator in the current configuration.

## Authors

Ankur Handa

## Acknowledgements

We’d like to thank Erwin Coumans for typos and suggestions.

## Bibliography

https://conversationofmomentum.wordpress.com/2014/08/05/euler-lagrange-equations/

https://www.ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/6-dynamics.pdf

How physics engines work https://www.haroldserrano.com/blog/how-a-physics-engine-works-an-overview

https://scaron.info/teaching/equations-of-motion.html

Principle of least action: https://www.feynmanlectures.caltech.edu/II_19.html

Dynamic model of robot manipulators http://campus.unibo.it/218782/25/FIR_05_Dynamics.pdf, Claudio Melchiorri, Universita di Bologna