From c14a7ef4a7912423dfb0520c519f5feaae586afc Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 8 Jul 2022 03:49:29 -0700 Subject: [PATCH] Add `clock` sensor type. PiperOrigin-RevId: 459720950 Change-Id: Ibda48ae7dc4aa7be451a947a1c53d10a7eaf176b --- doc/XMLreference.rst | 10 +++++++++ doc/changelog.rst | 1 + include/mujoco/mjmodel.h | 3 +++ introspect/enums.py | 3 ++- src/engine/engine_io.c | 1 + src/engine/engine_sensor.c | 19 ++++++++++------ src/user/user_objects.cc | 8 ++++++- src/xml/xml_native_reader.cc | 9 +++++++- src/xml/xml_native_writer.cc | 5 +++++ test/engine/engine_sensor_test.cc | 36 +++++++++++++++++++++++++++++++ 10 files changed, 85 insertions(+), 10 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 9b28fac8..35c3fa52 100644 --- a/doc/XMLreference.rst +++ b/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** (*) diff --git a/doc/changelog.rst b/doc/changelog.rst index cac45aca..4d645d9c 100644 --- a/doc/changelog.rst +++ b/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` for more details. - Added ``ctrl`` attribute to :ref:`keyframes`. +- Added ``clock`` sensor which :ref:`measures time`. - Added visualisation groups to skins. - Added actuator visualisation for ``free`` and ``ball`` joints and for actuators with ``site`` transmission. - Added visualisation for actuator activations. diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 43086516..5e2bc93a 100644 --- a/include/mujoco/mjmodel.h +++ b/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; diff --git a/introspect/enums.py b/introspect/enums.py index d36fb89e..fd771d6c 100755 --- a/introspect/enums.py +++ b/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', diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 9c407b38..3bb02b04 100644 --- a/src/engine/engine_io.c +++ b/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: diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index c8600ad2..dddb3f8c 100644 --- a/src/engine/engine_sensor.c +++ b/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; jsensor_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; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index c484a92f..9738fd75 100644 --- a/src/user/user_objects.cc +++ b/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) { diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 9bcce884..dae0b24d 100644 --- a/src/xml/xml_native_reader.cc +++ b/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; diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 1a41fb3f..27dc99d2 100644 --- a/src/xml/xml_native_writer.cc +++ b/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"); diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index c003b070..8c65c477 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -24,6 +24,7 @@ #include #include #include +#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 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"( + + + )"; + 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