# Constrained equations of motion

Let us take a closer look at how Jacobian-transpose term appears in the equations of motion of legged robots in contact with their environment:

\begin{equation*} \bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ_c^\top \bff \end{equation*}

The Jacobian-transpose term maps external contact forces to joint torques (for actuated coordinates) and to the net floating-base wrench (for unactuated coordinates). We will see how to derive it from the principle of least constraint, using the method of Lagrange multipliers.

## Constraint derivatives We consider a legged robot, for example the humanoid depicted in the figure to the right, making point contacts or surface contacts with its environment. These kinematic constraints can be summarized into a holonomic constraint $$\bfc(\bfq) = \bfzero$$. In the case of point contacts, it would be the stack of contact point equality constraints $$\bfp_C(\bfq) = \bfp_{D}$$, with $$\bfp_C$$ the mobile point on the robot's foot and $$\bfp_D$$ the ground contact point in the inertial frame. Differentiating $$\bfc(\bfq) = \bfzero$$ twice with respect to time yields:

\begin{equation*} \bfJ_c(\bfq) \qdd + \qd^\top \bfH_c(\bfq) \qd = \boldsymbol{0} \end{equation*}

where the constraint Jacobian and Hessian matrices are defined by:

\begin{align} \bfJ_c(\bfq) & = \frac{\partial \bfc}{\partial \bfq}(\bfq) & \bfH_c(\bfq) & = \frac{\partial^2 \bfc}{\partial \bfq^2}(\bfq) \end{align}

The Jacobian is a $$3 k \times n$$ matrix, with $$k$$ the number of contact poins and with $$n$$ the degree of freedom of our robot (number of actuated joints plus six), while the Hessian is an $$n \times 3 k \times n$$ tensor. Notations are the same for surface contacts, except that the degree of constraint for each contact is six rather than three.

## Principle of least constraint

Remember how, in the absence of constraint, the equations of free motion of our system would be:

\begin{equation*} \bfM(\bfq) \qdd_{\mathrm{free}} + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_{\mathit{ext}} \end{equation*}

where:

• $$\bfM(\q)$$ is the $$n \times n$$ joint-space inertia matrix,
• $$\bfC(\q)$$ is the $$n \times n \times n$$ joint-space Coriolis tensor,
• $$\bftau$$ is the $$n$$-dimensional vector of actuated joint torques,
• $$\bftau_g(\q)$$ is the $$n$$-dimensional vector of gravity torques,
• $$\bftau_{\mathit{ext}}$$ is an optional vector of external forces and torques.

We know how to compute these matrices and vectors from link inertias and Jacobians. However, when the robot is in contact, its motion doesn't follow the vector $$\qdd_{\mathrm{free}}$$ defined by this equation because of the additional contact constraint: the actual $$\qdd$$ has to be such that

\begin{equation*} \bfJ_c(\bfq) \qdd + \qd^\top \bfH_c(\bfq) \qd = \boldsymbol{0} \end{equation*}

Gauss's principle of least constraint states that the robot's actual acceleration $$\qdd$$ is as close as possible to the free acceleration $$\qdd_{\mathrm{free}}$$, subject to this constraint:

\begin{equation*} \begin{array}{rl} \qdd = \arg\min_{\bfa} & \frac{1}{2} \| \bfa - \qdd_{\mathrm{free}}\|_{\bfM}^2 \\ \textrm{subject to} & \bfJ_c(\bfq) \bfa + \qd^\top \bfH_c(\bfq) \qd = \boldsymbol{0} \end{array} \end{equation*}

Note how the inertia matrix appears in the objective function to define what as close as possible means here. The inertia matrix is positive definite so this objective function is strictly convex.

### Introducing contact forces

We can write the Lagrangian of this equality-constrained least-squares problem as:

\begin{equation*} \calL(\bfa, \bflambda) = \frac{1}{2} (\bfa - \qdd_{\mathrm{free}})^\top \bfM (\bfa - \qdd_{\mathrm{free}}) - \bflambda^\top \left(\bfJ_c \bfa + \qd^\top \bfH_c \qd\right) \end{equation*}

where we omit dependencies on $$\bfq$$ for concision. Dual variables stacked in the vector $$\bflambda$$ correspond to contact forces. Why? By definition! In Lagrangian mechanics, there are two kinds of forces: constraint and non-constraint forces. Non-constraint forces are somehow easier to deal with: they correspond to the vectors $$\bftau_g(\bfq)$$ and $$\bftau_{\mathit{ext}}$$ in the equations of free motion, and we already know how to compute them. Contact forces are the constraint forces arising from our kinematic contact constraint.

We can think of forces as dual variables that appear in the equations of motion to ensure that the system's motion ends up satisfying the kinematic constraint. Our assumption here is that the constraint will hold, i.e. that the robot will stay in contact with its environment. In practice this is not true for any command: if we send maximum joint torques, our robot will likely shake and jump all over the place, breaking the assumption of fixed contact. To make sure that this assumption holds, we will complement our kinematic constraint with a dynamic contact stability condition, but this is another story. Let us get back to our derivation.

### Method of Lagrange multipliers

From the method of Lagrange multipliers, the optimum $$(\bfa^*, \bflambda^*)$$ of our constrained problem is a critical point of the Lagrange function:

\begin{equation*} \begin{array}{ccc} \displaystyle \frac{\partial \calL}{\partial \bfa}(\bfa^*, \bflambda^*) = \bfzero & \wedge & \displaystyle \frac{\partial \calL}{\partial \bflambda}(\bfa^*, \bflambda^*) = \bfzero \end{array} \end{equation*}

The second equation is, by construction, our contact constraint. The first one gives us:

\begin{equation*} \bfM (\bfa^* - \qdd_{\mathrm{free}}) - \bfJ_c^T \bflambda^* = \bfzero \end{equation*}

In fine, replacing $$\bfa^*$$ by our notation for joint accelerations $$\qdd$$ and similarly $$\bflambda^*$$ by $$\bff$$, we obtain the constrained equations of motion in joint space:

\begin{equation*} \bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ_c^\top \bff \end{equation*}

with $$\bfM(\q)$$ the joint-space inertia matrix, $$\bfC(\q)$$ the joint-space Coriolis tensor, $$\bftau_g(\q)$$ the vector of gravity torques, $$\bftau$$ the vector of actuated joint torques, $$\bfJ_c$$ the stacked Jacobian of kinematic contact constraints and $$\bff$$ the corresponding stacked vector of contact forces.

## To go further

This page is based on Wieber's comments on the structure of the dynamics of articulated motion, as well as on very good a summary in Section IV.A of this paper by Budhiraja et al.

Forces are virtual quantities that make our calculations convenient. Defining them as the dual multipliers of motion constraints is a key conceptual step in Lagrangian mechanics. One way to think about this step is to ponder the more general question: "What does it mean to define [force, mass, charge, ...]?" Poincaré's Science and Hypothesis (1902) shines a refreshing light on this topic.