From 4787c8094c13e95bbd7423c0f6607f0cea0b0261 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 15 Jul 2026 17:57:28 -0700 Subject: [PATCH] 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 --- doc/XMLreference.rst | 32 ++- doc/XMLschema.rst | 9 + doc/changelog.rst | 13 ++ doc/computation/index.rst | 6 +- doc/includes/references.h | 3 + doc/programming/simulation.rst | 4 + include/mujoco/mjmodel.h | 2 + include/mujoco/mjspec.h | 1 + include/mujoco/mjspecmacro.h | 1 + include/mujoco/mjxmacro.h | 2 + model/surfacevel/carousel.xml | 98 +++++++++ model/surfacevel/treadmill.xml | 72 +++++++ python/mujoco/introspect/structs.py | 21 ++ src/engine/engine_core_constraint.c | 75 +++++++ src/engine/engine_core_util.c | 17 ++ src/engine/engine_core_util.h | 4 + src/engine/engine_io.c | 4 +- src/engine/engine_setconst.c | 10 + src/engine/engine_vis_visualize.c | 42 ++++ src/user/user_model.cc | 2 + src/user/user_objects.cc | 18 ++ src/xml/xml_native_reader.cc | 11 +- src/xml/xml_native_writer.cc | 2 +- test/engine/engine_core_constraint_test.cc | 200 ++++++++++++++++++ .../core_constraint/surfacevel_conveyor.xml | 55 +++++ .../core_constraint/surfacevel_turntable.xml | 45 ++++ test/user/user_composite_test.cc | 20 ++ test/xml/mjz/mjz_encoder_test.cc | 5 +- test/xml/xml_write_read_test.cc | 1 + unity/Runtime/Bindings/MjBindings.cs | 2 + wasm/codegen/generated/bindings.cc | 3 + wasm/codegen/generated/bindings.h | 12 ++ 32 files changed, 781 insertions(+), 11 deletions(-) create mode 100644 model/surfacevel/carousel.xml create mode 100644 model/surfacevel/treadmill.xml create mode 100644 test/engine/testdata/core_constraint/surfacevel_conveyor.xml create mode 100644 test/engine/testdata/core_constraint/surfacevel_turntable.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 62d0c7e6..3ce59649 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -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`. +.. _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 ` attributes. @@ -9540,6 +9568,8 @@ if omitted. .. _default-geom-gap: +.. _default-geom-surfacevel: + .. _default-geom-fromto: .. _default-geom-axisangle: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 3e065070..e82a994b 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -908,6 +908,9 @@ .. grid-item:: :ref:`gap` + .. grid-item:: + :ref:`surfacevel` + .. grid-item:: :ref:`fromto` @@ -1373,6 +1376,9 @@ .. grid-item:: :ref:`gap` + .. grid-item:: + :ref:`surfacevel` + .. dropdown:: :ref:`site` :octicon:`dot` @@ -5492,6 +5498,9 @@ .. grid-item:: :ref:`gap` + .. grid-item:: + :ref:`surfacevel` + .. grid-item:: :ref:`fromto` diff --git a/doc/changelog.rst b/doc/changelog.rst index 83d21310..7e0e9a27 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,6 +7,19 @@ Upcoming version (not yet released) General ^^^^^^^ +.. youtube:: PdSdrqhSiZA + :align: right + :width: 35% + +- Added :ref:`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` in the ``implicitfast`` :ref:`integrator`: 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. diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 8f191e6f..b4efde27 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -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` section of the -Modeling chapter. +Modeling chapter. For the tangential rows of contacts whose geoms specify a +:ref:`surface velocity`, 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 0`, and stiffness :math:`k \geq 0`. These are computed from the :at:`solimp` and :at:`solref` attributes as diff --git a/doc/includes/references.h b/doc/includes/references.h index 324ad35a..776361c2 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -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 diff --git a/doc/programming/simulation.rst b/doc/programming/simulation.rst index 8fe9bc1c..e9da74fc 100644 --- a/doc/programming/simulation.rst +++ b/doc/programming/simulation.rst @@ -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. diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 48eadb4a..fd7b2178 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -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) diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index e139a4ca..b2d74e68 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -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 diff --git a/include/mujoco/mjspecmacro.h b/include/mujoco/mjspecmacro.h index 5ad8a057..45aa974d 100644 --- a/include/mujoco/mjspecmacro.h +++ b/include/mujoco/mjspecmacro.h @@ -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 ) \ diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index fc78487a..7a8d916f 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -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 ) diff --git a/model/surfacevel/carousel.xml b/model/surfacevel/carousel.xml new file mode 100644 index 00000000..6f1d1d1b --- /dev/null +++ b/model/surfacevel/carousel.xml @@ -0,0 +1,98 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/model/surfacevel/treadmill.xml b/model/surfacevel/treadmill.xml new file mode 100644 index 00000000..a6c31701 --- /dev/null +++ b/model/surfacevel/treadmill.xml @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 92c26e35..4a7f3c85 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -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'), diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index aadbbc75..e8c86def 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -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] diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index ebb00313..5853215d 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -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) { diff --git a/src/engine/engine_core_util.h b/src/engine/engine_core_util.h index 9f958937..0f02d4a0 100644 --- a/src/engine/engine_core_util.h +++ b/src/engine/engine_core_util.h @@ -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); diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index e3b9f103..db0f87c8 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -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; diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 521b927e..dd5621f1 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -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); diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 2a1db45d..52cfd781 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -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) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 829a5838..43384de4 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 68d74528..726036a6 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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 diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 6012112f..31161c2e 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -238,7 +238,7 @@ std::vector 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 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 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()); } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index cb43c698..97f3654c 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -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); diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index f8ff8553..ffe7f31f 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -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"( + + + + + + + + + + )"; + 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"( + + + + + + + + + + )"; + 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"( + + + + + + + + + + )"; + 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"( + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + + )"; + 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 = diff --git a/test/engine/testdata/core_constraint/surfacevel_conveyor.xml b/test/engine/testdata/core_constraint/surfacevel_conveyor.xml new file mode 100644 index 00000000..d43713d7 --- /dev/null +++ b/test/engine/testdata/core_constraint/surfacevel_conveyor.xml @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/core_constraint/surfacevel_turntable.xml b/test/engine/testdata/core_constraint/surfacevel_turntable.xml new file mode 100644 index 00000000..f42b0777 --- /dev/null +++ b/test/engine/testdata/core_constraint/surfacevel_turntable.xml @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/user/user_composite_test.cc b/test/user/user_composite_test.cc index 45a9867d..7944a48a 100644 --- a/test/user/user_composite_test.cc +++ b/test/user/user_composite_test.cc @@ -52,6 +52,26 @@ TEST_F(UserCompositeTest, ShapeCanBeOmitted) { EXPECT_THAT(m.get(), NotNull()) << error.data(); } +TEST_F(UserCompositeTest, GeomSurfacevel) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array 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"( diff --git a/test/xml/mjz/mjz_encoder_test.cc b/test/xml/mjz/mjz_encoder_test.cc index c6a6999d..bd296bc3 100644 --- a/test/xml/mjz/mjz_encoder_test.cc +++ b/test/xml/mjz/mjz_encoder_test.cc @@ -19,7 +19,7 @@ #include #include #include -#include +#include // NOLINT #include #include @@ -87,6 +87,7 @@ std::vector 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 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; } diff --git a/test/xml/xml_write_read_test.cc b/test/xml/xml_write_read_test.cc index 2968f13f..cbf278bb 100644 --- a/test/xml/xml_write_read_test.cc +++ b/test/xml/xml_write_read_test.cc @@ -55,6 +55,7 @@ std::vector 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") || diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 88caadd3..14d2305b 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -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; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 68803a25..48f381f4 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -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()); diff --git a/wasm/codegen/generated/bindings.h b/wasm/codegen/generated/bindings.h index 32838547..fe193bd3 100644 --- a/wasm/codegen/generated/bindings.h +++ b/wasm/codegen/generated/bindings.h @@ -4227,6 +4227,12 @@ struct MjModel { void set_ngravcomp(int value) { ptr_->ngravcomp = static_cast(value); } + int nsurfacevel() const { + return static_cast(ptr_->nsurfacevel); + } + void set_nsurfacevel(int value) { + ptr_->nsurfacevel = static_cast(value); + } int nemax() const { return static_cast(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; }