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

Add geom surfacevel: zero-dof conveyors, treadmills and turntables.

Browse Source 
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
2026-07-15 17:57:28 -07:00
committed by Copybara-Service
parent 97eb1e5d0c
commit 4787c8094c
32 changed files with 781 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+31 -1
View File
@@ -2771,6 +2771,32 @@ helps clarify the role of bodies and geoms in MuJoCo.
actuators which use contacts in the gap zone to generate adhesive forces without producing contact forces.
See :ref:`margin and gap<coMarginGap>`.
.. _body-geom-surfacevel:
.. youtube:: PdSdrqhSiZA
:align: right
:width: 35%
:at:`surfacevel`: :at-val:`real(6), "0 0 0 0 0 0"`
Velocity of the geom's surface as seen by contacts, given as a velocity field :math:`\sigma(x)` with two components:
a constant velocity :math:`v` (first three numbers) and a rotational field with angular velocity :math:`\omega` (last
three numbers) about the geom frame origin :math:`p`, both expressed in the geom frame:
.. math::
\sigma(x) = v + \omega \times (x - p)
A contact with the geom observes the surface moving along this field, with the velocity projected onto the contact's
tangent plane: no normal velocity is imparted. When :at:`condim` is 4 or larger, the angular velocity :math:`\omega`
also drives torsional friction. :at:`surfacevel` models surfaces that move while the geom itself does not: conveyor
belts, treadmills and turntables can be constructed with no degrees of freedom. Friction drives touching bodies along
the motion of the surface: objects placed on a conveyor are transported at belt speed, and a turntable (angular
surface velocity about the cylinder axis) imparts tangential velocity that grows with radius. Surface velocities of
two touching geoms compose as relative velocity, and compose correctly with body motion (a conveyor mounted on a
moving vehicle works as expected). Note that this attribute describes the geom's *entire* surface: a box with
constant ``surfacevel`` moves all six faces. When contact points are visualized, a contact with a moving surface
additionally displays an arrow along the tangential surface velocity at the contact point. This attribute can be
modified at runtime.
.. _body-geom-fromto:
:at:`fromto`: :at-val:`real(6), optional`
@@ -3460,10 +3486,12 @@ joints and tendons have different sets of attributes, while all geoms in the com
.. _composite-geom-gap:
.. _composite-geom-surfacevel:
.. |body/composite/geom attrib list| replace::
:at:`type`, :at:`contype`, :at:`conaffinity`, :at:`condim`, :at:`group`, :at:`priority`, :at:`size`, :at:`material`,
:at:`rgba`, :at:`friction`, :at:`mass`, :at:`density`, :at:`solmix`, :at:`solref`, :at:`solimp`, :at:`margin`,
:at:`gap`
:at:`gap`, :at:`surfacevel`
|body/composite/geom attrib list|
Same meaning as regular :ref:`geom <body-geom>` attributes.
@@ -9540,6 +9568,8 @@ if omitted.
.. _default-geom-gap:
.. _default-geom-surfacevel:
.. _default-geom-fromto:
.. _default-geom-axisangle:
doc/XMLschema.rst
+9
View File
@@ -908,6 +908,9 @@
.. grid-item::
:ref:`gap<body-geom-gap>`
.. grid-item::
:ref:`surfacevel<body-geom-surfacevel>`
.. grid-item::
:ref:`fromto<body-geom-fromto>`
@@ -1373,6 +1376,9 @@
.. grid-item::
:ref:`gap<composite-geom-gap>`
.. grid-item::
:ref:`surfacevel<composite-geom-surfacevel>`
.. dropdown:: :ref:`site<composite-site>` :octicon:`dot`
@@ -5492,6 +5498,9 @@
.. grid-item::
:ref:`gap<default-geom-gap>`
.. grid-item::
:ref:`surfacevel<default-geom-surfacevel>`
.. grid-item::
:ref:`fromto<default-geom-fromto>`
doc/changelog.rst
+13
View File
@@ -7,6 +7,19 @@ Upcoming version (not yet released)
General
^^^^^^^
.. youtube:: PdSdrqhSiZA
:align: right
:width: 35%
- Added :ref:`geom/surfacevel<body-geom-surfacevel>`: the velocity of a geom's surface as seen by contacts, given as
a velocity field with a constant component and a rotational component about the geom frame origin. This allows
conveyor belts, treadmills and turntables to be modeled with static geoms and no degrees of freedom: friction
drives touching bodies toward the motion of the surface, with the field projected onto each contact's tangent
plane. Surface velocities compose correctly with each other and with body motion.
Note that the contact rows of ``mjData.efc_vel``, and the constraint-state sensors that read them, report the
velocity relative to the moving surface rather than to the geom, since that is the quantity the constraint acts
on; for geoms without :at:`surfacevel` the two are identical. Contact-point visualization draws an arrow along the
surface velocity at contacts with moving surfaces.
- Replaced midpoint integration of free bodies with :ref:`gyroscopic derivatives<geFreeBody>` in the ``implicitfast``
:ref:`integrator<geIntegrators>`: the bias-force derivative of every standalone free body is applied via a local
unsymmetric solve of its decoupled block, making ``implicitfast`` identical to ``implicit`` for such bodies.
doc/computation/index.rst
+5 -1
View File
@@ -1531,7 +1531,11 @@ Recall that :math:`r` is the position residual, while :math:`J v` is the joint v
the indexing notation refers to one component of the projected velocity vector. For friction loss and friction
dimensions of elliptic cones, :math:`r \equiv 0` and therefore :math:`k=0`, so the reference acceleration reduces to
pure damping: :math:`\ari = -b_i (J v)_i`. More detail is given in the :ref:`Friction<CSolverFriction>` section of the
Modeling chapter.
Modeling chapter. For the tangential rows of contacts whose geoms specify a
:ref:`surface velocity<body-geom-surfacevel>`, the projected velocity :math:`(J v)_i` is biased by the relative
velocity of the surface material, so that the reference acceleration drives the contact toward moving *with* the
surface; this is how conveyor belts and turntables are implemented, and it is also the quantity reported in the
contact rows of ``mjData.efc_vel``.
To summarize, the constraint behavior is determined by three per-constraint quantities: impedance :math:`0<d<1`, damping
:math:`b > 0`, and stiffness :math:`k \geq 0`. These are computed from the :at:`solimp` and :at:`solref` attributes as
doc/includes/references.h
+3
View File
@@ -661,6 +661,7 @@ typedef struct mjModel_ {
mjtSize nnames_map; // number of slots in the names hash map
mjtSize nJmom; // number of non-zeros in sparse actuator_moment matrix
mjtSize ngravcomp; // number of bodies with nonzero gravcomp
mjtSize nsurfacevel; // number of geoms with nonzero surfacevel
mjtSize nemax; // number of potential equality-constraint rows
mjtSize njmax; // number of available rows in constraint Jacobian (legacy)
mjtSize nconmax; // number of potential contacts in contact list (legacy)
@@ -801,6 +802,7 @@ typedef struct mjModel_ {
mjtNum* geom_friction; // friction for (slide, spin, roll) (ngeom x 3)
mjtNum* geom_margin; // geometric inflation for contact (ngeom x 1)
mjtNum* geom_gap; // additional contact detection buffer (ngeom x 1)
mjtNum* geom_surfacevel; // surface velocity in local frame: lin,ang (ngeom x 6)
mjtNum* geom_fluid; // fluid interaction parameters (ngeom x mjNFLUID)
mjtNum* geom_user; // user data (ngeom x nuser_geom)
float* geom_rgba; // rgba when material is omitted (ngeom x 4)
@@ -1895,6 +1897,7 @@ typedef struct mjsGeom_ { // geom specification
mjtNum solimp[mjNIMP]; // solver impedance
double margin; // margin for contact detection
double gap; // additional contact detection buffer
double surfacevel[6]; // surface velocity in local frame: linear, angular
// inertia inference
double mass; // used to compute density
doc/programming/simulation.rst
+4
View File
@@ -723,6 +723,10 @@ Exceptions to the general rule that **real-valued** types **are safe to change**
* - ``geom_pos`` |br| ``geom_quat`` |br| ``geom_size`` |br| ``geom_rbound`` |br| ``geom_aabb``
- Unsafe.
-
* - ``geom_surfacevel``
- Safe.
- If the number of geoms with nonzero surface velocity is changed from zero to non-zero (or vice versa),
:ref:`mj_setConst` must be called.
* - ``{site,cam,light}_`` |br| ``{pos,quat}``
- Mostly safe.
- For cameras and lights with tracking or targeting, :ref:`mj_setConst` is required.
include/mujoco/mjmodel.h
+2
View File
@@ -332,6 +332,7 @@ typedef struct mjModel_ {
mjtSize nnames_map; // number of slots in the names hash map
mjtSize nJmom; // number of non-zeros in sparse actuator_moment matrix
mjtSize ngravcomp; // number of bodies with nonzero gravcomp
mjtSize nsurfacevel; // number of geoms with nonzero surfacevel
mjtSize nemax; // number of potential equality-constraint rows
mjtSize njmax; // number of available rows in constraint Jacobian (legacy)
mjtSize nconmax; // number of potential contacts in contact list (legacy)
@@ -472,6 +473,7 @@ typedef struct mjModel_ {
mjtNum* geom_friction; // friction for (slide, spin, roll) (ngeom x 3)
mjtNum* geom_margin; // geometric inflation for contact (ngeom x 1)
mjtNum* geom_gap; // additional contact detection buffer (ngeom x 1)
mjtNum* geom_surfacevel; // surface velocity in local frame: lin,ang (ngeom x 6)
mjtNum* geom_fluid; // fluid interaction parameters (ngeom x mjNFLUID)
mjtNum* geom_user; // user data (ngeom x nuser_geom)
float* geom_rgba; // rgba when material is omitted (ngeom x 4)
include/mujoco/mjspec.h
+1
View File
@@ -358,6 +358,7 @@ typedef struct mjsGeom_ { // geom specification
mjtNum solimp[mjNIMP]; // solver impedance
double margin; // margin for contact detection
double gap; // additional contact detection buffer
double surfacevel[6]; // surface velocity in local frame: linear, angular
// inertia inference
double mass; // used to compute density
include/mujoco/mjspecmacro.h
+1
View File
@@ -182,6 +182,7 @@
XVEC( mjtNum, solimp, mjNIMP ) \
X ( double, margin, 1 ) \
X ( double, gap, 1 ) \
XVEC( double, surfacevel, 6 ) \
X ( double, mass, 1 ) \
X ( double, density, 1 ) \
X ( mjtGeomInertia, typeinertia, 1 ) \
include/mujoco/mjxmacro.h
+2
View File
@@ -247,6 +247,7 @@
X( nnames_map ) \
X( nJmom ) \
X( ngravcomp ) \
X( nsurfacevel ) \
X( nemax ) \
X( njmax ) \
X( nconmax ) \
@@ -385,6 +386,7 @@
X ( mjtNum, geom_friction, ngeom, 3 ) \
X ( mjtNum, geom_margin, ngeom, 1 ) \
X ( mjtNum, geom_gap, ngeom, 1 ) \
X ( mjtNum, geom_surfacevel, ngeom, 6 ) \
XNV ( mjtNum, geom_fluid, ngeom, mjNFLUID ) \
X ( mjtNum, geom_user, ngeom, MJ_M(nuser_geom) ) \
X ( float, geom_rgba, ngeom, 4 )
model/surfacevel/carousel.xml
+98
View File
@@ -0,0 +1,98 @@
<mujoco model="Luggage carousel">
<visual>
<headlight diffuse=".7 .7 .7" ambient=".25 .25 .25"/>
<quality shadowsize="8192"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".35 .5 .65" rgb2=".05 .05 .1" width="32" height="512"/>
<texture name="floor" type="2d" builtin="checker" rgb1=".18 .2 .22" rgb2=".24 .27 .3" width="300" height="300"/>
<material name="floor" texture="floor" texrepeat="12 12" reflectance=".1"/>
<material name="belt" rgba=".3 .45 .7 1"/>
<material name="island" rgba=".6 .3 .15 1"/>
<mesh name="ring" builtin="supertorus" params="64 .2 .4 1" scale="1.7 1.7 .9"/>
<material name="rail" rgba=".55 .55 .58 1"/>
<mesh name="bag1" builtin="supersphere" params="32 .3 .4" scale=".2 .13 .085"/>
<mesh name="bag2" builtin="supersphere" params="32 .55 .5" scale=".17 .12 .075"/>
<mesh name="bag3" builtin="supersphere" params="32 .25 .3" scale=".19 .14 .1"/>
<mesh name="bag4" builtin="supersphere" params="32 .6 .45" scale=".15 .1 .065"/>
<mesh name="bag5" builtin="supersphere" params="32 .4 .6" scale=".16 .11 .07"/>
<mesh name="bag6" builtin="supersphere" params="32 .35 .35" scale=".18 .12 .08"/>
</asset>
<worldbody>
<light pos="2 -4 5" dir="-.3 .5 -.8" diffuse=".6 .6 .6" intensity="3000000" range="20"/>
<light pos="-3 2 4" dir=".5 -.3 -.7" diffuse=".4 .4 .4" intensity="2000000" range="20"/>
<light pos="-1 -4.4 4" dir=".05 .05 -1" diffuse=".6 .55 .45" intensity="2500000" range="25" cutoff="30"/>
<light pos="0 -2.6 4.5" dir="0 .12 -1" diffuse=".6 .55 .45" intensity="2500000" range="25" cutoff="30"/>
<!-- hero downlight over the carousel: tight cone, cool white -->
<light pos="0 0 5.2" dir="0 0 -1" diffuse=".55 .6 .68" intensity="4000000" range="25" cutoff="26"/>
<!-- high ambience: dim blue night-wash point light, no shadows, lifts the
pitch black between the pools without flattening them -->
<light type="point" pos="1 0 7.5" diffuse=".25 .3 .4" intensity="600000" range="40" castshadow="false"/>
<geom name="floor" type="plane" size="8 8 .1" material="floor"/>
<!-- carousel ring: square-profile supertorus, surface spinning about +z -->
<geom name="carousel" type="mesh" mesh="ring" pos="0 0 .24" material="belt" surfacevel="0 0 0 0 0 .4" friction=".1" priority="1"/>
<!-- center island: dome closing the ring hole, only the top 45-degree cap shows -->
<geom name="island" type="sphere" size="1.85" pos="0 0 -0.7314" material="island"/>
<!-- guardrail: capsule ring centered on the top-outer corner of the torus,
so about three quarters of the tube sticks out -->
<replicate count="36" euler="0 0 10">
<geom type="capsule" size="0.1" fromto="2.0322 -0.1778 0.42 2.0322 0.1778 0.42" material="rail"/>
</replicate>
<!-- three-belt cascade, one conveyor definition (origin at belt center).
C2 (low, knee height) holds the definition and runs toward the carousel;
C1 (top, where bags start) and C3 (incline) are attached copies. -->
<frame pos="0 -4.2 .29" euler="0 0 90">
<frame name="conveyor">
<geom name="belt" type="box" size=".74 .3 .06" material="belt" surfacevel=".7 0 0 0 0 0"/>
<geom name="belt_rail1" type="box" size=".74 .02 .1" pos="0 -.33 .04" material="rail"/>
<geom name="belt_rail2" type="box" size=".74 .02 .1" pos="0 .33 .04" material="rail"/>
<geom name="belt_roller1" type="cylinder" size=".06 .3" pos="-.74 0 0" euler="90 0 0" material="belt" surfacevel="0 0 0 0 0 -11.667"/>
<geom name="belt_roller2" type="cylinder" size=".06 .3" pos=".74 0 0" euler="90 0 0" material="belt" surfacevel="0 0 0 0 0 -11.667"/>
</frame>
</frame>
<!-- C1: top conveyor, perpendicular, discharging onto C2's upstream end -->
<frame pos="-1.19 -4.355 .84">
<attach frame="conveyor" prefix="top_"/>
</frame>
<!-- C3: incline continuing C2, tilted 20 degrees, discharging over the ring -->
<frame pos="0 -2.6 0.5950" euler="0 0 90">
<frame euler="0 -24.34 0">
<attach frame="conveyor" prefix="up_"/>
</frame>
</frame>
<!-- bags, distributed along the cascade as if mid-operation -->
<body name="bag1" pos="0 -2.37 .88" euler="24.34 0 0">
<freejoint/>
<geom type="mesh" mesh="bag1" mass="4" rgba=".65 .2 .15 1"/>
</body>
<body name="bag2" pos="0 -3.6 .44" euler="0 0 -20">
<freejoint/>
<geom type="mesh" mesh="bag2" mass="3" rgba=".2 .45 .45 1"/>
</body>
<body name="bag3" pos="-1.6 -4.355 1.01" euler="0 0 75">
<freejoint/>
<geom type="mesh" mesh="bag3" mass="4.5" rgba=".4 .3 .2 1"/>
</body>
<body name="bag4" pos="-.75 -4.355 .97" euler="0 0 -10">
<freejoint/>
<geom type="mesh" mesh="bag4" mass="2.5" rgba=".25 .25 .3 1"/>
</body>
<body name="bag5" pos="-1.15 -4.31 .98" euler="0 0 40">
<freejoint/>
<geom type="mesh" mesh="bag5" mass="2.8" rgba=".6 .5 .2 1"/>
</body>
<body name="bag6" pos="0 -4.7 .44" euler="0 0 55">
<freejoint/>
<geom type="mesh" mesh="bag6" mass="3.5" rgba=".4 .25 .45 1"/>
</body>
</worldbody>
</mujoco>
model/surfacevel/treadmill.xml
+72
View File
@@ -0,0 +1,72 @@
<mujoco model="Treadmill">
<!-- A treadmill with no degrees of freedom: the deck surface moves via surfacevel,
with matched spinning end rollers (w = v/r). Standard running-treadmill
dimensions: 0.51 x 1.52 m belt, deck top 0.20 m, console at 1.35 m. The
attached humanoid is passive: it collapses and is swept off the back. -->
<statistic center="0 0 .7" extent="2.5"/>
<visual>
<headlight diffuse=".7 .7 .7" ambient=".3 .3 .3"/>
<quality shadowsize="8192"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2=".1 .1 .15"
width="32" height="512"/>
<texture name="tm_floor" type="2d" builtin="checker" rgb1=".25 .27 .28" rgb2=".3 .32 .34"
width="300" height="300"/>
<material name="tm_floor" texture="tm_floor" texrepeat="10 10" reflectance=".1"/>
<material name="tm_belt" rgba=".3 .45 .7 1"/>
<material name="tm_frame" rgba=".2 .2 .22 1"/>
<material name="tm_step" rgba=".5 .5 .52 1"/>
<model name="humanoid" file="../humanoid/humanoid.xml"/>
</asset>
<worldbody>
<!-- soft key (large bulb = soft shadows), shadowless cool fill, dim ambience -->
<light pos="-2.5 -1.5 3" dir=".6 .35 -.7" diffuse=".65 .6 .52" intensity="1000000" range="15"
bulbradius=".15"/>
<light pos="2 1.5 2.5" dir="-.5 -.4 -.6" diffuse=".35 .4 .48" intensity="700000" range="15"
castshadow="false"/>
<light type="point" pos="0 0 4" diffuse=".3 .3 .33" intensity="300000" range="20"
castshadow="false"/>
<geom name="floor" type="plane" size="4 4 .1" material="tm_floor"/>
<!-- deck: belt surface flows front to rear (+x), sweeping the rider off the back;
end rollers have radius = deck half-thickness, spin matched: w = 1.4/.05 -->
<geom name="deck" type="box" size=".76 .255 .05" pos="0 0 .15" material="tm_belt"
surfacevel="1.4 0 0 0 0 0"/>
<geom name="roller_front" type="cylinder" size=".05 .255" pos="-.76 0 .15" euler="90 0 0"
material="tm_belt" surfacevel="0 0 0 0 0 -28"/>
<geom name="roller_rear" type="cylinder" size=".05 .255" pos=".76 0 .15" euler="90 0 0"
material="tm_belt" surfacevel="0 0 0 0 0 -28"/>
<!-- side steps, flush with the belt surface -->
<geom name="step_left" type="box" size=".76 .04 .05" pos="0 -.295 .155" material="tm_step"/>
<geom name="step_right" type="box" size=".76 .04 .05" pos="0 .295 .155" material="tm_step"/>
<!-- motor hood over the front roller -->
<geom name="hood" type="box" size=".16 .3 .05" pos="-.86 0 .25" euler="0 -10 0"
material="tm_frame"/>
<!-- uprights, handrails and console -->
<geom name="upright_left" type="capsule" size=".025" fromto="-.8 -.28 .25 -.95 -.28 1.15"
material="tm_frame"/>
<geom name="upright_right" type="capsule" size=".025" fromto="-.8 .28 .25 -.95 .28 1.15"
material="tm_frame"/>
<geom name="rail_left" type="capsule" size=".02" fromto="-.95 -.28 1.15 -.35 -.28 1.05"
material="tm_step"/>
<geom name="rail_right" type="capsule" size=".02" fromto="-.95 .28 1.15 -.35 .28 1.05"
material="tm_step"/>
<geom name="console" type="box" size=".04 .3 .12" pos="-1 0 1.3" euler="0 -30 0"
material="tm_frame"/>
<geom name="screen" type="box" size=".005 .14 .07" pos="-.963 0 1.32" euler="0 -30 0"
rgba=".1 .25 .2 1"/>
<!-- passive rider -->
<frame pos="-.3 0 .21" euler="0 0 180">
<attach model="humanoid" body="torso" prefix="rider_"/>
</frame>
</worldbody>
</mujoco>
python/mujoco/introspect/structs.py
+21
View File
@@ -1368,6 +1368,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='mjtSize'),
doc='number of bodies with nonzero gravcomp',
),
StructFieldDecl(
name='nsurfacevel',
type=ValueType(name='mjtSize'),
doc='number of geoms with nonzero surfacevel',
),
StructFieldDecl(
name='nemax',
type=ValueType(name='mjtSize'),
@@ -2245,6 +2250,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='additional contact detection buffer',
array_extent=('ngeom',),
),
StructFieldDecl(
name='geom_surfacevel',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='surface velocity in local frame: lin,ang',
array_extent=('ngeom', 6),
),
StructFieldDecl(
name='geom_fluid',
type=PointerType(
@@ -7957,6 +7970,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='double'),
doc='additional contact detection buffer',
),
StructFieldDecl(
name='surfacevel',
type=ArrayType(
inner_type=ValueType(name='double'),
extents=(6,),
),
doc='surface velocity in local frame: linear, angular',
),
StructFieldDecl(
name='mass',
type=ValueType(name='double'),
src/engine/engine_core_constraint.c
+75
View File
@@ -3127,6 +3127,78 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
}
// add relative surface velocity of contacting geoms to contact rows of efc_vel
static void mj_addSurfaceVel(const mjModel* m, mjData* d) {
// no surface velocity on any geom: quick return
if (!m->nsurfacevel) {
return;
}
int ispyramid = mj_isPyramidal(m);
int ncon = d->ncon;
// loop over contacts, add surface velocity to efc_vel
for (int i=0; i < ncon; i++) {
// get contact, skip excluded
const mjContact* con = d->contact + i;
if (con->efc_address < 0) { continue; }
// relative surface velocity in world frame: geom2 minus geom1, linear and angular
mjtNum svel[3] = {0, 0, 0}, sang[3] = {0, 0, 0};
int active = 0;
for (int side=0; side < 2; side++) {
int g = con->geom[side];
if (g < 0) {
// TODO(team): support flex
continue;
}
const mjtNum* sv = m->geom_surfacevel + 6*g;
// skip geom with no surface velocity
if (!sv[0] && !sv[1] && !sv[2] && !sv[3] && !sv[4] && !sv[5]) {
continue;
}
active = 1;
// rotate to world frame, add angular contribution at contact point
mjtNum sgn = side ? 1 : -1;
mjtNum vw[3], ww[3];
mj_geomSurfaceVelocity(m, d, g, con->pos, vw, ww);
mju_addToScl3(svel, vw, sgn);
mju_addToScl3(sang, ww, sgn);
}
if (!active) {
continue;
}
// rotate to contact frame: (normal, tangent1, tangent2, spin, roll1, roll2)
mjtNum cs[6];
mju_mulMatVec3(cs, con->frame, svel);
mju_mulMatVec3(cs + 3, con->frame, sang);
// surface velocity acts in the tangent plane and torsional direction only
cs[0] = 0; // no normal push
cs[4] = 0; // no rolling drive
cs[5] = 0; // no rolling drive
// add to contact rows
int adr = con->efc_address, dim = con->dim;
if (dim == 1 || !ispyramid) {
for (int j=0; j < dim; j++) {
d->efc_vel[adr + j] += cs[j];
}
} else {
for (int k=1; k < dim; k++) {
mjtNum mu = con->friction[k-1];
d->efc_vel[adr + 2*(k-1)] += cs[0] + mu*cs[k];
d->efc_vel[adr + 2*(k-1) + 1] += cs[0] - mu*cs[k];
}
}
}
}
// compute efc_vel, efc_aref
void mj_referenceConstraint(const mjModel* m, mjData* d) {
int nefc = d->nefc;
@@ -3135,6 +3207,9 @@ void mj_referenceConstraint(const mjModel* m, mjData* d) {
// compute efc_vel
mj_mulJacVec(m, d, d->efc_vel, d->qvel);
// add relative surface velocity to contact rows
mj_addSurfaceVel(m, d);
// compute aref = -B*vel - K*I*(pos-margin)
for (int i=0; i < nefc; i++) {
d->efc_aref[i] = -KBIP[4*i+1]*d->efc_vel[i]
src/engine/engine_core_util.c
+17
View File
@@ -877,6 +877,23 @@ void mj_objectVelocity(const mjModel* m, const mjData* d,
}
// compute material surface velocity of a geom at a point, in the world frame
void mj_geomSurfaceVelocity(const mjModel* m, const mjData* d, int geomid,
const mjtNum point[3], mjtNum linear[3], mjtNum angular[3]) {
const mjtNum* sv = m->geom_surfacevel + 6*geomid;
// rotate local linear and angular surface velocities to the world frame
mji_mulMatVec3(linear, d->geom_xmat + 9*geomid, sv);
mji_mulMatVec3(angular, d->geom_xmat + 9*geomid, sv + 3);
// add angular velocity contribution (w x r) at the query point
mjtNum arm[3], wxr[3];
mji_sub3(arm, point, d->geom_xpos + 3*geomid);
mji_cross(wxr, angular, arm);
mji_addTo3(linear, wxr);
}
// compute object 6D acceleration in object-centered frame, world/local orientation
void mj_objectAcceleration(const mjModel* m, const mjData* d,
int objtype, int objid, mjtNum res[6], int flg_local) {
src/engine/engine_core_util.h
+4
View File
@@ -117,6 +117,10 @@ MJAPI void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body);
MJAPI void mj_objectVelocity(const mjModel* m, const mjData* d,
int objtype, int objid, mjtNum res[6], int flg_local);
// compute material surface velocity of a geom at a point, in world frame
void mj_geomSurfaceVelocity(const mjModel* m, const mjData* d, int geomid,
const mjtNum point[3], mjtNum linear[3], mjtNum angular[3]);
// compute object 6D acceleration in object-centered frame, world/local orientation
MJAPI void mj_objectAcceleration(const mjModel* m, const mjData* d,
int objtype, int objid, mjtNum res[6], int flg_local);
src/engine/engine_io.c
+2 -2
View File
@@ -250,7 +250,7 @@ void mj_makeModel(mjModel** dest,
// CHECK SIZE PARAMETERS
{
// dummy variables for MJMODEL_SIZES set after mjModel construction
int nnames_map = 0, nJmom = 0, ngravcomp = 0, nemax = 0, njmax = 0, nconmax=0;
int nnames_map = 0, nJmom = 0, ngravcomp = 0, nsurfacevel = 0, nemax = 0, njmax = 0, nconmax=0;
int npolygonmax = 0, nmeshdegmax = 0;
int nuserdata=0, nsensordata=0, npluginstate=0, nhistory=0, narena=0, nbuffer=0;
@@ -270,7 +270,7 @@ void mj_makeModel(mjModel** dest,
#undef X
// suppress unused variable warnings
(void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nemax; (void)njmax; (void)nconmax;
(void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nsurfacevel; (void)nemax; (void)njmax; (void)nconmax;
(void)npolygonmax; (void)nmeshdegmax;
(void)nuserdata; (void)nsensordata; (void)npluginstate; (void)nhistory; (void)narena;
(void)nbuffer;
src/engine/engine_setconst.c
+10
View File
@@ -211,6 +211,16 @@ static void setFixed(mjModel* m, mjData* d) {
}
m->ngravcomp = ngravcomp;
// compute nsurfacevel: number of geoms with nonzero surfacevel
int nsurfacevel = 0;
for (int i=0; i < m->ngeom; i++) {
const mjtNum* sv = m->geom_surfacevel + 6*i;
if (sv[0] || sv[1] || sv[2] || sv[3] || sv[4] || sv[5]) {
nsurfacevel++;
}
}
m->nsurfacevel = nsurfacevel;
// set jnt_actuatorid and tendon_actuatorid
mju_fillInt(m->jnt_actuatorid, -1, m->njnt);
mju_fillInt(m->tendon_actuatorid, -1, m->ntendon);
src/engine/engine_vis_visualize.c
+42
View File
@@ -656,7 +656,49 @@ static void addContactGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjSNPRINTF(thisgeom->label, "%s | %s", contactlabel[0], contactlabel[1]);
}
float contactrgba[4];
f2f(contactrgba, thisgeom->rgba, 4);
releaseGeom(&thisgeom, scn);
// surface velocity: one arrow per moving surface, pointing along the
// tangential material velocity at the contact point
const mjtNum kVelocityMap = 0.5; // units of time: arrow length = velocity * kVelocityMap
for (int side=0; side < 2; side++) {
int g = con->geom[side];
if (g < 0) {
// TODO(team): support flex
continue;
}
const mjtNum* sv = m->geom_surfacevel + 6*g;
if (!sv[0] && !sv[1] && !sv[2] && !sv[3] && !sv[4] && !sv[5]) {
continue;
}
// material velocity at the contact point, world frame
mjtNum vw[3], ww[3];
mj_geomSurfaceVelocity(m, d, g, con->pos, vw, ww);
// project out the normal component: only the tangential part acts
mjtNum vn = mju_dot3(vw, con->frame);
mju_addToScl3(vw, con->frame, -vn);
if (mju_norm3(vw) < mjMINVAL) {
continue;
}
// anchor slightly off the contact point on the owning geom's side
mjtNum from[3], to[3];
mjtNum offset = (side ? 1 : -1) * 0.5 * m->vis.scale.forcewidth * scl;
mju_addScl3(from, con->pos, con->frame, offset);
mju_addScl3(to, from, vw, kVelocityMap);
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
mjv_connector(thisgeom, mjGEOM_ARROW, m->vis.scale.forcewidth * scl, from, to);
f2f(thisgeom->rgba, contactrgba, 4);
releaseGeom(&thisgeom, scn);
}
}
// mat = contact frame rotation matrix (normal along x)
src/user/user_model.cc
+2
View File
@@ -3062,6 +3062,7 @@ void mjCModel::CopyTree(mjModel* m) {
mjuu_copyvec(m->geom_solimp+mjNIMP*gid, pg->solimp, mjNIMP);
m->geom_margin[gid] = (mjtNum)pg->margin;
m->geom_gap[gid] = (mjtNum)pg->gap;
mjuu_copyvec(m->geom_surfacevel+6*gid, pg->surfacevel, 6);
mjuu_copyvec(m->geom_fluid+mjNFLUID*gid, pg->fluid, mjNFLUID);
mjuu_copyvec(m->geom_user+nuser_geom*gid, pg->get_userdata().data(), nuser_geom);
mjuu_copyvec(m->geom_rgba+4*gid, pg->rgba, 4);
@@ -5841,6 +5842,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
pg->solmix = (double)m->geom_solmix[i];
pg->margin = (double)m->geom_margin[i];
pg->gap = (double)m->geom_gap[i];
mjuu_copyvec(pg->surfacevel, m->geom_surfacevel+6*i, 6);
if (nuser_geom) {
mjuu_copyvec(pg->userdata_.data(), m->geom_user + nuser_geom*i, nuser_geom);
src/user/user_objects.cc
+18
View File
@@ -4051,6 +4051,24 @@ void mjCGeom::Compile(void) {
// accumulate mesh frame into geom frame
mjuu_frameaccum(pos, quat, meshpos, pmesh->GetQuatPtr());
// re-express surfacevel in the compiled geom frame, which absorbed the mesh frame
if (surfacevel[0] || surfacevel[1] || surfacevel[2] ||
surfacevel[3] || surfacevel[4] || surfacevel[5]) {
// angular origin moves to meshpos: linear part gains omega x meshpos
double wxp[3];
mjuu_crossvec(wxp, surfacevel+3, meshpos);
mjuu_addtovec(surfacevel, wxp, 3);
// rotate both parts by the inverse mesh orientation
const double* mq = pmesh->GetQuatPtr();
double invq[4] = {mq[0], -mq[1], -mq[2], -mq[3]};
double tmp[3];
mjuu_rotVecQuat(tmp, surfacevel, invq);
mjuu_copyvec(surfacevel, tmp, 3);
mjuu_rotVecQuat(tmp, surfacevel+3, invq);
mjuu_copyvec(surfacevel+3, tmp, 3);
}
}
// check size parameters
src/xml/xml_native_reader.cc
+7 -4
View File
@@ -238,7 +238,7 @@ std::vector<const char*> MJCF[nMJCF] = {
{"geom", "?", "type", "pos", "quat", "contype", "conaffinity", "condim",
"group", "priority", "size", "material", "friction", "mass", "density",
"shellinertia", "solmix", "solref", "solimp",
"margin", "gap", "fromto", "axisangle", "xyaxes", "zaxis", "euler",
"margin", "gap", "surfacevel", "fromto", "axisangle", "xyaxes", "zaxis", "euler",
"hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"},
{"site", "?", "type", "group", "pos", "quat", "material",
"size", "fromto", "axisangle", "xyaxes", "zaxis", "euler", "rgba", "user"},
@@ -349,8 +349,9 @@ std::vector<const char*> MJCF[nMJCF] = {
{"geom", "*", "name", "class", "type", "contype", "conaffinity", "condim",
"group", "priority", "size", "material", "friction", "mass", "density",
"shellinertia", "solmix", "solref", "solimp",
"margin", "gap", "fromto", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler",
"hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"},
"margin", "gap", "surfacevel", "fromto", "pos", "quat", "axisangle", "xyaxes",
"zaxis", "euler", "hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef",
"user"},
{"<"},
{"plugin", "*", "plugin", "instance"},
{"<"},
@@ -380,7 +381,7 @@ std::vector<const char*> MJCF[nMJCF] = {
{"skin", "?", "texcoord", "material", "group", "rgba", "inflate", "subgrid"},
{"geom", "?", "type", "contype", "conaffinity", "condim",
"group", "priority", "size", "material", "rgba", "friction", "mass",
"density", "solmix", "solref", "solimp", "margin", "gap"},
"density", "solmix", "solref", "solimp", "margin", "gap", "surfacevel"},
{"site", "?", "group", "size", "material", "rgba"},
{"plugin", "*", "plugin", "instance"},
{"<"},
@@ -1973,6 +1974,7 @@ void mjXReader::OneGeom(XMLElement* elem, mjsGeom* geom) {
ReadAttr(elem, "solimp", mjNIMP, geom->solimp, text, false, false);
ReadAttr(elem, "margin", 1, &geom->margin, text);
ReadAttr(elem, "gap", 1, &geom->gap, text);
ReadAttr(elem, "surfacevel", 6, geom->surfacevel, text, false, false);
if (ReadAttrTxt(elem, "hfield", hfieldname)) {
mjs_setString(geom->hfieldname, hfieldname.c_str());
}
@@ -2782,6 +2784,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, mjsFrame* frame, c
ReadAttr(egeom, "solimp", mjNIMP, dgeom.solimp, text, false, false);
ReadAttr(egeom, "margin", 1, &dgeom.margin, text);
ReadAttr(egeom, "gap", 1, &dgeom.gap, text);
ReadAttr(egeom, "surfacevel", 6, dgeom.surfacevel, text, false, false);
if (ReadAttrTxt(egeom, "material", material)) {
mjs_setString(dgeom.material, material.c_str());
}
src/xml/xml_native_writer.cc
+1 -1
View File
@@ -497,7 +497,7 @@ void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* geom, mjCDef* def, stri
WriteAttr(elem, "solimp", mjNIMP, geom->solimp, def->Geom().solimp, true);
WriteAttr(elem, "margin", 1, &geom->margin, &def->Geom().margin);
WriteAttr(elem, "gap", 1, &geom->gap, &def->Geom().gap);
WriteAttr(elem, "gap", 1, &geom->gap, &def->Geom().gap);
WriteAttr(elem, "surfacevel", 6, geom->surfacevel, def->Geom().surfacevel, true);
WriteAttrKey(elem, "fluidshape",
fluid_map, 2, geom->fluid_ellipsoid, def->Geom().fluid_ellipsoid);
WriteAttr(elem, "fluidcoef", 5, geom->fluid_coefs, def->Geom().fluid_coefs);
test/engine/engine_core_constraint_test.cc
+200
View File
@@ -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 =
test/engine/testdata/core_constraint/surfacevel_conveyor.xml
Vendored
+55
View File
@@ -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>
test/engine/testdata/core_constraint/surfacevel_turntable.xml
Vendored
+45
View File
@@ -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>
test/user/user_composite_test.cc
+20
View File
@@ -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>
test/xml/mjz/mjz_encoder_test.cc
+3 -2
View File
@@ -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;
}
test/xml/xml_write_read_test.cc
+1
View File
@@ -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") ||
unity/Runtime/Bindings/MjBindings.cs
+2
View File
@@ -1105,6 +1105,7 @@ public unsafe struct mjModel_ {
public UInt64 nnames_map;
public UInt64 nJmom;
public UInt64 ngravcomp;
public UInt64 nsurfacevel;
public UInt64 nemax;
public UInt64 njmax;
public UInt64 nconmax;
@@ -1220,6 +1221,7 @@ public unsafe struct mjModel_ {
public double* geom_friction;
public double* geom_margin;
public double* geom_gap;
public double* geom_surfacevel;
public double* geom_fluid;
public double* geom_user;
public float* geom_rgba;
wasm/codegen/generated/bindings.cc
+3
View File
@@ -5005,6 +5005,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("geom_solimp", &MjModel::geom_solimp)
.property("geom_solmix", &MjModel::geom_solmix)
.property("geom_solref", &MjModel::geom_solref)
.property("geom_surfacevel", &MjModel::geom_surfacevel)
.property("geom_type", &MjModel::geom_type)
.property("geom_user", &MjModel::geom_user)
.property("hfield_adr", &MjModel::hfield_adr)
@@ -5209,6 +5210,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("nskinface", &MjModel::nskinface, &MjModel::set_nskinface, reference())
.property("nskintexvert", &MjModel::nskintexvert, &MjModel::set_nskintexvert, reference())
.property("nskinvert", &MjModel::nskinvert, &MjModel::set_nskinvert, reference())
.property("nsurfacevel", &MjModel::nsurfacevel, &MjModel::set_nsurfacevel, reference())
.property("ntendon", &MjModel::ntendon, &MjModel::set_ntendon, reference())
.property("ntex", &MjModel::ntex, &MjModel::set_ntex, reference())
.property("ntexdata", &MjModel::ntexdata, &MjModel::set_ntexdata, reference())
@@ -5777,6 +5779,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("solimp", &MjsGeom::solimp)
.property("solmix", &MjsGeom::solmix, &MjsGeom::set_solmix, reference())
.property("solref", &MjsGeom::solref)
.property("surfacevel", &MjsGeom::surfacevel)
.property("type", &MjsGeom::type, &MjsGeom::set_type, reference())
.property("typeinertia", &MjsGeom::typeinertia, &MjsGeom::set_typeinertia, reference())
.property("userdata", &MjsGeom::userdata, reference());
wasm/codegen/generated/bindings.h
+12
View File
@@ -4227,6 +4227,12 @@ struct MjModel {
void set_ngravcomp(int value) {
ptr_->ngravcomp = static_cast<mjtSize>(value);
}
int nsurfacevel() const {
return static_cast<int>(ptr_->nsurfacevel);
}
void set_nsurfacevel(int value) {
ptr_->nsurfacevel = static_cast<mjtSize>(value);
}
int nemax() const {
return static_cast<int>(ptr_->nemax);
}
@@ -4596,6 +4602,9 @@ struct MjModel {
emscripten::val geom_gap() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ngeom, ptr_->geom_gap));
}
emscripten::val geom_surfacevel() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ngeom * 6, ptr_->geom_surfacevel));
}
emscripten::val geom_fluid() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ngeom * mjNFLUID, ptr_->geom_fluid));
}
@@ -6257,6 +6266,9 @@ struct MjsGeom {
void set_gap(double value) {
ptr_->gap = value;
}
emscripten::val surfacevel() const {
return emscripten::val(emscripten::typed_memory_view(6, ptr_->surfacevel));
}
double mass() const {
return ptr_->mass;
}