Skip to content

Commit f0fa3d8

Browse files
yuvaltassacopybara-github
authored andcommitted
Remove midpoint integration, superseded by free-body gyroscopic derivatives.
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
1 parent b2106db commit f0fa3d8

12 files changed

Lines changed: 607 additions & 816 deletions

‎doc/XMLreference.rst‎

Lines changed: 7 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -661,22 +661,13 @@ from its default.
661661
.. _option-flag-invdiscrete:
662662

663663
:at:`invdiscrete`: :at-val:`[disable, enable], "disable"`
664-
This dual-purpose flag enables discrete-time inverse dynamics and disables :ref:`midpoint integration<geMidpoint>`.
665-
666-
Enable discrete-time inverse dynamics
667-
This flag **enables** discrete-time inverse dynamics with :ref:`mj_inverse` for all
668-
:ref:`integrators<option-integrator>` other than ``RK4``. Recall from the :ref:`numerical
669-
integration<geIntegration>` section that the one-step integrators (``Euler``, ``implicit`` and ``implicitfast``),
670-
modify the mass matrix :math:`M \rightarrow M-hD`. This implies that finite-differenced accelerations
671-
:math:`(v_{t+h} - v_t)/h` will not correspond to the continuous-time acceleration ``mjData.qacc``. When this flag
672-
is enabled, :ref:`mj_inverse` will interpret ``qacc`` as having been computed from the difference of two sequential
673-
velocities, and undo the above modification.
674-
675-
Disable midpoint integration
676-
Additionally and relatedly, this flag **disables** :ref:`midpoint integration<geMidpoint>` for free bodies, which
677-
would otherwise break the linear relationship between finite-differenced velocities and forces assumed by discrete
678-
inverse dynamics. Note that disabling midpoint integration might be useful for debugging or for other reasons,
679-
regardless or whether inverse dynamics are used.
664+
This flag enables discrete-time inverse dynamics with :ref:`mj_inverse` for all
665+
:ref:`integrators<option-integrator>` other than ``RK4``. Recall from the
666+
:ref:`numerical integration<geIntegration>` section that the one-step integrators (``Euler``, ``implicit`` and
667+
``implicitfast``), modify the mass matrix :math:`M \rightarrow M-hD`. This implies that finite-differenced
668+
accelerations :math:`(v_{t+h} - v_t)/h` will not correspond to the continuous-time acceleration ``mjData.qacc``.
669+
When this flag is enabled, :ref:`mj_inverse` will interpret ``qacc`` as having been computed from the difference of
670+
two sequential velocities, and undo the above modification.
680671

681672

682673
.. _option-flag-multiccd:

‎doc/changelog.rst‎

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,14 @@ Upcoming version (not yet released)
77

