Add clock sensor type.

PiperOrigin-RevId: 459720950
Change-Id: Ibda48ae7dc4aa7be451a947a1c53d10a7eaf176b

doc/XMLreference.rst

@@ -5135,6 +5135,16 @@ at a specified body, in global coordinates.
 :at:`body`: :at-val:`string, required`
    Name of the body where the kinematic subtree is rooted.
 
+.. _sensor-clock:
+
+:el-prefix:`sensor/` **clock** (*)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This element creates sensor that returns the simulation time.
+
+:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user`
+   See :ref:`CSensor`.
+
 .. _sensor-user:
 
 :el-prefix:`sensor/` **user** (*)

doc/changelog.rst

@@ -11,6 +11,7 @@ General
 - Added ``mjd_transitionFD`` to compute efficient finite difference approximations of the state-transition and
   control-transition matrices, :ref:`see here<derivatives>` for more details.
 - Added ``ctrl`` attribute to :ref:`keyframes<keyframe>`.
+- Added ``clock`` sensor which :ref:`measures time<sensor-clock>`.
 - Added visualisation groups to skins.
 - Added actuator visualisation for ``free`` and ``ball`` joints and for actuators with ``site`` transmission.
 - Added visualisation for actuator activations.

include/mujoco/mjmodel.h

@@ -310,6 +310,9 @@ typedef enum mjtSensor_ {         // type of sensor
   mjSENS_SUBTREELINVEL,           // 3D linear velocity of subtree
   mjSENS_SUBTREEANGMOM,           // 3D angular momentum of subtree
 
+  // global sensors
+  mjSENS_CLOCK,                   // simulation time
+
   // user-defined sensor
   mjSENS_USER                     // sensor data provided by mjcb_sensor callback
 } mjtSensor;

introspect/enums.py

@@ -330,7 +330,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjSENS_SUBTREECOM', 32),
              ('mjSENS_SUBTREELINVEL', 33),
              ('mjSENS_SUBTREEANGMOM', 34),
-             ('mjSENS_USER', 35),
+             ('mjSENS_CLOCK', 35),
+             ('mjSENS_USER', 36),
          ]),
      )),
     ('mjtStage',

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

test/engine/engine_sensor_test.cc

@@ -24,6 +24,7 @@
 #include <mujoco/mjmodel.h>
 #include <mujoco/mjtnum.h>
 #include <mujoco/mujoco.h>
+#include "src/engine/engine_support.h"
 #include "src/engine/engine_util_blas.h"
 #include "src/engine/engine_util_spatial.h"
 #include "test/fixture.h"
@@ -42,6 +43,7 @@ static std::vector<mjtNum> GetSensor(const mjModel* model, const mjData* data, i
 
 using ::testing::Pointwise;
 using ::testing::DoubleNear;
+using ::testing::StrEq;
 using RelativeFrameSensorTest = MujocoTest;
 
 const mjtNum tol = 1e-14;  // nearness tolerance for floating point numbers
@@ -356,6 +358,40 @@ TEST_F(RelativeFrameSensorTest, FrameVelGeneral) {
   mj_deleteModel(model);
 }
 
+// ------------------------- general sensor tests  -----------------------------
+using SensorTest = MujocoTest;
+
+// test clock sensor
+TEST_F(SensorTest, Clock) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <option timestep="1e-3"/>
+    <sensor>
+      <clock/>
+      <clock name="clampedclock" cutoff="3e-3"/>
+    </sensor>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml, 0, 0);
+  mjData* data = mj_makeData(model);
+
+  // call step 4 times, checking that clock works as expected
+  for (int i=0; i<5; i++) {
+    mj_step(model, data);
+    mj_step1(model, data); // update values of position-based sensors
+    EXPECT_EQ(data->sensordata[0], data->time);
+    EXPECT_EQ(data->sensordata[1], mju_min(data->time, 3e-3));
+  }
+
+  // chack names
+  const char* name0 = mj_id2name(model, mjOBJ_SENSOR, 0);
+  EXPECT_EQ(name0, nullptr);
+  const char* name1 = mj_id2name(model, mjOBJ_SENSOR, 1);
+  EXPECT_THAT(name1, StrEq("clampedclock"));
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
 
 }  // namespace
 }  // namespace mujoco