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

Move flexedge sparse Jacobian indices to mjModel.

Browse Source 
PiperOrigin-RevId: 855718305
Change-Id: Ic136f4efa6735b934288f65c332d71ee8b0f6da5
This commit is contained in:
Alessio Quaglino
2026-01-13 07:32:31 -08:00
committed by Copybara-Service
parent 09a6fb7d4d
commit a49576e469
17 changed files with 70 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+3 -3
View File
@@ -627,10 +627,10 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
// add constraint: sparse or dense
if (issparse) {
mj_addConstraint(m, d, d->flexedge_J+d->flexedge_J_rowadr[e], cpos, 0, 0,
mj_addConstraint(m, d, d->flexedge_J+m->flexedge_J_rowadr[e], cpos, 0, 0,
1, mjCNSTR_EQUALITY, i,
d->flexedge_J_rownnz[e],
d->flexedge_J_colind+d->flexedge_J_rowadr[e]);
m->flexedge_J_rownnz[e],
m->flexedge_J_colind+m->flexedge_J_rowadr[e]);
} else {
mj_addConstraint(m, d, d->flexedge_J+e*nv, cpos, 0, 0,
1, mjCNSTR_EQUALITY, i,
src/engine/engine_core_smooth.c
+2 -2
View File
@@ -536,7 +536,7 @@ void mj_updateDynamicBVH(const mjModel* m, mjData* d, int bvhadr, int bvhnum) {
// compute flex-related quantities
void mj_flex(const mjModel* m, mjData* d) {
int nv = m->nv, issparse = mj_isSparse(m);
int* rowadr = d->flexedge_J_rowadr, *rownnz = d->flexedge_J_rownnz;
int* rowadr = m->flexedge_J_rowadr, *rownnz = m->flexedge_J_rownnz;
mjtNum* J = d->flexedge_J;
// skip if no flexes
@@ -720,7 +720,7 @@ void mj_flex(const mjModel* m, mjData* d) {
// copy sparsity info
rownnz[ebase+e] = NV;
mju_copyInt(d->flexedge_J_colind + rowadr[ebase+e], chain, NV);
mju_copyInt(m->flexedge_J_colind + rowadr[ebase+e], chain, NV);
}
// dense edge Jacobian
src/engine/engine_derivative.c
+1 -1
View File
@@ -1498,7 +1498,7 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
// add sparse or dense
if (mj_isSparse(m)) {
addJTBJSparse(m, d, d->flexedge_J, &B, 1, e,
d->flexedge_J_rownnz, d->flexedge_J_rowadr, d->flexedge_J_colind);
m->flexedge_J_rownnz, m->flexedge_J_rowadr, m->flexedge_J_colind);
} else {
addJTBJ(m, d, d->flexedge_J+e*nv, &B, 1);
}
src/engine/engine_forward.c
+1 -1
View File
@@ -221,7 +221,7 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
// flexedge velocity: dense or sparse
if (mj_isSparse(m)) {
mju_mulMatVecSparse(d->flexedge_velocity, d->flexedge_J, d->qvel, m->nflexedge,
d->flexedge_J_rownnz, d->flexedge_J_rowadr, d->flexedge_J_colind, NULL);
m->flexedge_J_rownnz, m->flexedge_J_rowadr, m->flexedge_J_colind, NULL);
} else {
mju_mulMatVec(d->flexedge_velocity, d->flexedge_J, d->qvel, m->nflexedge, m->nv);
}
src/engine/engine_passive.c
+3 -3
View File
@@ -414,9 +414,9 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// transform to joint torque, add to qfrc_{spring, damper}: dense or sparse
if (issparse) {
int end = d->flexedge_J_rowadr[e] + d->flexedge_J_rownnz[e];
for (int j=d->flexedge_J_rowadr[e]; j < end; j++) {
int colind = d->flexedge_J_colind[j];
int end = m->flexedge_J_rowadr[e] + m->flexedge_J_rownnz[e];
for (int j=m->flexedge_J_rowadr[e]; j < end; j++) {
int colind = m->flexedge_J_colind[j];
mjtNum J = d->flexedge_J[j];
d->qfrc_spring[colind] += J * frc_spring;
d->qfrc_damper[colind] += J * frc_damper;
src/engine/engine_print.c
+3 -5
View File
@@ -1284,12 +1284,10 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena
printArray2d("FLEXEDGE_J", m->nflexedge, m->nv, d->flexedge_J, fp, float_format);
} else {
mj_printSparsity("FLEXEDGE_J: flex edge connectivity", m->nflexedge, m->nv,
d->flexedge_J_rowadr, NULL, d->flexedge_J_rownnz, NULL, d->flexedge_J_colind,
m->flexedge_J_rowadr, NULL, m->flexedge_J_rownnz, NULL, m->flexedge_J_colind,
fp);
printArray2dInt("FLEXEDGE_J_ROWNNZ", m->nflexedge, 1, d->flexedge_J_rownnz, fp);
printArray2dInt("FLEXEDGE_J_ROWADR", m->nflexedge, 1, d->flexedge_J_rowadr, fp);
printSparse("FLEXEDGE_J", d->flexedge_J, m->nflexedge, d->flexedge_J_rownnz,
d->flexedge_J_rowadr, d->flexedge_J_colind, fp, float_format);
printSparse("FLEXEDGE_J", d->flexedge_J, m->nflexedge, m->flexedge_J_rownnz,
m->flexedge_J_rowadr, m->flexedge_J_colind, fp, float_format);
}
printArray2d("FLEXEDGE_LENGTH", m->nflexedge, 1, d->flexedge_length, fp, float_format);
src/engine/engine_setconst.c
+3 -3
View File
@@ -470,9 +470,9 @@ static void set0(mjModel* m, mjData* d) {
// make dense vector into tmp
if (mj_isSparse(m)) {
mju_zero(tmp, nv);
int end = d->flexedge_J_rowadr[i] + d->flexedge_J_rownnz[i];
for (int j=d->flexedge_J_rowadr[i]; j < end; j++) {
tmp[d->flexedge_J_colind[j]] = d->flexedge_J[j];
int end = m->flexedge_J_rowadr[i] + m->flexedge_J_rownnz[i];
for (int j=m->flexedge_J_rowadr[i]; j < end; j++) {
tmp[m->flexedge_J_colind[j]] = d->flexedge_J[j];
}
} else {
mju_copy(tmp, d->flexedge_J+i*nv, nv);