Fix tendon sphere wrapping when using external sidesite.

PiperOrigin-RevId: 668391942
Change-Id: I78b44e1f90bb26fa6f5544eeb884c8ea22e41d33

doc/changelog.rst

@@ -25,6 +25,8 @@ General
   parent model, but only on the first attachment.
 - Added the :ref:`mjtSameFrame` enum which contains the possible frame alignments of bodies and their children. These
   alignments are used as shortcuts in :ref:`mj_kinematics`.
+- Fixed a bug in tendon wrapping around spheres. Before this fix, tendons that wrapped around spheres with an
+  externally-placed :ref:`sidesite<spatial-geom-sidesite>` could jump inside the sphere instead of wrapping around it.
 
 MJX
 ^^^

src/engine/engine_util_misc.c

@@ -362,21 +362,17 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
   if (side) {
     // side point: apply same projection as x0, x1
     mju_sub3(tmp, side, xpos);
-    mju_mulMatTVec(s, xmat, tmp, 3, 3);
+    mju_mulMatTVec3(s, xmat, tmp);
+
+    // side point: project and rescale
     sd[0] = mju_dot3(s, axis[0]);
     sd[1] = mju_dot3(s, axis[1]);
-
+    mju_normalize(sd, 2);
-    // map to circle if outside, set to (0,0) if inside
+    mju_scl(sd, sd, size[0], 2);
-    if (mju_norm(sd, 2) >= size[0]) {
-      mju_normalize(sd, 2);
-      mju_scl(sd, sd, size[0], 2);
-    } else {
-      sd[0] = sd[1] = 0;
-    }
   }
 
   // apply inside wrap
-  if (side && sd[0] == 0 && sd[1] == 0) {
+  if (side && mju_norm3(s) < size[0]) {
     wlen = wrap_inside(pnt, d, size[0]);
   }
 

test/engine/engine_util_misc_test.cc

@@ -35,7 +35,9 @@ using ::testing::Ne;
 using ::testing::StrEq;
 using ::testing::ElementsAreArray;
 
-TEST_F(MujocoTest, PrintsMemoryWarning) {
+using UtilMiscTest = MujocoTest;
+
+TEST_F(UtilMiscTest, PrintsMemoryWarning) {
   EXPECT_THAT(mju_warningText(mjWARN_CNSTRFULL, pow(2, 10)),
               HasSubstr("1K bytes"));
   EXPECT_THAT(mju_warningText(mjWARN_CNSTRFULL, pow(2, 20)),
@@ -52,7 +54,7 @@ TEST_F(MujocoTest, PrintsMemoryWarning) {
               HasSubstr("1073741825 bytes"));
 }
 
-TEST_F(MujocoTest, Sigmoid) {
+TEST_F(UtilMiscTest, Sigmoid) {
   // function values
   EXPECT_EQ(mju_sigmoid(-1),  0);
   EXPECT_EQ(mju_sigmoid(0),   0);
@@ -78,6 +80,65 @@ TEST_F(MujocoTest, Sigmoid) {
   EXPECT_THAT(dy_dx_0p5, DoubleNear(expected, dx));
 }
 
+TEST_F(UtilMiscTest, SphereWrap) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <default>
+      <site size=".015" rgba="1 0 0 1"/>
+    </default>
+
+    <worldbody>
+      <light pos="0 0 3"/>
+
+      <site name="fixed" pos="0 0 1"/>
+      <geom name="sphere" size=".1" pos="0 0 0.5"/>
+      <site name="sidesite" pos=".52 0 .5"/>
+      <body pos="0 0 .1">
+        <freejoint/>
+        <geom size=".05"/>
+        <site name="body" pos="0 0 .05"/>
+      </body>
+    </worldbody>
+
+    <tendon>
+      <spatial name="tendon" range="0 0.8">
+        <site site="fixed"/>
+        <geom geom="sphere" sidesite="sidesite"/>
+        <site site="body"/>
+      </spatial>
+    </tendon>
+
+    <sensor>
+      <tendonpos tendon="tendon"/>
+    </sensor>
+
+    <keyframe>
+      <key qpos="-0.00653537 -0.068031 0.301253 0.982186 -0.180204 -0.0273515 0.0457068"/>
+      <key qpos="-0.00653537 -0.069 0.301253 0.982186 -0.180204 -0.0273515 0.0457068"/>
+    </keyframe>
+  </mujoco>
+  )";
+
+  mjModel* model = LoadModelFromString(xml);
+  mjData* data = mj_makeData(model);
+
+  // measure tendon length for keyframe 0
+  mj_resetDataKeyframe(model, data, 0);
+  mj_forward(model, data);
+  mjtNum ten_length0 = data->sensordata[0];
+
+  // measure tendon length for keyframe 1
+  mj_resetDataKeyframe(model, data, 1);
+  mj_forward(model, data);
+  mjtNum ten_length1 = data->sensordata[0];
+
+  mjtNum diff = ten_length1 - ten_length0;
+  EXPECT_LT(mju_abs(diff), 1e-3);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 // compute time constant as in Millard et al. (2013) https://doi.org/10.1115/1.4023390
 mjtNum muscleDynamicsMillard(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) {
   // clamp control
@@ -97,7 +158,7 @@ mjtNum muscleDynamicsMillard(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) {
   return (ctrlclamp-act) / mjMAX(mjMINVAL, tau);
 }
 
-TEST_F(MujocoTest, SmoothMuscleDynamics) {
+TEST_F(UtilMiscTest, SmoothMuscleDynamics) {
   mjtNum prm[3] = {0.01, 0.04, 0.0};
 
   // exact equality if tau_smooth = 0
@@ -135,7 +196,7 @@ TEST_F(MujocoTest, SmoothMuscleDynamics) {
   }
 }
 
-TEST_F(MujocoTest, MuscleGainLength) {
+TEST_F(UtilMiscTest, MuscleGainLength) {
   mjtNum lmin = 0.5;
   mjtNum lmax = 1.5;