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

Add implicit integrator.

Browse Source 
Added analytic derivatives of smooth (unconstrained) dynamics forces, with respect to velocities:
  - Centripetal and Coriolis forces computed by the Recursive Newton-Euler algorithm.
  - Damping and fluid-drag passive forces.
  - Actuation forces.

A new implicit-in-velocity integrator is implemented using the analytic derivatives. This integrator lies between the Euler and Runge Kutta integrators in terms of both stability and computational cost.

PiperOrigin-RevId: 450377010
Change-Id: Ie192b441876c22e732fb749333926f296e0a09cc
This commit is contained in:
DeepMind
2022-05-23 01:21:24 -07:00
committed by Copybara-Service
parent 1913a02b40
commit 64bc6d27b2
30 changed files with 1974 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/mujoco/mjdata.h
+13 -2
View File
@@ -225,10 +225,10 @@ struct mjData_ {
// computed by mj_fwdPosition/mj_crb
mjtNum* crb; // com-based composite inertia and mass (nbody x 10)
mjtNum* qM; // total inertia (nM x 1)
mjtNum* qM; // total inertia (sparse) (nM x 1)
// computed by mj_fwdPosition/mj_factorM
mjtNum* qLD; // L'*D*L factorization of M (nM x 1)
mjtNum* qLD; // L'*D*L factorization of M (sparse) (nM x 1)
mjtNum* qLDiagInv; // 1/diag(D) (nv x 1)
mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1)
@@ -286,6 +286,17 @@ struct mjData_ {
mjtNum* subtree_linvel; // linear velocity of subtree com (nbody x 3)
mjtNum* subtree_angmom; // angular momentum about subtree com (nbody x 3)
// computed by mj_implicit
int* D_rownnz; // non-zeros in each row (nv x 1)
int* D_rowadr; // address of each row in D_colind (nv x 1)
int* D_colind; // column indices of non-zeros (nD x 1)
// computed by mj_implicit/mj_derivative
mjtNum* qDeriv; // d (passive + actuator - bias) / d qvel (nD x 1)
// computed by mj_implicit/mju_factorLUSparse
mjtNum* qLU; // sparse LU of (qM - dt*qDeriv) (nD x 1)
//-------------------------------- POSITION, VELOCITY, CONTROL/ACCELERATION dependent
// computed by mj_fwdActuation
include/mujoco/mjmodel.h
+3 -1
View File
@@ -126,7 +126,8 @@ typedef enum mjtTexture_ { // type of texture
typedef enum mjtIntegrator_ { // integrator mode
mjINT_EULER = 0, // semi-implicit Euler
mjINT_RK4 // 4th-order Runge Kutta
mjINT_RK4, // 4th-order Runge Kutta
mjINT_IMPLICIT // implicit in velocity
} mjtIntegrator;
@@ -568,6 +569,7 @@ struct mjModel_ {
// sizes set after mjModel construction (only affect mjData)
int nM; // number of non-zeros in sparse inertia matrix
int nD; // number of non-zeros in sparse derivative matrix
int nemax; // number of potential equality-constraint rows
int njmax; // number of available rows in constraint Jacobian
int nconmax; // number of potential contacts in contact list
include/mujoco/mjxmacro.h
+6
View File
@@ -112,6 +112,7 @@
X( nuser_sensor ) \
X( nnames ) \
X( nM ) \
X( nD ) \
X( nemax ) \
X( njmax ) \
X( nconmax ) \
@@ -508,6 +509,11 @@
X( mjtNum, efc_aref, njmax, 1 ) \
X( mjtNum, subtree_linvel, nbody, 3 ) \
X( mjtNum, subtree_angmom, nbody, 3 ) \
X( int, D_rownnz, nv, 1 ) \
X( int, D_rowadr, nv, 1 ) \
X( int, D_colind, nD, 1 ) \
X( mjtNum, qDeriv, nD, 1 ) \
X( mjtNum, qLU, nD, 1 ) \
X( mjtNum, actuator_force, nu, 1 ) \
X( mjtNum, qfrc_actuator, nv, 1 ) \
X( mjtNum, qfrc_smooth, nv, 1 ) \