diff --git a/doc/APIreference.rst b/doc/APIreference.rst index 05f323ce..c9c397cf 100644 --- a/doc/APIreference.rst +++ b/doc/APIreference.rst @@ -86,8 +86,9 @@ mjtDisableBit mjDSBL_FILTERPARENT = 1<<9, // remove collisions with parent body mjDSBL_ACTUATION = 1<<10, // apply actuation forces mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep + mjDSBL_SENSOR = 1<<12, // sensors - mjNDISABLE = 12 // number of disable flags + mjNDISABLE = 13 // number of disable flags } mjtDisableBit; | Defined in `mjmodel.h `_ diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 69616399..6a8386b2 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -94,9 +94,9 @@ in the second column of the table have the following meaning: | | | +-------------------------+-------------------------+-------------------------+ | | | | | :at:`filterparent` | :at:`actuation` | :at:`refsafe` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`override` | :at:`energy` | :at:`fwdinv` | | +| | | | :at:`sensor` | :at:`override` | :at:`energy` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`sensornoise` | :at:`multiccd` | | | +| | | | :at:`fwdinv` | :at:`sensornoise` | :at:`multiccd` | | | | | +-------------------------+-------------------------+-------------------------+ | +--------------------------+----+------------------------------------------------------------------------------------+ | |_|:el:`size` | \* | .. table:: | @@ -1847,6 +1847,9 @@ from its default. simulation timestep. Recall that solref[0] is the stiffness of the virtual spring-damper used for constraint stabilization. If this setting is enabled, the solver uses max(solref[0], 2*timestep) in place of solref[0] separately for each active constraint. +:at:`sensor`: :at-val:`[disable, enable], "enable"` + This flag disables all computations related to sensors. When disabled, sensor values will remain constant, either + zeros if disabled at the start of simulation, or, if disabled at runtime, whatever value was last computed. :at:`override`: :at-val:`[disable, enable], "disable"` This flag enables to :ref:`Contact override ` mechanism explained above. :at:`energy`: :at-val:`[disable, enable], "disable"` @@ -4265,7 +4268,7 @@ specify them independently. If true, the internal state (activation) associated with this actuator is automatically clamped to :at:`actrange` at runtime. If false, activation clamping is disabled. If auto, activation clamping will automatically be set to true if :at:`actrange` is defined without explicitly setting this attribute to "true". See the :ref:`Activation clamping ` - section for more details. + section for more details. :at:`ctrlrange`: :at-val:`real(2), "0 0"` Range for clamping the control input. The compiler expects the first value to be smaller than the second value. :at:`forcerange`: :at-val:`real(2), "0 0"` diff --git a/doc/changelog.rst b/doc/changelog.rst index d8519200..66a05ef4 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -40,6 +40,7 @@ General the free camera at model load time. - Added ``mjv_defaultFreeCamera`` which sets the default free camera, respecting the above attributes. - ``simulate`` now supports taking a screenshot via a button in the File section or via ``Ctrl-P``. +- Added a disable flag for sensors. Deleted/deprecated features ^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 1017b0b9..baf65736 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -58,8 +58,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_FILTERPARENT = 1<<9, // remove collisions with parent body mjDSBL_ACTUATION = 1<<10, // apply actuation forces mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep + mjDSBL_SENSOR = 1<<12, // sensors - mjNDISABLE = 12 // number of disable flags + mjNDISABLE = 13 // number of disable flags } mjtDisableBit; diff --git a/introspect/enums.py b/introspect/enums.py index c319d69e..123b0cb3 100755 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -39,7 +39,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDSBL_FILTERPARENT', 512), ('mjDSBL_ACTUATION', 1024), ('mjDSBL_REFSAFE', 2048), - ('mjNDISABLE', 12), + ('mjDSBL_SENSOR', 4096), + ('mjNDISABLE', 13), ]), )), ('mjtEnableBit', diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 463f1aa3..48dcc7b5 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -193,6 +193,11 @@ void mj_sensorPos(const mjModel* m, mjData* d) { int ne = d->ne, nf = d->nf, nefc = d->nefc; mjtNum rvec[3], *xpos, *xmat, *xpos_ref, *xmat_ref; + // disabled sensors: return + if (mjDISABLED(mjDSBL_SENSOR)) { + return; + } + // process sensors matching stage for (int i=0; insensor; i++) { if (m->sensor_needstage[i]==mjSTAGE_POS) { @@ -349,6 +354,11 @@ void mj_sensorVel(const mjModel* m, mjData* d) { int ne = d->ne, nf = d->nf, nefc = d->nefc; mjtNum xvel[6]; + // disabled sensors: return + if (mjDISABLED(mjDSBL_SENSOR)) { + return; + } + // process sensors matching stage int subtreeVel = 0; for (int i=0; insensor; i++) { @@ -502,6 +512,11 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { mjtNum tmp[6], conforce[6], conray[3]; mjContact* con; + // disabled sensors: return + if (mjDISABLED(mjDSBL_SENSOR)) { + return; + } + // process sensors matching stage int rnePost = 0; for (int i=0; insensor; i++) { diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 72835b4b..ceeddf3e 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -51,7 +51,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = { "Warmstart", "Filterparent", "Actuation", - "Refsafe" + "Refsafe", + "Sensor" }; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index f5eda65c..1947c0fc 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -63,9 +63,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "integrator", "collision", "cone", "jacobian", "solver", "iterations", "noslip_iterations", "mpr_iterations"}, {"<"}, - {"flag", "?", "17", "constraint", "equality", "frictionloss", "limit", "contact", + {"flag", "?", "18", "constraint", "equality", "frictionloss", "limit", "contact", "passive", "gravity", "clampctrl", "warmstart", - "filterparent", "actuation", "refsafe", + "filterparent", "actuation", "refsafe", "sensor", "override", "energy", "fwdinv", "sensornoise", "multiccd"}, {">"}, @@ -887,6 +887,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { READDSBL("filterparent", mjDSBL_FILTERPARENT) READDSBL("actuation", mjDSBL_ACTUATION) READDSBL("refsafe", mjDSBL_REFSAFE) + READDSBL("sensor", mjDSBL_SENSOR) #undef READDSBL #define READENBL(NAME, MASK) \ diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index e22280b6..9f7245a4 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -752,6 +752,7 @@ void mjXWriter::Option(XMLElement* root) { WRITEDSBL("filterparent", mjDSBL_FILTERPARENT) WRITEDSBL("actuation", mjDSBL_ACTUATION) WRITEDSBL("refsafe", mjDSBL_REFSAFE) + WRITEDSBL("sensor", mjDSBL_SENSOR) #undef WRITEDSBL #define WRITEENBL(NAME, MASK) \ diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index 8c65c477..66a0e58b 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -32,21 +32,65 @@ namespace mujoco { namespace { +const mjtNum tol = 1e-14; // nearness tolerance for floating point numbers + // returns as a vector the measured values from sensor with index `id` -static std::vector GetSensor(const mjModel* model, const mjData* data, int id) { - return std::vector(data->sensordata + model->sensor_adr[id], - data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]); +static std::vector GetSensor(const mjModel* model, + const mjData* data, + int id) { + return std::vector( + data->sensordata + model->sensor_adr[id], + data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]); } - -// --------------------- test relative frame sensors -------------------------- - using ::testing::Pointwise; using ::testing::DoubleNear; using ::testing::StrEq; -using RelativeFrameSensorTest = MujocoTest; -const mjtNum tol = 1e-14; // nearness tolerance for floating point numbers +using SensorTest = MujocoTest; + +// --------------------- test sensor disableflag ----------------------------- + +// hand-picked positions and orientations for simple expected values +TEST_F(SensorTest, DisableSensors) { + constexpr char xml[] = R"( + + + + + + )"; + mjModel* model = LoadModelFromString(xml, 0, 0); + mjData* data = mj_makeData(model); + + // before calling anything, check that sensors are initialised to 0 + EXPECT_EQ(data->sensordata[0], 0.0); + + // call mj_step, mj_step1, expect clock to be incremented by timestep + mj_step(model, data); + mj_step1(model, data); + EXPECT_EQ(data->sensordata[0], model->opt.timestep); + + // disable sensors, call mj_step, mj_step1, expect clock to not increment + model->opt.disableflags |= mjDSBL_SENSOR; + mj_step(model, data); + mj_step1(model, data); + EXPECT_EQ(data->time, 2*model->opt.timestep); + EXPECT_EQ(data->sensordata[0], model->opt.timestep); + + // re-enable sensors, call mj_step, mj_step1, expect clock to match time + model->opt.disableflags = 0; + mj_step(model, data); + mj_step1(model, data); + EXPECT_EQ(data->time, data->sensordata[0]); + + mj_deleteData(data); + mj_deleteModel(model); +} + +// --------------------- test relative frame sensors -------------------------- + +using RelativeFrameSensorTest = MujocoTest; // hand-picked positions and orientations for simple expected values TEST_F(RelativeFrameSensorTest, ReferencePosMat) { @@ -161,7 +205,7 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) { std::vector expected_values(data->sensordata, data->sensordata+nsensordata/2); // set qpos to arbitrary values, call mj_forward - for (int i=0; i<7; i++) { + for (int i=0; i < 7; i++) { data->qpos[i] = i+1; } mj_forward(model, data); diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index ad5a8f09..b0c18679 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -52,6 +52,20 @@ using ::testing::NotNull; using XMLWriterTest = MujocoTest; +TEST_F(XMLWriterTest, SavesDisableSensor) { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("sensor=\"disable\"")); + mj_deleteModel(model); +} + TEST_F(XMLWriterTest, KeepsEmptyClasses) { static constexpr char xml[] = R"(