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

Move tendon sparsity fields from mjData to mjModel

Browse Source 
PiperOrigin-RevId: 875087590
Change-Id: I1a5489d2d2011795ee38b09d547e68acd72e3cc7
This commit is contained in:
Taylor Howell
2026-02-25 04:31:37 -08:00
committed by Copybara-Service
parent 253494739a
commit 52ed96bc3a
25 changed files with 332 additions and 178 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+23 -23
View File
@@ -568,16 +568,16 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
// copy Jacobian: sparse or dense
if (issparse) {
if (j == 0) {
NV = d->ten_J_rownnz[id[j]];
mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV);
mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV);
NV = m->ten_J_rownnz[id[j]];
mju_copyInt(chain, m->ten_J_colind+m->ten_J_rowadr[id[j]], NV);
mju_copy(jac[j], d->ten_J+m->ten_J_rowadr[id[j]], NV);
} else {
NV2 = d->ten_J_rownnz[id[j]];
mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2);
mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV2);
NV2 = m->ten_J_rownnz[id[j]];
mju_copyInt(chain2, m->ten_J_colind+m->ten_J_rowadr[id[j]], NV2);
mju_copy(jac[j], d->ten_J+m->ten_J_rowadr[id[j]], NV2);
}
} else {
mju_sparse2dense(jac[j], d->ten_J, 1, nv, d->ten_J_rownnz+id[j], d->ten_J_rowadr+id[j], d->ten_J_colind);
mju_sparse2dense(jac[j], d->ten_J, 1, nv, m->ten_J_rownnz+id[j], m->ten_J_rowadr+id[j], m->ten_J_colind);
}
}
}
@@ -737,13 +737,13 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
int efcadr = d->nefc;
// add constraint
if (issparse) {
mj_addConstraint(m, d, d->ten_J + d->ten_J_rowadr[i],
mj_addConstraint(m, d, d->ten_J + m->ten_J_rowadr[i],
0, 0, m->tendon_frictionloss[i],
1, mjCNSTR_FRICTION_TENDON, i,
d->ten_J_rownnz[i],
d->ten_J_colind+d->ten_J_rowadr[i]);
m->ten_J_rownnz[i],
m->ten_J_colind+m->ten_J_rowadr[i]);
} else {
mju_sparse2dense(jac, d->ten_J, 1, nv, d->ten_J_rownnz+i, d->ten_J_rowadr+i, d->ten_J_colind);
mju_sparse2dense(jac, d->ten_J, 1, nv, m->ten_J_rownnz+i, m->ten_J_rowadr+i, m->ten_J_colind);
mj_addConstraint(m, d, jac, 0, 0, m->tendon_frictionloss[i],
1, mjCNSTR_FRICTION_TENDON, i, 0, NULL);
}
@@ -885,13 +885,13 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
// prepare Jacobian
int efcadr = d->nefc;
if (issparse) {
mju_scl(jac, d->ten_J+d->ten_J_rowadr[i], -side, d->ten_J_rownnz[i]);
mju_scl(jac, d->ten_J+m->ten_J_rowadr[i], -side, m->ten_J_rownnz[i]);
mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_TENDON, i,
d->ten_J_rownnz[i],
d->ten_J_colind+d->ten_J_rowadr[i]);
m->ten_J_rownnz[i],
m->ten_J_colind+m->ten_J_rowadr[i]);
} else {
mju_sparse2dense(jac, d->ten_J, 1, nv, d->ten_J_rownnz+i, d->ten_J_rowadr+i, d->ten_J_colind);
mju_sparse2dense(jac, d->ten_J, 1, nv, m->ten_J_rownnz+i, m->ten_J_rowadr+i, m->ten_J_colind);
mju_scl(jac, jac, -side, nv);
mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_TENDON, i, 0, NULL);
@@ -1738,11 +1738,11 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
}
} else {
if (!j) {
NV = d->ten_J_rownnz[id[j]];
mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV);
NV = m->ten_J_rownnz[id[j]];
mju_copyInt(chain, m->ten_J_colind+m->ten_J_rowadr[id[j]], NV);
} else {
NV2 = d->ten_J_rownnz[id[j]];
mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2);
NV2 = m->ten_J_rownnz[id[j]];
mju_copyInt(chain2, m->ten_J_colind+m->ten_J_rowadr[id[j]], NV2);
}
}
}
@@ -1840,8 +1840,8 @@ static int mj_nf(const mjModel* m, const mjData* d, int *nnz) {
for (int i=0; i < ntendon; i++) {
if (m->tendon_frictionloss[i] > 0) {
nf += mj_addConstraintCount(m, 1, d->ten_J_rownnz[i]);
if (nnz) *nnz += d->ten_J_rownnz[i];
nf += mj_addConstraintCount(m, 1, m->ten_J_rownnz[i]);
if (nnz) *nnz += m->ten_J_rownnz[i];
}
}
@@ -1908,8 +1908,8 @@ static int mj_nl(const mjModel* m, const mjData* d, int *nnz) {
for (int i=0; i < ntendon; i++) {
int count = tendonLimit(m, d->ten_length, i);
for (int j = 0; j < count; j++) {
nl += mj_addConstraintCount(m, 1, d->ten_J_rownnz[i]);
if (nnz) *nnz += d->ten_J_rownnz[i];
nl += mj_addConstraintCount(m, 1, m->ten_J_rownnz[i]);
if (nnz) *nnz += m->ten_J_rownnz[i];
}
}
src/engine/engine_core_smooth.c
+20 -26
View File
@@ -905,7 +905,7 @@ void mj_flex(const mjModel* m, mjData* d) {
// compute tendon lengths and moments
void mj_tendon(const mjModel* m, mjData* d) {
int nv = m->nv, nten = m->ntendon;
int *rownnz = d->ten_J_rownnz, *rowadr = d->ten_J_rowadr, *colind = d->ten_J_colind;
const int *rownnz = m->ten_J_rownnz, *rowadr = m->ten_J_rowadr, *colind = m->ten_J_colind;
mjtNum *L = d->ten_length, *J = d->ten_J;
if (!nten) {
@@ -913,22 +913,20 @@ void mj_tendon(const mjModel* m, mjData* d) {
}
// allocate stack arrays
int *chain, *buf_ind;
mjtNum *jac1, *jac2, *jacdif, *tmp, *sparse_buf;
int *chain;
mjtNum *jac1, *jac2, *jacdif, *tmp;
mj_markStack(d);
jac1 = mjSTACKALLOC(d, 3*nv, mjtNum);
jac2 = mjSTACKALLOC(d, 3*nv, mjtNum);
jacdif = mjSTACKALLOC(d, 3*nv, mjtNum);
tmp = mjSTACKALLOC(d, nv, mjtNum);
chain = mjSTACKALLOC(d, nv, int);
buf_ind = mjSTACKALLOC(d, nv, int);
sparse_buf = mjSTACKALLOC(d, nv, mjtNum);
// clear results
mju_zero(L, nten);
// clear Jacobian
mju_zeroInt(rownnz, nten);
mju_zero(J, m->nJten);
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
@@ -947,9 +945,6 @@ void mj_tendon(const mjModel* m, mjData* d) {
d->ten_wrapnum[i] = 0;
int tendon_num = m->tendon_num[i];
// sparse Jacobian row init
rowadr[i] = (i > 0 ? rowadr[i-1] + rownnz[i-1] : 0);
// process fixed tendon
if (m->wrap_type[adr] == mjWRAP_JOINT) {
// process all defined joints
@@ -960,11 +955,10 @@ void mj_tendon(const mjModel* m, mjData* d) {
// add to length
L[i] += m->wrap_prm[adr+j] * d->qpos[m->jnt_qposadr[k]];
// add to moment
rownnz[i] = mju_combineSparse(J+rowadr[i], &m->wrap_prm[adr+j], 1, 1,
rownnz[i], 1,
colind+rowadr[i], &m->jnt_dofadr[k],
sparse_buf, buf_ind);
mjtNum coef = 1;
int dofadr = m->jnt_dofadr[k];
mju_combineSparseInc(J + rowadr[i], &coef, m->nv, 1, m->wrap_prm[adr+j],
rownnz[i], 1, colind + rowadr[i], &dofadr);
}
continue;
@@ -1059,9 +1053,9 @@ void mj_tendon(const mjModel* m, mjData* d) {
mju_mulMatTVec(tmp, jacdif, dif, 3, NV);
// add to existing
rownnz[i] = mju_combineSparse(J+rowadr[i], tmp, 1, 1/divisor,
rownnz[i], NV, colind+rowadr[i],
chain, sparse_buf, buf_ind);
mju_combineSparseInc(J+rowadr[i], tmp, nv, 1, 1/divisor,
rownnz[i], NV, colind+rowadr[i],
chain);
}
}
@@ -1419,10 +1413,10 @@ void mj_transmission(const mjModel* m, mjData* d) {
// moment
{
int ten_J_rownnz = d->ten_J_rownnz[id];
int ten_J_rowadr = d->ten_J_rowadr[id];
int ten_J_rownnz = m->ten_J_rownnz[id];
int ten_J_rowadr = m->ten_J_rowadr[id];
rownnz[i] = ten_J_rownnz;
mju_copyInt(colind + adr, d->ten_J_colind + ten_J_rowadr, ten_J_rownnz);
mju_copyInt(colind + adr, m->ten_J_colind + ten_J_rowadr, ten_J_rownnz);
mju_scl(moment + adr, d->ten_J + ten_J_rowadr, gear[0], ten_J_rownnz);
}
@@ -1717,9 +1711,9 @@ void mj_tendonArmature(const mjModel* m, mjData* d) {
}
// get sparse info for tendon k
int J_rowadr = d->ten_J_rowadr[k];
int J_rownnz = d->ten_J_rownnz[k];
const int* J_colind = d->ten_J_colind + J_rowadr;
int J_rowadr = m->ten_J_rowadr[k];
int J_rownnz = m->ten_J_rownnz[k];
const int* J_colind = m->ten_J_colind + J_rowadr;
mjtNum* ten_J = d->ten_J + J_rowadr;
// M += armature * ten_J' * ten_J
@@ -2649,9 +2643,9 @@ void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) {
if (coef) {
// sparse
int nnz = d->ten_J_rownnz[i];
int adr = d->ten_J_rowadr[i];
const int* colind = d->ten_J_colind + adr;
int nnz = m->ten_J_rownnz[i];
int adr = m->ten_J_rowadr[i];
const int* colind = m->ten_J_colind + adr;
const mjtNum* ten_J = d->ten_J + adr;
for (int j=0; j < nnz; j++) {
qfrc[colind[j]] += coef * ten_J[j];
src/engine/engine_derivative.c
+1 -1
View File
@@ -1776,7 +1776,7 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
}
// add sparse
addJTBJSparse(m, d, d->ten_J, &B, 1, i, d->ten_J_rownnz, d->ten_J_rowadr, d->ten_J_colind);
addJTBJSparse(m, d, d->ten_J, &B, 1, i, m->ten_J_rownnz, m->ten_J_rowadr, m->ten_J_colind);
}
}
src/engine/engine_forward.c
+1 -1
View File
@@ -232,7 +232,7 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
// tendon velocity: always sparse
mju_mulMatVecSparse(d->ten_velocity, d->ten_J, d->qvel, m->ntendon,
d->ten_J_rownnz, d->ten_J_rowadr, d->ten_J_colind, NULL);
m->ten_J_rownnz, m->ten_J_rowadr, m->ten_J_colind, NULL);
// actuator velocity: always sparse
if (!mjDISABLED(mjDSBL_ACTUATION)) {
src/engine/engine_io.c
+14 -12
View File
@@ -211,10 +211,10 @@ void mj_makeModel(mjModel** dest,
mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, mjtSize nskin,
mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone,
mjtSize nskinbonevert, mjtSize nhfield, mjtSize nhfielddata, mjtSize ntex, mjtSize ntexdata,
mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nwrap,
mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext, mjtSize ntextdata,
mjtSize ntuple, mjtSize ntupledata, mjtSize nkey, mjtSize nmocap, mjtSize nplugin,
mjtSize npluginattr, mjtSize nuser_body, mjtSize nuser_jnt, mjtSize nuser_geom,
mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nJten,
mjtSize nwrap, mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext,
mjtSize ntextdata, mjtSize ntuple, mjtSize ntupledata, mjtSize nkey, mjtSize nmocap,
mjtSize nplugin, mjtSize npluginattr, mjtSize nuser_body, mjtSize nuser_jnt, mjtSize nuser_geom,
mjtSize nuser_site, mjtSize nuser_cam, mjtSize nuser_tendon, mjtSize nuser_actuator,
mjtSize nuser_sensor, mjtSize nnames, mjtSize npaths) {
intptr_t offset = 0;
@@ -224,7 +224,7 @@ void mj_makeModel(mjModel** dest,
// CHECK SIZE PARAMETERS
{
// dummy variables for MJMODEL_SIZES set after mjModel construction
int nnames_map = 0, nJmom = 0, nJten = 0, ngravcomp = 0, nemax = 0, njmax = 0, nconmax=0;
int nnames_map = 0, nJmom = 0, ngravcomp = 0, nemax = 0, njmax = 0, nconmax=0;
int nuserdata=0, nsensordata=0, npluginstate=0, nhistory=0, narena=0, nbuffer=0;
// sizes must be non-negative and fit in int, except for the byte arrays texdata and textdata
@@ -243,7 +243,7 @@ void mj_makeModel(mjModel** dest,
#undef X
// suppress unused variable warnings
(void)nnames_map; (void)nJmom; (void)nJten; (void)ngravcomp; (void)nemax; (void)njmax; (void)nconmax;
(void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nemax; (void)njmax; (void)nconmax;
(void)nuserdata; (void)nsensordata; (void)npluginstate; (void)nhistory; (void)narena;
(void)nbuffer;
}
@@ -323,6 +323,7 @@ void mj_makeModel(mjModel** dest,
m->nexclude = nexclude;
m->neq = neq;
m->ntendon = ntendon;
m->nJten = nJten;
m->nwrap = nwrap;
m->nsensor = nsensor;
m->nnumeric = nnumeric;
@@ -410,11 +411,11 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
src->nskin, src->nskinvert, src->nskintexvert, src->nskinface,
src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata,
src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude,
src->neq, src->ntendon, src->nwrap, src->nsensor, src->nnumeric,
src->nnumericdata, src->ntext, src->ntextdata, src->ntuple,
src->ntupledata, src->nkey, src->nmocap, src->nplugin, src->npluginattr,
src->nuser_body, src->nuser_jnt, src->nuser_geom, src->nuser_site,
src->nuser_cam, src->nuser_tendon, src->nuser_actuator,
src->neq, src->ntendon, src->nJten, src->nwrap, src->nsensor,
src->nnumeric, src->nnumericdata, src->ntext, src->ntextdata,
src->ntuple, src->ntupledata, src->nkey, src->nmocap, src->nplugin,
src->npluginattr, src->nuser_body, src->nuser_jnt, src->nuser_geom,
src->nuser_site, src->nuser_cam, src->nuser_tendon, src->nuser_actuator,
src->nuser_sensor, src->nnames, src->npaths);
}
if (!dest) {
@@ -597,7 +598,8 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) {
sizes[49], sizes[50], sizes[51], sizes[52], sizes[53], sizes[54], sizes[55],
sizes[56], sizes[57], sizes[58], sizes[59], sizes[60], sizes[61], sizes[62],
sizes[63], sizes[64], sizes[65], sizes[66], sizes[67], sizes[68], sizes[69],
sizes[70], sizes[71], sizes[72], sizes[73], sizes[74], sizes[75], sizes[76]);
sizes[70], sizes[71], sizes[72], sizes[73], sizes[74], sizes[75], sizes[76],
sizes[77]);
// mj_makeModel may fail if the input buffer has invalid sizes
if (!m) {
src/engine/engine_io.h
+4 -4
View File
@@ -58,10 +58,10 @@ void mj_makeModel(mjModel** dest,
mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, mjtSize nskin,
mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone,
mjtSize nskinbonevert, mjtSize nhfield, mjtSize nhfielddata, mjtSize ntex, mjtSize ntexdata,
mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nwrap,
mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext, mjtSize ntextdata,
mjtSize ntuple, mjtSize ntupledata, mjtSize nkey, mjtSize nmocap, mjtSize nplugin,
mjtSize npluginattr, mjtSize nuser_body, mjtSize nuser_jnt, mjtSize nuser_geom,
mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nJten,
mjtSize nwrap, mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext,
mjtSize ntextdata, mjtSize ntuple, mjtSize ntupledata, mjtSize nkey, mjtSize nmocap,
mjtSize nplugin, mjtSize npluginattr, mjtSize nuser_body, mjtSize nuser_jnt, mjtSize nuser_geom,
mjtSize nuser_site, mjtSize nuser_cam, mjtSize nuser_tendon, mjtSize nuser_actuator,
mjtSize nuser_sensor, mjtSize nnames, mjtSize npaths);
src/engine/engine_passive.c
+3 -3
View File
@@ -473,9 +473,9 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// transform to joint torque, add to qfrc_{spring, damper}
if (frc_spring || frc_damper) {
int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i];
for (int j=d->ten_J_rowadr[i]; j < end; j++) {
int k = d->ten_J_colind[j];
int end = m->ten_J_rowadr[i] + m->ten_J_rownnz[i];
for (int j=m->ten_J_rowadr[i]; j < end; j++) {
int k = m->ten_J_colind[j];
mjtNum J = d->ten_J[j];
d->qfrc_spring[k] += J * frc_spring;
d->qfrc_damper[k] += J * frc_damper;
src/engine/engine_print.c
+6 -6
View File
@@ -1441,12 +1441,12 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena
printArray2d("FLEXEDGE_LENGTH", m->nflexedge, 1, d->flexedge_length, fp, float_format);
printArray2d("TEN_LENGTH", m->ntendon, 1, d->ten_length, fp, float_format);
mj_printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, d->ten_J_rowadr, NULL,
d->ten_J_rownnz, NULL, d->ten_J_colind, fp);
printArray2dInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp);
printArray2dInt("TEN_J_ROWADR", m->ntendon, 1, d->ten_J_rowadr, fp);
printSparse("TEN_J", d->ten_J, m->ntendon, d->ten_J_rownnz,
d->ten_J_rowadr, d->ten_J_colind, fp, float_format);
mj_printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, m->ten_J_rowadr, NULL,
m->ten_J_rownnz, NULL, m->ten_J_colind, fp);
printArray2dInt("TEN_J_ROWNNZ", m->ntendon, 1, m->ten_J_rownnz, fp);
printArray2dInt("TEN_J_ROWADR", m->ntendon, 1, m->ten_J_rowadr, fp);
printSparse("TEN_J", d->ten_J, m->ntendon, m->ten_J_rownnz,
m->ten_J_rowadr, m->ten_J_colind, fp, float_format);
for (int i=0; i < m->ntendon; i++) {
fprintf(fp, "TENDON %d: %d wrap points\n", i, d->ten_wrapnum[i]);
for (int j=0; j < d->ten_wrapnum[i]; j++) {
src/engine/engine_setconst.c
+89 -1
View File
@@ -282,6 +282,93 @@ static void setFixed(mjModel* m, mjData* d) {
mj_freeStack(d);
}
// compute tendon Jacobian sparsity
static void makeTendonSparse(mjModel* m) {
int ntendon = m->ntendon;
int* rownnz = m->ten_J_rownnz;
int* rowadr = m->ten_J_rowadr;
int* colind = m->ten_J_colind;
if (!ntendon) {
return;
}
// clear
mju_zeroInt(rownnz, ntendon);
mju_zeroInt(rowadr, ntendon);
// compute rownnz, rowadr, and colind for each tendon
for (int i = 0; i < ntendon; i++) {
rowadr[i] = (i > 0 ? rowadr[i-1] + rownnz[i-1] : 0);
int adr = m->tendon_adr[i];
int num = m->tendon_num[i];
// joint tendon: each wrap object is a joint, colind is its dofadr
if (m->wrap_type[adr] == mjWRAP_JOINT) {
for (int j = 0; j < num; j++) {
colind[rowadr[i] + j] = m->jnt_dofadr[m->wrap_objid[adr + j]];
}
rownnz[i] = num;
} else {
// spatial tendon: collect used dofs from wrap object bodies
int nnz = 0;
for (int j = 0; j < num; j++) {
int type = m->wrap_type[adr + j];
// get body id from site or geom wrap object
int bodyid = -1;
if (type == mjWRAP_SITE) {
bodyid = m->site_bodyid[m->wrap_objid[adr + j]];
} else if (type == mjWRAP_SPHERE || type == mjWRAP_CYLINDER) {
bodyid = m->geom_bodyid[m->wrap_objid[adr + j]];
}
// walk up the body tree, collecting used dofs
if (bodyid > 0) {
int bid = bodyid;
while (bid > 0) {
int bdofadr = m->body_dofadr[bid];
int bdofnum = m->body_dofnum[bid];
for (int k = 0; k < bdofnum; k++) {
int dof = bdofadr + k;
// check if dof already in colind
int found = 0;
for (int l = 0; l < nnz; l++) {
if (colind[rowadr[i] + l] == dof) {
found = 1;
break;
}
}
// append new dof
if (!found) {
colind[rowadr[i] + nnz] = dof;
nnz++;
}
}
bid = m->body_parentid[bid];
}
}
}
rownnz[i] = nnz;
}
// sort colind for this tendon
int nnz = rownnz[i];
for (int j = 0; j < nnz - 1; j++) {
for (int k = j + 1; k < nnz; k++) {
// swap out-of-order entries
if (colind[rowadr[i] + k] < colind[rowadr[i] + j]) {
int tmp = colind[rowadr[i] + j];
colind[rowadr[i] + j] = colind[rowadr[i] + k];
colind[rowadr[i] + k] = tmp;
}
}
}
}
}
// compute flex sparsity: flexedge_J_{rowadr,rownnz,colind} and flexvert_J_{rowadr,rownnz}
static void makeFlexSparse(mjModel* m, mjData* d) {
int nv = m->nv;
@@ -552,6 +639,7 @@ static void mj_alignFlex(mjModel* m, mjData* d) {
// set quantities that depend on qpos0
static void set0(mjModel* m, mjData* d) {
makeTendonSparse(m);
makeFlexSparse(m, d);
mj_alignFlex(m, d);
int nv = m->nv;
@@ -756,7 +844,7 @@ static void set0(mjModel* m, mjData* d) {
// compute tendon_invweight0
for (int i=0; i < m->ntendon; i++) {
mju_sparse2dense(tmp, d->ten_J, 1, nv, d->ten_J_rownnz+i, d->ten_J_rowadr+i, d->ten_J_colind);
mju_sparse2dense(tmp, d->ten_J, 1, nv, m->ten_J_rownnz+i, m->ten_J_rowadr+i, m->ten_J_colind);
// solve into tmp+nv
mj_solveM(m, d, tmp+nv, tmp, 1);