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
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
b2106db52f
commit
f0fa3d8260
@@ -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
|
||||
|
||||
@@ -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,
|
||||
|
||||
+17
-398
@@ -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);
|
||||
|
||||
@@ -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 -----------------------------------------------
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user