diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 3dc486b8..62d0c7e6 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -661,22 +661,13 @@ from its default. .. _option-flag-invdiscrete: :at:`invdiscrete`: :at-val:`[disable, enable], "disable"` - This dual-purpose flag enables discrete-time inverse dynamics and disables :ref:`midpoint integration`. - - Enable discrete-time inverse dynamics - This flag **enables** discrete-time inverse dynamics with :ref:`mj_inverse` for all - :ref:`integrators` other than ``RK4``. Recall from the :ref:`numerical - integration` section that the one-step integrators (``Euler``, ``implicit`` and ``implicitfast``), - modify the mass matrix :math:`M \rightarrow M-hD`. This implies that finite-differenced accelerations - :math:`(v_{t+h} - v_t)/h` will not correspond to the continuous-time acceleration ``mjData.qacc``. When this flag - is enabled, :ref:`mj_inverse` will interpret ``qacc`` as having been computed from the difference of two sequential - velocities, and undo the above modification. - - Disable midpoint integration - Additionally and relatedly, this flag **disables** :ref:`midpoint integration` for free bodies, which - would otherwise break the linear relationship between finite-differenced velocities and forces assumed by discrete - inverse dynamics. Note that disabling midpoint integration might be useful for debugging or for other reasons, - regardless or whether inverse dynamics are used. + This flag enables discrete-time inverse dynamics with :ref:`mj_inverse` for all + :ref:`integrators` other than ``RK4``. Recall from the + :ref:`numerical integration` section that the one-step integrators (``Euler``, ``implicit`` and + ``implicitfast``), modify the mass matrix :math:`M \rightarrow M-hD`. This implies that finite-differenced + accelerations :math:`(v_{t+h} - v_t)/h` will not correspond to the continuous-time acceleration ``mjData.qacc``. + When this flag is enabled, :ref:`mj_inverse` will interpret ``qacc`` as having been computed from the difference of + two sequential velocities, and undo the above modification. .. _option-flag-multiccd: diff --git a/doc/changelog.rst b/doc/changelog.rst index b4fc3405..83d21310 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,6 +7,14 @@ Upcoming version (not yet released) General ^^^^^^^ +- Replaced midpoint integration of free bodies with :ref:`gyroscopic derivatives` in the ``implicitfast`` + :ref:`integrator`: the bias-force derivative of every standalone free body is applied via a local + unsymmetric solve of its decoupled block, making ``implicitfast`` identical to ``implicit`` for such bodies. + Unlike midpoint integration, which required vacuum and no constraints, this applies in all environments (contacts, + fluid, constraints), and is compatible with discrete-time inverse dynamics. Spinning free bodies no + longer gain energy, but tumbling motion is now mildly damped; models requiring long-horizon energy conservation of + tumbling bodies in vacuum should use ``RK4``. The :ref:`invdiscrete` flag no longer has any + effect on forward dynamics. - Added Nesterov momentum extrapolation with adaptive gradient restart (O'Donoghue-Candès) to the PGS solver, significantly improving convergence. Overall PGS now requires ~2x fewer iterations. - Added the Newton decrement -- the quadratic model's predicted cost improvement of the next iteration -- as a third @@ -233,7 +241,7 @@ General improving performance by ~20%. 3. :commit:`b9c1877e` Added support for :ref:`elastic2d` for trilinear and quadratic flex :ref:`dofs`. -4. :commit:`910b3336` :ref:`Midpoint integration` is now restricted to the ``implicitfast`` +4. :commit:`910b3336` Midpoint integration is now restricted to the ``implicitfast`` :ref:`integrator` and is disabled when fluid forces are active (nonzero :ref:`density` or :ref:`viscosity`). Midpoint integration treats external forces as zero-order-hold constants, which causes @@ -335,7 +343,7 @@ General scalar arrays (``jnt_stiffness``, ``dof_damping``, etc.) continue to hold the linear coefficient and are unchanged. The polynomial order is defined by the new constant :ref:`mjNPOLY`. A future breaking C-API change may unify the linear and higher-order coefficients into a single array. -4. :commit:`0c337799` Added :ref:`midpoint integration` for standalone free bodies in ``implicit`` and +4. :commit:`0c337799` Added midpoint integration for standalone free bodies in ``implicit`` and ``implicitfast`` :ref:`integrators`. This applies the implicit midpoint rule to the rotational dynamics of free bodies with no children, conserving kinetic energy to machine precision in the absence of external torques. The :ref:`invdiscrete` flag now also disables midpoint integration, providing an diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 412495ce..8f191e6f 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -580,50 +580,24 @@ Solving for :math:`v_{t+h}`, we obtain the implicit-in-velocity update \widehat{M} &\equiv M-h D \end{aligned} -.. _geMidpoint: +.. _geFreeBody: -Midpoint integration for free bodies in vacuum - The implicit-in-velocity update :eq:`eq_implicit_update` treats the acceleration as a function of velocity and - linearizes. While effective for damping-like forces, it is sub-optimal for rotational dynamics, where - Coriolis and gyroscopic forces are *quadratic* in angular velocity. For this case, a better approach is to directly - discretize the rotational equations of motion using the *midpoint method*. +Gyroscopic derivatives for free bodies + The ``implicitfast`` integrator described :ref:`below` excludes the derivatives of centripetal, + Coriolis and gyroscopic forces from :math:`D`, so that :math:`\widehat M` remains symmetric and can be factorized + with the faster Cholesky decomposition. However integrating gyroscopic forces explicitly can lead to + energy gain and divergence of fast-spinning free bodies with asymmetric inertia. - Consider a rigid body rotating in its principal-axis frame with angular velocity - :math:`\omega \in \mathbb{R}^3` and diagonal inertia tensor :math:`I = \text{diag}(I_1, I_2, I_3)`. The rotational - dynamics are given by `Euler's rotation equation - `__: + Therefore for *standalone free bodies* (free joints whose body has no children), these derivatives are reinstated. + The rows of :math:`\widehat M` corresponding to such a body form a :math:`6\times 6` block which is decoupled from + the rest of the system. After the global Cholesky solve, this block is re-assembled with the exact derivative of the + body's bias force and re-solved with an optimized :math:`6\times 6` LU routine. For standalone free bodies, + ``implicitfast`` and ``implicit`` therefore compute identical updates. - .. math:: - I \dot{\omega} + \omega \times I\omega = \tau - - where :math:`\tau` is the external torque in the principal-axis frame. - Evaluating the velocities at the midpoint, :math:`\omega_\text{mid} = (\omega_t + \omega_{t+h})/2`, gives: - - .. math:: - \frac{2}{h} I (\omega_\text{mid} - \omega_t) + \omega_\text{mid} \times I \omega_\text{mid} = \tau - - This is a system of 3 nonlinear equations in 3 unknowns :math:`\omega_\text{mid}`, solved at each timestep using - Newton's method with a backtracking line search. After solving, the new velocity is recovered as - :math:`\omega_{t+h} = 2\omega_\text{mid} - \omega_t`. - - **Properties.** The midpoint method preserves all `quadratic first integrals - `__ of the ODE. For Euler's equations, these are the - kinetic energy :math:`H = \frac{1}{2}\omega^T I\omega` and the squared angular momentum - :math:`C = \frac{1}{2}|I\omega|^2`, both conserved exactly in the absence of external torque. Since :math:`C` is the - Casimir function of the `Lie-Poisson `__ structure, the midpoint - method is a symmetric (time-reversible) and second-order accurate *Poisson integrator*. - - **Eligibility.** Midpoint integration is only applied when using the ``implicitfast`` integrator, to - free bodies with no child bodies, and only when the medium has zero :ref:`density` and - :ref:`viscosity`. - - **Performance.** While the midpoint method carries computational overhead, we've found it to be - negligible compared to the rest of the pipeline, on the order of 1% in the worst case. - - **Disabling.** Because midpoint integration solves a nonlinear equation for the next velocity, it breaks the linear - relationship between finite-differenced velocities and forces assumed by discrete inverse dynamics. Therefore, - setting the :ref:`invdiscrete` flag disables midpoint integration, and also provides a - general opt-out mechanism for this integrator. + **Properties.** The kinetic energy of a spinning free body is non-increasing in the absence of applied force. + Steady spins about principal axes are conserved almost exactly; tumbling motion is mildly damped, at a rate + scaling like :math:`(h|\omega|)^2` per step. Systems requiring long-horizon energy conservation of tumbling + bodies should use the ``RK4`` integrator. .. _geIntegrators: @@ -661,8 +635,8 @@ Fast implicit-in-velocity (``implicitfast``) scenarios which are not common and already well-handled by the Runge-Kutta integrator (see below). Because the RNE derivatives are also the main source of asymmetry of :math:`D`, by dropping them and symmetrizing, we can use the faster :math:`L^TL` rather than :math:`LU` decomposition. - The ``implicitfast`` integrator applies :ref:`midpoint integration` to eligible free bodies in vacuum, - providing exact energy conservation for spinning objects at negligible additional cost. + For standalone free bodies, the dropped :ref:`gyroscopic derivatives` are reinstated with a local + unsymmetric solve, preventing energy gain of spinning bodies at negligible additional cost. 4th-order Runge-Kutta (``RK4``) One advantage of our continuous-time formulation is that we can use higher order integrators such as Runge-Kutta or @@ -696,11 +670,10 @@ Fast implicit-in-velocity (``implicitfast``) increased stability, and is therefore a strict improvement. It is the recommended integrator for most models. **implicit**: The benefit over ``implicitfast`` is the implicit integration of Coriolis and centripetal forces for *coupled* - rotational systems such as multi-link pendula. Note that ``implicit`` does not apply :ref:`midpoint - integration` (only ``implicitfast`` does), but its RNE derivatives provide comparable stability - for free-body rotation. For example, `gyroscopic.xml <../_static/gyroscopic.xml>`__ shows an ellipsoid rolling - on an inclined plane; both ``implicitfast`` and ``implicit`` handle this case well, while ``Euler`` quickly - diverges. + rotational systems such as multi-link pendula. For standalone free bodies the two integrators coincide, since + ``implicitfast`` applies the :ref:`gyroscopic derivatives` to such bodies. For example, + `gyroscopic.xml <../_static/gyroscopic.xml>`__ shows an ellipsoid rolling on an inclined plane; both + ``implicitfast`` and ``implicit`` handle this case well, while ``Euler`` quickly diverges. **RK4**: This integrator is best for systems which are energy conserving, or almost energy-conserving. `pendulum.xml <../_static/pendulum.xml>`__ shows a complicated pendulum mechanism which diverges quickly using ``Euler`` or diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 48c59d1c..b79bb48e 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -19,6 +19,7 @@ #include // IWYU pragma: keep #include "engine/engine_core_util.h" #include "engine/engine_crossplatform.h" +#include "engine/engine_inline.h" #include "engine/engine_memory.h" #include "engine/engine_passive.h" #include "engine/engine_sleep.h" @@ -705,6 +706,186 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) { } +// 3x3 sub-blocks of (d qfrc_bias / d qvel) for a standalone free body +// outputs the two 3x3 blocks lin and rot such that the rotational columns +// of the full 6x6 bias Jacobian B are [-mass*lin; rot] (linear columns are zero) +// +// derivation: let R = xmat, s = xipos - xpos, w = R*qvel[rot] (world angular velocity), +// Iw = ximat * diag(body_inertia) * ximat' (world inertia about the CoM). with qacc = 0, +// the CoM acceleration is w x (w x s) and the world bias force/torque at the CoM are +// f = mass * w x (w x s), tau = w x Iw*w +// projected onto the joint coordinates: bias = [f; R'*(s x f + tau)]. differentiating +// w.r.t. the rotational dofs (through w = R*qvel[rot]), with K = [w x s]_x + [w]_x [s]_x: +// d f / d w = -mass * K => lin = K * R +// d tau / d w = [w]_x Iw - [Iw*w]_x => rot = R' * (-mass*[s]_x K + d tau/d w) * R +static void freeBias_vel_blocks(mjtNum mass, const mjtNum R[9], const mjtNum Xi[9], + const mjtNum inertia[3], const mjtNum s[3], + const mjtNum qvel_rot[3], mjtNum lin[9], mjtNum rot[9]) { + // world-frame angular velocity + mjtNum w[3]; + mji_mulMatVec3(w, R, qvel_rot); + + // world-frame inertia about CoM: Iw = Xi * diag(inertia) * Xi^T + mjtNum Xi_I[9]; + for (int i=0; i < 3; i++) { + Xi_I[3*i+0] = Xi[3*i+0] * inertia[0]; + Xi_I[3*i+1] = Xi[3*i+1] * inertia[1]; + Xi_I[3*i+2] = Xi[3*i+2] * inertia[2]; + } + mjtNum Iw[9]; + Iw[0] = Xi_I[0]*Xi[0] + Xi_I[1]*Xi[1] + Xi_I[2]*Xi[2]; + Iw[4] = Xi_I[3]*Xi[3] + Xi_I[4]*Xi[4] + Xi_I[5]*Xi[5]; + Iw[8] = Xi_I[6]*Xi[6] + Xi_I[7]*Xi[7] + Xi_I[8]*Xi[8]; + Iw[1] = Iw[3] = Xi_I[0]*Xi[3] + Xi_I[1]*Xi[4] + Xi_I[2]*Xi[5]; + Iw[2] = Iw[6] = Xi_I[0]*Xi[6] + Xi_I[1]*Xi[7] + Xi_I[2]*Xi[8]; + Iw[5] = Iw[7] = Xi_I[3]*Xi[6] + Xi_I[4]*Xi[7] + Xi_I[5]*Xi[8]; + + // intermediate vectors: ws = w x s (CoM offset velocity), Iww = Iw * w (angular momentum) + mjtNum ws[3], Iww[3]; + mji_cross(ws, w, s); + mji_mulMatVec3(Iww, Iw, w); + + // K = [w x s]_x + [w]_x [s]_x = s w^T - (w . s) I + [ws]_x + mjtNum w_dot_s = w[0]*s[0] + w[1]*s[1] + w[2]*s[2]; + mjtNum K[9]; + K[0] = s[0]*w[0] - w_dot_s; + K[1] = s[0]*w[1] - ws[2]; + K[2] = s[0]*w[2] + ws[1]; + + K[3] = s[1]*w[0] + ws[2]; + K[4] = s[1]*w[1] - w_dot_s; + K[5] = s[1]*w[2] - ws[0]; + + K[6] = s[2]*w[0] - ws[1]; + K[7] = s[2]*w[1] + ws[0]; + K[8] = s[2]*w[2] - w_dot_s; + + // lin = K * R + mji_mulMatMat3(lin, K, R); + + // C = -mass * [s]_x K + [w]_x Iw - [Iww]_x, column by column + // the last term (-[Iww]_x) is the negated cross-product matrix, added via ternaries + mjtNum C[9]; + for (int c=0; c < 3; c++) { + mjtNum s_x_K_row0 = s[1]*K[6+c] - s[2]*K[3+c]; + mjtNum s_x_K_row1 = s[2]*K[c] - s[0]*K[6+c]; + mjtNum s_x_K_row2 = s[0]*K[3+c] - s[1]*K[c]; + + mjtNum w_x_Iw_row0 = w[1]*Iw[6+c] - w[2]*Iw[3+c]; + mjtNum w_x_Iw_row1 = w[2]*Iw[c] - w[0]*Iw[6+c]; + mjtNum w_x_Iw_row2 = w[0]*Iw[3+c] - w[1]*Iw[c]; + + C[c] = -mass * s_x_K_row0 + w_x_Iw_row0 + (c == 1 ? Iww[2] : (c == 2 ? -Iww[1] : 0)); + C[3 + c] = -mass * s_x_K_row1 + w_x_Iw_row1 + (c == 0 ? -Iww[2] : (c == 2 ? Iww[0] : 0)); + C[6 + c] = -mass * s_x_K_row2 + w_x_Iw_row2 + (c == 0 ? Iww[1] : (c == 1 ? -Iww[0] : 0)); + } + + // rot = R^T * C * R + mjtNum tmp[9]; + mji_mulMatTMat3(tmp, R, C); + mji_mulMatMat3(rot, tmp, R); +} + + +// 6x6 block B = d qfrc_bias / d qvel for a standalone free body +// assembles the full 6x6 from the 3x3 sub-blocks computed by freeBias_vel_blocks +// rows/cols ordered like the free joint dofs: [linear(3); rotational(3)] +// linear columns are zero: the bias force does not depend on linear velocity +void mjd_freeBias_vel(const mjModel* m, const mjData* d, int jnt, mjtNum B[36]) { + int body = m->jnt_bodyid[jnt]; + int adr = m->jnt_dofadr[jnt]; + mjtNum mass = m->body_mass[body]; + const mjtNum* R = d->xmat + 9*body; // body -> world + const mjtNum* Xi = d->ximat + 9*body; // inertia -> world + const mjtNum* inertia = m->body_inertia + 3*body; + + // CoM offset from joint origin, world frame + mjtNum s[3]; + mji_sub3(s, d->xipos + 3*body, d->xpos + 3*body); + + mjtNum lin[9], rot[9]; + freeBias_vel_blocks(mass, R, Xi, inertia, s, d->qvel + adr + 3, lin, rot); + + mju_zero(B, 36); + for (int r=0; r < 3; r++) { + for (int c=0; c < 3; c++) { + B[6*r + 3+c] = -mass * lin[3*r+c]; + B[6*(3+r) + 3+c] = rot[3*r+c]; + } + } +} + + +// 6x6 block A = M - h * (d qfrc_smooth / d qvel) for the free joint of a standalone body +// returns 1 and writes A if jnt is the free joint of a standalone awake body, 0 otherwise +// requires valid d->qDeriv rows for the block, computed with flg_bias = 0; the bias +// derivative excluded from qDeriv is added here via freeBias_vel_blocks +int mjd_freeMhat(const mjModel* m, const mjData* d, int jnt, mjtNum h, mjtNum A[36]) { + // must be a free joint + if (m->jnt_type[jnt] != mjJNT_FREE) { + return 0; + } + + int body = m->jnt_bodyid[jnt]; + int adr = m->jnt_dofadr[jnt]; + int tree = m->dof_treeid[adr]; + mjtNum mass = m->body_mass[body]; + + // must be a standalone 6-DOF tree with no children, awake + if (m->tree_dofnum[tree] != 6 || + m->body_subtreemass[body] != mass || + !d->tree_awake[tree]) { + return 0; + } + + // D rows of a standalone free body are exactly the 6x6 block (D sparsity is tree-local); + // guard the gathers below against any violation of this invariant + if (m->D_rownnz[adr] != 6) { + return 0; + } + + // A = M block (gather from sparse lower triangle) + mju_zero(A, 36); + for (int r=0; r < 6; r++) { + int rowadr = m->M_rowadr[adr+r]; + int rownnz = m->M_rownnz[adr+r]; + for (int k=0; k < rownnz; k++) { + int c = m->M_colind[rowadr+k] - adr; + A[6*r+c] = A[6*c+r] = d->M[rowadr+k]; + } + } + + // A -= h * qDeriv block (actuator and passive derivatives) + for (int r=0; r < 6; r++) { + int rowadr = m->D_rowadr[adr+r]; + int rownnz = m->D_rownnz[adr+r]; + for (int k=0; k < rownnz; k++) { + int c = m->D_colind[rowadr+k] - adr; + A[6*r+c] -= h * d->qDeriv[rowadr+k]; + } + } + + // A -= h * d(qfrc_smooth)/d(qvel) for the bias term missing from qDeriv; + // qfrc_smooth includes -qfrc_bias, so subtracting its derivative adds +h*B + mjtNum s[3]; + mji_sub3(s, d->xipos + 3*body, d->xpos + 3*body); + + mjtNum lin[9], rot[9]; + freeBias_vel_blocks(mass, d->xmat + 9*body, d->ximat + 9*body, + m->body_inertia + 3*body, s, d->qvel + adr + 3, lin, rot); + + mjtNum h_mass = -h * mass; + for (int r=0; r < 3; r++) { + for (int c=0; c < 3; c++) { + A[6*r + 3+c] += h_mass * lin[3*r+c]; + A[6*(3+r) + 3+c] += h * rot[3*r+c]; + } + } + + return 1; +} + + //--------------------- utility functions for (d force / d vel) Jacobians -------------------------- // add J'*B*J to qDeriv diff --git a/src/engine/engine_derivative.h b/src/engine/engine_derivative.h index 3c648b1c..c6968b87 100644 --- a/src/engine/engine_derivative.h +++ b/src/engine/engine_derivative.h @@ -43,6 +43,15 @@ MJAPI void mjd_passive_vel(const mjModel* m, mjData* d); // subtract (d qfrc_bias / d qvel) from qDeriv (dense version) MJAPI void mjd_rne_vel_dense(const mjModel* m, mjData* d); +// 6x6 block B = d qfrc_bias / d qvel for the free joint of a standalone body +MJAPI void mjd_freeBias_vel(const mjModel* m, const mjData* d, int jnt, + mjtNum B[36]); + +// 6x6 block A = M - h * (d qfrc_smooth / d qvel) for the free joint of a standalone body +// returns 1 and writes A if jnt is the free joint of a standalone awake body, 0 otherwise +// requires valid d->qDeriv rows for the block, computed with flg_bias = 0 +MJAPI int mjd_freeMhat(const mjModel* m, const mjData* d, int jnt, mjtNum h, mjtNum A[36]); + // compute res += (s1 + s2*damping) * J'*K*J * vec, for all interpolated flexes // K_rot_cache: if non-NULL, use pre-cached K_rot (same layout as m->flex_stiffness) MJAPI void mjd_flexInterp_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 28ad4960..cee6b670 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -1433,352 +1433,6 @@ static void flexInterp_cgsolve(const mjModel* m, mjData* d, } -// return 1 if free joint is eligible for midpoint quaternion integration: -// standalone 6-DOF tree with no children, awake, and unconstrained -static int midpoint_eligible(const mjModel* m, const mjData* d, int jnt) { - if (m->jnt_type[jnt] != mjJNT_FREE) { - return 0; - } - - int body = m->jnt_bodyid[jnt]; - int adr = m->jnt_dofadr[jnt]; - int tree = m->dof_treeid[adr]; - - // must be standalone 6-DOF tree with no children - if (m->tree_dofnum[tree] != 6 || - m->body_subtreemass[body] != m->body_mass[body]) { - return 0; - } - - // must be awake - if (!d->tree_awake[tree]) { - return 0; - } - - // must be unconstrained - if (d->nefc) { - // islands enabled: O(1) lookup - if (!mjDISABLED(mjDSBL_ISLAND)) { - if (d->dof_island[adr] >= 0) { - return 0; - } - } - - // islands disabled: check if any constraint involves this tree - else { - for (int c=0; c < d->nefc; c++) { - int type = d->efc_type[c]; - int id = d->efc_id[c]; - - // contact: check if either geom belongs to this body - if (type == mjCNSTR_CONTACT_FRICTIONLESS || - type == mjCNSTR_CONTACT_PYRAMIDAL || - type == mjCNSTR_CONTACT_ELLIPTIC) { - int g1 = d->contact[id].geom[0]; - int g2 = d->contact[id].geom[1]; - if (g1 >= 0 && m->geom_bodyid[g1] == body) return 0; - if (g2 >= 0 && m->geom_bodyid[g2] == body) return 0; - } - - // connect or weld: check if either body is this body - else if (type == mjCNSTR_EQUALITY && - (m->eq_type[id] == mjEQ_CONNECT || m->eq_type[id] == mjEQ_WELD)) { - int b1 = m->eq_obj1id[id]; - int b2 = m->eq_obj2id[id]; - if (m->eq_objtype[id] == mjOBJ_SITE) { - b1 = m->site_bodyid[b1]; - b2 = m->site_bodyid[b2]; - } - if (b1 == body || b2 == body) return 0; - } - - // tendon limit or friction: check first two trees - else if (type == mjCNSTR_LIMIT_TENDON || type == mjCNSTR_FRICTION_TENDON) { - if (m->tendon_treeid[2*id] == tree || - m->tendon_treeid[2*id+1] == tree) return 0; - } - } - } - } - - // otherwise eligible - return 1; -} - - -// return 1 if the body's CoM is at the joint origin (no translational-rotational coupling) -static int midpoint_aligned(const mjModel* m, int jnt) { - int body = m->jnt_bodyid[jnt]; - return m->body_ipos[3*body+0] == 0 && - m->body_ipos[3*body+1] == 0 && - m->body_ipos[3*body+2] == 0; -} - - -// implicit midpoint integration for 3D rotation of a single body -// -// solves the Euler rigid body equation in the inertial frame: -// I * (w_new - w) / h = tau - w_mid x (I*w_mid) -// where w_mid = (w + w_new) / 2 is solved via Newton iteration. -// -// inputs: -// inertia: principal moments of inertia (3) -// w: initial angular velocity in principal axes frame (3) -// tau: external torque in principal axes frame (3) -// h: timestep -// outputs: -// w_mid: midpoint angular velocity in principal axes frame (3) -// returns: number of Newton iterations -static int midpointNewton(const mjtNum inertia[3], const mjtNum w[3], - const mjtNum tau[3], mjtNum h, mjtNum w_mid[3]) { - // precompute constants - mjtNum i2h = 2.0 / h; - mjtNum dI[3] = {inertia[2]-inertia[1], inertia[0]-inertia[2], inertia[1]-inertia[0]}; - mjtNum i2h_I[3] = {i2h*inertia[0], i2h*inertia[1], i2h*inertia[2]}; - - // initialize solution to previous angular velocity - mji_copy3(w_mid, w); - - // Newton iteration - int niter; - for (niter=0; niter < 100; niter++) { - // compute Coriolis term - mjtNum Iw[3] = {inertia[0]*w_mid[0], inertia[1]*w_mid[1], inertia[2]*w_mid[2]}; - mjtNum coriolis[3]; - mji_cross(coriolis, w_mid, Iw); - - // residual: f = i2h*I*(w_mid - w) + w_mid x (I*w_mid) - tau - mjtNum f[3]; - for (int k=0; k < 3; k++) { - f[k] = i2h_I[k]*(w_mid[k] - w[k]) + coriolis[k] - tau[k]; - } - - // check convergence - mjtNum fnorm = mju_norm3(f); -#ifndef mjUSESINGLE - mjtNum tol = 1e-13; -#else - mjtNum tol = 1e-6f; -#endif - if (fnorm < tol*(1 + i2h*mju_norm3(Iw))) break; - - // Jacobian: J = i2h*diag(I) + d(w x Iw)/dw - mjtNum J[9]; - J[0] = i2h_I[0]; J[1] = w_mid[2]*dI[0]; J[2] = w_mid[1]*dI[0]; - J[3] = w_mid[2]*dI[1]; J[4] = i2h_I[1]; J[5] = w_mid[0]*dI[1]; - J[6] = w_mid[1]*dI[2]; J[7] = w_mid[0]*dI[2]; J[8] = i2h_I[2]; - - // solve J*delta = -f for search direction delta - mjtNum neg_f[3] = {-f[0], -f[1], -f[2]}; - mjtNum delta[3]; - mju_solve3(delta, J, neg_f); - - // backtracking line search - mjtNum step = 1.0; - for (int ls=0; ls < 20; ls++) { - // candidate step - mjtNum w_try[3], Iw_try[3]; - for (int k=0; k < 3; k++) { - w_try[k] = w_mid[k] + step*delta[k]; - Iw_try[k] = inertia[k]*w_try[k]; - } - mjtNum coriolis_try[3]; - mji_cross(coriolis_try, w_try, Iw_try); - - // residual at candidate step - mjtNum f_try[3]; - for (int k=0; k < 3; k++) { - f_try[k] = i2h_I[k]*(w_try[k] - w[k]) + coriolis_try[k] - tau[k]; - } - - // accept step if residual decreased, otherwise backtrack - if (mju_norm3(f_try) < fnorm) { - mji_copy3(w_mid, w_try); - break; - } - step *= 0.5; - } - } - - return niter; -} - - -// implicit midpoint integration for one free body -// -// solves the Euler rigid body equation in the inertial frame: -// I * dw/dt = tau - w x (I*w) -// using the implicit midpoint rule: -// I * (w_new - w_old) / h = tau_mid - w_mid x (I*w_mid) -// where w_mid = (w_old + w_new) / 2 is solved via Newton iteration. -// -// inputs: -// mass: body mass -// inertia: principal moments of inertia -// ipos: CoM offset from joint origin, in body frame -// iquat: inertial quaternion (body_iquat) -// xquat: body orientation in world frame -// qvel_old: current velocity (lin in world : rot in body) -// qfrc: external force (lin in world : rot in body) -// gravity: gravitational acceleration in world frame (NULL: no gravity) -// h: timestep -// outputs: -// qvel_new: next velocity (lin in world : rot in body) -int mj_midpoint(mjtNum mass, const mjtNum inertia[3], const mjtNum ipos[3], - const mjtNum iquat[4], const mjtNum xquat[4], const mjtNum qvel_old[6], - const mjtNum qfrc[6], const mjtNum gravity[3], mjtNum h, - mjtNum qvel_new[6]) { - // transform angular velocity and torque to inertial frame - mjtNum iquat_neg[4], w[3], tau[3]; - mji_negQuat(iquat_neg, iquat); - mji_rotVecQuat(w, qvel_old+3, iquat_neg); // qvel+3 (angular) is in body frame - mji_rotVecQuat(tau, qfrc+3, iquat_neg); // qfrc+3 (angular) is in body frame - - // check for translational-rotational coupling - int aligned = (ipos[0] == 0 && ipos[1] == 0 && ipos[2] == 0); - - mjtNum r_com[3]; // joint-to-CoM vector in inertial frame - mjtNum tau_com[3]; // torque at CoM in inertial frame - mjtNum rot_x2i[4]; // quaternion rotation from world to inertial frame - mjtNum force[3]; // external force in inertial frame - - // compute torque at CoM in inertial frame - if (aligned) { - mji_copy3(tau_com, tau); - } else { - // rotation from world to inertial frame - mjtNum xquat_neg[4]; - mji_negQuat(xquat_neg, xquat); - mji_mulQuat(rot_x2i, iquat_neg, xquat_neg); - - // force and CoM offset in inertial frame - mji_rotVecQuat(force, qfrc, rot_x2i); - mji_rotVecQuat(r_com, ipos, iquat_neg); - - // torque at CoM in inertial frame - mjtNum rxf[3]; - mji_cross(rxf, r_com, force); - mji_sub3(tau_com, tau, rxf); - } - - // solve for midpoint angular velocity - mjtNum w_mid[3]; - int niter = midpointNewton(inertia, w, tau_com, h, w_mid); - - // next and mid angular velocities in inertial frame, rotate both to body frame - mjtNum w_new[3], w_new_body[3], w_mid_body[3]; - for (int k=0; k < 3; k++) { - w_new[k] = 2.0*w_mid[k] - w[k]; - } - mji_rotVecQuat(w_new_body, w_new, iquat); - mji_rotVecQuat(w_mid_body, w_mid, iquat); - mji_copy3(qvel_new+3, w_new_body); - - // === aligned: return - if (aligned) { - return niter; - } - - // === non-aligned: solve for translational velocity - - // rotate linear velocity to inertial frame - mjtNum v[3]; - mji_rotVecQuat(v, qvel_old, rot_x2i); - - // current CoM velocities (rot, lin) in inertial frame - mjtNum wxr[3]; - mji_cross(wxr, w, r_com); - mjtNum vcom[3]; - mji_add3(vcom, v, wxr); - - // right-hand side for midpoint CoM velocity - mjtNum i2h = 2.0 / h; - mjtNum b[3]; - for (int k=0; k < 3; k++) { - b[k] = force[k]/mass + i2h*vcom[k]; - } - - // add gravity, if any - if (gravity) { - mjtNum g_inertial[3]; - mji_rotVecQuat(g_inertial, gravity, rot_x2i); - mji_addTo3(b, g_inertial); - } - - // analytic solution for (i2h*Id + [w_mid]x) * vcom_mid = b - mjtNum wnorm2 = mju_dot3(w_mid, w_mid); - mjtNum denom = i2h*i2h + wnorm2; - mjtNum w_dot_b = mju_dot3(w_mid, b); - mjtNum w_cross_b[3]; - mji_cross(w_cross_b, w_mid, b); - mjtNum vcom_mid[3]; - for (int k=0; k < 3; k++) { - vcom_mid[k] = (i2h*b[k] + (w_dot_b/i2h)*w_mid[k] - w_cross_b[k]) / denom; - } - - // recover midpoint and new joint velocity in inertial frame - mjtNum wxr_mid[3]; - mji_cross(wxr_mid, w_mid, r_com); - mjtNum v_mid[3], v_new[3]; - for (int k=0; k < 3; k++) { - v_mid[k] = vcom_mid[k] - wxr_mid[k]; - v_new[k] = 2.0*v_mid[k] - v[k]; - } - - // estimate new orientation - mjtNum axis[3]; - mji_copy3(axis, w_mid_body); - mjtNum wnorm = mju_normalize3(axis); - mjtNum qrot_new[4]; - mji_axisAngle2Quat(qrot_new, axis, h*wnorm); - mjtNum xquat_new[4]; - mji_mulQuat(xquat_new, xquat, qrot_new); - - // v_new (linear): inertial → body → world using new orientation - mjtNum v_body[3]; - mji_rotVecQuat(v_body, v_new, iquat); - mji_rotVecQuat(qvel_new, v_body, xquat_new); - - return niter; -} - - -// compute next velocities via midpoint integration for eligible free bodies -// qfrc: total force (qfrc_smooth + qfrc_constraint) -// free_jntid: list of eligible free joint IDs -// nfree: number of eligible free joints -// qvel_old: output array for old velocities (6 per joint) -// qvel_new: output array for new velocities (6 per joint) -// dofadr: output array for DOF addresses (1 per joint) -static void midpoint(const mjModel* m, const mjData* d, const mjtNum* qfrc, - const int* free_jntid, int nfree, - mjtNum* qvel_old, mjtNum* qvel_new, int* dofadr) { - for (int i=0; i < nfree; i++) { - int j = free_jntid[i]; - int body = m->jnt_bodyid[j]; - - // save DOF address - int adr = m->jnt_dofadr[j]; - dofadr[i] = adr; - - // save old (current) velocity, needed after mj_advance (which overwrites qvel) - mju_copy(qvel_old+6*i, d->qvel+adr, 6); - - // compute external force = qfrc + qfrc_bias (undo bias subtraction) - mjtNum qfrc_total[6]; - mju_add(qfrc_total, qfrc+adr, d->qfrc_bias+adr, 6); - - // gravity handled inside mj_midpoint (accelerating frame of reference) - const mjtNum* gravity = mjDISABLED(mjDSBL_GRAVITY) ? NULL : m->opt.gravity; - - // midpoint solver for free joint j - mj_midpoint(m->body_mass[body], m->body_inertia+3*body, m->body_ipos+3*body, - m->body_iquat+4*body, d->xquat+4*body, - d->qvel+adr, qfrc_total, gravity, m->opt.timestep, qvel_new+6*i); - } -} - - // fully implicit in velocity, possibly skipping factorization void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { TM_START; @@ -1859,66 +1513,31 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { flexInterp_cgsolve(m, d, qacc, qfrc, m->nv); } - // count and list joints of free bodies eligible for midpoint integration - int nfree = 0; - int* free_jntid = NULL; - if (!mjENABLED(mjENBL_INVDISCRETE) && - m->opt.integrator == mjINT_IMPLICITFAST && - m->opt.density == 0 && m->opt.viscosity == 0) { - free_jntid = mjSTACKALLOC(d, m->njnt, int); + // implicitfast: local unsymmetric solve for standalone free bodies + // adds the bias (gyroscopic) derivative, dropped from the global symmetric solve; the + // 6x6 block of M - h*D is decoupled from the rest of the system (D sparsity is tree-local), + // so overwriting these rows of qacc leaves all other DOFs unaffected + if (m->opt.integrator == mjINT_IMPLICITFAST) { for (int j=0; j < m->njnt; j++) { - if (midpoint_eligible(m, d, j)) { - free_jntid[nfree++] = j; - } - } - } - - // compute midpoint velocities (used to update positions) - int* dofadr = NULL; - mjtNum* qvel_old = NULL; - mjtNum* qvel_new = NULL; - mjtNum* qvel_mid = NULL; - if (nfree) { - // allocate arrays, call midpoint solver for all eligible free joints - dofadr = mjSTACKALLOC(d, nfree, int); - qvel_new = mjSTACKALLOC(d, 6*nfree, mjtNum); - qvel_old = mjSTACKALLOC(d, 6*nfree, mjtNum); - midpoint(m, d, qfrc, free_jntid, nfree, qvel_old, qvel_new, dofadr); - - // build qvel_mid = d->qvel + h*qacc for all DOFs, then overwrite midpoint DOFs - qvel_mid = mjSTACKALLOC(d, m->nv, mjtNum); - mju_addScl(qvel_mid, d->qvel, qacc, m->opt.timestep, m->nv); - for (int i=0; i < nfree; i++) { - int adr = dofadr[i]; - int start = midpoint_aligned(m, free_jntid[i]) ? 3 : 0; - for (int k=start; k < 6; k++) { - qvel_mid[adr+k] = 0.5*(qvel_new[6*i+k] + qvel_old[6*i+k]); - } - } - } - - // advance state and time (use qvel_mid if allocated, NULL otherwise) - mj_advance(m, d, d->act_dot, qacc, qvel_mid); - - // overwrite midpoint DOFs with true next velocity and acceleration - if (nfree) { - mjtNum h_inv = 1.0 / m->opt.timestep; - for (int i=0; i < nfree; i++) { - // skip sleeping tree (may have been put to sleep during mj_advance) - int adr = dofadr[i]; - if (!d->tree_awake[m->dof_treeid[adr]]) { + mjtNum A[36]; + if (!mjd_freeMhat(m, d, j, m->opt.timestep, A)) { continue; } - // overwrite 3 or 6 midpoint DOFs with true next velocity and acceleration - int start = midpoint_aligned(m, free_jntid[i]) ? 3 : 0; - for (int k=start; k < 6; k++) { - d->qvel[adr+k] = qvel_new[6*i+k]; - d->qacc[adr+k] = (qvel_new[6*i+k] - qvel_old[6*i+k]) * h_inv; + // solve A * qacc_block = qfrc_block + int adr = m->jnt_dofadr[j]; + int pivot[6]; + if (mju_factorLU6(A, pivot)) { + mjtNum x[6]; // local vector for guaranteed memory alignment + mju_solveLU6(x, A, qfrc+adr, pivot); + mji_copy6(qacc+adr, x); } } } + // advance state and time + mj_advance(m, d, d->act_dot, qacc, NULL); + mj_freeStack(d); TM_END(mjTIMER_ADVANCE); diff --git a/src/engine/engine_forward.h b/src/engine/engine_forward.h index f0ba9e40..cb905d75 100644 --- a/src/engine/engine_forward.h +++ b/src/engine/engine_forward.h @@ -64,13 +64,6 @@ MJAPI void mj_implicit(const mjModel *m, mjData *d); // fully implicit in velocity, possibly skipping factorization MJAPI void mj_implicitSkip(const mjModel *m, mjData *d, int skipfactor); -// implicit midpoint integration for 6 DOFs (translation + rotation) of a single body -// returns number of Newton iterations -MJAPI int mj_midpoint(mjtNum mass, const mjtNum inertia[3], const mjtNum ipos[3], - const mjtNum iquat[4], const mjtNum xquat[4], const mjtNum qvel[6], - const mjtNum qfrc[6], const mjtNum gravity[3], mjtNum h, - mjtNum qvel_new[6]); - //-------------------------------- solver components ----------------------------------------------- diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index bc10ea45..0523216a 100644 --- a/src/engine/engine_inverse.c +++ b/src/engine/engine_inverse.c @@ -159,6 +159,17 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { // set qfrc = (M - dt*qDeriv) * qacc mju_mulSymVecSparse(qfrc, d->qH, qacc, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind); + + // standalone free bodies: overwrite block rows with the unsymmetric local product, + // including the bias (gyroscopic) derivative, mirroring mj_implicitSkip + for (int j=0; j < m->njnt; j++) { + mjtNum A[36]; + if (!mjd_freeMhat(m, d, j, m->opt.timestep, A)) { + continue; + } + int adr = m->jnt_dofadr[j]; + mju_mulMatVec(qfrc+adr, A, qacc+adr, 6, 6); + } break; } diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index b1cccf1e..ccce2e1c 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -147,6 +147,84 @@ TEST_F(DerivativeTest, SmoothDvel) { } } +// mjd_freeBias_vel: 6x6 bias-derivative block for a standalone free body +// validated against mjd_rne_vel and against finite-differenced mj_rne +TEST_F(DerivativeTest, FreeBiasVel) { + // free body with offset CoM, rotated inertia, non-identity orientation + static constexpr char xml[] = R"( + + + + + + + + + )"; + + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mjModel* m = model.get(); + mjData* d = data.get(); + + // set fast, fully populated velocity + mjtNum qvel[6] = {0.4, -0.3, 0.2, 5, -3, 2}; + mju_copy(d->qvel, qvel, 6); + mj_forward(m, d); + + // analytic block + mjtNum B[36]; + mjd_freeBias_vel(m, d, /*jnt=*/0, B); + + // linear columns are zero by construction + for (int r = 0; r < 6; r++) { + for (int c = 0; c < 3; c++) { + EXPECT_EQ(B[6 * r + c], 0); + } + } + + // compare with mjd_rne_vel: B == -(qDeriv(flg_bias=1) - qDeriv(flg_bias=0)) + mju_zero(d->qDeriv, m->nD); + mjd_smooth_vel(m, d, /*flg_bias=*/1); + vector qDeriv_bias = AsVector(d->qDeriv, m->nD); + mju_zero(d->qDeriv, m->nD); + mjd_smooth_vel(m, d, /*flg_bias=*/0); + for (int r = 0; r < 6; r++) { + int rowadr = m->D_rowadr[r]; + ASSERT_EQ(m->D_rownnz[r], 6); + for (int k = 0; k < 6; k++) { + int c = m->D_colind[rowadr + k]; + mjtNum rne_val = -(qDeriv_bias[rowadr + k] - d->qDeriv[rowadr + k]); + EXPECT_NEAR(B[6 * r + c], rne_val, MjTol(1e-14, 1e-6)) + << "mismatch at (" << r << ", " << c << ")"; + } + } + + // compare with central finite differences of mj_rne + mjtNum eps = MjTol(1e-6, 1e-3); + for (int c = 0; c < 6; c++) { + mjtNum bias_plus[6], bias_minus[6]; + + d->qvel[c] = qvel[c] + eps; + mj_comVel(m, d); + mj_rne(m, d, /*flg_acc=*/0, bias_plus); + + d->qvel[c] = qvel[c] - eps; + mj_comVel(m, d); + mj_rne(m, d, /*flg_acc=*/0, bias_minus); + + d->qvel[c] = qvel[c]; + + for (int r = 0; r < 6; r++) { + mjtNum fd = (bias_plus[r] - bias_minus[r]) / (2 * eps); + EXPECT_NEAR(B[6 * r + c], fd, MjTol(1e-7, 1e-2)) + << "FD mismatch at (" << r << ", " << c << ")"; + } + } +} + // disabled actuators do not contribute to d_qfrc_actuator/d_qvel TEST_F(DerivativeTest, DisabledActuators) { // model with only a position actuator diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index ce3ab5f1..e664b47f 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -464,112 +464,149 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) { mj_deleteModel(model); } -// Energy and angmom conservation for free body with implicitfast (IMR) -TEST_F(ImplicitIntegratorTest, ConservationMidpoint) { - // aligned: CoM at joint origin - static constexpr char xml1[] = R"( +// free-body local solve: implicitfast matches implicit exactly for a standalone +// free body +TEST_F(ImplicitIntegratorTest, FreeBodyMatchesImplicit) { + static constexpr char xml[] = R"( - + )"; - // auto-aligned: CoM at joint origin - static constexpr char xml2[] = R"( - - - - - - - - - - )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr d1 = MakeData(model); + MjDataPtr d2 = MakeData(model); + mjModel* m = model.get(); - // non-aligned: CoM offset from joint origin - static constexpr char xml3[] = R"( - - - - - - - - - - )"; - int xml_idx = 1; - for (auto xml : {xml1, xml2, xml3}) { - SCOPED_TRACE(testing::Message() << "XML case " << xml_idx++); - char error[1024]; - MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model.get(), NotNull()) << error; - MjDataPtr data = MakeData(model); + // tumbling initial velocity + mj_resetData(m, d1.get()); + d1->qvel[3] = 5; + d1->qvel[4] = -3; + d1->qvel[5] = 2; - const int nstep = 500; - mjtNum energy_drift[2], angmom_drift[2]; // [0]=midpoint, [1]=rk4 + // step both integrators from identical states, re-synchronizing each step + // to avoid chaotic divergence of tumbling trajectories + int nstate = mj_stateSize(m, mjSTATE_INTEGRATION); + std::vector state(nstate); + mjtNum tol = MjTol(1e-14, 1e-6); + for (int i = 0; i < 50; i++) { + mj_getState(m, d1.get(), state.data(), mjSTATE_INTEGRATION); + mj_setState(m, d2.get(), state.data(), mjSTATE_INTEGRATION); - for (int integrator : {mjINT_IMPLICITFAST, mjINT_RK4}) { - int idx = (integrator == mjINT_IMPLICITFAST) ? 0 : 1; - model->opt.integrator = integrator; + m->opt.integrator = mjINT_IMPLICITFAST; + mj_step(m, d1.get()); + m->opt.integrator = mjINT_IMPLICIT; + mj_step(m, d2.get()); - // reset - mj_resetData(model.get(), data.get()); - data->qvel[3] = 1.0; - data->qvel[4] = 2.0; - data->qvel[5] = 3.0; - mj_forward(model.get(), data.get()); - mjtNum initial_energy = data->energy[1]; - mjtNum initial_angmom[3]; - mj_subtreeVel(model.get(), data.get()); - mju_copy3(initial_angmom, data->subtree_angmom); - - for (int i = 0; i < nstep; i++) { - mj_step(model.get(), data.get()); - } - - energy_drift[idx] = fabs(data->energy[1] - initial_energy); - mj_subtreeVel(model.get(), data.get()); - mjtNum angmom_err[3]; - mju_sub3(angmom_err, data->subtree_angmom, initial_angmom); - angmom_drift[idx] = mju_norm3(angmom_err); + for (int k = 0; k < m->nv; k++) { + EXPECT_NEAR(d1->qvel[k], d2->qvel[k], tol) + << "step " << i << " dof " << k; } - - // midpoint should conserve energy better than RK4 (double only) -#ifndef mjUSESINGLE - EXPECT_LT(energy_drift[0], energy_drift[1]); -#endif - - // both should conserve angular momentum well - EXPECT_LT(angmom_drift[0], MjTol(1e-3, 1e-2)); - EXPECT_LT(angmom_drift[1], MjTol(1e-3, 1e-2)); } } -// verify second-order convergence of midpoint integration -TEST_F(ImplicitIntegratorTest, MidpointConvergenceOrder) { - // aligned: CoM at joint origin - static constexpr char xml1[] = R"( +// free-body local solve: spinning free bodies do not gain energy in vacuum +TEST_F(ImplicitIntegratorTest, FreeBodyGyroStable) { + static constexpr char xml[] = R"( - + )"; + + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mjModel* m = model.get(); + mjData* d = data.get(); + + // middle-axis tumble and fast principal-axis spin + static constexpr mjtNum qvel0[2][3] = {{0.05, 5, 0.05}, {20, 0.05, 0.05}}; + + for (int c = 0; c < 2; c++) { + SCOPED_TRACE(testing::Message() << "velocity case " << c); + mj_resetData(m, d); + mju_copy3(d->qvel + 3, qvel0[c]); + mj_forward(m, d); + mjtNum initial_energy = d->energy[1]; + + // 100 simulated seconds + for (int i = 0; i < 20000; i++) { + mj_step(m, d); + ASSERT_LT(d->energy[1], 1.01 * initial_energy) + << "energy gain at step " << i; + } + } +} + +// free-body local solve: applies to bodies in contact +TEST_F(ImplicitIntegratorTest, FreeBodyGyroStableContact) { + // spinning ellipsoid on an inclined plane, as in gyroscopic.xml + static constexpr char xml[] = R"( + + + )"; + + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mjModel* m = model.get(); + mjData* d = data.get(); + + mj_resetData(m, d); + d->qvel[3] = 30; + mjtNum initial_speed = mju_norm(d->qvel, m->nv); + + int ncon_total = 0; + for (int i = 0; i < 5000; i++) { + mj_step(m, d); + ncon_total += d->ncon; + ASSERT_LT(mju_norm(d->qvel, m->nv), 2 * initial_speed) + << "speed gain at step " << i; + } + + // the body was in contact while spinning + EXPECT_GT(ncon_total, 1000); +} + +// free-body local solve: energy of a tumbling free body never increases and is +// only mildly damped; angular momentum drift is bounded +TEST_F(ImplicitIntegratorTest, FreeBodyConservation) { + // aligned: CoM at joint origin + static constexpr char xml1[] = R"( + + + + + + @@ -578,14 +615,13 @@ TEST_F(ImplicitIntegratorTest, MidpointConvergenceOrder) { // non-aligned: CoM offset from joint origin static constexpr char xml2[] = R"( - @@ -597,302 +633,131 @@ TEST_F(ImplicitIntegratorTest, MidpointConvergenceOrder) { char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mjModel* m = model.get(); + mjData* d = data.get(); - mjtNum T = 1.0; - mjtNum h_coarse = 0.02; - mjtNum quat_coarse[4], quat_fine[4], quat_ref[4]; + mj_resetData(m, d); + d->qvel[3] = 1.0; + d->qvel[4] = 2.0; + d->qvel[5] = 3.0; + mj_forward(m, d); + mjtNum initial_energy = d->energy[1]; + mjtNum initial_angmom[3]; + mj_subtreeVel(m, d); + mju_copy3(initial_angmom, d->subtree_angmom); - auto run = [&](mjtNum h, mjtNum quat_out[4]) { - model->opt.timestep = h; - MjDataPtr data = MakeData(model); + for (int i = 0; i < 500; i++) { + mj_step(m, d); - data->qvel[3] = 1.0; - data->qvel[4] = 2.0; - data->qvel[5] = 3.0; - - int nstep = (int)(T / h + 0.5); - for (int i = 0; i < nstep; i++) { - mj_step(model.get(), data.get()); - } - - mju_copy4(quat_out, data->qpos + 3); - }; - - run(h_coarse, quat_coarse); - run(h_coarse / 2, quat_fine); - run(h_coarse / 16, quat_ref); - - // quaternion distance: ||quat - quat_ref|| (handles sign ambiguity) - auto quat_dist = [](const mjtNum a[4], const mjtNum b[4]) -> mjtNum { - mjtNum pos = 0, neg = 0; - for (int i = 0; i < 4; i++) { - pos += (a[i] - b[i]) * (a[i] - b[i]); - neg += (a[i] + b[i]) * (a[i] + b[i]); - } - return mju_sqrt(mju_min(pos, neg)); - }; - - mjtNum err_coarse = quat_dist(quat_coarse, quat_ref); - mjtNum err_fine = quat_dist(quat_fine, quat_ref); - - // second-order: error ratio should be ~4 when halving timestep - mjtNum ratio = err_coarse / err_fine; - EXPECT_GT(ratio, 3.5); - EXPECT_LT(ratio, 4.5); - } -} - -// verify that Newton iteration in mj_midpoint converges quickly (aligned case) -TEST_F(ImplicitIntegratorTest, MidpointNewtonConvergence) { - // inertia ratios: symmetric, mildly asymmetric, extremely asymmetric - mjtNum inertias[][3] = { - {1.0, 1.0, 1.0}, - {1.0, 2.0, 3.0}, - {0.01, 1.0, 100.0}, - {1.0, 1.0, 1000.0}, - }; - - mjtNum timesteps[] = {0.001, 0.01, 0.1}; - - mjtNum velocities[][3] = { - {1.0, 2.0, 3.0}, - {100.0, 0.0, 0.0}, - {10.0, 10.0, 10.0}, - {0.01, 0.01, 100.0}, - }; - - mjtNum q_identity[4] = {1, 0, 0, 0}; - mjtNum torques[][3] = { - {0, 0, 0}, - {10.0, 20.0, 30.0}, - {100.0, 0.0, 0.0}, - {0.0, 0.0, 100.0}, - }; - - int max_iter = 0; - int total_iter = 0; - int ncases = 0; - - for (auto& I : inertias) { - for (mjtNum h : timesteps) { - for (auto& w : velocities) { - for (auto& tau : torques) { - mjtNum vel[6] = {0, 0, 0, w[0], w[1], w[2]}; - mjtNum tau_ext[6] = {0, 0, 0, tau[0], tau[1], tau[2]}; - mjtNum v_new[6]; - mjtNum ipos[3] = {0, 0, 0}; - int niter = mj_midpoint(1.0, I, ipos, q_identity, q_identity, vel, - tau_ext, NULL, h, v_new); - EXPECT_LT(niter, 10) - << "Failed for I=(" << I[0] << "," << I[1] << "," << I[2] << ")" - << " h=" << h << " w=(" << w[0] << "," << w[1] << "," << w[2] - << ")" - << " tau=(" << tau[0] << "," << tau[1] << "," << tau[2] << ")"; - max_iter = std::max(max_iter, niter); - total_iter += niter; - ncases++; - } - } + // energy never increases (small tolerance for rounding) + ASSERT_LT(d->energy[1], initial_energy * (1 + MjTol(1e-9, 1e-4))) + << "energy gain at step " << i; } - } - EXPECT_LE(max_iter, 4); - EXPECT_LT((mjtNum)total_iter / ncases, 2.0); + // implicit damping of tumbling is mild: measured E_end/E0 = 0.93 + EXPECT_GT(d->energy[1], 0.7 * initial_energy); + + // angular momentum drift is bounded: measured 5e-3 + mj_subtreeVel(m, d); + mjtNum angmom_err[3]; + mju_sub3(angmom_err, d->subtree_angmom, initial_angmom); + EXPECT_LT(mju_norm3(angmom_err), 0.05); + } } -// verify that Newton iteration in mj_midpoint converges quickly (non-aligned) -TEST_F(ImplicitIntegratorTest, MidpointFullNewtonConvergence) { - mjtNum masses[] = {0.1, 1.0, 10.0}; - - mjtNum inertias[][3] = { - {1.0, 1.0, 1.0}, - {1.0, 2.0, 3.0}, - {0.01, 1.0, 100.0}, - }; - - mjtNum offsets[][3] = { - {0.1, 0.0, 0.0}, - {0.05, 0.03, 0.02}, - {0.0, 0.0, 0.5}, - }; - - mjtNum timesteps[] = {0.001, 0.01, 0.1}; - - mjtNum velocities[][6] = { - {1.0, 0.0, 0.0, 1.0, 2.0, 3.0}, - {0.0, 0.0, 0.0, 10.0, 10.0, 10.0}, - {5.0, 5.0, 5.0, 0.01, 0.01, 100.0}, - }; - - mjtNum q_identity[4] = {1, 0, 0, 0}; - mjtNum forces[][6] = { - {0, 0, 0, 0, 0, 0}, - {10.0, 20.0, 30.0, 1.0, 2.0, 3.0}, - }; - - int max_iter = 0; - int total_iter = 0; - int ncases = 0; - - for (mjtNum mass : masses) { - for (auto& I : inertias) { - for (auto& r : offsets) { - for (mjtNum h : timesteps) { - for (auto& vel : velocities) { - for (auto& frc : forces) { - mjtNum v_new[6]; - int niter = mj_midpoint(mass, I, r, q_identity, q_identity, vel, - frc, NULL, h, v_new); - EXPECT_LT(niter, 10) - << "Failed for mass=" << mass << " I=(" << I[0] << "," << I[1] - << "," << I[2] << ")" - << " r=(" << r[0] << "," << r[1] << "," << r[2] << ")" - << " h=" << h; - max_iter = std::max(max_iter, niter); - total_iter += niter; - ncases++; - } - } - } - } - } - } - - EXPECT_LE(max_iter, 6); - EXPECT_LT((mjtNum)total_iter / ncases, 3.0); -} - -// verify midpoint eligibility: compare with/without invdiscrete -// if trajectories differ, midpoint was applied -// if trajectories match, midpoint was skipped -TEST_F(ImplicitIntegratorTest, MidpointEligibility) { - // free body with asymmetric inertia, optionally near a plane +// gyroscopic instability: Euler gains energy where implicitfast does not +TEST_F(ImplicitIntegratorTest, FreeBodyEulerGainsImplicitfastDissipates) { static constexpr char xml[] = R"( - )"; char error[1024]; - MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m.get(), NotNull()) << error; - MjDataPtr d1 = MakeData(m); - MjDataPtr d2 = MakeData(m); - int nsteps = 50; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mjModel* m = model.get(); + mjData* d = data.get(); - auto spin_and_compare = [&](const char* label, bool expect_midpoint) { - mj_resetData(m.get(), d1.get()); - mj_resetData(m.get(), d2.get()); - d1->qvel[3] = d2->qvel[3] = 5; - d1->qvel[4] = d2->qvel[4] = 3; - d1->qvel[5] = d2->qvel[5] = 1; - - // d1: midpoint enabled (default) - m->opt.enableflags &= ~mjENBL_INVDISCRETE; - for (int i = 0; i < nsteps; i++) mj_step(m.get(), d1.get()); - - // d2: midpoint disabled - m->opt.enableflags |= mjENBL_INVDISCRETE; - mj_resetData(m.get(), d2.get()); - d2->qvel[3] = 5; - d2->qvel[4] = 3; - d2->qvel[5] = 1; - for (int i = 0; i < nsteps; i++) mj_step(m.get(), d2.get()); - m->opt.enableflags &= ~mjENBL_INVDISCRETE; - - // compare angular velocities - mjtNum diff = 0; - for (int k = 3; k < 6; k++) { - mjtNum d = d1->qvel[k] - d2->qvel[k]; - diff += d * d; + mjtNum energy_end[2]; + for (int integrator : {mjINT_EULER, mjINT_IMPLICITFAST}) { + m->opt.integrator = integrator; + mj_resetData(m, d); + d->qvel[3] = 1.0; + d->qvel[4] = 2.0; + d->qvel[5] = 3.0; + mj_forward(m, d); + mjtNum initial_energy = d->energy[1]; + for (int i = 0; i < 500; i++) { + mj_step(m, d); + } + energy_end[integrator == mjINT_IMPLICITFAST] = + d->energy[1] / initial_energy; + } + + // Euler gains energy (measured: 1.09), implicitfast does not + EXPECT_GT(energy_end[0], 1.01); + EXPECT_LT(energy_end[1], 1.0); +} + +// the invdiscrete flag has no effect on forward dynamics +TEST_F(ImplicitIntegratorTest, InvdiscreteForwardNoop) { + static constexpr char xml[] = R"( + + + )"; + + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr d1 = MakeData(model); + MjDataPtr d2 = MakeData(model); + mjModel* m = model.get(); + + for (int integrator : {mjINT_IMPLICITFAST, mjINT_IMPLICIT}) { + m->opt.integrator = integrator; + + mj_resetData(m, d1.get()); + d1->qvel[3] = 5; + d1->qvel[5] = 2; + mj_resetData(m, d2.get()); + d2->qvel[3] = 5; + d2->qvel[5] = 2; + + for (int i = 0; i < 200; i++) { + m->opt.enableflags &= ~mjENBL_INVDISCRETE; + mj_step(m, d1.get()); + m->opt.enableflags |= mjENBL_INVDISCRETE; + mj_step(m, d2.get()); + } + m->opt.enableflags &= ~mjENBL_INVDISCRETE; + + // trajectories are bit-identical + for (int k = 0; k < m->nq; k++) { + EXPECT_EQ(d1->qpos[k], d2->qpos[k]) << "qpos " << k; + } + for (int k = 0; k < m->nv; k++) { + EXPECT_EQ(d1->qvel[k], d2->qvel[k]) << "qvel " << k; } - if (expect_midpoint) { - EXPECT_GT(diff, 1e-6) << label << ": expected midpoint to be applied"; - } else { - EXPECT_LT(diff, 1e-20) << label << ": expected midpoint to be skipped"; - } - }; - - // case 1: free body in vacuum, implicitfast -> midpoint applied - m->opt.integrator = mjINT_IMPLICITFAST; - m->opt.density = 0; - m->opt.viscosity = 0; - spin_and_compare("vacuum+implicitfast", true); - - // case 2: implicit integrator -> midpoint NOT applied - m->opt.integrator = mjINT_IMPLICIT; - spin_and_compare("vacuum+implicit", false); - - // case 3: fluid (nonzero density) -> midpoint NOT applied - m->opt.integrator = mjINT_IMPLICITFAST; - m->opt.density = 1.2; - spin_and_compare("fluid+implicitfast", false); - m->opt.density = 0; - - // case 4: fluid (nonzero viscosity) -> midpoint NOT applied - m->opt.viscosity = 0.001; - spin_and_compare("viscosity+implicitfast", false); - m->opt.viscosity = 0; - - // case 5: body with active contacts -> midpoint NOT applied - // test both island-enabled and island-disabled branches - for (int disable_island = 0; disable_island < 2; disable_island++) { - m->opt.integrator = mjINT_IMPLICITFAST; - if (disable_island) { - m->opt.disableflags |= mjDSBL_ISLAND; - } else { - m->opt.disableflags &= ~mjDSBL_ISLAND; - } - - mj_resetData(m.get(), d1.get()); - mj_resetData(m.get(), d2.get()); - d1->qpos[2] = d2->qpos[2] = 0.05; - d1->qvel[3] = d2->qvel[3] = 5; - d1->qvel[4] = d2->qvel[4] = 3; - d1->qvel[5] = d2->qvel[5] = 1; - - // verify contacts are active - mj_forward(m.get(), d1.get()); - ASSERT_GT(d1->ncon, 0) << "body should be in contact with the plane"; - - // single step with midpoint enabled - mj_resetData(m.get(), d1.get()); - d1->qpos[2] = 0.05; - d1->qvel[3] = 5; - d1->qvel[4] = 3; - d1->qvel[5] = 1; - m->opt.enableflags &= ~mjENBL_INVDISCRETE; - mj_step(m.get(), d1.get()); - - // single step with midpoint disabled - mj_resetData(m.get(), d2.get()); - d2->qpos[2] = 0.05; - d2->qvel[3] = 5; - d2->qvel[4] = 3; - d2->qvel[5] = 1; - m->opt.enableflags |= mjENBL_INVDISCRETE; - mj_step(m.get(), d2.get()); - m->opt.enableflags &= ~mjENBL_INVDISCRETE; - - mjtNum diff = 0; - for (int k = 0; k < m->nv; k++) { - mjtNum d = d1->qvel[k] - d2->qvel[k]; - diff += d * d; - } - EXPECT_LT(diff, 1e-20) << "contact (island " - << (disable_island ? "disabled" : "enabled") - << "): expected midpoint to be skipped"; } - m->opt.disableflags &= ~mjDSBL_ISLAND; } // model with degenerate translational inertia diff --git a/test/engine/engine_inverse_test.cc b/test/engine/engine_inverse_test.cc index 428ea2ce..a687e97f 100644 --- a/test/engine/engine_inverse_test.cc +++ b/test/engine/engine_inverse_test.cc @@ -17,6 +17,7 @@ #include "src/engine/engine_inverse.h" #include +#include #include #include @@ -100,7 +101,7 @@ TEST_F(InverseTest, DiscreteInverseMatch) { mjtNum* qvel_next = (mjtNum*)mju_malloc(nv * sizeof(mjtNum)); mjtNum* qacc_fd = (mjtNum*)mju_malloc(nv * sizeof(mjtNum)); - for (auto integrator : {mjINT_EULER, mjINT_IMPLICIT}) { + for (auto integrator : {mjINT_EULER, mjINT_IMPLICIT, mjINT_IMPLICITFAST}) { model->opt.integrator = integrator; for (bool invdiscrete : {false, true}) { // set/unset mjENBL_INVDISCRETE flag (affects both forward and inverse) @@ -154,5 +155,67 @@ TEST_F(InverseTest, DiscreteInverseMatch) { mj_deleteModel(model); } +// discrete-time inverse dynamics for a spinning free body under implicitfast: +// exercises the local unsymmetric block (bias derivative) in mj_discreteAcc +TEST_F(InverseTest, DiscreteInverseFreeBody) { + // spinning box resting on a plane: standalone free body with active contacts + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mjModel* m = model.get(); + mjData* d = data.get(); + int nv = m->nv; + + // spin about the vertical, small tumble components + mj_resetData(m, d); + d->qvel[3] = 0.5; + d->qvel[4] = -0.3; + d->qvel[5] = 20; + + // settle into persistent contact while still spinning + for (int i = 0; i < kSteps; i++) { + mj_step(m, d); + } + + // save state, step, compute finite-differenced acceleration + int nstate = mj_stateSize(m, mjSTATE_INTEGRATION); + std::vector state(nstate), qvel_next(nv), qacc_fd(nv); + mj_getState(m, d, state.data(), mjSTATE_INTEGRATION); + mj_step(m, d); + mju_copy(qvel_next.data(), d->qvel, nv); + mj_setState(m, d, state.data(), mjSTATE_INTEGRATION); + mju_sub(qacc_fd.data(), qvel_next.data(), d->qvel, nv); + mju_scl(qacc_fd.data(), qacc_fd.data(), 1 / m->opt.timestep, nv); + + // forward, overwrite qacc with finite-differenced acceleration, compare + mj_forward(m, d); + ASSERT_GT(d->ncon, 0) << "body should be in contact"; + ASSERT_GT(mju_abs(d->qvel[5]), 1) << "body should still be spinning"; + mju_copy(d->qacc, qacc_fd.data(), nv); + mj_compareFwdInv(m, d); + + // measured residuals: ~6e-12 double, ~1.5e-2 single (float solver + // convergence) + mjtNum epsilon = MjTol(1e-10, 0.05); + EXPECT_LT(d->solver_fwdinv[0], epsilon); + EXPECT_LT(d->solver_fwdinv[1], epsilon); +} + } // namespace } // namespace mujoco diff --git a/test/engine/engine_sleep_test.cc b/test/engine/engine_sleep_test.cc index 98858a76..48d35e0e 100644 --- a/test/engine/engine_sleep_test.cc +++ b/test/engine/engine_sleep_test.cc @@ -501,10 +501,10 @@ TEST_F(SleepTest, Equality) { mj_deleteModel(m); } -// Test that the midpoint integrator doesn't break the sleep qvel=0 invariant. -// A standalone free body (eligible for midpoint) with high viscosity should +// Test that the free-body implicit (gyroscopic) solve doesn't break the sleep +// qvel=0 invariant. A standalone free body with high viscosity should // eventually go to sleep, and after sleeping, qvel/qacc must be exactly zero. -TEST_F(SleepTest, MidpointSleepZeroVelocity) { +TEST_F(SleepTest, FreeBodySleepZeroVelocity) { static constexpr char xml[] = R"(