The Manipulator Equations

If you crank through the Lagrangian dynamics for a few simple robotic manipulators, you will begin to see a pattern emerge - the resulting equations of motion all have a characteristic form. For example, the kinetic energy of your robot can always be written in the form: $\begin{matrix} (2) & T = \frac{1}{2} {\dot{q}}^{T} M (q) \dot{q}, \end{matrix}$ where $M$ is the state-dependent inertia matrix (aka mass matrix). This observation affords some insight into general manipulator dynamics - for example we know that $M$ is always positive definite, and symmetric[3, p.107] and has a beautiful sparsity pattern[4] that we should take advantage of in our algorithms.

Continuing our abstractions, we find that the equations of motion of a general robotic manipulator (without kinematic loops) take the form $\begin{matrix} (3) & M (q) \ddot{q} + C (q, \dot{q}) \dot{q} = τ_{g} (q) + B u, \end{matrix}$ where $q$ is the joint position vector, $M$ is the inertia matrix, $C$ captures Coriolis forces, and $τ_{g}$ is the gravity vector. The matrix $B$ maps control inputs $u$ into generalized forces. Note that we pair $M \ddot{q} + C \dot{q}$ together on the left side because these terms together represent the force of inertia; the contribution from kinetic energy to the Lagrangian: $\frac{d}{d t} {\frac{\partial T}{\partial \dot{q}}}^{T} - {\frac{\partial T}{\partial q}}^{T} = M (q) \ddot{q} + C (q, \dot{q}) \dot{q} .$ Whenever I write Eq. ( $3$ ), I see $m a = f$ .

Manipulator Equation form of the Simple Double Pendulum

The equations of motion from Example 1 can be written compactly as:

\begin{aligned} M (q) = & [\begin{array}{c} (m_{1} + m_{2}) l_{1}^{2} + m_{2} l_{2}^{2} + 2 m_{2} l_{1} l_{2} c_{2} & m_{2} l_{2}^{2} + m_{2} l_{1} l_{2} c_{2} \\ m_{2} l_{2}^{2} + m_{2} l_{1} l_{2} c_{2} & m_{2} l_{2}^{2} \end{array}] \\ C (q, \dot{q}) = & [\begin{array}{c} 0 & - m_{2} l_{1} l_{2} (2 {\dot{q}}_{1} + {\dot{q}}_{2}) s_{2} \\ \frac{1}{2} m_{2} l_{1} l_{2} (2 {\dot{q}}_{1} + {\dot{q}}_{2}) s_{2} & - \frac{1}{2} m_{2} l_{1} l_{2} {\dot{q}}_{1} s_{2} \end{array}] \\ τ_{g} (q) = & - g [\begin{array}{c} (m_{1} + m_{2}) l_{1} s_{1} + m_{2} l_{2} s_{1 + 2} \\ m_{2} l_{2} s_{1 + 2} \end{array}], B = [\begin{array}{c} 1 & 0 \\ 0 & 1 \end{array}] \end{aligned}

Note that this choice of the

C

matrix was not unique.

The manipulator equations are very general, but they do define some important characteristics. For example, $\ddot{q}$ is (state-dependent) linearly related to the control input, $u$ . This observation justifies the control-affine form of the dynamics assumed throughout the notes. There are many other important properties that might prove useful. For instance, we know that the $i$ element of $C \dot{q}$ is given by [3, §5.2.3]: ${[C (q, \dot{q}) \dot{q}]}_{i} = {\dot{q}}^{T} {\bar{C}}_{i} \dot{q}, {\bar{C}}_{i; j, k} (q) = \frac{1}{2} [\frac{\partial M_{i j}}{\partial q_{k}} + \frac{\partial M_{i k}}{\partial q_{j}} - \frac{\partial M_{j k}}{\partial q_{i}}],$ (e.g. it also quadratic in $\dot{q}$ ). For the appropriate choice of $C$ , we have that $\dot{M} (q) - 2 C (q, \dot{q})$ is skew-symmetric[5, §9.1].

Note that we have chosen to use the notation of second-order systems (with $\dot{q}$ and $\ddot{q}$ appearing in the equations) throughout this book. Although I believe it provides more clarity, there is an important limitation to this notation: it is impossible to describe 3D rotations in "minimal coordinates" using this notation without introducing kinematic singularities (like the famous "gimbal lock"). For instance, a common singularity-free choice for representing a 3D rotation is the unit quaternion, described by 4 real values (plus a norm constraint). However we can still represent the rotational velocity without singularities using just 3 real values. This means that the length of our velocity vector is no longer the same as the length of our position vector. For this reason, you will see that most of the software in Drake uses the more general notation with $v$ to represent velocity, $q$ to represent positions, and the manipulator equations are written as $\begin{matrix} (4) & M (q) \dot{v} + C (q, v) v = τ_{g} (q) + B u, \end{matrix}$ which is not necessarily a second-order system. $v$ and $\dot{q}$ are related linearly by $\dot{q} = N (q) v$ . Although $N (q)$ is not necessarily square, its left Moore-Penrose pseudo-inverse $N^{+} (q)$ can be used to invert that relationship without residual error, provided that $\dot{q}$ is in the range space of $N (q)$ (that is, if it could have been produced as $\dot{q} = N (q) v$ for some $v$ ). See [6] for a nice discussion of this topic.

Recursive Dynamics Algorithms

The equations of motions for our machines get complicated quickly. Fortunately, for robots with a tree-link kinematic structure, there are very efficient and natural recursive algorithms for generating these equations of motion. For a detailed reference on these methods, see [7]; some people prefer reading about the Articulated Body Method in [8]. The implementation in Drake uses a related formulation from [9].

Bilateral Position Constraints

If our robot has closed-kinematic chains, for instance those that arise from a four-bar linkage, then we need a little more. The Lagrangian machinery above assumes "minimal coordinates"; if our state vector $q$ contains all of the links in the kinematic chain, then we do not have a minimal parameterization -- each kinematic loop adds (at least) one constraint so should remove (at least) one degree of freedom. Although some constraints can be solved away, the more general solution is to use the Lagrangian to derive the dynamics of the unconstrained system (a kinematic tree without the closed-loop constraints), then add additional generalized forces that ensure that the constraint is always satisfied.

