Alessio Quaglino
d933b195ee
This change adds a new passive force computation for flexes with elastic2d="bend" and dof="trilinear". The bending energy is based on the squared difference of normals between adjacent face elements at their shared edge midpoint. The edge data is precomputed during model compilation and stored in flex_bending. PiperOrigin-RevId: 910772638 Change-Id: I3b12c7b7f1ba6ac1875df495d89e8cfec921ca80
1002 lines
27 KiB
C++
1002 lines
27 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 <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];
|
|
mjModel* m = LoadModelFromString(flex_xml, error, sizeof(error));
|
|
ASSERT_THAT(m, testing::NotNull()) << error;
|
|
mjData* d = mj_makeData(m);
|
|
mj_resetDataKeyframe(m, d, 0);
|
|
|
|
mj_forward(m, d);
|
|
EXPECT_MJTNUM_EQ(d->qacc[0], 11);
|
|
|
|
m->opt.disableflags = mjDSBL_DAMPER;
|
|
mj_forward(m, d);
|
|
EXPECT_MJTNUM_EQ(d->qacc[0], 10);
|
|
|
|
m->opt.disableflags = mjDSBL_SPRING;
|
|
mj_forward(m, d);
|
|
EXPECT_MJTNUM_EQ(d->qacc[0], 1);
|
|
|
|
m->opt.disableflags = mjDSBL_SPRING | mjDSBL_DAMPER;
|
|
mj_forward(m, d);
|
|
EXPECT_EQ(d->qacc[0], -10);
|
|
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
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];
|
|
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m, NotNull()) << error;
|
|
mjData* d = mj_makeData(m);
|
|
|
|
mj_forward(m, d);
|
|
|
|
EXPECT_GT(d->qacc[2], 0);
|
|
EXPECT_NEAR(d->qacc[2], 2.0, 0.1);
|
|
|
|
mj_deleteData(d);
|
|
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_MJTNUM_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_MJTNUM_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_MJTNUM_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;
|
|
|
|
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];
|
|
mjModel* m2 = LoadModelFromString(two_bodies_xml, error, sizeof(error));
|
|
ASSERT_THAT(m2, NotNull()) << error;
|
|
mjData* d2 = mj_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>
|
|
)";
|
|
|
|
mjModel* m1 = LoadModelFromString(one_body_xml, error, sizeof(error));
|
|
ASSERT_THAT(m1, NotNull()) << error;
|
|
mjData* d1 = mj_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
|
|
constexpr mjtNum tol = MjTol(1e-14, 1e-5);
|
|
|
|
EXPECT_EQ(m1->nv, m2->nv);
|
|
|
|
mj_forward(m2, d2);
|
|
mj_forward(m1, d1);
|
|
for (int i = 0; i < m1->nv; i++) {
|
|
EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol);
|
|
}
|
|
|
|
mj_deleteData(d1);
|
|
mj_deleteModel(m1);
|
|
mj_deleteData(d2);
|
|
mj_deleteModel(m2);
|
|
}
|
|
|
|
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];
|
|
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(model, 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));
|
|
mj_deleteModel(model);
|
|
}
|
|
|
|
|
|
// ------------------------------ 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};
|
|
mjModel* m = LoadModelFromString(flex_xml, error, sizeof(error));
|
|
ASSERT_THAT(m, testing::NotNull()) << error;
|
|
|
|
mjData* d = mj_makeData(m);
|
|
mj_deleteData(d);
|
|
mj_deleteModel(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};
|
|
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
|
ASSERT_THAT(m, testing::NotNull()) << error;
|
|
mjData* d = mj_makeData(m);
|
|
mj_kinematics(m, d);
|
|
mj_flex(m, d);
|
|
|
|
// 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());
|
|
}
|
|
}
|
|
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
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};
|
|
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
|
ASSERT_THAT(m, testing::NotNull()) << error;
|
|
mjData* d = mj_makeData(m);
|
|
mj_kinematics(m, d);
|
|
mj_flex(m, d);
|
|
mj_passive(m, d);
|
|
|
|
// 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);
|
|
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
// -------------------------------- 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};
|
|
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
|
ASSERT_THAT(m, testing::NotNull()) << error;
|
|
mjData* d = mj_makeData(m);
|
|
|
|
mj_kinematics(m, d);
|
|
mj_flex(m, d);
|
|
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.);
|
|
}
|
|
}
|
|
constexpr mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-5);
|
|
EXPECT_NEAR(4*energy/volume, 2*scale*scale, tol);
|
|
}
|
|
}
|
|
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
// -------------------------------- 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};
|
|
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
|
ASSERT_THAT(m, testing::NotNull()) << error;
|
|
mjData* d = mj_makeData(m);
|
|
|
|
mj_kinematics(m, d);
|
|
mj_flex(m, d);
|
|
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.);
|
|
}
|
|
}
|
|
constexpr mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-4);
|
|
EXPECT_NEAR(energy/volume, 3*scale*scale, tol);
|
|
}
|
|
}
|
|
|
|
mj_deleteData(d);
|
|
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);
|
|
}
|
|
|
|
// 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};
|
|
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m, testing::NotNull()) << error;
|
|
mjData* d = mj_makeData(m);
|
|
|
|
mj_forward(m, d);
|
|
|
|
// 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;
|
|
}
|
|
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
// 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};
|
|
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m, testing::NotNull()) << error;
|
|
mjData* d = mj_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, d);
|
|
|
|
// 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);
|
|
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
// 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 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};
|
|
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(m, testing::NotNull()) << error;
|
|
mjData* d = mj_makeData(m);
|
|
|
|
// apply a rigid rotation by setting all body quats to a 30 degree rotation
|
|
// about z-axis (all flex node bodies get the same rotation)
|
|
mjtNum angle = 30 * 3.14159265358979 / 180.0;
|
|
mjtNum sa = mju_sin(angle / 2), ca = mju_cos(angle / 2);
|
|
for (int b = 1; b < m->nbody; b++) {
|
|
int qadr = m->jnt_qposadr[m->body_jntadr[b]];
|
|
if (m->body_jntnum[b] > 0 && m->jnt_type[m->body_jntadr[b]] == mjJNT_FREE) {
|
|
d->qpos[qadr + 3] = ca;
|
|
d->qpos[qadr + 4] = 0;
|
|
d->qpos[qadr + 5] = 0;
|
|
d->qpos[qadr + 6] = sa;
|
|
}
|
|
}
|
|
|
|
mj_forward(m, d);
|
|
|
|
// spring forces should still be zero (or very small) after rigid rotation
|
|
for (int i = 0; i < m->nv; i++) {
|
|
EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-6)
|
|
<< "nonzero spring force at DOF " << i << " after rigid rotation";
|
|
}
|
|
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
} // namespace
|
|
} // namespace mujoco
|