A Versatile Quaternion-Based Constrained Rigid Body Dynamics

We present a constrained Rigid Body Dynamics (RBD) that guarantees satisfaction of kinematic constraints, enabling direct simulation of complex mechanical systems with arbitrary kinematic structures. To ensure constraint satisfaction, we use an implicit integration scheme. For this purpose, we derive compatible dynamic equations expressed through the quaternion time derivative, adopting an additive approach to quaternion updates instead of a multiplicative one, while enforcing quaternion unit-length as a constraint. We support all joints between rigid bodies that restrict subsets of the three translational or three rotational degrees of freedom, including position- and force-based actuation. Their constraints are formulated such that Lagrange multipliers are interpretable as joint forces and torques. We discuss a unified solution strategy for systems with redundant constraints, overactuation, and passive degrees of freedom, by eliminating redundant constraints and navigating the subspaces spanned by multipliers. As our method uses a standard additive update, we can interface with unconditionally-stable implicit integrators. Moreover, the simulation can readily be made differentiable as we show with examples.