Add clock sensor type.

PiperOrigin-RevId: 459720950
Change-Id: Ibda48ae7dc4aa7be451a947a1c53d10a7eaf176b

src/engine/engine_io.c

@@ -1053,6 +1053,7 @@ static int sensorSize(mjtSensor sensor_type, int nuser_sensor) {
   case mjSENS_TENDONLIMITPOS:
   case mjSENS_TENDONLIMITVEL:
   case mjSENS_TENDONLIMITFRC:
+  case mjSENS_CLOCK:
     return 1;
 
   case mjSENS_ACCELEROMETER:

src/engine/engine_sensor.c

@@ -113,17 +113,18 @@ static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) {
       mjtNum cutoff = m->sensor_cutoff[i];
 
       // process all dimensions
-      for (int j=0; j<dim; j++)
+      for (int j=0; j<dim; j++) {
 
         // real: apply on both sides
-        if (m->sensor_datatype[i]==mjDATATYPE_REAL)
-          d->sensordata[adr+j] =
-            mju_min(cutoff, mju_max(-cutoff, d->sensordata[adr+j]));
+        if (m->sensor_datatype[i]==mjDATATYPE_REAL) {
+          d->sensordata[adr+j] = mju_clip(d->sensordata[adr+j], -cutoff, cutoff);
+        }
 
         // positive: apply on positive side only
-        else if (m->sensor_datatype[i]==mjDATATYPE_POSITIVE)
-          d->sensordata[adr+j] =
-            mju_min(cutoff, d->sensordata[adr+j]);
+        else if (m->sensor_datatype[i]==mjDATATYPE_POSITIVE) {
+          d->sensordata[adr+j] = mju_min(cutoff, d->sensordata[adr+j]);
+        }
+      }
     }
   }
 }
@@ -312,6 +313,10 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
         mju_copy3(d->sensordata+adr, d->subtree_com+3*objid);
         break;
 
+      case mjSENS_CLOCK:                                  // clock
+        d->sensordata[adr] = d->time;
+        break;
+
       case mjSENS_USER:                                   // user
         nusersensor++;
         break;

src/user/user_objects.cc

@@ -3533,7 +3533,7 @@ void mjCSensor::Compile(void) {
 
     // get sensorized object id
     objid = pobj->id;
-  } else {
+  } else if (type != mjSENS_CLOCK) {
     throw mjCError(this, "invalid type in sensor '%s' (id = %d)", name.c_str(), id);
   }
 
@@ -3804,6 +3804,12 @@ void mjCSensor::Compile(void) {
     }
     break;
 
+  case mjSENS_CLOCK:
+    dim = 1;
+    needstage = mjSTAGE_POS;
+    datatype = mjDATATYPE_REAL;
+    break;
+
   case mjSENS_USER:
     // check for negative dim
     if (dim<0) {

src/xml/xml_native_reader.cc

@@ -42,7 +42,7 @@ using tinyxml2::XMLElement;
 
 //---------------------------------- MJCF schema ---------------------------------------------------
 
-static const int nMJCF = 162;
+static const int nMJCF = 163;
 static const char* MJCF[nMJCF][mjXATTRNUM] = {
 {"mujoco", "!", "1", "model"},
 {"<"},
@@ -342,6 +342,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
         {"subtreecom", "*", "5", "name", "body", "cutoff", "noise", "user"},
         {"subtreelinvel", "*", "5", "name", "body", "cutoff", "noise", "user"},
         {"subtreeangmom", "*", "5", "name", "body", "cutoff", "noise", "user"},
+        {"clock", "*", "4", "name", "cutoff", "noise", "user"},
         {"user", "*", "9", "name", "objtype", "objname", "datatype", "needstage",
             "dim", "cutoff", "noise", "user"},
     {">"},
@@ -2735,6 +2736,12 @@ void mjXReader::Sensor(XMLElement* section) {
       ReadAttrTxt(elem, "body", psen->objname, true);
     }
 
+    // global sensors
+    else if (type=="clock") {
+      psen->type = mjSENS_CLOCK;
+      psen->objtype = mjOBJ_UNKNOWN;
+    }
+
     // user-defined sensor
     else if (type=="user") {
       psen->type = mjSENS_USER;

src/xml/xml_native_writer.cc

@@ -1541,6 +1541,11 @@ void mjXWriter::Sensor(XMLElement* root) {
       WriteAttrTxt(elem, "body", psen->objname);
       break;
 
+    // global sensors
+    case mjSENS_CLOCK:
+      elem = InsertEnd(section, "clock");
+      break;
+
     // user-defined sensor
     case mjSENS_USER:
       elem = InsertEnd(section, "user");