Support edge contacts with multiccd in nativeccd.
PiperOrigin-RevId: 730489114 Change-Id: I02847d84bbc15a1526391d5e1b7109ab83ea4a59
This commit is contained in:
Kyle Bayes
committed by
Copybara-Service
Copybara-Service
parent
a6f1dc5e9e
commit
024616345d
@@ -1167,6 +1167,237 @@ TEST_F(MjGjkTest, MeshMeshPrune) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(MjGjkTest, BoxEdge) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option>
|
||||
<flag nativeccd="enable" multiccd="enable"/>
|
||||
</option>
|
||||
|
||||
<worldbody>
|
||||
<geom type="box" name="box1" size="5 5 .1" pos="0 0 0"/>
|
||||
<body pos="0 0 2">
|
||||
<freejoint/>
|
||||
<geom type="box" name="box2" size="1 1 1"/>
|
||||
</body>
|
||||
<body pos="0 0 4.4" euler="0 90 40">
|
||||
<freejoint/>
|
||||
<geom type="box" name="box3" size="1 1 1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>)";
|
||||
|
||||
std::array<char, 1000> error;
|
||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
mj_forward(model, data);
|
||||
|
||||
int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2");
|
||||
int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3");
|
||||
std::vector<mjtNum> dir, pos;
|
||||
mjtNum dist;
|
||||
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4);
|
||||
|
||||
EXPECT_EQ(ncons, 2);
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(MjGjkTest, BoxEdge2) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option>
|
||||
<flag nativeccd="enable" multiccd="enable"/>
|
||||
</option>
|
||||
|
||||
<worldbody>
|
||||
<geom type="box" name="box1" size="5 5 .1" pos="0 0 0"/>
|
||||
<body pos="0 0 2">
|
||||
<freejoint/>
|
||||
<geom type="box" name="box2" size="1 1 1"/>
|
||||
</body>
|
||||
|
||||
<body pos="0 0 4.4" euler="0 90 40">
|
||||
<freejoint/>
|
||||
<geom type="box" name="box3" size="1 1 1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>)";
|
||||
|
||||
std::array<char, 1000> error;
|
||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
mj_forward(model, data);
|
||||
|
||||
mjtNum* xmat = data->geom_xmat + 9;
|
||||
mjtNum* xpos = data->geom_xpos + 3;
|
||||
|
||||
xmat[0] = 0.9999979704374094557906005320546682924032;
|
||||
xmat[1] = -0.0017789363449516469497385662279498319549;
|
||||
xmat[2] = -0.0009457835609818190025430140188689165370;
|
||||
xmat[3] = 0.0017817418144636251123996695255868871755;
|
||||
xmat[4] = 0.9999939910254954655854930933855939656496;
|
||||
xmat[5] = 0.0029737701675293876438233020564894104609;
|
||||
xmat[6] = 0.0009404877299599626429629783963548561587;
|
||||
xmat[7] = -0.0029754492741947335607277658198199787876;
|
||||
xmat[8] = 0.9999951310803708581786963804916013032198;
|
||||
|
||||
xpos[0] = 0.0005578602979296120537716641152314878127;
|
||||
xpos[1] = 0.0098645950089783600300830102014515432529;
|
||||
xpos[2] = 1.1037596929447945903746131079969927668571;
|
||||
|
||||
xmat = data->geom_xmat + 18;
|
||||
xpos = data->geom_xpos + 6;
|
||||
|
||||
xmat[0] = 0.0006737475542006746490053537002040684456;
|
||||
xmat[1] = -0.0095603689585630827196816028390458086506;
|
||||
xmat[2] = 0.9999540716500983084102927023195661604404;
|
||||
xmat[3] = -0.1095658134726250898527410981841967441142;
|
||||
xmat[4] = 0.9939334085756179604231874691322445869446;
|
||||
xmat[5] = 0.0095766296438734854756802405972848646343;
|
||||
xmat[6] = -0.9939793149670246297233688892447389662266;
|
||||
xmat[7] = -0.1095672335264066821203243762283818796277;
|
||||
xmat[8] = -0.0003778293989772788311065632171903416747;
|
||||
|
||||
xpos[0] = -0.0218119359455731035013492657981259981170;
|
||||
xpos[1] = 0.9828851949225971829093850828940048813820;
|
||||
xpos[2] = 3.0930077345364814789263618877157568931580;
|
||||
|
||||
int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2");
|
||||
int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3");
|
||||
std::vector<mjtNum> dir, pos;
|
||||
mjtNum dist;
|
||||
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4);
|
||||
|
||||
EXPECT_EQ(ncons, 2);
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(MjGjkTest, BoxEdgeEdge) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option>
|
||||
<flag nativeccd="enable" multiccd="enable"/>
|
||||
</option>
|
||||
|
||||
<worldbody>
|
||||
<geom type="box" name="box1" size="5 5 .1" pos="0 0 -.1"/>
|
||||
<body pos="-2 0 2.99" euler="0 10 0">
|
||||
<freejoint/>
|
||||
<geom type="box" name="box2" size=".15 1 3"/>
|
||||
</body>
|
||||
<body pos="2 0 2.99" euler="0 -10 0">
|
||||
<freejoint/>
|
||||
<geom type="box" name="box3" size=".15 1 3"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>)";
|
||||
|
||||
std::array<char, 1000> error;
|
||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
mj_forward(model, data);
|
||||
|
||||
mjtNum* xmat = data->geom_xmat + 9;
|
||||
mjtNum* xpos = data->geom_xpos + 3;
|
||||
|
||||
xmat[0] = 0.9182779243587342321575306414160877466202;
|
||||
xmat[1] = -0.0000000000000000000364268564068890756444;
|
||||
xmat[2] = 0.3959364262547898638544552341045346111059;
|
||||
xmat[3] = -0.0000000000000000000591321577502441383525;
|
||||
xmat[4] = 1.0000000000000000000000000000000000000000;
|
||||
xmat[5] = 0.0000000000000000002291443915550544102718;
|
||||
xmat[6] = -0.3959364262547898638544552341045346111059;
|
||||
xmat[7] = -0.0000000000000000002338308114719865891040;
|
||||
xmat[8] = 0.9182779243587342321575306414160877466202;
|
||||
|
||||
xpos[0] = -1.3241298058948087756903078116010874509811;
|
||||
xpos[1] = 0.0000000000000000007148993364299687318184;
|
||||
xpos[2] = 2.8141526153588731773425024584867060184479;
|
||||
|
||||
xmat = data->geom_xmat + 18;
|
||||
xpos = data->geom_xpos + 6;
|
||||
|
||||
xmat[0] = 0.9182779243587342321575306414160877466202;
|
||||
xmat[1] = 0.0000000000000000000728398144756416399722;
|
||||
xmat[2] = -0.3959364262547898638544552341045346111059;
|
||||
xmat[3] = -0.0000000000000000001674060251593158490713;
|
||||
xmat[4] = 1.0000000000000000000000000000000000000000;
|
||||
xmat[5] = -0.0000000000000000002042889652712837518138;
|
||||
xmat[6] = 0.3959364262547898638544552341045346111059;
|
||||
xmat[7] = 0.0000000000000000002538761903338069406631;
|
||||
xmat[8] = 0.9182779243587342321575306414160877466202;
|
||||
|
||||
xpos[0] = 1.3241298058948089977349127366323955357075;
|
||||
xpos[1] = -0.0000000000000000008679606505055748997840;
|
||||
xpos[2] = 2.8141526153588731773425024584867060184479;
|
||||
|
||||
int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2");
|
||||
int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3");
|
||||
std::vector<mjtNum> dir, pos;
|
||||
mjtNum dist;
|
||||
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4);
|
||||
|
||||
EXPECT_EQ(ncons, 2);
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(MjGjkTest, MeshEdge) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option>
|
||||
<flag nativeccd="enable" multiccd="enable"/>
|
||||
</option>
|
||||
<asset>
|
||||
<mesh name="smallbox"
|
||||
vertex="-1 -1 -1 1 -1 -1 1 1 -1
|
||||
1 1 1 1 -1 1 -1 1 -1
|
||||
-1 1 1 -1 -1 1"/>
|
||||
<mesh name="floor"
|
||||
vertex="-1 -1 -1 1 -1 -1 1 1 -1
|
||||
1 1 1 1 -1 1 -1 1 -1
|
||||
-1 1 1 -1 -1 1"
|
||||
scale="5 5 1"/>
|
||||
</asset>
|
||||
<worldbody>
|
||||
<geom type="mesh" name="box1" mesh="floor" pos="0 0 0"/>
|
||||
<body pos="0 0 2">
|
||||
<freejoint/>
|
||||
<geom type="mesh" mesh="smallbox" name="box2" size="1 1 1"/>
|
||||
</body>
|
||||
<body pos="0 0 4.4" euler="0 90 40">
|
||||
<freejoint/>
|
||||
<geom type="mesh" mesh="smallbox" name="box3" size="1 1 1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>)";
|
||||
|
||||
std::array<char, 1000> error;
|
||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
mj_forward(model, data);
|
||||
|
||||
int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2");
|
||||
int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3");
|
||||
std::vector<mjtNum> dir, pos;
|
||||
mjtNum dist;
|
||||
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4);
|
||||
|
||||
EXPECT_EQ(ncons, 2);
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) {
|
||||
std::array<char, 1000> error;
|
||||
mjModel* model = LoadModelFromString(kEllipoid, error.data(), error.size());
|
||||
|
||||
Reference in New Issue
Block a user