From 252a0d73dfddeecba028e1ba32b031d417e95e79 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 6 Nov 2025 12:09:59 -0800 Subject: [PATCH] Add sleep related data structures PiperOrigin-RevId: 829055431 Change-Id: I1ccbd77a57044a754ae7db611b4c2c0010fbda53 --- doc/APIreference/APIglobals.rst | 6 + doc/APIreference/APItypes.rst | 20 ++ doc/XMLreference.rst | 15 + doc/XMLschema.rst | 16 +- doc/changelog.rst | 1 + doc/includes/references.h | 168 +++++---- include/mujoco/mjdata.h | 137 ++++--- include/mujoco/mjmodel.h | 35 +- include/mujoco/mjspec.h | 4 + include/mujoco/mjxmacro.h | 28 +- mjx/mujoco/mjx/_src/io.py | 8 + mjx/mujoco/mjx/_src/types.py | 55 ++- python/mujoco/bindings_test.py | 2 +- python/mujoco/introspect/enums.py | 26 +- python/mujoco/introspect/enums_test.py | 3 +- python/mujoco/introspect/structs.py | 174 +++++++++ src/engine/CMakeLists.txt | 2 + src/engine/engine_init.c | 3 + src/engine/engine_io.c | 14 + src/engine/engine_island.c | 52 ++- src/engine/engine_print.c | 23 +- src/engine/engine_setconst.c | 261 +++++++++++++- src/engine/engine_sleep.c | 99 +++++ src/engine/engine_sleep.h | 36 ++ src/engine/engine_support.c | 3 +- src/user/user_model.cc | 23 +- src/user/user_objects.cc | 4 +- src/xml/xml_base.h | 2 + src/xml/xml_native_reader.cc | 32 +- src/xml/xml_native_writer.cc | 10 + test/engine/CMakeLists.txt | 4 + test/engine/engine_core_smooth_test.cc | 2 +- test/engine/engine_setconst_test.cc | 479 +++++++++++++++++++++++++ test/engine/engine_sleep_test.cc | 181 ++++++++++ unity/Runtime/Bindings/MjBindings.cs | 35 +- wasm/codegen/generated/bindings.cc | 128 +++++++ wasm/codegen/helpers/constants.py | 2 + 37 files changed, 1907 insertions(+), 186 deletions(-) create mode 100644 src/engine/engine_sleep.c create mode 100644 src/engine/engine_sleep.h create mode 100644 test/engine/engine_setconst_test.cc create mode 100644 test/engine/engine_sleep_test.cc diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index 5ba9608c..1adebb87 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -414,6 +414,12 @@ shown in the table below. Their names are in the format ``mjKEY_XXX``. They corr - 50 - The maximum depth of each body and mesh bounding volume hierarchy. If this large limit is exceeded, a warning is raised and ray casting may not be possible. For a balanced hierarchy, this implies 1E15 bounding volumes. + * - ``mjMAXFLEXNODES`` + - 27 + - Some number by Alessio that needs documentation. I guess it's related to trilinear flexes? + * - ``mjMINAWAKE`` + - 10 + - The minimum number of timesteps that must pass after a tree is awoken, before it is allowed to go back to sleep. * - ``mjNEQDATA`` - 11 - The maximal number of real-valued parameters used to define each equality constraint. Determines the size of diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index 783ff2d0..07498d5b 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -380,6 +380,17 @@ last argument to :ref:`mj_local2global`. .. mujoco-include:: mjtSameFrame +.. _mjtSleepPolicy: + +mjtSleepPolicy +~~~~~~~~~~~~~~ + +Sleep policy associated with a tree. The compiler automatically chooses between ``NEVER`` and ``ALLOWED``, but the user +can override this choice. Only the user can set the ``INIT`` policy (initialized as asleep). + +.. mujoco-include:: mjtSleepPolicy + + .. _mjtFlexSelf: mjtFlexSelf @@ -463,6 +474,15 @@ Timer types. The number of timer types is given by ``mjNTIMER`` which is also th .. mujoco-include:: mjtTimer +.. _mjtSleepState: + +mjtSleepState +~~~~~~~~~~~~~ + +Sleep state of an object. + +.. mujoco-include:: mjtSleepState + .. _tyVisEnums: diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 5dfd736c..aa80f6f5 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -460,6 +460,11 @@ adjust it properly through the XML. :at:`ccd_tolerance`: :at-val:`real, "1e-6"` Tolerance threshold used for early termination of the convex collision algorithm. +.. _option-sleep_tolerance: + +:at:`sleep_tolerance`: :at-val:`real, "1e-4"` + Velocity tolerance below which sleeping is possible. Feature under development, documentation pending. + .. _option-sdf_iterations: :at:`sdf_iterations`: :at-val:`int, "10"` @@ -666,6 +671,11 @@ from its default. to instabilities that typically manifest as sliding or wobbling. The implementation of this feature depends on the selected convex collision pipeline, see :ref:`convex collisions` for more details. +.. _option-flag-sleep: + +:at:`sleep`: :at-val:`[disable, enable], "disable"` + This flag enables sleeping. Feature under development, documentation pending. + .. _compiler: **compiler** (*) @@ -2059,6 +2069,11 @@ defined. Its body name is automatically defined as "world". equal to the body's weight and compensates for gravity exactly. Values greater than ``1`` will create a net upwards force or buoyancy effect. +.. _body-sleep: + +:at:`sleep`: :at-val:`[auto, never, allowed, init], "auto"` + Sleep policy for the tree under this body. Feature under development, documentation pending. + .. _body-user: :at:`user`: :at-val:`real(nbody_user), "0 0 ..."` diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 9651628d..56901533 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -12,17 +12,17 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`timestep` | :ref:`impratio` | :ref:`tolerance` | :ref:`ls_tolerance` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`noslip_tolerance` | :ref:`ccd_tolerance` | :ref:`gravity` | :ref:`wind` | | +| | | | :ref:`noslip_tolerance` | :ref:`ccd_tolerance` | :ref:`sleep_tolerance` | :ref:`gravity` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`magnetic` | :ref:`density` | :ref:`viscosity` | :ref:`o_margin` | | +| | | | :ref:`wind` | :ref:`magnetic` | :ref:`density` | :ref:`viscosity` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`o_solref` | :ref:`o_solimp` | :ref:`o_friction` | :ref:`integrator` | | +| | | | :ref:`o_margin` | :ref:`o_solref` | :ref:`o_solimp` | :ref:`o_friction` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`cone` | :ref:`jacobian` | :ref:`solver` | :ref:`iterations` | | +| | | | :ref:`integrator` | :ref:`cone` | :ref:`jacobian` | :ref:`solver` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`ls_iterations` | :ref:`noslip_iterations` | :ref:`ccd_iterations` | :ref:`sdf_iterations` | | +| | | | :ref:`iterations` | :ref:`ls_iterations` | :ref:`noslip_iterations` | :ref:`ccd_iterations` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`sdf_initpoints` | :ref:`actuatorgroupdisable` | | | | +| | | | :ref:`sdf_iterations` | :ref:`sdf_initpoints` | :ref:`actuatorgroupdisable` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| option |br| |_| |L| | | .. table:: | @@ -41,6 +41,8 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`energy` | :ref:`fwdinv` | :ref:`invdiscrete` | :ref:`multiccd` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`sleep` | | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | mujoco |br| |L| | | .. table:: | | :ref:`compiler | \* | :class: mjcf-attributes | @@ -201,7 +203,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`mocap` | :ref:`axisangle` | :ref:`xyaxes` | :ref:`zaxis` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`euler` | :ref:`gravcomp` | :ref:`user` | | | +| | | | :ref:`euler` | :ref:`gravcomp` | :ref:`sleep` | :ref:`user` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| body |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 11ac1091..ab5bd276 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -8,6 +8,7 @@ Upcoming version (not yet released) General ^^^^^^^^^ +- Initial changes related to sleeping. Feature under development, documentation pending. - Added "quadratic" option to :ref:`flexcomp/dof`. This type of fast :ref:`deformable` flex object is similar to the "trilinear" option, but it includes curved deformations. - Raise an error if there are name collisions also during parsing. diff --git a/doc/includes/references.h b/doc/includes/references.h index d7ada630..ce24d87e 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -16,40 +16,40 @@ // Error: C reference not found // NOLINTBEGIN -typedef enum mjtState_ { // state elements - mjSTATE_TIME = 1<<0, // time - mjSTATE_QPOS = 1<<1, // position - mjSTATE_QVEL = 1<<2, // velocity - mjSTATE_ACT = 1<<3, // actuator activation - mjSTATE_WARMSTART = 1<<4, // acceleration used for warmstart - mjSTATE_CTRL = 1<<5, // control - mjSTATE_QFRC_APPLIED = 1<<6, // applied generalized force - mjSTATE_XFRC_APPLIED = 1<<7, // applied Cartesian force/torque - mjSTATE_EQ_ACTIVE = 1<<8, // enable/disable constraints - mjSTATE_MOCAP_POS = 1<<9, // positions of mocap bodies - mjSTATE_MOCAP_QUAT = 1<<10, // orientations of mocap bodies - mjSTATE_USERDATA = 1<<11, // user data - mjSTATE_PLUGIN = 1<<12, // plugin state +typedef enum mjtState_ { // state elements + mjSTATE_TIME = 1<<0, // time + mjSTATE_QPOS = 1<<1, // position + mjSTATE_QVEL = 1<<2, // velocity + mjSTATE_ACT = 1<<3, // actuator activation + mjSTATE_WARMSTART = 1<<4, // acceleration used for warmstart + mjSTATE_CTRL = 1<<5, // control + mjSTATE_QFRC_APPLIED = 1<<6, // applied generalized force + mjSTATE_XFRC_APPLIED = 1<<7, // applied Cartesian force/torque + mjSTATE_EQ_ACTIVE = 1<<8, // enable/disable constraints + mjSTATE_MOCAP_POS = 1<<9, // positions of mocap bodies + mjSTATE_MOCAP_QUAT = 1<<10, // orientations of mocap bodies + mjSTATE_USERDATA = 1<<11, // user data + mjSTATE_PLUGIN = 1<<12, // plugin state - mjNSTATE = 13, // number of state elements + mjNSTATE = 13, // number of state elements // convenience values for commonly used state specifications - mjSTATE_PHYSICS = mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_ACT, - mjSTATE_FULLPHYSICS = mjSTATE_TIME | mjSTATE_PHYSICS | mjSTATE_PLUGIN, - mjSTATE_USER = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED | + mjSTATE_PHYSICS = mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_ACT, + mjSTATE_FULLPHYSICS = mjSTATE_TIME | mjSTATE_PHYSICS | mjSTATE_PLUGIN, + mjSTATE_USER = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED | mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT | mjSTATE_USERDATA, - mjSTATE_INTEGRATION = mjSTATE_FULLPHYSICS | mjSTATE_USER | mjSTATE_WARMSTART + mjSTATE_INTEGRATION = mjSTATE_FULLPHYSICS | mjSTATE_USER | mjSTATE_WARMSTART } mjtState; -typedef enum mjtConstraint_ { // type of constraint - mjCNSTR_EQUALITY = 0, // equality constraint - mjCNSTR_FRICTION_DOF, // dof friction - mjCNSTR_FRICTION_TENDON, // tendon friction - mjCNSTR_LIMIT_JOINT, // joint limit - mjCNSTR_LIMIT_TENDON, // tendon limit - mjCNSTR_CONTACT_FRICTIONLESS, // frictionless contact - mjCNSTR_CONTACT_PYRAMIDAL, // frictional contact, pyramidal friction cone - mjCNSTR_CONTACT_ELLIPTIC // frictional contact, elliptic friction cone +typedef enum mjtConstraint_ { // type of constraint + mjCNSTR_EQUALITY = 0, // equality constraint + mjCNSTR_FRICTION_DOF, // dof friction + mjCNSTR_FRICTION_TENDON, // tendon friction + mjCNSTR_LIMIT_JOINT, // joint limit + mjCNSTR_LIMIT_TENDON, // tendon limit + mjCNSTR_CONTACT_FRICTIONLESS, // frictionless contact + mjCNSTR_CONTACT_PYRAMIDAL, // frictional contact, pyramidal friction cone + mjCNSTR_CONTACT_ELLIPTIC // frictional contact, elliptic friction cone } mjtConstraint; typedef enum mjtConstraintState_ { // constraint state mjCNSTRSTATE_SATISFIED = 0, // constraint satisfied, zero cost (limit, contact) @@ -58,44 +58,49 @@ typedef enum mjtConstraintState_ { // constraint state mjCNSTRSTATE_LINEARPOS, // linear cost, positive side (friction) mjCNSTRSTATE_CONE // squared distance to cone cost (elliptic contact) } mjtConstraintState; -typedef enum mjtWarning_ { // warning types - mjWARN_INERTIA = 0, // (near) singular inertia matrix - mjWARN_CONTACTFULL, // too many contacts in contact list - mjWARN_CNSTRFULL, // too many constraints - mjWARN_VGEOMFULL, // too many visual geoms - mjWARN_BADQPOS, // bad number in qpos - mjWARN_BADQVEL, // bad number in qvel - mjWARN_BADQACC, // bad number in qacc - mjWARN_BADCTRL, // bad number in ctrl +typedef enum mjtWarning_ { // warning types + mjWARN_INERTIA = 0, // (near) singular inertia matrix + mjWARN_CONTACTFULL, // too many contacts in contact list + mjWARN_CNSTRFULL, // too many constraints + mjWARN_VGEOMFULL, // too many visual geoms + mjWARN_BADQPOS, // bad number in qpos + mjWARN_BADQVEL, // bad number in qvel + mjWARN_BADQACC, // bad number in qacc + mjWARN_BADCTRL, // bad number in ctrl - mjNWARNING // number of warnings + mjNWARNING // number of warnings } mjtWarning; -typedef enum mjtTimer_ { // internal timers +typedef enum mjtTimer_ { // internal timers // main api - mjTIMER_STEP = 0, // step - mjTIMER_FORWARD, // forward - mjTIMER_INVERSE, // inverse + mjTIMER_STEP = 0, // step + mjTIMER_FORWARD, // forward + mjTIMER_INVERSE, // inverse // breakdown of step/forward - mjTIMER_POSITION, // fwdPosition - mjTIMER_VELOCITY, // fwdVelocity - mjTIMER_ACTUATION, // fwdActuation - mjTIMER_CONSTRAINT, // fwdConstraint - mjTIMER_ADVANCE, // mj_Euler, mj_implicit + mjTIMER_POSITION, // fwdPosition + mjTIMER_VELOCITY, // fwdVelocity + mjTIMER_ACTUATION, // fwdActuation + mjTIMER_CONSTRAINT, // fwdConstraint + mjTIMER_ADVANCE, // mj_Euler, mj_implicit // breakdown of fwdPosition - mjTIMER_POS_KINEMATICS, // kinematics, com, tendon, transmission - mjTIMER_POS_INERTIA, // inertia computations - mjTIMER_POS_COLLISION, // collision detection - mjTIMER_POS_MAKE, // make constraints - mjTIMER_POS_PROJECT, // project constraints + mjTIMER_POS_KINEMATICS, // kinematics, com, tendon, transmission + mjTIMER_POS_INERTIA, // inertia computations + mjTIMER_POS_COLLISION, // collision detection + mjTIMER_POS_MAKE, // make constraints + mjTIMER_POS_PROJECT, // project constraints // breakdown of mj_collision - mjTIMER_COL_BROAD, // broadphase - mjTIMER_COL_NARROW, // narrowphase + mjTIMER_COL_BROAD, // broadphase + mjTIMER_COL_NARROW, // narrowphase - mjNTIMER // number of timers + mjNTIMER // number of timers } mjtTimer; +typedef enum mjtSleepState_ { // sleep state of an object + mjS_STATIC = -1, // object is static + mjS_ASLEEP = 0, // object is asleep + mjS_AWAKE = 1 // object is awake +} mjtSleepState; struct mjContact_ { // result of collision detection functions // contact parameters set by near-phase collision function mjtNum dist; // distance between nearest points; neg: penetration @@ -189,6 +194,10 @@ struct mjData_ { int nA; // number of non-zeros in constraint inverse inertia matrix int nisland; // number of detected constraint islands int nidof; // number of dofs in all islands + int ntree_awake; // number of awake trees + int nbody_awake; // number of awake dynamic and static bodies + int nparent_awake; // number of bodies with awake parents + int nv_awake; // number of awake dofs // global properties mjtNum time; // simulation time @@ -229,6 +238,9 @@ struct mjData_ { // sensors mjtNum* sensordata; // sensor data array (nsensordata x 1) + // sleep state + int* tree_asleep; // <0: awake; >=0: index cycle of sleeping trees (ntree x 1) + // plugins int* plugin; // copy of m->plugin, required for deletion (nplugin x 1) uintptr_t* plugin_data; // pointer to plugin-managed data structure (nplugin x 1) @@ -297,6 +309,13 @@ struct mjData_ { // computed by mj_collision/mj_collideTree mjtByte* bvh_active; // was bounding volume checked for collision (nbvh x 1) + // computed by mj_updateSleep + int* tree_awake; // is tree awake; 0: asleep; 1: awake (ntree x 1) + int* body_awake; // body sleep state (mjtSleepState) (nbody x 1) + int* body_awake_ind; // indices of awake and static bodies (nbody x 1) + int* parent_awake_ind; // indices of bodies with awake or static parents (nbody x 1) + int* dof_awake_ind; // indices of awake dofs (nv x 1) + //-------------------- POSITION, VELOCITY dependent // computed by mj_fwdVelocity @@ -376,6 +395,12 @@ struct mjData_ { mjtNum* efc_R; // inverse constraint mass (nefc x 1) int* tendon_efcadr; // first efc address involving tendon; -1: none (ntendon x 1) + // computed by mj_island (island tree structure) + int* tree_island; // island id of this tree; -1: none (ntree x 1) + int* island_ntree; // number of trees in this island (nisland x 1) + int* island_itreeadr; // island start address in itree vector (nisland x 1) + int* map_itree2tree; // map from itree to tree (ntree x 1) + // computed by mj_island (island dof structure) int* dof_island; // island id of this dof; -1: none (nv x 1) int* island_nv; // number of dofs in this island (nisland x 1) @@ -476,8 +501,9 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics // experimental features: mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection + mjENBL_SLEEP = 1<<5, // sleeping - mjNENABLE = 5 // number of enable flags + mjNENABLE = 6 // number of enable flags } mjtEnableBit; typedef enum mjtJoint_ { // type of degree of freedom mjJNT_FREE = 0, // global position and orientation (quat) (7) @@ -520,7 +546,7 @@ typedef enum mjtCamLight_ { // tracking mode for camera and light mjCAMLIGHT_TARGETBODYCOM // pos fixed in body, rot tracks target subtree com } mjtCamLight; typedef enum mjtLightType_ { // type of light - mjLIGHT_SPOT = 0, // spot + mjLIGHT_SPOT = 0, // spot mjLIGHT_DIRECTIONAL, // directional mjLIGHT_POINT, // point mjLIGHT_IMAGE, // image-based @@ -754,21 +780,29 @@ typedef enum mjtSameFrame_ { // frame alignment of bodies with their childr mjSAMEFRAME_BODYROT, // frame orientation is same as body orientation mjSAMEFRAME_INERTIAROT // frame orientation is same as inertia orientation } mjtSameFrame; +typedef enum mjtSleepPolicy_ { // per-tree sleep policy + mjSLEEP_AUTO = 0, // compiler chooses sleep policy + mjSLEEP_AUTO_NEVER, // compiler sleep policy: never + mjSLEEP_AUTO_ALLOWED, // compiler sleep policy: allowed + mjSLEEP_NEVER, // user sleep policy: never + mjSLEEP_ALLOWED, // user sleep policy: allowed + mjSLEEP_INIT, // user sleep policy: initialized asleep +} mjtSleepPolicy; typedef enum mjtLRMode_ { // mode for actuator length range computation - mjLRMODE_NONE = 0, // do not process any actuators + mjLRMODE_NONE = 0, // do not process any actuators mjLRMODE_MUSCLE, // process muscle actuators mjLRMODE_MUSCLEUSER, // process muscle and user actuators mjLRMODE_ALL // process all actuators } mjtLRMode; typedef enum mjtFlexSelf_ { // mode for flex selfcollide - mjFLEXSELF_NONE = 0, // no self-collisions + mjFLEXSELF_NONE = 0, // no self-collisions mjFLEXSELF_NARROW, // skip midphase, go directly to narrowphase mjFLEXSELF_BVH, // use BVH in midphase (if midphase enabled) mjFLEXSELF_SAP, // use SAP in midphase mjFLEXSELF_AUTO // choose between BVH and SAP automatically } mjtFlexSelf; typedef enum mjtSDFType_ { // signed distance function (SDF) type - mjSDFTYPE_SINGLE = 0, // single SDF + mjSDFTYPE_SINGLE = 0, // single SDF mjSDFTYPE_INTERSECTION, // max(A, B) mjSDFTYPE_MIDSURFACE, // A - B mjSDFTYPE_COLLISION, // A + B + abs(max(A, B)) @@ -808,6 +842,9 @@ struct mjOption_ { // physics options mjtNum noslip_tolerance; // noslip solver tolerance mjtNum ccd_tolerance; // convex collision solver tolerance + // sleep settings + mjtNum sleep_tolerance; // sleep velocity tolerance + // physical constants mjtNum gravity[3]; // gravitational acceleration mjtNum wind[3]; // wind (for lift, drag and viscosity) @@ -1129,6 +1166,14 @@ struct mjModel_ { mjtNum* dof_damping; // damping coefficient (nv x 1) mjtNum* dof_invweight0; // diag. inverse inertia in qpos0 (nv x 1) mjtNum* dof_M0; // diag. inertia in qpos0 (nv x 1) + mjtNum* dof_length; // linear: 1; angular: approx. length scale (nv x 1) + + // trees + int* tree_bodyadr; // start addr of bodies (ntree x 1) + int* tree_bodynum; // number of bodies in tree (ntree x 1) + int* tree_dofadr; // start addr of dofs (ntree x 1) + int* tree_dofnum; // number of dofs in tree (ntree x 1) + int* tree_sleep_policy; // sleep policy (mjtSleepPolicy) (ntree x 1) // geoms int* geom_type; // geometric type (mjtGeom) (ngeom x 1) @@ -1396,6 +1441,8 @@ struct mjModel_ { int* tendon_num; // number of objects in tendon's path (ntendon x 1) int* tendon_matid; // material id for rendering (ntendon x 1) int* tendon_group; // group for visibility (ntendon x 1) + int* tendon_treenum; // number of trees along tendon's path (ntendon x 1) + int* tendon_treeid; // first two trees along tendon's path (ntendon x 2) mjtByte* tendon_limited; // does tendon have length limits (ntendon x 1) mjtByte* tendon_actfrclimited; // does tendon have actuator force limits (ntendon x 1) mjtNum* tendon_width; // width for rendering (ntendon x 1) @@ -1928,6 +1975,7 @@ typedef struct mjsBody_ { // body specification // other mjtByte mocap; // is this a mocap body double gravcomp; // gravity compensation + mjtSleepPolicy sleep; // sleep policy mjDoubleVec* userdata; // user data mjtByte explicitinertial; // whether to save the body with explicit inertial clause mjsPlugin plugin; // passive force plugin diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 08642535..d9b539b4 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -24,42 +24,42 @@ //---------------------------------- primitive types (mjt) ----------------------------------------- -typedef enum mjtState_ { // state elements - mjSTATE_TIME = 1<<0, // time - mjSTATE_QPOS = 1<<1, // position - mjSTATE_QVEL = 1<<2, // velocity - mjSTATE_ACT = 1<<3, // actuator activation - mjSTATE_WARMSTART = 1<<4, // acceleration used for warmstart - mjSTATE_CTRL = 1<<5, // control - mjSTATE_QFRC_APPLIED = 1<<6, // applied generalized force - mjSTATE_XFRC_APPLIED = 1<<7, // applied Cartesian force/torque - mjSTATE_EQ_ACTIVE = 1<<8, // enable/disable constraints - mjSTATE_MOCAP_POS = 1<<9, // positions of mocap bodies - mjSTATE_MOCAP_QUAT = 1<<10, // orientations of mocap bodies - mjSTATE_USERDATA = 1<<11, // user data - mjSTATE_PLUGIN = 1<<12, // plugin state +typedef enum mjtState_ { // state elements + mjSTATE_TIME = 1<<0, // time + mjSTATE_QPOS = 1<<1, // position + mjSTATE_QVEL = 1<<2, // velocity + mjSTATE_ACT = 1<<3, // actuator activation + mjSTATE_WARMSTART = 1<<4, // acceleration used for warmstart + mjSTATE_CTRL = 1<<5, // control + mjSTATE_QFRC_APPLIED = 1<<6, // applied generalized force + mjSTATE_XFRC_APPLIED = 1<<7, // applied Cartesian force/torque + mjSTATE_EQ_ACTIVE = 1<<8, // enable/disable constraints + mjSTATE_MOCAP_POS = 1<<9, // positions of mocap bodies + mjSTATE_MOCAP_QUAT = 1<<10, // orientations of mocap bodies + mjSTATE_USERDATA = 1<<11, // user data + mjSTATE_PLUGIN = 1<<12, // plugin state - mjNSTATE = 13, // number of state elements + mjNSTATE = 13, // number of state elements // convenience values for commonly used state specifications - mjSTATE_PHYSICS = mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_ACT, - mjSTATE_FULLPHYSICS = mjSTATE_TIME | mjSTATE_PHYSICS | mjSTATE_PLUGIN, - mjSTATE_USER = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED | + mjSTATE_PHYSICS = mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_ACT, + mjSTATE_FULLPHYSICS = mjSTATE_TIME | mjSTATE_PHYSICS | mjSTATE_PLUGIN, + mjSTATE_USER = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED | mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT | mjSTATE_USERDATA, - mjSTATE_INTEGRATION = mjSTATE_FULLPHYSICS | mjSTATE_USER | mjSTATE_WARMSTART + mjSTATE_INTEGRATION = mjSTATE_FULLPHYSICS | mjSTATE_USER | mjSTATE_WARMSTART } mjtState; -typedef enum mjtConstraint_ { // type of constraint - mjCNSTR_EQUALITY = 0, // equality constraint - mjCNSTR_FRICTION_DOF, // dof friction - mjCNSTR_FRICTION_TENDON, // tendon friction - mjCNSTR_LIMIT_JOINT, // joint limit - mjCNSTR_LIMIT_TENDON, // tendon limit - mjCNSTR_CONTACT_FRICTIONLESS, // frictionless contact - mjCNSTR_CONTACT_PYRAMIDAL, // frictional contact, pyramidal friction cone - mjCNSTR_CONTACT_ELLIPTIC // frictional contact, elliptic friction cone +typedef enum mjtConstraint_ { // type of constraint + mjCNSTR_EQUALITY = 0, // equality constraint + mjCNSTR_FRICTION_DOF, // dof friction + mjCNSTR_FRICTION_TENDON, // tendon friction + mjCNSTR_LIMIT_JOINT, // joint limit + mjCNSTR_LIMIT_TENDON, // tendon limit + mjCNSTR_CONTACT_FRICTIONLESS, // frictionless contact + mjCNSTR_CONTACT_PYRAMIDAL, // frictional contact, pyramidal friction cone + mjCNSTR_CONTACT_ELLIPTIC // frictional contact, elliptic friction cone } mjtConstraint; @@ -72,48 +72,55 @@ typedef enum mjtConstraintState_ { // constraint state } mjtConstraintState; -typedef enum mjtWarning_ { // warning types - mjWARN_INERTIA = 0, // (near) singular inertia matrix - mjWARN_CONTACTFULL, // too many contacts in contact list - mjWARN_CNSTRFULL, // too many constraints - mjWARN_VGEOMFULL, // too many visual geoms - mjWARN_BADQPOS, // bad number in qpos - mjWARN_BADQVEL, // bad number in qvel - mjWARN_BADQACC, // bad number in qacc - mjWARN_BADCTRL, // bad number in ctrl +typedef enum mjtWarning_ { // warning types + mjWARN_INERTIA = 0, // (near) singular inertia matrix + mjWARN_CONTACTFULL, // too many contacts in contact list + mjWARN_CNSTRFULL, // too many constraints + mjWARN_VGEOMFULL, // too many visual geoms + mjWARN_BADQPOS, // bad number in qpos + mjWARN_BADQVEL, // bad number in qvel + mjWARN_BADQACC, // bad number in qacc + mjWARN_BADCTRL, // bad number in ctrl - mjNWARNING // number of warnings + mjNWARNING // number of warnings } mjtWarning; -typedef enum mjtTimer_ { // internal timers +typedef enum mjtTimer_ { // internal timers // main api - mjTIMER_STEP = 0, // step - mjTIMER_FORWARD, // forward - mjTIMER_INVERSE, // inverse + mjTIMER_STEP = 0, // step + mjTIMER_FORWARD, // forward + mjTIMER_INVERSE, // inverse // breakdown of step/forward - mjTIMER_POSITION, // fwdPosition - mjTIMER_VELOCITY, // fwdVelocity - mjTIMER_ACTUATION, // fwdActuation - mjTIMER_CONSTRAINT, // fwdConstraint - mjTIMER_ADVANCE, // mj_Euler, mj_implicit + mjTIMER_POSITION, // fwdPosition + mjTIMER_VELOCITY, // fwdVelocity + mjTIMER_ACTUATION, // fwdActuation + mjTIMER_CONSTRAINT, // fwdConstraint + mjTIMER_ADVANCE, // mj_Euler, mj_implicit // breakdown of fwdPosition - mjTIMER_POS_KINEMATICS, // kinematics, com, tendon, transmission - mjTIMER_POS_INERTIA, // inertia computations - mjTIMER_POS_COLLISION, // collision detection - mjTIMER_POS_MAKE, // make constraints - mjTIMER_POS_PROJECT, // project constraints + mjTIMER_POS_KINEMATICS, // kinematics, com, tendon, transmission + mjTIMER_POS_INERTIA, // inertia computations + mjTIMER_POS_COLLISION, // collision detection + mjTIMER_POS_MAKE, // make constraints + mjTIMER_POS_PROJECT, // project constraints // breakdown of mj_collision - mjTIMER_COL_BROAD, // broadphase - mjTIMER_COL_NARROW, // narrowphase + mjTIMER_COL_BROAD, // broadphase + mjTIMER_COL_NARROW, // narrowphase - mjNTIMER // number of timers + mjNTIMER // number of timers } mjtTimer; +typedef enum mjtSleepState_ { // sleep state of an object + mjS_STATIC = -1, // object is static + mjS_ASLEEP = 0, // object is asleep + mjS_AWAKE = 1 // object is awake +} mjtSleepState; + + //---------------------------------- mjContact ----------------------------------------------------- struct mjContact_ { // result of collision detection functions @@ -221,6 +228,10 @@ struct mjData_ { int nA; // number of non-zeros in constraint inverse inertia matrix int nisland; // number of detected constraint islands int nidof; // number of dofs in all islands + int ntree_awake; // number of awake trees + int nbody_awake; // number of awake dynamic and static bodies + int nparent_awake; // number of bodies with awake parents + int nv_awake; // number of awake dofs // global properties mjtNum time; // simulation time @@ -261,6 +272,9 @@ struct mjData_ { // sensors mjtNum* sensordata; // sensor data array (nsensordata x 1) + // sleep state + int* tree_asleep; // <0: awake; >=0: index cycle of sleeping trees (ntree x 1) + // plugins int* plugin; // copy of m->plugin, required for deletion (nplugin x 1) uintptr_t* plugin_data; // pointer to plugin-managed data structure (nplugin x 1) @@ -329,6 +343,13 @@ struct mjData_ { // computed by mj_collision/mj_collideTree mjtByte* bvh_active; // was bounding volume checked for collision (nbvh x 1) + // computed by mj_updateSleep + int* tree_awake; // is tree awake; 0: asleep; 1: awake (ntree x 1) + int* body_awake; // body sleep state (mjtSleepState) (nbody x 1) + int* body_awake_ind; // indices of awake and static bodies (nbody x 1) + int* parent_awake_ind; // indices of bodies with awake or static parents (nbody x 1) + int* dof_awake_ind; // indices of awake dofs (nv x 1) + //-------------------- POSITION, VELOCITY dependent // computed by mj_fwdVelocity @@ -408,6 +429,12 @@ struct mjData_ { mjtNum* efc_R; // inverse constraint mass (nefc x 1) int* tendon_efcadr; // first efc address involving tendon; -1: none (ntendon x 1) + // computed by mj_island (island tree structure) + int* tree_island; // island id of this tree; -1: none (ntree x 1) + int* island_ntree; // number of trees in this island (nisland x 1) + int* island_itreeadr; // island start address in itree vector (nisland x 1) + int* map_itree2tree; // map from itree to tree (ntree x 1) + // computed by mj_island (island dof structure) int* dof_island; // island id of this dof; -1: none (nv x 1) int* island_nv; // number of dofs in this island (nisland x 1) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index c664847f..c3ff376a 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -30,6 +30,7 @@ #define mjMAXCONPAIR 50 // maximum number of contacts per geom pair #define mjMAXTREEDEPTH 50 // maximum bounding volume hierarchy depth #define mjMAXFLEXNODES 27 // maximum number of flex nodes +#define mjMINAWAKE 10 // minimum number of timesteps before sleeping //---------------------------------- sizes --------------------------------------------------------- @@ -80,8 +81,9 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics // experimental features: mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection + mjENBL_SLEEP = 1<<5, // sleeping - mjNENABLE = 5 // number of enable flags + mjNENABLE = 6 // number of enable flags } mjtEnableBit; @@ -132,7 +134,7 @@ typedef enum mjtCamLight_ { // tracking mode for camera and light typedef enum mjtLightType_ { // type of light - mjLIGHT_SPOT = 0, // spot + mjLIGHT_SPOT = 0, // spot mjLIGHT_DIRECTIONAL, // directional mjLIGHT_POINT, // point mjLIGHT_IMAGE, // image-based @@ -406,8 +408,18 @@ typedef enum mjtSameFrame_ { // frame alignment of bodies with their childr } mjtSameFrame; +typedef enum mjtSleepPolicy_ { // per-tree sleep policy + mjSLEEP_AUTO = 0, // compiler chooses sleep policy + mjSLEEP_AUTO_NEVER, // compiler sleep policy: never + mjSLEEP_AUTO_ALLOWED, // compiler sleep policy: allowed + mjSLEEP_NEVER, // user sleep policy: never + mjSLEEP_ALLOWED, // user sleep policy: allowed + mjSLEEP_INIT, // user sleep policy: initialized asleep +} mjtSleepPolicy; + + typedef enum mjtLRMode_ { // mode for actuator length range computation - mjLRMODE_NONE = 0, // do not process any actuators + mjLRMODE_NONE = 0, // do not process any actuators mjLRMODE_MUSCLE, // process muscle actuators mjLRMODE_MUSCLEUSER, // process muscle and user actuators mjLRMODE_ALL // process all actuators @@ -415,7 +427,7 @@ typedef enum mjtLRMode_ { // mode for actuator length range computation typedef enum mjtFlexSelf_ { // mode for flex selfcollide - mjFLEXSELF_NONE = 0, // no self-collisions + mjFLEXSELF_NONE = 0, // no self-collisions mjFLEXSELF_NARROW, // skip midphase, go directly to narrowphase mjFLEXSELF_BVH, // use BVH in midphase (if midphase enabled) mjFLEXSELF_SAP, // use SAP in midphase @@ -424,7 +436,7 @@ typedef enum mjtFlexSelf_ { // mode for flex selfcollide typedef enum mjtSDFType_ { // signed distance function (SDF) type - mjSDFTYPE_SINGLE = 0, // single SDF + mjSDFTYPE_SINGLE = 0, // single SDF mjSDFTYPE_INTERSECTION, // max(A, B) mjSDFTYPE_MIDSURFACE, // A - B mjSDFTYPE_COLLISION, // A + B + abs(max(A, B)) @@ -477,6 +489,9 @@ struct mjOption_ { // physics options mjtNum noslip_tolerance; // noslip solver tolerance mjtNum ccd_tolerance; // convex collision solver tolerance + // sleep settings + mjtNum sleep_tolerance; // sleep velocity tolerance + // physical constants mjtNum gravity[3]; // gravitational acceleration mjtNum wind[3]; // wind (for lift, drag and viscosity) @@ -810,6 +825,14 @@ struct mjModel_ { mjtNum* dof_damping; // damping coefficient (nv x 1) mjtNum* dof_invweight0; // diag. inverse inertia in qpos0 (nv x 1) mjtNum* dof_M0; // diag. inertia in qpos0 (nv x 1) + mjtNum* dof_length; // linear: 1; angular: approx. length scale (nv x 1) + + // trees + int* tree_bodyadr; // start addr of bodies (ntree x 1) + int* tree_bodynum; // number of bodies in tree (ntree x 1) + int* tree_dofadr; // start addr of dofs (ntree x 1) + int* tree_dofnum; // number of dofs in tree (ntree x 1) + int* tree_sleep_policy; // sleep policy (mjtSleepPolicy) (ntree x 1) // geoms int* geom_type; // geometric type (mjtGeom) (ngeom x 1) @@ -1077,6 +1100,8 @@ struct mjModel_ { int* tendon_num; // number of objects in tendon's path (ntendon x 1) int* tendon_matid; // material id for rendering (ntendon x 1) int* tendon_group; // group for visibility (ntendon x 1) + int* tendon_treenum; // number of trees along tendon's path (ntendon x 1) + int* tendon_treeid; // first two trees along tendon's path (ntendon x 2) mjtByte* tendon_limited; // does tendon have length limits (ntendon x 1) mjtByte* tendon_actfrclimited; // does tendon have actuator force limits (ntendon x 1) mjtNum* tendon_width; // width for rendering (ntendon x 1) diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 99fa9ab2..170a1879 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -70,6 +70,7 @@ typedef enum mjtMeshInertia_ { // type of mesh inertia mjMESH_INERTIA_SHELL // shell mesh inertia } mjtMeshInertia; + typedef enum mjtMeshBuiltin_ { // type of built-in procedural mesh mjMESH_BUILTIN_NONE = 0, // no built-in mesh mjMESH_BUILTIN_SPHERE, // sphere @@ -81,6 +82,7 @@ typedef enum mjtMeshBuiltin_ { // type of built-in procedural mesh mjMESH_BUILTIN_PLATE, // plate } mjtMeshBuiltin; + typedef enum mjtBuiltin_ { // type of built-in procedural texture mjBUILTIN_NONE = 0, // no built-in texture mjBUILTIN_GRADIENT, // gradient: rgb1->rgb2 @@ -103,6 +105,7 @@ 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 @@ -233,6 +236,7 @@ typedef struct mjsBody_ { // body specification // other mjtByte mocap; // is this a mocap body double gravcomp; // gravity compensation + mjtSleepPolicy sleep; // sleep policy mjDoubleVec* userdata; // user data mjtByte explicitinertial; // whether to save the body with explicit inertial clause mjsPlugin plugin; // passive force plugin diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 06427085..1606aebd 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -26,6 +26,7 @@ X( mjtNum, ls_tolerance ) \ X( mjtNum, noslip_tolerance ) \ X( mjtNum, ccd_tolerance ) \ + X( mjtNum, sleep_tolerance ) \ X( mjtNum, density ) \ X( mjtNum, viscosity ) \ X( mjtNum, o_margin ) @@ -243,7 +244,15 @@ X ( mjtNum, dof_armature, nv, 1 ) \ X ( mjtNum, dof_damping, nv, 1 ) \ X ( mjtNum, dof_invweight0, nv, 1 ) \ - X ( mjtNum, dof_M0, nv, 1 ) + X ( mjtNum, dof_M0, nv, 1 ) \ + X ( mjtNum, dof_length, nv, 1 ) + +#define MJMODEL_POINTERS_TREE \ + X ( int, tree_bodyadr, ntree, 1 ) \ + X ( int, tree_bodynum, ntree, 1 ) \ + X ( int, tree_dofadr, ntree, 1 ) \ + X ( int, tree_dofnum, ntree, 1 ) \ + X ( int, tree_sleep_policy, ntree, 1 ) #define MJMODEL_POINTERS_GEOM \ X ( int, geom_type, ngeom, 1 ) \ @@ -509,6 +518,8 @@ X ( int, tendon_num, ntendon, 1 ) \ X ( int, tendon_matid, ntendon, 1 ) \ X ( int, tendon_group, ntendon, 1 ) \ + X ( int, tendon_treenum, ntendon, 1 ) \ + X ( int, tendon_treeid, ntendon, 2 ) \ X ( mjtByte, tendon_limited, ntendon, 1 ) \ X ( mjtByte, tendon_actfrclimited, ntendon, 1 ) \ X ( mjtNum, tendon_width, ntendon, 1 ) \ @@ -586,6 +597,7 @@ X ( mjtNum, oct_coeff, noct, 8 ) \ MJMODEL_POINTERS_JOINT \ MJMODEL_POINTERS_DOF \ + MJMODEL_POINTERS_TREE \ MJMODEL_POINTERS_GEOM \ MJMODEL_POINTERS_SITE \ MJMODEL_POINTERS_CAMERA \ @@ -694,6 +706,7 @@ X ( mjtNum, act_dot, na, 1 ) \ X ( mjtNum, userdata, nuserdata, 1 ) \ X ( mjtNum, sensordata, nsensordata, 1 ) \ + X ( int, tree_asleep, ntree, 1 ) \ X ( int, plugin, nplugin, 1 ) \ X ( uintptr_t, plugin_data, nplugin, 1 ) \ X ( mjtNum, xpos, nbody, 3 ) \ @@ -742,6 +755,11 @@ XNV ( mjtNum, qLD, nC, 1 ) \ X ( mjtNum, qLDiagInv, nv, 1 ) \ X ( mjtByte, bvh_active, nbvh, 1 ) \ + X ( int, tree_awake, ntree, 1 ) \ + X ( int, body_awake, nbody, 1 ) \ + X ( int, body_awake_ind, nbody, 1 ) \ + X ( int, parent_awake_ind, nbody, 1 ) \ + X ( int, dof_awake_ind, nv, 1 ) \ X ( mjtNum, flexedge_velocity, nflexedge, 1 ) \ X ( mjtNum, ten_velocity, ntendon, 1 ) \ X ( mjtNum, actuator_velocity, nu, 1 ) \ @@ -810,6 +828,10 @@ // array fields of mjData that are used for constraint islands #define MJDATA_ARENA_POINTERS_ISLAND \ + X ( int, tree_island, MJ_M(ntree), 1 ) \ + X ( int, island_ntree, MJ_D(nisland), 1 ) \ + X ( int, island_itreeadr, MJ_D(nisland), 1 ) \ + X ( int, map_itree2tree, MJ_M(ntree), 1 ) \ X ( int, dof_island, MJ_M(nv), 1 ) \ X ( int, island_nv, MJ_D(nisland), 1 ) \ X ( int, island_idofadr, MJ_D(nisland), 1 ) \ @@ -876,6 +898,10 @@ X( int, nA ) \ X( int, nisland ) \ X( int, nidof ) \ + X( int, ntree_awake ) \ + X( int, nbody_awake ) \ + X( int, nparent_awake ) \ + X( int, nv_awake ) \ X( mjtNum, time ) \ X( uintptr_t, threadpool ) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index e1a9d65e..22ab4e24 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -739,6 +739,14 @@ def _make_data_c( 'actuator_moment': (m.nJmom, float_), 'bvh_aabb_dyn': (nbvhdynamic, 6, float_), 'bvh_active': (nbvh, np.uint8), + 'tree_asleep': (m.ntree, int_), + 'tree_awake': (m.ntree, int_), + 'body_awake': (m.nbody, int_), + 'body_awake_ind': (m.nbody, int_), + 'parent_awake_ind': (m.nbody, int_), + 'dof_awake_ind': (m.nv, int_), + 'tree_island': (m.ntree, int_), + 'map_itree2tree': (m.ntree, int_), 'flexedge_velocity': (nflexedge, float_), 'crb': (m.nbody, 10, float_), 'qM': (m.nM, float_), diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 70152333..ea7d869f 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -499,6 +499,7 @@ class OptionC(PyTreeNode): has_fluid_params: bool noslip_tolerance: jax.Array ccd_tolerance: jax.Array + sleep_tolerance: jax.Array noslip_iterations: int ccd_iterations: int sdf_iterations: int @@ -533,6 +534,7 @@ class ModelC(PyTreeNode): nbvh: jax.Array nbvhstatic: jax.Array nbvhdynamic: jax.Array + ntree: jax.Array nflex: jax.Array nflexvert: jax.Array nflexedge: jax.Array @@ -542,7 +544,6 @@ class ModelC(PyTreeNode): nflexevpair: jax.Array nflextexcoord: jax.Array nplugin: jax.Array - ntree: jax.Array narena: jax.Array body_bvhadr: jax.Array body_bvhnum: jax.Array @@ -552,6 +553,12 @@ class ModelC(PyTreeNode): oct_child: jax.Array oct_aabb: jax.Array oct_coeff: jax.Array + dof_length: jax.Array + tree_bodyadr: jax.Array + tree_bodynum: jax.Array + tree_dofadr: jax.Array + tree_dofnum: jax.Array + tree_sleep_policy: jax.Array geom_plugin: jax.Array light_bodyid: jax.Array light_targetbodyid: jax.Array @@ -596,6 +603,17 @@ class ModelC(PyTreeNode): flex_centered: jax.Array flex_bvhadr: jax.Array flex_bvhnum: jax.Array + mesh_polynum: jax.Array + mesh_polyadr: jax.Array + mesh_polynormal: jax.Array + mesh_polyvertadr: jax.Array + mesh_polyvertnum: jax.Array + mesh_polyvert: jax.Array + mesh_polymapadr: jax.Array + mesh_polymapnum: jax.Array + mesh_polymap: jax.Array + tendon_treenum: jax.Array + tendon_treeid: jax.Array actuator_plugin: jax.Array sensor_plugin: jax.Array plugin: jax.Array @@ -613,15 +631,6 @@ class ModelC(PyTreeNode): D_colind: jax.Array # pylint:disable=invalid-name mapM2D: jax.Array # pylint:disable=invalid-name mapD2M: jax.Array # pylint:disable=invalid-name - mesh_polynum: jax.Array - mesh_polyadr: jax.Array - mesh_polynormal: jax.Array - mesh_polyvertadr: jax.Array - mesh_polyvertnum: jax.Array - mesh_polyvert: jax.Array - mesh_polymapadr: jax.Array - mesh_polymapnum: jax.Array - mesh_polymap: jax.Array class ModelJAX(PyTreeNode): @@ -1000,17 +1009,19 @@ class DataC(PyTreeNode): # constant sizes: # TODO(stunya): make these sizes jax.Array? + ncon: int ne: int nf: int nl: int nefc: int - ncon: int # TODO(stunya): remove most of these fields solver_niter: jax.Array - cdof: jax.Array - cinert: jax.Array + tree_asleep: jax.Array + plugin_data: jax.Array light_xpos: jax.Array light_xdir: jax.Array + cdof: jax.Array + cinert: jax.Array flexvert_xpos: jax.Array flexelem_aabb: jax.Array flexedge_J_rownnz: jax.Array # pylint:disable=invalid-name @@ -1018,6 +1029,7 @@ class DataC(PyTreeNode): flexedge_J_colind: jax.Array # pylint:disable=invalid-name flexedge_J: jax.Array # pylint:disable=invalid-name flexedge_length: jax.Array + bvh_aabb_dyn: jax.Array ten_wrapadr: jax.Array ten_wrapnum: jax.Array ten_J_rownnz: jax.Array # pylint:disable=invalid-name @@ -1036,25 +1048,28 @@ class DataC(PyTreeNode): M: jax.Array # pylint:disable=invalid-name qLD: jax.Array # pylint:disable=invalid-name qLDiagInv: jax.Array # pylint:disable=invalid-name - bvh_aabb_dyn: jax.Array bvh_active: jax.Array + tree_awake: jax.Array + body_awake: jax.Array + body_awake_ind: jax.Array + parent_awake_ind: jax.Array + dof_awake_ind: jax.Array # position, velocity dependent: flexedge_velocity: jax.Array ten_velocity: jax.Array actuator_velocity: jax.Array cdof_dot: jax.Array - plugin_data: jax.Array + qfrc_spring: jax.Array + qfrc_damper: jax.Array + subtree_linvel: jax.Array + subtree_angmom: jax.Array qH: jax.Array # pylint:disable=invalid-name qHDiagInv: jax.Array # pylint:disable=invalid-name qDeriv: jax.Array # pylint:disable=invalid-name qLU: jax.Array # pylint:disable=invalid-name - qfrc_spring: jax.Array - qfrc_damper: jax.Array cacc: jax.Array cfrc_int: jax.Array cfrc_ext: jax.Array - subtree_linvel: jax.Array - subtree_angmom: jax.Array # dynamically sized arrays which are made static for the frontend JAX API # TODO(stunya): remove these dynamic fields entirely contact: Contact @@ -1064,6 +1079,8 @@ class DataC(PyTreeNode): efc_margin: jax.Array efc_frictionloss: jax.Array efc_D: jax.Array # pylint:disable=invalid-name + tree_island: jax.Array + map_itree2tree: jax.Array efc_aref: jax.Array efc_force: jax.Array diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 2d1f9e2f..2b3856ba 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -950,7 +950,7 @@ Euler integrator, semi-implicit in velocity. self.assertEqual(mujoco.mjtEnableBit.mjENBL_OVERRIDE, 1 << 0) self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1 << 1) self.assertEqual(mujoco.mjtEnableBit.mjENBL_FWDINV, 1 << 2) - self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 5) + self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 6) self.assertEqual(mujoco.mjtGeom.mjGEOM_PLANE, 0) self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1) self.assertEqual(mujoco.mjtGeom.mjGEOM_SPHERE, 2) diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index bb4abd59..a4ba2b37 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -59,7 +59,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjENBL_FWDINV', 4), ('mjENBL_INVDISCRETE', 8), ('mjENBL_MULTICCD', 16), - ('mjNENABLE', 5), + ('mjENBL_SLEEP', 32), + ('mjNENABLE', 6), ]), )), ('mjtJoint', @@ -418,6 +419,19 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjSAMEFRAME_INERTIAROT', 4), ]), )), + ('mjtSleepPolicy', + EnumDecl( + name='mjtSleepPolicy', + declname='enum mjtSleepPolicy_', + values=dict([ + ('mjSLEEP_AUTO', 0), + ('mjSLEEP_AUTO_NEVER', 1), + ('mjSLEEP_AUTO_ALLOWED', 2), + ('mjSLEEP_NEVER', 3), + ('mjSLEEP_ALLOWED', 4), + ('mjSLEEP_INIT', 5), + ]), + )), ('mjtLRMode', EnumDecl( name='mjtLRMode', @@ -553,6 +567,16 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjNTIMER', 15), ]), )), + ('mjtSleepState', + EnumDecl( + name='mjtSleepState', + declname='enum mjtSleepState_', + values=dict([ + ('mjS_STATIC', -1), + ('mjS_ASLEEP', 0), + ('mjS_AWAKE', 1), + ]), + )), ('mjtGeomInertia', EnumDecl( name='mjtGeomInertia', diff --git a/python/mujoco/introspect/enums_test.py b/python/mujoco/introspect/enums_test.py index f2e3c3c0..d023fbcb 100644 --- a/python/mujoco/introspect/enums_test.py +++ b/python/mujoco/introspect/enums_test.py @@ -43,7 +43,8 @@ class EnumsTest(absltest.TestCase): ('mjENBL_FWDINV', 1<<2), ('mjENBL_INVDISCRETE', 1<<3), ('mjENBL_MULTICCD', 1<<4), - ('mjNENABLE', 5))) + ('mjENBL_SLEEP', 1<<5), + ('mjNENABLE', 6))) # values mostly increment by one with occasional overrides def test_mjtGeom(self): # pylint: disable=invalid-name diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 7f88a5ab..d7ba4920 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -148,6 +148,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjtNum'), doc='convex collision solver tolerance', ), + StructFieldDecl( + name='sleep_tolerance', + type=ValueType(name='mjtNum'), + doc='sleep velocity tolerance', + ), StructFieldDecl( name='gravity', type=ArrayType( @@ -1846,6 +1851,54 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='diag. inertia in qpos0', array_extent=('nv',), ), + StructFieldDecl( + name='dof_length', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='linear: 1; angular: approx. length scale', + array_extent=('nv',), + ), + StructFieldDecl( + name='tree_bodyadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='start addr of bodies', + array_extent=('ntree',), + ), + StructFieldDecl( + name='tree_bodynum', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='number of bodies in tree', + array_extent=('ntree',), + ), + StructFieldDecl( + name='tree_dofadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='start addr of dofs', + array_extent=('ntree',), + ), + StructFieldDecl( + name='tree_dofnum', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='number of dofs in tree', + array_extent=('ntree',), + ), + StructFieldDecl( + name='tree_sleep_policy', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='sleep policy (mjtSleepPolicy)', + array_extent=('ntree',), + ), StructFieldDecl( name='geom_type', type=PointerType( @@ -3742,6 +3795,22 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='group for visibility', array_extent=('ntendon',), ), + StructFieldDecl( + name='tendon_treenum', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc="number of trees along tendon's path", + array_extent=('ntendon',), + ), + StructFieldDecl( + name='tendon_treeid', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc="first two trees along tendon's path", + array_extent=('ntendon', 2), + ), StructFieldDecl( name='tendon_limited', type=PointerType( @@ -5136,6 +5205,26 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='number of dofs in all islands', ), + StructFieldDecl( + name='ntree_awake', + type=ValueType(name='int'), + doc='number of awake trees', + ), + StructFieldDecl( + name='nbody_awake', + type=ValueType(name='int'), + doc='number of awake dynamic and static bodies', + ), + StructFieldDecl( + name='nparent_awake', + type=ValueType(name='int'), + doc='number of bodies with awake parents', + ), + StructFieldDecl( + name='nv_awake', + type=ValueType(name='int'), + doc='number of awake dofs', + ), StructFieldDecl( name='time', type=ValueType(name='mjtNum'), @@ -5283,6 +5372,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='sensor data array', array_extent=('nsensordata',), ), + StructFieldDecl( + name='tree_asleep', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='<0: awake; >=0: index cycle of sleeping trees', + array_extent=('ntree',), + ), StructFieldDecl( name='plugin', type=PointerType( @@ -5667,6 +5764,46 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='was bounding volume checked for collision', array_extent=('nbvh',), ), + StructFieldDecl( + name='tree_awake', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='is tree awake; 0: asleep; 1: awake', + array_extent=('ntree',), + ), + StructFieldDecl( + name='body_awake', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='body sleep state (mjtSleepState)', + array_extent=('nbody',), + ), + StructFieldDecl( + name='body_awake_ind', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='indices of awake and static bodies', + array_extent=('nbody',), + ), + StructFieldDecl( + name='parent_awake_ind', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='indices of bodies with awake or static parents', + array_extent=('nbody',), + ), + StructFieldDecl( + name='dof_awake_ind', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='indices of awake dofs', + array_extent=('nv',), + ), StructFieldDecl( name='flexedge_velocity', type=PointerType( @@ -6003,6 +6140,38 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='first efc address involving tendon; -1: none', array_extent=('ntendon',), ), + StructFieldDecl( + name='tree_island', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='island id of this tree; -1: none', + array_extent=('ntree',), + ), + StructFieldDecl( + name='island_ntree', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='number of trees in this island', + array_extent=('nisland',), + ), + StructFieldDecl( + name='island_itreeadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='island start address in itree vector', + array_extent=('nisland',), + ), + StructFieldDecl( + name='map_itree2tree', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='map from itree to tree', + array_extent=('ntree',), + ), StructFieldDecl( name='dof_island', type=PointerType( @@ -6815,6 +6984,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='double'), doc='gravity compensation', ), + StructFieldDecl( + name='sleep', + type=ValueType(name='mjtSleepPolicy'), + doc='sleep policy', + ), StructFieldDecl( name='userdata', type=PointerType( diff --git a/src/engine/CMakeLists.txt b/src/engine/CMakeLists.txt index 96fc10ff..f9980eb4 100644 --- a/src/engine/CMakeLists.txt +++ b/src/engine/CMakeLists.txt @@ -67,6 +67,8 @@ set(MUJOCO_ENGINE_SRCS engine_sensor.h engine_setconst.c engine_setconst.h + engine_sleep.c + engine_sleep.h engine_solver.c engine_solver.h engine_sort.h diff --git a/src/engine/engine_init.c b/src/engine/engine_init.c index 6ca8a7b0..15fe707d 100644 --- a/src/engine/engine_init.c +++ b/src/engine/engine_init.c @@ -62,6 +62,9 @@ void mj_defaultOption(mjOption* opt) { opt->noslip_tolerance = 1e-6; opt->ccd_tolerance = 1e-6; + // sleep settings + opt->sleep_tolerance = 1e-4; + // physical constants opt->gravity[0] = 0; opt->gravity[1] = 0; diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index bdedbaab..53bc690b 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -30,6 +30,7 @@ #include "engine/engine_macro.h" #include "engine/engine_memory.h" #include "engine/engine_plugin.h" +#include "engine/engine_sleep.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" @@ -1361,6 +1362,16 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { mju_copy(d->qpos, m->qpos0, m->nq); } + static int kAwake = -(1+mjMINAWAKE); // tree_asleep value for fully awake tree + + // set all trees to awake + for (int i=0; i < m->ntree; i++) { + d->tree_asleep[i] = kAwake; + } + + // update sleep arrays and counters + mj_updateSleep(m, d); + // set mocap_pos/quat = body_pos/quat for mocap bodies if (m->body_mocapid) { for (int i=0; i < m->nbody; i++) { @@ -1606,6 +1617,8 @@ const char* mj_validateReferences(const mjModel* m) { X(dof_jntid, nv, njnt , 0 ) \ X(dof_parentid, nv, nv , 0 ) \ X(dof_Madr, nv, nM , 0 ) \ + X(tree_bodyadr, ntree, nbody , m->tree_bodynum ) \ + X(tree_dofadr, ntree, nv , m->tree_dofnum ) \ X(geom_bodyid, ngeom, nbody , 0 ) \ X(geom_matid, ngeom, nmat , 0 ) \ X(site_bodyid, nsite, nbody , 0 ) \ @@ -1653,6 +1666,7 @@ const char* mj_validateReferences(const mjModel* m) { X(plugin_attradr, nplugin, npluginattr , 0 ) \ X(tendon_adr, ntendon, nwrap , m->tendon_num ) \ X(tendon_matid, ntendon, nmat , 0 ) \ + X(tendon_treeid, ntendon*2, ntree , 0 ) \ X(numeric_adr, nnumeric, nnumericdata , m->numeric_size ) \ X(text_adr, ntext, ntextdata , m->text_size ) \ X(tuple_adr, ntuple, ntupledata , m->tuple_size ) \ diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 1dff33b1..0d9847d0 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -400,7 +400,7 @@ static int findEdges(const mjModel* m, const mjData* d, int* treenedge, int* edg // discover islands: // nisland, island_idofadr, dof_island, dof_islandnext, island_efcadr, efc_island, efc_islandnext void mj_island(const mjModel* m, mjData* d) { - int nv = m->nv, nefc = d->nefc, ntree=m->ntree; + int nv = m->nv, nefc = d->nefc, ntree = m->ntree, nJ = d->nJ; // no constraints: quick return if (mjDISABLED(mjDSBL_ISLAND) || !nefc) { @@ -411,11 +411,11 @@ void mj_island(const mjModel* m, mjData* d) { mj_markStack(d); // allocate edge array, nJ is an upper bound - int* edge = mjSTACKALLOC(d, 2*d->nJ, int); + int* edge = mjSTACKALLOC(d, 2*nJ, int); // get tree-tree edges and rownnz counts from efc arrays int* rownnz = mjSTACKALLOC(d, ntree, int); // number of edges per tree - int nedge = findEdges(m, d, rownnz, edge, d->nJ); + int nedge = findEdges(m, d, rownnz, edge, nJ); // compute starting address of tree's column indices while resetting rownnz int* rowadr = mjSTACKALLOC(d, ntree, int); @@ -448,8 +448,10 @@ void mj_island(const mjModel* m, mjData* d) { // count nidof: total number of dofs in islands int nidof = 0; - for (int i=0; i < nv; i++) { - nidof += (tree_island[m->dof_treeid[i]] >= 0); + for (int i=0; i < ntree; i++) { + if (tree_island[i] >= 0) { + nidof += m->tree_dofnum[i]; + } } d->nidof = nidof; @@ -463,6 +465,44 @@ void mj_island(const mjModel* m, mjData* d) { int nisland = d->nisland; + // ------------------------------------- trees --------------------------------------------------- + + // copy tree_island from stack to arena + mju_copyInt(d->tree_island, tree_island, ntree); + + // compute island_ntree, number of trees per island + mju_zeroInt(d->island_ntree, nisland); + for (int i=0; i < ntree; i++) { + int island = tree_island[i]; + if (island >= 0) { + d->island_ntree[island]++; + } + } + + // compute island_itreeadr (cumsum of island_ntree) + d->island_itreeadr[0] = 0; + for (int i=1; i < nisland; i++) { + d->island_itreeadr[i] = d->island_itreeadr[i-1] + d->island_ntree[i-1]; + } + int last_tree = d->island_itreeadr[nisland-1] + d->island_ntree[nisland-1]; + + // compute map_itree2tree + int* island_ntree2 = mjSTACKALLOC(d, nisland + 1, int); // last elem counts unconstrained trees + mju_zeroInt(island_ntree2, nisland + 1); + for (int i=0; i < ntree; i++) { + int island = tree_island[i]; + if (island >= 0) { + d->map_itree2tree[d->island_itreeadr[island] + island_ntree2[island]++] = i; + } else { + d->map_itree2tree[last_tree + island_ntree2[nisland]++] = i; + } + } + + // SHOULD NOT OCCUR + if (!mju_compare(island_ntree2, d->island_ntree, nisland)) mjERROR("island_ntree miscount"); + if (last_tree + island_ntree2[nisland] != ntree) mjERROR("miscount of unconstrained trees"); + + // ------------------------------------- degrees of freedom -------------------------------------- // compute dof_island, island_nv @@ -499,7 +539,7 @@ void mj_island(const mjModel* m, mjData* d) { } d->map_dof2idof[dof] = idof; - d->map_idof2dof[idof] = dof; // only the first ni elements of map_idof2dof are in some island + d->map_idof2dof[idof] = dof; // only the first nidof elements of map_idof2dof are in some island } // SHOULD NOT OCCUR diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 396d04da..67dd9d5f 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -665,6 +665,14 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* } if (m->nv) fprintf(fp, "\n"); + // trees + object_class = &m->ntree; + for (int i=0; i < m->ntree; i++) { + fprintf(fp, "\nTREE %d:\n", i); + MJMODEL_POINTERS_TREE + } + if (m->ntree) fprintf(fp, "\n"); + // geoms object_class = &m->ngeom; for (int i=0; i < m->ngeom; i++) { @@ -1227,6 +1235,7 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena printArray2d("ACT_DOT", m->na, 1, d->act_dot, fp, float_format); printArray2d("USERDATA", m->nuserdata, 1, d->userdata, fp, float_format); printArray2d("SENSOR", m->nsensordata, 1, d->sensordata, fp, float_format); + printArray2dInt("TREE_ASLEEP", m->ntree, 1, d->tree_asleep, fp); printArray2d("XPOS", m->nbody, 3, d->xpos, fp, float_format); printArray2d("XQUAT", m->nbody, 4, d->xquat, fp, float_format); @@ -1312,6 +1321,13 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena printArray2d("QHDIAGINV", m->nv, 1, d->qHDiagInv, fp, float_format); } + // computed sleep state + printArray2dInt("TREE_AWAKE", 1, m->ntree, d->tree_awake, fp); + printArray2dInt("BODY_AWAKE", 1, m->nbody, d->body_awake, fp); + printArray2dInt("BODY_AWAKE_IND", 1, d->nbody_awake, d->body_awake_ind, fp); + printArray2dInt("PARENT_AWAKE_IND", 1, d->nparent_awake, d->parent_awake_ind, fp); + printArray2dInt("DOF_AWAKE_IND", 1, d->nv_awake, d->dof_awake_ind, fp); + // print qDeriv if (!mju_isZero(d->qDeriv, m->nD)) { printSparse("QDERIV", d->qDeriv, m->nv, m->D_rownnz, m->D_rowadr, m->D_colind, @@ -1324,7 +1340,7 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena } // contact - fprintf(fp, "CONTACT\n"); + if (d->ncon) fprintf(fp, "CONTACT\n"); for (int i=0; i < d->ncon; i++) { fprintf(fp, " %d:\n dim %d\n", i, d->contact[i].dim); int g1 = d->contact[i].geom[0]; @@ -1454,6 +1470,11 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena printArray2d("CFRC_EXT", m->nbody, 6, d->cfrc_ext, fp, float_format); if (d->nisland) { + printArray2dInt("TREE_ISLAND", 1, m->ntree, d->tree_island, fp); + printArray2dInt("ISLAND_NTREE", 1, d->nisland, d->island_ntree, fp); + printArray2dInt("ISLAND_ITREEADR", 1, d->nisland, d->island_itreeadr, fp); + printArray2dInt("MAP_ITREE2TREE", 1, m->ntree, d->map_itree2tree, fp); + fprintf(fp, NAME_FORMAT, "DOF_ISLAND"); for (int i = 0; i < m->nv; i++) { fprintf(fp, " %d", d->dof_island[i]); diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index a6379ef8..ece96e5a 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -59,6 +59,230 @@ static void mj_setM0(mjModel* m, mjData* d) { } +// helper function to get the tree id of a wrap object +static int GetWrapBodyTreeId(const mjModel* m, int wrap_index) { + int bodyid = -1; + int objid = m->wrap_objid[wrap_index]; + switch ((mjtWrap)m->wrap_type[wrap_index]) { + case mjWRAP_JOINT: + bodyid = m->jnt_bodyid[objid]; + break; + case mjWRAP_SITE: + bodyid = m->site_bodyid[objid]; + break; + case mjWRAP_SPHERE: + case mjWRAP_CYLINDER: + bodyid = m->geom_bodyid[objid]; + break; + case mjWRAP_PULLEY: + case mjWRAP_NONE: + break; + } + return (bodyid != -1) ? m->body_treeid[bodyid] : -1; +} + +// set fixed quantities (do not depend on qpos0) +static void setFixed(mjModel* m, mjData* d) { + mj_markStack(d); + + // ----- general + + // compute subtreemass + for (int i=0; i < m->nbody; i++) { + m->body_subtreemass[i] = m->body_mass[i]; + } + for (int i=m->nbody-1; i > 0; i--) { + m->body_subtreemass[m->body_parentid[i]] += m->body_subtreemass[i]; + } + + // compute ngravcomp: number of bodies with gravity compensation + int ngravcomp = 0; + for (int i=0; i < m->nbody; i++) { + ngravcomp += (m->body_gravcomp[i] > 0); + } + m->ngravcomp = ngravcomp; + + + // ----- tree related (body_treeid and dof_treeid already computed) + + // compute body_treeid + for (int i=0; i < m->nbody; i++) { + int weldid = m->body_weldid[i]; + if (m->body_dofnum[weldid]) { + m->body_treeid[i] = m->dof_treeid[m->body_dofadr[weldid]]; + } else { + m->body_treeid[i] = -1; + } + } + + // compute tree_bodyadr, tree_bodynum + mju_zeroInt(m->tree_bodynum, m->ntree); + int tree_current = -1; + for (int i=1; i < m->nbody; i++) { + int treeid = m->body_treeid[i]; + if (treeid != -1) { + if (treeid > tree_current) { + m->tree_bodyadr[++tree_current] = i; + } + m->tree_bodynum[tree_current]++; + } + } + + // compute tree_dofadr, tree_dofnum + mju_zeroInt(m->tree_dofnum, m->ntree); + tree_current = -1; + for (int i=0; i < m->nv; i++) { + if (m->dof_treeid[i] > tree_current) { + m->tree_dofadr[++tree_current] = i; + } + m->tree_dofnum[tree_current]++; + } + + // compute tendon_treeid, tendon_treenum + int* tree_marker = mjSTACKALLOC(d, m->ntree, int); // 1 if tree has been visited, 0 otherwise + for (int i = 0; i < m->ntendon; i++) { + mju_zeroInt(tree_marker, m->ntree); + m->tendon_treenum[i] = 0; + m->tendon_treeid[2*i] = -1; + m->tendon_treeid[2*i+1] = -1; + + for (int j = m->tendon_adr[i]; j < m->tendon_adr[i] + m->tendon_num[i]; j++) { + int wrap_treeid = GetWrapBodyTreeId(m, j); + if (wrap_treeid != -1 && !tree_marker[wrap_treeid]) { + tree_marker[wrap_treeid] = 1; + if (m->tendon_treenum[i] == 0) { + m->tendon_treeid[2*i] = wrap_treeid; + } else if (m->tendon_treenum[i] == 1) { + m->tendon_treeid[2*i+1] = wrap_treeid; + } + m->tendon_treenum[i]++; + } + } + } + + // ----- apply compiler AUTO tree sleep policy + + // actuators: trees with any actuated joint, site, body, or tendon do not auto-sleep + for (int i=0; i < m->nu; i++) { + int bodyid = -1; + int tid = m->actuator_trnid[2*i]; + switch ((mjtTrn)m->actuator_trntype[i]) { + case mjTRN_JOINT: + case mjTRN_JOINTINPARENT: + bodyid = m->jnt_bodyid[tid]; + break; + case mjTRN_SITE: + case mjTRN_SLIDERCRANK: + bodyid = m->site_bodyid[tid]; + break; + case mjTRN_BODY: + bodyid = tid; + break; + case mjTRN_TENDON: + // wake all trees connected by this actuated tendon + for (int j = m->tendon_adr[tid]; j < m->tendon_adr[tid] + m->tendon_num[tid]; j++) { + int treeid = GetWrapBodyTreeId(m, j); + if (treeid != -1 && m->tree_sleep_policy[treeid] == mjSLEEP_AUTO) { + m->tree_sleep_policy[treeid] = mjSLEEP_AUTO_NEVER; + } + } + continue; // next actuator + case mjTRN_UNDEFINED: + continue; // next actuator + } + + // wake tree containing bodyid, if any + if (bodyid != -1) { + int treeid = m->body_treeid[bodyid]; + if (treeid != -1 && m->tree_sleep_policy[treeid] == mjSLEEP_AUTO) { + m->tree_sleep_policy[treeid] = mjSLEEP_AUTO_NEVER; + } + } + } + + // trees with inter-tree tendons that have non-zero stiffness or damping do not auto-sleep + // if the tendon spans more than 2 trees. + for (int i=0; i < m->ntendon; i++) { + int treenum = m->tendon_treenum[i]; + + // tendon spans 1 or 0 trees: skip + if (treenum < 2) { + continue; + } + + // tendon spans 2 trees and has no stiffness or damping: skip + if (treenum == 2 && m->tendon_stiffness[i] == 0 && m->tendon_damping[i] == 0) { + continue; + } + + // tendon spans two trees with stiffness or damping or more than two trees: wake all trees + mju_zeroInt(tree_marker, m->ntree); + for (int j = m->tendon_adr[i]; j < m->tendon_adr[i] + m->tendon_num[i]; j++) { + int treeid = GetWrapBodyTreeId(m, j); + + // if the tree is not yet marked, mark it and wake it up + if (treeid != -1 && !tree_marker[treeid]) { + tree_marker[treeid] = 1; + int policy = m->tree_sleep_policy[treeid]; + + // mark tree as never sleeping + if (policy == mjSLEEP_AUTO) { + m->tree_sleep_policy[treeid] = mjSLEEP_AUTO_NEVER; + } + + // if the user marked it as sleepable, throw an error + else if (policy == mjSLEEP_ALLOWED || policy == mjSLEEP_INIT) { + mj_freeStack(d); + if (treenum > 2) { + mjERROR("tree %d connected to tendon %d which spans more than 2 trees, " + "sleeping not allowed", treeid, i); + } else { + mjERROR("tree %d connected to tendon %d with non-zero stiffness or damping, " + "sleeping not allowed", treeid, i); + } + } + } + } + } + + // flexes: trees containing bodies that are part of any flex are not allowed to sleep + for (int i = 0; i < m->nflex; ++i) { + // node-based flex + if (m->flex_interp[i]) { + int nodenum = m->flex_nodenum[i]; + int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[i]; + for (int j = 0; j < nodenum; ++j) { + int treeid = m->body_treeid[bodyid[j]]; + if (treeid != -1 && m->tree_sleep_policy[treeid] == mjSLEEP_AUTO) { + m->tree_sleep_policy[treeid] = mjSLEEP_AUTO_NEVER; + } + } + } + + // vertex-based flex + else { + int vertnum = m->flex_vertnum[i]; + int* bodyid = m->flex_vertbodyid + m->flex_vertadr[i]; + for (int j = 0; j < vertnum; ++j) { + int treeid = m->body_treeid[bodyid[j]]; + if (treeid != -1 && m->tree_sleep_policy[treeid] == mjSLEEP_AUTO) { + m->tree_sleep_policy[treeid] = mjSLEEP_AUTO_NEVER; + } + } + } + } + + // set remaining trees with mjSLEEP_AUTO policy to mjSLEEP_AUTO_ALLOWED + for (int i = 0; i < m->ntree; i++) { + if (m->tree_sleep_policy[i] == mjSLEEP_AUTO) { + m->tree_sleep_policy[i] = mjSLEEP_AUTO_ALLOWED; + } + } + + mj_freeStack(d); +} + + // set quantities that depend on qpos0 static void set0(mjModel* m, mjData* d) { int nv = m->nv; @@ -429,6 +653,8 @@ static void setStat(mjModel* m, mjData* d) { mjtNum xmax[3] = {-1E+10, -1E+10, -1E+10}; mjtNum rbound; mj_markStack(d); + + // approximate length associated with each body mjtNum* body = mjSTACKALLOC(d, m->nbody, mjtNum); // compute bounding box of bodies, joint centers, geoms and sites @@ -528,6 +754,22 @@ static void setStat(mjModel* m, mjData* d) { } } + // inherit dof length from parent body + for (int i=0; i < m->nv; i++) { + // default to linear dof, already has length units + m->dof_length[i] = 1; + + // if rotational dof, inherit from body + int jnt = m->dof_jntid[i]; + mjtJoint type = m->jnt_type[jnt]; + int offset = i - m->jnt_dofadr[jnt]; + if (type == mjJNT_BALL || + type == mjJNT_HINGE || + (type == mjJNT_FREE && offset >= 3)) { + m->dof_length[i] = body[m->dof_bodyid[i]]; + } + } + // fix extent if too small compared to meanbody m->stat.extent = mju_max(m->stat.extent, 2 * m->stat.meansize); @@ -553,7 +795,7 @@ static void setStat(mjModel* m, mjData* d) { } -// set quantities that depend on qpos_spring +// set quantities that depend qpos_spring static void setSpring(mjModel* m, mjData* d) { // run computations in qpos_spring mju_copy(d->qpos, m->qpos_spring, m->nq); @@ -572,19 +814,18 @@ static void setSpring(mjModel* m, mjData* d) { } -// entry point: set all constant fields of mjModel, except for lengthrange +// entry point: set all remaining constant fields of mjModel, except for lengthrange void mj_setConst(mjModel* m, mjData* d) { - // compute subtreemass - for (int i=0; i < m->nbody; i++) { - m->body_subtreemass[i] = m->body_mass[i]; - } - for (int i=m->nbody-1; i > 0; i--) { - m->body_subtreemass[m->body_parentid[i]] += m->body_subtreemass[i]; - } + // set fixed quantities + setFixed(m, d); - // call functions + // set quantities that depend on qpos0 set0(m, d); + + // compute statistics setStat(m, d); + + // set quantities that depend qpos_spring setSpring(m, d); } diff --git a/src/engine/engine_sleep.c b/src/engine/engine_sleep.c new file mode 100644 index 00000000..99231859 --- /dev/null +++ b/src/engine/engine_sleep.c @@ -0,0 +1,99 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "engine/engine_sleep.h" + +#include +#include + +#include +#include + + +//-------------------------------- update ---------------------------------------------------------- + +// compute sleeping arrays from tree_asleep, if flg_staticawake is set treat static bodies as awake +void mj_updateSleepInit(const mjModel* m, mjData* d, int flg_staticawake) { + int ntree = m->ntree, nbody = m->nbody, nv = m->nv; + + // input arrays + const int* tree_asleep = d->tree_asleep; // sleep state source of truth + const int* body_treeid = m->body_treeid; + const int* body_parentid = m->body_parentid; + const int* body_mocapid = m->body_mocapid; + const int* dof_bodyid = m->dof_bodyid; + + // output arrays + int* tree_awake = d->tree_awake; + int* body_awake = d->body_awake; + int* dof_awake_ind = d->dof_awake_ind; + int* body_awake_ind = d->body_awake_ind; + int* parent_awake_ind = d->parent_awake_ind; + + // tree_awake + int ntree_awake = 0; + for (int i=0; i < ntree; i++) { + tree_awake[i] = tree_asleep[i] < 0; + ntree_awake += tree_awake[i]; + } + d->ntree_awake = ntree_awake; + + // {body,parent}_awake_ind + int nbody_awake = 0; + int nparent_awake = 0; + for (int i=0; i < nbody; i++) { + // static body + if (body_treeid[i] < 0) { + if (body_mocapid[i] >= 0) { + // mocap body are always awake + body_awake[i] = mjS_AWAKE; + } else { + // mark static body unless flg_staticawake is set + body_awake[i] = flg_staticawake ? mjS_AWAKE : mjS_STATIC; + } + } + + // dynamic body + else { + body_awake[i] = tree_awake[body_treeid[i]] ? mjS_AWAKE : mjS_ASLEEP; + } + + // body_awake_ind: list of awake and static bodies + if (body_awake[i] != mjS_ASLEEP) { + body_awake_ind[nbody_awake++] = i; + } + + // parent_awake_ind: list of bodies with awake or static parents + if (i && body_awake[body_parentid[i]] != mjS_ASLEEP) { + parent_awake_ind[nparent_awake++] = i; + } + } + d->nbody_awake = nbody_awake; + d->nparent_awake = nparent_awake; + + // dof_awake_ind: list of awake degrees of freedom + int nv_awake = 0; + for (int i=0; i < nv; i++) { + int bodyid = dof_bodyid[i]; + if (body_treeid[bodyid] >= 0 && body_awake[bodyid] == mjS_AWAKE) { + dof_awake_ind[nv_awake++] = i; + } + } + d->nv_awake = nv_awake; +} + +// compute sleep arrays from tree_asleep +void mj_updateSleep(const mjModel* m, mjData* d) { + mj_updateSleepInit(m, d, /*flg_staticawake*/0); +} diff --git a/src/engine/engine_sleep.h b/src/engine/engine_sleep.h new file mode 100644 index 00000000..92a7a27d --- /dev/null +++ b/src/engine/engine_sleep.h @@ -0,0 +1,36 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_SRC_ENGINE_ENGINE_SLEEP_H_ +#define MUJOCO_SRC_ENGINE_ENGINE_SLEEP_H_ + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +// compute sleeping arrays from tree_asleep, if flg_staticawake is set, treat static bodies as awake +MJAPI void mj_updateSleepInit(const mjModel* m, mjData* d, int flg_staticawake); + +// compute {ntree,nbody,nv}_awake, {tree,body}_awake, {body,dof}_awake_ind from tree_asleep +MJAPI void mj_updateSleep(const mjModel* m, mjData* d); + +#ifdef __cplusplus +} +#endif + +#endif // MUJOCO_SRC_ENGINE_ENGINE_SLEEP_H_ diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index c2fbd2ff..4090673f 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -74,7 +74,8 @@ const char* mjENABLESTRING[mjNENABLE] = { "Energy", "Fwdinv", "InvDiscrete", - "MultiCCD" + "MultiCCD", + "Sleep" }; diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 5be44e27..c2f11402 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2931,12 +2931,25 @@ void mjCModel::CopyTree(mjModel* m) { } } - // count bodies with gravity compensation, compute ngravcomp - int ngravcomp = 0; - for (int i=0; i < nbody; i++) { - ngravcomp += (m->body_gravcomp[i] > 0); + // initialize AUTO sleep policy for all trees + for (int i=0; i < m->ntree; i++) { + m->tree_sleep_policy[i] = mjSLEEP_AUTO; + } + + // loop over bodies, check and set non-default sleep policy + for (int i=1; i < nbody; i++) { + mjCBody* pb = bodies_[i]; + + // validate and set non-default sleep policy + if (pb->sleep != mjSLEEP_AUTO) { + int treeid = m->body_treeid[i]; + // non-default sleep policy only allowed for first body in a tree + if (treeid == -1 || treeid == m->body_treeid[i-1]) { + throw mjCError(pb, "sleep policy only allowed for movable root bodies"); + } + m->tree_sleep_policy[treeid] = pb->sleep; + } } - m->ngravcomp = ngravcomp; // recompute nM and dof_Madr given m.dof_parentid, validate int nM_post = 0; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index aaf72291..758783e4 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -2489,8 +2489,8 @@ void mjCBody::Compile(void) { 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" - compiler->alignfree))); // and compiler->align="true" + (joints[0]->spec.align == 2 && // or (joint.align="auto" + compiler->alignfree))); // and compiler->align="true") // free-joint alignment, phase 1 (this body + child geoms) double ipos_inverse[3], iquat_inverse[4]; diff --git a/src/xml/xml_base.h b/src/xml/xml_base.h index 10dda197..9c43c018 100644 --- a/src/xml/xml_base.h +++ b/src/xml/xml_base.h @@ -26,6 +26,7 @@ // keyword maps (defined in implementation files) extern const int joint_sz; +extern const int bodysleep_sz; extern const int camlight_sz; extern const int lighttype_sz; extern const int integrator_sz; @@ -50,6 +51,7 @@ extern const mjMap bool_map[]; extern const mjMap fluid_map[]; extern const mjMap TFAuto_map[]; extern const mjMap joint_map[]; +extern const mjMap bodysleep_map[]; extern const mjMap geom_map[]; extern const mjMap camlight_map[]; extern const mjMap lighttype_map[]; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 7ffdf629..440297be 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -110,18 +110,18 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { "inttotal", "interval", "tolrange"}, {">"}, - {"option", "*", "26", + {"option", "*", "27", "timestep", "impratio", "tolerance", "ls_tolerance", "noslip_tolerance", - "ccd_tolerance", "gravity", "wind", "magnetic", "density", "viscosity", + "ccd_tolerance", "sleep_tolerance", "gravity", "wind", "magnetic", "density", "viscosity", "o_margin", "o_solref", "o_solimp", "o_friction", "integrator", "cone", "jacobian", "solver", "iterations", "ls_iterations", "noslip_iterations", "ccd_iterations", "sdf_iterations", "sdf_initpoints", "actuatorgroupdisable"}, {"<"}, - {"flag", "?", "24", "constraint", "equality", "frictionloss", "limit", "contact", - "spring", "damper", "gravity", "clampctrl", "warmstart", - "filterparent", "actuation", "refsafe", "sensor", "midphase", "eulerdamp", "autoreset", - "nativeccd", "island", "override", "energy", "fwdinv", "invdiscrete", "multiccd"}, + {"flag", "?", "25", "constraint", "equality", "frictionloss", "limit", "contact", + "spring", "damper", "gravity", "clampctrl", "warmstart", "filterparent", "actuation", + "refsafe", "sensor", "midphase", "eulerdamp", "autoreset", "nativeccd", "island", + "override", "energy", "fwdinv", "invdiscrete", "multiccd", "sleep"}, {">"}, {"size", "*", "14", "memory", "njmax", "nconmax", "nstack", "nuserdata", "nkey", @@ -257,8 +257,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"model", "*", "3", "name", "file", "content_type"}, {">"}, - {"body", "R", "11", "name", "childclass", "pos", "quat", "mocap", - "axisangle", "xyaxes", "zaxis", "euler", "gravcomp", "user"}, + {"body", "R", "12", "name", "childclass", "pos", "quat", "mocap", + "axisangle", "xyaxes", "zaxis", "euler", "gravcomp", "sleep", "user"}, {"<"}, {"inertial", "?", "9", "pos", "quat", "mass", "diaginertia", "axisangle", "xyaxes", "zaxis", "euler", "fullinertia"}, @@ -554,6 +554,15 @@ const mjMap TFAuto_map[3] = { }; +// body sleep type +const int bodysleep_sz = 4; +const mjMap bodysleep_map[bodysleep_sz] = { + {"auto", mjSLEEP_AUTO}, + {"never", mjSLEEP_NEVER}, + {"allowed", mjSLEEP_ALLOWED}, + {"init", mjSLEEP_INIT} +}; + // joint type const int joint_sz = 4; const mjMap joint_map[joint_sz] = { @@ -1136,6 +1145,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { ReadAttr(section, "ls_tolerance", 1, &opt->ls_tolerance, text); ReadAttr(section, "noslip_tolerance", 1, &opt->noslip_tolerance, text); ReadAttr(section, "ccd_tolerance", 1, &opt->ccd_tolerance, text); + ReadAttr(section, "sleep_tolerance", 1, &opt->sleep_tolerance, text); ReadAttr(section, "gravity", 3, opt->gravity, text); ReadAttr(section, "wind", 3, opt->wind, text); ReadAttr(section, "magnetic", 3, opt->magnetic, text); @@ -1213,6 +1223,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { READENBL("fwdinv", mjENBL_FWDINV) READENBL("invdiscrete", mjENBL_INVDISCRETE) READENBL("multiccd", mjENBL_MULTICCD) + READENBL("sleep", mjENBL_SLEEP) #undef READENBL } } @@ -3709,8 +3720,11 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, } ReadAlternative(elem, child->alt); - // read gravcomp + // gravcomp, sleep policy ReadAttr(elem, "gravcomp", 1, &child->gravcomp, text); + if (MapValue(elem, "sleep", &n, bodysleep_map, bodysleep_sz)) { + child->sleep = (mjtSleepPolicy) n; + } // read userdata std::vector userdata; diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index ad591f5e..848a7615 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -995,6 +995,7 @@ void mjXWriter::Option(XMLElement* root) { WriteAttr(section, "ls_tolerance", 1, &model->option.ls_tolerance, &opt.ls_tolerance); WriteAttr(section, "noslip_tolerance", 1, &model->option.noslip_tolerance, &opt.noslip_tolerance); WriteAttr(section, "ccd_tolerance", 1, &model->option.ccd_tolerance, &opt.ccd_tolerance); + WriteAttr(section, "sleep_tolerance", 1, &model->option.sleep_tolerance, &opt.sleep_tolerance); WriteAttr(section, "gravity", 3, model->option.gravity, opt.gravity); WriteAttr(section, "wind", 3, model->option.wind, opt.wind); WriteAttr(section, "magnetic", 3, model->option.magnetic, opt.magnetic); @@ -1067,6 +1068,7 @@ void mjXWriter::Option(XMLElement* root) { WRITEENBL("fwdinv", mjENBL_FWDINV) WRITEENBL("invdiscrete", mjENBL_INVDISCRETE) WRITEENBL("multiccd", mjENBL_MULTICCD) + WRITEENBL("sleep", mjENBL_SLEEP) #undef WRITEENBL } @@ -1644,6 +1646,14 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, string_vi if (body->gravcomp) { WriteAttr(elem, "gravcomp", 1, &body->gravcomp); } + + // sleep policy + if (body->sleep != mjSLEEP_AUTO && + body->sleep != mjSLEEP_AUTO_NEVER && + body->sleep != mjSLEEP_AUTO_ALLOWED) { + WriteAttrKey(elem, "sleep", bodysleep_map, bodysleep_sz, body->sleep); + } + // userdata WriteVector(elem, "user", body->get_userdata()); diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index 1884ae05..71c28fb4 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -51,6 +51,10 @@ mujoco_test(engine_ray_test) mujoco_test(engine_sensor_test) +mujoco_test(engine_setconst_test) + +mujoco_test(engine_sleep_test) + mujoco_test(engine_solver_test) mujoco_test(engine_sort_test) diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 7e60e6f2..0f4a10ae 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -79,7 +79,7 @@ TEST_F(CoreSmoothTest, MjDataWorldBodyValuesAreInitialized) { EXPECT_THAT(values, ElementsAre(1, 0, 0, 0)) << #name; \ } else if constexpr (EndsWith(#name, "mat")) { \ EXPECT_THAT(values, ElementsAre(1, 0, 0, 0, 1, 0, 0, 0, 1)) << #name; \ - } else { \ + } else if constexpr (std::string_view(#type) == "mjtNum") { \ EXPECT_THAT(values, Each(Eq(0))) << #name; \ } \ } diff --git a/test/engine/engine_setconst_test.cc b/test/engine/engine_setconst_test.cc new file mode 100644 index 00000000..445bc33a --- /dev/null +++ b/test/engine/engine_setconst_test.cc @@ -0,0 +1,479 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Tests for engine/engine_setconst.c. + +#include +#include + +#include +#include +#include +#include +#include "test/fixture.h" + +namespace mujoco { +namespace { + +using ::std::string; +using ::testing::DoubleNear; +using ::testing::HasSubstr; +using ::testing::IsNull; +using ::testing::NotNull; + + +using SetConstTest = MujocoTest; + +TEST_F(SetConstTest, AwakeActuatedJoint) { + string xml = R"( + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* m; + + string sleep[] = {"auto", "never", "allowed", "init"}; + int tsp0[] = {mjSLEEP_AUTO_NEVER, mjSLEEP_NEVER, mjSLEEP_ALLOWED, + mjSLEEP_INIT}; + int tsp1[] = {mjSLEEP_AUTO_ALLOWED, mjSLEEP_NEVER, mjSLEEP_ALLOWED, + mjSLEEP_INIT}; + + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + string xml_copy = xml; + size_t pos1 = xml_copy.find("POLICY1"); + xml_copy.replace(pos1, 7, sleep[i]); + size_t pos2 = xml_copy.find("POLICY2"); + xml_copy.replace(pos2, 7, sleep[j]); + m = LoadModelFromString(xml_copy.c_str(), error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + EXPECT_EQ(m->tree_sleep_policy[0], tsp0[i]); + EXPECT_EQ(m->tree_sleep_policy[1], tsp1[j]); + mj_deleteModel(m); + } + } +} + +TEST_F(SetConstTest, AwakeActuatedSite) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); + EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); + + mj_deleteModel(model); +} + +TEST_F(SetConstTest, AwakeActuatedBody) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); + EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); + + mj_deleteModel(model); +} + +TEST_F(SetConstTest, AwakeActuatedTendon) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); + EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); + + mj_deleteModel(model); +} + +TEST_F(SetConstTest, AwakeStiffTendonMultiTree) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); + EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_NEVER); + + mj_deleteModel(model); +} + +TEST_F(SetConstTest, SleepyTendonSingleTree) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED); + + mj_deleteModel(model); +} + +TEST_F(SetConstTest, SleepyTendonZeroStiffness) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED); + EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); + + mj_deleteModel(model); +} + +TEST_F(SetConstTest, TendonTreeId) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + int t_static_id = mj_name2id(model, mjOBJ_TENDON, "T_static"); + int t_tree1_id = mj_name2id(model, mjOBJ_TENDON, "T_tree1"); + int t_intertree12_id = mj_name2id(model, mjOBJ_TENDON, "T_intertree12"); + int t_intertree123_id = mj_name2id(model, mjOBJ_TENDON, "T_intertree123"); + + int b1_1_treeid = model->body_treeid[mj_name2id(model, mjOBJ_BODY, "B1_1")]; + int b2_1_treeid = model->body_treeid[mj_name2id(model, mjOBJ_BODY, "B2_1")]; + + // Tendon 1: Not associated with any tree + EXPECT_EQ(model->tendon_treenum[t_static_id], 0); + EXPECT_EQ(model->tendon_treeid[2*t_static_id], -1); + EXPECT_EQ(model->tendon_treeid[2*t_static_id+1], -1); + + // Tendon 2: Should be in Tree 1 + EXPECT_EQ(model->tendon_treenum[t_tree1_id], 1); + EXPECT_EQ(model->tendon_treeid[2*t_tree1_id], b1_1_treeid); + EXPECT_EQ(model->tendon_treeid[2*t_tree1_id+1], -1); + EXPECT_GE(model->tendon_treeid[2*t_tree1_id], 0); + + // Tendon 3: Spans two trees (Tree 1 and Tree 2) + EXPECT_EQ(model->tendon_treenum[t_intertree12_id], 2); + EXPECT_EQ(model->tendon_treeid[2*t_intertree12_id], b1_1_treeid); + EXPECT_EQ(model->tendon_treeid[2*t_intertree12_id+1], b2_1_treeid); + + // Tendon 4: Spans three trees (Tree 1, 2 and 3) + EXPECT_EQ(model->tendon_treenum[t_intertree123_id], 3); + EXPECT_EQ(model->tendon_treeid[2*t_intertree123_id], b1_1_treeid); + EXPECT_EQ(model->tendon_treeid[2*t_intertree123_id+1], b2_1_treeid); + // The third tree ID is not stored in tendon_treeid + + mj_deleteModel(model); +} + +TEST_F(SetConstTest, SleepingNotAllowed) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + EXPECT_THAT(model, IsNull()) << error; + EXPECT_THAT(string(error), HasSubstr( + "tree 1 connected to tendon 0 which spans more than 2 trees, " + "sleeping not allowed")); +} + + +TEST_F(SetConstTest, DofLength) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + mjtNum tol = 1e-5; + + // B1: Slider + EXPECT_EQ(model->dof_length[0], 1); + + // B2: Hinge + EXPECT_THAT(model->dof_length[1], DoubleNear(3, tol)); + + // B3: Ball + EXPECT_THAT(model->dof_length[2], DoubleNear(4, tol)); + EXPECT_THAT(model->dof_length[3], DoubleNear(4, tol)); + EXPECT_THAT(model->dof_length[4], DoubleNear(4, tol)); + + // B4: Free + EXPECT_EQ(model->dof_length[5], 1); + EXPECT_EQ(model->dof_length[6], 1); + EXPECT_EQ(model->dof_length[7], 1); + EXPECT_THAT(model->dof_length[8], DoubleNear(5, tol)); + EXPECT_THAT(model->dof_length[9], DoubleNear(5, tol)); + EXPECT_THAT(model->dof_length[10], DoubleNear(5, tol)); + + mj_deleteModel(model); +} + +} // namespace +} // namespace mujoco diff --git a/test/engine/engine_sleep_test.cc b/test/engine/engine_sleep_test.cc new file mode 100644 index 00000000..da8a3681 --- /dev/null +++ b/test/engine/engine_sleep_test.cc @@ -0,0 +1,181 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Tests for engine/engine_sleep.c. + +#include +#include +#include +#include +#include "src/engine/engine_sleep.h" +#include "test/fixture.h" + +namespace mujoco { +namespace { + +using ::testing::ElementsAre; +using ::testing::NotNull; + +using SleepTest = MujocoTest; + +static constexpr char kSimple[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +static constexpr int kAwake = -(1+mjMINAWAKE); + +TEST_F(SleepTest, MjSleepUpdate) { + char error[1024]; + mjModel* m = LoadModelFromString(kSimple, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + + // ntree = 2, nbody = 5, nv = 5, njnt = 3, ngeom = 6 + // body 0: world, 1 geom + // body 1: 1 ball join, 3 dofs, 1 geom + // body 2: no joint, 1 geom + // body 3: hinge joint, 1 dof, 2 geoms + // body 4: child of body 2, hinge joint, 1 dof, 1 geom + + EXPECT_EQ(m->ntree, 2); + EXPECT_EQ(m->nbody, 5); + EXPECT_EQ(m->nv, 5); + EXPECT_EQ(m->njnt, 3); + EXPECT_EQ(m->ngeom, 6); + + EXPECT_THAT(AsVector(m->body_treeid, m->nbody), + ElementsAre(-1, 0, -1, 1, 1)); + EXPECT_THAT(AsVector(m->dof_bodyid, m->nv), + ElementsAre(1, 1, 1, 3, 4)); + EXPECT_THAT(AsVector(m->geom_bodyid, m->ngeom), + ElementsAre(0, 1, 2, 3, 3, 4)); + EXPECT_THAT(AsVector(m->jnt_bodyid, m->njnt), + ElementsAre(1, 3, 4)); + + // Test Case 1: Initial state + EXPECT_THAT(AsVector(d->tree_asleep, m->ntree), + ElementsAre(kAwake, kAwake)); + EXPECT_EQ(d->ntree_awake, 2); + EXPECT_EQ(d->nv_awake, 5); + EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), + ElementsAre(0, 1, 2, 3, 4)); + EXPECT_THAT(AsVector(d->tree_awake, m->ntree), + ElementsAre(1, 1)); + EXPECT_THAT(AsVector(d->body_awake, m->nbody), + ElementsAre(mjS_STATIC, + mjS_AWAKE, + mjS_STATIC, + mjS_AWAKE, + mjS_AWAKE)); + + // Test Case 2: Call mj_sleepUpdate, expect no changes + mj_updateSleep(m, d); + EXPECT_EQ(d->ntree_awake, 2); + EXPECT_EQ(d->nv_awake, 5); + EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), + ElementsAre(0, 1, 2, 3, 4)); + EXPECT_THAT(AsVector(d->body_awake, m->nbody), + ElementsAre(mjS_STATIC, + mjS_AWAKE, + mjS_STATIC, + mjS_AWAKE, + mjS_AWAKE)); + EXPECT_THAT(AsVector(d->tree_awake, m->ntree), + ElementsAre(1, 1)); + + // Test Case 3: Tree 0 asleep + d->tree_asleep[0] = 0; d->tree_asleep[1] = -1; + mj_updateSleep(m, d); + EXPECT_EQ(d->ntree_awake, 1); + EXPECT_EQ(d->nv_awake, 2); + EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), + ElementsAre(3, 4)); + EXPECT_THAT(AsVector(d->tree_awake, m->ntree), + ElementsAre(0, 1)); + EXPECT_THAT(AsVector(d->body_awake, m->nbody), + ElementsAre(mjS_STATIC, + mjS_ASLEEP, + mjS_STATIC, + mjS_AWAKE, + mjS_AWAKE)); + + // Test Case 4: Tree 1 asleep + d->tree_asleep[0] = -1; d->tree_asleep[1] = 1; + mj_updateSleep(m, d); + EXPECT_EQ(d->ntree_awake, 1); + EXPECT_EQ(d->nv_awake, 3); + EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), + ElementsAre(0, 1, 2)); + EXPECT_THAT(AsVector(d->tree_awake, m->ntree), + ElementsAre(1, 0)); + EXPECT_THAT(AsVector(d->body_awake, m->nbody), + ElementsAre(mjS_STATIC, + mjS_AWAKE, + mjS_STATIC, + mjS_ASLEEP, + mjS_ASLEEP)); + + // Test Case 5: All trees asleep + d->tree_asleep[0] = 0; d->tree_asleep[1] = 1; + mj_updateSleep(m, d); + EXPECT_EQ(d->ntree_awake, 0); + EXPECT_EQ(d->nv_awake, 0); + EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), + ElementsAre()); + EXPECT_THAT(AsVector(d->tree_awake, m->ntree), + ElementsAre(0, 0)); + EXPECT_THAT(AsVector(d->body_awake, m->nbody), + ElementsAre(mjS_STATIC, + mjS_ASLEEP, + mjS_STATIC, + mjS_ASLEEP, + mjS_ASLEEP)); + + mj_deleteData(d); + mj_deleteModel(m); +} + +} // namespace +} // namespace mujoco diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 4f464e1a..3239c5f0 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -42,6 +42,7 @@ public const double mjMAXIMP = 0.9999; public const int mjMAXCONPAIR = 50; public const int mjMAXTREEDEPTH = 50; public const int mjMAXFLEXNODES = 27; +public const int mjMINAWAKE = 10; public const int mjNEQDATA = 11; public const int mjNDYN = 10; public const int mjNGAIN = 10; @@ -189,7 +190,8 @@ public enum mjtEnableBit : int{ mjENBL_FWDINV = 4, mjENBL_INVDISCRETE = 8, mjENBL_MULTICCD = 16, - mjNENABLE = 5, + mjENBL_SLEEP = 32, + mjNENABLE = 6, } public enum mjtJoint : int{ mjJNT_FREE = 0, @@ -432,6 +434,14 @@ public enum mjtSameFrame : int{ mjSAMEFRAME_BODYROT = 3, mjSAMEFRAME_INERTIAROT = 4, } +public enum mjtSleepPolicy : int{ + mjSLEEP_AUTO = 0, + mjSLEEP_AUTO_NEVER = 1, + mjSLEEP_AUTO_ALLOWED = 2, + mjSLEEP_NEVER = 3, + mjSLEEP_ALLOWED = 4, + mjSLEEP_INIT = 5, +} public enum mjtLRMode : int{ mjLRMODE_NONE = 0, mjLRMODE_MUSCLE = 1, @@ -4903,6 +4913,10 @@ public unsafe struct mjData_ { public int nA; public int nisland; public int nidof; + public int ntree_awake; + public int nbody_awake; + public int nparent_awake; + public int nv_awake; public double time; public fixed double energy[2]; public void* buffer; @@ -4922,6 +4936,7 @@ public unsafe struct mjData_ { public double* act_dot; public double* userdata; public double* sensordata; + public int* tree_asleep; public int* plugin; public UIntPtr* plugin_data; public double* xpos; @@ -4970,6 +4985,11 @@ public unsafe struct mjData_ { public double* qLD; public double* qLDiagInv; public byte* bvh_active; + public int* tree_awake; + public int* body_awake; + public int* body_awake_ind; + public int* parent_awake_ind; + public int* dof_awake_ind; public double* flexedge_velocity; public double* ten_velocity; public double* actuator_velocity; @@ -5012,6 +5032,10 @@ public unsafe struct mjData_ { public double* efc_D; public double* efc_R; public int* tendon_efcadr; + public int* tree_island; + public int* island_ntree; + public int* island_itreeadr; + public int* map_itree2tree; public int* dof_island; public int* island_nv; public int* island_idofadr; @@ -5094,6 +5118,7 @@ public unsafe struct mjOption_ { public double ls_tolerance; public double noslip_tolerance; public double ccd_tolerance; + public double sleep_tolerance; public fixed double gravity[3]; public fixed double wind[3]; public fixed double magnetic[3]; @@ -5399,6 +5424,12 @@ public unsafe struct mjModel_ { public double* dof_damping; public double* dof_invweight0; public double* dof_M0; + public double* dof_length; + public int* tree_bodyadr; + public int* tree_bodynum; + public int* tree_dofadr; + public int* tree_dofnum; + public int* tree_sleep_policy; public int* geom_type; public int* geom_contype; public int* geom_conaffinity; @@ -5636,6 +5667,8 @@ public unsafe struct mjModel_ { public int* tendon_num; public int* tendon_matid; public int* tendon_group; + public int* tendon_treenum; + public int* tendon_treeid; public byte* tendon_limited; public byte* tendon_actfrclimited; public double* tendon_width; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 15e24fcc..5418c534 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -262,6 +262,12 @@ struct MjOption { void set_ccd_tolerance(mjtNum value) { ptr_->ccd_tolerance = value; } + mjtNum sleep_tolerance() const { + return ptr_->sleep_tolerance; + } + void set_sleep_tolerance(mjtNum value) { + ptr_->sleep_tolerance = value; + } emscripten::val gravity() const { return emscripten::val(emscripten::typed_memory_view(3, ptr_->gravity)); } @@ -3399,6 +3405,12 @@ struct MjsBody { void set_gravcomp(double value) { ptr_->gravcomp = value; } + mjtSleepPolicy sleep() const { + return ptr_->sleep; + } + void set_sleep(mjtSleepPolicy value) { + ptr_->sleep = value; + } mjDoubleVec &userdata() const { return *(ptr_->userdata); } @@ -4562,6 +4574,24 @@ struct MjModel { emscripten::val dof_M0() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nv, ptr_->dof_M0)); } + emscripten::val dof_length() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nv, ptr_->dof_length)); + } + emscripten::val tree_bodyadr() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->ntree, ptr_->tree_bodyadr)); + } + emscripten::val tree_bodynum() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->ntree, ptr_->tree_bodynum)); + } + emscripten::val tree_dofadr() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->ntree, ptr_->tree_dofadr)); + } + emscripten::val tree_dofnum() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->ntree, ptr_->tree_dofnum)); + } + emscripten::val tree_sleep_policy() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->ntree, ptr_->tree_sleep_policy)); + } emscripten::val geom_type() const { return emscripten::val(emscripten::typed_memory_view(ptr_->ngeom, ptr_->geom_type)); } @@ -5273,6 +5303,12 @@ struct MjModel { emscripten::val tendon_group() const { return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_group)); } + emscripten::val tendon_treenum() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_treenum)); + } + emscripten::val tendon_treeid() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon * 2, ptr_->tendon_treeid)); + } emscripten::val tendon_limited() const { return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_limited)); } @@ -5798,6 +5834,30 @@ struct MjData { void set_nidof(int value) { ptr_->nidof = value; } + int ntree_awake() const { + return ptr_->ntree_awake; + } + void set_ntree_awake(int value) { + ptr_->ntree_awake = value; + } + int nbody_awake() const { + return ptr_->nbody_awake; + } + void set_nbody_awake(int value) { + ptr_->nbody_awake = value; + } + int nparent_awake() const { + return ptr_->nparent_awake; + } + void set_nparent_awake(int value) { + ptr_->nparent_awake = value; + } + int nv_awake() const { + return ptr_->nv_awake; + } + void set_nv_awake(int value) { + ptr_->nv_awake = value; + } mjtNum time() const { return ptr_->time; } @@ -5858,6 +5918,9 @@ struct MjData { emscripten::val sensordata() const { return emscripten::val(emscripten::typed_memory_view(model->nsensordata, ptr_->sensordata)); } + emscripten::val tree_asleep() const { + return emscripten::val(emscripten::typed_memory_view(model->ntree, ptr_->tree_asleep)); + } emscripten::val plugin() const { return emscripten::val(emscripten::typed_memory_view(model->nplugin, ptr_->plugin)); } @@ -6002,6 +6065,21 @@ struct MjData { emscripten::val bvh_active() const { return emscripten::val(emscripten::typed_memory_view(model->nbvh, ptr_->bvh_active)); } + emscripten::val tree_awake() const { + return emscripten::val(emscripten::typed_memory_view(model->ntree, ptr_->tree_awake)); + } + emscripten::val body_awake() const { + return emscripten::val(emscripten::typed_memory_view(model->nbody, ptr_->body_awake)); + } + emscripten::val body_awake_ind() const { + return emscripten::val(emscripten::typed_memory_view(model->nbody, ptr_->body_awake_ind)); + } + emscripten::val parent_awake_ind() const { + return emscripten::val(emscripten::typed_memory_view(model->nbody, ptr_->parent_awake_ind)); + } + emscripten::val dof_awake_ind() const { + return emscripten::val(emscripten::typed_memory_view(model->nv, ptr_->dof_awake_ind)); + } emscripten::val flexedge_velocity() const { return emscripten::val(emscripten::typed_memory_view(model->nflexedge, ptr_->flexedge_velocity)); } @@ -6126,6 +6204,18 @@ struct MjData { emscripten::val tendon_efcadr() const { return emscripten::val(emscripten::typed_memory_view(model->ntendon, ptr_->tendon_efcadr)); } + emscripten::val tree_island() const { + return emscripten::val(emscripten::typed_memory_view(model->ntree, ptr_->tree_island)); + } + emscripten::val island_ntree() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nisland, ptr_->island_ntree)); + } + emscripten::val island_itreeadr() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nisland, ptr_->island_itreeadr)); + } + emscripten::val map_itree2tree() const { + return emscripten::val(emscripten::typed_memory_view(model->ntree, ptr_->map_itree2tree)); + } emscripten::val dof_island() const { return emscripten::val(emscripten::typed_memory_view(model->nv, ptr_->dof_island)); } @@ -6792,6 +6882,7 @@ EMSCRIPTEN_BINDINGS(mujoco_enums) { .value("mjENBL_FWDINV", mjENBL_FWDINV) .value("mjENBL_INVDISCRETE", mjENBL_INVDISCRETE) .value("mjENBL_MULTICCD", mjENBL_MULTICCD) + .value("mjENBL_SLEEP", mjENBL_SLEEP) .value("mjNENABLE", mjNENABLE); enum_("mjtJoint") @@ -7035,6 +7126,14 @@ EMSCRIPTEN_BINDINGS(mujoco_enums) { .value("mjSAMEFRAME_BODYROT", mjSAMEFRAME_BODYROT) .value("mjSAMEFRAME_INERTIAROT", mjSAMEFRAME_INERTIAROT); + enum_("mjtSleepPolicy") + .value("mjSLEEP_AUTO", mjSLEEP_AUTO) + .value("mjSLEEP_AUTO_NEVER", mjSLEEP_AUTO_NEVER) + .value("mjSLEEP_AUTO_ALLOWED", mjSLEEP_AUTO_ALLOWED) + .value("mjSLEEP_NEVER", mjSLEEP_NEVER) + .value("mjSLEEP_ALLOWED", mjSLEEP_ALLOWED) + .value("mjSLEEP_INIT", mjSLEEP_INIT); + enum_("mjtLRMode") .value("mjLRMODE_NONE", mjLRMODE_NONE) .value("mjLRMODE_MUSCLE", mjLRMODE_MUSCLE) @@ -7125,6 +7224,11 @@ EMSCRIPTEN_BINDINGS(mujoco_enums) { .value("mjTIMER_COL_NARROW", mjTIMER_COL_NARROW) .value("mjNTIMER", mjNTIMER); + enum_("mjtSleepState") + .value("mjS_STATIC", mjS_STATIC) + .value("mjS_ASLEEP", mjS_ASLEEP) + .value("mjS_AWAKE", mjS_AWAKE); + enum_("mjtGeomInertia") .value("mjINERTIA_VOLUME", mjINERTIA_VOLUME) .value("mjINERTIA_SHELL", mjINERTIA_SHELL); @@ -8547,6 +8651,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("ls_tolerance", &MjOption::ls_tolerance, &MjOption::set_ls_tolerance, reference()) .property("noslip_tolerance", &MjOption::noslip_tolerance, &MjOption::set_noslip_tolerance, reference()) .property("ccd_tolerance", &MjOption::ccd_tolerance, &MjOption::set_ccd_tolerance, reference()) + .property("sleep_tolerance", &MjOption::sleep_tolerance, &MjOption::set_sleep_tolerance, reference()) .property("gravity", &MjOption::gravity) .property("wind", &MjOption::wind) .property("magnetic", &MjOption::magnetic) @@ -8870,6 +8975,10 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("nA", &MjData::nA, &MjData::set_nA, reference()) .property("nisland", &MjData::nisland, &MjData::set_nisland, reference()) .property("nidof", &MjData::nidof, &MjData::set_nidof, reference()) + .property("ntree_awake", &MjData::ntree_awake, &MjData::set_ntree_awake, reference()) + .property("nbody_awake", &MjData::nbody_awake, &MjData::set_nbody_awake, reference()) + .property("nparent_awake", &MjData::nparent_awake, &MjData::set_nparent_awake, reference()) + .property("nv_awake", &MjData::nv_awake, &MjData::set_nv_awake, reference()) .property("time", &MjData::time, &MjData::set_time, reference()) .property("energy", &MjData::energy) .property("buffer", &MjData::buffer) @@ -8889,6 +8998,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("act_dot", &MjData::act_dot) .property("userdata", &MjData::userdata) .property("sensordata", &MjData::sensordata) + .property("tree_asleep", &MjData::tree_asleep) .property("plugin", &MjData::plugin) .property("plugin_data", &MjData::plugin_data) .property("xpos", &MjData::xpos) @@ -8937,6 +9047,11 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("qLD", &MjData::qLD) .property("qLDiagInv", &MjData::qLDiagInv) .property("bvh_active", &MjData::bvh_active) + .property("tree_awake", &MjData::tree_awake) + .property("body_awake", &MjData::body_awake) + .property("body_awake_ind", &MjData::body_awake_ind) + .property("parent_awake_ind", &MjData::parent_awake_ind) + .property("dof_awake_ind", &MjData::dof_awake_ind) .property("flexedge_velocity", &MjData::flexedge_velocity) .property("ten_velocity", &MjData::ten_velocity) .property("actuator_velocity", &MjData::actuator_velocity) @@ -8979,6 +9094,10 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("efc_D", &MjData::efc_D) .property("efc_R", &MjData::efc_R) .property("tendon_efcadr", &MjData::tendon_efcadr) + .property("tree_island", &MjData::tree_island) + .property("island_ntree", &MjData::island_ntree) + .property("island_itreeadr", &MjData::island_itreeadr) + .property("map_itree2tree", &MjData::map_itree2tree) .property("dof_island", &MjData::dof_island) .property("island_nv", &MjData::island_nv) .property("island_idofadr", &MjData::island_idofadr) @@ -9207,6 +9326,12 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("dof_damping", &MjModel::dof_damping) .property("dof_invweight0", &MjModel::dof_invweight0) .property("dof_M0", &MjModel::dof_M0) + .property("dof_length", &MjModel::dof_length) + .property("tree_bodyadr", &MjModel::tree_bodyadr) + .property("tree_bodynum", &MjModel::tree_bodynum) + .property("tree_dofadr", &MjModel::tree_dofadr) + .property("tree_dofnum", &MjModel::tree_dofnum) + .property("tree_sleep_policy", &MjModel::tree_sleep_policy) .property("geom_type", &MjModel::geom_type) .property("geom_contype", &MjModel::geom_contype) .property("geom_conaffinity", &MjModel::geom_conaffinity) @@ -9444,6 +9569,8 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("tendon_num", &MjModel::tendon_num) .property("tendon_matid", &MjModel::tendon_matid) .property("tendon_group", &MjModel::tendon_group) + .property("tendon_treenum", &MjModel::tendon_treenum) + .property("tendon_treeid", &MjModel::tendon_treeid) .property("tendon_limited", &MjModel::tendon_limited) .property("tendon_actfrclimited", &MjModel::tendon_actfrclimited) .property("tendon_width", &MjModel::tendon_width) @@ -9947,6 +10074,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("fullinertia", &MjsBody::fullinertia) .property("mocap", &MjsBody::mocap, &MjsBody::set_mocap, reference()) .property("gravcomp", &MjsBody::gravcomp, &MjsBody::set_gravcomp, reference()) + .property("sleep", &MjsBody::sleep, &MjsBody::set_sleep, reference()) .property("userdata", &MjsBody::userdata, reference()) .property("explicitinertial", &MjsBody::explicitinertial, &MjsBody::set_explicitinertial, reference()) .property("plugin", &MjsBody::plugin, reference()) diff --git a/wasm/codegen/helpers/constants.py b/wasm/codegen/helpers/constants.py index e24fcecd..aae6eb5a 100644 --- a/wasm/codegen/helpers/constants.py +++ b/wasm/codegen/helpers/constants.py @@ -423,9 +423,11 @@ MJDATA_SIZES: List[str] = [ "island_efcnum", "island_idofadr", "island_iefcadr", + "island_itreeadr", "island_ne", "island_nefc", "island_nf", + "island_ntree", "island_nv", "map_efc2iefc", "map_iefc2efc",