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