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

Add flag for disabling implicit integration of joint damping in the Euler integrator.

Browse Source 
PiperOrigin-RevId: 559398414
Change-Id: I43b23c5e75a2830a64b1898daeaa16f19e011718
This commit is contained in:
Yuval Tassa
2023-08-23 05:50:52 -07:00
committed by Copybara-Service
parent 5f2a2a325c
commit b7cf479abe
13 changed files with 156 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+6
View File
@@ -2104,6 +2104,12 @@ from its default.
This flag disables the mid-phase collision filtering using a static AABB bounding volume hierarchy (a BVH binary
tree). If disabled, all geoms pairs that are allowed to collide are checked for collisions.
.. _option-flag-eulerdamp:
:at:`eulerdamp`: :at-val:`[disable, enable], "enable"`
This flag disables implicit integration with respect to joint damping in the Euler integrator. See the
:ref:`Numerical Integration<geIntegration>` section for more details.
.. _option-flag-override:
:at:`override`: :at-val:`[disable, enable], "disable"`
doc/XMLschema.rst
+4 -2
View File
@@ -239,9 +239,11 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`warmstart<option-flag-warmstart>` | :ref:`filterparent<option-flag-filterparent>` | :ref:`actuation<option-flag-actuation>` | :ref:`refsafe<option-flag-refsafe>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`sensor<option-flag-sensor>` | :ref:`override<option-flag-override>` | :ref:`energy<option-flag-energy>` | :ref:`fwdinv<option-flag-fwdinv>` | |
| | | | :ref:`sensor<option-flag-sensor>` | :ref:`midphase<option-flag-midphase>` | :ref:`eulerdamp<option-flag-eulerdamp>` | :ref:`override<option-flag-override>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`sensornoise<option-flag-sensornoise>` | :ref:`multiccd<option-flag-multiccd>` | :ref:`island<option-flag-island>` | | |
| | | | :ref:`energy<option-flag-energy>` | :ref:`fwdinv<option-flag-fwdinv>` | :ref:`sensornoise<option-flag-sensornoise>` | :ref:`multiccd<option-flag-multiccd>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`island<option-flag-island>` | | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| mujoco |br| |L| | | .. table:: |
doc/changelog.rst
+3 -1
View File
@@ -34,11 +34,13 @@ General
#. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
:ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and
:ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively.
#. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
Euler integrator. See the :ref:`Numerical Integration<geIntegration>` section for more details.
Python bindings
^^^^^^^^^^^^^^^
4. Fixed `#870 <https://github.com/deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
7. Fixed `#870 <https://github.com/deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
camera name used the default camera.
doc/computation.rst
+4 -2
View File
@@ -539,7 +539,9 @@ All three single-step integrators in MuJoCo use the update :eq:`eq_implicit_upda
Semi-implicit with implicit joint damping (``Euler``)
For this method, :math:`D` only includes derivatives of joint damping. Note that in this case :math:`D` is diagonal
and :math:`M-h D` is symmetric, so Cholesky decomposition can be used.
and :math:`M-h D` is symmetric, so Cholesky decomposition can be used. If the model has no joint damping or the
:ref:`eulerdamp<option-flag-eulerdamp>` disable-flag is set, implicit damping is disabled and the semi-implicit
update :eq:`eq_semimplicit` is used, rather than :eq:`eq_implicit_update`.
Implicit-in-velocity (``implicit``)
For this method, :math:`D` includes derivatives of all forces except the constraint forces :math:`J^T f(v)`. These
@@ -690,7 +692,7 @@ also treated as an input variable. All other :ref:`mjData` fields are functions
Note that the full integration state as given by :ref:`mjSTATE_INTEGRATION<mjtState>` is maximalist and includes fields
which are often unused. If a small state size is desired, it might be sensible to avoid saving unused fields.
In particular `xfrc_applied`` can be quite large (``6 x nbody``) yet is often unused.
In particular ``xfrc_applied`` can be quite large (``6 x nbody``) yet is often unused.
.. _geSimulationState:
doc/includes/references.h
+2 -1
View File
@@ -387,8 +387,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags
mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep
mjDSBL_SENSOR = 1<<12, // sensors
mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering
mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator
mjNDISABLE = 14 // number of disable flags
mjNDISABLE = 15 // number of disable flags
} mjtDisableBit;
typedef enum mjtEnableBit_ { // enable optional feature bitflags
mjENBL_OVERRIDE = 1<<0, // override contact parameters
include/mujoco/mjmodel.h
+2 -1
View File
@@ -58,8 +58,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags
mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep
mjDSBL_SENSOR = 1<<12, // sensors
mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering
mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator
mjNDISABLE = 14 // number of disable flags
mjNDISABLE = 15 // number of disable flags
} mjtDisableBit;
introspect/enums.py
+2 -1
View File
@@ -41,7 +41,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjDSBL_REFSAFE', 2048),
('mjDSBL_SENSOR', 4096),
('mjDSBL_MIDPHASE', 8192),
('mjNDISABLE', 14),
('mjDSBL_EULERDAMP', 16384),
('mjNDISABLE', 15),
]),
)),
('mjtEnableBit',
src/engine/engine_forward.c
+8 -6
View File
@@ -593,16 +593,18 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
mjtNum* qfrc = mj_stackAlloc(d, nv);
mjtNum* qacc = mj_stackAlloc(d, nv);
// check for dof damping
// check for dof damping if disable flag is not set
int dof_damping = 0;
for (int i=0; i < nv; i++) {
if (m->dof_damping[i] > 0) {
dof_damping = 1;
break;
if (!mjDISABLED(mjDSBL_EULERDAMP)) {
for (int i=0; i < nv; i++) {
if (m->dof_damping[i] > 0) {
dof_damping = 1;
break;
}
}
}
// no damping: explicit velocity integration
// no damping or disabled: explicit velocity integration
if (!dof_damping) {
mju_copy(qacc, d->qacc, nv);
}
src/engine/engine_support.c
+2 -1
View File
@@ -56,7 +56,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = {
"Actuation",
"Refsafe",
"Sensor",
"Midphase"
"Midphase",
"Eulerdamp"
};
src/xml/xml_native_reader.cc
+3 -2
View File
@@ -103,9 +103,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"solver", "iterations", "noslip_iterations", "mpr_iterations",
"sdf_iterations", "sdf_initpoints"},
{"<"},
{"flag", "?", "19", "constraint", "equality", "frictionloss", "limit", "contact",
{"flag", "?", "21", "constraint", "equality", "frictionloss", "limit", "contact",
"passive", "gravity", "clampctrl", "warmstart",
"filterparent", "actuation", "refsafe", "sensor",
"filterparent", "actuation", "refsafe", "sensor", "midphase", "eulerdamp",
"override", "energy", "fwdinv", "sensornoise", "multiccd", "island"},
{">"},
@@ -1003,6 +1003,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
READDSBL("refsafe", mjDSBL_REFSAFE)
READDSBL("sensor", mjDSBL_SENSOR)
READDSBL("midphase", mjDSBL_MIDPHASE)
READDSBL("eulerdamp", mjDSBL_EULERDAMP)
#undef READDSBL
#define READENBL(NAME, MASK) \
src/xml/xml_native_writer.cc
+1
View File
@@ -836,6 +836,7 @@ void mjXWriter::Option(XMLElement* root) {
WRITEDSBL("refsafe", mjDSBL_REFSAFE)
WRITEDSBL("sensor", mjDSBL_SENSOR)
WRITEDSBL("midphase", mjDSBL_MIDPHASE)
WRITEDSBL("eulerdamp", mjDSBL_EULERDAMP)
#undef WRITEDSBL
#define WRITEENBL(NAME, MASK) \
test/engine/engine_forward_test.cc
+117
View File
@@ -164,6 +164,123 @@ TEST_F(ForwardTest, DamperDampens) {
using ImplicitIntegratorTest = MujocoTest;
// Disabling implicit joint damping works as expected
TEST_F(ImplicitIntegratorTest, EulerDampDisable) {
static constexpr char xml[] = R"(
<mujoco>
<option>
<flag eulerdamp="disable"/>
</option>
<worldbody>
<body>
<joint axis="1 0 0" damping="2"/>
<geom type="capsule" size=".01" fromto="0 0 0 0 .1 0"/>
<body pos="0 .1 0">
<joint axis="0 1 0" damping="1"/>
<geom type="capsule" size=".01" fromto="0 0 0 .1 0 0"/>
</body>
</body>
</worldbody>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
// step once, call mj_forward, save qvel and qacc
mj_step(model, data);
mj_forward(model, data);
std::vector<mjtNum> qvel = AsVector(data->qvel, model->nv);
std::vector<mjtNum> qacc = AsVector(data->qacc, model->nv);
// second step
mj_step(model, data);
// compute finite-difference acceleration
std::vector<mjtNum> qacc_fd(model->nv);
for (int i=0; i < model->nv; i++) {
qacc_fd[i] = (data->qvel[i] - qvel[i]) / model->opt.timestep;
}
// expect finite-differenced qacc to match to high precision
EXPECT_THAT(qacc_fd, Pointwise(DoubleNear(1e-14), qacc));
// reach the the same initial state
mj_resetData(model, data);
mj_step(model, data);
// second step again, but with implicit integration of joint damping
model->opt.disableflags &= ~mjDSBL_EULERDAMP;
mj_step(model, data);
// compute finite-difference acceleration difference
std::vector<mjtNum> dqacc(model->nv);
for (int i=0; i < model->nv; i++) {
dqacc[i] = (data->qvel[i] - qvel[i]) / model->opt.timestep;
}
// expect finite-differenced qacc to not match
EXPECT_GT(mju_norm(dqacc.data(), model->nv), 1);
mj_deleteData(data);
mj_deleteModel(model);
}
// Reducing timesteps reduces the difference between implicit/explicit
TEST_F(ImplicitIntegratorTest, EulerDampLimit) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint axis="1 0 0" damping="2"/>
<geom type="capsule" size=".01" fromto="0 0 0 0 .1 0"/>
<body pos="0 .1 0">
<joint axis="0 1 0" damping="1"/>
<geom type="capsule" size=".01" fromto="0 0 0 .1 0 0"/>
</body>
</body>
</worldbody>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
mjtNum diff_norm_prev = -1;
for (const mjtNum dt : {1e-2, 1e-3, 1e-4, 1e-5, 1e-6, 1e-7, 1e-8}) {
// set timestep
model->opt.timestep = dt;
// step twice with implicit damping, save qvel
model->opt.disableflags &= ~mjDSBL_EULERDAMP;
mj_resetData(model, data);
mj_step(model, data);
mj_step(model, data);
std::vector<mjtNum> qvel_imp = AsVector(data->qvel, model->nv);
// step once, step again without implicit damping, save qvel
mj_resetData(model, data);
mj_step(model, data);
model->opt.disableflags |= mjDSBL_EULERDAMP;
mj_step(model, data);
std::vector<mjtNum> qvel_exp = AsVector(data->qvel, model->nv);
mjtNum diff_norm = 0;
for (int i=0; i < model->nv; i++) {
diff_norm += (qvel_imp[i] - qvel_exp[i]) * (qvel_imp[i] - qvel_exp[i]);
}
diff_norm = mju_sqrt(diff_norm);
if (diff_norm_prev != -1){
EXPECT_LT(diff_norm, diff_norm_prev);
}
diff_norm_prev = diff_norm;
}
mj_deleteData(data);
mj_deleteModel(model);
}
// Euler and implicit should be equivalent if there is only joint damping
TEST_F(ImplicitIntegratorTest, EulerImplicitEqivalent) {
static constexpr char xml[] = R"(
unity/Runtime/Bindings/MjBindings.cs
+2 -1
View File
@@ -151,7 +151,8 @@ public enum mjtDisableBit : int{
mjDSBL_REFSAFE = 2048,
mjDSBL_SENSOR = 4096,
mjDSBL_MIDPHASE = 8192,
mjNDISABLE = 14,
mjDSBL_EULERDAMP = 16384,
mjNDISABLE = 15,
}
public enum mjtEnableBit : int{
mjENBL_OVERRIDE = 1,