This paper extends an existing algorithm for the first-order derivatives of rigid-body dynamics to the case of closed-chain kinematic systems modeled using constraint embed- ding. Many standard dynamics algorithms apply to both open- chain and constraint-embedded models, but existing efficient derivative methods assume joint velocity effects are locally config- uration invariant. We remove this assumption and derive adapted algorithms that extend dynamics derivatives to more general joint types, including those arising in constraint-embedded closed- chain models. Our results compare conventional pin-joint robot models with more complete actuation models that capture local closed chains. We show that the additional terms introduced by these generalizations have low computational impact when mod- eling actuation kinematics alone, but can incur higher cost when additional rigid bodies, such as motor rotors, are included in the actuation chain, or when considering non-local loops. Overall, these results enable more accurate dynamics computations for constraint-embedded actuation submechanisms to be adopted in model-predictive control and differentiable simulation.
Figures & tables
Fig. 1: Body numbering and notation examples for (a) an open-chain system and (b) a constraint-embedding model applied to a closed-chain system.
Fig. 2: Actuation mechanisms for the (a) MIT Humanoid Leg, (b) Tello humanoid [ 14 ] , and (c) Cassie Leg. The MIT Humanoid [ 9 ] has a coupled belt drive for the knee and ankle. Tello [ 8 ] has a 2DoF differential hip mechanism and a coupled linkage mechanism for the knee and ankle. Cassie has four bar mechanisms for its distal DoFs.
Robot
∂τ/∂q
∂τ/∂q˙
Kuka LWR 4+
1.07e-14
3.33e-16
Mini Cheetah
2.84e-14
2.66e-15
MIT Humanoid
2.84e-14
1.07e-14
Cassie
8.53e-14
1.11e-14
Tello
4.26e-14
1.78e-15
TABLE I: Maximum Error of the Inverse Dynamics Derivatives
Fig. 3: Per-joint error for the inverse dynamics derivatives for MIT Mini Cheetah with motor rotors modeled using constraint embedding.
Fig. 4: Algorithm speed for full and simplified robot models with clang. Fwd refers to the forward pass of Algorithm 1 (lines 2-11) and Bwd refers to its backward pass (lines 12-28). +/− M indicate with/without actuation mechanism transmission kinematics and +/− R indicate with and without motor rotors.
Fig. 5: Scaling performance of kinematic trees with different joint types and topologies (serial chain vs. binary tree).
Fig. 6: Computation time vs. loop depth for two 10-body chains connected by an additional body at varying depths. Two three link chains connected at a depth of 2 are shown in the inset.