88
General
99
^^^^^^^
10+
- Replaced midpoint integration of free bodies with :ref:`gyroscopic derivatives<geFreeBody>` in the ``implicitfast``
11+
:ref:`integrator<geIntegrators>`: the bias-force derivative of every standalone free body is applied via a local
12+
unsymmetric solve of its decoupled block, making ``implicitfast`` identical to ``implicit`` for such bodies.
13+
Unlike midpoint integration, which required vacuum and no constraints, this applies in all environments (contacts,
14+
fluid, constraints), and is compatible with discrete-time inverse dynamics. Spinning free bodies no
15+
longer gain energy, but tumbling motion is now mildly damped; models requiring long-horizon energy conservation of
16+
tumbling bodies in vacuum should use ``RK4``. The :ref:`invdiscrete<option-flag-invdiscrete>` flag no longer has any
17+
effect on forward dynamics.
1018
- Added Nesterov momentum extrapolation with adaptive gradient restart (O'Donoghue-Candès) to the PGS solver,
1119
significantly improving convergence. Overall PGS now requires ~2x fewer iterations.
1220
- Added the Newton decrement -- the quadratic model's predicted cost improvement of the next iteration -- as a third
@@ -233,7 +241,7 @@ General
233241
improving performance by ~20%.
234242
3. :commit:`b9c1877e` Added support for :ref:`elastic2d<flex-elasticity-elastic2d>` for trilinear and quadratic flex
235243
:ref:`dofs<body-flexcomp-dof>`.
236-
4. :commit:`910b3336` :ref:`Midpoint integration<geMidpoint>` is now restricted to the ``implicitfast``
244+
4. :commit:`910b3336` Midpoint integration is now restricted to the ``implicitfast``
237245
:ref:`integrator<geIntegrators>` and is disabled when fluid forces are active
238246
(nonzero :ref:`density<option-density>` or :ref:`viscosity<option-viscosity>`).
239247
Midpoint integration treats external forces as zero-order-hold constants, which causes
@@ -335,7 +343,7 @@ General
335343
scalar arrays (``jnt_stiffness``, ``dof_damping``, etc.) continue to hold the linear coefficient and are unchanged.
336344
The polynomial order is defined by the new constant :ref:`mjNPOLY<glNumericSizes>`. A future breaking C-API change
337345
may unify the linear and higher-order coefficients into a single array.
338-
4. :commit:`0c337799` Added :ref:`midpoint integration<geMidpoint>` for standalone free bodies in ``implicit`` and
346+
4. :commit:`0c337799` Added midpoint integration for standalone free bodies in ``implicit`` and
339347
``implicitfast`` :ref:`integrators<geIntegrators>`. This applies the implicit midpoint rule to the rotational
340348
dynamics of free bodies with no children, conserving kinetic energy to machine precision in the absence of external
341349
torques. The :ref:`invdiscrete<option-flag-invdiscrete>` flag now also disables midpoint integration, providing an

‎doc/computation/index.rst‎

Lines changed: 21 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -580,50 +580,24 @@ Solving for :math:`v_{t+h}`, we obtain the implicit-in-velocity update
580580
\widehat{M} &\equiv M-h D
581581
\end{aligned}
582582
583-
.. _geMidpoint:
583+
.. _geFreeBody:
584584

585-
Midpoint integration for free bodies in vacuum
586-
The implicit-in-velocity update :eq:`eq_implicit_update` treats the acceleration as a function of velocity and
587-
linearizes. While effective for damping-like forces, it is sub-optimal for rotational dynamics, where
588-
Coriolis and gyroscopic forces are *quadratic* in angular velocity. For this case, a better approach is to directly
589-
discretize the rotational equations of motion using the *midpoint method*.
585+
Gyroscopic derivatives for free bodies
586+
The ``implicitfast`` integrator described :ref:`below<geIntegrators>` excludes the derivatives of centripetal,
587+
Coriolis and gyroscopic forces from :math:`D`, so that :math:`\widehat M` remains symmetric and can be factorized
588+
with the faster Cholesky decomposition. However integrating gyroscopic forces explicitly can lead to
589+
energy gain and divergence of fast-spinning free bodies with asymmetric inertia.
590590

591-
Consider a rigid body rotating in its principal-axis frame with angular velocity
592-
:math:`\omega \in \mathbb{R}^3` and diagonal inertia tensor :math:`I = \text{diag}(I_1, I_2, I_3)`. The rotational
593-
dynamics are given by `Euler's rotation equation
594-
<https://en.wikipedia.org/wiki/Euler%27s_equations_(rigid_body_dynamics)>`__:
591+
Therefore for *standalone free bodies* (free joints whose body has no children), these derivatives are reinstated.
592+
The rows of :math:`\widehat M` corresponding to such a body form a :math:`6\times 6` block which is decoupled from
593+
the rest of the system. After the global Cholesky solve, this block is re-assembled with the exact derivative of the
594+
body's bias force and re-solved with an optimized :math:`6\times 6` LU routine. For standalone free bodies,
595+
``implicitfast`` and ``implicit`` therefore compute identical updates.
595596

596-
.. math::
597-
I \dot{\omega} + \omega \times I\omega = \tau
598-
599-
where :math:`\tau` is the external torque in the principal-axis frame.
600-
Evaluating the velocities at the midpoint, :math:`\omega_\text{mid} = (\omega_t + \omega_{t+h})/2`, gives:
601-
602-
.. math::
603-
\frac{2}{h} I (\omega_\text{mid} - \omega_t) + \omega_\text{mid} \times I \omega_\text{mid} = \tau
604-
605-
This is a system of 3 nonlinear equations in 3 unknowns :math:`\omega_\text{mid}`, solved at each timestep using
606-
Newton's method with a backtracking line search. After solving, the new velocity is recovered as
607-
:math:`\omega_{t+h} = 2\omega_\text{mid} - \omega_t`.
608-
609-
**Properties.** The midpoint method preserves all `quadratic first integrals
610-
<https://doi.org/10.1007/3-540-30666-8>`__ of the ODE. For Euler's equations, these are the
611-
kinetic energy :math:`H = \frac{1}{2}\omega^T I\omega` and the squared angular momentum
612-
:math:`C = \frac{1}{2}|I\omega|^2`, both conserved exactly in the absence of external torque. Since :math:`C` is the
613-
Casimir function of the `Lie-Poisson <https://en.wikipedia.org/wiki/Poisson_bracket>`__ structure, the midpoint
614-
method is a symmetric (time-reversible) and second-order accurate *Poisson integrator*.
615-
616-
**Eligibility.** Midpoint integration is only applied when using the ``implicitfast`` integrator, to
617-
free bodies with no child bodies, and only when the medium has zero :ref:`density<option-density>` and
618-
:ref:`viscosity<option-viscosity>`.
619-
620-
**Performance.** While the midpoint method carries computational overhead, we've found it to be
621-
negligible compared to the rest of the pipeline, on the order of 1% in the worst case.
622-
623-
**Disabling.** Because midpoint integration solves a nonlinear equation for the next velocity, it breaks the linear
624-
relationship between finite-differenced velocities and forces assumed by discrete inverse dynamics. Therefore,
625-
setting the :ref:`invdiscrete<option-flag-invdiscrete>` flag disables midpoint integration, and also provides a
626-
general opt-out mechanism for this integrator.
597+
**Properties.** The kinetic energy of a spinning free body is non-increasing in the absence of applied force.
598+
Steady spins about principal axes are conserved almost exactly; tumbling motion is mildly damped, at a rate
599+
scaling like :math:`(h|\omega|)^2` per step. Systems requiring long-horizon energy conservation of tumbling
600+
bodies should use the ``RK4`` integrator.
627601

628602
.. _geIntegrators:
629603

@@ -661,8 +635,8 @@ Fast implicit-in-velocity (``implicitfast``)
661635
scenarios which are not common and already well-handled by the Runge-Kutta integrator (see below). Because the RNE
662636
derivatives are also the main source of asymmetry of :math:`D`, by dropping them and symmetrizing, we can use the
663637
faster :math:`L^TL` rather than :math:`LU` decomposition.
664-
The ``implicitfast`` integrator applies :ref:`midpoint integration<geMidpoint>` to eligible free bodies in vacuum,
665-
providing exact energy conservation for spinning objects at negligible additional cost.
638+
For standalone free bodies, the dropped :ref:`gyroscopic derivatives<geFreeBody>` are reinstated with a local
639+
unsymmetric solve, preventing energy gain of spinning bodies at negligible additional cost.
666640

667641
4th-order Runge-Kutta (``RK4``)
668642
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``)
696670
increased stability, and is therefore a strict improvement. It is the recommended integrator for most models.
697671
**implicit**:
698672
The benefit over ``implicitfast`` is the implicit integration of Coriolis and centripetal forces for *coupled*
699-
rotational systems such as multi-link pendula. Note that ``implicit`` does not apply :ref:`midpoint
700-
integration<geMidpoint>` (only ``implicitfast`` does), but its RNE derivatives provide comparable stability
701-
for free-body rotation. For example, `gyroscopic.xml <../_static/gyroscopic.xml>`__ shows an ellipsoid rolling
702-
on an inclined plane; both ``implicitfast`` and ``implicit`` handle this case well, while ``Euler`` quickly
703-
diverges.
673+
rotational systems such as multi-link pendula. For standalone free bodies the two integrators coincide, since
674+
``implicitfast`` applies the :ref:`gyroscopic derivatives<geFreeBody>` to such bodies. For example,
675+
`gyroscopic.xml <../_static/gyroscopic.xml>`__ shows an ellipsoid rolling on an inclined plane; both
676+
``implicitfast`` and ``implicit`` handle this case well, while ``Euler`` quickly diverges.
704677
**RK4**:
705678
This integrator is best for systems which are energy conserving, or almost energy-conserving. `pendulum.xml
706679
<../_static/pendulum.xml>`__ shows a complicated pendulum mechanism which diverges quickly using ``Euler`` or

