Lagrangian Dynamics: Energy Methods and the Equation of Motion
The Lagrangian is not just an alternative derivation — it is the formulation that makes the structure of M, C, and g analytically tractable.
🎯 By the end you'll understand
What generalized coordinates are and why they simplify constrained systems. How kinetic and potential energy assemble the Lagrangian, and why the Euler-Lagrange equation is a machinery for producing equations of motion. How to derive the inertia matrix M(q), Coriolis/centrifugal matrix C(q,q̇), and gravity vector g(q) for a 2-link arm — step by step. Where Newton-Euler is more efficient, and where the Lagrangian wins. And the critical structural property of C — skew-symmetry of (Ṁ − 2C) — that passivity-based controllers depend on, which non-canonical formulations of C can inadvertently destroy.
KEY INSIGHTEnergy accounting is a complete theory of motion
Newton asks "what forces act?" The Lagrangian asks "how does energy flow?" Both are complete descriptions of the same physics — but only one makes the structure of the answer legible to a controller designer.
If you've worked through Lesson 3.1, you know that Newton-Euler dynamics recursively propagates forces and torques link-by-link. It is efficient for numerical computation and essential for real-time inverse dynamics. But when you want to write down M(q), C(q,q̇), and g(q) symbolically — to analyze stability, design a passivity-based controller, or prove a Lyapunov argument — Newton-Euler produces a thicket of intermediate vectors that obscure the structure of the answer. The Lagrangian formulation avoids that thicket entirely. It is energy all the way down.
The equation of motion for any robotic manipulator has exactly the form M(q)q̈ + C(q,q̇)q̇ + g(q) = τ. The Lagrangian formulation makes each of these three matrix/vector functions fall out of derivatives of a scalar — the Lagrangian L = T − V. M comes from the second derivative of T with respect to q̇; C comes from a Christoffel-symbol construction on T; g comes from the derivative of V with respect to q. The scalar origin is why each matrix has the analytical structure it does — most importantly, why (Ṁ − 2C) is skew-symmetric. That property is not an accident. It is a consequence of energy conservation, and controllers that depend on it must use a C consistent with it.
CORE CONCEPTGeneralized coordinates: describe the state, not the space
In Newtonian mechanics you write equations in Cartesian coordinates for every mass. A 2-link planar arm has two rigid links, each with a center of mass that moves in 2D. That is four Cartesian coordinates — but the arm only has two independent degrees of freedom, the two joint angles. The other two coordinates are redundant: once you know θ₁ and θ₂, all Cartesian positions follow from kinematics.
Generalized coordinates are the minimal independent set that completely specifies the system configuration. For a 2-link arm, q = [θ₁, θ₂]ᵀ. For a 6-DOF arm, q ∈ ℝ⁶. For a free-floating base plus a 6-DOF arm, q ∈ ℝ¹². The number of generalized coordinates equals the number of degrees of freedom — no more, no less.
A constraint is holonomic if it can be written as f(q) = 0 — a purely positional condition. The "end of link 1 is where link 2 begins" constraint is holonomic: it is already encoded in the forward kinematics. You never write it explicitly in the Lagrangian; you simply parametrize by the joint angles and the constraint is automatically satisfied. Non-holonomic constraints (like a no-slip wheel) cannot be integrated to a position-only form; they require additional treatment. FollowBot's base is non-holonomic; its arm joints are holonomic.
The velocity of every mass element in the system can be expressed as a linear function of the generalized velocities q̇ via the Jacobian of the kinematics. This is the bridge from configuration space to kinetic energy:
import numpy as np def fk_2link(theta1, theta2, l1, l2): # Position of each link's centre of mass (at link midpoint) c1, s1 = np.cos(theta1), np.sin(theta1) c12 = np.cos(theta1 + theta2) s12 = np.sin(theta1 + theta2) p1 = np.array([l1/2 * c1, l1/2 * s1]) # CoM of link 1 p2 = np.array([l1*c1 + l2/2*c12, # CoM of link 2 l1*s1 + l2/2*s12]) return p1, p2 def jacobian_com_2link(theta1, theta2, l1, l2): # Jv1: ∂p1/∂q — (2×2), columns are ∂p1/∂θ1 and ∂p1/∂θ2 c1, s1 = np.cos(theta1), np.sin(theta1) c12 = np.cos(theta1 + theta2) s12 = np.sin(theta1 + theta2) Jv1 = np.array([[-l1/2*s1, 0 ], [ l1/2*c1, 0 ]]) Jv2 = np.array([[-l1*s1 - l2/2*s12, -l2/2*s12], [ l1*c1 + l2/2*c12, l2/2*c12]]) return Jv1, Jv2
The velocity of link i's center of mass is v_i = J_vi(q) · q̇. The kinetic energy contribution is ½ · m_i · v_iᵀ · v_i = ½ q̇ᵀ · (m_i · J_viᵀ J_vi) · q̇. Sum over all links and you have the total kinetic energy as a quadratic form in q̇. The coefficient matrix is M(q).
CORE CONCEPTThe Lagrangian and the Euler-Lagrange equation
Define the Lagrangian L(q, q̇) = T(q, q̇) − V(q), the difference of kinetic and potential energy. The Euler-Lagrange equation states that the equations of motion are:
This single equation, applied coordinate-by-coordinate, generates all n equations of motion simultaneously. No free-body diagrams. No recursive propagation. You differentiate a scalar function twice and you are done.
Write T in terms of q and q̇
T = ½ q̇ᵀ M(q) q̇, where M(q) = Σᵢ [ mᵢ Jᵥᵢᵀ Jᵥᵢ + Jωᵢᵀ Iᵢ Jωᵢ ]. The first term is translational kinetic energy; the second is rotational (Iᵢ is the inertia tensor of link i, Jωi is the angular velocity Jacobian). For a planar arm, the rotational term simplifies to ½ Iᵢ(θ̇₁ + θ̇₂)² for link 2.
Write V in terms of q
V = Σᵢ mᵢ g hᵢ(q), where hᵢ is the height of link i's center of mass. For a planar arm in a vertical plane, h₁ = (l₁/2) sin θ₁ and h₂ = l₁ sin θ₁ + (l₂/2) sin(θ₁ + θ₂). Gravity acts downward (negative y if y is up). This expression encodes which configurations are energetically costly to maintain.
Form L = T − V and apply the Euler-Lagrange operator
The term d/dt(∂T/∂q̇) − ∂T/∂q produces M(q)q̈ + C(q,q̇)q̇, where the Coriolis matrix C is assembled from Christoffel symbols of the first kind: C_ij = Σ_k c_ijk q̇_k with c_ijk = ½(∂M_ij/∂q_k + ∂M_ik/∂q_j − ∂M_jk/∂q_i). The gravity term ∂V/∂q produces g(q). The right side of the Euler-Lagrange equation is τ.
Read off the equation of motion
M(q)q̈ + C(q,q̇)q̇ + g(q) = τ. This is the standard form used by every manipulator controller in this stage. Each matrix has physically interpretable structure: M is symmetric positive definite (it is a generalized inertia), C contains the velocity-dependent Coriolis and centrifugal terms, g contains the configuration-dependent gravity loads.
HOW IT WORKSDeriving M, C, g for a 2-link planar arm
Let the arm have link lengths l₁, l₂; link masses m₁, m₂ concentrated at each link's midpoint; and rotational inertias I₁, I₂ about each joint axis (into the page). Gravity is g = 9.81 m/s² pointing downward. The arm moves in a vertical plane.
Step 1: Kinetic energy. The velocity of link 1's CoM is v₁ = (l₁/2)θ̇₁ (tangential). The velocity of link 2's CoM has contributions from both joints. After computing the Jacobians and summing translational and rotational KE:
import numpy as np def inertia_matrix(q, l1, l2, m1, m2, I1, I2): # q = [theta1, theta2] c2 = np.cos(q[1]) # M11: inertia seen by joint 1 M11 = (I1 + I2 + m1*(l1/2)**2 + m2*(l1**2 + (l2/2)**2 + 2*l1*(l2/2)*c2)) # M12 = M21: coupling inertia M12 = I2 + m2*((l2/2)**2 + l1*(l2/2)*c2) # M22: inertia seen by joint 2 alone M22 = I2 + m2*(l2/2)**2 return np.array([[M11, M12], [M12, M22]]) def coriolis_matrix(q, qdot, l1, l2, m1, m2): # Christoffel symbol c_ijk = ½(∂M_ij/∂q_k + ∂M_ik/∂q_j − ∂M_jk/∂q_i) # For 2-link arm, only ∂M/∂θ2 is non-zero (M depends on cos(θ2)) s2 = np.sin(q[1]) h = m2 * l1 * (l2/2) * s2 # the coupling parameter # C assembled from Christoffel symbols (canonical form) C = np.array([[-h*qdot[1], -h*(qdot[0]+qdot[1])], [ h*qdot[0], 0 ]]) return C def gravity_vector(q, l1, l2, m1, m2, g=9.81): c1 = np.cos(q[0]) c12 = np.cos(q[0] + q[1]) g1 = (m1*(l1/2) + m2*l1) * g * c1 + m2*(l2/2) * g * c12 # gravity on joint 1 (∂V/∂q1: spans both links) g2 = m2 * (l2/2) * g * c12 # gravity on joint 2 return np.array([g1, g2]) def forward_dynamics(q, qdot, tau, l1, l2, m1, m2, I1, I2, g=9.81): # q̈ = M⁻¹ (τ − C·q̇ − g) M = inertia_matrix(q, l1, l2, m1, m2, I1, I2) C = coriolis_matrix(q, qdot, l1, l2, m1, m2) gv = gravity_vector(q, l1, l2, m1, m2, g) return np.linalg.solve(M, tau - C@qdot - gv)
Three things to internalize from this code. First, M depends on θ₂ but not θ₁ — the inertia matrix is invariant to the absolute orientation of the arm (no gravity in T) but sensitive to the elbow bend. This is why carrying a heavy load at full arm extension feels much harder than with the elbow bent. Second, C has exactly the Christoffel structure — C[0,0] = −h θ̇₂ represents the centrifugal effect of link 2 spinning on joint 1; C[1,0] = +h θ̇₁ is the Coriolis reaction. Third, g(q) depends on both joint angles — the load on joint 2 is zero when the elbow is horizontal (cos(θ₁ + θ₂) = 0) even though gravity still acts; this is a useful null-space exploitation in real controllers.
M₁₁ is the moment of inertia that joint 1's motor must overcome when accelerating the entire arm. M₁₂ = M₂₁ is the coupling inertia: commanding joint 1 to accelerate also accelerates joint 2's motor (and vice versa). When the arm is fully extended (θ₂ = 0), M₁₂ is at its maximum — the coupling is strongest. When fully folded (θ₂ = π), M₁₂ ≈ I₂ only — the coupling is weakest. Independent joint PID controllers ignore M₁₂. That is exactly why they struggle at high speeds and extended configurations.
SEE ITLagrangian energy breakdown — sweep joints, toggle Coriolis
The widget below shows a 2-link arm with configurable joint angles. The stacked bar shows the energy budget: kinetic energy T (broken into link 1 and link 2 contributions) and potential energy V. Toggle Coriolis terms on or off, then set a non-zero joint velocity — you will see the trajectory diverge when Coriolis terms are dropped. The bad choice: setting high velocity and toggling Coriolis off. That is exactly what a naive PD controller does implicitly.
The critical observation: at low velocity scale the energy breakdown is dominated by potential energy (the arm must hold itself against gravity). As velocity scale increases, kinetic energy rises quadratically — and the Coriolis terms that couple the joints become significant. When you toggle Coriolis off, the ghost trajectory in the SVG (red dashes) shows where the end effector would travel under a controller that ignores cross-joint coupling. The divergence grows with velocity. This is why gravity-compensation-only feedforward works at low speeds but fails at high speeds: Coriolis is not a second-order correction, it is a first-order physics term at the velocities a robot arm reaches during rapid point-to-point motion.
COMPARISONNewton-Euler vs Lagrangian: when each wins
The two formulations produce identical physics. The choice is about which direction of computation is cheaper and more legible for your purpose.
| Criterion | Newton-Euler (Lesson 3.1) | Lagrangian (this lesson) |
|---|---|---|
| Numerical efficiency | O(n) recursive — fast for real-time inverse dynamics on 6+ DOF arms | O(n³) for full M — expensive to compute numerically for large n |
| Symbolic M, C, g structure | Requires symbolic manipulation of the recursive chain — algebraically dense | Falls out of scalar differentiation — structure is explicit and readable |
| Controller design and analysis | Hard to reason about passivity, skew-symmetry, energy shaping | Energy shaping, passivity proofs, Lyapunov arguments all natural |
| Constrained systems | Constraints require additional multiplier terms | Holonomic constraints absorbed automatically by choice of coordinates |
| Real-time inverse dynamics | Industry standard — used in ROS 2 ros2_control and most commercial platforms | Not used directly — pre-computed symbolically, then evaluated numerically |
| Teaching controller stability | Difficult to introduce passivity concept cleanly | Passivity argument is one line: d/dt (T+V) = τᵀq̇ − losses |
In practice, roboticists derive M, C, g symbolically using the Lagrangian (often with a computer algebra system like SymPy), verify the result, then evaluate the symbolic expressions numerically at runtime. This gives the analytical structure of the Lagrangian with the numerical speed of explicit matrix evaluation. The Newton-Euler recursion is reserved for cases where the number of DOF makes even O(n²) symbolic expressions impractical — typically 7+ DOF arms or full humanoid bodies.
There is also a third method worth naming: the Kane method (also called virtual work / generalized forces). It avoids the explicit Lagrangian but recovers the same generalized-coordinate structure. For systems with many non-holonomic constraints (like legged robots), Kane's method can be more convenient than either Newton-Euler or the Lagrangian. It is not covered in this lesson; see Craig's Introduction to Robotics for a careful treatment.
FOLLOWBOTFull dynamics model for FollowBot's arm
FollowBot's 6-DOF arm is used for handoff and gesture response — it is not a high-speed industrial arm, but it does need to move smoothly under varying payload (holding a package vs. empty). The dynamics model is the foundation for every controller choice in Stage 3.
Using the Lagrangian formulation (extended to 6 DOF with SymPy for symbolic derivation), we have: M(q) ∈ ℝ⁶ˣ⁶, C(q,q̇) ∈ ℝ⁶ˣ⁶, g(q) ∈ ℝ⁶. These are exported as optimized C++ functions via code generation and loaded into the ROS 2 ros2_control hardware interface. The PID velocity controller in Lesson 3.3 runs on top of this model. Lesson 3.4 adds feedforward from inverse dynamics — computed via the Newton-Euler recursion (O(n)) — to reduce tracking error. The figure-eight trajectory milestone (cross-track error below 8 mm) requires both the dynamical model and the feedforward to be correct.
A minimal ROS 2 node that publishes the instantaneous inertia matrix and potential energy for FollowBot's arm — useful for monitoring during commissioning:
import rclpy from rclpy.node import Node from sensor_msgs.msg import JointState from std_msgs.msg import Float64MultiArray import numpy as np # Import auto-generated dynamics (SymPy → C++ → Python wrapper) from followbot_dynamics import inertia_matrix, gravity_vector class DynamicsMonitor(Node): def __init__(self): super().__init__('dynamics_monitor') self.create_subscription( JointState, '/joint_states', self.cb, 10 ) self.pub_M = self.create_publisher( Float64MultiArray, '/arm/inertia_diag', 10 ) self.pub_V = self.create_publisher( Float64MultiArray, '/arm/potential_energy', 10 ) def cb(self, msg): q = np.array(msg.position[:6]) # first 6 joints = arm M = inertia_matrix(q) g = gravity_vector(q) # Diagonal of M: each element is the "apparent inertia" of that joint out_M = Float64MultiArray() out_M.data = M.diagonal().tolist() self.pub_M.publish(out_M) # Potential energy: V = ∫ g(q) dq — approximated as sum(g·q offset) # For monitoring, publish the gravity torque vector directly out_V = Float64MultiArray() out_V.data = g.tolist() self.pub_V.publish(out_V) def main(): rclpy.init() rclpy.spin(DynamicsMonitor()) rclpy.shutdown() if __name__ == '__main__': main()
Three things to notice. First, the dynamics functions (inertia_matrix, gravity_vector) are auto-generated by SymPy's code generation and compiled — they run in C++ and are called via a Python wrapper. This is the standard pattern in production: derive symbolically, deploy numerically. Second, M.diagonal() gives you the apparent joint inertia at each joint. Watching this change as the arm extends versus folds is a powerful commissioning check — a sudden discontinuity indicates a kinematics bug. Third, publishing the gravity torque vector g(q) continuously lets you check gravity compensation in near-static tests before enabling full dynamic control.
WEAKNESSWhere this formulation bites
The product C(q,q̇)q̇ is unique — there is only one correct centrifugal/Coriolis force vector. But the matrix C itself is not: you can add any matrix N to C and subtract N·q̇ from the product without changing the resulting force. The canonical choice — the Christoffel-symbol form — gives C the special property that (Ṁ − 2C) is skew-symmetric. This means q̇ᵀ(Ṁ − 2C)q̇ = 0, which is the key step in the passivity proof for computed-torque and adaptive controllers. A non-canonical C — for example, the naive factorization sometimes found in older textbooks that symmetrizes a different way — can break this property. If you then use that C in a passivity-based stability proof, the proof is wrong. The robot may still work, but you have lost the theoretical guarantee.
Every derivation above assumes rigid-body kinematics: link lengths and inertias are constant, no flexibility. Real industrial arms have joint flexibility (backlash, gear compliance) and, for long links, structural flexibiliy. When the bandwidth of your controller exceeds the lowest structural resonance frequency of the arm (common above 20 Hz on large arms), the rigid-body model is actively wrong — it predicts stable behavior where the true system oscillates. This is the sim-to-real gap covered in Lesson 3.3. The fix is either to limit controller bandwidth well below the resonance, or to augment the Lagrangian with elastic potential energy terms (flexible link dynamics). Most practical arms stay below the resonance frequency by design; Lesson 3.3 addresses what to do when you cannot.
Many deployed joint-space PD controllers treat each joint independently: τᵢ = Kₚ(qᵢ_d − qᵢ) + Kd(q̇ᵢ_d − q̇ᵢ). This is exactly equivalent to assuming M = diag(M₁₁, M₂₂, …) and C = 0. At low speeds and for arms with small coupling inertias (M₁₂ small relative to M₁₁ and M₂₂), this works. The problem is that as the arm extends or speeds up, M₁₂ grows and C·q̇ becomes significant — and the independent PD controller compensates with higher gains, which moves the closed-loop poles toward instability. Engineers who do not know the dynamics model often respond by reducing gains to avoid oscillation, then wonder why tracking error increases. The root cause is an implicit wrong model, not a tuning problem.
KEY INSIGHTHow this connects
- 3.1 · Newton-Euler Dynamics: Forces, Torques, and Rigid Bodies — the recursive formulation that precedes this lesson. Newton-Euler is numerically efficient; the Lagrangian is analytically tractable. Both produce the same M, C, g — use Newton-Euler for real-time O(n) inverse dynamics, use the Lagrangian for symbolic derivation and controller analysis.
- 3.3 · Actuator Models and the Sim-to-Real Dynamics Gap — next lesson. The dynamics model derived here assumes rigid links and ideal torque actuation. Lesson 3.3 introduces motor dynamics, gear-train elasticity, and friction models that modify the effective equation of motion and create the gap between simulation and hardware.
The Lagrangian formulation is the analytic engine of Stage 3. Every controller that follows — PD with gravity compensation (Lesson 3.5), computed-torque (Lesson 3.6), LQR (Lesson 3.9) — is designed by manipulating the equation of motion M(q)q̈ + C(q,q̇)q̇ + g(q) = τ. The passivity property of C, derived here, is load-bearing for the stability proof in Lesson 3.6. The energy terms T and V, visualized in the widget, are the Lyapunov function candidates used in Lesson 3.7. Everything in Stage 3 flows from this lesson's scalar Lagrangian.
GLOSSARYKey terms introduced
- Generalized coordinates (q)
- The minimal independent set of configuration variables that completely specifies the system state — e.g., joint angles for a manipulator. Eliminates redundant Cartesian coordinates by working directly in the configuration space of the system.
- Lagrangian (L)
- L(q, q̇) = T(q, q̇) − V(q), the difference of kinetic and potential energy. The scalar function from which the equations of motion are derived via the Euler-Lagrange equation. Its scalar nature is why the resulting matrix structure is analytically transparent.
- Euler-Lagrange equation
- d/dt(∂L/∂q̇) − ∂L/∂q = τ. Applied coordinate-by-coordinate to the Lagrangian, it produces all n equations of motion simultaneously. The left side yields M(q)q̈ + C(q,q̇)q̇ + g(q); the right side is the applied torque.
- Inertia matrix M(q)
- The n×n symmetric positive definite matrix that maps joint accelerations to inertial torques: M(q)q̈. Assembled from the Jacobians of each link's center of mass: M = Σᵢ(mᵢJᵥᵢᵀJᵥᵢ + JωᵢᵀIᵢJωᵢ). Depends on configuration but not velocity.
- Coriolis matrix C(q,q̇)
- The n×n matrix encoding Coriolis and centrifugal terms: C(q,q̇)q̇. Assembled from Christoffel symbols of the first kind derived from M. The canonical Christoffel-symbol form ensures (Ṁ − 2C) is skew-symmetric — a property required by passivity-based stability proofs.
- Gravity vector g(q)
- The n-dimensional vector of gravity-induced torques at each joint: g(q) = ∂V/∂q. Configuration-dependent. Gravity compensation controllers cancel this term explicitly, allowing the remaining controller to act on a "gravity-free" system.
- Skew-symmetry of (Ṁ − 2C)
- The property that q̇ᵀ(Ṁ − 2C)q̇ = 0 for all q̇. It follows from the fact that T = ½ q̇ᵀMq̇ is the total kinetic energy, and differentiating gives a relationship that forces this cancellation. It is the key step in passivity proofs for computed-torque and adaptive controllers.
- Holonomic constraint
- A constraint that can be written as f(q) = 0 — a purely position-level condition. Holonomic constraints reduce the number of generalized coordinates needed. In contrast, non-holonomic constraints (e.g., no-slip wheel rolling) involve velocities and cannot be integrated to a position-only form, requiring separate treatment.
ACTIVE RECALLCheck your understanding
Answer in your own words first, then reveal.
1Why does the inertia matrix M(q) for a 2-link arm depend on θ₂ but not θ₁?›
Kinetic energy involves velocities, not absolute positions. The absolute orientation of the arm (captured by θ₁) does not change the kinetic energy of a uniform gravitational field — it only matters for potential energy (which goes into g(q), not M). The elbow angle θ₂, however, changes the geometry of how the links relate to each other — specifically, it changes the cross-term cos θ₂ in M₁₂ and in M₁₁. When θ₂ = 0 (arm fully extended), link 2's mass is at maximum moment arm from joint 1, making M₁₁ large. When θ₂ = π (fully folded), link 2's mass is near joint 1, making M₁₁ smaller. This configuration sensitivity of M is exactly why independent joint controllers struggle: the "plant" they are trying to control changes as the arm moves.
2A robotics paper proposes a new Coriolis matrix C* that is symmetric (C* = C*ᵀ). What property is likely lost, and why does it matter for control?›
The canonical Christoffel-symbol form of C is generally not symmetric — it is constructed so that (Ṁ − 2C) is skew-symmetric. A symmetrized C* gives the same C*q̇ force vector at every instant, but it does not satisfy q̇ᵀ(Ṁ − 2C*)q̇ = 0. That means the passivity property is lost. This matters because passivity-based computed-torque controllers and many adaptive controllers have stability proofs that use the step: "the term q̇ᵀ(Ṁ − 2C)q̇ vanishes due to skew-symmetry." If C is replaced with C*, that step is invalid. The controller may still work in practice (the force vector is the same), but the published stability proof is incorrect, and stability guarantees derived from it are unreliable. A reviewer should always check which C formulation a control paper uses before accepting its stability argument.
3When would you choose Newton-Euler over the Lagrangian formulation for a 6-DOF arm?›
Choose Newton-Euler for real-time inverse dynamics: given q, q̇, q̈ and the desired acceleration, compute the required joint torques τ = M(q)q̈ + C(q,q̇)q̇ + g(q). The Newton-Euler recursion is O(n) — linear in the number of joints — and runs in under 10 µs on a modern CPU for a 6-DOF arm. The Lagrangian formulation requires assembling M (O(n³) matrix operations if done naively, or O(n²) with care) and is not competitive for real-time use. Conversely, choose the Lagrangian whenever you need the symbolic form of M, C, or g — for controller design, stability analysis, passivity proofs, or parameter identification. In practice, use the Lagrangian to derive, use Newton-Euler to evaluate at runtime.
4An independent joint PD controller performs well at low speeds but begins to oscillate at high joint velocities. Without changing the gains, identify the dynamic term most likely responsible.›
The Coriolis/centrifugal term C(q,q̇)q̇ is most likely responsible. At low velocities, C·q̇ is quadratically small and the independent PD controller can compensate it with its derivative gain. As velocity increases, C·q̇ grows as q̇², and the un-modeled coupling torques between joints drive the joints off trajectory. The PD controller, seeing increasing tracking error, increases its commanded torque — which can excite oscillations in the coupled dynamics. The fix is to add a feedforward Coriolis term computed from the current q and q̇, which cancels C·q̇ before it reaches the tracking loop. The coupling inertia M₁₂ may also contribute if the arm is near full extension, but M₁₂ only causes cross-coupling under acceleration, not sustained velocity — making the velocity-dependent Coriolis term the primary suspect here.