Polynomial stiffness and damping https://youtu.be/aKa3ZlEF9_Y

PiperOrigin-RevId: 884607673
Change-Id: If8088dbf37fed1055304778a7eb84dec52cba920
This commit is contained in:
Yuval Tassa
2026-03-16 13:24:44 -07:00
committed by Copybara-Service
parent aec1b45dce
commit efae9157a7
38 changed files with 1093 additions and 176 deletions
+67
View File
@@ -1168,6 +1168,73 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
mj_deleteModel(m);
}
TEST_F(DerivativeTest, NonlinearDampingDerivative) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" damping="2 3 4"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qvel="3"/>
</keyframe>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjtNum dt_small = 1e-4;
mjtNum dt_large = 1e-2;
mjtNum duration = 1.0;
mjData* d_gt = mj_makeData(m);
mjData* d_enabled = mj_makeData(m);
mjData* d_disabled = mj_makeData(m);
mj_resetDataKeyframe(m, d_gt, 0);
mj_resetDataKeyframe(m, d_enabled, 0);
mj_resetDataKeyframe(m, d_disabled, 0);
m->opt.integrator = mjINT_EULER;
mjtNum error_enabled = 0;
mjtNum error_disabled = 0;
int nsteps_large = static_cast<int>(duration / dt_large);
int substeps = static_cast<int>(dt_large / dt_small);
for (int i = 0; i < nsteps_large; i++) {
m->opt.timestep = dt_small;
m->opt.disableflags |= mjDSBL_EULERDAMP; // disable implicit damping
for (int j = 0; j < substeps; j++) {
mj_step(m, d_gt);
}
m->opt.timestep = dt_large;
mj_step(m, d_disabled);
m->opt.disableflags &= ~mjDSBL_EULERDAMP; // enable implicit damping
mj_step(m, d_enabled);
mjtNum diff_enabled = d_gt->qvel[0] - d_enabled->qvel[0];
mjtNum diff_disabled = d_gt->qvel[0] - d_disabled->qvel[0];
error_enabled += diff_enabled * diff_enabled;
error_disabled += diff_disabled * diff_disabled;
}
EXPECT_LT(error_enabled, error_disabled)
<< "Euler with implicit damping should be more accurate than without "
<< "when nonlinear damping derivatives are correctly handled";
mj_deleteData(d_disabled);
mj_deleteData(d_enabled);
mj_deleteData(d_gt);
mj_deleteModel(m);
}
// implicit derivatives should use next activation when actearly is set
TEST_F(DerivativeTest, ActearlyDerivative) {
static constexpr char xml[] = R"(
+1
View File
@@ -1665,6 +1665,7 @@ TEST_F(ForwardTest, FlexTrilinearInstability) {
mj_deleteData(data);
mj_deleteModel(model);
}
// Verify that flex damping does not affect rigid body motion
TEST_F(ForwardTest, FlexDampingRigidMotion) {
constexpr char xml[] = R"(
+371
View File
@@ -102,6 +102,142 @@ TEST_F(PassiveTest, GravcompNestedBody) {
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolyStiffnessSlide) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" stiffness="10 5 1"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qpos="2"/>
</keyframe>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
EXPECT_FLOAT_EQ(d->qfrc_spring[0], -48);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolyStiffnessAntiSymmetric) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" stiffness="10 5 1"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qpos="-2"/>
</keyframe>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
EXPECT_FLOAT_EQ(d->qfrc_spring[0], 8);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolyStiffnessTendon) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" name="j"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<tendon>
<fixed>
<joint joint="j" coef="1"/>
</fixed>
</tendon>
<keyframe>
<key qpos="2"/>
</keyframe>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
m->tendon_stiffness[0] = 10;
m->tendon_stiffnesspoly[0] = 5;
m->tendon_stiffnesspoly[1] = 1;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
EXPECT_FLOAT_EQ(d->qfrc_spring[0], -48);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolyStiffnessEnergy) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.0001">
<flag energy="enable"/>
</option>
<worldbody>
<body>
<joint type="slide" stiffness="10 5 1"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qpos="2"/>
</keyframe>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
mjtNum total_energy = d->energy[0] + d->energy[1];
for (int i = 0; i < 100; i++) {
mj_step(m, d);
EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.002);
}
mj_deleteData(d);
mj_deleteModel(m);
}
// ------------------------ ellipsoid fluid model ------------------------------
using EllipsoidFluidTest = MujocoTest;
@@ -475,5 +611,240 @@ TEST_F(ElasticityTest, ElasticEnergySolid) {
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolynomialStiffnessJoint) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" stiffness="2 3 4"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qpos="0.5"/>
</keyframe>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull());
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
mjtNum x = 0.5;
mjtNum a = 2, b = 3, c = 4;
mjtNum expected = -(a + b * x + c * x * x) * x;
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolynomialStiffnessNegativeDisplacement) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" stiffness="2 3 4"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qpos="-0.5"/>
</keyframe>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull());
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
mjtNum x = -0.5;
mjtNum a = 2, b = 3, c = 4;
mjtNum expected = -(a + b * x + c * x * x) * x;
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolyStiffnessFixedTendon) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" name="j"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<tendon>
<fixed stiffness="10 5 1">
<joint joint="j" coef="1"/>
</fixed>
</tendon>
<keyframe>
<key qpos="2"/>
</keyframe>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
mjtNum x = d->ten_length[0] - m->tendon_lengthspring[1];
mjtNum expected = -(10 + 5*x + 1*x*x) * x;
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolyStiffnessSpatialTendon) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<site name="s0"/>
<body>
<joint type="slide" name="j"/>
<geom size="1" mass="1"/>
<site name="s1"/>
</body>
</worldbody>
<tendon>
<spatial stiffness="10 5 1">
<site site="s0"/>
<site site="s1"/>
</spatial>
</tendon>
<keyframe>
<key qpos="2"/>
</keyframe>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
mjtNum x = d->ten_length[0] - m->tendon_lengthspring[1];
mjtNum expected = -x * (10 + 5*x + 1*x*x);
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolynomialDampingJoint) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" damping="2 3 4"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qvel="0.5"/>
</keyframe>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull());
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
mjtNum v = 0.5;
mjtNum a = 2, b = 3, c = 4;
mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v);
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolynomialDampingNegativeVelocity) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" damping="2 3 4"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qvel="-0.5"/>
</keyframe>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull());
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
mjtNum v = -0.5;
mjtNum a = 2, b = 3, c = 4;
mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v);
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolynomialDampingTendon) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" name="j"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<tendon>
<fixed damping="10 5 1">
<joint joint="j" coef="1"/>
</fixed>
</tendon>
<keyframe>
<key qvel="2"/>
</keyframe>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
mjtNum v = d->ten_velocity[0];
mjtNum expected = -(10*v + 5*v*mju_abs(v) + 1*v*v*v);
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco
+38
View File
@@ -538,6 +538,44 @@ TEST_F(SensorTest, KineticEnergy) {
mj_deleteModel(model);
}
TEST_F(SensorTest, PolyStiffnessEnergy) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.0001">
<flag energy="enable"/>
</option>
<worldbody>
<body>
<joint type="slide" stiffness="10 5 1"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qpos="2"/>
</keyframe>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
mjtNum total_energy = d->energy[0] + d->energy[1];
for (int i = 0; i < 100; i++) {
mj_step(m, d);
EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.002);
}
mj_deleteData(d);
mj_deleteModel(m);
}
// test clock sensor
TEST_F(SensorTest, Clock) {
constexpr char xml[] = R"(
+89
View File
@@ -0,0 +1,89 @@
<mujoco model="Polynomial Stiffness">
<!--
Four boxes hanging from spatial tendons, demonstrating polynomial stiffness.
Each tendon has a different stiffness profile:
1. Linear: k(q) = a*q (stiffness="100 0 0")
2. Quadratic: k(q) = b*q^2 (stiffness="0 200 0")
3. Cubic: k(q) = c*q^3 (stiffness="0 0 300")
4. Combination: k(q) = a*q + b*q^2 + c*q^3 (stiffness="100 -400 400")
-->
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global offwidth="1280" offheight="720" elevation="-10"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="32" height="32"/>
<texture name="grid" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 .15 .2" width="512" height="512"/>
<material name="grid" reflectance="0.3" texture="grid" texrepeat="5 5" texuniform="true"/>
</asset>
<default>
<geom condim="3"/>
<site rgba="0 .8 0 .5" size=".03"/>
<tendon width=".01" rgba=".9 .9 .9 1" springlength="0 .3"/>
<default class="box">
<geom type="box" size=".2 .2 .2" mass="5"/>
</default>
</default>
<worldbody>
<geom type="plane" size="3 3 .01" material="grid"/>
<light directional="true" diffuse=".4 .4 .4" specular="0 0 0" pos="0 0 5" dir="0 0 -1" castshadow="false"/>
<light directional="false" diffuse=".8 .8 .8" specular=".3 .3 .3" pos="0 -2 4" dir="0 .5 -1"/>
<!-- anchor sites -->
<site name="anchor_linear" pos="-1.5 0 2"/>
<site name="anchor_quadratic" pos="-.5 0 2"/>
<site name="anchor_cubic" pos=".5 0 2"/>
<site name="anchor_combi" pos="1.5 0 2"/>
<body name="linear" pos="-1.5 0 1.5">
<freejoint/>
<geom class="box" rgba=".2 .6 1 1"/>
<site name="hook_linear" pos="0 0 .2"/>
</body>
<body name="quadratic" pos="-.5 0 1.5">
<freejoint/>
<geom class="box" rgba="1 .4 .1 1"/>
<site name="hook_quadratic" pos="0 0 .2"/>
</body>
<body name="cubic" pos=".5 0 1.5">
<freejoint/>
<geom class="box" rgba=".2 .8 .2 1"/>
<site name="hook_cubic" pos="0 0 .2"/>
</body>
<body name="combi" pos="1.5 0 1.5">
<freejoint/>
<geom class="box" rgba=".8 .2 .8 1"/>
<site name="hook_combi" pos="0 0 .2"/>
</body>
</worldbody>
<tendon>
<spatial name="linear" stiffness="100">
<site site="anchor_linear"/>
<site site="hook_linear"/>
</spatial>
<spatial name="quadratic" stiffness="0 200">
<site site="anchor_quadratic"/>
<site site="hook_quadratic"/>
</spatial>
<spatial name="cubic" stiffness="0 0 300">
<site site="anchor_cubic"/>
<site site="hook_cubic"/>
</spatial>
<spatial name="combination" stiffness="100 -400 400">
<site site="anchor_combi"/>
<site site="hook_combi"/>
</spatial>
</tendon>
</mujoco>