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

Add heightfield collision support for GJK + EPA implementation.

Browse Source 
PiperOrigin-RevId: 663321119
Change-Id: I01a85ae15386c997bff4e9c6d71555a4380dfffe
This commit is contained in:
Kyle Bayes
2024-08-15 08:48:50 -07:00
committed by Copybara-Service
parent 4b88e9bebf
commit 64575f399b
4 changed files with 110 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+68 -58
View File
@@ -360,8 +360,10 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
int mjc_Convex(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
ccd_t ccd;
mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}};
mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0},
mjc_center, mjc_support};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0},
mjc_center, mjc_support};
// init ccd structure
mjc_initCCD(&ccd, m);
@@ -586,46 +588,45 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
//---------------------------- heightfield collisions ---------------------------------------------
// ccd prism object type
struct _mjtPrism {
mjtNum v[6][3];
};
typedef struct _mjtPrism mjtPrism;
// ccd prism support function
static void prism_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec) {
// prism support function
static void mjc_prism_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
int istart, ibest;
mjtNum best, tmp;
const mjtPrism* p = (const mjtPrism*)obj;
// find best vertex in halfspace determined by dir.z
istart = dir->v[2] < 0 ? 0 : 3;
istart = dir[2] < 0 ? 0 : 3;
ibest = istart;
best = mju_dot3(p->v[istart], dir->v);
best = mju_dot3(obj->prism[istart], dir);
for (int i=istart+1; i < istart+3; i++) {
if ((tmp = mju_dot3(p->v[i], dir->v)) > best) {
if ((tmp = mju_dot3(obj->prism[i], dir)) > best) {
ibest = i;
best = tmp;
}
}
// copy best point
mju_copy3(vec->v, p->v[ibest]);
mju_copy3(res, obj->prism[ibest]);
}
// ccd prism support function
static void mjccd_prism_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec) {
mjc_prism_support(vec->v, (mjCCDObj*) obj, dir->v);
}
// ccd prism center function
static void prism_center(const void *obj, ccd_vec3_t *center) {
const mjtPrism* p = (const mjtPrism*)obj;
// prism center function
static void mjc_prism_center(mjtNum res[3], const mjCCDObj* obj) {
// compute mean
mju_zero3(center->v);
mju_zero3(res);
for (int i=0; i < 6; i++) {
mju_addTo3(center->v, p->v[i]);
mju_addTo3(res, obj->prism[i]);
}
mju_scl3(center->v, center->v, 1.0/6.0);
mju_scl3(res, res, 1.0/6.0);
}
// ccd prism center function
static void mjccd_prism_center(const void *obj, ccd_vec3_t *center) {
mjc_prism_center(center->v, (const mjCCDObj*) obj);
}
@@ -636,17 +637,17 @@ static void prism_firstdir(const void* o1, const void* o2, ccd_vec3_t *vec) {
// add vertex to prism, count vertices
static void addVert(int* nvert, mjtPrism* prism, mjtNum x, mjtNum y, mjtNum z) {
static void addVert(int* nvert, mjCCDObj* obj, mjtNum x, mjtNum y, mjtNum z) {
// move old data
mju_copy3(prism->v[0], prism->v[1]);
mju_copy3(prism->v[1], prism->v[2]);
mju_copy3(prism->v[3], prism->v[4]);
mju_copy3(prism->v[4], prism->v[5]);
mju_copy3(obj->prism[0], obj->prism[1]);
mju_copy3(obj->prism[1], obj->prism[2]);
mju_copy3(obj->prism[3], obj->prism[4]);
mju_copy3(obj->prism[4], obj->prism[5]);
// add new vertex at last position
prism->v[2][0] = prism->v[5][0] = x;
prism->v[2][1] = prism->v[5][1] = y;
prism->v[5][2] = z;
obj->prism[2][0] = obj->prism[5][0] = x;
obj->prism[2][1] = obj->prism[5][1] = y;
obj->prism[5][2] = z;
// count
(*nvert)++;
@@ -665,12 +666,15 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
int ncol = m->hfield_ncol[hid];
int dr[2], cnt, rmin, rmax, cmin, cmax;
const float* data = m->hfield_data + m->hfield_adr[hid];
mjtPrism prism;
mjCCDObj obj1;
obj1.center = mjc_prism_center;
obj1.support = mjc_prism_support;
// ccd-related
ccd_vec3_t dirccd, vecccd;
ccd_real_t depth;
mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0},
mjc_center, mjc_support};
ccd_t ccd;
// point size1 to hfield size instead of geom1 size
@@ -713,32 +717,32 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
// get support point in +X
ccdVec3Set(&dirccd, 1, 0, 0);
mjccd_support(&obj, &dirccd, &vecccd);
mjccd_support(&obj2, &dirccd, &vecccd);
xmax = vecccd.v[0];
// get support point in -X
ccdVec3Set(&dirccd, -1, 0, 0);
mjccd_support(&obj, &dirccd, &vecccd);
mjccd_support(&obj2, &dirccd, &vecccd);
xmin = vecccd.v[0];
// get support point in +Y
ccdVec3Set(&dirccd, 0, 1, 0);
mjccd_support(&obj, &dirccd, &vecccd);
mjccd_support(&obj2, &dirccd, &vecccd);
ymax = vecccd.v[1];
// get support point in -Y
ccdVec3Set(&dirccd, 0, -1, 0);
mjccd_support(&obj, &dirccd, &vecccd);
mjccd_support(&obj2, &dirccd, &vecccd);
ymin = vecccd.v[1];
// get support point in +Z
ccdVec3Set(&dirccd, 0, 0, 1);
mjccd_support(&obj, &dirccd, &vecccd);
mjccd_support(&obj2, &dirccd, &vecccd);
zmax = vecccd.v[2];
// get support point in -Z
ccdVec3Set(&dirccd, 0, 0, -1);
mjccd_support(&obj, &dirccd, &vecccd);
mjccd_support(&obj2, &dirccd, &vecccd);
zmin = vecccd.v[2];
// box-box test
@@ -767,13 +771,13 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
// init ccd structure
mjc_initCCD(&ccd, m);
ccd.first_dir = prism_firstdir;
ccd.center1 = prism_center;
ccd.center1 = mjccd_prism_center;
ccd.center2 = mjccd_center;
ccd.support1 = prism_support;
ccd.support1 = mjccd_prism_support;
ccd.support2 = mjccd_support;
// geom margin needed for actual collision test
obj.margin = margin;
obj2.margin = margin;
// compute real-valued grid step, and triangulation direction
dx = (2.0*size1[0]) / (ncol-1);
@@ -782,7 +786,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
dr[1] = 0;
// set zbottom value using base size
prism.v[0][2] = prism.v[1][2] = prism.v[2][2] = -size1[3];
obj1.prism[0][2] = obj1.prism[1][2] = obj1.prism[2][2] = -size1[3];
// process all prisms in sub-grid
cnt = 0;
@@ -791,19 +795,20 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
for (int c=cmin; c <= cmax; c++) {
for (int i=0; i < 2; i++) {
// send vertex to prism constructor
addVert(&nvert, &prism, dx*c-size1[0], dy*(r+dr[i])-size1[1],
addVert(&nvert, &obj1, dx*c-size1[0], dy*(r+dr[i])-size1[1],
data[(r+dr[i])*ncol+c]*size1[2]+margin);
// check for enough vertices
if (nvert > 2) {
// prism height test
if (prism.v[3][2] < zmin && prism.v[4][2] < zmin && prism.v[5][2] < zmin) {
if (obj1.prism[3][2] < zmin && obj1.prism[4][2] < zmin
&& obj1.prism[5][2] < zmin) {
continue;
}
// run MPR, save contact
if (_mjCCDPENETRATION(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0 &&
!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
if (_mjCCDPENETRATION(&obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0
&& !ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates
con[cnt].dist = -depth;
mju_mulMatVec3(con[cnt].frame, mat1, dirccd.v);
@@ -1103,8 +1108,10 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) {
ccd_t ccd;
mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, {0, 0, 0}};
mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, {0, 0, 0}};
mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, {0, 0, 0},
mjc_center, mjc_support};
mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, {0, 0, 0},
mjc_center, mjc_support};
// init ccd structure
mjc_initCCD(&ccd, m);
@@ -1128,7 +1135,9 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
mjtNum vec[3], dx, dy;
mjtNum xmin, xmax, ymin, ymax, zmin, zmax;
int dr[2], cnt, rmin, rmax, cmin, cmax;
mjtPrism prism;
mjCCDObj obj1;
obj1.center = mjc_prism_center;
obj1.support = mjc_prism_support;
// get hfield info
int hid = m->geom_dataid[g];
@@ -1151,7 +1160,8 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
// ccd-related
ccd_vec3_t dirccd, vecccd;
ccd_real_t depth;
mjCCDObj obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, {0, 0, 0}};
mjCCDObj obj2 = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, {0, 0, 0},
mjc_center, mjc_support};
ccd_t ccd;
//------------------------------------- AABB computation, box-box test
@@ -1211,9 +1221,9 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
// init ccd structure
CCD_INIT(&ccd);
ccd.first_dir = prism_firstdir;
ccd.center1 = prism_center;
ccd.center1 = mjccd_prism_center;
ccd.center2 = mjccd_center;
ccd.support1 = prism_support;
ccd.support1 = mjccd_prism_support;
ccd.support2 = mjccd_support;
// set ccd parameters
@@ -1227,7 +1237,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
dr[1] = 0;
// set zbottom value using base size
prism.v[0][2] = prism.v[1][2] = prism.v[2][2] = -hsize[3];
obj1.prism[0][2] = obj1.prism[1][2] = obj1.prism[2][2] = -hsize[3];
// process all prisms in sub-grid
cnt = 0;
@@ -1236,18 +1246,18 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
for (int c=cmin; c <= cmax; c++) {
for (int k=0; k < 2; k++) {
// send vertex to prism constructor
addVert(&nvert, &prism, dx*c-hsize[0], dy*(r+dr[k])-hsize[1],
addVert(&nvert, &obj1, dx*c-hsize[0], dy*(r+dr[k])-hsize[1],
hdata[(r+dr[k])*ncol+c]*hsize[2]+margin);
// check for enough vertices
if (nvert > 2) {
// prism height test
if (prism.v[3][2] < zmin && prism.v[4][2] < zmin && prism.v[5][2] < zmin) {
if (obj1.prism[3][2] < zmin && obj1.prism[4][2] < zmin && obj1.prism[5][2] < zmin) {
continue;
}
// run MPR, save contact
if (_mjCCDPENETRATION(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0) {
if (_mjCCDPENETRATION(&obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0) {
if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates
con[cnt].dist = -depth;
src/engine/engine_collision_convex.h
+3
View File
@@ -52,6 +52,9 @@ struct _mjCCDObj {
mjtNum margin;
mjtNum rotate[4];
mjtNum x0[3]; // initial guess of the witness point
void (*center)(mjtNum res[3], const struct _mjCCDObj* obj);
void (*support)(mjtNum res[3], struct _mjCCDObj* obj, const mjtNum dir[3]);
mjtNum prism[6][3]; // for hfield
};
typedef struct _mjCCDObj mjCCDObj;
src/engine/engine_collision_gjk.c
+6 -6
View File
@@ -203,8 +203,8 @@ static void gjk_support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* ob
mju_scl3(dir, dir_neg, -1);
// compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
mjc_support(s1, obj1, dir);
mjc_support(s2, obj2, dir_neg);
obj1->support(s1, obj1, dir);
obj2->support(s2, obj2, dir_neg);
}
@@ -218,8 +218,8 @@ static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
mju_scl3(dir_neg, dir, -1);
// compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
mjc_support(s1, obj1, dir);
mjc_support(s2, obj2, dir_neg);
obj1->support(s1, obj1, dir);
obj2->support(s2, obj2, dir_neg);
}
@@ -1007,8 +1007,8 @@ int mj_gjkPenetration(const void *obj1, const void *obj2, const ccd_t *ccd,
mjCCDObj* o2 = (mjCCDObj*) obj2;
nearest.n[1] = 34;
mjc_center(o1->x0, o1);
mjc_center(o2->x0, o2);
o1->center(o1->x0, o1);
o2->center(o2->x0, o2);
config.max_iterations = ccd->max_iterations;
config.tolerance = ccd->mpr_tolerance;
test/engine/engine_collision_gjk_test.cc
+33 -4
View File
@@ -50,8 +50,10 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
mjtNum x2[3]) {
mjCCDConfig config = {kMaxIterations, kTolerance};
mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}};
mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0},
mjc_center, mjc_support};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0},
mjc_center, mjc_support};
mjc_center(obj1.x0, &obj1);
mjc_center(obj2.x0, &obj2);
mjtNum dist = mj_gjk(&config, &obj1, &obj2);
@@ -63,8 +65,10 @@ mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
mjtNum run_gjkPenetration(mjModel* m, mjData* d, int g1, int g2,
mjtNum dir[3] = nullptr, mjtNum pos[3] = nullptr) {
mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}};
mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0},
mjc_center, mjc_support};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0},
mjc_center, mjc_support};
ccd_t ccd;
ccd.mpr_tolerance = kTolerance;
ccd.epa_tolerance = kTolerance;
@@ -198,6 +202,31 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) {
mj_deleteModel(model);
}
TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="ellipsoid" pos="1.5 0 -.5" size="2.25 4.5 3"/>
<geom name="geom2" type="ellipsoid" pos="1.5 .5 .5" size="1.5 1.5 2.25"/>
</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 dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_NEAR(dist, 0, kTolerance);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, CapsuleCapsule) {
static constexpr char xml[] = R"(
<mujoco>