Introduce trilinear flex parametrization.
These flexes use only 24 DOFs (3 per vertex of the bounding box), while colliding with the full high resolution mesh. On an 8x8x8 cube, the performance using DOFs at all vertices is ``` Simulation time : 18.74 s Steps per second : 533 Realtime factor : 0.53 x Time per step : 1874.4 µs Contacts per step : 114.88 Constraints per step : 3322.51 Degrees of freedom : 1536 ``` With the new implementation, it is the following: ``` Simulation time : 1.82 s Steps per second : 5507 Realtime factor : 5.51 x Time per step : 181.6 µs Contacts per step : 38.84 Constraints per step : 155.36 Degrees of freedom : 24 ``` PiperOrigin-RevId: 721008829 Change-Id: I833df027527db578d86667cc4b24295bcf6f7d22
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
1a4b821b6b
commit
7cdf180641
@@ -32,6 +32,7 @@ namespace {
|
||||
using ::testing::DoubleNear;
|
||||
using ::testing::HasSubstr;
|
||||
using ::testing::Ne;
|
||||
using ::testing::Pointwise;
|
||||
using ::testing::StrEq;
|
||||
using ::testing::ElementsAreArray;
|
||||
|
||||
@@ -210,6 +211,94 @@ TEST_F(UtilMiscTest, MuscleGainLength) {
|
||||
EXPECT_EQ(mju_muscleGainLength(2.0, lmin, lmax), 0);
|
||||
}
|
||||
|
||||
// --------------------------------- Interpolation -----------------------------
|
||||
|
||||
using InterpolationTest = MujocoTest;
|
||||
|
||||
TEST_F(InterpolationTest, mju_defGradient) {
|
||||
int order = 1;
|
||||
mjtNum mat[9];
|
||||
mjtNum p1[3] = {.5, .5, .5};
|
||||
mjtNum p2[3] = {.25, .25, .25};
|
||||
mjtNum dof0[24] = {0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 1, 1,
|
||||
1, 0, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1};
|
||||
|
||||
// identity
|
||||
mjtNum dof1[24];
|
||||
for (int i = 0; i < 24; ++i) dof1[i] = dof0[i];
|
||||
mju_defGradient(mat, p1, dof1, order);
|
||||
EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 1, 0, 0, 0, 1}));
|
||||
|
||||
// translation
|
||||
mjtNum dof2[24];
|
||||
for (int i = 0; i < 24; ++i) dof2[i] = 2 + dof0[i];
|
||||
mju_defGradient(mat, p1, dof2, order);
|
||||
EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 1, 0, 0, 0, 1}));
|
||||
mju_defGradient(mat, p2, dof2, order);
|
||||
EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 1, 0, 0, 0, 1}));
|
||||
|
||||
// constant stretch
|
||||
mjtNum dof3[24];
|
||||
for (int i = 0; i < 24; ++i) dof3[i] = 2*dof0[i];
|
||||
mju_defGradient(mat, p1, dof3, order);
|
||||
EXPECT_THAT(mat, ElementsAreArray({2, 0, 0, 0, 2, 0, 0, 0, 2}));
|
||||
mju_defGradient(mat, p2, dof3, order);
|
||||
EXPECT_THAT(mat, ElementsAreArray({2, 0, 0, 0, 2, 0, 0, 0, 2}));
|
||||
|
||||
// axial stretch
|
||||
mjtNum dof4[24];
|
||||
for (int i = 0; i < 24; ++i) dof4[i] = (i%3 == 1 ? 2 : 1)*dof0[i];
|
||||
mju_defGradient(mat, p1, dof4, order);
|
||||
EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 2, 0, 0, 0, 1}));
|
||||
mju_defGradient(mat, p2, dof4, order);
|
||||
EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 2, 0, 0, 0, 1}));
|
||||
|
||||
// z-axis 90 degree rotation
|
||||
mjtNum dof5[24];
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
mjtNum quat[4] = {0, 0, 0, 1};
|
||||
mjtNum axis[3] = {0, 0, 1};
|
||||
mju_axisAngle2Quat(quat, axis, mjPI/2);
|
||||
mju_rotVecQuat(dof5 + 3*i, dof0 + 3*i, quat);
|
||||
}
|
||||
mju_defGradient(mat, p1, dof5, order);
|
||||
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), {0, -1, 0, 1, 0, 0, 0, 0, 1}));
|
||||
mju_defGradient(mat, p2, dof5, order);
|
||||
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), {0, -1, 0, 1, 0, 0, 0, 0, 1}));
|
||||
|
||||
// z-axis 30 degree rotation
|
||||
mjtNum dof6[24];
|
||||
mjtNum rot6[9];
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
mjtNum quat[4];
|
||||
mjtNum axis[3] = {0, 0, 1};
|
||||
mju_axisAngle2Quat(quat, axis, mjPI/6);
|
||||
mju_rotVecQuat(dof6 + 3*i, dof0 + 3*i, quat);
|
||||
mju_quat2Mat(rot6, quat);
|
||||
}
|
||||
mju_defGradient(mat, p1, dof6, order);
|
||||
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot6));
|
||||
mju_defGradient(mat, p2, dof6, order);
|
||||
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot6));
|
||||
|
||||
// z-axis CoM rotation
|
||||
mjtNum dof7[24];
|
||||
mjtNum rot7[9];
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
mjtNum quat[4];
|
||||
mjtNum axis[3] = {0, 0, 1};
|
||||
mjtNum offset[3] = {-.5, -.5, 0};
|
||||
mju_axisAngle2Quat(quat, axis, mjPI/6);
|
||||
mju_add3(dof7 + 3*i, dof0 + 3*i, offset);
|
||||
mju_rotVecQuat(dof7 + 3*i, dof0 + 3*i, quat);
|
||||
mju_quat2Mat(rot7, quat);
|
||||
}
|
||||
mju_defGradient(mat, p1, dof7, order);
|
||||
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot7));
|
||||
mju_defGradient(mat, p2, dof7, order);
|
||||
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot7));
|
||||
}
|
||||
|
||||
// --------------------------------- Base64 ------------------------------------
|
||||
|
||||
using Base64Test = MujocoTest;
|
||||
|
||||
Vendored
+7
-1
@@ -29,7 +29,7 @@
|
||||
<freejoint/>
|
||||
|
||||
<flexcomp name="f1" type="ellipsoid" rgba=".8 .2 .2 1" radius="0.001" count="4 4 4"
|
||||
spacing=".025 .025 .025" dim="3" mass="1">
|
||||
spacing=".025 .025 .025" dim="3" mass="1" dof="radial">
|
||||
<edge equality="true" solref="0.15 0.2" stiffness="0" damping="0"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
@@ -44,6 +44,12 @@
|
||||
zaxis="1 1 1" count="3 3 3" mass="1" spacing="0.02 0.03 0.04">
|
||||
<edge equality="true" solref="0.15 0.2" stiffness="0" damping="0"/>
|
||||
</flexcomp>
|
||||
|
||||
<flexcomp type="grid" count="8 8 8" spacing=".007 .007 .007" pos="0 0 1.5" dim="3"
|
||||
radius=".0001" rgba="0 .7 .7 1" mass=".25" name="softbody" dof="trilinear">
|
||||
<elasticity young="1e4" poisson="0.1" damping="0.01"/>
|
||||
<contact selfcollide="none" internal="false"/>
|
||||
</flexcomp>
|
||||
</worldbody>
|
||||
|
||||
</mujoco>
|
||||
|
||||
+154
-1
@@ -28,14 +28,15 @@
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
using ::testing::DoubleNear;
|
||||
using ::testing::IsNull;
|
||||
using ::testing::NotNull;
|
||||
using ::testing::HasSubstr;
|
||||
using ::testing::Pointwise;
|
||||
using UserFlexTest = MujocoTest;
|
||||
|
||||
|
||||
|
||||
|
||||
TEST_F(UserFlexTest, ParentMustHaveName) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
@@ -282,6 +283,158 @@ TEST_F(UserFlexTest, BoundingBoxCoordinates) {
|
||||
mj_deleteData(d);
|
||||
}
|
||||
|
||||
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>
|
||||
)";
|
||||
mjModel* m1 = LoadModelFromString(xml_selfcoll, error.data(), error.size());
|
||||
EXPECT_THAT(m1, 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>
|
||||
)";
|
||||
mjModel* m2 = LoadModelFromString(xml_internal, error.data(), error.size());
|
||||
EXPECT_THAT(m2, 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;
|
||||
mjModel* m1 = LoadModelFromString(xml_trilinear, error.data(), error.size());
|
||||
ASSERT_THAT(m1, NotNull()) << error.data();
|
||||
mjData* d1 = mj_makeData(m1);
|
||||
mj_step(m1, d1);
|
||||
|
||||
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>
|
||||
)";
|
||||
mjModel* m2 = LoadModelFromString(xml_linear, error.data(), error.size());
|
||||
ASSERT_THAT(m2, NotNull()) << error.data();
|
||||
mjData* d2 = mj_makeData(m2);
|
||||
mj_step(m2, d2);
|
||||
|
||||
EXPECT_EQ(m1->nflexvert, m2->nflexvert);
|
||||
for (int i = 0; i < 3*m1->nflexvert; ++i) {
|
||||
EXPECT_EQ(m1->flex_vert[i], d2->flexvert_xpos[i]);
|
||||
EXPECT_EQ(m1->flex_vert0[i], m2->flex_vert0[i]);
|
||||
EXPECT_EQ(d1->flexvert_xpos[i], d2->flexvert_xpos[i]);
|
||||
}
|
||||
|
||||
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_diagApprox[i], d2->efc_diagApprox[i]);
|
||||
EXPECT_EQ(d1->efc_D[i], d2->efc_D[i]);
|
||||
}
|
||||
|
||||
mj_deleteModel(m1);
|
||||
mj_deleteModel(m2);
|
||||
mj_deleteData(d1);
|
||||
mj_deleteData(d2);
|
||||
}
|
||||
|
||||
TEST_F(UserFlexTest, StiffnessMatrix) {
|
||||
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;
|
||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
||||
ASSERT_THAT(m, NotNull()) << error.data();
|
||||
EXPECT_NE(m->flex_stiffness[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, ones, 3*m->nflexnode, 3*m->nflexnode);
|
||||
EXPECT_THAT(res, Pointwise(DoubleNear(1e-8), zeros));
|
||||
|
||||
mj_deleteModel(m);
|
||||
}
|
||||
|
||||
TEST_F(UserFlexTest, LoadMSHBinary_41_Success) {
|
||||
const std::string xml_path =
|
||||
GetTestDataFilePath("user/testdata/cube_41_binary_vol_gmshApp.xml");
|
||||
|
||||
Reference in New Issue
Block a user