From 3e701b2125a42ff91c5f985dcba170202daed9d4 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Wed, 28 Aug 2024 04:29:00 -0700 Subject: [PATCH] Fix tendon sphere wrapping when using external sidesite. PiperOrigin-RevId: 668391942 Change-Id: I78b44e1f90bb26fa6f5544eeb884c8ea22e41d33 --- doc/changelog.rst | 2 + src/engine/engine_util_misc.c | 16 +++---- test/engine/engine_util_misc_test.cc | 69 ++++++++++++++++++++++++++-- 3 files changed, 73 insertions(+), 14 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 368a6348..750b7525 100644 --- a/doc/changelog.rst +++ b/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` could jump inside the sphere instead of wrapping around it. MJX ^^^ diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 7f716b39..95c66781 100644 --- a/src/engine/engine_util_misc.c +++ b/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]); - - // map to circle if outside, set to (0,0) if inside - if (mju_norm(sd, 2) >= size[0]) { - mju_normalize(sd, 2); - mju_scl(sd, sd, size[0], 2); - } else { - sd[0] = sd[1] = 0; - } + mju_normalize(sd, 2); + mju_scl(sd, sd, size[0], 2); } // 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]); } diff --git a/test/engine/engine_util_misc_test.cc b/test/engine/engine_util_misc_test.cc index aca400a5..bf785a00 100644 --- a/test/engine/engine_util_misc_test.cc +++ b/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"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + 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;