Add overloaded Ground function to enable custom ground geom groups.

PiperOrigin-RevId: 551561019
Change-Id: I27e2ae164898264f29429ed007dab0e32d771546

mjpc/utilities.cc

@@ -571,15 +571,17 @@ void StateDiff(const mjModel* m, mjtNum* ds, const mjtNum* s1, const mjtNum* s2,
 }
 
 // return global height of nearest group 0 geom under given position
-mjtNum Ground(const mjModel* model, const mjData* data, const mjtNum pos[3]) {
-  const mjtByte geomgroup[6] = {1, 0, 0, 0, 0, 0};  // only detect group 0
+mjtNum Ground(const mjModel* model, const mjData* data, const mjtNum pos[3],
+              const mjtByte* geomgroup) {
   mjtNum down[3] = {0, 0, -1};                      // aim ray straight down
   const mjtNum height_offset = .5;  // add some height in case of penetration
   const mjtByte flg_static = 1;     // include static geoms
   const int bodyexclude = -1;       // don't exclude any bodies
   int geomid;                       // id of intersecting geom
   mjtNum query[3] = {pos[0], pos[1], pos[2] + height_offset};
-  mjtNum dist = mj_ray(model, data, query, down, geomgroup, flg_static,
+  const mjtByte default_geomgroup[6] = {1, 0, 0, 0, 0, 0};
+  const mjtByte* query_geomgroup = geomgroup ? geomgroup : default_geomgroup;
+  mjtNum dist = mj_ray(model, data, query, down, query_geomgroup, flg_static,
                        bodyexclude, &geomid);
 
   if (dist < 0) {  // SHOULD NOT OCCUR

mjpc/utilities.h

@@ -157,8 +157,9 @@ void Diff(mjtNum* dx, const mjtNum* x1, const mjtNum* x2, mjtNum h, int n);
 void StateDiff(const mjModel* m, mjtNum* ds, const mjtNum* s1, const mjtNum* s2,
                mjtNum h);
 
-// return global height of nearest group 0 geom under given position
-mjtNum Ground(const mjModel* model, const mjData* data, const mjtNum pos[3]);
+// return global height of nearest geom in geomgroup under given position
+mjtNum Ground(const mjModel* model, const mjData* data, const mjtNum pos[3],
+             const mjtByte* geomgroup = nullptr);
 
 // set x to be the point on the segment [p0 p1] that is nearest to x
 void ProjectToSegment(double x[3], const double p0[3], const double p1[3]);