Consider the constraint equation $\begin{matrix} (5) & h (q) = 0. \end{matrix}$ For example, in the case of the kinematic closed-chain, this can be the kinematic constraint that the location of one end of the chain is equal to the location of the other end of the chain. In the following derivation, we'll be using the time derivatives of this constraint: $\begin{matrix} (6) & \dot{h} = H v = 0, \\ (7) & \ddot{h} = H \dot{v} + \dot{H} v = 0, \end{matrix}$ where $H (q) = \frac{\partial h}{\partial q} N (q)$ is the "Jacobian with respect to $v$ ". Note that $\dot{H} v$ is best calculated directly (rather than trying to solve for $\dot{H}$ as an object by itself).

The equations of motion can be written $\begin{matrix} (8) & M (q) \dot{v} + C (q, v) v = τ_{g} (q) + B u + H^{T} (q) λ, \end{matrix}$ where $λ$ is the constraint force. Let's use the shorthand $\begin{matrix} (9) & M (q) \dot{v} = τ (q, \dot{q}, u) + H^{T} (q) λ . \end{matrix}$ In an optimization-based formulation (for instance, to implement this constraint in a trajectory optimization), we can simply make $λ$ a decision variable, and use equations ( $7$ ) and ( $9$ ) as constraints.

In other cases, it can be desirable to solve explicitly for $λ$ . Inserting the dynamics ( $9$ ) into ( $7$ ) yields $\begin{matrix} (10) & λ = - (H M^{- 1} H^{T})^{+} (H M^{- 1} τ + \dot{H} v) . \end{matrix}$ In many cases, this matrix will be full rank (for instance, in the case of multiple independent four-bar linkages) and the traditional inverse could have been used. When the matrix drops rank (multiple solutions), then the pseudo-inverse will select the solution with the smallest constraint forces in the least-squares sense.

To combat numerical "constraint drift", one might like to add a restoring force in the event that the constraint is not satisfied to numerical precision. To accomplish this, rather than solving for $\ddot{h} = 0$ as above, we can instead solve for $\begin{matrix} (11) & \ddot{h} = H \dot{v} + \dot{H} v = - 2 α \dot{h} - α^{2} h, \end{matrix}$ where $α > 0$ is a stiffness parameter. This is known as Baumgarte's stabilization technique, implemented here with a single parameter to provide a critically-damped response. Carrying this through yields $\begin{matrix} (12) & λ = - (H M^{- 1} H^{T})^{+} (H M^{- 1} τ + \dot{H} v + 2 α H v + α^{2} h) . \end{matrix}$

There is an important optimization-based derivation/interpretation of these equations, which we will build on below, using Gauss's Principle of Least Constraint. This principle says that we can derive the constrained dynamics as : $\begin{aligned} (13) & min_{\dot{v}} & \frac{1}{2} (\dot{v} - {\dot{v}}_{u c})^{T} M (\dot{v} - {\dot{v}}_{u c}), \\ subject to & \ddot{h} (q, v, \dot{v}) = 0 = H \dot{v} + \dot{H} v, \end{aligned}$ where ${\dot{v}}_{u c} = M^{- 1} τ$ is the "unconstrained acceleration" [10]. Equation $(8)$ comes right out of the optimality conditions with $λ$ acting precisely as the Lagrange multiplier. This is a convex quadratic program with equality constraints, which has a closed-form solution that is precisely Equation $(10)$ above. It is also illustrative to look at the dual formulation of this optimization, which can be written as $min_{λ} \frac{1}{2} λ^{T} H M^{- 1} H^{T} λ - λ^{T} (H M^{- 1} τ + \dot{H} v) .$ Observe that the linear term is contains the acceleration of $h$ if the dynamics evolved without the constraint forces applied; let's call that ${\ddot{h}}_{u c}$ : $min_{λ} \frac{1}{2} λ^{T} H M^{- 1} H^{T} λ - λ^{T} {\ddot{h}}_{u c} .$ The primal formulation has accelerations as the decision variables, and the dual formulation has constraint forces as the decision variables. For now this is merely a cute observation; we'll build on it more when we get to discussing contact forces.

Bilateral Velocity Constraints

Consider the constraint equation $\begin{matrix} (14) & h_{v} (q, v) = 0, \end{matrix}$ where $\frac{\partial h_{v}}{\partial v} \neq 0.$ These are less common, but arise when, for instance, a joint is driven through a prescribed motion. Again, we will use the derivatives, $\begin{matrix} (15) & {\dot{h}}_{v} = \frac{\partial h_{v}}{\partial q} N (q) v + \frac{\partial h_{v}}{\partial v} \dot{v} = 0, \end{matrix}$ and, the manipulator equations given by $\begin{matrix} (16) & M \dot{v} = τ + {\frac{\partial h_{v}}{\partial v}}^{T} λ . \end{matrix}$ These constraints can be added directly to an optimization with $λ$ as a decision variable. Alternatively, we can solve for $λ$ explicitly yielding $\begin{matrix} (17) & λ = - {(\frac{\partial h_{v}}{\partial v} M^{- 1} \frac{\partial h_{v}}{\partial v})}^{+} [\frac{\partial h_{v}}{\partial v} M^{- 1} τ + \frac{\partial h_{v}}{\partial q} N (q) v] . \end{matrix}$

Again, to combat constraint drift, we can ask instead for ${\dot{h}}_{v} = - α h_{v}$ , which yields $\begin{matrix} (18) & λ = - {(\frac{\partial h_{v}}{\partial v} M^{- 1} \frac{\partial h_{v}}{\partial v})}^{+} [\frac{\partial h_{v}}{\partial v} M^{- 1} τ + \frac{\partial h_{v}}{\partial q} N (q) v + α h_{v}] . \end{matrix}$

Joint locking

