Add geom surfacevel: zero-dof conveyors, treadmills and turntables.
https://www.youtube.com/watch?v=PdSdrqhSiZA The new geom attribute surfacevel (6 numbers: linear and angular velocity in the geom's local frame, angular about the geom frame origin) specifies the velocity of the geom's surface material relative to the geom frame. The relative surface velocity of the two geoms is added to the tangential contact rows of efc_vel in mj_referenceConstraint, so friction drives touching bodies toward the motion of the surface: objects on a conveyor are transported at belt speed, turntables impart omega x r with torsional spin-up for condim >= 4, and surface velocities compose with each other and with body motion. The component along the contact normal is projected out: probe experiments showed that velocity-space emission chatters mass-independently and ingestion merely deepens penetration; normal-direction effects belong to force-space features. surfacevel is interpreted in the geom frame as authored: for mesh geoms, whose compiled frame absorbs the mesh centering and principal-axes transform, the compiler re-expresses the authored value in the compiled frame. No special interaction with sleeping: objects being transported do not fall asleep because they are moving; objects at rest on an active surface may sleep like any other resting object. Includes showcase models (model/surfacevel/): a luggage carousel whose ring is a spinning square-profile supertorus fed by a cascade of belts with matched spinning end rollers, bags dropping in and circulating indefinitely; and a treadmill with a passive humanoid. PiperOrigin-RevId: 948647785 Change-Id: I0c6559a91cc7ece1237eb8ac2e51986e7342d962
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
97eb1e5d0c
commit
4787c8094c
@@ -95,6 +95,206 @@ TEST_F(CoreConstraintTest, RestPenetration) {
|
||||
}
|
||||
}
|
||||
|
||||
// surfacevel: conveyor belt drags a box to belt speed
|
||||
TEST_F(CoreConstraintTest, SurfaceVelocityConveyor) {
|
||||
constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<geom name="belt" type="plane" size="5 5 .1" surfacevel=".5 0 0 0 0 0"/>
|
||||
<body pos="0 0 .1">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".1 .1 .1" mass="1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
char error[1024];
|
||||
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model.get(), testing::NotNull()) << error;
|
||||
MjDataPtr data = MakeData(model);
|
||||
|
||||
for (const mjtCone cone : {mjCONE_PYRAMIDAL, mjCONE_ELLIPTIC}) {
|
||||
model->opt.cone = cone;
|
||||
mj_resetData(model.get(), data.get());
|
||||
while (data->time < 2) {
|
||||
mj_step(model.get(), data.get());
|
||||
}
|
||||
|
||||
// box is transported at belt speed, no lateral or angular motion
|
||||
EXPECT_NEAR(data->qvel[0], 0.5, 1e-3);
|
||||
EXPECT_NEAR(data->qvel[1], 0.0, 1e-3);
|
||||
EXPECT_NEAR(data->qvel[5], 0.0, 1e-3);
|
||||
}
|
||||
}
|
||||
|
||||
// surfacevel: turntable spins a centered box via the torsional friction row
|
||||
TEST_F(CoreConstraintTest, SurfaceVelocityTurntable) {
|
||||
constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<geom name="table" type="plane" size="5 5 .1" condim="6"
|
||||
surfacevel="0 0 0 0 0 1"/>
|
||||
<body pos="0 0 .1">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".1 .1 .1" mass="1" condim="6"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
char error[1024];
|
||||
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model.get(), testing::NotNull()) << error;
|
||||
MjDataPtr data = MakeData(model);
|
||||
|
||||
for (const mjtCone cone : {mjCONE_PYRAMIDAL, mjCONE_ELLIPTIC}) {
|
||||
model->opt.cone = cone;
|
||||
mj_resetData(model.get(), data.get());
|
||||
while (data->time < 4) {
|
||||
mj_step(model.get(), data.get());
|
||||
}
|
||||
|
||||
// box spins up to the table's angular velocity, stays in place
|
||||
EXPECT_NEAR(data->qvel[5], 1.0, 1e-3);
|
||||
EXPECT_NEAR(data->qvel[0], 0.0, 1e-3);
|
||||
EXPECT_NEAR(data->qvel[1], 0.0, 1e-3);
|
||||
}
|
||||
}
|
||||
|
||||
// surfacevel: the normal component is projected out, normal-only is inert
|
||||
TEST_F(CoreConstraintTest, SurfaceVelocityNormalProjected) {
|
||||
constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<geom type="plane" size="5 5 .1" surfacevel="0 0 1 0 0 0"/>
|
||||
<body pos="0 0 .1">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".1 .1 .1" mass="1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
char error[1024];
|
||||
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model.get(), testing::NotNull()) << error;
|
||||
MjDataPtr data = MakeData(model);
|
||||
|
||||
while (data->time < 2) {
|
||||
mj_step(model.get(), data.get());
|
||||
}
|
||||
|
||||
// box rests as if the floor were plain
|
||||
for (int i=0; i < 6; i++) {
|
||||
EXPECT_NEAR(data->qvel[i], 0.0, 1e-6);
|
||||
}
|
||||
}
|
||||
|
||||
// surfacevel: two facing belts launch a squeezed plank at belt speed
|
||||
TEST_F(CoreConstraintTest, SurfaceVelocityFacingBelts) {
|
||||
constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<geom type="box" size=".05 .3 .5" pos="-.152 0 .5" surfacevel="0 0 1 0 0 0"/>
|
||||
<geom type="box" size=".05 .3 .5" pos=".152 0 .5" surfacevel="0 0 1 0 0 0"/>
|
||||
<body pos="0 0 .5">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".103 .1 .1" mass=".2"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
char error[1024];
|
||||
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model.get(), testing::NotNull()) << error;
|
||||
MjDataPtr data = MakeData(model);
|
||||
|
||||
mjtNum vmax = 0;
|
||||
while (data->time < 2) {
|
||||
mj_step(model.get(), data.get());
|
||||
vmax = mju_max(vmax, data->qvel[2]);
|
||||
}
|
||||
EXPECT_NEAR(vmax, 1.0, 5e-3);
|
||||
}
|
||||
|
||||
// surfacevel: belt on a moving vehicle, cargo vel composes with body vel
|
||||
TEST_F(CoreConstraintTest, SurfaceVelocityComposition) {
|
||||
constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<geom type="plane" size="10 10 .1" friction="0 0 0" condim="1" priority="1"/>
|
||||
<body name="vehicle" pos="0 0 .15">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".5 .3 .04" mass="10"/>
|
||||
<geom type="box" size=".4 .25 .01" pos="0 0 .05" mass=".01"
|
||||
surfacevel=".3 0 0 0 0 0"/>
|
||||
</body>
|
||||
<body name="cargo" pos="0 0 .3">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".08 .08 .08" mass=".1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
<keyframe>
|
||||
<key qvel=".2 0 0 0 0 0 0 0 0 0 0 0"/>
|
||||
</keyframe>
|
||||
</mujoco>
|
||||
)";
|
||||
char error[1024];
|
||||
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model.get(), testing::NotNull()) << error;
|
||||
MjDataPtr data = MakeData(model);
|
||||
mj_resetDataKeyframe(model.get(), data.get(), 0);
|
||||
|
||||
// measure while the cargo is still riding the belt (exits the far end later)
|
||||
while (data->time < 1) {
|
||||
mj_step(model.get(), data.get());
|
||||
}
|
||||
EXPECT_NEAR(data->qvel[6] - data->qvel[0], 0.3, 1e-3);
|
||||
}
|
||||
|
||||
// surfacevel: interpreted in the authored geom frame, including for mesh geoms
|
||||
// whose compiled frame absorbs the mesh's centering/principal-axes transform
|
||||
TEST_F(CoreConstraintTest, SurfaceVelocityMeshFrame) {
|
||||
constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<asset>
|
||||
<mesh name="ring" builtin="supertorus" params="32 .3 .2 1" scale="1.6 1.6 .8"/>
|
||||
</asset>
|
||||
<worldbody>
|
||||
<geom name="carousel" type="mesh" mesh="ring" pos="0 0 .24"
|
||||
surfacevel="0 0 0 0 0 .4"/>
|
||||
<body pos="1.6 0 .6">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".1 .1 .1" mass="1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
char error[1024];
|
||||
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model.get(), testing::NotNull()) << error;
|
||||
MjDataPtr data = MakeData(model);
|
||||
mj_forward(model.get(), data.get());
|
||||
|
||||
// the compiled angular surfacevel, rotated by the compiled geom orientation,
|
||||
// recovers the authored world-frame spin (0, 0, .4)
|
||||
int carousel = mj_name2id(model.get(), mjOBJ_GEOM, "carousel");
|
||||
mjtNum w_world[3];
|
||||
mju_mulMatVec3(w_world, data->geom_xmat + 9*carousel,
|
||||
model->geom_surfacevel + 6*carousel + 3);
|
||||
EXPECT_NEAR(w_world[0], 0.0, MjTol(1e-10, 1e-6));
|
||||
EXPECT_NEAR(w_world[1], 0.0, MjTol(1e-10, 1e-6));
|
||||
EXPECT_NEAR(w_world[2], 0.4, MjTol(1e-10, 1e-6));
|
||||
|
||||
// functionally: the box is dragged around the ring at speed omega * r
|
||||
while (data->time < 3) {
|
||||
mj_step(model.get(), data.get());
|
||||
}
|
||||
mjtNum x = data->qpos[0], y = data->qpos[1];
|
||||
mjtNum r = mju_sqrt(x*x + y*y);
|
||||
mjtNum speed =
|
||||
mju_sqrt(data->qvel[0] * data->qvel[0] + data->qvel[1] * data->qvel[1]);
|
||||
EXPECT_NEAR(speed, 0.4*r, 5e-3);
|
||||
}
|
||||
|
||||
static const char* const kDoflessContactPath =
|
||||
"engine/testdata/core_constraint/dofless_contact.xml";
|
||||
static const char* const kDoflessTendonFrictionalPath =
|
||||
|
||||
@@ -0,0 +1,55 @@
|
||||
<mujoco model="surfacevel conveyor">
|
||||
<!-- two opposed conveyor belts made of static geoms with surfacevel:
|
||||
objects ride the blue belt +x, the red belt returns them -x -->
|
||||
|
||||
<visual>
|
||||
<headlight diffuse=".8 .8 .8" ambient=".2 .2 .2"/>
|
||||
</visual>
|
||||
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="32" height="512"/>
|
||||
<texture name="grid" type="2d" builtin="checker" rgb1=".1 .2 .3" rgb2=".2 .3 .4" width="300" height="300"/>
|
||||
<material name="grid" texture="grid" texrepeat="8 8" reflectance=".2"/>
|
||||
<material name="beltplus" rgba=".3 .5 .8 1"/>
|
||||
<material name="beltminus" rgba=".8 .4 .3 1"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<light pos="0 0 3" dir="0 0 -1"/>
|
||||
<geom name="floor" type="plane" size="4 4 .1" material="grid"/>
|
||||
|
||||
<!-- belt running +x, top surface at z=0.2 -->
|
||||
<geom name="belt1" type="box" size="2 .3 .1" pos="0 -.4 .1" material="beltplus" surfacevel=".8 0 0 0 0 0"/>
|
||||
|
||||
<geom name="seperator" type="box" size="2 .1 .1" pos="0 0 .1" condim="1" priority="1"/>
|
||||
|
||||
<!-- belt running -x, top surface at z=0.2 -->
|
||||
<geom name="belt2" type="box" size="2 .3 .1" pos="0 .4 .1" material="beltminus" surfacevel="-.8 0 0 0 0 0"/>
|
||||
|
||||
<!-- guard rails on the outer edges -->
|
||||
<geom name="rail1" type="box" size="2 .02 .05" pos="0 -.72 .25" rgba=".5 .5 .5 1"/>
|
||||
<geom name="rail2" type="box" size="2 .02 .05" pos="0 .72 .25" rgba=".5 .5 .5 1"/>
|
||||
|
||||
<!-- deflectors: kick objects from one belt to the other at each end -->
|
||||
<geom name="turn1" type="box" size=".02 .45 .06" pos="1.6 -.2 .26" euler="0 0 -45" rgba=".6 .6 .6 1"/>
|
||||
<geom name="turn2" type="box" size=".02 .45 .06" pos="-1.6 .2 .26" euler="0 0 -45" rgba=".6 .6 .6 1"/>
|
||||
|
||||
<!-- cargo -->
|
||||
<body name="box1" pos="-1.5 -.4 .3">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".08 .08 .08" rgba=".9 .8 .2 1" mass=".3"/>
|
||||
</body>
|
||||
<body name="box2" pos="-.8 -.4 .3">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".06 .1 .06" rgba=".2 .8 .4 1" mass=".3"/>
|
||||
</body>
|
||||
<body name="ball" pos="0 -.4 .3">
|
||||
<freejoint/>
|
||||
<geom type="sphere" size=".07" rgba=".9 .3 .7 1" mass=".3"/>
|
||||
</body>
|
||||
<body name="pill" pos=".8 -.4 .3">
|
||||
<freejoint/>
|
||||
<geom type="capsule" size=".05 .06" rgba=".4 .7 .9 1" mass=".3"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,45 @@
|
||||
<mujoco model="surfacevel turntable">
|
||||
<!-- record player: a static cylinder with angular surfacevel about its axis;
|
||||
condim 6 so the torsional row spins centered objects and rolling rows engage -->
|
||||
|
||||
<visual>
|
||||
<headlight diffuse=".8 .8 .8" ambient=".2 .2 .2"/>
|
||||
</visual>
|
||||
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="32" height="512"/>
|
||||
<texture name="grid" type="2d" builtin="checker" rgb1=".1 .2 .3" rgb2=".2 .3 .4" width="300" height="300"/>
|
||||
<material name="grid" texture="grid" texrepeat="8 8" reflectance=".2"/>
|
||||
<material name="platter" rgba=".25 .25 .3 1"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<light pos="0 0 3" dir="0 0 -1"/>
|
||||
<geom name="floor" type="plane" size="3 3 .1" material="grid"/>
|
||||
|
||||
<!-- platter spinning at 1 rad/s about +z, top surface at z=0.1 -->
|
||||
<geom name="platter" type="cylinder" size="1 .05" pos="0 0 .05" material="platter" condim="6" friction="1 .01 .003" surfacevel="0 0 0 0 0 1"/>
|
||||
|
||||
<!-- centered box: pure torsional spin-up -->
|
||||
<body name="center" pos="0 0 .2">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".08 .08 .08" rgba=".9 .8 .2 1" mass=".3"/>
|
||||
</body>
|
||||
|
||||
<!-- boxes at increasing radii: tangential speed grows with r -->
|
||||
<body name="mid" pos=".4 0 .2">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".06 .06 .06" rgba=".2 .8 .4 1" mass=".3"/>
|
||||
</body>
|
||||
<body name="rim" pos=".75 0 .2">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".06 .06 .06" rgba=".9 .3 .7 1" mass=".3"/>
|
||||
</body>
|
||||
|
||||
<!-- ball: rolling rows vs the spinning surface -->
|
||||
<body name="ball" pos="-.4 .3 .25">
|
||||
<freejoint/>
|
||||
<geom type="sphere" size=".07" rgba=".4 .7 .9 1" mass=".3"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -52,6 +52,26 @@ TEST_F(UserCompositeTest, ShapeCanBeOmitted) {
|
||||
EXPECT_THAT(m.get(), NotNull()) << error.data();
|
||||
}
|
||||
|
||||
TEST_F(UserCompositeTest, GeomSurfacevel) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<composite type="cable" count="5 1 1" curve="s">
|
||||
<geom type="capsule" size=".02" surfacevel="0 0 0 0 0 3"/>
|
||||
</composite>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
std::array<char, 1024> error;
|
||||
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||
for (int g = 0; g < m->ngeom; g++) {
|
||||
if (m->geom_type[g] == mjGEOM_CAPSULE) {
|
||||
EXPECT_EQ(m->geom_surfacevel[6*g + 5], 3);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(UserCompositeTest, InvalidShape) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
|
||||
@@ -19,7 +19,7 @@
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <filesystem>
|
||||
#include <filesystem> // NOLINT
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
@@ -87,6 +87,7 @@ std::vector<std::string> GetWriteReadTestModels() {
|
||||
absl::StrContains(xml, "rfcamera") ||
|
||||
absl::StrContains(xml, "tactile") ||
|
||||
absl::StrContains(xml, "makemesh") ||
|
||||
absl::StrContains(xml, "carousel") ||
|
||||
absl::StrContains(xml, "many_dependencies") ||
|
||||
absl::StrContains(xml, "usd") ||
|
||||
absl::StrContains(xml, "torus_maxhull") ||
|
||||
@@ -101,7 +102,7 @@ std::vector<std::string> GetWriteReadTestModels() {
|
||||
absl::StrContains(xml, "strain") ||
|
||||
// exclude conflict test assets (designed to fail compile)
|
||||
absl::StrContains(xml, "xml/testdata/parent_") ||
|
||||
// exclude mjz testdata with VFS files
|
||||
// exclude mjz test data with VFS files
|
||||
absl::StrContains(xml, "mixed_test")) {
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -55,6 +55,7 @@ std::vector<std::string> GetWriteReadTestModels() {
|
||||
absl::StrContains(xml, "rfcamera") ||
|
||||
absl::StrContains(xml, "tactile") ||
|
||||
absl::StrContains(xml, "makemesh") ||
|
||||
absl::StrContains(xml, "carousel") ||
|
||||
absl::StrContains(xml, "many_dependencies") ||
|
||||
absl::StrContains(xml, "usd") ||
|
||||
absl::StrContains(xml, "torus_maxhull") ||
|
||||
|
||||
Reference in New Issue
Block a user