// 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_collision_driver.c. #include #include #include #include #include #include #include #include #include #include #include "test/fixture.h" #include "src/engine/engine_collision_driver.h" namespace mujoco { namespace { using MjCollisionTest = MujocoTest; using GeomPair = std::pair; using ::testing::IsEmpty; using ::testing::ElementsAre; using ::testing::NotNull; // Returns a sorted list of pairs of colliding geom names, where each pair of // geom names is sorted. static std::vector colliding_pairs( const mjModel* model, const mjData* data) { std::vector result; for (int i = 0; i < data->ncon; i++) { std::string geom1 = mj_id2name(model, mjOBJ_GEOM, data->contact[i].geom[0]); std::string geom2 = mj_id2name(model, mjOBJ_GEOM, data->contact[i].geom[1]); result.push_back(GeomPair(std::min(geom1, geom2), std::max(geom1, geom2))); } std::sort(result.begin(), result.end()); return result; } TEST_F(MjCollisionTest, AllCollisions) { static const char* const kModelFilePath = "engine/testdata/collisions.xml"; const std::string xml_path = GetTestDataFilePath(kModelFilePath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); mjData* data = mj_makeData(model); // mjCOL_ALL is the default mj_fwdPosition(model, data); EXPECT_THAT(colliding_pairs(model, data), ElementsAre( GeomPair("box", "sphere_collides"), GeomPair("box", "sphere_predefined") )); mj_deleteData(data); mj_deleteModel(model); } TEST_F(MjCollisionTest, EmptyModel) { char error[1024]; mjModel* model = LoadModelFromString("", error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); mj_fwdPosition(model, data); EXPECT_THAT(colliding_pairs(model, data), IsEmpty()); mj_deleteData(data); mj_deleteModel(model); } TEST_F(MjCollisionTest, ZeroedHessian) { static const char* const kModelFilePath = "engine/testdata/collisions.xml"; const std::string xml_path = GetTestDataFilePath(kModelFilePath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); mjData* data = mj_makeData(model); mj_fwdPosition(model, data); for (int i = 0; i < data->ncon; i++) { for (int j = 0; j < 36; j++) { EXPECT_FALSE(isnan(data->contact[i].H[j])) << "NaN in contact[" << i << "].H[" << j << "]"; } } mj_deleteData(data); mj_deleteModel(model); } TEST_F(MjCollisionTest, ContactCount) { constexpr char xml[] = R"( )"; char error[1024]; mjModel* m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m, NotNull()) << error; mjData* d = mj_makeData(m); ASSERT_THAT(d, NotNull()); mj_forward(m, d); // there are 8 spheres, all touching the floor EXPECT_EQ(d->ncon, 8); mj_deleteData(d); mj_deleteModel(m); } TEST_F(MjCollisionTest, FilterParent) { constexpr char xml[] = R"( )"; char error[1024]; mjModel* m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m, NotNull()) << error; mjData* d = mj_makeData(m); ASSERT_THAT(d, NotNull()); mj_fwdPosition(m, d); // there should be zero contacts, because colliding1 and colliding2 are in // bodies that have a parent-child relationship, through welds EXPECT_EQ(d->ncon, 0); // when this filtering is disabled, the geoms should collide m->opt.disableflags |= mjDSBL_FILTERPARENT; mj_fwdPosition(m, d); EXPECT_THAT(colliding_pairs(m, d), ElementsAre(GeomPair("colliding1", "colliding2"))); mj_deleteData(d); mj_deleteModel(m); } TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) { constexpr char xml[] = R"( )"; char error[1024]; mjModel* m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m, NotNull()) << error; mjData* d = mj_makeData(m); ASSERT_THAT(d, NotNull()); mj_fwdPosition(m, d); // even though colliding1 and colliding2 are have a parent-child relationship, // they collide because colliding1 is in EXPECT_THAT(colliding_pairs(m, d), ElementsAre(GeomPair("colliding1", "colliding2"))); mj_deleteData(d); mj_deleteModel(m); } TEST_F(MjCollisionTest, TestOBB) { mjtNum bvh1[6] = {-1, -1, -1, 1, 1, 1}; mjtNum bvh2[6] = {-1, -1, -1, 1, 1, 1}; mjtNum pos1[3] = {0, 0, 0}; mjtNum mat1[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; mjtNum pos2[3] = {1.71, 1.71, 0}; // just a little more than 1+sqrt(2)/2 mjtNum mat2[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; EXPECT_THAT( mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0), true); // rotate by 45 degrees mat2[0] = 1./mju_sqrt(2.); mat2[1] = -1./mju_sqrt(2.); mat2[3] = 1./mju_sqrt(2.); mat2[4] = 1./mju_sqrt(2.); EXPECT_THAT( mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0), false); } TEST_F(MjCollisionTest, PlaneInBody) { constexpr char xml[] = R"( )"; char error[1024]; mjModel* m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m, NotNull()) << error; mjData* d = mj_makeData(m); ASSERT_THAT(d, NotNull()); mj_step(m, d); mj_deleteData(d); mj_deleteModel(m); } TEST_F(MjCollisionTest, PinchingSucceeds) { constexpr char xml[] = R"( )"; char error[1024]; mjModel* m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m, NotNull()) << error; mjData* d = mj_makeData(m); ASSERT_THAT(d, NotNull()); int lift_id = mj_name2id(m, mjOBJ_ACTUATOR, "lift"); int grasp_id = mj_name2id(m, mjOBJ_ACTUATOR, "grasp"); // Phase 1: Lower gripper. // The gripper base starts at z=0.5. The finger has length 0.2 (size 0.1), // extending from z=0.4 to z=0.2 relative to base (center at -0.1). // The cloth is at z=0.1. We need to lower the gripper so the fingertips // reach the cloth. A lift value of -0.35 places the fingertips near z=0.05. for (int i = 0; i < 500; ++i) { d->ctrl[lift_id] = -0.35; // Lower d->ctrl[grasp_id] = 0; // Open mj_step(m, d); } // Phase 2: Pinch for (int i = 0; i < 100; ++i) { d->ctrl[lift_id] = -0.35; // Hold height d->ctrl[grasp_id] = 0.8; // Close (max 1) mj_step(m, d); } // Phase 3: Lift for (int i = 0; i < 1000; ++i) { d->ctrl[lift_id] = 0.5; // Lift up d->ctrl[grasp_id] = 0.8; // Keep closed mj_step(m, d); } // Check if cloth is lifted // flex verts are in d->flexvert_xpos // original z is ~0.1 (falling to floor ~0.0) // gripper lifted to > 0.5 probably // Find average Z of cloth double avg_z = 0; int nvert = m->flex_vertnum[0]; for (int i = 0; i < nvert; ++i) { avg_z += d->flexvert_xpos[3 * i + 2]; } avg_z /= nvert; // If lifted, avg_z should be significantly > 0.1 // If failed (slipped), avg_z should be near 0 (floor) // Specialized primitives (mjraw_BoxTriangle, mjraw_CapsuleTriangle) should // enable stable pinching, so we expect the cloth to be lifted. EXPECT_GT(avg_z, 0.2) << "Cloth slipped out of gripper!"; mj_deleteData(d); mj_deleteModel(m); } TEST_F(MjCollisionTest, MarginSumming) { // Two spheres with size 0.1, placed 0.21 apart (distance of 0.01). // With margin summing, margin1 + margin2 = 0.00999 + 0.00999 = 0.01998 > 0.01 // so a contact should be generated. constexpr char xml[] = R"( )"; char error[1024]; mjModel* m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m, NotNull()) << error; mjData* d = mj_makeData(m); ASSERT_THAT(d, NotNull()); mj_fwdPosition(m, d); // With margin summing, we expect 1 contact EXPECT_EQ(d->ncon, 1); mj_deleteData(d); mj_deleteModel(m); } TEST_F(MjCollisionTest, MaxContact) { constexpr char xml[] = R"( )"; char error[1024]; mjModel* m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m, NotNull()) << error; mjData* d = mj_makeData(m); ASSERT_THAT(d, NotNull()); int mesh = mj_name2id(m, mjOBJ_GEOM, "mesh"); int box = mj_name2id(m, mjOBJ_GEOM, "box"); int plane = mj_name2id(m, mjOBJ_GEOM, "plane"); int sphere = mj_name2id(m, mjOBJ_GEOM, "sphere"); int capsule = mj_name2id(m, mjOBJ_GEOM, "capsule"); int ellipsoid = mj_name2id(m, mjOBJ_GEOM, "ellipsoid"); int cylinder = mj_name2id(m, mjOBJ_GEOM, "cylinder"); EXPECT_EQ(mj_maxContact(m, mesh, box, -1), 4); EXPECT_EQ(mj_maxContact(m, mesh, plane, -1), 3); EXPECT_EQ(mj_maxContact(m, box, plane, -1), 4); EXPECT_EQ(mj_maxContact(m, mesh, mesh, -1), 4); EXPECT_EQ(mj_maxContact(m, box, box, -1), 8); EXPECT_EQ(mj_maxContact(m, capsule, capsule, -1), 2); EXPECT_EQ(mj_maxContact(m, capsule, box, -1), 4); EXPECT_EQ(mj_maxContact(m, capsule, plane, -1), 2); EXPECT_EQ(mj_maxContact(m, cylinder, plane, -1), 4); EXPECT_EQ(mj_maxContact(m, sphere, sphere, -1), 1); EXPECT_EQ(mj_maxContact(m, sphere, capsule, -1), 1); EXPECT_EQ(mj_maxContact(m, sphere, box, -1), 1); EXPECT_EQ(mj_maxContact(m, sphere, mesh, -1), 1); EXPECT_EQ(mj_maxContact(m, sphere, plane, -1), 1); EXPECT_EQ(mj_maxContact(m, sphere, cylinder, -1), 1); EXPECT_EQ(mj_maxContact(m, ellipsoid, ellipsoid, -1), 1); EXPECT_EQ(mj_maxContact(m, ellipsoid, box, -1), 1); EXPECT_EQ(mj_maxContact(m, ellipsoid, mesh, -1), 1); EXPECT_EQ(mj_maxContact(m, ellipsoid, plane, -1), 1); EXPECT_EQ(mj_maxContact(m, ellipsoid, cylinder, -1), 1); EXPECT_EQ(mj_maxContact(m, ellipsoid, capsule, -1), 1); EXPECT_EQ(mj_maxContact(m, capsule, cylinder, -1), 5); EXPECT_EQ(mj_maxContact(m, capsule, mesh, -1), 5); EXPECT_EQ(mj_maxContact(m, cylinder, cylinder, -1), 5); EXPECT_EQ(mj_maxContact(m, cylinder, box, -1), 5); EXPECT_EQ(mj_maxContact(m, cylinder, mesh, -1), 5); mj_deleteData(d); mj_deleteModel(m); } } // namespace } // namespace mujoco