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:
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:
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).