// Copyright 2022 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 sdf collisions. #include #include #include #include #include #include #include #include "src/engine/engine_collision_sdf.h" #include "test/fixture.h" namespace mujoco { namespace { using ::testing::NotNull; using SdfTest = MujocoTest; static constexpr int kpoints = 6; static constexpr int kgeoms = 5; static constexpr char kSdfModel[] = R"( )"; TEST_F(SdfTest, SdfPrimitive) { char error[1024]; mjModel* model = LoadModelFromString(kSdfModel, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); ASSERT_THAT(data, NotNull()); ASSERT_THAT(model->ngeom, kgeoms); mjSDF sdf; const mjpPlugin* null_plugin = NULL; mjtNum gradient[3], dist[kgeoms][kpoints] = { {0, 0, 0, 0, 1, 1}, // plane {-1, 0, 0, mju_sqrt(2)-1, mju_sqrt(2)-1, mju_sqrt(3)-1}, // sphere {(mjtNum)-.1, (mjtNum).9, (mjtNum).9, mju_sqrt(2)-(mjtNum).1, (mjtNum).9, mju_sqrt(2)-(mjtNum).1}, // capsule {-1, 0, 0, mju_sqrt(2)-1, 0, mju_sqrt(2)-1}, // cylinder {-mju_sqrt(3), 0, 0, 0, 0, 0}, // box }; mjtNum points[kpoints][3] = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {1, 1, 0}, {0, 1, 1}, {1, 1, 1}}; for (int i = 0; i < kgeoms; i++) { sdf.plugin = &null_plugin; sdf.id = &i; sdf.type = mjSDFTYPE_SINGLE; sdf.geomtype = (mjtGeom*)(model->geom_type+i); for (int j = 0; j < kpoints; j++) { EXPECT_NEAR(mjc_distance(model, data, &sdf, points[j]), dist[i][j], MjTol(1e-9, 5e-7)); mjc_gradient(model, data, &sdf, gradient, points[j]); } } mj_deleteData(data); mj_deleteModel(model); } static constexpr char kFlexSdfModel[] = R"( )"; TEST_F(SdfTest, FlexSdfCollision) { char error[1024]; mjModel* model = LoadModelFromString(kFlexSdfModel, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); ASSERT_THAT(data, NotNull()); // check there is at least one flex ASSERT_GT(model->nflex, 0); // simulate for a few steps to let flex fall onto torus for (int i = 0; i < 100; i++) { mj_step(model, data); } // should have contacts between flex and SDF EXPECT_GT(data->ncon, 0) << "Expected flex-SDF contacts"; // check that contacts involve the flex bool has_flex_contact = false; for (int i = 0; i < data->ncon; i++) { if (data->contact[i].flex[0] >= 0 || data->contact[i].flex[1] >= 0) { has_flex_contact = true; break; } } EXPECT_TRUE(has_flex_contact) << "Expected at least one flex contact"; mj_deleteData(data); mj_deleteModel(model); } } // namespace } // namespace mujoco