diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 01911f0d..88702a06 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -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; } diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 916fb625..47e1ff49 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -109,6 +109,56 @@ TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) { mj_deleteModel(model); } +// ----------------------------- mj_tendon ------------------------------------- + +TEST_F(CoreSmoothTest, FixedTendonSortedIndices) { + constexpr char xml[] = R"( + + + )"; + 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(J, J + 3), ElementsAre(1, 2, 3)); + EXPECT_THAT(vector(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