Consider the simple, but important, case of "locking" a joint (making a particular joint act as if it is welded in the current configuration). One could write this as a bilateral velocity constraint $h_{v} (q, v) = v_{i} = 0.$ Since $\frac{\partial h_{v}}{\partial v} = e_{i},$ (the vector with $1$ in the $i$ th element and zero everywhere else, equation ( $15$ ) reduces to ${\dot{v}}_{i} = 0$ , and ${\frac{\partial h_{v}}{\partial v}}^{T} λ$ impacts the equation for ${\dot{v}}_{i}$ and \emph{only} the equation for ${\dot{v}}_{i}$ . The resulting equations can be obtained by simply removing the terms associated with $v_{i}$ from the manipulator dynamics (and setting ${\dot{v}}_{i} = 0$ ).

Hybrid models via constraint forces

For hybrid models of contact, it can be useful to work with a description of the dynamics in the floating-base coordinates and solve for the contact forces which enforce the constraints in each mode. We'll define a mode by a list of pairs of contact geometries that are in contact, and even define whether each contact is in sticking contact (with zero relative tangential velocity) or sliding contact. We can ask our geometry engine to return the contact points on the bodies, $^{A} p^{C_{a}}$ and $^{B} p^{C_{b}},$ where $C_{a}$ is the contact point (or closest point to contact) in body $A$ and $C_{b}$ is the contact point (or closest point to contact) in body $B$ . More generally, we will refer to $C_{a}$ and $C_{b}$ as contact frames with their origins at these points, their $z$ -axis pointing out along the contact normal, and their $x - y$ -axes chosen as an orthonormal basis orthogonal to the normal.

For each sticking contact, we have a bilateral position constraint of the form $h_{A, B} (q) =^{W} p_{C_{a}}^{C_{a}} -^{W} p_{C_{a}}^{C_{b}} = 0.$ We can assemble all of these constraints into a single bilateral constraint vector $h (q) = 0,$ and use almost precisely the recipe above. Note that applying Baumgarte stabilization here would only stabilize in the normal direction, by definition the vector between the two points, which is consistent with having (only) a zero-velocity constraint in the tangential direction. For sliding contacts, we have just the scalar constraint, $h_{A, B} (q) =^{W} p_{C_{a; z}}^{C_{a}} -^{W} p_{C_{a; z}}^{C_{b}} = 0,$ which says that the distance is zero in the normal direction, plus a constraint the imposes maximum dissipation $λ_{C_{a; x, y}}^{C_{a}} = - μ λ_{C_{a; z}}^{C_{a}} \frac{^{W} v_{C_{a; x, y}}^{C_{a}}}{|^{W} v_{C_{a; x, y}}^{C_{a}} |},$ that can be accommodated when we solve for $λ$ .

The Dynamics of Contact

The dynamics of multibody systems that make and break contact are closely related to the dynamics of constrained systems, but tend to be much more complex. In the simplest form, you can think of non-penetration as an inequality constraint: the signed distance between collision bodies must be non-negative. But, as we have seen in the chapters on walking, the transitions when these constraints become active correspond to collisions, and for systems with momentum they require some care. We'll also see that frictional contact adds its own challenges.

There are three main approaches used to modeling contact with "rigid" body systems: 1) rigid contact approximated with stiff compliant contact, 2) hybrid models with collision event detection, impulsive reset maps, and continuous (constrained) dynamics between collision events, and 3) rigid contact approximated with time-averaged forces (impulses) in a time-stepping scheme. Each modeling approach has advantages/disadvantages for different applications.

Before we begin, there is a bit of notation that we will use throughout. Let $ϕ (q)$ indicate the relative (signed) distance between two rigid bodies. For rigid contact, we would like to enforce the unilateral constraint: $\begin{matrix} (19) & ϕ (q) \geq 0. \end{matrix}$ We'll use $n = \frac{\partial ϕ}{\partial q}$ to denote the "contact normal", as well as $t_{1}$ and $t_{2}$ as a basis for the tangents at the contact (orthonormal vectors in the Cartesian space, projected into joint space), all represented as row vectors in joint coordinates.

Contact coordinates in 2D. (a) The signed distance between contact bodies, $ϕ (q)$ . (b) The normal ( $n$ ) and tangent ( $t$ ) contact vectors -- note that these can be drawn in 2D when the $q$ is the $x, y$ positions of one of the bodies, but more generally the vectors live in the configuration space. (c) Sometimes it will be helpful for us to express the tangential coordinates using $d_{1}$ and $d_{2}$ ; this will make more sense in the 3D case.

We will also find it useful to assemble the contact normal and tangent vectors into a single matrix, $J$ , that we'll call the contact Jacobian: $J (q) = [\begin{matrix} n \\ t_{1} \\ t_{2} \end{matrix}] .$ As written, $J v$ gives the relative velocity of the closest points in the contact coordinates; it can be also be extended with three more rows to output a full spatial velocity (e.g. when modeling torsional friction). The generalized force produced by the contact is given by $J^{T} λ$ , where $λ = [f_{n}, f_{t 1}, f_{t 2}]^{T} :$ $\begin{matrix} (20) & M (q) \dot{v} + C (q, v) v = τ_{g} (q) + B u + J^{T} (q) λ . \end{matrix}$

Compliant Contact Models

Most compliant contact models are conceptually straight-forward: we will implement contact forces using a stiff spring (and damper[11]) that produces forces to resist penetration (and grossly model the dissipation of collision and/or friction). For instance, for the normal force, $f_{n}$ , we can use a simple (piecewise) linear spring law:

Coulomb friction is described by two parameters -- $μ_{s t a t i c}$ and $μ_{d y n a m i c}$ -- which are the coefficients of static and dynamic friction. When the contact tangential velocity (given by $t v$ ) is zero, then friction will produce whatever force is necessary to resist motion, up to the threshold $μ_{s t a t i c} f_{n} .$ But once this threshold is exceeding, we have slip (non-zero contact tangential velocity); during slip friction will produce a constant drag force $| f_{t} | = μ_{d y n a m i c} f_{n}$ in the direction opposing the motion. This behavior is not a simple function of the current state, but we can approximate it with a continuous function as pictured below.

(left) The Coloumb friction model. (right) A continuous piecewise-linear approximation of friction (blue) and the Stribeck approximation of Coloumb friction (green); the $x$ -axis is the contact tangential velocity, and the $y$ -axis is the friction coefficient.

With these two laws, we can recover the contact forces as relatively simple functions of the current state. However, the devil is in the details. There are a few features of this approach that can make it numerically unstable. If you've ever been working in a robotics simulator and watched your robot take a step only to explode out of the ground and go flying through the air, you know what I mean.

In order to tightly approximate the (nearly) rigid contact that most robots make with the world, the stiffness of the contact "springs" must be quite high. For instance, I might want my 180kg humanoid robot model to penetrate into the ground no more than 1mm during steady-state standing. The challenge with adding stiff springs to our model is that this results in stiff differential equations (it is not a coincidence that the word stiff is the common term for this in both springs and ODEs). As a result, the best implementations of compliant contact for simulation make use of special-purpose numerical integration recipes (e.g. [12]) and compliant contact models are often difficult to use in e.g. trajectory/feedback optimization.

Note that there is one other type of friction that we talk about in robotics. In joints we also, additionally, model joint damping, often as a linear force proportional to the velocity which resists motion. Phenomenologically, this is due to a viscous (fluid) drag force produced by lubricants in the joint. The static and dynamic friction described above are sometimes collectively referred to as "dry" friction.

