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

Replace custom sort of sparse fixed tendons with mju_combineSparse.

Browse Source 
PiperOrigin-RevId: 720219887
Change-Id: I0491b3cb8987c119c3864fc0b5c38ca303d8f7f0
This commit is contained in:
Yuval Tassa
2025-01-27 10:23:59 -08:00
committed by Copybara-Service
parent d21f624264
commit 9605990648
2 changed files with 55 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+5 -23
View File
@@ -665,7 +665,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
rowadr[i] = (i > 0 ? rowadr[i-1] + rownnz[i-1] : 0);
}
// process joint tendon
// process fixed tendon
if (m->wrap_type[adr] == mjWRAP_JOINT) {
// process all defined joints
for (int j=0; j < tendon_num; j++) {
@@ -677,9 +677,10 @@ void mj_tendon(const mjModel* m, mjData* d) {
// add to moment
if (issparse) {
J[rowadr[i] + rownnz[i]] = m->wrap_prm[adr+j];
colind[rowadr[i] + rownnz[i]] = m->jnt_dofadr[k];
rownnz[i]++;
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);
}
// add to moment: dense
@@ -688,25 +689,6 @@ void mj_tendon(const mjModel* m, mjData* d) {
}
}
// sort on colind if sparse: custom insertion sort
if (issparse) {
int x, *list = colind+rowadr[i], nnz = rownnz[i];
mjtNum y, *listy = J+rowadr[i];
for (int k=1; k < nnz; k++) {
x = list[k];
y = listy[k];
int j = k-1;
while (j >= 0 && list[j] > x) {
list[j+1] = list[j];
listy[j+1] = listy[j];
j--;
}
list[j+1] = x;
listy[j+1] = y;
}
}
continue;
}
test/engine/engine_core_smooth_test.cc
+50
View File
@@ -109,6 +109,56 @@ TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) {
mj_deleteModel(model);
}
// ----------------------------- mj_tendon -------------------------------------
TEST_F(CoreSmoothTest, FixedTendonSortedIndices) {
constexpr char xml[] = R"(
<mujoco>
<option jacobian="sparse"/>
<worldbody>
<body>
<geom size=".1"/>
<joint name="0"/>
</body>
<body pos="1 0 0">
<geom size=".1"/>
<joint name="1"/>
</body>
<body pos="2 0 0">
<geom size=".1"/>
<joint name="2"/>
</body>
</worldbody>
<tendon>
<fixed>
<joint coef="3" joint="2"/>
<joint coef="2" joint="1"/>
<joint coef="1" joint="0"/>
</fixed>
</tendon>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
ASSERT_EQ(model->ntendon, 1);
ASSERT_EQ(model->nwrap, 3);
mjData* data = mj_makeData(model);
mj_fwdPosition(model, data);
int rowadr = data->ten_J_rowadr[0];
int* colind = data->ten_J_colind + rowadr;
mjtNum* J = data->ten_J + rowadr;
EXPECT_THAT(vector<mjtNum>(J, J + 3), ElementsAre(1, 2, 3));
EXPECT_THAT(vector<int>(colind, colind + 3), ElementsAre(0, 1, 2));
mj_deleteData(data);
mj_deleteModel(model);
}
// --------------------------- connect constraint ------------------------------
// test that bodies hanging on connects lead to expected force sensor readings