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

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]

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) {

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

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;

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

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)