diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index 52f2b317..d9ca5fd3 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -658,6 +658,15 @@ Type of limit specification. .. mujoco-include:: mjtLimited +.. _mjtAlignFree: + +mjtAlignFree +~~~~~~~~~~~~ + +Whether to align free joints with the inertial frame. + +.. mujoco-include:: mjtAlignFree + .. _mjtInertiaFromGeom: mjtInertiaFromGeom diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index f4c90da4..07ef2643 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -822,6 +822,14 @@ has any effect. The settings here are global and apply to the entire model. If this attribute is set to false, computes mesh inertia with the legacy algorithm, which is exact only for convex meshes. If set to true, it is exact for any closed mesh geometry. +.. _compiler-alignfree: + +:at:`alignfree`: :at-val:`[false, true], "false"` + This attribute toggles the default behaviour of an optimization that applies to bodies with a + :ref:`free joint` and no child bodies. + When true, the body frame and free joint will automatically be aligned with inertial frame, which leads to both + faster and more stable simulation. See :ref:`freejoint/align` for details. + .. _compiler-inertiagrouprange: :at:`inertiagrouprange`: :at-val:`int(2), "0 5"` @@ -2251,6 +2259,20 @@ inherited*. If the XML model is saved, it will appear as a regular joint of type Integer group to which the joint belongs. This attribute can be used for custom tags. It is also used by the visualizer to enable and disable the rendering of entire groups of joints. +.. _body-freejoint-align: + +:at:`align`: :at-val:`[false, true, auto], "auto"` + When set to :at-val:`true`, the body frame and free joint will automatically be aligned with inertial frame. When set + to :at-val:`false`, no alignment will occur. When set to :at-val:`auto`, the compiler's + :ref:`alignfree` global attribute will be respected. + + Inertial frame alignment is an optimization only applies to bodies with a free joint and no child bodies ("simple + free bodies"). The alignment diagonalizes the 6x6 inertia matrix and minimizes bias forces, leading to faster and + more stable simulation. While this behaviour is a strict improvement, it modifies the semantics of the free joint, + making ``qpos`` and ``qvel`` values saved in older versions (for example, in :ref:`keyframes`) invalid. + + Note that the :at:`align` attribute is never saved to XML. Instead, the pose of simple free bodies and their children + will be modified such that the body frame and inertial frame are aligned. .. _body-geom: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index a7aa3e66..02bccf3f 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -56,6 +56,8 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`inertiafromgeom` | :ref:`inertiagrouprange` | :ref:`exactmeshinertia` | :ref:`assetdir` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`alignfree` | | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| compiler |br| |_| |L| | | .. table:: | | :ref:`lengthrange | ? | :class: mjcf-attributes | @@ -290,7 +292,7 @@ | :ref:`freejoint | \* | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`name` | :ref:`group` | | | | +| | | | :ref:`name` | :ref:`group` | :ref:`align` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| body |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index ab3a708e..d422d1b0 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -18,13 +18,26 @@ General - The functions ``mjs_findMesh`` and ``mjs_findKeyframe`` were replaced by ``mjs_findElement``, which allows to look for any object type. +- Added the :ref:`nativeccd` flag. When this flag is enabled, general convex collision + detection is handled natively, as opposed to using `libccd `__. This feature is in + early stages of testing. - Added a new way of defining :ref:`connect` equality constraints, using two sites rather than bodies. The new semantic is useful when the assumption that the constraint is satisfied in the base configuration does not hold. In this case the sites will "snap together" at the beginning of the simulation. Additionally, changing the site positions in ``mjModel.site_pos`` at runtime can be used to modify the constraint. -- Added the :ref:`nativeccd` flag. When this flag is enabled, general convex collision - detection is handled natively, as opposed to using `libccd `__. This feature is in - early stages of testing. +- Introduced an optimization that applies to bodies with a :ref:`free joint` and no child bodies (i.e. + simple free-floating bodies): aligning the free joint (body frame) with the inertial frame. The alignment diagonalizes + the related 6x6 inertia sub-matrix, leading to faster simulation, and minimizes bias forces, leading to more + stable simulation of free bodies. + + While this optimization is a strict improvement over unaligned free joints, it also changes the semantics of the + joint's degrees-of-freedom w.r.t to previous versions. Therefore, ``qpos`` and ``qvel`` values saved in older versions + (for example, in :ref:`keyframes`) will become invalid. + + This feature can be toggled individually using the :ref:`freejoint/align` attribute or globally + using the compiler :ref:`alignfree` attribute. The latter attribute currently defaults to "false" + due to the potential breakage described above, but could be changed to "true" in a future release. Aligned free joints + are recommended for all new models. - Added :ref:`mjSpec` option for creating a texture from a buffer. - :ref:`shellinertia ` is now supported by all geom types. - When :ref:`attaching` sub-models, :ref:`keyframes` will now be correctly merged into the diff --git a/doc/includes/references.h b/doc/includes/references.h index 97ead8a2..f6407f6d 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1650,6 +1650,11 @@ typedef enum mjtLimited_ { // type of limit specification mjLIMITED_TRUE, // limited mjLIMITED_AUTO, // limited inferred from presence of range } mjtLimited; +typedef enum mjtAlignFree_ { // whether to align free joints with the inertial frame + mjALIGNFREE_FALSE = 0, // don't align + mjALIGNFREE_TRUE, // align + mjALIGNFREE_AUTO, // respect the global compiler flag +} mjtAlignFree; typedef enum mjtInertiaFromGeom_ { // whether to infer body inertias from child geoms mjINERTIAFROMGEOM_FALSE = 0, // do not use; inertial element required mjINERTIAFROMGEOM_TRUE, // always use; overwrite inertial element @@ -1688,6 +1693,7 @@ typedef struct mjSpec_ { // model specification int inertiafromgeom; // use geom inertias (mjtInertiaFromGeom) int inertiagrouprange[2]; // range of geom groups used to compute inertia mjtByte exactmeshinertia; // if false, use old formula + int alignfree; // align free joints with inertial frame mjLROpt LRopt; // options for lengthrange computation // engine data @@ -1778,6 +1784,7 @@ typedef struct mjsJoint_ { // joint specification double pos[3]; // anchor position double axis[3]; // joint axis double ref; // value at reference configuration: qpos0 + int align; // align free joint with body com (mjtAlignFree) // stiffness double stiffness; // stiffness coefficient diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 292c8b54..24b8c546 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -84,6 +84,12 @@ typedef enum mjtLimited_ { // type of limit specification mjLIMITED_AUTO, // limited inferred from presence of range } mjtLimited; +typedef enum mjtAlignFree_ { // whether to align free joints with the inertial frame + mjALIGNFREE_FALSE = 0, // don't align + mjALIGNFREE_TRUE, // align + mjALIGNFREE_AUTO, // respect the global compiler flag +} mjtAlignFree; + typedef enum mjtInertiaFromGeom_ { // whether to infer body inertias from child geoms mjINERTIAFROMGEOM_FALSE = 0, // do not use; inertial element required @@ -131,6 +137,7 @@ typedef struct mjSpec_ { // model specification int inertiafromgeom; // use geom inertias (mjtInertiaFromGeom) int inertiagrouprange[2]; // range of geom groups used to compute inertia mjtByte exactmeshinertia; // if false, use old formula + int alignfree; // align free joints with inertial frame mjLROpt LRopt; // options for lengthrange computation // engine data @@ -231,6 +238,7 @@ typedef struct mjsJoint_ { // joint specification double pos[3]; // anchor position double axis[3]; // joint axis double ref; // value at reference configuration: qpos0 + int align; // align free joint with body com (mjtAlignFree) // stiffness double stiffness; // stiffness coefficient diff --git a/introspect/enums.py b/introspect/enums.py index 9e24b3b9..f22f485e 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -759,6 +759,16 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjLIMITED_AUTO', 2), ]), )), + ('mjtAlignFree', + EnumDecl( + name='mjtAlignFree', + declname='enum mjtAlignFree_', + values=dict([ + ('mjALIGNFREE_FALSE', 0), + ('mjALIGNFREE_TRUE', 1), + ('mjALIGNFREE_AUTO', 2), + ]), + )), ('mjtInertiaFromGeom', EnumDecl( name='mjtInertiaFromGeom', diff --git a/introspect/structs.py b/introspect/structs.py index 301f2a7a..444b8398 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -8361,6 +8361,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjtByte'), doc='if false, use old formula', ), + StructFieldDecl( + name='alignfree', + type=ValueType(name='int'), + doc='align free joints with inertial frame', + ), StructFieldDecl( name='LRopt', type=ValueType(name='mjLROpt'), @@ -8792,6 +8797,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='double'), doc='value at reference configuration: qpos0', ), + StructFieldDecl( + name='align', + type=ValueType(name='int'), + doc='align free joint with body com (mjtAlignFree)', + ), StructFieldDecl( name='stiffness', type=ValueType(name='double'), diff --git a/mjx/mujoco/mjx/test_data/constraints.xml b/mjx/mujoco/mjx/test_data/constraints.xml index 49895470..08ee2b6a 100644 --- a/mjx/mujoco/mjx/test_data/constraints.xml +++ b/mjx/mujoco/mjx/test_data/constraints.xml @@ -24,7 +24,7 @@ - + @@ -44,22 +44,22 @@ - + - + - + - + diff --git a/src/user/user_init.c b/src/user/user_init.c index e7a1f885..e7028b75 100644 --- a/src/user/user_init.c +++ b/src/user/user_init.c @@ -104,6 +104,7 @@ void mjs_defaultJoint(mjsJoint* joint) { joint->axis[2] = 1; joint->limited = mjLIMITED_AUTO; joint->actfrclimited = mjLIMITED_AUTO; + joint->align = mjALIGNFREE_AUTO; mj_defaultSolRefImp(joint->solref_limit, joint->solimp_limit); mj_defaultSolRefImp(joint->solref_friction, joint->solimp_friction); } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 370140ab..a46c3948 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -1579,6 +1579,33 @@ void mjCBody::Compile(void) { margin = std::max(margin, geoms[i]->margin); } + // check conditions for free-joint alignment + bool align_free = (joints.size() == 1 && // only one joint AND + joints[0]->spec.type == mjJNT_FREE && // it is a free joint AND + bodies.empty() && // no child bodies AND + (joints[0]->spec.align == 1 || // either joint.align="true" + (joints[0]->spec.align == 2 && // or joint.align="auto" + model->alignfree))); // and compiler.align="true" + + // free-joint alignment, phase 1 (this body + child geoms) + double ipos_inverse[3], iquat_inverse[4]; + if (align_free) { + // accumulate iframe transformation to body frame + mjuu_frameaccum(pos, quat, ipos, iquat); + + // compute inverse iframe transformation + mjuu_frameinvert(ipos_inverse, iquat_inverse, ipos, iquat); + + // save iframe, set it to null + mjuu_setvec(ipos, 0, 0, 0); + mjuu_setvec(iquat, 1, 0, 0, 0); + + // apply inverse iframe transformation to all child geoms + for (int i=0; ipos, geoms[i]->quat); + } + } + // compute bounding volume hierarchy ComputeBVH(); @@ -1652,6 +1679,28 @@ void mjCBody::Compile(void) { } } } + + // free joint alignment, phase 2 (transform sites, cameras and lights) + if (align_free) { + // frames have already been compiled and applied to children + + // sites + for (int i=0; ipos, sites[i]->quat); + } + + // cameras + for (int i=0; ipos, cameras[i]->quat); + } + + // lights + for (int i=0; ipos, qunit); + mjuu_rotVecQuat(lights[i]->dir, lights[i]->dir, iquat_inverse); + } + } } diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index a8a81ae6..ea16835f 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -97,11 +97,11 @@ static void UpdateString(string& psuffix, int count, int i) { const char* MJCF[nMJCF][mjXATTRNUM] = { {"mujoco", "!", "1", "model"}, {"<"}, - {"compiler", "*", "20", "autolimits", "boundmass", "boundinertia", "settotalmass", + {"compiler", "*", "21", "autolimits", "boundmass", "boundinertia", "settotalmass", "balanceinertia", "strippath", "coordinate", "angle", "fitaabb", "eulerseq", "meshdir", "texturedir", "discardvisual", "convexhull", "usethread", "fusestatic", "inertiafromgeom", "inertiagrouprange", "exactmeshinertia", - "assetdir"}, + "assetdir", "alignfree"}, {"<"}, {"lengthrange", "?", "10", "mode", "useexisting", "uselimit", "accel", "maxforce", "timeconst", "timestep", @@ -271,7 +271,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { "solreflimit", "solimplimit", "solreffriction", "solimpfriction", "stiffness", "range", "actuatorfrcrange", "actuatorgravcomp", "margin", "ref", "springref", "armature", "damping", "frictionloss", "user"}, - {"freejoint", "*", "2", "name", "group"}, + {"freejoint", "*", "3", "name", "group", "align"}, {"geom", "*", "33", "name", "class", "type", "contype", "conaffinity", "condim", "group", "priority", "size", "material", "friction", "mass", "density", "shellinertia", "solmix", "solref", "solimp", @@ -1021,6 +1021,9 @@ void mjXReader::Compiler(XMLElement* section, mjSpec* spec) { if (MapValue(section, "exactmeshinertia", &n, bool_map, 2)){ spec->exactmeshinertia = (n==1); } + if (MapValue(section, "alignfree", &n, bool_map, 2)) { + spec->alignfree = (n==1); + } // lengthrange subelement XMLElement* elem = FindSubElem(section, "lengthrange"); @@ -3423,6 +3426,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, mjs_setString(joint->name, name.c_str()); } ReadAttrInt(elem, "group", &joint->group); + MapValue(elem, "align", &joint->align, TFAuto_map, 3); } // geom sub-element diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index d572fba4..7de1a05b 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -924,6 +924,9 @@ void mjXWriter::Compiler(XMLElement* root) { if (model->boundinertia) { WriteAttr(section, "boundinertia", 1, &model->boundinertia); } + if (model->alignfree) { + WriteAttrTxt(section, "alignfree", "true"); + } if (!model->autolimits) { WriteAttrTxt(section, "autolimits", "false"); } diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index e3139d0b..167ee0a4 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -72,7 +72,7 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {