From 96059906480ab3f301568f53ca22b1b578b07743 Mon Sep 17 00:00:00 2001
From: Yuval Tassa <tassa@google.com>
Date: Mon, 27 Jan 2025 10:23:59 -0800
Subject: [PATCH] Replace custom sort of sparse fixed tendons with
 `mju_combineSparse`.

PiperOrigin-RevId: 720219887
Change-Id: I0491b3cb8987c119c3864fc0b5c38ca303d8f7f0
---
 src/engine/engine_core_smooth.c        | 28 +++------------
 test/engine/engine_core_smooth_test.cc | 50 ++++++++++++++++++++++++++
 2 files changed, 55 insertions(+), 23 deletions(-)

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"(
+  <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