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

Remove libccd compatibility layer from native ccd.

Browse Source 
PiperOrigin-RevId: 671740643
Change-Id: If3c1097df5c505f70c2dc0c090a7f5a93a43c683
This commit is contained in:
Kyle Bayes
2024-09-06 06:55:01 -07:00
committed by Copybara-Service
parent fa156ed22f
commit 4fc6fb179b
5 changed files with 218 additions and 242 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_collision_gjk_test.cc
+77 -76
View File
@@ -47,28 +47,58 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
mjc_support(vec->v, (mjCCDObj*) obj, _dir->v);
}
mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
mjtNum x2[3]) {
mjtNum GeomDist(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},
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);
if (x1 != nullptr) mju_copy3(x1, obj1.x0);
if (x2 != nullptr) mju_copy3(x2, obj2.x0);
mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center,
mjc_support};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center,
mjc_support};
mjCCDStatus status;
mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2);
if (x1 != nullptr) mju_copy3(x1, status.x1);
if (x2 != nullptr) mju_copy3(x2, status.x2);
return dist;
}
// drop in replacement for ccdMPRPenetration taken from mjc_penetration
int PenetrationWrapper(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd,
ccd_real_t* depth, ccd_vec3_t* dir, ccd_vec3_t* pos) {
mjCCDConfig config;
mjCCDStatus status;
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},
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};
// set config
config.max_iterations = ccd->max_iterations,
config.tolerance = ccd->mpr_tolerance,
config.contacts = 1;
config.distances = 0; // no geom distances needed
mjtNum dist = mjc_ccd(&config, &status, obj1, obj2);
if (dist < 0) {
if (depth) *depth = -dist;
if (dir) {
mju_sub3(dir->v, status.x1, status.x2);
mju_normalize3(dir->v);
}
if (pos) {
pos->v[0] = 0.5 * (status.x1[0] + status.x2[0]);
pos->v[1] = 0.5 * (status.x1[1] + status.x2[1]);
pos->v[2] = 0.5 * (status.x1[2] + status.x2[2]);
}
return 0;
}
if (depth) *depth = 0;
if (dir) mju_zero3(dir->v);
if (pos) mju_zero3(dir->v);
return 1;
}
mjtNum Penetration(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}, mjc_center,
mjc_support};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center,
mjc_support};
ccd_t ccd;
// CCD_INIT(&ccd); // uncomment to run ccdMPRPenetration
ccd.mpr_tolerance = kTolerance;
@@ -82,7 +112,7 @@ mjtNum run_gjkPenetration(mjModel* m, mjData* d, int g1, int g2,
ccd_real_t depth;
ccd_vec3_t ccd_dir, ccd_pos;
int ret = mj_gjkPenetration(&obj1, &obj2, &ccd, &depth, &ccd_dir, &ccd_pos);
int ret = PenetrationWrapper(&obj1, &obj2, &ccd, &depth, &ccd_dir, &ccd_pos);
if (ret) return 0; // objects not colliding
if (dir) mju_copy3(dir, ccd_dir.v);
if (pos) mju_copy3(pos, ccd_pos.v);
@@ -116,7 +146,7 @@ TEST_F(MjGjkTest, SphereSphere) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum x1[3], x2[3];
mjtNum dist = run_gjk(model, data, geom1, geom2, x1, x2);
mjtNum dist = GeomDist(model, data, geom1, geom2, x1, x2);
EXPECT_EQ(dist, 1);
EXPECT_THAT(x1, ElementsAre(-.5, 0, 0));
@@ -125,60 +155,6 @@ TEST_F(MjGjkTest, SphereSphere) {
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBox) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<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"/>
</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_EQ(dist, 1);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBoxIntersect) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<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"/>
</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 = run_gjkPenetration(model, data, geom1, geom2, dir, pos);
EXPECT_NEAR(dist, -1, kTolerance);
EXPECT_NEAR(dir[0], 1, kTolerance);
EXPECT_NEAR(dir[1], 0, kTolerance);
EXPECT_NEAR(dir[2], 0, kTolerance);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, EllipsoidEllipsoid) {
static constexpr char xml[] = R"(
<mujoco>
@@ -197,13 +173,38 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) {
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);
mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_NEAR(dist, 0.7542, .0001);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBox) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<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"/>
</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 = GeomDist(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_EQ(dist, 1);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {
static constexpr char xml[] = R"(
<mujoco>
@@ -222,7 +223,7 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {
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);
mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_NEAR(dist, 0, kTolerance);
mj_deleteData(data);
@@ -247,7 +248,7 @@ TEST_F(MjGjkTest, CapsuleCapsule) {
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);
mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_NEAR(dist, 0.4711, .0001);
mj_deleteData(data);