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

Rename efc_diagApprox -> efc_diagA.

Browse Source 
The field now stores either an approximate or exact diagonal of the constraint matrix A, so the name is made more general.

PiperOrigin-RevId: 924244954
Change-Id: I62b2f76531fb88b7b3bf96e6769940197596702b
This commit is contained in:
Yuval Tassa
2026-05-31 04:53:36 -07:00
committed by Copybara-Service
parent cd6db9ebe2
commit 4548e81e4d
13 changed files with 32 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+12 -12
View File
@@ -1651,7 +1651,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
void mj_diagApprox(const mjModel* m, mjData* d) {
int id, dim, b1, b2, f, weldcnt = 0;
int nefc = d->nefc;
mjtNum tran, rot, fri, *dA = d->efc_diagApprox;
mjtNum tran, rot, fri, *dA = d->efc_diagA;
mjContact* con = NULL;
// loop over all constraints, compute approximate inverse inertia
@@ -2079,7 +2079,7 @@ static void getimpedance(const mjtNum* solimp, mjtNum pos, mjtNum margin,
}
// compute efc_R, efc_D, efc_KBIP, adjust efc_diagApprox
// compute efc_R, efc_D, efc_KBIP, adjust efc_diagA
void mj_makeImpedance(const mjModel* m, mjData* d) {
int dim, nefc = d->nefc;
mjtNum *R = d->efc_R, *KBIP = d->efc_KBIP;
@@ -2099,7 +2099,7 @@ void mj_makeImpedance(const mjModel* m, mjData* d) {
// set R and KBIP for all constraint dimensions
for (int j=0; j < dim; j++) {
// R = (1-imp)/imp * diagApprox
R[i+j] = mju_max(mjMINVAL, (1-imp)*d->efc_diagApprox[i+j]/imp);
R[i+j] = mju_max(mjMINVAL, (1-imp)*d->efc_diagA[i+j]/imp);
// constraint type
int tp = d->efc_type[i+j];
@@ -2190,9 +2190,9 @@ void mj_makeImpedance(const mjModel* m, mjData* d) {
d->efc_D[i] = 1 / R[i];
}
// adjust diagApprox so that R = (1-imp)/imp * diagApprox
// adjust diagA so that R = (1-imp)/imp * diagA
for (int i=0; i < nefc; i++) {
d->efc_diagApprox[i] = R[i] * KBIP[4*i+2] / (1-KBIP[4*i+2]);
d->efc_diagA[i] = R[i] * KBIP[4*i+2] / (1-KBIP[4*i+2]);
}
}
@@ -2830,12 +2830,12 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
// compute diagApprox
mj_diagApprox(m, d);
// compute KBIP, D, R, adjust diagApprox
// compute KBIP, D, R, adjust diagA
mj_makeImpedance(m, d);
}
// compute Y = J*M^{-1/2}; if flg_diagexact, overwrite efc_diagApprox with ||Y_i||^2
// compute Y = J*M^{-1/2}; if flg_diagexact, overwrite efc_diagA with ||Y_i||^2
static void mj_makeY(const mjModel* m, mjData* d, int flg_diagexact) {
int nefc = d->nefc, nv = m->nv;
@@ -2888,12 +2888,12 @@ static void mj_makeY(const mjModel* m, mjData* d, int flg_diagexact) {
d->efc_Y_colind, nefc,
d->qLD, m->M_rownnz, m->M_rowadr, m->M_colind, sqrtInvD);
// overwrite diagApprox with exact diagonal: diagApprox[i] = ||Y_i||^2
// overwrite diagA with exact diagonal: diagA[i] = ||Y_i||^2
if (flg_diagexact) {
for (int i=0; i < nefc; i++) {
int adr = d->efc_Y_rowadr[i];
int nnz = d->efc_Y_rownnz[i];
d->efc_diagApprox[i] = mju_dot(d->efc_Y+adr, d->efc_Y+adr, nnz);
d->efc_diagA[i] = mju_dot(d->efc_Y+adr, d->efc_Y+adr, nnz);
}
}
}
@@ -2914,10 +2914,10 @@ static void mj_makeY(const mjModel* m, mjData* d, int flg_diagexact) {
// Y = backsubM2(J')'
mj_solveM2(m, d, d->efc_Y, d->efc_J, sqrtInvD, nefc);
// overwrite diagApprox with exact diagonal: diagApprox[i] = ||Y_i||^2
// overwrite diagA with exact diagonal: diagA[i] = ||Y_i||^2
if (flg_diagexact) {
for (int i=0; i < nefc; i++) {
d->efc_diagApprox[i] = mju_dot(d->efc_Y+i*nv, d->efc_Y+i*nv, nv);
d->efc_diagA[i] = mju_dot(d->efc_Y+i*nv, d->efc_Y+i*nv, nv);
}
}
}
@@ -3023,7 +3023,7 @@ static void mj_makeAR(const mjModel* m, mjData* d) {
}
// compute efc_Y, optionally efc_diagApprox, optionally efc_AR
// compute efc_Y, optionally efc_diagA, optionally efc_AR
void mj_projectConstraint(const mjModel* m, mjData* d) {
int nefc = d->nefc;
src/engine/engine_core_constraint.h
+2 -2
View File
@@ -74,10 +74,10 @@ int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int di
//------------------------ parameter computation/extraction ----------------------------------------
// compute efc_diagApprox
// compute efc_diagA
void mj_diagApprox(const mjModel* m, mjData* d);
// compute efc_R, efc_D, efc_KDIP, adjust diagApprox
// compute efc_R, efc_D, efc_KDIP, adjust diagA
void mj_makeImpedance(const mjModel* m, mjData* d);
src/engine/engine_print.c
+1 -1
View File
@@ -1598,7 +1598,7 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena
printArray2d("EFC_POS", d->nefc, 1, d->efc_pos, fp, float_format);
printArray2d("EFC_MARGIN", d->nefc, 1, d->efc_margin, fp, float_format);
printArray2d("EFC_FRICTIONLOSS", d->nefc, 1, d->efc_frictionloss, fp, float_format);
printArray2d("EFC_DIAGAPPROX", d->nefc, 1, d->efc_diagApprox, fp, float_format);
printArray2d("EFC_DIAGA", d->nefc, 1, d->efc_diagA, fp, float_format);
printArray2d("EFC_KBIP", d->nefc, 4, d->efc_KBIP, fp, float_format);
printArray2d("EFC_D", d->nefc, 1, d->efc_D, fp, float_format);
printArray2d("EFC_R", d->nefc, 1, d->efc_R, fp, float_format);