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

Improved upper bound for nJten

Browse Source 
PiperOrigin-RevId: 872479828
Change-Id: I750d56320d6ace6145475d378cb9d0c30919438d
This commit is contained in:
Taylor Howell
2026-02-19 11:07:41 -08:00
committed by Copybara-Service
parent 9efe41c0c1
commit 84431fbf7e
1 changed files with 39 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_model.cc
+39 -3
View File
@@ -3231,9 +3231,45 @@ int mjCModel::CountNJten(const mjModel* m) {
int nv = m->nv;
int ntendon = m->ntendon;
// conservative upper bound: each tendon can have at most nv non-zeros
// TODO(taylorhowell): compute tighter bound
int count = ntendon * nv;
std::vector<bool> dof_bitmap(nv, false);
int count = 0;
for (int i = 0; i < ntendon; i++) {
int adr = m->tendon_adr[i];
int num = m->tendon_num[i];
if (m->wrap_type[adr] == mjWRAP_JOINT) {
count += num;
continue;
}
std::fill(dof_bitmap.begin(), dof_bitmap.end(), false);
for (int j = 0; j < num; j++) {
int type = m->wrap_type[adr + j];
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]];
}
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++) {
dof_bitmap[bdofadr + k] = true;
}
bid = m->body_parentid[bid];
}
}
}
// only count unique dofs
for (int j = 0; j < nv; j++) {
count += dof_bitmap[j];
}
}
return count;
}