chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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];
}
}