Kyle Bayes
1490336955
PiperOrigin-RevId: 935980153 Change-Id: I41d25bfab4935494dc984168820cb7cad123cadf
1371 lines
43 KiB
C++
1371 lines
43 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 user/user_model.cc.
|
|
|
|
#include <array>
|
|
#include <memory>
|
|
#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::HasSubstr;
|
|
using ::testing::IsNull;
|
|
using ::testing::NotNull;
|
|
using ::testing::Pointwise;
|
|
using UserFlexTest = MujocoTest;
|
|
|
|
TEST_F(UserFlexTest, ParentMustHaveName) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp/>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
EXPECT_THAT(m.get(), IsNull());
|
|
EXPECT_THAT(error.data(), HasSubstr("required attribute missing: 'name'"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, InvalidDim) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" dim="4"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
EXPECT_THAT(m.get(), IsNull());
|
|
EXPECT_THAT(error.data(), HasSubstr("Invalid dim, must be between 1 and 3"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, CountTooSmall) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" count="2 2 0"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
EXPECT_THAT(m.get(), IsNull());
|
|
EXPECT_THAT(error.data(), HasSubstr("Count too small"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, CellnumZeroInterpolated) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="b0"/>
|
|
<body name="b1"/>
|
|
<body name="b2"/>
|
|
<body name="b3"/>
|
|
</worldbody>
|
|
<deformable>
|
|
<flex name="test" cellcount="2 2 0" dof="trilinear"
|
|
dim="3" body="b0 b1 b2 b3" element="0 1 2 3"/>
|
|
</deformable>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
EXPECT_THAT(m.get(), IsNull());
|
|
EXPECT_THAT(error.data(), HasSubstr("cellcount cannot be 0"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, SpacingGreaterThanGeometry) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" spacing="0.5 0.5 0.5" radius="1"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
|
EXPECT_THAT(error.data(),
|
|
HasSubstr("Spacing must be larger than geometry size"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, ScaleMinValue) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" scale="0 1 1"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
|
EXPECT_THAT(error.data(), HasSubstr("Scale must be larger than mjMINVAL"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, MassMinValue) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" mass="0"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
|
EXPECT_THAT(error.data(),
|
|
HasSubstr("Mass and inertiabox must be larger than mjMINVAL"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, PointSizeNotMultipleOf3) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" point="0 0 0 0"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
|
EXPECT_THAT(error.data(), HasSubstr("Point size must be a multiple of 3"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, ElementSize) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" point="0 0 0" element="0 1 2" dim="3"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
|
EXPECT_THAT(error.data(),
|
|
HasSubstr("Element size must be a multiple of dim+1"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, PointAndElementNotInDirect) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="direct"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
|
EXPECT_THAT(error.data(), HasSubstr("Point and element required"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, UnknownFlexCompType) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="unknown"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
|
EXPECT_THAT(error.data(), HasSubstr("invalid keyword: 'unknown'"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, InvalidPinid) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="direct" point="0 0 0" element="0 1 2">
|
|
<pin id="1"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
|
EXPECT_THAT(error.data(),
|
|
HasSubstr("element 1 has point id 1, number of points is 1"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, MeshFileMissing) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="mesh"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
|
EXPECT_THAT(error.data(), HasSubstr("File is required"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, VertexOrFaceDataMissing) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/malformed_flex_nofaces.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
EXPECT_THAT(m, IsNull()) << error.data();
|
|
EXPECT_THAT(error.data(), HasSubstr("Vertex and face data required"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, CreateBVHSuccess) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/robot_arm.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
mjData* d = mj_makeData(m);
|
|
mj_step(m, d);
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, RigidFlex) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/rigid_flex.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
mjData* d = mj_makeData(m);
|
|
mj_step(m, d);
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, FlexNotCollide) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" pos="1 0 -1" type="grid"
|
|
count="5 5 5" spacing="1 1 1" dim="3">
|
|
<contact contype="0" conaffinity="0"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
|
MjDataPtr d = MakeData(m);
|
|
mj_step(m.get(), d.get());
|
|
}
|
|
|
|
TEST_F(UserFlexTest, BoundingBoxCoordinates) {
|
|
#ifdef mjUSESINGLE
|
|
GTEST_SKIP()
|
|
<< "Float32 rounding in bounding box centering gives ~3e-8 error";
|
|
#endif
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" pos="1 0 -1" type="grid"
|
|
count="5 5 5" spacing="1 1 1" dim="3"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
|
MjDataPtr d = MakeData(m);
|
|
mj_kinematics(m.get(), d.get());
|
|
mj_flex(m.get(), d.get());
|
|
|
|
EXPECT_EQ(m->nflexvert, 5 * 5 * 5);
|
|
EXPECT_EQ(m->nflexelem, 4 * 4 * 4 * 6);
|
|
EXPECT_EQ(m->flex_dim[0], 3);
|
|
|
|
// Cartesian coordinates
|
|
EXPECT_EQ(d->flexvert_xpos[0], -1);
|
|
EXPECT_EQ(d->flexvert_xpos[1], -2);
|
|
EXPECT_EQ(d->flexvert_xpos[2], -3);
|
|
EXPECT_EQ(d->flexvert_xpos[3 * m->nflexvert - 3], 3);
|
|
EXPECT_EQ(d->flexvert_xpos[3 * m->nflexvert - 2], 2);
|
|
EXPECT_EQ(d->flexvert_xpos[3 * m->nflexvert - 1], 1);
|
|
|
|
// bounding box coordinates
|
|
EXPECT_EQ(m->flex_vert0[0], 0);
|
|
EXPECT_EQ(m->flex_vert0[1], 0);
|
|
EXPECT_EQ(m->flex_vert0[2], 0);
|
|
EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 3], 1);
|
|
EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 2], 1);
|
|
EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 1], 1);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) {
|
|
std::array<char, 1024> error;
|
|
static constexpr char xml_selfcoll[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1" dim="3" dof="trilinear">
|
|
<contact selfcollide="auto" internal="false"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
MjModelPtr m1 = LoadModelFromString(xml_selfcoll, error.data(), error.size());
|
|
EXPECT_THAT(m1.get(), IsNull()) << error.data();
|
|
EXPECT_THAT(error.data(),
|
|
HasSubstr("trilinear interpolation cannot do self-collision"));
|
|
static constexpr char xml_internal[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1" dim="3" dof="trilinear">
|
|
<contact selfcollide="none" internal="true"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
MjModelPtr m2 = LoadModelFromString(xml_internal, error.data(), error.size());
|
|
EXPECT_THAT(m2.get(), IsNull()) << error.data();
|
|
EXPECT_THAT(error.data(),
|
|
HasSubstr("trilinear interpolation cannot do internal"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, TrilinearInterpolation) {
|
|
static constexpr char xml_trilinear[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<geom type="plane" pos="0 0 -.5" size="10 10 .1"/>
|
|
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1" dim="3" dof="trilinear">
|
|
<contact selfcollide="none" internal="false"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m1 =
|
|
LoadModelFromString(xml_trilinear, error.data(), error.size());
|
|
ASSERT_THAT(m1.get(), NotNull()) << error.data();
|
|
MjDataPtr d1 = MakeData(m1);
|
|
mj_step(m1.get(), d1.get());
|
|
|
|
static constexpr char xml_linear[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<geom type="plane" pos="0 0 -.5" size="10 10 .1"/>
|
|
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1" dim="3">
|
|
<contact selfcollide="none" internal="false"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
MjModelPtr m2 = LoadModelFromString(xml_linear, error.data(), error.size());
|
|
ASSERT_THAT(m2.get(), NotNull()) << error.data();
|
|
MjDataPtr d2 = MakeData(m2);
|
|
mj_step(m2.get(), d2.get());
|
|
|
|
EXPECT_EQ(m1->nflexvert, m2->nflexvert);
|
|
for (int i = 0; i < 3 * m1->nflexvert; ++i) {
|
|
EXPECT_NEAR(m1->flex_vert[i], d2->flexvert_xpos[i], 1e-7);
|
|
EXPECT_NEAR(m1->flex_vert0[i], m2->flex_vert0[i], 1e-7);
|
|
EXPECT_NEAR(d1->flexvert_xpos[i], d2->flexvert_xpos[i], 1e-7);
|
|
}
|
|
|
|
EXPECT_EQ(m1->nM, m2->nM);
|
|
for (int i = 0; i < m1->nM; ++i) {
|
|
EXPECT_EQ(d1->qM[i], d2->qM[i]);
|
|
}
|
|
|
|
EXPECT_EQ(m1->nbody, m2->nbody);
|
|
for (int i = 0; i < m2->nbody; ++i) {
|
|
if (i == 0) {
|
|
continue;
|
|
}
|
|
EXPECT_EQ(m1->body_mass[i], m2->body_mass[i]);
|
|
for (int j = 0; j < 2; ++j) {
|
|
EXPECT_EQ(m1->body_invweight0[i * 2 + j], m2->body_invweight0[i * 2 + j]);
|
|
}
|
|
for (int j = 0; j < 10; ++j) {
|
|
EXPECT_NEAR(d1->cinert[10 * i + j], d2->cinert[i * 10 + j], 1e-5) << i;
|
|
EXPECT_NEAR(d1->crb[10 * i + j], d2->crb[i * 10 + j], 1e-5) << i;
|
|
}
|
|
}
|
|
|
|
EXPECT_EQ(d1->ncon, 4);
|
|
EXPECT_EQ(d2->ncon, 4);
|
|
for (int i = 0; i < d1->ncon; ++i) {
|
|
EXPECT_EQ(d1->contact[i].dist, d2->contact[i].dist);
|
|
EXPECT_EQ(d1->contact[i].mu, d2->contact[i].mu);
|
|
for (int j = 0; j < 5; ++j) {
|
|
EXPECT_EQ(d1->contact[i].friction[j], d2->contact[i].friction[j]);
|
|
}
|
|
for (int j = 0; j < 3; ++j) {
|
|
EXPECT_EQ(d1->contact[i].pos[j], d2->contact[i].pos[j]);
|
|
}
|
|
for (int j = 0; j < 9; ++j) {
|
|
EXPECT_EQ(d1->contact[i].frame[j], d2->contact[i].frame[j]);
|
|
}
|
|
for (int j = 0; j < 36; ++j) {
|
|
EXPECT_EQ(d1->contact[i].H[j], d2->contact[i].H[j]);
|
|
}
|
|
}
|
|
|
|
EXPECT_EQ(d1->nefc, 4 * (d1->contact[0].dim - 1) * 2);
|
|
EXPECT_EQ(d2->nefc, 4 * (d2->contact[0].dim - 1) * 2);
|
|
EXPECT_EQ(d1->nJ, d2->nJ);
|
|
for (int i = 0; i < d1->nefc; ++i) {
|
|
EXPECT_EQ(d1->efc_diagA[i], d2->efc_diagA[i]);
|
|
EXPECT_EQ(d1->efc_D[i], d2->efc_D[i]);
|
|
}
|
|
}
|
|
|
|
TEST_F(UserFlexTest, StiffnessMatrix) {
|
|
#ifdef mjUSESINGLE
|
|
GTEST_SKIP() << "Stiffness matrix kernel check fails in float32 precision";
|
|
#endif
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="grid" count="3 3 3" spacing="1 1 1" dim="3" dof="trilinear">
|
|
<contact selfcollide="none" internal="false"/>
|
|
<elasticity young="1"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
|
EXPECT_NE(m->flex_stiffness[m->flex_stiffnessadr[0]], 0);
|
|
EXPECT_EQ(m->nflexnode, 8);
|
|
|
|
// constants are in the kernel
|
|
mjtNum ones[24], zeros[24], res[24];
|
|
for (int i = 0; i < 3 * m->nflexnode; ++i) {
|
|
zeros[i] = 0;
|
|
ones[i] = 1;
|
|
}
|
|
mju_mulMatVec(res, m->flex_stiffness + m->flex_stiffnessadr[0], ones,
|
|
3 * m->nflexnode, 3 * m->nflexnode);
|
|
EXPECT_THAT(res, Pointwise(MjNear(1e-8, 1e-4), zeros));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, StiffnessCacheDiffersByGeometry) {
|
|
std::array<char, 1024> error;
|
|
|
|
// Create two flexes with same material but different bounding boxes
|
|
static constexpr char xml_small[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="grid" count="3 3 3" spacing="1 1 1" dim="3" dof="trilinear">
|
|
<contact selfcollide="none" internal="false"/>
|
|
<elasticity young="1" poisson="0.3"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
static constexpr char xml_large[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="grid" count="3 3 3" spacing="2 2 2" dim="3" dof="trilinear">
|
|
<contact selfcollide="none" internal="false"/>
|
|
<elasticity young="1" poisson="0.3"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
MjModelPtr m_small =
|
|
LoadModelFromString(xml_small, error.data(), error.size());
|
|
ASSERT_THAT(m_small.get(), NotNull()) << error.data();
|
|
|
|
MjModelPtr m_large =
|
|
LoadModelFromString(xml_large, error.data(), error.size());
|
|
ASSERT_THAT(m_large.get(), NotNull()) << error.data();
|
|
|
|
// Same number of nodes but different stiffness due to different geometry
|
|
EXPECT_EQ(m_small->nflexnode, m_large->nflexnode);
|
|
EXPECT_NE(m_small->flex_stiffness[m_small->flex_stiffnessadr[0]],
|
|
m_large->flex_stiffness[m_large->flex_stiffnessadr[0]]);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadTexture) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/textured_torus_flex.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
EXPECT_THAT(m->nflextexcoord, 637);
|
|
EXPECT_THAT(m->flex_elemtexcoord[0], 0);
|
|
EXPECT_THAT(m->flex_elemtexcoord[1], 1);
|
|
EXPECT_THAT(m->flex_elemtexcoord[2], 2);
|
|
EXPECT_THAT(m->flex_elemtexcoord[3], 0);
|
|
EXPECT_THAT(m->flex_elemtexcoord[4], 2);
|
|
EXPECT_THAT(m->flex_elemtexcoord[5], 3);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHBinary_41_Success) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/cube_41_binary_vol_gmshApp.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
mjData* d = mj_makeData(m);
|
|
EXPECT_EQ(m->nflexvert, 14);
|
|
EXPECT_EQ(m->nflexelem, 24);
|
|
EXPECT_EQ(m->flex_dim[0], 3);
|
|
mj_step(m, d);
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHBinary_22_Success) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/cube_22_binary_vol_gmshApp.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
mjData* d = mj_makeData(m);
|
|
EXPECT_EQ(m->nflexvert, 14);
|
|
EXPECT_EQ(m->nflexelem, 24);
|
|
EXPECT_EQ(m->flex_dim[0], 3);
|
|
mj_step(m, d);
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHSurfaceBinary_41_Success) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/cube_41_binary_surf_gmshApp.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
mjData* d = mj_makeData(m);
|
|
mj_kinematics(m, d);
|
|
mj_flex(m, d);
|
|
EXPECT_EQ(m->nflexvert, 14);
|
|
EXPECT_EQ(m->nflexelem, 24);
|
|
EXPECT_EQ(m->flex_dim[0], 2);
|
|
|
|
// first node x y z
|
|
EXPECT_EQ(d->flexvert_xpos[0], -0.5);
|
|
EXPECT_EQ(d->flexvert_xpos[1], -0.5);
|
|
EXPECT_EQ(d->flexvert_xpos[2], 0);
|
|
|
|
// first element
|
|
EXPECT_EQ(m->flex_elem[0], 9 - 1);
|
|
EXPECT_EQ(m->flex_elem[1], 4 - 1);
|
|
EXPECT_EQ(m->flex_elem[2], 3 - 1);
|
|
|
|
mj_step(m, d);
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHSurfaceBinary_22_Success) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/cube_22_binary_surf_gmshApp.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
mjData* d = mj_makeData(m);
|
|
mj_kinematics(m, d);
|
|
mj_flex(m, d);
|
|
EXPECT_EQ(m->nflexvert, 14);
|
|
EXPECT_EQ(m->nflexelem, 24);
|
|
EXPECT_EQ(m->flex_dim[0], 2);
|
|
|
|
// first node x y z
|
|
EXPECT_EQ(d->flexvert_xpos[0], -0.5);
|
|
EXPECT_EQ(d->flexvert_xpos[1], -0.5);
|
|
EXPECT_EQ(d->flexvert_xpos[2], 0);
|
|
|
|
// first element
|
|
EXPECT_EQ(m->flex_elem[0], 9 - 1);
|
|
EXPECT_EQ(m->flex_elem[1], 4 - 1);
|
|
EXPECT_EQ(m->flex_elem[2], 3 - 1);
|
|
|
|
mj_step(m, d);
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHBinaryFTETWILD_22_Success) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/shark_22_binary_vol_fTetWild.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
mjData* d = mj_makeData(m);
|
|
EXPECT_EQ(m->nflexvert, 429);
|
|
EXPECT_EQ(m->nflexelem, 1073);
|
|
EXPECT_EQ(m->flex_dim[0], 3);
|
|
mj_step(m, d);
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHASCII_41_Success) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/cube_41_ascii_vol_gmshApp.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
mjData* d = mj_makeData(m);
|
|
EXPECT_EQ(m->nflexvert, 14);
|
|
EXPECT_EQ(m->nflexelem, 24);
|
|
EXPECT_EQ(m->flex_dim[0], 3);
|
|
mj_step(m, d);
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHASCII_22_Success) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/cube_22_ascii_vol_gmshApp.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
mjData* d = mj_makeData(m);
|
|
EXPECT_EQ(m->nflexvert, 14);
|
|
EXPECT_EQ(m->nflexelem, 24);
|
|
EXPECT_EQ(m->flex_dim[0], 3);
|
|
mj_step(m, d);
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHSurfaceASCII_41_Success) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/cube_41_ascii_surf_gmshApp.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
mjData* d = mj_makeData(m);
|
|
mj_kinematics(m, d);
|
|
mj_flex(m, d);
|
|
EXPECT_EQ(m->nflexvert, 14);
|
|
EXPECT_EQ(m->nflexelem, 24);
|
|
EXPECT_EQ(m->flex_dim[0], 2);
|
|
|
|
// first node x y z
|
|
EXPECT_EQ(d->flexvert_xpos[0], -0.5);
|
|
EXPECT_EQ(d->flexvert_xpos[1], -0.5);
|
|
EXPECT_EQ(d->flexvert_xpos[2], 0);
|
|
|
|
// first element
|
|
EXPECT_EQ(m->flex_elem[0], 9 - 1);
|
|
EXPECT_EQ(m->flex_elem[1], 4 - 1);
|
|
EXPECT_EQ(m->flex_elem[2], 3 - 1);
|
|
|
|
mj_step(m, d);
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHSurfaceASCII_22_Success) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/cube_22_ascii_surf_gmshApp.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
mjData* d = mj_makeData(m);
|
|
mj_kinematics(m, d);
|
|
mj_flex(m, d);
|
|
EXPECT_EQ(m->nflexvert, 14);
|
|
EXPECT_EQ(m->nflexelem, 24);
|
|
EXPECT_EQ(m->flex_dim[0], 2);
|
|
|
|
// first node x y z
|
|
EXPECT_EQ(d->flexvert_xpos[0], -0.5);
|
|
EXPECT_EQ(d->flexvert_xpos[1], -0.5);
|
|
EXPECT_EQ(d->flexvert_xpos[2], 0);
|
|
|
|
// first element
|
|
EXPECT_EQ(m->flex_elem[0], 9 - 1);
|
|
EXPECT_EQ(m->flex_elem[1], 4 - 1);
|
|
EXPECT_EQ(m->flex_elem[2], 3 - 1);
|
|
|
|
mj_step(m, d);
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHASCIIFTETWILD_22_Success) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/shark_22_ascii_vol_fTetWild.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
mjData* d = mj_makeData(m);
|
|
EXPECT_EQ(m->nflexvert, 425);
|
|
EXPECT_EQ(m->nflexelem, 1070);
|
|
EXPECT_EQ(m->flex_dim[0], 3);
|
|
mj_step(m, d);
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingNodeHeader_Fail) {
|
|
const std::string xml_path = GetTestDataFilePath(
|
|
"user/testdata/malformed_cube_41_ascii_missing_node_header.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
EXPECT_THAT(error.data(),
|
|
HasSubstr("XML Error: Error: All nodes must be in single block"));
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingNodeIndex_Fail) {
|
|
const std::string xml_path = GetTestDataFilePath(
|
|
"user/testdata/malformed_cube_41_ascii_missing_node_index.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
EXPECT_THAT(error.data(),
|
|
HasSubstr("XML Error: Error: Node tags must be sequential"));
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingElementHeader_Fail) {
|
|
const std::string xml_path = GetTestDataFilePath(
|
|
"user/testdata/malformed_cube_41_ascii_missing_element_header.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
EXPECT_THAT(
|
|
error.data(),
|
|
HasSubstr("XML Error: Error: All elements must be in single block"));
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingElement_Fail) {
|
|
const std::string xml_path = GetTestDataFilePath(
|
|
"user/testdata/malformed_cube_41_ascii_missing_element.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
EXPECT_THAT(error.data(),
|
|
HasSubstr("XML Error: Error: Error reading Elements"));
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest,
|
|
LoadMSHASCII_41_MismatchBetweenMaxNodesAndNodesInBlock_Fail) {
|
|
const std::string xml_path = GetTestDataFilePath(
|
|
"user/testdata/"
|
|
"malformed_cube_41_ascii_mismatch_between_max_nodes_and_nodes_in_block."
|
|
"xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
EXPECT_THAT(
|
|
error.data(),
|
|
HasSubstr("XML Error: Error: Maximum number of nodes must be equal to "
|
|
"number of nodes in a block\nElement 'flexcomp', line 22\n"));
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumNodes_Fail) {
|
|
const std::string xml_path = GetTestDataFilePath(
|
|
"user/testdata/malformed_cube_22_ascii_missing_num_nodes.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
// TODO(mohammadhamid): Replace with an assertion about the error message. For
|
|
// some reason, on Windows the error message is different on GH Actions
|
|
EXPECT_THAT(m, IsNull());
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNode_Fail) {
|
|
const std::string xml_path = GetTestDataFilePath(
|
|
"user/testdata/malformed_cube_22_ascii_missing_node.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
EXPECT_THAT(error.data(),
|
|
HasSubstr("XML Error: Error: Error reading node tags"));
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumElements_Fail) {
|
|
const std::string xml_path = GetTestDataFilePath(
|
|
"user/testdata/malformed_shark_22_ascii_missing_num_elements.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
// TODO(mohammadhamid): Replace with an assertion about the error message. For
|
|
// some reason, on Windows the error message is different on GH Actions
|
|
EXPECT_THAT(m, IsNull());
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingElement_Fail) {
|
|
const std::string xml_path = GetTestDataFilePath(
|
|
"user/testdata/malformed_cube_22_ascii_missing_element.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
EXPECT_THAT(error.data(),
|
|
HasSubstr("XML Error: Error: Error reading Elements"));
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, LoadMSHASCII_dim_missing_in_xml) {
|
|
const std::string xml_path = GetTestDataFilePath(
|
|
"user/testdata/cube_22_ascii_vol_gmshApp_missing_dim.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
EXPECT_EQ(m->flex_dim[0], 3);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, TrilinearUnusedVertices_Crash) {
|
|
// This XML defines 5 points but only uses 4 in the element.
|
|
// The last point (2 2 2) is unused.
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="direct"
|
|
point="0 0 0 1 0 0 0 1 0 0 0 1 2 2 2"
|
|
element="0 1 2 3"
|
|
dof="trilinear" dim="3">
|
|
<contact selfcollide="none"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
ASSERT_THAT(m.get(), testing::NotNull()) << error.data();
|
|
}
|
|
|
|
TEST_F(UserFlexTest, MeshNodePinning) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="direct"
|
|
point="0 0 0 1 0 0 0 1 0 0 0 1 2 2 2"
|
|
element="0 1 2 3"
|
|
dof="trilinear" dim="3">
|
|
<contact selfcollide="none"/>
|
|
<pin id="0"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
ASSERT_THAT(m.get(), testing::NotNull()) << error.data();
|
|
|
|
// Verify that node 0 (corner) is pinned
|
|
// Trilinear 3D has 8 nodes.
|
|
// If 0 are pinned, we get 8 bodies.
|
|
// If 1 is pinned, we get 7 bodies (created) + 1 existing (the parent).
|
|
// m->nbody should reflect this. Flex bodies are added to the model.
|
|
// Model has 1 world body + flex bodies.
|
|
EXPECT_EQ(m->nbody, 1 + 7); // 1 world + 7 flex nodes (1 pinned)
|
|
}
|
|
|
|
TEST_F(UserFlexTest, FlexcompMeshLoadsFromVFS) {
|
|
// read cube.stl from testdata into a buffer
|
|
const std::string stl_path = GetTestDataFilePath("user/testdata/cube.stl");
|
|
FILE* f = fopen(stl_path.c_str(), "rb");
|
|
ASSERT_THAT(f, NotNull()) << "Could not open " << stl_path;
|
|
fseek(f, 0, SEEK_END);
|
|
long stl_size = ftell(f);
|
|
fseek(f, 0, SEEK_SET);
|
|
std::string stl_data(stl_size, '\0');
|
|
fread(stl_data.data(), 1, stl_size, f);
|
|
fclose(f);
|
|
|
|
// add the STL data to a VFS
|
|
mjVFS vfs;
|
|
mj_defaultVFS(&vfs);
|
|
mj_addBufferVFS(&vfs, "cube.stl", stl_data.data(), stl_size);
|
|
|
|
// XML that uses flexcomp type="mesh" referencing the VFS file
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="flex_body" pos="0 0 1">
|
|
<flexcomp name="flex_object" type="mesh" file="cube.stl" rigid="true">
|
|
<contact contype="1" conaffinity="1"/>
|
|
</flexcomp>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
// cleanup
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size(), &vfs);
|
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
|
MjDataPtr d = MakeData(m);
|
|
mj_step(m.get(), d.get());
|
|
mj_deleteVFS(&vfs);
|
|
}
|
|
|
|
// Test that flex constraints are preserved when attaching a model
|
|
TEST_F(UserFlexTest, FlexAttachConstraintPreserved) {
|
|
// Child model with flex and strain constraint
|
|
static constexpr char flex_xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="flex_parent">
|
|
<flexcomp name="test" type="box"
|
|
spacing=".1 .1 .1" radius="0.001"
|
|
dof="trilinear" mass="1" dim="3">
|
|
<contact selfcollide="none"/>
|
|
<edge equality="strain"/>
|
|
</flexcomp>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
// Parent model that attaches the flex model
|
|
static constexpr char parent_xml[] = R"(
|
|
<mujoco>
|
|
<asset>
|
|
<model name="flex" file="flex.xml"/>
|
|
</asset>
|
|
<worldbody>
|
|
<frame pos="0 0 0.3">
|
|
<attach model="flex" prefix="flex_"/>
|
|
</frame>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
// Set up VFS with both XML files
|
|
auto vfs = std::make_unique<mjVFS>();
|
|
mj_defaultVFS(vfs.get());
|
|
mj_addBufferVFS(vfs.get(), "flex.xml", flex_xml, sizeof(flex_xml));
|
|
|
|
// First verify the standalone flex model has constraints
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m_standalone =
|
|
LoadModelFromString(flex_xml, error.data(), error.size(), vfs.get());
|
|
ASSERT_THAT(m_standalone.get(), NotNull()) << error.data();
|
|
MjDataPtr d_standalone = MakeData(m_standalone);
|
|
mj_forward(m_standalone.get(), d_standalone.get());
|
|
int standalone_neq = m_standalone->neq;
|
|
EXPECT_GT(standalone_neq, 0) << "Standalone flex should have constraints";
|
|
|
|
// Now load the parent model which attaches the flex
|
|
MjModelPtr m_attached =
|
|
LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get());
|
|
ASSERT_THAT(m_attached.get(), NotNull()) << error.data();
|
|
MjDataPtr d_attached = MakeData(m_attached);
|
|
mj_forward(m_attached.get(), d_attached.get());
|
|
|
|
// THE BUG: flex constraints disappear when attached
|
|
EXPECT_GT(m_attached->neq, 0)
|
|
<< "Attached flex should preserve strain constraints";
|
|
EXPECT_EQ(m_attached->neq, standalone_neq)
|
|
<< "Attached flex should have same number of constraints as standalone";
|
|
|
|
mj_deleteVFS(vfs.get());
|
|
}
|
|
|
|
TEST_F(UserFlexTest, FlexNoConstraintsWarning) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="grid" count="4 4 1" spacing=".2 .2 .2"
|
|
dim="2" radius=".1"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
|
EXPECT_THAT(error.data(),
|
|
HasSubstr("no equality constraints or passive forces"));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, EmptyCellNodePinning) {
|
|
// A 2x2x2 grid with a box mesh that fills all cells.
|
|
// No nodes should be pinned.
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="box" spacing=".1 .1 .1" dim="3"
|
|
dof="trilinear" mass="1" cellcount="2 2 2">
|
|
<contact selfcollide="none"/>
|
|
<elasticity young="1"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
|
|
|
// A 2x2x2 grid with trilinear order has (2+1)^3 = 27 node positions.
|
|
int nadr = m->flex_nodeadr[0];
|
|
int nnode = m->flex_nodenum[0];
|
|
EXPECT_EQ(nnode, 27);
|
|
|
|
// All cells are occupied by the box, so no node should be pinned.
|
|
int pinned = 0;
|
|
for (int n = nadr; n < nadr + nnode; n++) {
|
|
int bid = m->flex_nodebodyid[n];
|
|
if (m->body_jntnum[bid] == 0) {
|
|
pinned++;
|
|
}
|
|
}
|
|
EXPECT_EQ(pinned, 0);
|
|
|
|
// Verify simulation works
|
|
MjDataPtr d = MakeData(m);
|
|
for (int i = 0; i < 10; i++) {
|
|
mj_step(m.get(), d.get());
|
|
}
|
|
}
|
|
|
|
TEST_F(UserFlexTest, EmptyCellNodePinningMesh) {
|
|
// Load bunny_multicell.xml which has a 3x3x3 grid.
|
|
// The bunny mesh only occupies some cells, so many nodes should be pinned.
|
|
const std::string xml_path = GetModelPath("flex/bunny_multicell.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
|
|
// 3x3x3 grid, order=1: (3+1)^3 = 64 node positions
|
|
int nadr = m->flex_nodeadr[0];
|
|
int nnode = m->flex_nodenum[0];
|
|
EXPECT_EQ(nnode, 64);
|
|
|
|
// Count pinned nodes (no joints)
|
|
int pinned = 0;
|
|
int free_nodes = 0;
|
|
for (int n = nadr; n < nadr + nnode; n++) {
|
|
int bid = m->flex_nodebodyid[n];
|
|
if (m->body_jntnum[bid] == 0) {
|
|
pinned++;
|
|
} else {
|
|
free_nodes++;
|
|
}
|
|
}
|
|
|
|
// At least some nodes should be pinned since the bunny doesn't fill all cells
|
|
EXPECT_GT(pinned, 0) << "Expected some nodes to be pinned from empty cells";
|
|
EXPECT_GT(free_nodes, 0) << "Expected some nodes to remain free";
|
|
EXPECT_EQ(pinned + free_nodes, nnode);
|
|
|
|
// Verify the model can simulate
|
|
mjData* d = mj_makeData(m);
|
|
mj_forward(m, d);
|
|
for (int i = 0; i < 10; i++) {
|
|
mj_step(m, d);
|
|
}
|
|
|
|
mj_deleteData(d);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) {
|
|
// Regression test for ci_min calculation with order=2.
|
|
// A 2x1x1 quadratic grid has nodes at gi=0..4 (5 nodes per axis).
|
|
// We place mesh vertices only in cell 0 (x in [0, 0.5]), so cell 1 is empty.
|
|
//
|
|
// Node gi=3 belongs only to cell 1 (1*2 <= 3 <= 2*2).
|
|
// With the old formula (gi-order)/order = (3-2)/2 = 0, it would also check
|
|
// cell 0 (non-empty), incorrectly marking gi=3 as non-pinned.
|
|
// Single hex element at x=[0,0.3], well inside cell 0 of a 3x1x1 grid.
|
|
// Anchor vertex at x=1.0 extends the bounding box to [0,1]^3.
|
|
// The 3x1x1 quadratic grid splits at x=0.33, 0.67.
|
|
// Cell 0 has vertices, cells 1 and 2 are empty.
|
|
// Interior nodes for cells 1,2 should be pinned to the parent body.
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="parent">
|
|
<freejoint/>
|
|
<inertial mass="0.01" pos="0 0 0"
|
|
diaginertia="0.001 0.001 0.001"/>
|
|
<flexcomp name="test" type="direct" dim="3"
|
|
dof="quadratic" mass="1" cellcount="3 1 1"
|
|
point="0.0 0.0 0.0 0.3 0.0 0.0
|
|
0.0 1.0 0.0 0.3 1.0 0.0
|
|
0.0 0.0 1.0 0.3 0.0 1.0
|
|
0.0 1.0 1.0 0.3 1.0 1.0
|
|
1.0 0.5 0.5"
|
|
element="0 1 3 2 4 5 7 6">
|
|
<contact selfcollide="none"/>
|
|
<elasticity young="1"/>
|
|
</flexcomp>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
|
|
|
// 3x1x1 quadratic grid: (3*2+1) * (1*2+1) * (1*2+1) = 7*3*3 = 63 nodes
|
|
int nadr = m->flex_nodeadr[0];
|
|
int nnode = m->flex_nodenum[0];
|
|
EXPECT_EQ(nnode, 63);
|
|
|
|
// Count pinned nodes: pinned nodes are assigned to the parent body.
|
|
int parent_bid = mj_name2id(m.get(), mjOBJ_BODY, "parent");
|
|
ASSERT_GT(parent_bid, 0);
|
|
int pinned = 0;
|
|
for (int n = nadr; n < nadr + nnode; n++) {
|
|
if (m->flex_nodebodyid[n] == parent_bid) {
|
|
pinned++;
|
|
}
|
|
}
|
|
|
|
// Cells 1 and 2 are empty, so nodes exclusively in those cells are pinned.
|
|
// Nodes at gi=3..6 (with any gj, gk) are only in cells 1 and/or 2.
|
|
// That's 4 * 3 * 3 = 36 nodes.
|
|
EXPECT_EQ(pinned, 36);
|
|
|
|
mj_deleteData(mj_makeData(m.get()));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, EmptyCellDetectsElements) {
|
|
// A cube surface mesh (dim=2, 12 triangles) spanning [0,1]^3.
|
|
// With cellcount="6 6 6" (216 cells), only 8 corner cells contain
|
|
// mesh vertices.
|
|
//
|
|
// Bug: MarkEmptyCells only checked vertices, so 208/216 cells are
|
|
// marked empty, causing most interior nodes to be incorrectly pinned.
|
|
// Fix: check element AABBs to correctly identify occupied cells.
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="parent">
|
|
<freejoint/>
|
|
<inertial mass="0.01" pos="0.5 0.5 0.5"
|
|
diaginertia="0.001 0.001 0.001"/>
|
|
<flexcomp name="test" type="direct" dim="2"
|
|
dof="trilinear" mass="1" cellcount="6 6 6"
|
|
point="0 0 0 1 0 0 1 1 0 0 1 0
|
|
0 0 1 1 0 1 1 1 1 0 1 1"
|
|
element="0 1 2 0 2 3 4 6 5 4 7 6
|
|
0 5 1 0 4 5 2 7 3 2 6 7
|
|
0 3 7 0 7 4 1 5 6 1 6 2">
|
|
<contact selfcollide="none"/>
|
|
</flexcomp>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
|
|
|
// 6x6x6 trilinear grid: (6+1)^3 = 343 nodes
|
|
int nadr = m->flex_nodeadr[0];
|
|
int nnode = m->flex_nodenum[0];
|
|
ASSERT_EQ(nnode, 343);
|
|
|
|
// Count pinned nodes: those assigned to the parent body.
|
|
int parent_bid = mj_name2id(m.get(), mjOBJ_BODY, "parent");
|
|
ASSERT_GT(parent_bid, 0);
|
|
int pinned = 0;
|
|
for (int n = nadr; n < nadr + nnode; n++) {
|
|
if (m->flex_nodebodyid[n] == parent_bid) {
|
|
pinned++;
|
|
}
|
|
}
|
|
|
|
// The cube surface fills the entire bounding box. The element-AABB
|
|
// marks all boundary cells as surface cells (152/216). The interior
|
|
// flood-fill finds no exterior seeds (all boundary cells are surface),
|
|
// so the remaining 64 cells are classified as interior (non-empty).
|
|
// No cells are empty → 0 nodes pinned.
|
|
EXPECT_EQ(pinned, 0);
|
|
|
|
mj_deleteData(mj_makeData(m.get()));
|
|
}
|
|
|
|
TEST_F(UserFlexTest, TotalMassTrilinear) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1"
|
|
dim="3" dof="trilinear" mass="1.5">
|
|
<contact selfcollide="none" internal="false"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
|
|
|
double total_mass = 0;
|
|
for (int i = 1; i < m->nbody; ++i) {
|
|
total_mass += m->body_mass[i];
|
|
}
|
|
|
|
EXPECT_NEAR(total_mass, 1.5, 1e-5);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, TotalMassQuadratic) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="grid" count="3 2 2" spacing="1 1 1"
|
|
dim="3" dof="quadratic" mass="2.0">
|
|
<contact selfcollide="none" internal="false"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
|
|
|
double total_mass = 0;
|
|
for (int i = 1; i < m->nbody; ++i) {
|
|
total_mass += m->body_mass[i];
|
|
}
|
|
|
|
EXPECT_NEAR(total_mass, 2.0, 1e-5);
|
|
}
|
|
|
|
TEST_F(UserFlexTest, Dof2d) {
|
|
// 3x3 grid with dof="2d": 9 vertices, 2 DOFs each -> nv = 18
|
|
static constexpr char xml_2d[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="grid" count="3 3 1" spacing=".1 .1 .1"
|
|
dim="2" radius=".01" dof="2d">
|
|
<edge equality="true"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
// same model with dof="full" for comparison: 9 vertices, 3 DOFs each -> nv =
|
|
// 27
|
|
static constexpr char xml_full[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<flexcomp name="test" type="grid" count="3 3 1" spacing=".1 .1 .1"
|
|
dim="2" radius=".01">
|
|
<edge equality="true"/>
|
|
</flexcomp>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
std::array<char, 1024> error;
|
|
|
|
// load 2d model
|
|
MjModelPtr m_2d = LoadModelFromString(xml_2d, error.data(), error.size());
|
|
ASSERT_THAT(m_2d.get(), NotNull()) << error.data();
|
|
MjDataPtr d_2d = MakeData(m_2d);
|
|
|
|
// load full model
|
|
MjModelPtr m_full = LoadModelFromString(xml_full, error.data(), error.size());
|
|
ASSERT_THAT(m_full.get(), NotNull()) << error.data();
|
|
MjDataPtr d_full = MakeData(m_full);
|
|
|
|
// verify DOF counts
|
|
EXPECT_EQ(m_2d->nv, 18); // 9 vertices * 2 DOFs
|
|
EXPECT_EQ(m_full->nv, 27); // 9 vertices * 3 DOFs
|
|
|
|
// same number of vertices and elements
|
|
EXPECT_EQ(m_2d->nflexvert, m_full->nflexvert);
|
|
EXPECT_EQ(m_2d->nflexelem, m_full->nflexelem);
|
|
|
|
// each body has 2 DOFs in 2d mode, 3 in full mode
|
|
for (int i = 1; i < m_2d->nbody; i++) {
|
|
EXPECT_EQ(m_2d->body_dofnum[i], 2) << "body " << i;
|
|
}
|
|
for (int i = 1; i < m_full->nbody; i++) {
|
|
EXPECT_EQ(m_full->body_dofnum[i], 3) << "body " << i;
|
|
}
|
|
|
|
// simulate a few steps to make sure nothing crashes
|
|
for (int i = 0; i < 10; i++) {
|
|
mj_step(m_2d.get(), d_2d.get());
|
|
mj_step(m_full.get(), d_full.get());
|
|
}
|
|
}
|
|
|
|
TEST_F(UserFlexTest, Vert0RotationInvariant) {
|
|
// unrotated trilinear grid
|
|
static constexpr char xml_unrotated[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="parent">
|
|
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1"
|
|
dim="3" dof="trilinear">
|
|
<contact selfcollide="none" internal="false"/>
|
|
</flexcomp>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
// same grid rotated 45 degrees around Z via parent body quaternion
|
|
static constexpr char xml_rotated[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="parent" quat="0.9238795 0 0 0.3826834">
|
|
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1"
|
|
dim="3" dof="trilinear">
|
|
<contact selfcollide="none" internal="false"/>
|
|
</flexcomp>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)";
|
|
|
|
std::array<char, 1024> error;
|
|
MjModelPtr m1 =
|
|
LoadModelFromString(xml_unrotated, error.data(), error.size());
|
|
ASSERT_THAT(m1.get(), NotNull()) << error.data();
|
|
|
|
MjModelPtr m2 = LoadModelFromString(xml_rotated, error.data(), error.size());
|
|
ASSERT_THAT(m2.get(), NotNull()) << error.data();
|
|
|
|
// same number of vertices
|
|
ASSERT_EQ(m1->nflexvert, m2->nflexvert);
|
|
|
|
// vert0 must be identical regardless of rotation
|
|
for (int i = 0; i < 3 * m1->nflexvert; ++i) {
|
|
EXPECT_NEAR(m1->flex_vert0[i], m2->flex_vert0[i], 1e-10)
|
|
<< "vert0 mismatch at index " << i;
|
|
}
|
|
}
|
|
|
|
TEST_F(UserFlexTest, Load1DFlexFromOBJ) {
|
|
const std::string xml_path =
|
|
GetTestDataFilePath("user/testdata/flex_line_obj.xml");
|
|
std::array<char, 1024> error;
|
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
|
ASSERT_THAT(m, NotNull()) << error.data();
|
|
EXPECT_EQ(m->nflexvert, 4);
|
|
EXPECT_EQ(m->nflexelem, 3);
|
|
EXPECT_EQ(m->flex_dim[0], 1);
|
|
mj_deleteModel(m);
|
|
}
|
|
|
|
} // namespace
|
|
} // namespace mujoco
|