chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Fix tendon sphere wrapping when using external sidesite.

Browse Source 
PiperOrigin-RevId: 668391942
Change-Id: I78b44e1f90bb26fa6f5544eeb884c8ea22e41d33
This commit is contained in:
Taylor Howell
2024-08-28 04:29:00 -07:00
committed by Copybara-Service
parent 72a0ab145d
commit 3e701b2125
3 changed files with 73 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+2
View File
@@ -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
+6 -10
View File
@@ -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]);
}
test/engine/engine_util_misc_test.cc
+65 -4
View File
@@ -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;