‎src/engine/engine_derivative.c‎

Lines changed: 181 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <mujoco/mjsan.h> // IWYU pragma: keep
2020
#include "engine/engine_core_util.h"
2121
#include "engine/engine_crossplatform.h"
22+
#include "engine/engine_inline.h"
2223
#include "engine/engine_memory.h"
2324
#include "engine/engine_passive.h"
2425
#include "engine/engine_sleep.h"
@@ -705,6 +706,186 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) {
705706
}
706707

707708

709+
// 3x3 sub-blocks of (d qfrc_bias / d qvel) for a standalone free body
710+
// outputs the two 3x3 blocks lin and rot such that the rotational columns
711+
// of the full 6x6 bias Jacobian B are [-mass*lin; rot] (linear columns are zero)
712+
//
713+
// derivation: let R = xmat, s = xipos - xpos, w = R*qvel[rot] (world angular velocity),
714+
// Iw = ximat * diag(body_inertia) * ximat' (world inertia about the CoM). with qacc = 0,
715+
// the CoM acceleration is w x (w x s) and the world bias force/torque at the CoM are
716+
// f = mass * w x (w x s), tau = w x Iw*w
717+
// projected onto the joint coordinates: bias = [f; R'*(s x f + tau)]. differentiating
718+
// w.r.t. the rotational dofs (through w = R*qvel[rot]), with K = [w x s]_x + [w]_x [s]_x:
719+
// d f / d w = -mass * K => lin = K * R
720+
// d tau / d w = [w]_x Iw - [Iw*w]_x => rot = R' * (-mass*[s]_x K + d tau/d w) * R
721+
static void freeBias_vel_blocks(mjtNum mass, const mjtNum R[9], const mjtNum Xi[9],
722+
const mjtNum inertia[3], const mjtNum s[3],
723+
const mjtNum qvel_rot[3], mjtNum lin[9], mjtNum rot[9]) {
724+
// world-frame angular velocity
725+
mjtNum w[3];
726+
mji_mulMatVec3(w, R, qvel_rot);
727+
728+
// world-frame inertia about CoM: Iw = Xi * diag(inertia) * Xi^T
729+
mjtNum Xi_I[9];
730+
for (int i=0; i < 3; i++) {
731+
Xi_I[3*i+0] = Xi[3*i+0] * inertia[0];
732+
Xi_I[3*i+1] = Xi[3*i+1] * inertia[1];
733+
Xi_I[3*i+2] = Xi[3*i+2] * inertia[2];
734+
}
735+
mjtNum Iw[9];
736+
Iw[0] = Xi_I[0]*Xi[0] + Xi_I[1]*Xi[1] + Xi_I[2]*Xi[2];
737+
Iw[4] = Xi_I[3]*Xi[3] + Xi_I[4]*Xi[4] + Xi_I[5]*Xi[5];
738+
Iw[8] = Xi_I[6]*Xi[6] + Xi_I[7]*Xi[7] + Xi_I[8]*Xi[8];
739+
Iw[1] = Iw[3] = Xi_I[0]*Xi[3] + Xi_I[1]*Xi[4] + Xi_I[2]*Xi[5];
740+
Iw[2] = Iw[6] = Xi_I[0]*Xi[6] + Xi_I[1]*Xi[7] + Xi_I[2]*Xi[8];
741+
Iw[5] = Iw[7] = Xi_I[3]*Xi[6] + Xi_I[4]*Xi[7] + Xi_I[5]*Xi[8];
742+
743+
// intermediate vectors: ws = w x s (CoM offset velocity), Iww = Iw * w (angular momentum)
744+
mjtNum ws[3], Iww[3];
745+
mji_cross(ws, w, s);
746+
mji_mulMatVec3(Iww, Iw, w);
747+
748+
// K = [w x s]_x + [w]_x [s]_x = s w^T - (w . s) I + [ws]_x
749+
mjtNum w_dot_s = w[0]*s[0] + w[1]*s[1] + w[2]*s[2];
750+
mjtNum K[9];
751+
K[0] = s[0]*w[0] - w_dot_s;
752+
K[1] = s[0]*w[1] - ws[2];
753+
K[2] = s[0]*w[2] + ws[1];
754+
755+
K[3] = s[1]*w[0] + ws[2];
756+
K[4] = s[1]*w[1] - w_dot_s;
757+
K[5] = s[1]*w[2] - ws[0];
758+
759+
K[6] = s[2]*w[0] - ws[1];
760+
K[7] = s[2]*w[1] + ws[0];
761+
K[8] = s[2]*w[2] - w_dot_s;
762+
763+
// lin = K * R
764+
mji_mulMatMat3(lin, K, R);
765+
766+
// C = -mass * [s]_x K + [w]_x Iw - [Iww]_x, column by column
767+
// the last term (-[Iww]_x) is the negated cross-product matrix, added via ternaries
768+
mjtNum C[9];
769+
for (int c=0; c < 3; c++) {
770+
mjtNum s_x_K_row0 = s[1]*K[6+c] - s[2]*K[3+c];
771+
mjtNum s_x_K_row1 = s[2]*K[c] - s[0]*K[6+c];
772+
mjtNum s_x_K_row2 = s[0]*K[3+c] - s[1]*K[c];
773+
774+
mjtNum w_x_Iw_row0 = w[1]*Iw[6+c] - w[2]*Iw[3+c];
775+
mjtNum w_x_Iw_row1 = w[2]*Iw[c] - w[0]*Iw[6+c];
776+
mjtNum w_x_Iw_row2 = w[0]*Iw[3+c] - w[1]*Iw[c];
777+
778+
C[c] = -mass * s_x_K_row0 + w_x_Iw_row0 + (c == 1 ? Iww[2] : (c == 2 ? -Iww[1] : 0));
779+
C[3 + c] = -mass * s_x_K_row1 + w_x_Iw_row1 + (c == 0 ? -Iww[2] : (c == 2 ? Iww[0] : 0));
780+
C[6 + c] = -mass * s_x_K_row2 + w_x_Iw_row2 + (c == 0 ? Iww[1] : (c == 1 ? -Iww[0] : 0));
781+
}
782+
783+
// rot = R^T * C * R
784+
mjtNum tmp[9];
785+
mji_mulMatTMat3(tmp, R, C);
786+
mji_mulMatMat3(rot, tmp, R);
787+
}
788+
789+
790+
// 6x6 block B = d qfrc_bias / d qvel for a standalone free body
791+
// assembles the full 6x6 from the 3x3 sub-blocks computed by freeBias_vel_blocks
792+
// rows/cols ordered like the free joint dofs: [linear(3); rotational(3)]
793+
// linear columns are zero: the bias force does not depend on linear velocity
794+
void mjd_freeBias_vel(const mjModel* m, const mjData* d, int jnt, mjtNum B[36]) {
795+
int body = m->jnt_bodyid[jnt];
796+
int adr = m->jnt_dofadr[jnt];
797+
mjtNum mass = m->body_mass[body];
798+
const mjtNum* R = d->xmat + 9*body; // body -> world
799+
const mjtNum* Xi = d->ximat + 9*body; // inertia -> world
800+
const mjtNum* inertia = m->body_inertia + 3*body;
801+
802+
// CoM offset from joint origin, world frame
803+
mjtNum s[3];
804+
mji_sub3(s, d->xipos + 3*body, d->xpos + 3*body);
805+
806+
mjtNum lin[9], rot[9];
807+
freeBias_vel_blocks(mass, R, Xi, inertia, s, d->qvel + adr + 3, lin, rot);
808+
809+
mju_zero(B, 36);
810+
for (int r=0; r < 3; r++) {
811+
for (int c=0; c < 3; c++) {
812+
B[6*r + 3+c] = -mass * lin[3*r+c];
813+
B[6*(3+r) + 3+c] = rot[3*r+c];
814+
}
815+
}
816+
}
817+
818+
819+
// 6x6 block A = M - h * (d qfrc_smooth / d qvel) for the free joint of a standalone body
820+
// returns 1 and writes A if jnt is the free joint of a standalone awake body, 0 otherwise
821+
// requires valid d->qDeriv rows for the block, computed with flg_bias = 0; the bias
822+
// derivative excluded from qDeriv is added here via freeBias_vel_blocks
823+
int mjd_freeMhat(const mjModel* m, const mjData* d, int jnt, mjtNum h, mjtNum A[36]) {
824+
// must be a free joint
825+
if (m->jnt_type[jnt] != mjJNT_FREE) {
826+
return 0;
827+
}
828+
829+
int body = m->jnt_bodyid[jnt];
830+
int adr = m->jnt_dofadr[jnt];
831+
int tree = m->dof_treeid[adr];
832+
mjtNum mass = m->body_mass[body];
833+
834+
// must be a standalone 6-DOF tree with no children, awake
835+
if (m->tree_dofnum[tree] != 6 ||
836+
m->body_subtreemass[body] != mass ||
837+
!d->tree_awake[tree]) {
838+
return 0;
839+
}
840+
841+
// D rows of a standalone free body are exactly the 6x6 block (D sparsity is tree-local);
842+
// guard the gathers below against any violation of this invariant
843+
if (m->D_rownnz[adr] != 6) {
844+
return 0;
845+
}
846+
847+
// A = M block (gather from sparse lower triangle)
848+
mju_zero(A, 36);
849+
for (int r=0; r < 6; r++) {
850+
int rowadr = m->M_rowadr[adr+r];
851+
int rownnz = m->M_rownnz[adr+r];
852+
for (int k=0; k < rownnz; k++) {
853+
int c = m->M_colind[rowadr+k] - adr;
854+
A[6*r+c] = A[6*c+r] = d->M[rowadr+k];
855+
}
856+
}
857+
858+
// A -= h * qDeriv block (actuator and passive derivatives)
859+
for (int r=0; r < 6; r++) {
860+
int rowadr = m->D_rowadr[adr+r];
861+
int rownnz = m->D_rownnz[adr+r];
862+
for (int k=0; k < rownnz; k++) {
863+
int c = m->D_colind[rowadr+k] - adr;
864+
A[6*r+c] -= h * d->qDeriv[rowadr+k];
865+
}
866+
}
867+
868+
// A -= h * d(qfrc_smooth)/d(qvel) for the bias term missing from qDeriv;
869+
// qfrc_smooth includes -qfrc_bias, so subtracting its derivative adds +h*B
870+
mjtNum s[3];
871+
mji_sub3(s, d->xipos + 3*body, d->xpos + 3*body);
872+
873+
mjtNum lin[9], rot[9];
874+
freeBias_vel_blocks(mass, d->xmat + 9*body, d->ximat + 9*body,
875+
m->body_inertia + 3*body, s, d->qvel + adr + 3, lin, rot);
876+
877+
mjtNum h_mass = -h * mass;
878+
for (int r=0; r < 3; r++) {
879+
for (int c=0; c < 3; c++) {
880+
A[6*r + 3+c] += h_mass * lin[3*r+c];
881+
A[6*(3+r) + 3+c] += h * rot[3*r+c];
882+
}
883+
}
884+
885+
return 1;
886+
}
887+
888+
708889
//--------------------- utility functions for (d force / d vel) Jacobians --------------------------
709890

