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
src/engine/engine_print.c
+31
View File
@@ -28,6 +28,7 @@
#include "engine/engine_support.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_sparse.h"
#define FLOAT_FORMAT "% -9.2g"
#define FLOAT_FORMAT_MAX_LEN 20
@@ -848,6 +849,36 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
printArray("QLDIAGINV", m->nv, 1, d->qLDiagInv, fp, float_format);
printArray("QLDIAGSQRTINV", m->nv, 1, d->qLDiagSqrtInv, fp, float_format);
// D_rownnz
fprintf(fp, NAME_FORMAT, "D_rownnz");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, "%d ", d->D_rownnz[i]);
}
fprintf(fp, "\n\n");
// D_rowadr
fprintf(fp, NAME_FORMAT, "D_rowadr");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, "%d ", d->D_rowadr[i]);
}
fprintf(fp, "\n\n");
// D_colind
fprintf(fp, NAME_FORMAT, "D_colind");
for (int i = 0; i < m->nD; i++) {
fprintf(fp, "%d ", d->D_colind[i]);
}
fprintf(fp, "\n\n");
// print qDeriv
mju_sparse2dense(M, d->qDeriv, m->nv, m->nv, d->D_rownnz, d->D_rowadr, d->D_colind);
printArray("QDERIV", m->nv, m->nv, M, fp, float_format);
// print qLU
mju_sparse2dense(M, d->qLU, m->nv, m->nv, d->D_rownnz, d->D_rowadr,
d->D_colind);
printArray("QLU", m->nv, m->nv, M, fp, float_format);
// contact
fprintf(fp, "CONTACT\n");
for (int i=0; i<d->ncon; i++) {