Lagrange equations and manipulator dynamics

Learning outcomes

Derive M and c from kinetic and potential energy.
Recognize the nonuniqueness of a Coriolis matrix.
Validate a small arm model through energy and engine comparisons.

Prerequisites: Linear algebra, ODEs, and earlier lectures in this sequence.

Deriving equations from energy

For independent scalar coordinates q, define $L(q,\dot q)=T-V$ . Lagrange's equations are

\frac{d}{dt}\frac{\partial L}{\partial\dot q_i} -\frac{\partial L}{\partial q_i}=\tau_i.

T=\frac12\dot q^\top M(q)\dot q

, collecting accelerations produces

M(q)\ddot q+c(q,\dot q)=\tau

. Here c combines velocity-dependent inertial forces and the potential gradient.

These coordinate equations cannot be applied blindly to four independent quaternion coordinates. A quaternion has a constraint and qvel is a tangent quantity. Use generalized-speed formulations or a library with a declared manifold convention for floating-base systems.

A two-link planar arm

Take point masses $m_1,m_2$ at the link ends, massless rods $l_1,l_2$ , and angles measured from downward vertical. Let $q_2$ be relative to link 1. Then

M_{11}=(m_1+m_2)l_1^2+m_2l_2^2+2m_2l_1l_2\cos q_2,

M_{12}=M_{21}=m_2l_2^2+m_2l_1l_2\cos q_2,\quad M_{22}=m_2l_2^2.

With

a=m_2l_1l_2\sin q_2

, the velocity bias is

(-a(2v_1v_2+v_2^2),av_1^2)

. Gravity is

g_q=\begin{bmatrix}(m_1+m_2)gl_1\sin q_1+m_2gl_2\sin(q_1+q_2)\\m_2gl_2\sin(q_1+q_2)\end{bmatrix}.

Thus c is the sum of these two vectors.

Worked mass-matrix check

For unit masses and lengths at $q_2=0$ ,

M=\begin{bmatrix}5&2\\2&1\end{bmatrix}.

Its determinant is 1 and its trace is 6, so both eigenvalues are positive. Off-diagonal entries express coupling: accelerating one joint generally requires torque at the other.

The point-mass model differs from an arm built from solid capsules. A fair MuJoCo comparison must match centers of mass and inertias rather than merely match link lengths.

Why c rather than C?

Many texts write $C(q,v)v+g(q)$ . The matrix C is not unique, even though the resulting bias vector is physically determined. This course uses MuJoCo's c and qfrc_bias. The common h notation is an alias, not an additional force. Passive damping remains separate.

Practice and exercise

Implement the point-mass arm acceleration with a linear solve, then test M symmetry and positive definiteness over random poses. Compare finite-difference energy changes with mechanical power.

Check: At zero velocity and with gravity disabled, what should c be? Answer: zero for this ideal model. A nonzero value suggests a derivation, configuration, or force-accounting error. Dissipative forces should not be hidden in the inertial bias.