chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Remove midpoint integration, superseded by free-body gyroscopic derivatives.

Browse Source 
The gyroscopic (bias) derivatives applied to standalone free bodies by the
implicitfast integrator provide comparable stability for spinning bodies,
with none of midpoint's restrictions: they apply under contacts, fluid
forces and constraints, and preserve the linear force-velocity relation
required by discrete-time inverse dynamics. The invdiscrete flag reverts to
its original single meaning and no longer affects forward dynamics.

Restore implicitfast coverage in the DiscreteInverseMatch test, removed
when midpoint made discrete inverse dynamics untestable.

Add implicit gyroscopic (bias) derivatives for free bodies in implicitfast.

The implicitfast integrator drops the RNE (bias) derivative to stay on the
symmetric Cholesky path, so fast-spinning free bodies integrate gyroscopic
forces explicitly and can gain energy. Symmetrizing the gyroscopic Jacobian
is not an option: its stabilizing content is the antisymmetric part, and
adding only the symmetric part is destabilizing.

Instead, exploit the fact that for a standalone free body the 6x6 block of
M - h*D is decoupled from the rest of the system (qDeriv sparsity is
tree-local): after the global solve, rebuild the block with the exact bias
derivative in closed form (mjd_freeBias_vel) and re-solve it with dense
unsymmetric LU, overwriting the block's rows of qacc. For lone spinning
bodies this makes implicitfast match implicit to rounding, at ~150ns per
eligible body: cheaper than the midpoint machinery it will replace.
Eligibility is structural only; contacts, fluid and constraints need no
gating. The same block is mirrored in discrete inverse dynamics
(mj_discreteAcc), making invdiscrete exact for spinning free bodies.