But there is another serious numerical challenge with the basic implementation of this model. Computing the signed-distance function, $ϕ (q)$ , when it is non-negative is straightforward, but robustly computing the "penetration depth" once two bodies are in collision is not. Consider the case of use $ϕ (q)$ to represent the maximum penetration depth of, say, two boxes that are contacting each other. With compliant contact, we must have some penetration to produce any contact force. But the direction of the normal vector, $n = \frac{\partial ϕ}{\partial q},$ can easily change discontinuously as the point of maximal penetration moves, as I've tried to illustrate here:

(Left) Two boxes in penetration, with the signed distance defined by the maximum penetration depth. (Right) The contact normal for various points of maximal penetration.

If you really think about this example, it actually reveals even more of the foibles of trying to even define the contact points and normals consistently. It seems reasonable to define the point on body B as the point of maximal penetration into body A, but notice that as I've drawn it, that isn't actually unique! The corresponding point on body A should probably be the point on the surface with the minimal distance from the max penetration point (other tempting choices would likely cause the distance to be discontinuous at the onset of penetration). But this whole strategy is asymmetric; why shouldn't I have picked the vector going with maximal penetration into body B? My point is simply that these cases exist, and that even our best software implementations get pretty unsatisfying when you look into the details. And in practice, it's a lot to expect the collision engine to give consistent and smooth contact points out in every situation.

Some of the advantages of this approach include (1) the fact that it is easy to implement, at least for simple geometries, (2) by virtue of being a continuous-time model it can be simulated with error-controlled integrators, and (3) the tightness of the approximation of "rigid" contact can be controlled through relatively intuitive parameters. However, the numerical challenges of dealing with penetration are very real, and they motivate our other two approaches that attempt to more strictly enforce the non-penetration constraints.

Rigid Contact with Event Detection

Impulsive Collisions

