Newton-Euler equations

The Newton-Euler equations of motion correspond to the six unactuated coordinates in the equations of motion of humanoid robots. Newton's equation applies to (linear) translational motions:

\begin{equation*} \displaystyle \sum_{\textrm{link }i} m_i \bfpdd_i = m \bfg + \sum_{\textrm{contact }i} \bff_i \end{equation*}

Meanwhile, Euler's equation applies to angular motions:

\begin{equation*} \displaystyle \sum_{\textrm{link }i} (\bfp_i - \bfp_G) \times m_i \bfpdd_i + \bfI_i \dot{\bfomega}_i + \bfomega_i \times (\bfI_i \bfomega_i) = \sum_{\textrm{contact }i} (\bfp_i - \bfp_G) \times \bff_i + \bftau_i \end{equation*}

Let us detail notations and how these equations are used.

Newton's equation

Positions

Let \(\bfp_i\) denote the position in the world frame of the center of mass of the robot's \(i^{\textrm{th}}\) link. Also, let \(m_i\) denote the mass of link \(i\), and \(m = \sum_i m_i\) the total mass of the robot. The overall center of mass \(G\) is located at the position \(\bfp_G\) in the world frame such that:

\begin{equation*} m \bfp_G = \sum_{\textrm{link }i} m_i \bfp_i \end{equation*}

In other words, the robot's center of mass is a convex combination of the respective centers of mass of all of its links, where each link is weighed by its respective mass.

Forces

The robot is subject to gravity and contact forces. Let \(\bfg\) denote the gravity vector. For a link \(i\) in contact with the environment, we write \(\bff_i\) the resultant force exerted on the link. Let \(\bfh_{ij}\) denote the internal force exerted by link \(i\) on link \(j\). We take the convention that \(\bfh_{ij} = 0\) if links \(i\) and \(j\) are not connected (similarly, \(\bff_i = 0\) if link \(i\) is not in contact). All force vectors are expressed in the world frame.

Humanoid whole-body dynamics involve contact forces and the dynamic momentum

Equation

Newton's equation of motion links the resultant accelerations and forces:

\begin{equation*} \sum_{\textrm{link } i} m_i \bfpdd_i = \sum_{\textrm{link } i} m_i \bfg + \bff_i + \sum_{\textrm{link } j \neq i} \bfh_{ij} \end{equation*}

A key point here is that \(\bfh_{ij} = - \bfh_{ji}\) by Newton's third law of motion, so that internal forces vanish in this sum over all links: \(\sum_i \sum_{j \neq i} \bfh_{ij} = 0\). In concise form, Newton's equation then binds the acceleration of the center of mass with the whole-body resultant force.

Momentum

The linear momentum of the robot is defined by:

\begin{equation*} \bfP := m \bfpd_G \end{equation*}

Then, Newton's equation can be written in concise form as:

\begin{equation*} \dot{\bfP} = m \bfpdd_G = m \bfg + \sum_{\textrm{contact } i} \bff_i \end{equation*}

In other words, the rate of change of the linear momentum is equal to the resultant of external forces exerted on the robot.

Euler's equation

Newton's equation is related to translational motions of the robot. Euler's equation provides a similar relation for angular motions.

Positions

Let \(\bfR_i\) denote the rotation matrix from the \(i^{\textrm{th}}\) link frame to the world frame, and \(\bfomega_i\) denote the spatial angular velocity of the link, that is, the angular velocity from the link frame to the world frame expressed in the world frame. Also, let \(\bfI_i\) denote the inertia matrix of the \(i^{\textrm{th}}\) link, expressed in the world frame and taken at the center of mass \(\bfp_i\) of the link. (Yes, it is verbose to be precise if we want to define all our spatial vectors without ambiguity.)

Forces

For a link \(i\) in contact with the environment, we write \(\bftau_i\) the resultant moment of contact forces exerted on the link at \(\bfp_i\). If the link is in point contact with the environment at \(\bfp_i\), the moment will be zero. If the link is in surface contact, both \(\bff_i\) and \(\bftau_i\) may be non-zero. See for instance the Section III of this paper for more details on point and surface contacts.

Equation

Euler's equation of motion links the angular momenta and resultant moments of external forces:

\begin{equation*} \sum_{\textrm{link }i} (\bfp_i - \bfp_G) \times m_i \bfpdd_i + \bfI_i \dot{\bfomega}_i + \bfomega_i \times (\bfI_i \bfomega_i) = \sum_{\textrm{link }i} (\bfp_i - \bfp_G) \times (\bff_i + m_i \bfg) + \bftau_i \end{equation*}

