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 sub-mechanisms 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 multi-handle 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 non-recursive alternative in cases involving local kinematic loops.