The collision event is described by the zero-crossings (from positive to negative) of $ϕ$ . Let us start by assuming frictionless collisions, allowing us to write $\begin{matrix} (21) & M \dot{v} = τ + λ n^{T}, \end{matrix}$ where $n$ is the "contact normal" we defined above and $λ \geq 0$ is now a (scalar) impulsive force that is well-defined when integrated over the time of the collision (denoted $t_{c}^{-}$ to $t_{c}^{+}$ ). Integrate both sides of the equation over that (instantaneous) interval: $\begin{array}{r} \int_{t_{c}^{-}}^{t_{c}^{+}} d t [M \dot{v}] = \int_{t_{c}^{-}}^{t_{c}^{+}} d t [τ + λ n^{T}] \end{array}$ Since $M$ , $τ$ , and $n$ are constants over this interval, we are left with $M v^{+} - M v^{-} = n^{T} \int_{t_{c}^{-}}^{t_{c}^{+}} λ d t,$ where $v^{+}$ is short-hand for $v (t_{c}^{+})$ . Multiplying both sides by $n M^{- 1}$ , we have $n v^{+} - n v^{-} = n M^{- 1} n^{T} \int_{t_{c}^{-}}^{t_{c}^{+}} λ d t .$ After the collision, we have ${\dot{ϕ}}^{+} = - e {\dot{ϕ}}^{-}$ , with $0 \leq e \leq 1$ denoting the coefficient of restitution, yielding: $n v^{+} - n v^{-} = - (1 + e) n v^{-},$ and therefore $\int_{t_{c}^{-}}^{t_{c}^{+}} λ d t = - (1 + e) {[n M^{- 1} n^{T}]}^{#} n v^{-} .$ I've used the notation $A^{#}$ here to denote the pseudoinverse of $A$ (I would normally write $A^{+},$ but have changed it for this section to avoid confusion). Substituting this back in above results in $\begin{matrix} (22) & v^{+} = [I - (1 + e) M^{- 1} n^{T} {[n M^{- 1} n^{T}]}^{#} n] v^{-} . \end{matrix}$

We can add friction at the contact. Rigid bodies will almost always experience contact at only a point, so we typically ignore torsional friction [7], and model only tangential friction by extending $n$ to a matrix $J = [\begin{matrix} n \\ t_{1} \\ t_{2} \end{matrix}],$ to capture the gradient of the contact location tangent to the contact surface. Then $λ$ becomes a Cartesian vector representing the contact impulse, and (for infinite friction) the post-impact velocity condition becomes $J v^{+} = diag (- e, 0, 0) J v^{-},$ resulting in the equations: $\begin{matrix} (23) & v^{+} = [I - M^{- 1} J^{T} diag (1 + e, 1, 1) {[J M^{- 1} J^{T}]}^{#} J] v^{-} . \end{matrix}$ Often in optimization, we can directly use the implicit form $\begin{matrix} M (v^{+} - v^{-}) = J^{T} Λ \\ J v^{+} = diag (- e, 0, 0) J v^{-} \end{matrix}$ If $Λ$ is restricted to a friction cone, as in Coulomb friction, in general we have to solve an optimization to solve for $v^{+}$ subject to the friction cone constraints on $Λ .$

A spinning ball bouncing on the ground.

Imagine a ball (a hollow-sphere) in the plane with mass $m$ , radius $r$ . The configuration is given by $q = {[\begin{matrix} x, z, θ \end{matrix}]}^{T} .$ The equations of motion are $M (q) \ddot{q} = [\begin{matrix} m & 0 & 0 \\ 0 & m & 0 \\ 0 & 0 & \frac{2}{3} m r^{2} \end{matrix}] \ddot{q} = [\begin{matrix} 0 \\ - g \\ 0 \end{matrix}] + [\begin{matrix} 0 & 1 \\ 1 & 0 \\ 0 & r \end{matrix}] [\begin{matrix} λ_{z} \\ λ_{x} \end{matrix}] = τ_{g} + J^{T} λ,$ where $λ$ are the contact forces (which are zero except during the impulsive collision). Given a coefficient of restitution $e$ and a collision with a horizontal ground, the post-impact velocities are: ${\dot{q}}^{+} = [\begin{matrix} \frac{3}{5} & 0 & - \frac{2}{5} r \\ 0 & - e & 0 \\ - \frac{3}{5 r} & 0 & \frac{2}{5} \end{matrix}] {\dot{q}}^{-} .$

Putting it all together

We can put the bilateral constraint equations and the impulsive collision equations together to implement a hybrid model for unilateral constraints of the form. Let us generalize $\begin{matrix} (24) & ϕ (q) \geq 0, \end{matrix}$ to be the vector of all pairwise (signed) distance functions between rigid bodies; this vector describes all possible contacts. At every moment, some subset of the contacts are active, and can be treated as a bilateral constraint ( $ϕ_{i} = 0$ ). The guards of the hybrid model are when an inactive constraint becomes active ( $ϕ_{i} > 0 \to ϕ_{i} = 0$ ), or when an active constraint becomes inactive ( $λ_{i} > 0 \to λ_{i} = 0$ and ${\dot{ϕ}}_{i} > 0$ ). Note that collision events will (almost always) only result in new active constraints when $e = 0$ , e.g. we have pure inelastic collisions, because elastic collisions will rarely permit sustained contact.

If the contact involves Coulomb friction, then the transitions between sticking and sliding can be modeled as additional hybrid guards.

Time-stepping Approximations for Rigid Contact

So far we have seen two different approaches to enforcing the inequality constraints of contact, $ϕ (q) \geq 0$ and the friction cone. First we introduced compliant contact which effectively enforces non-penetration with a stiff spring. Then we discussed event detection as a means to switch between different models which treat the active constraints as equality constraints. But, perhaps surprisingly, one of the most popular and scalable approaches is something different: it involves formulating a mathematical program that can solve the inequality constraints directly on each time step of the simulation. These algorithms may be more expensive to compute on each time step, but they allow for stable simulations with potentially much larger and more consistent time steps.

Complementarity formulations

What class of mathematical program due we need to simulate contact? The discrete nature of contact suggests that we might need some form of combinatorial optimization. Indeed, the most common transcription is to a Linear Complementarity Problem (LCP) [13], as introduced by [14] and [15]. An LCP is typically written as $\begin{aligned} {find}_{w, z} subject to & w = q + M z, \\ w \geq 0, z \geq 0, w^{T} z = 0. \end{aligned}$ They are directly related to the optimality conditions of a (potentially non-convex) quadratic program and [15]> showed that the LCP's generated by our rigid-body contact problems can be solved by Lemke's algorithm. As short-hand we will write these complementarity constraints as ${find}_{z} subject to 0 \leq (q + M z) ⊥ z \geq 0.$

Rather than dive into the full transcription, which has many terms and can be relatively difficult to parse, let me start with two very simple examples.

Time-stepping LCP: Normal force

Consider our favorite simple mass being actuated by a horizontal force (with the double integrator dynamics), but this time we will add a wall that will prevent the cart position from taking negative values: our non-penetration constraint is $q \geq 0$ . Physically, this constraint is implemented by a normal (horizontal) force, $f$ , yielding the equations of motion: $m \ddot{q} = u + f .$

$f$ is defined as the force required to enforce the non-penetration constraint; certainly the following are true: $f \geq 0$ and $q \cdot f = 0$ . $q \cdot f = 0$ is the "complementarity constraint", and you can read it here as "either q is zero or force is zero" (or both); it is our "no force at a distance" constraint, and it is clearly non-convex. It turns out that satisfying these constraints, plus $q \geq 0$ , is sufficient to fully define $f$ .

To define the LCP, we first discretize time, by approximating $\begin{matrix} q [n + 1] = q [n] + h v [n + 1], \\ v [n + 1] = v [n] + \frac{h}{m} (u [n] + f [n]) . \end{matrix}$ This is almost the standard Euler approximation, but note the use of $v [n + 1]$ in the right-hand side of the first equation -- this is actually a semi-implicit Euler approximation, and this choice is essential in the derivation.

Given $h, q [n], v [n],$ and $u [n]$ , we can solve for $f [n]$ and $q [n + 1]$ simultaneously, by solving the following LCP: $\begin{matrix} q [n + 1] = [q [n] + h v [n] + \frac{h^{2}}{m} u [n]] + \frac{h^{2}}{m} f [n] \\ q [n + 1] \geq 0, f [n] \geq 0, q [n + 1] \cdot f [n] = 0. \end{matrix}$ Take a moment and convince yourself that this fits into the LCP prescription given above.

Note: Please don't confuse this visualization with the more common visualization of the solution space for an LCP (in two or more dimensions) in terms of "complementary cones"[13].

Perhaps it's also helpful to plot the solution, $q [n + 1], f [n]$ as a function of $q [n], v [n]$ . I've done it here for $m = 1, h = 0.1, u [n] = 0$ :

In the (time-stepping, "impulse-velocity") LCP formulation, we write the contact problem in it's combinatorial form. In the simple example above, the complementarity constraints force any solution to lie on either the positive x-axis ( $f [n] \geq 0$ ) or the positive y-axis ( $q [n + 1] \geq 0$ ). The equality constraint is simply a line that will intersect with this constraint manifold at the solution. But in this frictionless case, it is important to realize that these are simply the optimality conditions of a convex optimization problem: the discrete-time version of Gauss's principle that we used above. Using $v^{'}$ as shorthand for $v [n + 1]$ , and replacing $\ddot{q} = \frac{v^{'} - v}{h}$ in Eq $(13)$ and scaling the objective by $h^{2}$ we have: $\begin{aligned} min_{v^{'}} & \frac{1}{2} {(v^{'} - v - h M^{- 1} τ)}^{T} M (v^{'} - v - h M^{- 1} τ) \\ subject to & \frac{1}{h} ϕ (q^{'}) = \frac{1}{h} ϕ (q + h v^{'}) \approx \frac{1}{h} ϕ (q) + n v^{'} \geq 0. \end{aligned}$ The objective is even cleaner/more intuitive if we denote the next step velocity that would have occurred before the contact impulse is applied as $v^{-} = v + h M^{- 1} τ$ : $\begin{aligned} min_{v^{'}} & \frac{1}{2} {(v^{'} - v^{-})}^{T} M (v^{'} - v^{-}) \\ subject to & \frac{1}{h} ϕ (q^{'}) \geq 0. \end{aligned}$ The LCP for the frictionless contact dynamics is precisely the optimality conditions of this convex (because $M ⪰ 0$ ) quadratic program, and once again the contact impulse, $f \geq 0$ , plays the role of the Lagrange multiplier (with units $N \cdot s$ ).

So why do we talk so much about LCPs instead of QPs? Well LCPs can also represent a wider class of problems, which is what we arrive at with the standard transcription of Coulomb friction. In the limit of infinite friction, then we could add an additional constraint that the tangential velocity at each contact point was equal to zero (but these equation may not always have a feasible solution). Once we admit limits on the magnitude of the friction forces, the non-convexity of the disjunctive form rear's its ugly head.

Time-stepping LCP: Coulomb Friction

We can use LCP to find a feasible solution with Coulomb friction, but it requires some gymnastics with slack variables to make it work. For this case in particular, I believe a very simple example is best. Let's take our brick and remove the wall and the wheels (so we now have friction with the ground).

The dynamics are the same as our previous example, $m \ddot{q} = u + f,$ but this time I'm using $f$ for the friction force which is inside the friction cone if $\dot{q} = 0$ and on the boundary of the friction cone if $\dot{q} \neq 0;$ this is known as the principle of maximum dissipation[14]. Here the magnitude of the normal force is always $m g$ , so we have $| f | \leq μ m g,$ where $μ$ is the coefficient of friction. And we will use the same semi-implicit Euler approximation to cast this into discrete time.

Now, to write the friction constraints as an LCP, we must introduce some slack variables. First, we'll break the force into a positive component and a negative component: $f [n] = f^{+} - f^{-} .$ And we will introduce one more variable $v_{a b s}$ which will be non-zero if the velocity is non-zero (in either direction). Now we can write the LCP: $\begin{aligned} {find}_{v_{a b s}, f^{+}, f^{-}} subject to \\ 0 \leq v_{a b s} + v [n + 1] & ⊥ & f^{+} \geq 0, \\ 0 \leq v_{a b s} - v [n + 1] & ⊥ & f^{-} \geq 0, \\ 0 \leq μ m g - f^{+} - f^{-} & ⊥ & v_{a b s} \geq 0, \end{aligned}$ where each instance of $v [n + 1]$ we write out with $v [n + 1] = v [n] + \frac{h}{m} (u + f^{+} - f^{-}) .$ It's enough to make your head spin! But if you work it out, all of the constraints we want are there. For example, for $v [n + 1] > 0$ , then we must have $f^{+} = 0$ , $v_{a b s} = v [n + 1]$ , and $f^{-} = μ m g$ . When $v [n + 1] = 0$ , we can have $v_{a b s} = 0$ , $f^{+} - f^{-} \leq μ m g$ , and those forces must add up to make $v [n + 1] = 0$ .

We can put it all together and write an LCP with both normal forces and friction forces, related by Coulomb friction (using a polyhedral approximation of the friction cone in 3d)[14]. Although the solution to any LCP can also be represented as the solution to a QP, the QP for this problem is non-convex.

Anitescu's convex formulation

In [16], Anitescu described the natural convex formulation of this problem by dropping the maximum dissipation inequalities and allowing any force inside the friction cone. Consider the following optimization: $\begin{aligned} min_{v^{'}} & \frac{1}{2} {(v^{'} - v^{-})}^{T} M (v^{'} - v^{-}) \\ subject to & \frac{1}{h} ϕ (q^{'}) + μ d_{i} v^{'} \geq 0, \forall i \in 1, . . ., m \end{aligned}$ where $d_{i}, \forall i \in 1, . . ., m$ are a set of tangent row vectors in joint coordinates parameterizing the polyhedral approximation of the friction cone (as in [14]). Importantly, for each $d_{i}$ we must also have $- d_{i}$ in the set. By writing the Lagrangian, $L (v^{'}, β) = \frac{1}{2} (v^{'} - v^{-})^{T} M (v^{'} - v^{-}) - \sum_{i} β_{i} (\frac{1}{h} ϕ (q) + (n + μ d_{i}) v^{'}),$ and checking the stationarity condition: ${\frac{\partial L}{\partial v^{'}}}^{T} = M (v^{'} - v) - h τ - \sum_{i} β_{i} (n + μ d_{i})^{T} = 0,$ we can see that the Lagrange multiplier, $β_{i} \geq 0$ , associated with the $i$ th constraint is the magnitude of the impulse (with units $N \cdot s$ ) in the direction $n - μ d_{i},$ where $n = \frac{\partial ϕ}{\partial q}$ is the contact normal; the sum of the forces is an affine combination of these extreme rays of the polyhedral friction cone. As written, the optimization is a QP.

But be careful! Although the primal solution was convex, and the dual problems are always convex, the objective here can be positive semi-definite. This isn't a mistake -- [16] describes a few simple examples where the solution to $v^{'}$ is unique, but the impulses that produce it are not (think of a table with four legs).

When the tangential velocity is zero, this constraint is tight; for sliding contact the relaxation effectively causes the contact to "hydroplane" up out of contact, because $ϕ (q^{'}) \geq h μ d_{i} v^{'} .$ It seems like a quite reasonable approximation to me, especially for small $h$ !

Let's continue on and write the dual program. To make the notation a little better, let us stack the Jacobian vectors into $J_{β}$ , such that the $i$ th row is $n + μ d_{i}$ , so that we have ${\frac{\partial L}{\partial v^{'}}}^{T} = M (v^{'} - v) - h τ - J_{β}^{T} β = 0.$ Substituting this back into the Lagrangian give us the dual program: $min_{β \geq 0} \frac{1}{2} β^{T} J_{β} M^{- 1} J_{β}^{T} β + \frac{1}{h} ϕ (q) \sum_{i} β_{i} + β^{T} J_{β} v^{-} .$

One final note: I've written just one contact point here to simplify the notation, but of course we repeat the constraint(s) for each contact point; [16] studies the typical case of only including potential contact pairs with $ϕ (q) \leq ϵ$ , for small $ϵ \geq 0$ .

Todorov's regularization

According to [17], the popular MuJoCo simulator uses a slight variation of this convex relaxation, with the optimization: $\begin{aligned} min_{λ} & \frac{1}{2} λ^{T} J M^{- 1} J^{T} λ + \frac{1}{h} λ^{T} (J v^{-} - {\dot{x}}^{d}), \\ subject to & λ \in FC (q), \end{aligned}$ where ${\dot{x}}^{d}$ is a "desired contact velocity", and $FC (q)$ describes the friction cone. The friction cone constraint looks new but is not; observe that $J^{T} λ = J_{β}^{T} β$ , where $β \geq 0$ is a clever parameterization of the friction cone in the polyhedral case. The bigger difference is in the linear term: [17] proposed ${\dot{x}}^{d} = J v - h B J v - h K [ϕ (q), 0, 0]^{T},$ with $B$ and $K$ stabilization gains that are chosen to yield a critically damped response.

How should we interpret this? If you expand the linear terms, you will see that this is almost exactly the dual form we get from the position equality constraints formulation, including the Baumgarte stabilization. It's interesting to think about the implications of this formulation -- like the Anitescu relaxation, it is possible to get some "force at a distance" because we have not in any way expressed that $λ = 0$ when $ϕ (q) > 0.$ In Anitescu, it happened in a velocity-dependent boundary layer; here it could happen if you are "coming in hot" -- moving towards contact with enough relative velocity that the stabilization terms want to slow you down.

In dual form, it is natural to consider the full conic description of the friction cone: $FC (q) = {λ = [f_{n}, f_{t 1}, f_{t 2}]^{T} | f_{n} \geq 0, \sqrt{f_{t 1}^{2} + f_{t 2}^{2}} \leq μ f_{n}} .$ The resulting dual optimization is has a quadratic objective and second-order cone constraints (SOCP).

There are a number of other important relaxations that happen in MuJoCo. To address the positive indefiniteness in the objective, [17] relaxes the dual objective by adding a small term to the diagonal. This guarantees convexity, but the convex optimization can still be poorly conditioned. The stabilization terms in the objective are another form of relaxation, and [17] also implements adding additional stabilization to the inertial matrix, called the "implicit damping inertia". These relaxations can dramatically improve simulation speed, both by making each convex optimization faster to solve, and by allowing the simulator to take larger integration time steps. MuJoCo boasts a special-purpose solver that can simulate complex scenes at seriously impressive speeds -- often orders of magnitude faster than real time. But these relaxations can also be abused -- it's unfortunately quite common to see physically absurd MuJoCo simulations, especially when researchers who don't care much about the physics start changing simulation parameters to optimize their learning curves!

The Semi-Analytic Primal (SAP) solver

The contact solver that we use in Drake further extends and improves upon this line of work... [18].

Relaxed complementarity-free formulation: Normal force

What equations do we get if we simulate the simple cart colliding with a wall example that we used above?

Following [17], the first relaxation we find is the addition of "implicit damping inertia", which effectively adds some inertia and some damping, parameterized by the regularization term $b$ , to the explicit form of the equations. As a result our discrete-time cart dynamics are given by $(m + b) \frac{v [n + 1] - v [n]}{h} = u [n] + f [n] - b v [n] .$

What remains is to compute $f [n]$ , and here is where things get interesting. Motivated by the idea of "softening the Gauss principle" (e.g. the principle of least constraint [10]) by replacing the hard constraints with a soft penalty term, we end up with a convex optimization which, in this scalar frictionless case, reduces to: $\begin{matrix} f [n] = {argmin}_{f \geq 0} \frac{1}{2} f^{2} \frac{1 + ϵ}{m + b} + f (v^{-} - v^{*}), \end{matrix}$ where $v^{-} = v [n] + \frac{h}{m + b} (u [n] - b v [n])$ is the next-step velocity before the contact impulse. $v^{*}$ is another regularization term: the "desired contact velocity" which is implemented like a by Baumgarte stabilization: $v^{*} = v [n] - 2 h \frac{1 + ϵ}{κ} v [n] - h \frac{1 + ϵ}{κ^{2}} q [n] .$ This introduced two additional relaxation parameters; MuJoCo calls $ϵ$ the "impulse regularization scaling" and $κ$ the "error-reduction time constant".

Beyond Point Contact

Coming soon... Box-on-plane example. Multi-contact.

Also a single point force cannot capture effects like torsional friction, and performs badly in some very simple cases (imaging a box sitting on a table). As a result, many of the most effective algorithms for contact restrict themselves to very limited/simplified geometry. For instance, one can place "point contacts" (zero-radius spheres) in the four corners of a robot's foot; in this case adding forces at exactly those four points as they penetrate some other body/mesh can give more consistent contact force locations/directions. A surprising number of simulators, especially for legged robots, do this.

In practice, most collision detection algorithms will return a list of "collision point pairs" given the list of bodies (with one point pair per pair of bodies in collision, or more if they are using the aforementioned "multi-contact" heuristics), and our contact force model simply attaches springs between these pairs. Given a point-pair, $p_{A}$ and $p_{B}$ , both in world coordinates, ...

Hydroelastic model in drake[19].

Variational mechanics

The field of analytic mechanics (or variational mechanics), has given us a number of different and potentially powerful interpretations of these equations. While our principle of virtual work was very much rooted in mechanical work, these variational principles have proven to be much more general. In the context of these notes, the variational principles also provide some deep connections between mechanics and optimization. So let's consider some of the equations above, but through the more general lens of variational mechanics.

Virtual work

Most of us are comfortable with the idea that a body is in equilibrium if the sum of all forces acting on that body (including gravity) is zero: $\sum_{i} f_{i} = 0.$ It turns out that there are some limitations in working with forces directly: forces require us to (arbitrarily) define a coordinate system, and to achieve force balance we might need to consider all of the forces in the system, including internal forces like the forces at the joints that hold our robots together.

So it turns out to be better to think not about forces directly, but to think instead about the work, $w$ , done by those forces. Work is force times distance. It's a scalar quantity, and it is invariant to the choice of coordinate system. Even for a system that is in equilibrium, we can reason about the virtual work that would occur if we were to make an infinitesimal change to the configuration of the system. Even better, since we know that the internal forces do no work, we can ignore them completely and only consider the virtual work that occurs due to a variation in the configuration variables. A multibody system in equilibrium must have zero virtual work: $δ w = \sum_{i} τ_{i}^{T} δ q = 0.$ Notice that these forces are the generalized forces (they have the same dimension as $q$ ). We can map between a Cartesian force and the generalized force using a kinematic Jacobian: $τ_{i} = J_{i}^{T} (q) f_{i} .$ In particular, forces due to gravity, or any forces described by a "potential energy" function, are can be written as $τ_{g} = - {\frac{\partial U_{g} (q)}{\partial q}}^{T} .$

If all of the generalized forces can be derived from such a potential function, then we can go even further. For a system to be in equilibrium, we must have that varations in the potential energy function must be zero, $δ U (q) = 0.$ This follows simply from $δ U = \frac{\partial U}{\partial q} δ q$ and the principle of virtual work. Moreover, for the system to be in a stable equilibrium, $q$ must not only be a stationary point of the potential, but a minimum of the potential. This is our first important connection in this section between mechanics and optimization.

D'Alembert's principle and the force of inertia

What about bodies in motion? d'Alembert's principle states that we can reduce dynamics problems to statics problems by simply including a "force of inertia", which is the familiar mass times acceleration for simple particles. In order to have $δ w = \sum_{i} τ_{i}^{T} δ q = 0,$ for all $δ q$ , we must have $\sum_{i} τ_{i} = 0,$ so for multibody systems we know that the force of inertia must be: $τ_{i n e r t i a} = - M (q) \ddot{q} - C (q, \dot{q}) \dot{q} .$ But where did that term come from? Is it the result of nature "optimizing" some scalar function?

A natural candidate would be the kinetic energy: $T (q, \dot{q}) = \frac{1}{2} {\dot{q}}^{T} M (q) \dot{q} .$ What would it mean to take a variation of the kinetic energy, $δ T ?$ So far we've only had variations with respect to $δ q$ , but the kinetic energy depends on both $q$ and $\dot{q}$ . We cannot vary these variables independently, but how are they related?

The solution is to take a variation not at a single point, but along a trajectory, where the relationship between $δ q$ and $δ \dot{q}$ is well defined. If we consider $A (q (t)) = \int_{t_{0}}^{t_{1}} T (q, \dot{q}) d t,$ then a variation of $A$ (using the calculus of variations) w.r.t. any trajectory $q (t)$ , will take the form: $δ A = A (q (t) + δ q (t)) - A (q (t) = \int_{t_{0}}^{t_{1}} T (q + δ q (t), \dot{q} + δ \dot{q} (t)) d t - \int_{t_{0}}^{t_{1}} T (q (t), \dot{q} (t)) d t,$ where $δ q (t)$ is defined to be an arbitrary (small) differentiable function in the space of $q$ that is zero at $t_{0}$ and $t_{1} .$

The derivation takes a few steps, which can be found in most mechanics texts. Click here to see my version.

We can expand the term in the first integral: $T (q (t) + q (t), \dot{q} (t) + δ \dot{q} (t)) = T (q (t), \dot{q} (t)) + \frac{\partial T}{\partial q (t)} δ q (t) + \frac{\partial T}{\partial \dot{q} (t)} δ \dot{q} (t) .$ Substituting this back in and simplifying leaves: $δ A = \int_{t_{0}}^{t_{1}} [\frac{\partial T}{\partial q (t)} δ q (t) + \frac{\partial T}{\partial \dot{q} (t)} δ \dot{q} (t)] d t .$ Now use integration by parts on the second term: $\int_{t_{0}}^{t_{1}} \frac{\partial T}{\partial \dot{q} (t)} δ \dot{q} (t) d t = {[\frac{\partial T}{\partial \dot{q} (t)} δ q (t)]}_{t_{0}}^{t_{1}} - \int_{t_{0}}^{t_{1}} \frac{d}{d t} \frac{\partial T}{\partial \dot{q} (t)} δ q (t) d t,$ and observe that the first term in the right-hand side is zero since $δ q (t_{0}) = δ q (t_{1}) = 0$ . This leaves $δ A = \int_{t_{0}}^{t_{1}} [\frac{\partial T}{\partial q} - \frac{d}{d t} \frac{\partial T}{\partial \dot{q}}] δ q (t) d t .$ If our goal is to analyze the stationary points of this variation, then this quantity must integrate to zero for any $q (t)$ , so we must have $\frac{\partial T}{\partial q} - \frac{d}{d t} \frac{\partial T}{\partial \dot{q}} = 0.$

The result is that we find that the natural generalization of $- {\frac{\partial U (q)}{\partial q}}^{T}$ for stationary points of variations of quantities that also depend on velocity is $τ_{i n e r t i a} = - {\frac{\partial T}{\partial q}}^{T} + \frac{d}{d t} {\frac{\partial T}{\partial \dot{q}}}^{T} .$ This is exactly (the negation of) our familiar $M (q) \ddot{q} + C (q, \dot{q}) \dot{q} .$

Principle of Stationary Action

The integral quantity $A$ that we used in the last section is commonly called the "action" of the system. More generally, the action is defined as $A = \int_{t_{0}}^{t_{1}} L (q, \dot{q}) d t,$ where $L (q, \dot{q})$ is an appropriate "Lagrangian" for the system. The Lagrangian has units of energy, but it is not uniquely defined. Any function which generates the correct equations of motion, in agreement with physical laws, can be taken as a Lagrangian. For our mechanical systems, the familiar form of the Lagrangian is what we presented above: $L = T - U$ (kinetic energy minus potential energy). Since the terms involving $U (q)$ do not depend directly on $\dot{q}$ , they only contribute the ${\frac{\partial U}{\partial q}}^{T}$ terms that we expect, and taking $δ A = 0$ for this Lagrangian is a way to generate the entire equations of motion in one go. For our purposes, where we care about conservative forces but also control inputs (as generalized forces $B u$ ), the principle of virtual work tells us that we will have $δ A + \int_{t_{0}}^{t_{1}} u^{T} B^{T} δ q (t) d t = 0.$

The viewpoint of variational mechanics is that it is this scalar quantity $L (q, \dot{q})$ that should be the central object of study (as opposed to coordinate dependent forces), and this has proven to generalize beautifully. [20] says

The principle of least action -- really the principle of stationary action -- is the most compact form of the classical laws of physics. This simple rule (it can be written in a single line) summarizes everything! Not only the principles of classical mechanics, but electromagnetism, general relativity, quantum mechanics, everything known about chemistry -- right down to the ultimate known constituents of matter, elementary particles.

It's only a stationary point, not a minima. Can I say that stable trajectories are minima? (it's the natural extension of the potential energy case)? In what sense, precisely, are they stable?

Hamiltonian Mechanics

In some cases, it might be preferable to use the Hamiltonian formulation of the dynamics, with state variables $q$ and $p$ (instead of $\dot{q}$ ), where $p = {\frac{\partial L}{\partial \dot{q}}}^{T} = M \dot{q}$ results in the dynamics: $\begin{aligned} M (q) \dot{q} = & p \\ \dot{p} = & c_{H} (q, p) + τ_{g} (q) + B u, where c_{H} (q, p) = \frac{1}{2} {(\frac{\partial [{\dot{q}}^{T} M (q) \dot{q}]}{\partial q})}^{T} . \end{aligned}$ Recall that the Hamiltonian is $H (p, q) = p^{T} \dot{q} - L (q, \dot{q})$ ; these equations of motion are the familiar $\dot{q} = {\frac{\partial H}{\partial p}}^{T}, \dot{p} = - {\frac{\partial H}{\partial q}}^{T} .$

Underactuated Robotics

Multi-Body Dynamics

Deriving the equations of motion

Simple Double Pendulum