Closed loops by constraint embedding#

GRiD models a closed kinematic loop by embedding it: the bodies of the loop stay in the spanning tree, the dependent joint coordinates become explicit functions of the independent ones, and the tree recursions run on the reduced coordinates. A follower joint whose coordinate is a function of a leader’s is a one-body cluster with the motion subspace \(S(y) = X\,\mathrm{diag}(S_i)\,G(y)\) of the constraint-embedding literature (Jain; Chignoli and Wensing); the URDF mimic joint is the case of a constant \(G\), and GRiD’s mimic fold has always been that embedding. Multipliers, proximal iterations and soft equality constraints are not used: the tree recursions, the branch parallelism, the per-body threads and the derivative column parallelism are unchanged, and no constraint force appears.

The projection#

With \(q = F(y)\) the full-tree coordinates of the independent ones, \(G = \partial F / \partial y\) and \(\dot q = G \dot y\), \(\ddot q = G \ddot y + \dot G \dot y\), every reduced quantity is the projection of the full tree’s:

\[\tau_y = G^{\top} \tau(F(y), G\dot y, G\ddot y + \dot G \dot y), \qquad M_y = G^{\top} M G, \qquad c_y = G^{\top}\,\mathrm{rnea}(F(y), G\dot y, \dot G \dot y),\]

and the derivatives follow by the chain rule (\(\partial G / \partial y\) carries the second derivative of \(F\)). The numpy reference implements exactly this projection (RBDReference/_embedding.py) on the full spanning tree, which URDFParser.parse(..., reduce_couplings=False) returns; on a mimic robot it agrees with the reduced recursions to round-off, and it is the oracle the generated kernels are checked against.

Explicit coupling kinds#

  • Linear (the URDF <mimic joint multiplier offset>): a constant multiplier. Every algorithm supports it; the fold is baked into the generated code and the headers of such robots are unchanged by the machinery below.

  • Polynomial (<mimic joint="l" polycoef="c0 c1 c2 c3 c4" q0="..."/>, the MuJoCo joint equality): \(q = \sum_i c_i (q_l - q_0)^i\) of degree at most four. The fold multiplier \(\alpha(q_l) = dq/dq_l\) and its derivatives are evaluated on the device from the leader’s coordinate; the follower accelerates by \(\alpha' \dot q_l^{\,2}\) on top of \(\alpha \ddot q_l\), and the gradients carry the \(\alpha'\), \(\alpha''\) and \((\partial G/\partial q)^{\top} f\) terms. See URDFParser for the element.

Joint damping and friction on a follower#

A follower’s <dynamics damping="b" friction="f"/> acts in the follower’s own coordinate, \(b \dot q_f + f\,\mathrm{sign}(\dot q_f)\) with \(\dot q_f = \alpha \dot q_l\), and projects onto its leader through \(\alpha\) like every other follower torque:

\[\tau_l \mathrel{+}= \alpha^{2} b\,\dot q_l + |\alpha|\, f\,\mathrm{sign}(\dot q_l),\]

the congruence \(G^{\top} B G\) of the embedding. The sign of \(\alpha\) cancels in the Coulomb term, and the dissipated power \(\dot q_l \tau_l\) is non-negative for every ratio (a single power of \(\alpha\) would inject energy on a negative one). Under the smooth friction_model="tanh" the width belongs to the reduced coordinate: the shape is \(\tanh(\dot q_l / w_l)\), scaled by \(|\alpha|\). For a polynomial follower \(\alpha(q_l)\) is evaluated on the device, and the gradient of the bias then carries, on the leader’s diagonal of \(\partial \tau / \partial q\), the term \(2 \alpha \alpha' b\,\dot q_l + \mathrm{sign}(\alpha)\,\alpha' f\,s(\dot q_l)\); the runtime-mutable joint-dynamics table (runtime_joint_dynamics), a constant per reduced slot, is refused for a damped polynomial follower. The reference checks this fold against the projection of a full spanning tree that carries the damping itself, on a chain with ratios 2 and -1.5 (RBDReference/tests/test_joint_dynamics_coupling_fold.py).

Planar four-bar closures and loops without a closed form (the <loop> element of URDF+, the connect and weld equalities of MJCF) are the next cluster kinds; they close the loop numerically inside the kernel and differentiate it by the implicit function theorem.

What a polynomial robot can generate today#

The generator emits, for a robot with a polynomial follower: the inverse dynamics, CRBA, \(M^{-1}\), forward dynamics and ABA and both first order gradients; the end-effector pose and its gradient, also with runtime targets; the frame Jacobian family (J, J̇, the task inertia, the frame velocity and acceleration and their derivatives); the six integrators with their [A | B] gradients; the joint-torque regressor, the energy regressors and the forward-dynamics parameter gradient; the generalized gravity, the bias, the Coriolis matrix, the CoM, the centroidal momentum matrix and its derivatives (dccrba and cmm_time_variation, which carry the term \(A_f[:, f]\,\alpha'\) of \(A_y = A_f G\) at the leader’s cell), the energies, the joint damping and friction folds, and the external-force gradients (f_ext_gradient and its configuration derivative, whose reduced form \(\partial(G^{\top} J_f^{\top})/\partial y\) carries the follower’s own Jacobian row times \(\alpha'\)). The reduced Coriolis matrix is \(C_y = G^{\top} C_f G + G^{\top} M_f \dot G\), so that \(C_y \dot y + g_y\) is the bias and \(C_y + C_y^{\top} = \dot M_y\); the kernel assembles the second term from the composite inertias of its backward pass. Each is checked on the GPU against the projection on a fixed and a floating-base chain (test/cuda_equivalents/test_cuda_polynomial_coupling.py).

Every other algorithm still bakes a constant multiplier into its fold and gen_all_code refuses it for such a robot with a message that names it: the pose Hessian and the multi-target kernels, the regressor gradient, the second-order derivatives and the plant layer. register_robot therefore takes an explicit algorithm_list for a polynomial robot (test/python_wrappers/test_polynomial_coupling_handle.py).