chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add a disable flag for sensors.

Browse Source 
PiperOrigin-RevId: 469473066
Change-Id: I296ed790c6261c4fb15b349540edcb77e53e0567
This commit is contained in:
Yuval Tassa
2022-08-23 09:02:15 -07:00
committed by Copybara-Service
parent 83ce236479
commit 090fe2db3f
11 changed files with 101 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference.rst
+2 -1
View File
@@ -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 <https://github.com/deepmind/mujoco/blob/main/include/mujoco/mjmodel.h>`_
doc/XMLreference.rst
+6 -3
View File
@@ -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 <COverride>` 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 <CActRange>`
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"`
doc/changelog.rst
+1
View File
@@ -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
^^^^^^^^^^^^^^^^^^^^^^^^^^^
include/mujoco/mjmodel.h
+2 -1
View File
@@ -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;
introspect/enums.py
+2 -1
View File
@@ -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',
src/engine/engine_sensor.c
+15
View File
@@ -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; i<m->nsensor; 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; i<m->nsensor; 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; i<m->nsensor; i++) {
src/engine/engine_support.c
+2 -1
View File
@@ -51,7 +51,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = {
"Warmstart",
"Filterparent",
"Actuation",
"Refsafe"
"Refsafe",
"Sensor"
};
src/xml/xml_native_reader.cc
+3 -2
View File
@@ -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) \
src/xml/xml_native_writer.cc
+1
View File
@@ -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) \
test/engine/engine_sensor_test.cc
+53 -9
View File
@@ -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<mjtNum> GetSensor(const mjModel* model, const mjData* data, int id) {
return std::vector<mjtNum>(data->sensordata + model->sensor_adr[id],
data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]);
static std::vector<mjtNum> GetSensor(const mjModel* model,
const mjData* data,
int id) {
return std::vector<mjtNum>(
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"(
<mujoco>
<sensor>
<clock/>
</sensor>
</mujoco>
)";
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);
test/xml/xml_native_writer_test.cc
+14
View File
@@ -52,6 +52,20 @@ using ::testing::NotNull;
using XMLWriterTest = MujocoTest;
TEST_F(XMLWriterTest, SavesDisableSensor) {
static constexpr char xml[] = R"(
<mujoco>
<option>
<flag sensor="disable"/>
</option>
</mujoco>
)";
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"(
<mujoco>