710891
// add J'*B*J to qDeriv

‎src/engine/engine_derivative.h‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,15 @@ MJAPI void mjd_passive_vel(const mjModel* m, mjData* d);
4343
// subtract (d qfrc_bias / d qvel) from qDeriv (dense version)
4444
MJAPI void mjd_rne_vel_dense(const mjModel* m, mjData* d);
4545

46+
// 6x6 block B = d qfrc_bias / d qvel for the free joint of a standalone body
47+
MJAPI void mjd_freeBias_vel(const mjModel* m, const mjData* d, int jnt,
48+
mjtNum B[36]);
49+
50+
// 6x6 block A = M - h * (d qfrc_smooth / d qvel) for the free joint of a standalone body
51+
// returns 1 and writes A if jnt is the free joint of a standalone awake body, 0 otherwise
52+
// requires valid d->qDeriv rows for the block, computed with flg_bias = 0
53+
MJAPI int mjd_freeMhat(const mjModel* m, const mjData* d, int jnt, mjtNum h, mjtNum A[36]);
54+
4655
// compute res += (s1 + s2*damping) * J'*K*J * vec, for all interpolated flexes
4756
// K_rot_cache: if non-NULL, use pre-cached K_rot (same layout as m->flex_stiffness)
4857
MJAPI void mjd_flexInterp_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec,

0 commit comments

Comments
 (0)
⌂ 换网址