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

Add mj_geomDistance API function and sensors for geometric distance, normal direction and segment between between two geoms. Fixes #51

Browse Source 
PiperOrigin-RevId: 632458301
Change-Id: I65b7e5bb0df59008028114cf4a07e0d6365ff3d3
This commit is contained in:
Yuval Tassa
2024-05-10 04:51:46 -07:00
committed by Copybara-Service
parent 31de1dad9d
commit 02d015458c
29 changed files with 1002 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_sensor_test.cc
+82 -7
View File
@@ -14,6 +14,8 @@
// Tests for engine/engine_sensor.c.
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
@@ -44,7 +46,7 @@ using ::testing::StrEq;
using SensorTest = MujocoTest;
// --------------------- test sensor disableflag -----------------------------
// --------------------- test sensor disableflag ------------------------------
// hand-picked positions and orientations for simple expected values
TEST_F(SensorTest, DisableSensors) {
@@ -255,7 +257,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelLinearFixed) {
mj_deleteModel(model);
}
// object and reference in the same body, expect angular velocites to be zero
// object and reference in the same body, expect angular velocities to be zero
TEST_F(RelativeFrameSensorTest, FrameVelAngFixed) {
constexpr char xml[] = R"(
<mujoco>
@@ -279,7 +281,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelAngFixed) {
data->qvel[0] = 1;
mj_forward(model, data);
// obj and ref rotate together, relative angular velocites should be zero
// obj and ref rotate together, relative angular velocities should be zero
std::vector angvel = GetSensor(model, data, 0);
EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), {0, 0, 0}));
@@ -415,14 +417,14 @@ TEST_F(SensorTest, Clock) {
mjData* data = mj_makeData(model);
// call step 4 times, checking that clock works as expected
for (int i=0; i<5; i++) {
for (int i=0; i < 5; i++) {
mj_step(model, data);
mj_step1(model, data); // update values of position-based sensors
mj_step1(model, data); // update values of position-based sensors
EXPECT_EQ(data->sensordata[0], data->time);
EXPECT_EQ(data->sensordata[1], mju_min(data->time, 3e-3));
}
// chack names
// check names
const char* name0 = mj_id2name(model, mjOBJ_SENSOR, 0);
EXPECT_EQ(name0, nullptr);
const char* name1 = mj_id2name(model, mjOBJ_SENSOR, 1);
@@ -432,7 +434,80 @@ TEST_F(SensorTest, Clock) {
mj_deleteModel(model);
}
// ------------------------- camera sensor tests -----------------------------
// test clock sensor
TEST_F(SensorTest, CollisionSequential) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="plane" type="plane" size="1 1 1"/>
<geom name="sphere1" pos="0 0 1" size="0.2"/>
<geom name="sphere2" pos="1 0 1" size="0.3"/>
</worldbody>
<sensor>
<distance name="0" geom1="plane" geom2="sphere1" cutoff="1"/>
<distance name="1" geom1="sphere2" geom2="plane" cutoff="1"/>
<distance name="2" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<normal name="3" geom1="plane" geom2="sphere1" cutoff="1"/>
<normal name="4" geom1="sphere2" geom2="plane" cutoff="1"/>
<normal name="5" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<fromto name="6" geom1="plane" geom2="sphere1" cutoff="1"/>
<fromto name="7" geom1="sphere2" geom2="plane" cutoff="1"/>
<fromto name="8" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<!-- sequential sensors with identical signature -->
<distance name="9" geom1="plane" geom2="sphere1" cutoff="1"/>
<fromto name="10" geom1="plane" geom2="sphere1" cutoff="1"/>
<normal name="11" geom1="plane" geom2="sphere1" cutoff="1"/>
<normal name="12" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<fromto name="13" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<distance name="14" geom1="sphere1" geom2="sphere2" cutoff="1"/>
</sensor>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
mj_forward(model, data);
EXPECT_DOUBLE_EQ(data->sensordata[0], 0.8);
EXPECT_DOUBLE_EQ(data->sensordata[1], 0.7);
EXPECT_DOUBLE_EQ(data->sensordata[2], 0.5);
mjtNum eps = 1e-14;
EXPECT_THAT(GetSensor(model, data, 3),
Pointwise(DoubleNear(eps), std::vector<mjtNum>{0, 0, 1}));
EXPECT_THAT(GetSensor(model, data, 4),
Pointwise(DoubleNear(eps), std::vector<mjtNum>{0, 0, -1}));
EXPECT_THAT(GetSensor(model, data, 5),
Pointwise(DoubleNear(eps), std::vector<mjtNum>{1, 0, 0}));
EXPECT_THAT(GetSensor(model, data, 6),
Pointwise(DoubleNear(eps),
std::vector<mjtNum>{0, 0, 0, 0, 0, .8}));
EXPECT_THAT(GetSensor(model, data, 7),
Pointwise(DoubleNear(eps),
std::vector<mjtNum>{1, 0, .7, 1, 0, 0}));
EXPECT_THAT(GetSensor(model, data, 8),
Pointwise(DoubleNear(eps),
std::vector<mjtNum>{.2, 0, 1, .7, 0, 1}));
EXPECT_THAT(GetSensor(model, data, 9),
Pointwise(DoubleNear(eps), GetSensor(model, data, 0)));
EXPECT_THAT(GetSensor(model, data, 10),
Pointwise(DoubleNear(eps), GetSensor(model, data, 6)));
EXPECT_THAT(GetSensor(model, data, 11),
Pointwise(DoubleNear(eps), GetSensor(model, data, 3)));
EXPECT_THAT(GetSensor(model, data, 12),
Pointwise(DoubleNear(eps), GetSensor(model, data, 5)));
EXPECT_THAT(GetSensor(model, data, 13),
Pointwise(DoubleNear(eps), GetSensor(model, data, 8)));
EXPECT_THAT(GetSensor(model, data, 14),
Pointwise(DoubleNear(eps), GetSensor(model, data, 2)));
mj_deleteData(data);
mj_deleteModel(model);
}
// ------------------------- camera sensor tests ------------------------------
// test clock sensor
TEST_F(SensorTest, CameraProjection) {
test/engine/engine_support_test.cc
+76 -1
View File
@@ -34,7 +34,8 @@ std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
}
using ::testing::DoubleNear;
using ::testing::ContainsRegex;
using ::testing::Eq;
using ::testing::ContainsRegex; // NOLINT
using ::testing::MatchesRegex;
using ::testing::Pointwise;
using ::testing::ElementsAreArray;
@@ -647,5 +648,79 @@ TEST_F(SupportTest, MulMIsland) {
mj_deleteModel(model);
}
static constexpr char GeomDistanceTestingModel[] = R"(
<mujoco>
<asset>
<mesh name="box" scale=".1 .1 .1" vertex="0 0 0 1 0 0 0 1 0 1 1 0
0 0 1 1 0 1 0 1 1 1 1 1"/>
</asset>
<worldbody>
<geom type="plane" size="1 1 1"/>
<geom pos="0 0 1" size="0.2"/>
<geom pos="1 0 1" size="0.3"/>
<geom type="mesh" mesh="box"/>
</worldbody>
</mujoco>
)";
TEST_F(SupportTest, GeomDistance) {
mjModel* model = LoadModelFromString(GeomDistanceTestingModel);
mjData* data = mj_makeData(model);
mj_kinematics(model, data);
// plane-sphere, distmax too small
mjtNum distmax = 0.5;
EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, nullptr), 0.5);
mjtNum fromto[6];
EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, fromto), 0.5);
EXPECT_THAT(fromto, Pointwise(Eq(), std::vector<mjtNum>{0, 0, 0, 0, 0, 0}));
// plane-sphere
distmax = 1.0;
EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 0, 1, 1.0, fromto), 0.8);
mjtNum eps = 1e-12;
EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
std::vector<mjtNum>{0, 0, 0, 0, 0, 0.8}));
// sphere-plane
EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 1, 0, 1.0, fromto), 0.8);
EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
std::vector<mjtNum>{0, 0, 0.8, 0, 0, 0}));
// sphere-sphere
EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 1, 2, 1.0, fromto), 0.5);
EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
std::vector<mjtNum>{.2, 0, 1, .7, 0, 1}));
// sphere-sphere, flipped order
EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 2, 1, 1.0, fromto), 0.5);
EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
std::vector<mjtNum>{.7, 0, 1, .2, 0, 1}));
// TODO: b/339596989 - Improve the bounds below (mjc_Convex).
// mesh-sphere (close distmax)
distmax = 0.701;
eps = 1e-5;
EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto),
DoubleNear(0.7, eps));
eps = 1e-3;
EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
std::vector<mjtNum>{0, 0, .1, 0, 0, .8}));
// mesh-sphere (far distmax)
distmax = 1.0;
eps = 1e-3;
EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto),
DoubleNear(0.7, eps));
eps = 2e-2;
EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
std::vector<mjtNum>{0, 0, .1, 0, 0, .8}));
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/testdata/sensor/fromto_body_body.xml
Vendored
+29
View File
@@ -0,0 +1,29 @@
<mujoco model="Body-Body collisions fromto">
<option>
<flag gravity="disable"/>
</option>
<worldbody>
<light pos=".5 0 3"/>
<light pos="-.5 0 3"/>
<geom type="plane" size="2 2 .01"/>
<body name="1" pos="0 0 .3">
<freejoint/>
<replicate count="50" offset="0 0 0.01" euler="0 0 25">
<geom type="capsule" size=".01 .05" pos="0 -.23 -.2" euler="0 84.5 0" rgba="0 0 1 1"/>
</replicate>
</body>
<body name="2" pos=".6 0 .3" euler="0 0 90">
<freejoint/>
<replicate count="50" offset="0 0 0.01" euler="0 0 15">
<geom type="capsule" pos=".025 -.2 -.2" size=".032 .04" euler="-24 -12 2" rgba=".8 0 0 1"/>
</replicate>
</body>
</worldbody>
<sensor>
<fromto body1="1" body2="2" cutoff="1"/>
</sensor>
</mujoco>
test/engine/testdata/sensor/fromto_convex.xml
Vendored
+83
View File
@@ -0,0 +1,83 @@
<mujoco model="Convex-Convex collisions">
<option>
<flag gravity="disable"/>
</option>
<asset>
<mesh name="halfbox1" scale=".2 .3 .1"
vertex="0 0 0 1 0 0 0 1 0 1 1 0
0 0 1 1 0 1 0 1 1 "/>
<mesh name="halfbox2" scale=".2 .3 .1"
vertex="1 0 0 0 1 0 1 1 0
0 0 1 1 0 1 0 1 1 "/>
<hfield name="hfield" nrow="3" ncol="4" size="1 1 .1 .08"
elevation="1 0 1 2
0 2 1 1
1 0 1 0"/>
</asset>
<default>
<geom rgba=".2 .2 .6 1"/>
</default>
<worldbody>
<light pos=".5 0 3"/>
<light pos="-.5 0 3"/>
<geom name="hfield" type="hfield" hfield="hfield" rgba="0.5 0.5 0.5 1"/>
<body pos=".2 0 .6">
<freejoint/>
<geom name="sphere" type="sphere" size=".15"/>
</body>
<body pos="0 0 .3">
<freejoint/>
<geom name="ellipsoid" type="ellipsoid" size=".1 .1 .2"/>
</body>
<body pos=".4 0 .3">
<freejoint/>
<geom name="cylinder" type="cylinder" size=".1 .15"/>
</body>
<body pos="-.5 0 .3">
<freejoint/>
<geom name="box" type="box" size=".2 .15 .1"/>
</body>
<body pos="-.5 0 .6">
<freejoint/>
<geom name="mesh1" type="mesh" mesh="halfbox1"/>
</body>
<body pos="-.5 -.6 .3">
<freejoint/>
<geom name="mesh2" type="mesh" mesh="halfbox2"/>
</body>
</worldbody>
<sensor>
<fromto geom1="ellipsoid" geom2="box" cutoff="1"/>
<fromto geom1="ellipsoid" geom2="cylinder" cutoff="1"/>
<fromto geom1="ellipsoid" geom2="mesh2" cutoff="1"/>
<fromto geom1="ellipsoid" geom2="hfield" cutoff="1"/>
<fromto geom1="mesh1" geom2="sphere" cutoff="1"/>
<fromto geom1="mesh1" geom2="box" cutoff="1"/>
<fromto geom1="mesh1" geom2="mesh2" cutoff="1"/>
<fromto geom1="hfield" geom2="sphere" cutoff="1"/>
<fromto geom1="hfield" geom2="box" cutoff="1"/>
<fromto geom1="hfield" geom2="mesh1" cutoff="1"/>
<fromto geom1="ellipsoid" geom2="box" cutoff="0.2"/>
<fromto geom1="ellipsoid" geom2="cylinder" cutoff="0.2"/>
<fromto geom1="ellipsoid" geom2="mesh2" cutoff="0.2"/>
<fromto geom1="ellipsoid" geom2="hfield" cutoff="0.2"/>
<fromto geom1="mesh1" geom2="sphere" cutoff="0.2"/>
<fromto geom1="mesh1" geom2="box" cutoff="0.2"/>
<fromto geom1="mesh1" geom2="mesh2" cutoff="0.2"/>
<fromto geom1="hfield" geom2="sphere" cutoff="0.2"/>
<fromto geom1="hfield" geom2="box" cutoff="0.2"/>
<fromto geom1="hfield" geom2="mesh1" cutoff="0.2"/>
</sensor>
</mujoco>
test/engine/testdata/sensor/fromto_primitive.xml
Vendored
+71
View File
@@ -0,0 +1,71 @@
<mujoco model="Primitive collisions">
<asset>
<mesh name="halfbox" scale=".2 .3 .1"
vertex="0 0 0 1 0 0 0 1 0 1 1 0
0 0 1 1 0 1 0 1 1 "/>
</asset>
<default>
<geom rgba=".2 .2 .6 1"/>
</default>
<worldbody>
<light pos=".5 0 3"/>
<light pos="-.5 0 3"/>
<geom name="plane" type="plane" size="2 2 .01" rgba="0.5 0.5 0.5 1"/>
<body pos=".2 0 .6">
<freejoint/>
<geom name="sphere" type="sphere" size=".15"/>
</body>
<body pos="0 0 .3">
<freejoint/>
<geom name="capsule1" type="capsule" size=".1 .1"/>
</body>
<body pos="-.2 0 .4" euler="90 0 0">
<freejoint/>
<geom name="capsule2" type="capsule" size=".05 .1"/>
</body>
<body pos=".4 0 .3">
<freejoint/>
<geom name="cylinder" type="cylinder" size=".1 .15"/>
</body>
<body pos="-.5 0 .3">
<freejoint/>
<geom name="box1" type="box" size=".2 .15 .1"/>
</body>
<body pos="-.5 0 .6">
<freejoint/>
<geom name="box2" type="box" size=".1 .05 .1"/>
</body>
<body pos="-.5 -.6 .3">
<freejoint/>
<geom name="mesh" type="mesh" mesh="halfbox"/>
</body>
</worldbody>
<sensor>
<fromto geom1="plane" geom2="sphere" cutoff="1"/>
<fromto geom1="plane" geom2="capsule1" cutoff="1"/>
<fromto geom1="plane" geom2="box1" cutoff="1"/>
<fromto geom1="plane" geom2="cylinder" cutoff="1"/>
<fromto geom1="plane" geom2="mesh" cutoff="1"/>
<fromto geom1="capsule2" geom2="capsule1" cutoff="1"/>
<fromto geom1="capsule1" geom2="sphere" cutoff="1"/>
<fromto geom1="sphere" geom2="cylinder" cutoff="1"/>
<fromto geom1="box1" geom2="sphere" cutoff="1"/>
<fromto geom1="box2" geom2="capsule2" cutoff="1"/>
<fromto geom1="box1" geom2="box2" cutoff="1"/>
</sensor>
</mujoco>