chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add unit tests for NativeCCD to produce better test coverage.

Browse Source 
PiperOrigin-RevId: 677793306
Change-Id: I435865f00553c74d1d0c0fa86d9c41fb1e5497ff
This commit is contained in:
Kyle Bayes
2024-09-23 07:54:39 -07:00
committed by Copybara-Service
parent b5bcc91a03
commit 04f8403d8e
2 changed files with 185 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+46 -43
View File
@@ -372,16 +372,30 @@ static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const
// find the smallest distance, and use the corresponding barycentric coordinates // find the smallest distance, and use the corresponding barycentric coordinates
mjtNum dmin = mjMAXVAL; mjtNum dmin = mjMAXVAL;
if (!comp1) {
mjtNum lambda_2d[3], x[3];
S2D(lambda_2d, s2, s3, s4);
lincomb3(x, lambda_2d, s2, s3, s4);
mjtNum d = mju_dot3(x, x);
lambda[0] = 0;
lambda[1] = lambda_2d[0];
lambda[2] = lambda_2d[1];
lambda[3] = lambda_2d[2];
dmin = d;
}
if (!comp2) { if (!comp2) {
mjtNum lambda_2d[3], x[3]; mjtNum lambda_2d[3], x[3];
S2D(lambda_2d, s1, s3, s4); S2D(lambda_2d, s1, s3, s4);
lincomb3(x, lambda_2d, s1, s3, s4); lincomb3(x, lambda_2d, s1, s3, s4);
mjtNum d = mju_dot3(x, x); mjtNum d = mju_dot3(x, x);
lambda[0] = lambda_2d[0]; if (d < dmin) {
lambda[1] = 0; lambda[0] = lambda_2d[0];
lambda[2] = lambda_2d[1]; lambda[1] = 0;
lambda[3] = lambda_2d[2]; lambda[2] = lambda_2d[1];
dmin = d; lambda[3] = lambda_2d[2];
dmin = d;
}
} }
if (!comp3) { if (!comp3) {
@@ -411,20 +425,6 @@ static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const
dmin = d; dmin = d;
} }
} }
if (!comp1) {
mjtNum lambda_2d[3], x[3];
S2D(lambda_2d, s2, s3, s4);
lincomb3(x, lambda_2d, s2, s3, s4);
mjtNum d = mju_dot3(x, x);
if (d < dmin) {
lambda[0] = 0;
lambda[1] = lambda_2d[0];
lambda[2] = lambda_2d[1];
lambda[3] = lambda_2d[2];
dmin = d;
}
}
} }
@@ -520,15 +520,28 @@ static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const
// find the smallest distance, and use the corresponding barycentric coordinates // find the smallest distance, and use the corresponding barycentric coordinates
mjtNum dmin = mjMAXVAL; mjtNum dmin = mjMAXVAL;
if (!comp1) {
mjtNum lambda_1d[2], x[3];
S1D(lambda_1d, s2, s3);
lincomb2(x, lambda_1d, s2, s3);
mjtNum d = mju_dot3(x, x);
lambda[0] = 0;
lambda[1] = lambda_1d[0];
lambda[2] = lambda_1d[1];
dmin = d;
}
if (!comp2) { if (!comp2) {
mjtNum lambda_1d[2], x[3]; mjtNum lambda_1d[2], x[3];
S1D(lambda_1d, s1, s3); S1D(lambda_1d, s1, s3);
lincomb2(x, lambda_1d, s1, s3); lincomb2(x, lambda_1d, s1, s3);
mjtNum d = mju_dot3(x, x); mjtNum d = mju_dot3(x, x);
lambda[0] = lambda_1d[0]; if (d < dmin) {
lambda[1] = 0; lambda[0] = lambda_1d[0];
lambda[2] = lambda_1d[1]; lambda[1] = 0;
dmin = d; lambda[2] = lambda_1d[1];
dmin = d;
}
} }
if (!comp3) { if (!comp3) {
@@ -543,19 +556,6 @@ static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const
dmin = d; dmin = d;
} }
} }
if (!comp1) {
mjtNum lambda_1d[2], x[3];
S1D(lambda_1d, s2, s3);
lincomb2(x, lambda_1d, s2, s3);
mjtNum d = mju_dot3(x, x);
if (d < dmin) {
lambda[0] = 0;
lambda[1] = lambda_1d[0];
lambda[2] = lambda_1d[1];
dmin = d;
}
}
} }
@@ -924,16 +924,19 @@ void heapify(Polytope* pt, int i) {
// delete face from heap // delete face from heap
void deleteFace(Polytope* pt, Face* face) { void deleteFace(Polytope* pt, Face* face) {
// SHOULD NOT OCCUR
if (!pt->nheap) { if (!pt->nheap) {
mju_warning("EPA: trying to delete face from empty polytope");
return; return;
} }
face->dist = -1; face->dist = -1;
pt->nheap--; pt->nheap--;
// SHOULD NOT OCCUR
// last face; nothing to do // last face; nothing to do
if (!pt->nheap) { if (!pt->nheap) {
return; return; // EPA will flag a warning
} }
// bubble up face to top of heap // bubble up face to top of heap
@@ -1123,7 +1126,7 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o
for (k = 0; k < kmax; k++) { for (k = 0; k < kmax; k++) {
// find the face closest to the origin // find the face closest to the origin
if (!pt->nheap) { if (!pt->nheap) {
mju_warning("EPA: empty polytope (most likely a bug)"); mju_warning("EPA: empty polytope");
mj_freeStack(d); mj_freeStack(d);
return 0; // assume 0 depth return 0; // assume 0 depth
} }
@@ -1133,7 +1136,7 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o
// check if dist is 0 // check if dist is 0
if (dist <= 0) { if (dist <= 0) {
mju_warning("EPA: origin lies on affine hull of face (most likely a bug)"); mju_warning("EPA: origin lies on affine hull of face");
} }
// compute support point w from the closest face's normal // compute support point w from the closest face's normal
@@ -1158,11 +1161,11 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o
v2 = horFace->verts[(horEdge + 1) % 3]; v2 = horFace->verts[(horEdge + 1) % 3];
horFace->adj[horEdge] = nfaces; horFace->adj[horEdge] = nfaces;
if (attachFace(pt, wi, v2, v1, nfaces + nedges - 1, horIndex, nfaces + 1)) { if (attachFace(pt, wi, v2, v1, nfaces + nedges - 1, horIndex, nfaces + 1)) {
break; break; // out of memory
} }
// attach remaining faces // attach remaining faces
int exit = 0; int oom = 0; // set to 1 if out of memory
for (int i = 1; i < nedges; i++) { for (int i = 1; i < nedges; i++) {
int cur = nfaces + i; // index of attached face int cur = nfaces + i; // index of attached face
int next = nfaces + (i + 1) % nedges; // index of next face int next = nfaces + (i + 1) % nedges; // index of next face
@@ -1173,11 +1176,11 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o
v2 = horFace->verts[(horEdge + 1) % 3]; v2 = horFace->verts[(horEdge + 1) % 3];
horFace->adj[horEdge] = cur; horFace->adj[horEdge] = cur;
if (attachFace(pt, wi, v2, v1, cur - 1, horIndex, next)) { if (attachFace(pt, wi, v2, v1, cur - 1, horIndex, next)) {
exit = 1; oom = 1;
break; break;
} }
} }
if (exit) break; if (oom) break;
h.nedges = 0; // clear horizon h.nedges = 0; // clear horizon
} }
test/engine/engine_collision_gjk_test.cc
+139 -56
View File
@@ -36,6 +36,24 @@ using ::testing::ElementsAre;
constexpr mjtNum kTolerance = 1e-6; constexpr mjtNum kTolerance = 1e-6;
constexpr int kMaxIterations = 1000; constexpr int kMaxIterations = 1000;
constexpr char kEllipoid[] = R"(
<mujoco model="Ellipsoid Test">
<compiler angle="radian"/>
<size nkey="1"/>
<worldbody>
<geom name="geom1" size="0.1 0.1 0.1" pos="0 0 -0.1" type="ellipsoid"/>
<body pos="0 0 0.1">
<joint type="free" limited="false" actuatorfrclimited="false"/>
<geom name="geom2" size="0.01 0.02 0.1" type="ellipsoid"/>
</body>
</worldbody>
<keyframe>
<key time="1.446"
qpos="0.000886189 -0.0303047 0.0951303 0.98783 0.155468 0.00454524 1.60038e-06"
qvel="0.00769267 -0.258656 -0.0775641 2.73712 0.0813998 -0.000166485"/>
</keyframe>
</mujoco>)";
// ccd center function // ccd center function
void mjccd_center(const void *obj, ccd_vec3_t *center) { void mjccd_center(const void *obj, ccd_vec3_t *center) {
@@ -130,6 +148,72 @@ mjtNum Penetration(mjModel* m, mjData* d, int g1, int g2,
using MjGjkTest = MujocoTest; using MjGjkTest = MujocoTest;
TEST_F(MjGjkTest, SphereSphereDist) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body pos="-1.5 0 0">
<freejoint/>
<geom name="geom1" type="sphere" size="1"/>
</body>
<body pos="1.5 0 0">
<freejoint/>
<geom name="geom2" type="sphere" size="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, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum x1[3], x2[3];
mjtNum dist = GeomDist(model, data, geom1, geom2, x1, x2);
EXPECT_EQ(dist, 1);
EXPECT_THAT(x1, ElementsAre(-.5, 0, 0));
EXPECT_THAT(x2, ElementsAre(.5, 0, 0));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, SphereSphereNoDist) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body pos="-1.5 0 0">
<freejoint/>
<geom name="geom1" type="sphere" size="1"/>
</body>
<body pos="1.5 0 0">
<freejoint/>
<geom name="geom2" type="sphere" size="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, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dir[3], pos[3];
mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos);
EXPECT_EQ(dist, mjMAXVAL);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, SphereSphereIntersect) { TEST_F(MjGjkTest, SphereSphereIntersect) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -168,11 +252,11 @@ TEST_F(MjGjkTest, SphereSphereIntersect) {
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(MjGjkTest, BoxBoxIntersect) { TEST_F(MjGjkTest, BoxBoxDepth) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
<worldbody> <worldbody>
<geom name="geom1" type="box" pos="-1 0 0" size="2.5 2.5 2.5"/> <geom name="geom1" type="box" pos="-1 0 0" size="2.5 2.5 2.5"/>
<geom name="geom2" type="box" pos="1.5 0 0" size="1 1 1"/> <geom name="geom2" type="box" pos="1.5 0 0" size="1 1 1"/>
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
@@ -197,57 +281,33 @@ TEST_F(MjGjkTest, BoxBoxIntersect) {
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(MjGjkTest, EllipsoidEllipsoidTouching) { TEST_F(MjGjkTest, SmallBoxMesh) {
static constexpr char xml[] = R"()
<mujoco model="MuJoCo Model">
<compiler angle="radian"/>
<size nkey="1"/>
<worldbody>
<geom name="geom1" size="0.1 0.1 0.1" pos="0 0 -0.1" type="ellipsoid"/>
<body pos="0 0 0.1">
<joint type="free" limited="false" actuatorfrclimited="false"/>
<geom name="geom2" size="0.01 0.02 0.1" type="ellipsoid"/>
</body>
</worldbody>
<keyframe>
<key time="0.526"
qpos="7.11947e-07 -4.35075e-06 0.0955126 1 2.02101e-05 1.31548e-06 -4.92008e-09"
qvel="4.86327e-06 -3.27869e-05 -0.27468 0.000321014 2.68836e-05 -3.58931e-08"/>
</keyframe>
</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_resetDataKeyframe(model, data, 0);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dir[3], pos[3];
mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos);
EXPECT_NEAR(dist, -0.0044873597898091094, kTolerance);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, SphereSphere) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
<asset>
<mesh name="box" scale=".5 .5 .1"
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="smallbox" scale=".1 .1 .1"
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"/>
</asset>
<worldbody> <worldbody>
<body pos="-1.5 0 0"> <geom name="geom1" pos="0 0 -.1" mesh="box" type="mesh"/>
<freejoint/> <geom name="geom2" pos="0 0 .1" mesh="smallbox" type="mesh"/>
<geom name="geom1" type="sphere" size="1"/>
</body>
<body pos="1.5 0 0">
<freejoint/>
<geom name="geom2" type="sphere" size="1"/>
</body>
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
@@ -260,12 +320,35 @@ TEST_F(MjGjkTest, SphereSphere) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum x1[3], x2[3]; mjtNum dir[3], pos[3];
mjtNum dist = GeomDist(model, data, geom1, geom2, x1, x2); mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos);
EXPECT_EQ(dist, 1); EXPECT_NEAR(dist, 0, kTolerance);
EXPECT_THAT(x1, ElementsAre(-.5, 0, 0));
EXPECT_THAT(x2, ElementsAre(.5, 0, 0)); // direction
EXPECT_NEAR(dir[0], 0, kTolerance);
EXPECT_NEAR(dir[1], 0, kTolerance);
EXPECT_NEAR(dir[2], 1, kTolerance);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) {
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(kEllipoid, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_resetDataKeyframe(model, data, 0);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dir[3], pos[3];
mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos);
EXPECT_NEAR(dist, -0.00022548856248122027, kTolerance);
mj_deleteData(data); mj_deleteData(data);
mj_deleteModel(model); mj_deleteModel(model);
} }
@@ -299,7 +382,7 @@ TEST_F(MjGjkTest, BoxBox) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
<worldbody> <worldbody>
<geom name="geom1" type="box" pos="-1.5 .5 0" size="1 1 1"/> <geom name="geom1" type="box" pos="-1.5 .5 0" size="1 1 1"/>
<geom name="geom2" type="box" pos="1.5 0 0" size="1 1 1"/> <geom name="geom2" type="box" pos="1.5 0 0" size="1 1 1"/>
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";