PiperOrigin-RevId: 948472495
Change-Id: I813ef3d98c7b399881bc8603b9f9208cfb02eb58
This commit is contained in:
Yuval Tassa
2026-07-15 12:07:10 -07:00
committed by Copybara-Service
parent b2106db52f
commit f0fa3d8260
12 changed files with 631 additions and 840 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+7 -16
View File
@@ -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<geMidpoint>`.
Enable discrete-time inverse dynamics
This flag **enables** discrete-time inverse dynamics with :ref:`mj_inverse` for all
:ref:`integrators<option-integrator>` other than ``RK4``. Recall from the :ref:`numerical
integration<geIntegration>` 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<geMidpoint>` 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<option-integrator>` other than ``RK4``. Recall from the
:ref:`numerical integration<geIntegration>` 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:
doc/changelog.rst
+10 -2
View File
@@ -7,6 +7,14 @@ Upcoming version (not yet released)
General
^^^^^^^
- Replaced midpoint integration of free bodies with :ref:`gyroscopic derivatives<geFreeBody>` in the ``implicitfast``
:ref:`integrator<geIntegrators>`: 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<option-flag-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<flex-elasticity-elastic2d>` for trilinear and quadratic flex
:ref:`dofs<body-flexcomp-dof>`.
4. :commit:`910b3336` :ref:`Midpoint integration<geMidpoint>` is now restricted to the ``implicitfast``
4. :commit:`910b3336` Midpoint integration is now restricted to the ``implicitfast``
:ref:`integrator<geIntegrators>` and is disabled when fluid forces are active
(nonzero :ref:`density<option-density>` or :ref:`viscosity<option-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<glNumericSizes>`. 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<geMidpoint>` for standalone free bodies in ``implicit`` and
4. :commit:`0c337799` Added midpoint integration for standalone free bodies in ``implicit`` and
``implicitfast`` :ref:`integrators<geIntegrators>`. 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<option-flag-invdiscrete>` flag now also disables midpoint integration, providing an
doc/computation/index.rst
+21 -48
View File
@@ -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<geIntegrators>` 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
<https://en.wikipedia.org/wiki/Euler%27s_equations_(rigid_body_dynamics)>`__:
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
<https://doi.org/10.1007/3-540-30666-8>`__ 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 <https://en.wikipedia.org/wiki/Poisson_bracket>`__ 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<option-density>` and
:ref:`viscosity<option-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<option-flag-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<geMidpoint>` 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<geFreeBody>` 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<geMidpoint>` (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<geFreeBody>` 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
src/engine/engine_derivative.c
+181
View File
@@ -19,6 +19,7 @@
#include <mujoco/mjsan.h> // 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
src/engine/engine_derivative.h
+9
View File
@@ -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,
src/engine/engine_forward.c
+17 -398
View File
@@ -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);
src/engine/engine_forward.h
-7
View File
@@ -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 -----------------------------------------------
src/engine/engine_inverse.c
+11
View File
@@ -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;
}
test/engine/engine_derivative_test.cc
+78
View File
@@ -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"(
<mujoco>
<worldbody>
<body pos="0.1 -0.2 0.3" euler="20 -30 40">
<freejoint/>
<geom type="box" size=".1 .2 .3" mass="2" pos=".04 -.02 .03" euler="10 20 30"/>
</body>
</worldbody>
</mujoco>
)";
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<mjtNum> 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
test/engine/engine_forward_test.cc
+230 -365
View File
@@ -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"(
<mujoco>
<option integrator="implicitfast" timestep="0.01">
<flag energy="enable" gravity="disable"/>
</option>
<option timestep="0.005"/>
<worldbody>
<body>
<freejoint/>
<geom type="box" size=".1 .2 .3" mass="1" euler="10 20 30"/>
<body pos="0.1 -0.2 0.5" euler="20 -30 40">
<joint type="free" damping="0.1"/>
<geom type="box" size=".1 .2 .3" mass="2" pos=".04 -.02 .03" euler="10 20 30"/>
</body>
</worldbody>
</mujoco>
)";
// auto-aligned: CoM at joint origin
static constexpr char xml2[] = R"(
<mujoco>
<option integrator="implicitfast" timestep="0.01">
<flag energy="enable" gravity="disable"/>
</option>
<worldbody>
<body>
<freejoint align="true"/>
<geom type="box" size=".1 .2 .3" mass="1" euler="10 20 30" pos=".03 .02 .01"/>
</body>
</worldbody>
</mujoco>
)";
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"(
<mujoco>
<option integrator="implicitfast" timestep="0.01">
<flag energy="enable" gravity="disable"/>
</option>
<worldbody>
<body>
<freejoint/>
<geom type="box" size=".1 .2 .3" mass="1" euler="10 20 30" pos=".03 .02 .01"/>
</body>
</worldbody>
</mujoco>
)";
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<mjtNum> 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"(
<mujoco>
<option integrator="implicitfast">
<flag gravity="disable"/>
<option integrator="implicitfast" timestep="0.005">
<flag energy="enable" gravity="disable"/>
</option>
<worldbody>
<body>
<freejoint/>
<geom type="box" size=".1 .2 .3" mass="1" euler="10 20 30"/>
<geom type="box" size=".1 .2 .3" mass="1"/>
</body>
</worldbody>
</mujoco>
)";
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"(
<mujoco>
<option integrator="implicitfast" timestep="0.002"/>
<worldbody>
<geom type="plane" size="5 5 .1" euler="0 15 0"/>
<body pos="0 0 .2">
<freejoint/>
<geom type="ellipsoid" size=".05 .1 .15" mass="1"/>
</body>
</worldbody>
</mujoco>
)";
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"(
<mujoco>
<option integrator="implicitfast" timestep="0.01">
<flag energy="enable" gravity="disable"/>
</option>
<worldbody>
<body>
<freejoint/>
<geom type="box" size=".1 .2 .3" mass="1"/>
</body>
</worldbody>
</mujoco>
@@ -578,14 +615,13 @@ TEST_F(ImplicitIntegratorTest, MidpointConvergenceOrder) {
// non-aligned: CoM offset from joint origin
static constexpr char xml2[] = R"(
<mujoco>
<option integrator="implicitfast">
<flag gravity="disable"/>
<option integrator="implicitfast" timestep="0.01">
<flag energy="enable" gravity="disable"/>
</option>
<worldbody>
<body>
<freejoint/>
<geom type="box" size=".1 .2 .3" mass="1" euler="10 20 30"
pos=".05 .03 .02"/>
<geom type="box" size=".1 .2 .3" mass="1" euler="10 20 30" pos=".03 .02 .01"/>
</body>
</worldbody>
</mujoco>
@@ -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"(
<mujoco>
<option integrator="implicitfast" timestep="0.01">
<flag energy="enable"/>
<option timestep="0.01">
<flag energy="enable" gravity="disable"/>
</option>
<worldbody>
<geom type="plane" size="5 5 0.1"/>
<body name="free" pos="0 0 2">
<body>
<freejoint/>
<geom type="ellipsoid" size="0.3 0.2 0.1" mass="1"/>
<geom type="box" size=".1 .2 .3" mass="1"/>
</body>
</worldbody>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.005"/>
<worldbody>
<geom type="plane" size="2 2 .1"/>
<body pos="0 0 .3">
<joint type="free" damping="0.1"/>
<geom type="box" size=".1 .2 .3" mass="2" pos=".03 .02 .01"/>
</body>
</worldbody>
</mujoco>
)";
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
test/engine/engine_inverse_test.cc
+64 -1
View File
@@ -17,6 +17,7 @@
#include "src/engine/engine_inverse.h"
#include <string>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
@@ -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"(
<mujoco>
<option integrator="implicitfast" timestep="0.002">
<flag invdiscrete="enable"/>
</option>
<worldbody>
<geom type="plane" size="2 2 .1" friction="0.2"/>
<body pos="0 0 .1">
<joint type="free" damping="0.01"/>
<geom type="box" size=".2 .15 .1" mass="2" pos=".02 -.01 .03" friction="0.2"/>
</body>
</worldbody>
</mujoco>
)";
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<mjtNum> 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
test/engine/engine_sleep_test.cc
+3 -3
View File
@@ -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"(
<mujoco>
<option integrator="implicitfast" viscosity="10"