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

Support edge contacts with multiccd in nativeccd.

Browse Source 
PiperOrigin-RevId: 730489114
Change-Id: I02847d84bbc15a1526391d5e1b7109ab83ea4a59
This commit is contained in:
Kyle Bayes
2025-02-24 09:47:50 -08:00
committed by Copybara-Service
parent a6f1dc5e9e
commit 024616345d
7 changed files with 861 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+227 -47
View File
@@ -1572,6 +1572,11 @@ static mjtNum planeIntersect(mjtNum res[3], const mjtNum pn[3], mjtNum pd,
static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1, static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1,
const mjtNum* face2, int nface2, const mjtNum n[3], const mjtNum* face2, int nface2, const mjtNum n[3],
const mjtNum dir[3]) { const mjtNum dir[3]) {
// clipping face needs to be at least a triangle
if (nface1 < 3) {
return;
}
// compute plane normal and distance to plane for each vertex // compute plane normal and distance to plane for each vertex
mjtNum pn[3 * mjMAX_POLYVERT], pd[mjMAX_POLYVERT]; mjtNum pn[3 * mjMAX_POLYVERT], pd[mjMAX_POLYVERT];
for (int i = 0; i < nface1 - 1; i++) { for (int i = 0; i < nface1 - 1; i++) {
@@ -1658,11 +1663,23 @@ static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1,
} }
// no pruning needed // no pruning needed
status->nx = npolygon; int k = 0;
for (int i = 0; i < 3*npolygon; i += 3) { for (int i = 0; i < 3*npolygon; i += 3) {
copy3(status->x2 + i, polygon + i); int skip = 0;
sub3(status->x1 + i, status->x2 + i, dir);
// find possible duplicate vertices
for (int j = 0; j < k; j += 3) {
if (equal3(status->x2 + j, polygon + i)) {
skip = 1;
break;
}
}
if (skip) continue;
copy3(status->x2 + k, polygon + i);
sub3(status->x1 + k, status->x2 + k, dir);
k += 3;
} }
status->nx = k/3;
} }
@@ -1768,6 +1785,56 @@ static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj,
// compute normal directional vectors along possible edges given by up to two vertices
static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj,
const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) {
// mesh data
int g = obj->geom;
const mjtNum* mat = obj->data->geom_xmat + 9*g;
const mjtNum* pos = obj->data->geom_xpos + 3*g;
// only one edge
if (dim == 2) {
copy3(endverts, v2);
sub3(res, v2, v1);
mju_normalize3(res);
return 1;
}
if (dim == 1) {
const mjModel* m = obj->model;
int polyadr = m->mesh_polyadr[m->geom_dataid[g]];
int vertadr = m->mesh_vertadr[m->geom_dataid[g]];
int v1_adr = m->mesh_polymapadr[vertadr + v1i];
int v1_num = m->mesh_polymapnum[vertadr + v1i];
if (v1_num > mjMAX_POLYVERT) v1_num = mjMAX_POLYVERT;
// loop through all faces with vertex v1
for (int i = 0; i < v1_num; i++) {
int idx = m->mesh_polymap[v1_adr + i];
int adr = m->mesh_polyvertadr[polyadr + idx];
int nvert = m->mesh_polyvertnum[polyadr + idx];
// find previous vertex in polygon to form edge
for (int j = 0; j < nvert; j++) {
int v = m->mesh_polyvert[adr + j];
if (v == v1i) {
float* verts = m->mesh_vert + 3*vertadr;
int k = (j == 0) ? nvert - 1 : j - 1;
float* vert = verts + 3*k;
globalcoord(endverts + 3*i, mat, pos, vert[0], vert[1], vert[2]);
sub3(res + 3*i, endverts + 3*i, v1);
mju_normalize3(res + 3*i);
}
}
}
return 3;
}
return 0;
}
// compute possible face normals of a box given up to 3 vertices // compute possible face normals of a box given up to 3 vertices
static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj, static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
int v1, int v2, int v3) { int v1, int v2, int v3) {
@@ -1825,6 +1892,45 @@ static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
// compute possible edge normals for box for edge collisions
static int boxEdgeNormals(mjtNum res[9], mjtNum endverts[9], int dim, mjCCDObj* obj,
const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) {
// box data
int g = 3*obj->geom;
const mjtNum* mat = obj->data->geom_xmat + 3*g;
const mjtNum* pos = obj->data->geom_xpos + g;
const mjtNum* size = obj->model->geom_size + g;
if (dim == 2) {
copy3(endverts, v2);
sub3(res, v2, v1);
mju_normalize3(res);
return 1;
}
// return 3 adjacent vertices
if (dim == 1) {
mjtNum x = (v1i & 1) ? size[0] : -size[0];
mjtNum y = (v1i & 2) ? size[1] : -size[1];
mjtNum z = (v1i & 4) ? size[2] : -size[2];
globalcoord(endverts, mat, pos, -x, y, z);
sub3(res, endverts, v1);
mju_normalize3(res);
globalcoord(endverts + 3, mat, pos, x, -y, z);
sub3(res + 3, endverts + 3, v1);
mju_normalize3(res + 3);
globalcoord(endverts + 6, mat, pos, x, y, -z);
sub3(res + 6, endverts + 6, v1);
mju_normalize3(res + 6);
return 3;
}
return 0;
}
// recover face of a box from its index // recover face of a box from its index
static int boxFace(mjtNum res[12], mjCCDObj* obj, int idx) { static int boxFace(mjtNum res[12], mjCCDObj* obj, int idx) {
// box data // box data
@@ -1903,11 +2009,11 @@ static int meshFace(mjtNum* res, mjCCDObj* obj, int idx) {
// find two normals that are facing each other within a tolerance, return 1 if found // find two normals that are facing each other within a tolerance, return 1 if found
static inline int alignedNormals(int res[2], const mjtNum* v, int nv, static inline int alignedFaces(int res[2], const mjtNum* v, int nv,
const mjtNum* w, int nw) { const mjtNum* w, int nw) {
for (int i = 0; i < nv; i++) { for (int i = 0; i < nv; i++) {
for (int j = 0; j < nw; j++) { for (int j = 0; j < nw; j++) {
if (dot3(v + 3*i, w + 3*j) < -mjCOSINE_TOL) { if (dot3(v + 3*i, w + 3*j) < -mjFACE_TOL) {
res[0] = i; res[0] = i;
res[1] = j; res[1] = j;
return 1; return 1;
@@ -1919,16 +2025,33 @@ static inline int alignedNormals(int res[2], const mjtNum* v, int nv,
// return number of dimensions of a feature (1, 2 or 3) // find two normals that are perpendicular to each other within a tolerance, return 1 if found
static inline int simplexDim(int* v1, int* v2, int* v3) { static inline int alignedFaceEdge(int res[2], const mjtNum* edge, int nedge,
int val1 = *v1; const mjtNum* face, int nface) {
int val2 = *v2; for (int i = 0; i < nface; i++) {
int val3 = *v3; for (int j = 0; j < nedge; j++) {
if (mju_abs(dot3(edge + 3*j, face + 3*i)) < mjEDGE_TOL) {
res[0] = j;
res[1] = i;
return 1;
}
}
}
return 0;
}
// return number (1, 2 or 3) of dimensions of a simplex; reorder vertices if necessary
static inline int simplexDim(int* v1i, int* v2i, int* v3i, mjtNum** v1, mjtNum** v2, mjtNum** v3) {
int val1 = *v1i;
int val2 = *v2i;
int val3 = *v3i;
if (val1 != val2) { if (val1 != val2) {
return (val3 == val1 || val3 == val2) ? 2 : 3; return (val3 == val1 || val3 == val2) ? 2 : 3;
} }
if (val1 != val3) { if (val1 != val3) {
*v2i = *v3i;
*v2 = *v3; *v2 = *v3;
return 2; return 2;
} }
@@ -1940,65 +2063,122 @@ static inline int simplexDim(int* v1, int* v2, int* v3) {
// recover multiple contacts from EPA polytope // recover multiple contacts from EPA polytope
static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
mjCCDObj* obj1, mjCCDObj* obj2) { mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum face1[mjMAX_POLYVERT * 3], face2[mjMAX_POLYVERT * 3]; mjtNum face1[mjMAX_POLYVERT * 3], face2[mjMAX_POLYVERT * 3], endverts[mjMAX_POLYVERT * 3];
// get vertices of faces from EPA // get vertices of faces from EPA
int v11 = pt->verts[face->verts[0]].index1; int v11i = pt->verts[face->verts[0]].index1;
int v12 = pt->verts[face->verts[1]].index1; int v12i = pt->verts[face->verts[1]].index1;
int v13 = pt->verts[face->verts[2]].index1; int v13i = pt->verts[face->verts[2]].index1;
int v21 = pt->verts[face->verts[0]].index2; int v21i = pt->verts[face->verts[0]].index2;
int v22 = pt->verts[face->verts[1]].index2; int v22i = pt->verts[face->verts[1]].index2;
int v23 = pt->verts[face->verts[2]].index2; int v23i = pt->verts[face->verts[2]].index2;
mjtNum* v11 = pt->verts[face->verts[0]].vert1;
mjtNum* v12 = pt->verts[face->verts[1]].vert1;
mjtNum* v13 = pt->verts[face->verts[2]].vert1;
mjtNum* v21 = pt->verts[face->verts[0]].vert2;
mjtNum* v22 = pt->verts[face->verts[1]].vert2;
mjtNum* v23 = pt->verts[face->verts[2]].vert2;
// get dimensions of features of geoms 1 and 2 // get dimensions of features of geoms 1 and 2
int nface1 = simplexDim(&v11, &v12, &v13); int nface1 = simplexDim(&v11i, &v12i, &v13i, &v11, &v12, &v13);
int nface2 = simplexDim(&v21, &v22, &v23); int nface2 = simplexDim(&v21i, &v22i, &v23i, &v21, &v22, &v23);
int nnorms1 = 0, nnorms2 = 0; int nnorms1 = 0, nnorms2 = 0;
mjtNum n1[3 * mjMAX_POLYVERT], n2[3 * mjMAX_POLYVERT]; // normals of possible face collisions mjtNum n1[3 * mjMAX_POLYVERT], n2[3 * mjMAX_POLYVERT]; // normals of possible face collisions
int idx1[mjMAX_POLYVERT], idx2[mjMAX_POLYVERT]; // indices of faces int idx1[mjMAX_POLYVERT], idx2[mjMAX_POLYVERT]; // indices of faces
// get all possible face normals for each geom // get all possible face normals for each geom
if (obj1->geom_type == mjGEOM_BOX) { if (obj1->geom_type == mjGEOM_BOX) {
nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11, v12, v13); nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i);
} else if (obj1->geom_type == mjGEOM_MESH) { } else if (obj1->geom_type == mjGEOM_MESH) {
nnorms1 = meshNormals(n1, idx1, nface1, obj1, v11, v12, v13); nnorms1 = meshNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i);
} }
if (obj2->geom_type == mjGEOM_BOX) { if (obj2->geom_type == mjGEOM_BOX) {
nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21, v22, v23); nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i);
} else if (obj2->geom_type == mjGEOM_MESH) { } else if (obj2->geom_type == mjGEOM_MESH) {
nnorms2 = meshNormals(n2, idx2, nface2, obj2, v21, v22, v23); nnorms2 = meshNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i);
} }
// determine if any two normals match // determine if any two face normals match
int res[2]; int res[2], edgecon1 = 0, edgecon2 = 0;
if (!alignedNormals(res, n1, nnorms1, n2, nnorms2)) { if (!alignedFaces(res, n1, nnorms1, n2, nnorms2)) {
return; // check if edge-face collision
if (nface1 < 3 && nface1 <= nface2) {
nnorms1 = 0;
if (obj1->geom_type == mjGEOM_BOX) {
nnorms1 = boxEdgeNormals(n1, endverts, nface1, obj1, v11, v12, v11i, v12i);
} else if (obj1->geom_type == mjGEOM_MESH) {
nnorms1 = meshEdgeNormals(n1, endverts, nface1, obj1, v11, v12, v11i, v12i);
}
if (!alignedFaceEdge(res, n1, nnorms1, n2, nnorms2)) return;
edgecon1 = 1;
// check if face-edge collision
} else if (nface2 < 3) {
nnorms2 = 0;
if (obj2->geom_type == mjGEOM_BOX) {
nnorms2 = boxEdgeNormals(n2, endverts, nface2, obj2, v21, v22, v21i, v22i);
} else if (obj2->geom_type == mjGEOM_MESH) {
nnorms2 = meshEdgeNormals(n2, endverts, nface2, obj2, v21, v22, v21i, v22i);
}
if (!alignedFaceEdge(res, n2, nnorms2, n1, nnorms1)) return;
edgecon2 = 1;
} else {
// no multi-contact
return;
}
} }
int i = res[0], j = res[1]; int i = res[0], j = res[1];
// recover matching faces // recover geom1 matching edge or face
if (obj1->geom_type == mjGEOM_BOX) { if (edgecon1) {
nface1 = boxFace(face1, obj1, idx1[i]); copy3(face1, pt->verts[face->verts[0]].vert1);
} else if (obj1->geom_type == mjGEOM_MESH) { copy3(face1 + 3, endverts + 3*i);
nface1 = meshFace(face1, obj1, idx1[i]); nface1 = 2;
} } else {
if (obj2->geom_type == mjGEOM_BOX) { if (obj1->geom_type == mjGEOM_BOX) {
nface2 = boxFace(face2, obj2, idx2[j]); int ind = (edgecon2 ? idx1[j] : idx1[i]);
} else if (obj2->geom_type == mjGEOM_MESH) { nface1 = boxFace(face1, obj1, ind);
nface2 = meshFace(face2, obj2, idx2[j]); } else if (obj1->geom_type == mjGEOM_MESH) {
int ind = (edgecon2 ? idx1[j] : idx1[i]);
nface1 = meshFace(face1, obj1, ind);
}
} }
if (nface1 >= 3 && nface2 >= 3) { // recover geom2 matching edge or face
// TODO(kylebayes): this approximates the contact direction, by scaling the face normal by the if (edgecon2) {
// single contact direction's magnitude. This is effective, but polygonClip should compute copy3(face2, pt->verts[face->verts[0]].vert2);
// this for each contact point. copy3(face2 + 3, endverts + 3*i);
mjtNum diff[3], approx_dir[3]; nface2 = 2;
sub3(diff, status->x2, status->x1); } else {
if (obj2->geom_type == mjGEOM_BOX) {
nface2 = boxFace(face2, obj2, idx2[j]);
} else if (obj2->geom_type == mjGEOM_MESH) {
nface2 = meshFace(face2, obj2, idx2[j]);
}
}
// TODO(kylebayes): this approximates the contact direction, by scaling the face normal by the
// single contact direction's magnitude. This is effective, but polygonClip should compute
// this for each contact point.
mjtNum diff[3], approx_dir[3];
sub3(diff, status->x2, status->x1);
// face1 is an edge; clip face1 against face2
if (edgecon1) {
scl3(approx_dir, n2 + 3*j, norm3(diff)); scl3(approx_dir, n2 + 3*j, norm3(diff));
polygonClip(status, face2, nface2, face1, nface1, n2 + 3*j, approx_dir);
// clip the faces and store the results in status return;
polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir);
} }
// face2 is an edge; clip face2 against face1
if (edgecon2) {
scl3(approx_dir, n1 + 3*j, -norm3(diff));
polygonClip(status, face1, nface1, face2, nface2, n1 + 3*j, approx_dir);
return;
}
// face-face collision
scl3(approx_dir, n2 + 3*j, norm3(diff));
polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir);
} }
src/engine/engine_collision_gjk.h
+5 -2
View File
@@ -25,8 +25,11 @@
extern "C" { extern "C" {
#endif #endif
// tolerance for considering two normals to be aligned // tolerance for normal alignment of two faces (cosine of 1.6e-3)
#define mjCOSINE_TOL 0.99999872 #define mjFACE_TOL 0.99999872
// tolerance for edge-face alignment (sine of 1.6e-3)
#define mjEDGE_TOL 0.00159999931
// max number of supported vertices in a polygon face of a mesh // max number of supported vertices in a polygon face of a mesh
#define mjMAX_POLYVERT 150 #define mjMAX_POLYVERT 150
test/engine/engine_collision_gjk_test.cc
+231
View File
@@ -1167,6 +1167,237 @@ TEST_F(MjGjkTest, MeshMeshPrune) {
mj_deleteModel(model); 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) { TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) {
std::array<char, 1000> error; std::array<char, 1000> error;
mjModel* model = LoadModelFromString(kEllipoid, error.data(), error.size()); mjModel* model = LoadModelFromString(kEllipoid, error.data(), error.size());
test/engine/testdata/collision_convex/dice.xml
Vendored
+46
View File
@@ -0,0 +1,46 @@
<mujoco>
<option>
<flag multiccd="enable"/>
</option>
<visual>
<scale forcewidth="0.01" contactwidth="0.05" contactheight="0.05"/>
<map force="0.1"/>
</visual>
<asset>
<mesh name="die" file="die.obj" scale=".02 .02 .02"/>
</asset>
<default>
<geom condim="1" type="mesh" solref="2e-5 1"/>
<default class="wall">
<geom type="box" rgba="0.1 0.2 1 .2"/>
</default>
<default class="floor">
<geom type="box" rgba="0.1 0.2 1 1"/>
</default>
</default>
<worldbody>
<light pos="0 0 1" dir="0 0 -1" castshadow="false"/>
<body name="container" pos="0 0 -0.11">
<geom class="floor" size="0.27 0.27 0.01"/>>
<geom class="wall" size="0.26 0.01 0.10" pos="0.01 0.26 0.11"/>
<geom class="wall" size="0.26 0.01 0.10" pos="-0.01 -0.26 0.11"/>
<geom class="wall" size="0.01 0.26 0.10" pos="0.26 -0.01 0.11"/>
<geom class="wall" size="0.01 0.26 0.10" pos="-0.26 0.01 0.11"/>
</body>
<replicate count="5" offset=".1 0 0">
<replicate count="5" offset="0 .1 0">
<replicate count="4" offset="0 0 .1">
<body pos="-.2 -.2 0.15">
<freejoint/>
<geom size=".02 .02 .02" type="mesh" mesh="die"/>
</body>
</replicate>
</replicate>
</replicate>
</worldbody>
</mujoco>
test/engine/testdata/collision_convex/die.obj
Vendored
+296
View File
@@ -0,0 +1,296 @@
v 0.740000 0.740000 -1.000000
v 0.740000 1.000000 -0.740000
v 1.000000 0.740000 -0.740000
v 0.740000 0.807269 -0.991147
v 0.740000 0.870021 -0.965154
v 0.740000 0.923848 -0.923848
v 0.807269 0.740000 -0.991147
v 0.809387 0.809958 -0.980606
v 0.808254 0.870420 -0.954318
v 0.806858 0.917583 -0.917748
v 0.870021 0.740000 -0.965154
v 0.870490 0.808646 -0.954150
v 0.863847 0.863908 -0.932116
v 0.858399 0.903670 -0.903688
v 0.890111 0.890111 -0.890111
v 0.807269 0.991147 -0.740000
v 0.870021 0.965154 -0.740000
v 0.923848 0.923848 -0.740000
v 0.740000 0.991147 -0.807269
v 0.809958 0.980606 -0.809387
v 0.870420 0.954318 -0.808254
v 0.917583 0.917748 -0.806858
v 0.740000 0.965154 -0.870021
v 0.808646 0.954150 -0.870490
v 0.863908 0.932116 -0.863847
v 0.903670 0.903688 -0.858399
v 0.991147 0.740000 -0.807269
v 0.965154 0.740000 -0.870021
v 0.923848 0.740000 -0.923848
v 0.991147 0.807269 -0.740000
v 0.980606 0.809387 -0.809958
v 0.954318 0.808254 -0.870420
v 0.917748 0.806858 -0.917583
v 0.965154 0.870021 -0.740000
v 0.954150 0.870490 -0.808646
v 0.932116 0.863847 -0.863908
v 0.903688 0.858399 -0.903670
v 0.740000 -1.000000 -0.740000
v 0.740000 -0.740000 -1.000000
v 1.000000 -0.740000 -0.740000
v 0.740000 -0.991147 -0.807269
v 0.740000 -0.965154 -0.870021
v 0.740000 -0.923848 -0.923848
v 0.807269 -0.991147 -0.740000
v 0.809387 -0.980606 -0.809958
v 0.808254 -0.954318 -0.870420
v 0.806858 -0.917748 -0.917583
v 0.870021 -0.965154 -0.740000
v 0.870490 -0.954150 -0.808646
v 0.863847 -0.932116 -0.863908
v 0.858399 -0.903688 -0.903670
v 0.890111 -0.890111 -0.890111
v 0.807269 -0.740000 -0.991147
v 0.870021 -0.740000 -0.965154
v 0.923848 -0.740000 -0.923848
v 0.740000 -0.807269 -0.991147
v 0.809958 -0.809387 -0.980606
v 0.870420 -0.808254 -0.954318
v 0.917583 -0.806858 -0.917748
v 0.740000 -0.870021 -0.965154
v 0.808646 -0.870490 -0.954150
v 0.863908 -0.863847 -0.932116
v 0.903670 -0.858399 -0.903688
v 0.991147 -0.807269 -0.740000
v 0.965154 -0.870021 -0.740000
v 0.923848 -0.923848 -0.740000
v 0.991147 -0.740000 -0.807269
v 0.980606 -0.809958 -0.809387
v 0.954318 -0.870420 -0.808254
v 0.917748 -0.917583 -0.806858
v 0.965154 -0.740000 -0.870021
v 0.954150 -0.808646 -0.870490
v 0.932116 -0.863908 -0.863847
v 0.903688 -0.903670 -0.858399
v 1.000000 0.740000 0.740000
v 0.740000 1.000000 0.740000
v 0.740000 0.740000 1.000000
v 0.991147 0.807269 0.740000
v 0.965154 0.870021 0.740000
v 0.923848 0.923848 0.740000
v 0.991147 0.740000 0.807269
v 0.980606 0.809958 0.809387
v 0.954318 0.870420 0.808254
v 0.917748 0.917583 0.806858
v 0.965154 0.740000 0.870021
v 0.954150 0.808646 0.870490
v 0.932116 0.863908 0.863847
v 0.903688 0.903670 0.858399
v 0.890111 0.890111 0.890111
v 0.740000 0.991147 0.807269
v 0.740000 0.965154 0.870021
v 0.740000 0.923848 0.923848
v 0.807269 0.991147 0.740000
v 0.809387 0.980606 0.809958
v 0.808254 0.954318 0.870420
v 0.806858 0.917748 0.917583
v 0.870021 0.965154 0.740000
v 0.870490 0.954150 0.808646
v 0.863847 0.932116 0.863908
v 0.858399 0.903688 0.903670
v 0.807269 0.740000 0.991147
v 0.870021 0.740000 0.965154
v 0.923848 0.740000 0.923848
v 0.740000 0.807269 0.991147
v 0.809958 0.809387 0.980606
v 0.870420 0.808254 0.954318
v 0.917583 0.806858 0.917748
v 0.740000 0.870021 0.965154
v 0.808646 0.870490 0.954150
v 0.863908 0.863847 0.932116
v 0.903670 0.858399 0.903688
v 1.000000 -0.740000 0.740000
v 0.740000 -0.740000 1.000000
v 0.740000 -1.000000 0.740000
v 0.991147 -0.740000 0.807269
v 0.965154 -0.740000 0.870021
v 0.923848 -0.740000 0.923848
v 0.991147 -0.807269 0.740000
v 0.980606 -0.809387 0.809958
v 0.954318 -0.808254 0.870420
v 0.917748 -0.806858 0.917583
v 0.965154 -0.870021 0.740000
v 0.954150 -0.870490 0.808646
v 0.932116 -0.863847 0.863908
v 0.903688 -0.858399 0.903670
v 0.890111 -0.890111 0.890111
v 0.740000 -0.807269 0.991147
v 0.740000 -0.870021 0.965154
v 0.740000 -0.923848 0.923848
v 0.807269 -0.740000 0.991147
v 0.809387 -0.809958 0.980606
v 0.808254 -0.870420 0.954318
v 0.806858 -0.917583 0.917748
v 0.870021 -0.740000 0.965154
v 0.870490 -0.808646 0.954150
v 0.863847 -0.863908 0.932116
v 0.858399 -0.903670 0.903688
v 0.807269 -0.991147 0.740000
v 0.870021 -0.965154 0.740000
v 0.923848 -0.923848 0.740000
v 0.740000 -0.991147 0.807269
v 0.809958 -0.980606 0.809387
v 0.870420 -0.954318 0.808254
v 0.917583 -0.917748 0.806858
v 0.740000 -0.965154 0.870021
v 0.808646 -0.954150 0.870490
v 0.863908 -0.932116 0.863847
v 0.903670 -0.903688 0.858399
v -0.740000 0.740000 -1.000000
v -1.000000 0.740000 -0.740000
v -0.740000 1.000000 -0.740000
v -0.807269 0.740000 -0.991147
v -0.870021 0.740000 -0.965154
v -0.923848 0.740000 -0.923848
v -0.740000 0.807269 -0.991147
v -0.809958 0.809387 -0.980606
v -0.870420 0.808254 -0.954318
v -0.917583 0.806858 -0.917748
v -0.740000 0.870021 -0.965154
v -0.808646 0.870490 -0.954150
v -0.863908 0.863847 -0.932116
v -0.903670 0.858399 -0.903688
v -0.890111 0.890111 -0.890111
v -0.991147 0.807269 -0.740000
v -0.965154 0.870021 -0.740000
v -0.923848 0.923848 -0.740000
v -0.991147 0.740000 -0.807269
v -0.980606 0.809958 -0.809387
v -0.954318 0.870420 -0.808254
v -0.917748 0.917583 -0.806858
v -0.965154 0.740000 -0.870021
v -0.954150 0.808646 -0.870490
v -0.932116 0.863908 -0.863847
v -0.903688 0.903670 -0.858399
v -0.740000 0.991147 -0.807269
v -0.740000 0.965154 -0.870021
v -0.740000 0.923848 -0.923848
v -0.807269 0.991147 -0.740000
v -0.809387 0.980606 -0.809958
v -0.808254 0.954318 -0.870420
v -0.806858 0.917748 -0.917583
v -0.870021 0.965154 -0.740000
v -0.870490 0.954150 -0.808646
v -0.863847 0.932116 -0.863908
v -0.858399 0.903688 -0.903670
v -1.000000 -0.740000 -0.740000
v -0.740000 -0.740000 -1.000000
v -0.740000 -1.000000 -0.740000
v -0.991147 -0.740000 -0.807269
v -0.965154 -0.740000 -0.870021
v -0.923848 -0.740000 -0.923848
v -0.991147 -0.807269 -0.740000
v -0.980606 -0.809387 -0.809958
v -0.954318 -0.808254 -0.870420
v -0.917748 -0.806858 -0.917583
v -0.965154 -0.870021 -0.740000
v -0.954150 -0.870490 -0.808646
v -0.932116 -0.863847 -0.863908
v -0.903688 -0.858399 -0.903670
v -0.890111 -0.890111 -0.890111
v -0.740000 -0.807269 -0.991147
v -0.740000 -0.870021 -0.965154
v -0.740000 -0.923848 -0.923848
v -0.807269 -0.740000 -0.991147
v -0.809387 -0.809958 -0.980606
v -0.808254 -0.870420 -0.954318
v -0.806858 -0.917583 -0.917748
v -0.870021 -0.740000 -0.965154
v -0.870490 -0.808646 -0.954150
v -0.863847 -0.863908 -0.932116
v -0.858399 -0.903670 -0.903688
v -0.807269 -0.991147 -0.740000
v -0.870021 -0.965154 -0.740000
v -0.923848 -0.923848 -0.740000
v -0.740000 -0.991147 -0.807269
v -0.809958 -0.980606 -0.809387
v -0.870420 -0.954318 -0.808254
v -0.917583 -0.917748 -0.806858
v -0.740000 -0.965154 -0.870021
v -0.808646 -0.954150 -0.870490
v -0.863908 -0.932116 -0.863847
v -0.903670 -0.903688 -0.858399
v -1.000000 0.740000 0.740000
v -0.740000 0.740000 1.000000
v -0.740000 1.000000 0.740000
v -0.991147 0.740000 0.807269
v -0.965154 0.740000 0.870021
v -0.923848 0.740000 0.923848
v -0.991147 0.807269 0.740000
v -0.980606 0.809387 0.809958
v -0.954318 0.808254 0.870420
v -0.917748 0.806858 0.917583
v -0.965154 0.870021 0.740000
v -0.954150 0.870490 0.808646
v -0.932116 0.863847 0.863908
v -0.903688 0.858399 0.903670
v -0.890111 0.890111 0.890111
v -0.740000 0.807269 0.991147
v -0.740000 0.870021 0.965154
v -0.740000 0.923848 0.923848
v -0.807269 0.740000 0.991147
v -0.809387 0.809958 0.980606
v -0.808254 0.870420 0.954318
v -0.806858 0.917583 0.917748
v -0.870021 0.740000 0.965154
v -0.870490 0.808646 0.954150
v -0.863847 0.863908 0.932116
v -0.858399 0.903670 0.903688
v -0.807269 0.991147 0.740000
v -0.870021 0.965154 0.740000
v -0.923848 0.923848 0.740000
v -0.740000 0.991147 0.807269
v -0.809958 0.980606 0.809387
v -0.870420 0.954318 0.808254
v -0.917583 0.917748 0.806858
v -0.740000 0.965154 0.870021
v -0.808646 0.954150 0.870490
v -0.863908 0.932116 0.863847
v -0.903670 0.903688 0.858399
v -0.740000 -1.000000 0.740000
v -0.740000 -0.740000 1.000000
v -1.000000 -0.740000 0.740000
v -0.740000 -0.991147 0.807269
v -0.740000 -0.965154 0.870021
v -0.740000 -0.923848 0.923848
v -0.807269 -0.991147 0.740000
v -0.809387 -0.980606 0.809958
v -0.808254 -0.954318 0.870420
v -0.806858 -0.917748 0.917583
v -0.870021 -0.965154 0.740000
v -0.870490 -0.954150 0.808646
v -0.863847 -0.932116 0.863908
v -0.858399 -0.903688 0.903670
v -0.890111 -0.890111 0.890111
v -0.807269 -0.740000 0.991147
v -0.870021 -0.740000 0.965154
v -0.923848 -0.740000 0.923848
v -0.740000 -0.807269 0.991147
v -0.809958 -0.809387 0.980606
v -0.870420 -0.808254 0.954318
v -0.917583 -0.806858 0.917748
v -0.740000 -0.870021 0.965154
v -0.808646 -0.870490 0.954150
v -0.863908 -0.863847 0.932116
v -0.903670 -0.858399 0.903688
v -0.991147 -0.807269 0.740000
v -0.965154 -0.870021 0.740000
v -0.923848 -0.923848 0.740000
v -0.991147 -0.740000 0.807269
v -0.980606 -0.809958 0.809387
v -0.954318 -0.870420 0.808254
v -0.917748 -0.917583 0.806858
v -0.965154 -0.740000 0.870021
v -0.954150 -0.808646 0.870490
v -0.932116 -0.863908 0.863847
v -0.903688 -0.903670 0.858399
test/engine/testdata/collision_convex/edge_edge.xml
Vendored
+24
View File
@@ -0,0 +1,24 @@
<mujoco>
<option>
<flag multiccd="enable"/>
</option>
<asset>
<texture type="skybox" builtin="gradient" width="512" height="512"
rgb1="1 1 1" rgb2="1 1 1"/>
<mesh name="domino" 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=".15 1 3"/>
</asset>
<worldbody>
<geom type="box" size="5 5 .1" pos="0 0 -.1"/>
<body pos="-2 0 2.99" euler="0 10 0">
<freejoint/>
<geom type="mesh" name="domino1" mesh="domino" rgba=".1 .1 .1 1"/>
</body>
<body pos="2 0 2.99" euler="0 -10 0">
<freejoint/>
<geom type="mesh" name="domino2" mesh="domino" rgba=".1 .1 .1 1"/>
</body>
</worldbody>
</mujoco>
test/engine/testdata/sensor/fromto_capsule_ellipsoid.xml
Vendored
+32
View File
@@ -0,0 +1,32 @@
<mujoco model="Capsule-Ellipsoid Distance">
<visual>
<headlight ambient=".7 .7 .7"/>
<rgba rangefinder="1 .2 .1 1"/>
</visual>
<option>
<flag gravity="disable"/>
</option>
<asset>
<texture type="skybox" builtin="gradient" width="512" height="512" rgb1="1 1 1" rgb2="1 1 1"/>
</asset>
<default>
<geom rgba=".1 .1 .1 1"/>
</default>
<worldbody>
<body pos="-.5 0 0">
<freejoint/>
<geom name="capsule" type="capsule" size=".15 .30"/>
</body>
<body pos=".5 0 0">
<freejoint/>
<geom name="ellipsoid" type="ellipsoid" size=".10 .10 .15"/>
</body>
</worldbody>
<sensor>
<fromto geom1="capsule" geom2="ellipsoid" cutoff="15"/>
</sensor>
</mujoco>