A Propagation Perspective on Recursive Forward Dynamics for Systems With Kinematic Loops

We revisit the concept of constraint embedding as a means for dealing with kinematic loop constraints during dynamics computations for rigid-body systems. Specifically, we consider the local loop constraints emerging from common actuation submechanisms in modern robotics systems (e.g., geared motors, differential drives, and four-bar mechanisms). As a complementary perspective to prior work on constraint embedding, we present an analysis that generalizes the traditional concepts of joint models and motion/force subspaces between individual rigid bodies to generalized joint models and motion/force subspaces between groups of rigid bodies subject to loop constraints. We then use these generalized concepts to derive the constraint-embedded recursive forward dynamics algorithm using multihandle articulated bodies. We demonstrate the broad applicability of the generalized joint concepts by showing how they also lead to the constraint-embedding-based recursive algorithm for inverse dynamics. Lastly, we benchmark our open-source implementation in C++ for the forward dynamics algorithm against state-of-the-art, sparsity-exploiting algorithms. Our alternative derivation is intended to make the constraint-embedding methodology more accessible to the broader robotics community, while the benchmarking study clarifies the relative strengths and limitations of constraint embedding versus sparsity-exploiting methods. Indeed, our benchmarking validates that constraint embedding outperforms the nonrecursive alternative in cases involving local kinematic loops.