# 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:

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:

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:

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

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:

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:

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:

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

*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:

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 a very good summary in Section IV.A of this paper by Budhiraja *et al*.

Forces are virtual quantities that balance out our calculations. Defining them as the dual multipliers of motion constraints is a key step in Lagrangian mechanics: constraints "pull" on the system, which aspires to be somewhere else according to the principle of least motion, and multipliers can be seen as a measure of how hard the constraints need to pull to keep the system on track. Check out Jensen's explanation on the meaning of the multiplier for some great examples to build up intuition around this.