Alessio
55b43414dd
The implicit effective metric assembles the stretch stiffness from
world-space edge vectors, but a flex vertex body's slide dofs are
expressed in its parent body's frame. When that frame is rotated the
assembled operator is therefore not the Jacobian of the passive stretch
force, which mj_flexPassiveStretch already maps into the dof frame with
xmat^T. The metric is then inconsistent with the force it linearizes:
implicit integration loses its stability guarantee, and models that the
same flex handles comfortably in an unrotated frame diverge.
Apply the matching change of basis in both places that build or apply the
stretch stiffness: mjd_flexStretch_mul rotates the input dof vector into
world and the scattered result back, and mjd_flexStiff_assemble sandwiches
each 3x3 block as R_bi^T * blk * R_bj. Both are no-ops when the parent is
unrotated. Bending needs no change: its blocks are isotropic, and
R^T (q I) R = q I.
This completes the fix in fe9dc584, which covered the passive force paths
and the interp (trilinear) derivative but not the standard stretch one.
On a mesh flex inside a body with a 90-degree rotation, the metric's
directional agreement with the force Jacobian goes from cos = 0.57 to
cos = 1.0, and a hanging sheet that previously reached 176% strain
settles at 0.87%.
1187 lines
35 KiB
C++
1187 lines
35 KiB
C++
// Copyright 2021 DeepMind Technologies Limited
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
|
|
// Tests for engine/engine_core_smooth.c.
|
|
|
|
#include <cmath>
|
|
#include <limits>
|
|
#include <string>
|
|
|
|
#include <gmock/gmock.h>
|
|
#include <gtest/gtest.h>
|
|
#include <mujoco/mjmodel.h>
|
|
#include <mujoco/mujoco.h>
|
|
#include "test/fixture.h"
|
|
|
|
namespace mujoco {
|
|
namespace {
|
|
|
|
using ::testing::ElementsAre;
|
|
using ::testing::NotNull;
|
|
|
|
using PassiveTest = MujocoTest;
|
|
|
|
TEST_F(PassiveTest, DisableFlags) {
|
|
static constexpr char flex_xml[] = R"(
|
|
<mujoco>
|
|
<option gravity="0 0 -10"/>
|
|
<worldbody>
|
|
<body gravcomp="1">
|
|
<joint type="slide" springref="1" stiffness="10" damping="1"/>
|
|
<geom size="1" mass="1"/>
|
|
</body>
|
|
</worldbody>
|
|
|
|
<keyframe>
|
|
<key qvel="-1"/>
|
|
</keyframe>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024];
|
|
MjModelPtr m = LoadModelFromString(flex_xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
|
|
|
mj_forward(m.get(), d.get());
|
|
EXPECT_MJTNUM_EQ(d->qacc[0], 11);
|
|
|
|
m->opt.disableflags = mjDSBL_DAMPER;
|
|
mj_forward(m.get(), d.get());
|
|
EXPECT_MJTNUM_EQ(d->qacc[0], 10);
|
|
|
|
m->opt.disableflags = mjDSBL_SPRING;
|
|
mj_forward(m.get(), d.get());
|
|
EXPECT_MJTNUM_EQ(d->qacc[0], 1);
|
|
|
|
m->opt.disableflags = mjDSBL_SPRING | mjDSBL_DAMPER;
|
|
mj_forward(m.get(), d.get());
|
|
EXPECT_EQ(d->qacc[0], -10);
|
|
}
|
|
|
|
TEST_F(PassiveTest, GravcompNestedBody) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<option gravity="0 0 -10"/>
|
|
<worldbody>
|
|
<body pos="0 0 2">
|
|
<freejoint/>
|
|
<body gravcomp="1.2">
|
|
<geom size="0.2" mass="1"/>
|
|
</body>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024];
|
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
|
|
mj_forward(m.get(), d.get());
|
|
|
|
EXPECT_GT(d->qacc[2], 0);
|
|
EXPECT_NEAR(d->qacc[2], 2.0, 0.1);
|
|
}
|
|
|
|
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];
|
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
|
|
|
mj_forward(m.get(), d.get());
|
|
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48);
|
|
}
|
|
|
|
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];
|
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
|
|
|
mj_forward(m.get(), d.get());
|
|
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], 8);
|
|
}
|
|
|
|
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];
|
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
|
|
|
m->tendon_stiffness[0] = 10;
|
|
m->tendon_stiffnesspoly[0] = 5;
|
|
m->tendon_stiffnesspoly[1] = 1;
|
|
|
|
MjDataPtr d = MakeData(m);
|
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
|
|
|
mj_forward(m.get(), d.get());
|
|
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48);
|
|
}
|
|
|
|
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];
|
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
|
|
|
mj_forward(m.get(), d.get());
|
|
mjtNum total_energy = d->energy[0] + d->energy[1];
|
|
|
|
for (int i = 0; i < 100; i++) {
|
|
mj_step(m.get(), d.get());
|
|
EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.002);
|
|
}
|
|
}
|
|
|
|
// ------------------------ ellipsoid fluid model ------------------------------
|
|
|
|
using EllipsoidFluidTest = MujocoTest;
|
|
|
|
TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
|
|
static constexpr char two_bodies_xml[] = R"(
|
|
<mujoco>
|
|
<option wind="5 5 0" density="10"/>
|
|
<worldbody>
|
|
<body>
|
|
<freejoint/>
|
|
<body>
|
|
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" euler="40 0 0" fluidshape="ellipsoid"/>
|
|
</body>
|
|
<body>
|
|
<geom type="box" size=".1 .01 0.01" pos="-.1 0 0" euler="0 20 0" fluidshape="ellipsoid"/>
|
|
</body>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024];
|
|
MjModelPtr m2 = LoadModelFromString(two_bodies_xml, error, sizeof(error));
|
|
ASSERT_THAT(m2.get(), NotNull()) << error;
|
|
MjDataPtr d2 = MakeData(m2);
|
|
for (int i = 0; i < 6; i++) {
|
|
d2->qvel[i] = (mjtNum)i + 1;
|
|
}
|
|
d2->qpos[3] = 0.5;
|
|
d2->qpos[4] = 0.5;
|
|
d2->qpos[5] = 0.5;
|
|
d2->qpos[6] = 0.5;
|
|
|
|
static constexpr char one_body_xml[] = R"(
|
|
<mujoco>
|
|
<option wind="5 5 0" density="10"/>
|
|
<worldbody>
|
|
<body pos="1 2 3">
|
|
<freejoint align="false"/>
|
|
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" euler="40 0 0" fluidshape="ellipsoid"/>
|
|
<geom type="box" size=".1 .01 0.01" pos="-.1 0 0" euler="0 20 0" fluidshape="ellipsoid"/>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
MjModelPtr m1 = LoadModelFromString(one_body_xml, error, sizeof(error));
|
|
ASSERT_THAT(m1.get(), NotNull()) << error;
|
|
MjDataPtr d1 = MakeData(m1);
|
|
for (int i = 0; i < 6; i++) {
|
|
d1->qvel[i] = (mjtNum)i + 1;
|
|
}
|
|
d1->qpos[3] = 0.5;
|
|
d1->qpos[4] = 0.5;
|
|
d1->qpos[5] = 0.5;
|
|
d1->qpos[6] = 0.5;
|
|
|
|
// tolerance for floating point numbers
|
|
const mjtNum tol = MjTol(1e-14, 1e-5);
|
|
|
|
EXPECT_EQ(m1->nv, m2->nv);
|
|
|
|
mj_forward(m2.get(), d2.get());
|
|
mj_forward(m1.get(), d1.get());
|
|
for (int i = 0; i < m1->nv; i++) {
|
|
EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol);
|
|
}
|
|
}
|
|
|
|
TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<option wind="5 5 0" density="10"/>
|
|
<default>
|
|
<geom fluidshape="ellipsoid" fluidcoef="2 3 4 5 6"/>
|
|
<default class="test_class">
|
|
<geom fluidshape="none" fluidcoef="5 4 3 2 1"/>
|
|
</default>
|
|
</default>
|
|
<worldbody>
|
|
<body>
|
|
<freejoint/>
|
|
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" class="test_class"/>
|
|
<geom type="box" size=".1 .01 0.01" pos="-0.1 0 0"/>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024];
|
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
|
EXPECT_THAT(AsVector(model->geom_fluid, 6), ElementsAre(0, 0, 0, 0, 0, 0));
|
|
EXPECT_THAT(AsVector(model->geom_fluid + mjNFLUID, 6),
|
|
ElementsAre(1, 2, 3, 4, 5, 6));
|
|
}
|
|
|
|
// ------------------------------ tendons --------------------------------------
|
|
|
|
using TendonTest = MujocoTest;
|
|
|
|
// check tendon spring deadband using example model
|
|
TEST_F(TendonTest, SpringrangeDeadband) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("engine/testdata/tendon_springlength.xml");
|
|
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
|
ASSERT_THAT(model, NotNull());
|
|
mjData* data = mj_makeData(model);
|
|
|
|
// initial state outside deadband: spring is active
|
|
mj_forward(model, data);
|
|
mjtNum expected_force = model->tendon_stiffness[0] *
|
|
(model->tendon_lengthspring[1] - data->ten_length[0]);
|
|
EXPECT_EQ(expected_force, data->qfrc_passive[0]);
|
|
|
|
// put body inside deadband: spring is inactive
|
|
data->qpos[0] = -1;
|
|
mj_forward(model, data);
|
|
EXPECT_EQ(0, data->qfrc_passive[0]);
|
|
|
|
mj_deleteData(data);
|
|
mj_deleteModel(model);
|
|
}
|
|
|
|
// -------------------------------- flex ------------------------------------
|
|
|
|
using ElasticityTest = MujocoTest;
|
|
|
|
TEST_F(ElasticityTest, FlexCompatibility) {
|
|
static constexpr char flex_xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="parent">
|
|
<flexcomp name="soft" type="grid" count="3 3 3"
|
|
radius="0.01" dim="3"mass="1">
|
|
<pin id="2"/>
|
|
<elasticity young="5e4" poisson="0.2"/>
|
|
</flexcomp>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024] = {0};
|
|
MjModelPtr m = LoadModelFromString(flex_xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
|
|
|
MjDataPtr d = MakeData(m);
|
|
}
|
|
|
|
// -------------------------------- shell -----------------------------------
|
|
TEST_F(ElasticityTest, ElasticEnergyShell) {
|
|
static constexpr char cantilever_xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp type="grid" count="8 8 1" spacing="1 1 1"
|
|
radius=".025" name="test" dim="2">
|
|
<elasticity young="2" poisson="0" thickness="1"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024] = {0};
|
|
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
mj_kinematics(m.get(), d.get());
|
|
mj_flex(m.get(), d.get());
|
|
|
|
// check that a plane is in the kernel of the energy
|
|
for (mjtNum scale = 1; scale < 4; scale++) {
|
|
for (int e = 0; e < m->flex_edgenum[0]; e++) {
|
|
int* edge = m->flex_edge + 2 * (m->flex_edgeadr[0] + e);
|
|
int* flap = m->flex_edgeflap + 2 * (m->flex_edgeadr[0] + e);
|
|
int v[4] = {edge[0], edge[1], flap[0], flap[1]};
|
|
if (v[3] == -1) {
|
|
continue;
|
|
}
|
|
mjtNum energy = 0;
|
|
mjtNum volume = 1. / 2.;
|
|
for (int i = 0; i < 4; i++) {
|
|
for (int j = 0; j < 4; j++) {
|
|
for (int x = 0; x < 3; x++) {
|
|
mjtNum elongation1 = scale * d->flexvert_xpos[3 * v[i] + x];
|
|
mjtNum elongation2 = scale * d->flexvert_xpos[3 * v[j] + x];
|
|
energy +=
|
|
m->flex_bending[17 * e + 4 * i + j] * elongation1 * elongation2;
|
|
}
|
|
}
|
|
}
|
|
EXPECT_NEAR(4 * energy / volume, 0,
|
|
std::numeric_limits<float>::epsilon());
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST_F(ElasticityTest, CurvedShell) {
|
|
static constexpr char cantilever_xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="v0" pos="-0.5 -0.5 -0.5">
|
|
<inertial pos="0 0 0" mass="0.125" diaginertia="1e-4 1e-4 1e-4"/>
|
|
<joint axis="1 0 0" type="slide"/>
|
|
<joint axis="0 1 0" type="slide"/>
|
|
<joint axis="0 0 1" type="slide"/>
|
|
</body>
|
|
<body name="v1" pos="-0.5 0.5 -0.5">
|
|
<inertial pos="0 0 0" mass="0.125" diaginertia="1e-4 1e-4 1e-4"/>
|
|
<joint axis="1 0 0" type="slide"/>
|
|
<joint axis="0 1 0" type="slide"/>
|
|
<joint axis="0 0 1" type="slide"/>
|
|
</body>
|
|
<body name="v2" pos="0.5 -0.5 -0.5">
|
|
<inertial pos="0 0 0" mass="0.125" diaginertia="1e-4 1e-4 1e-4"/>
|
|
<joint axis="1 0 0" type="slide"/>
|
|
<joint axis="0 1 0" type="slide"/>
|
|
<joint axis="0 0 1" type="slide"/>
|
|
</body>
|
|
<body name="v3" pos="-0.5 -0.5 0.5">
|
|
<inertial pos="0 0 0" mass="0.125" diaginertia="1e-4 1e-4 1e-4"/>
|
|
<joint axis="1 0 0" type="slide"/>
|
|
<joint axis="0 1 0" type="slide"/>
|
|
<joint axis="0 0 1" type="slide"/>
|
|
</body>
|
|
</worldbody>
|
|
<deformable>
|
|
<flex name="test" radius="0.025" flatskin="true" body="v0 v1 v2 v3"
|
|
element="0 2 3 0 3 1">
|
|
<elasticity young="2" thickness="1" elastic2d="bend"/>
|
|
</flex>
|
|
</deformable>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024] = {0};
|
|
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
mj_kinematics(m.get(), d.get());
|
|
mj_flex(m.get(), d.get());
|
|
mj_passive(m.get(), d.get());
|
|
|
|
// v1 force component is in-plane along v1-v0 edge (y-axis)
|
|
EXPECT_NEAR(d->qfrc_spring[3], 0, 1e-6);
|
|
EXPECT_NEAR(d->qfrc_spring[5], 0, 1e-6);
|
|
|
|
// v2 force component is in-plane along v2-v0 edge (x-axis)
|
|
EXPECT_NEAR(d->qfrc_spring[7], 0, 1e-6);
|
|
EXPECT_NEAR(d->qfrc_spring[8], 0, 1e-6);
|
|
|
|
// v3 force component is in-plane along v3-v0 edge (z-axis)
|
|
EXPECT_NEAR(d->qfrc_spring[9], 0, 1e-6);
|
|
EXPECT_NEAR(d->qfrc_spring[10], 0, 1e-6);
|
|
}
|
|
|
|
// -------------------------------- membrane -----------------------------------
|
|
TEST_F(ElasticityTest, ElasticEnergyMembrane) {
|
|
static constexpr char cantilever_xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp type="grid" count="8 8 1" spacing="1 1 1"
|
|
radius=".025" name="test" dim="2">
|
|
<elasticity young="2" poisson="0" thickness="1" elastic2d="stretch"/>
|
|
<edge equality="false"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024] = {0};
|
|
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
|
|
mj_kinematics(m.get(), d.get());
|
|
mj_flex(m.get(), d.get());
|
|
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0];
|
|
|
|
// check that if the entire geometry is rescaled by a factor "scale", then
|
|
// trace(strain^2) = 2*scale^2
|
|
|
|
for (mjtNum scale = 1; scale < 4; scale++) {
|
|
for (int t = 0; t < m->flex_elemnum[0]; t++) {
|
|
mjtNum energy = 0;
|
|
mjtNum volume = 1. / 2.;
|
|
int idx = 0;
|
|
for (int e1 = 0; e1 < 3; e1++) {
|
|
for (int e2 = e1; e2 < 3; e2++) {
|
|
int idx1 = m->flex_elemedge[3 * t + e1 + m->flex_elemedgeadr[0]];
|
|
int idx2 = m->flex_elemedge[3 * t + e2 + m->flex_elemedgeadr[0]];
|
|
mjtNum elong1 =
|
|
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
|
|
mjtNum elong2 =
|
|
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2];
|
|
energy +=
|
|
metric[21 * t + idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
|
|
}
|
|
}
|
|
const mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-5);
|
|
EXPECT_NEAR(4 * energy / volume, 2 * scale * scale, tol);
|
|
}
|
|
}
|
|
}
|
|
|
|
// -------------------------------- solid -----------------------------------
|
|
TEST_F(ElasticityTest, ElasticEnergySolid) {
|
|
static constexpr char cantilever_xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp type="grid" count="8 8 8" spacing="1 1 1"
|
|
radius=".025" name="test" dim="3">
|
|
<elasticity young="2" poisson="0"/>
|
|
<edge equality="false"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024] = {0};
|
|
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
|
|
mj_kinematics(m.get(), d.get());
|
|
mj_flex(m.get(), d.get());
|
|
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0];
|
|
|
|
// check that if the entire geometry is rescaled by a factor "scale", then
|
|
// trace(strain^2) = 3*scale^2
|
|
|
|
for (mjtNum scale = 1; scale < 4; scale++) {
|
|
for (int t = 0; t < m->flex_elemnum[0]; t++) {
|
|
mjtNum energy = 0;
|
|
mjtNum volume = 1. / 6.;
|
|
int idx = 0;
|
|
for (int e1 = 0; e1 < 6; e1++) {
|
|
for (int e2 = e1; e2 < 6; e2++) {
|
|
int idx1 = m->flex_elemedge[6 * t + e1 + m->flex_elemedgeadr[0]];
|
|
int idx2 = m->flex_elemedge[6 * t + e2 + m->flex_elemedgeadr[0]];
|
|
mjtNum elong1 =
|
|
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
|
|
mjtNum elong2 =
|
|
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2];
|
|
energy +=
|
|
metric[21 * t + idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
|
|
}
|
|
}
|
|
const mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-4);
|
|
EXPECT_NEAR(energy / volume, 3 * scale * scale, tol);
|
|
}
|
|
}
|
|
}
|
|
|
|
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>
|
|
)";
|
|
MjModelPtr m = LoadModelFromString(xml);
|
|
ASSERT_THAT(m.get(), NotNull());
|
|
MjDataPtr d = MakeData(m);
|
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
|
mj_forward(m.get(), d.get());
|
|
|
|
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);
|
|
}
|
|
|
|
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>
|
|
)";
|
|
MjModelPtr m = LoadModelFromString(xml);
|
|
ASSERT_THAT(m.get(), NotNull());
|
|
MjDataPtr d = MakeData(m);
|
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
|
mj_forward(m.get(), d.get());
|
|
|
|
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);
|
|
}
|
|
|
|
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];
|
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
|
|
|
mj_forward(m.get(), d.get());
|
|
|
|
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);
|
|
}
|
|
|
|
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];
|
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
|
|
|
mj_forward(m.get(), d.get());
|
|
|
|
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);
|
|
}
|
|
|
|
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>
|
|
)";
|
|
MjModelPtr m = LoadModelFromString(xml);
|
|
ASSERT_THAT(m.get(), NotNull());
|
|
MjDataPtr d = MakeData(m);
|
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
|
mj_forward(m.get(), d.get());
|
|
|
|
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);
|
|
}
|
|
|
|
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>
|
|
)";
|
|
MjModelPtr m = LoadModelFromString(xml);
|
|
ASSERT_THAT(m.get(), NotNull());
|
|
MjDataPtr d = MakeData(m);
|
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
|
mj_forward(m.get(), d.get());
|
|
|
|
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);
|
|
}
|
|
|
|
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];
|
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
|
|
|
mj_forward(m.get(), d.get());
|
|
|
|
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);
|
|
}
|
|
|
|
// shell-mode (elastic2d=stretch) flexcomp must have zero passive spring forces
|
|
// at rest (initial configuration); any nonzero force indicates a rotation
|
|
// mismatch between compile-time reference positions and runtime corotation.
|
|
TEST_F(ElasticityTest, ShellModeZeroForceAtRest) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<option gravity="0 0 0"/>
|
|
<worldbody>
|
|
<flexcomp type="grid" count="8 8 8" spacing=".07 .07 .07" pos="0 0 1"
|
|
dim="3" cellcount="1 1 1" radius=".001" rgba="0 .7 .7 1"
|
|
mass="5" name="softbody" dof="trilinear">
|
|
<elasticity young="1e4" poisson="0.1" damping="0.01"
|
|
elastic2d="stretch" thickness="0.02"/>
|
|
<contact selfcollide="none" internal="false"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024] = {0};
|
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
|
|
mj_forward(m.get(), d.get());
|
|
|
|
// all spring forces should be zero at rest
|
|
for (int i = 0; i < m->nv; i++) {
|
|
EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10)
|
|
<< "nonzero spring force at DOF " << i;
|
|
}
|
|
}
|
|
|
|
// interpolated shell bending must produce zero spring forces at rest
|
|
TEST_F(ElasticityTest, InterpBendingZeroForceAtRest) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<option gravity="0 0 0"/>
|
|
<worldbody>
|
|
<flexcomp type="grid" count="8 8 8" spacing=".07 .07 .07" pos="0 0 1"
|
|
dim="3" cellcount="2 2 1" radius=".001" rgba="0 .7 .7 1"
|
|
mass="5" name="softbody" dof="trilinear">
|
|
<elasticity young="1e4" poisson="0.1" damping="0"
|
|
elastic2d="bend" thickness="0.02"/>
|
|
<contact selfcollide="none" internal="false"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024] = {0};
|
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
|
|
// verify bending data was compiled
|
|
const mjtNum* bdata = m->flex_bending + m->flex_bendingadr[0];
|
|
int nedge = (int)bdata[0];
|
|
EXPECT_GT(nedge, 0) << "no bending edges compiled";
|
|
|
|
mj_forward(m.get(), d.get());
|
|
|
|
// all spring forces should be zero at rest
|
|
for (int i = 0; i < m->nv; i++) {
|
|
EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10)
|
|
<< "nonzero spring force at DOF " << i;
|
|
}
|
|
|
|
// verify per-edge bending data
|
|
int n_flat = 0, n_corner = 0;
|
|
for (int e = 0; e < nedge; e++) {
|
|
const mjtNum* edata = bdata + 1 + e * 10;
|
|
mjtNum stiffness = edata[6];
|
|
mjtNum dn0[3] = {edata[7], edata[8], edata[9]};
|
|
mjtNum dn0_norm = mju_norm3(dn0);
|
|
|
|
// stiffness must be positive
|
|
EXPECT_GT(stiffness, 0) << "edge " << e << " has non-positive stiffness";
|
|
|
|
if (dn0_norm < 1e-10) {
|
|
// intra-surface edge: coplanar faces, zero normal jump
|
|
n_flat++;
|
|
} else {
|
|
// corner edge: 90° between perpendicular face normals, |dn0| = sqrt(2)
|
|
n_corner++;
|
|
EXPECT_NEAR(dn0_norm, mju_sqrt(2.0), 1e-10)
|
|
<< "corner edge " << e << " has unexpected |dn0|=" << dn0_norm;
|
|
}
|
|
}
|
|
|
|
// for a 2x2x1 box: 12 intra-surface + 20 corner = 32 edges
|
|
EXPECT_GT(n_flat, 0) << "no intra-surface edges found";
|
|
EXPECT_GT(n_corner, 0) << "no corner edges found";
|
|
EXPECT_EQ(n_flat + n_corner, nedge);
|
|
}
|
|
|
|
// interpolated shell bending must produce zero forces after a rigid rotation
|
|
TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<option gravity="0 0 0"/>
|
|
<worldbody>
|
|
<flexcomp type="grid" count="8 2 12" spacing=".025 .05 .025" pos="0 0 1"
|
|
dim="3" cellcount="6 1 6" radius=".001" rgba="0 .7 .7 1"
|
|
mass="5" name="softbody" dof="trilinear">
|
|
<elasticity young="1e5" poisson="0.3" damping="0"
|
|
elastic2d="bend" thickness="0.03"/>
|
|
<contact selfcollide="none" internal="false"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024] = {0};
|
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
|
|
// compute geometric center from body positions (skip world body)
|
|
mjtNum center[3] = {0, 0, 0};
|
|
int nnodes = 0;
|
|
for (int b = 1; b < m->nbody; b++) {
|
|
center[0] += m->body_pos[3 * b + 0];
|
|
center[1] += m->body_pos[3 * b + 1];
|
|
center[2] += m->body_pos[3 * b + 2];
|
|
nnodes++;
|
|
}
|
|
ASSERT_GT(nnodes, 0);
|
|
center[0] /= nnodes;
|
|
center[1] /= nnodes;
|
|
center[2] /= nnodes;
|
|
|
|
// rotation: 45 degrees about (1,1,1)/sqrt(3)
|
|
mjtNum angle = 45 * 3.14159265358979 / 180.0;
|
|
mjtNum sa = mju_sin(angle / 2), ca = mju_cos(angle / 2);
|
|
mjtNum inv_sqrt3 = 1.0 / mju_sqrt(3.0);
|
|
mjtNum quat[4] = {ca, sa * inv_sqrt3, sa * inv_sqrt3, sa * inv_sqrt3};
|
|
mjtNum neg_quat[4];
|
|
mju_negQuat(neg_quat, quat);
|
|
|
|
// apply rigid rotation via slide joint displacements:
|
|
// new_pos = center + R * (body_pos - center)
|
|
// qpos = new_pos - body_pos
|
|
for (int b = 1; b < m->nbody; b++) {
|
|
mjtNum rel[3] = {m->body_pos[3 * b + 0] - center[0],
|
|
m->body_pos[3 * b + 1] - center[1],
|
|
m->body_pos[3 * b + 2] - center[2]};
|
|
mjtNum rotated[3];
|
|
mju_rotVecQuat(rotated, rel, neg_quat);
|
|
|
|
// each body has 3 slide joints (x, y, z)
|
|
for (int j = 0; j < m->body_jntnum[b] && j < 3; j++) {
|
|
int jid = m->body_jntadr[b] + j;
|
|
int qadr = m->jnt_qposadr[jid];
|
|
int axis = -1;
|
|
for (int a = 0; a < 3; a++) {
|
|
if (m->jnt_axis[3 * jid + a] != 0) {
|
|
axis = a;
|
|
break;
|
|
}
|
|
}
|
|
if (axis >= 0) {
|
|
d->qpos[qadr] =
|
|
(center[axis] + rotated[axis]) - m->body_pos[3 * b + axis];
|
|
}
|
|
}
|
|
}
|
|
|
|
mj_forward(m.get(), d.get());
|
|
|
|
// spring forces should still be zero after rigid rotation
|
|
const mjtNum tol = MjTol(1e-6, 1e-3);
|
|
for (int i = 0; i < m->nv; i++) {
|
|
EXPECT_NEAR(d->qfrc_spring[i], 0, tol)
|
|
<< "nonzero spring force at DOF " << i << " after rigid rotation";
|
|
}
|
|
}
|
|
|
|
|
|
// verify that a pinned vertex (on a static body) gets zero bending force
|
|
// while its free neighbors get nonzero bending force
|
|
TEST_F(ElasticityTest, PinnedVertexBendingForce) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="parent">
|
|
<flexcomp name="test" type="grid" count="3 3 1" spacing="1 1 1"
|
|
radius="0.01" dim="2">
|
|
<elasticity young="1" poisson="0" thickness="1" elastic2d="bend"/>
|
|
<pin id="0"/>
|
|
</flexcomp>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024] = {0};
|
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
|
MjDataPtr d = MakeData(m);
|
|
|
|
// displace a free vertex out of plane to create bending
|
|
d->qpos[3] = 0.1;
|
|
mj_forward(m.get(), d.get());
|
|
|
|
// vertex 0 is pinned to the world body (body 0), which has 0 dofs,
|
|
// so no bending force is written for it — the reaction is absorbed by the pin
|
|
|
|
// verify that at least some free vertices have nonzero spring force
|
|
bool has_nonzero = false;
|
|
for (int i = 0; i < m->nv; i++) {
|
|
if (d->qfrc_spring[i] != 0) {
|
|
has_nonzero = true;
|
|
break;
|
|
}
|
|
}
|
|
EXPECT_TRUE(has_nonzero) << "bending should produce nonzero spring forces";
|
|
}
|
|
|
|
// verify that a flexcomp with dof="trilinear" inside a parent body
|
|
// with non-identity quaternion produces:
|
|
// 1. restoring forces (not expanding) for small perturbations
|
|
// 2. identical qfrc_passive between rotated and non-rotated models
|
|
// 3. stable simulation with implicit integration (no NaN, bounded qacc)
|
|
TEST_F(ElasticityTest, TrilinearParentBodyRotation) {
|
|
static constexpr char rotated_xml[] = R"(
|
|
<mujoco>
|
|
<option gravity="0 0 0" integrator="implicitfast"
|
|
timestep="0.0005" solver="CG"/>
|
|
<worldbody>
|
|
<body name="base" pos="0 0 0" quat="0.7071 0 0.7071 0">
|
|
<flexcomp type="grid" count="3 3 3" spacing=".05 .05 .05"
|
|
dim="3" radius=".001" mass=".005" name="soft" dof="trilinear">
|
|
<elasticity young="1e4" poisson="0.1" damping="0.1"/>
|
|
<contact selfcollide="none" internal="false"/>
|
|
</flexcomp>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
static constexpr char nonrotated_xml[] = R"(
|
|
<mujoco>
|
|
<option gravity="0 0 0" integrator="implicitfast"
|
|
timestep="0.0005" solver="CG"/>
|
|
<worldbody>
|
|
<body name="base" pos="0 0 0">
|
|
<flexcomp type="grid" count="3 3 3" spacing=".05 .05 .05"
|
|
dim="3" radius=".001" mass=".005" name="soft" dof="trilinear">
|
|
<elasticity young="1e4" poisson="0.1" damping="0.1"/>
|
|
<contact selfcollide="none" internal="false"/>
|
|
</flexcomp>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024] = {0};
|
|
|
|
// --- test 1: force sign and rotational invariance ---
|
|
MjModelPtr m_rot = LoadModelFromString(rotated_xml, error, sizeof(error));
|
|
ASSERT_THAT(m_rot.get(), NotNull()) << error;
|
|
MjDataPtr d_rot = MakeData(m_rot);
|
|
|
|
MjModelPtr m_non = LoadModelFromString(nonrotated_xml, error, sizeof(error));
|
|
ASSERT_THAT(m_non.get(), NotNull()) << error;
|
|
MjDataPtr d_non = MakeData(m_non);
|
|
|
|
// apply small +X perturbation to DOF 0 (node 0, X slide)
|
|
d_rot->qpos[0] = 1e-6;
|
|
d_non->qpos[0] = 1e-6;
|
|
|
|
mj_forward(m_rot.get(), d_rot.get());
|
|
mj_forward(m_non.get(), d_non.get());
|
|
|
|
// check force sign on the displaced DOF: positive qpos -> negative qfrc (restoring)
|
|
EXPECT_LT(d_rot->qfrc_passive[0], 0)
|
|
<< "rotated model: expected restoring force on displaced DOF 0";
|
|
EXPECT_LT(d_non->qfrc_passive[0], 0)
|
|
<< "non-rotated model: expected restoring force on displaced DOF 0";
|
|
|
|
// check rotational invariance: forces match within numerical precision
|
|
// (tol ~ 1e-13 due to floating-point differences in polar decomposition path)
|
|
const mjtNum tol = MjTol(1e-12, 1e-5);
|
|
for (int i = 0; i < m_rot->nv; i++) {
|
|
EXPECT_NEAR(d_rot->qfrc_passive[i], d_non->qfrc_passive[i], tol)
|
|
<< "rotated/non-rotated qfrc mismatch at DOF " << i;
|
|
}
|
|
|
|
// --- test 2: implicit integration stability (200 steps, no NaN) ---
|
|
mjtNum max_qacc = 0;
|
|
for (int step = 0; step < 200; step++) {
|
|
mj_step(m_rot.get(), d_rot.get());
|
|
|
|
for (int i = 0; i < m_rot->nv; i++) {
|
|
ASSERT_TRUE(std::isfinite(d_rot->qacc[i]))
|
|
<< "NaN/Inf in qacc at DOF " << i << " at step " << step;
|
|
mjtNum a = mju_abs(d_rot->qacc[i]);
|
|
if (a > max_qacc) max_qacc = a;
|
|
}
|
|
if (HasFatalFailure()) return;
|
|
}
|
|
|
|
EXPECT_LT(max_qacc, 1e4);
|
|
}
|
|
|
|
|
|
// A dim=2 flexcomp with stretch elasticity inside a parent body with a
|
|
// non-identity quaternion. The implicit metric assembles the stretch
|
|
// stiffness from world-space edge vectors, but the vertex bodies' slide
|
|
// dofs live in the (rotated) parent frame. Without the R^T (.) R change
|
|
// of basis the metric stops being the Jacobian of the passive force,
|
|
// which shows up as a loss of rotational invariance and, at stiffnesses
|
|
// the unrotated model handles comfortably, as divergence.
|
|
TEST_F(ElasticityTest, StretchParentBodyRotation) {
|
|
static constexpr char rotated_xml[] = R"(
|
|
<mujoco>
|
|
<option gravity="0 0 0" integrator="implicitfast"
|
|
timestep="0.001" solver="CG"/>
|
|
<worldbody>
|
|
<body name="base" pos="0 0 0" quat="0.7071 0.7071 0 0">
|
|
<flexcomp type="grid" count="5 5 1" spacing=".05 .05 .05"
|
|
dim="2" radius=".001" mass=".01" name="sheet">
|
|
<elasticity young="1e5" poisson="0" thickness="1e-3"
|
|
elastic2d="stretch"/>
|
|
<contact selfcollide="none" internal="false"/>
|
|
</flexcomp>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
static constexpr char nonrotated_xml[] = R"(
|
|
<mujoco>
|
|
<option gravity="0 0 0" integrator="implicitfast"
|
|
timestep="0.001" solver="CG"/>
|
|
<worldbody>
|
|
<body name="base" pos="0 0 0">
|
|
<flexcomp type="grid" count="5 5 1" spacing=".05 .05 .05"
|
|
dim="2" radius=".001" mass=".01" name="sheet">
|
|
<elasticity young="1e5" poisson="0" thickness="1e-3"
|
|
elastic2d="stretch"/>
|
|
<contact selfcollide="none" internal="false"/>
|
|
</flexcomp>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024] = {0};
|
|
|
|
MjModelPtr m_rot = LoadModelFromString(rotated_xml, error, sizeof(error));
|
|
ASSERT_THAT(m_rot.get(), NotNull()) << error;
|
|
MjDataPtr d_rot = MakeData(m_rot);
|
|
|
|
MjModelPtr m_non = LoadModelFromString(nonrotated_xml, error, sizeof(error));
|
|
ASSERT_THAT(m_non.get(), NotNull()) << error;
|
|
MjDataPtr d_non = MakeData(m_non);
|
|
|
|
// stretch one dof; the response is expressed in the parent frame in
|
|
// both models, so the passive force and the implicit acceleration must
|
|
// agree regardless of the parent's orientation
|
|
d_rot->qpos[0] = 1e-4;
|
|
d_non->qpos[0] = 1e-4;
|
|
|
|
mj_forward(m_rot.get(), d_rot.get());
|
|
mj_forward(m_non.get(), d_non.get());
|
|
|
|
EXPECT_LT(d_rot->qfrc_passive[0], 0)
|
|
<< "expected a restoring force on the stretched dof";
|
|
|
|
const mjtNum tol = MjTol(1e-12, 1e-5);
|
|
for (int i = 0; i < m_rot->nv; i++) {
|
|
EXPECT_NEAR(d_rot->qfrc_passive[i], d_non->qfrc_passive[i], tol)
|
|
<< "rotated/non-rotated qfrc mismatch at dof " << i;
|
|
}
|
|
|
|
// qacc exercises the metric itself (the force is only its right-hand
|
|
// side): a metric in the wrong basis breaks this invariance even though
|
|
// the force above is already correct
|
|
for (int i = 0; i < m_rot->nv; i++) {
|
|
EXPECT_NEAR(d_rot->qacc[i], d_non->qacc[i], MjTol(1e-9, 1e-3))
|
|
<< "rotated/non-rotated qacc mismatch at dof " << i;
|
|
}
|
|
|
|
// and the rotated model must integrate stably
|
|
for (int step = 0; step < 200; step++) {
|
|
mj_step(m_rot.get(), d_rot.get());
|
|
for (int i = 0; i < m_rot->nv; i++) {
|
|
ASSERT_TRUE(std::isfinite(d_rot->qacc[i]))
|
|
<< "NaN/Inf in qacc at dof " << i << " at step " << step;
|
|
}
|
|
if (HasFatalFailure()) return;
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
} // namespace mujoco
|