As a consequence of the definition of the center of mass as a convex combination of links' centers of mass, terms involving the acceleration \(\bfg\) due to gravity vanish from the right-hand side of this equation:

\begin{equation*} \sum_{\textrm{link }i} (\bfp_i - \bfp_G) \times m_i \bfg = 0 \end{equation*}

This leaves us with:

\begin{equation*} \sum_{\textrm{link }i} (\bfp_i - \bfp_G) \times m_i \bfpdd_i + \bfI_i \dot{\bfomega}_i + \bfomega_i \times (\bfI_i \bfomega_i) = \sum_{\textrm{contact }i} (\bfp_i - \bfp_G) \times \bff_i + \bftau_i \end{equation*}

Note how the right-hand summation is now over contacts only rather than over links. By a similar argument to the vanishing of internal forces in the translational case, the moments of internal forces do not appear in the equation above. For details on this, see D'Alembert's principle which is the axiom behind the vanishing of internal forces.

Momentum

The angular momentum of the robot, taken at the center of mass \(G\), is defined by:

\begin{equation*} \bfL_G := \sum_{\textrm{link }i} (\bfp_i - \bfp_G) \times m_i \bfpd_i + \bfI_i \bfomega_i \end{equation*}

Then, Euler's equation can be written in concise form as:

\begin{equation*} \dot{\bfL}_G = \sum_{\textrm{contact }i} (\bfp_i - \bfp_G) \times \bff_i + \bftau_i \end{equation*}

In other words, the rate of change of the angular momentum is equal to the resultant moment of external forces exerted on the robot.

Variant with body angular vectors

We took care to specify all our spatial vectors and matrices above with respect to the world frame. Let us denote by \(\bfomega_i^\ell = R_i^\top \bfomega_i\) the body angular velocity of the \(i^{\textrm{th}}\) link, that is, the angular velocity from the link frame to the world frame expressed in the link frame. Similarly, let \(\bfI_i^\ell = \bfR_i^\top \bfI_i \bfR_i\) denote the inertia matrix of the \(i^{\textrm{th}}\) link, expressed in the link frame and taken at the center of mass \(\bfp_i\) of the link, and let \(\bftau_i^\ell = \bfR_i^\top \bftau_i\) denote the moment of contact forces with respect to \(\bfp_i\) in the link frame. Euler's equation can equivalently be written as:

\begin{equation*} \sum_{\textrm{link }i} (\bfp_i - \bfp_G) \times m_i \bfpdd_i + \bfR_i ( \bfI_i^\ell \dot{\bfomega}_i^\ell + \bfomega_i^\ell \times (\bfI_i^\ell \bfomega_i^\ell)) = \sum_{\textrm{contact }i} (\bfp_i - \bfp_G) \times \bff_i + \bftau_i^\ell \end{equation*}

You can check this formula as an exercise.

Variant at the origin of the world frame

Together, the linear and angular momentum are the wrench coordinates of the robot's momentum. The linear momentum is the resultant of the wrench, hence its coordinates are the same everywhere, while the angular momentum is the moment of the wrench, whose coordinates depend on the application point. We selected it so far as the center of mass \(G\), but we can apply Varignon formula to compute the angular momentum at any other point. For instance, the origin \(O\) of the world frame:

\begin{equation*} \bfL_O = \bfL_G + \bfp_G \times \bfP = \sum_{\textrm{link }i} \bfp_i \times m_i \bfpd_i + \bfI_i \bfomega_i \end{equation*}

Then, Euler's equation can be written in concise form as:

\begin{equation*} \dot{\bfL}_O = \sum_{\textrm{link }i} \bfp_i \times (\bff_i + m_i \bfg) + \bftau_i \end{equation*}

Equivalently, expanding the expression of the angular momentum:

\begin{equation*} \sum_{\textrm{link }i} \bfp_i \times m_i \bfpdd_i + \bfI_i \dot{\bfomega}_i + \bfomega_i \times (\bfI_i \bfomega_i) = \sum_{\textrm{link }i} \bfp_i \times (\bff_i + m_i \bfg) + \bftau_i \end{equation*}

The benefit of taking the angular momentum at the center of mass is to simplify the right-hand side of this equation from a summation over links, with gravity applied to each, to a summation over contacts, where only contact forces are applied.

Newton-Euler with spatial vector algebra

When working with spatial vector algebra and wrench friction cones, it is common to express the left- and right-hand sides of Newton-Euler equations as wrenches.

Contact and gravito-inertial wrenches

