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; }