The contact wrench \(\bfw^c_G = (\bff^c, \bftau^c_G)\) corresponds to the sum of external contact forces. Its resultant \(\bff^c\) and moment \(\bftau^c_G\) at the center of mass are given by:

\begin{equation*} \begin{array}{rcl} \displaystyle \bff^c & = & \displaystyle \sum_{\textrm{contact }i} \bff_i \\ \displaystyle \bftau^c_G & = & \displaystyle \sum_{\textrm{contact }i} (\bfp_i - \bfp_G) \times \bff_i + \bftau_i \end{array} \end{equation*}

The gravito-inertial wrench \(\bfw^{gi}_G = (\bff^{gi}, \bftau_G^{gi})\) combines the effects of gravity and link accelerations. Its resultant \(\bff^{gi}\) and moment \(\bftau^{gi}_G\) at the center of mass are given by:

\begin{equation*} \begin{array}{rcl} \displaystyle \bff^{gi} & = & \displaystyle \sum_{\textrm{link }i} m_i (\bfg - \bfpdd_i) \\ \bftau^{gi}_G & = & \displaystyle \sum_{\textrm{link }i} (\bfp_i - \bfp_G) \times m_i (\bfg - \bfpdd_i) - \bfI_i \dot{\bfomega}_i - \bfomega_i \times (\bfI_i \bfomega_i) \end{array} \end{equation*}

Since \(\sum_i m_i (\bfp_i - \bfp_G) \times \bfg = \boldsymbol{0}\) the expression of the moment above can be simplified, but this form shows how the net gravito-inertial wrench is the sum of individual gravito-inertial wrenches over all links. As expected, when the robot is not moving, the gravito-inertial wrench reduces to the gravity wrench.

Newton-Euler as a wrench equation

In terms of these two wrenches, Newton-Euler equations become:

\begin{equation*} \bfw^c + \bfw^{gi} = \boldsymbol{0} \end{equation*}

That is, at any point in time, the sum of gravity, inertial and external contact wrenches is always zero. When the robot is not moving, that is the inertial wrench is zero, we recognize the condition for static equilibrium: external contact wrenches compensate gravity. When the robot is moving, the condition generalizes to dynamic equilibrium: external contact wrenches compensate for both gravity and inertial forces.

Note that this is an equality between wrenches, which means it holds for any application point:

\begin{equation*} \forall A, \bfw^c_A + \bfw^{gi}_A = \bfzero \end{equation*}

To prove that two wrenches are equal, we need them to have (1) the same resultant force (true from Newton's equation) and (2) the same moment at a given point (true from Euler's equation at the center of mass). In particular, the equation holds for the origin \(O\) in the world frame:

\begin{equation*} \bfw^c_O + \bfw^{gi}_O = \bfzero \end{equation*}

Which results in the same Newton and Euler equations we derived above:

\begin{align} \sum_{\textrm{link }i} m_i (\bfpdd_i - \bfg) & = \sum_{\textrm{contact }i} \bff_i \\ \sum_{\textrm{link }i} \bfp_i \times m_i (\bfpdd_i - \bfg) + \bfI_i \dot{\bfomega}_i + \bfomega_i \times (\bfI_i \bfomega_i) & = \sum_{\textrm{contact }i} \bfp_i \times \bff_i + \bftau_i \end{align}

One advantage of this equivalent pair of equations is that, while left-hand side summations are over all links, right-hand side summations are only over contacts.

To go further

My favorite introduction to humanoid equations of motion is Some comments on the structure of the dynamics of articulated motion by Wieber (2006). Check it out if you are looking for a derivation of Newton and Euler equations from Gauss's principle of least constraint. As a matter of fact, check it out even if you are only mildly curious about it. I warmly recommend it.

The link between Newton-Euler equations, the gravito-inertial and contact wrenches is central to the derivation of wrench friction cones and their projection for reduced dynamic models such as ZMP support areas. These are useful e.g. for locomotion over complex terrains yet with simple models.

Expressing Newton-Euler equations at the center of mass can be a practical idea to linearize the (otherwise non-convex) problem of integrating forward dynamics. For instance, Di Carlo et al. (2018) started from there to linearize the model predictive control problem for the MIT Cheetah quadruped.

Discussion

There are no comments yet. Feel free to leave a reply using the form below.

Post a comment

You can use Markdown with $\LaTeX$ formulas in your comment.

You agree to the publication of your comment on this page under the CC BY 4.0 license.

Your email address will not be published.

© Stéphane Caron — All content on this website is under the CC BY 4.0 license.
τ