Added mjData.eq_active user input variable, for enabling/disabling …

…the state of equality constraints.

Renamed `mjModel.eq_active` to `mjModel.eq_active0`, which now has the semantic of "initial value of `mjData.eq_active`".

Fixes #876.

PiperOrigin-RevId: 570410643
Change-Id: Id03171e751377c7cc453f143abee64239ee2e2ed

doc/XMLreference.rst

@@ -3854,8 +3854,8 @@ geoms volumes of either body. This constraint can be used to define ball joints
 
 :at:`active`: :at-val:`[false, true], "true"`
    If this attribute is set to "true", the constraint is active and the constraint solver will try to enforce it. The
-   corresponding field in mjModel is mjData.eq_active. This field can be used at runtime to turn specific constraints on
-   an off.
+   field :ref:`mjModel.eq_active0<mjModel>` corresponds to this value, and is used to initialize
+   :ref:`mjData.eq_active<mjData>`, which is user-settable at runtime.
 
 .. _equality-connect-solref:
 
@@ -3920,7 +3920,8 @@ of the other body, without any joint elements in the child body.
 
 :at:`body2`: :at-val:`string, optional`
    Name of the second body. If this attribute is omitted, the second body is the world body. Welding a body to the world
-   and changing the corresponding component of mjModel.eq_active at runtime can be used to fix the body temporarily.
+   and changing the corresponding component of :ref:`mjData.eq_active<mjData>` at runtime can be used to fix the body
+   temporarily.
 
 .. _equality-weld-relpose:
 

doc/changelog.rst

@@ -88,25 +88,31 @@ General
 
        **Migration:** Users should use the :at:`cable` and :at:`shell` elasticity plugins.
 
-12. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
+   12. Added :ref:`mjData.eq_active<mjData>` user input variable, for enabling/disabling the state of equality
+       constraints. Renamed ``mjModel.eq_active`` to :ref:`mjModel.eq_active0<mjModel>`, which now has the semantic of
+       "initial value of `mjData.eq_active`". Fixes `#876 <https://github.com/google-deepmind/mujoco/issues/876>`__
+
+       **Migration:** Replace uses of ``mjModel.eq_active`` with ``mjData.eq_active``.
+
+13. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
     the exact formula rather than with Euler integration.
-13. Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
+14. Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
     actuator forces: using the next step's actuator state to compute the current actuator forces at the current timestep.
-14. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
+15. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
     :ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and
     :ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively.
-15. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
+16. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
     Euler integrator. See the :ref:`Numerical Integration<geIntegration>` section for more details.
-16. Added the flag :ref:`invdiscrete<option-flag-invdiscrete>`, which enables discrete-time inverse dynamics for all
+17. Added the flag :ref:`invdiscrete<option-flag-invdiscrete>`, which enables discrete-time inverse dynamics for all
     :ref:`integrators<option-integrator>` other than ``RK4``. See the flag documentation for more details.
-17. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_tolerance<option-ls_tolerance>` options for adjusting
+18. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_tolerance<option-ls_tolerance>` options for adjusting
     linesearch stopping criteria in CG and Newton solvers. These can be useful for performance tuning.
-18. Added ``mesh_pos`` and ``mesh_quat`` fields to :ref:`mjModel` to store the normalizing transformation applied to
+19. Added ``mesh_pos`` and ``mesh_quat`` fields to :ref:`mjModel` to store the normalizing transformation applied to
     mesh assets. Fixes `#409 <https://github.com/google-deepmind/mujoco/issues/409>`__ .
-19. Added camera :ref:`resolution<body-camera-resolution>` attribute and :ref:`camprojection<sensor-camprojection>`
+20. Added camera :ref:`resolution<body-camera-resolution>` attribute and :ref:`camprojection<sensor-camprojection>`
     sensor. If camera resolution is set to positive values, the camera projection sensor will report the location of a
     target site, projected onto the camera image, in pixel coordinates.
-20. Added :ref:`camera<body-camera>` calibration attributes:
+21. Added :ref:`camera<body-camera>` calibration attributes:
 
     - The new attributes are :ref:`resolution<body-camera-resolution>`, :ref:`focal<body-camera-focal>`,
       :ref:`focalpixel<body-camera-focalpixel>`, :ref:`principal<body-camera-principal>`,
@@ -119,13 +125,13 @@ General
 Python bindings
 ^^^^^^^^^^^^^^^
 
-21. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
+22. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
     camera name used the default camera.
 
 Bug fixes
 ^^^^^^^^^
 
-22. Fixed a bug that was causing :ref:`geom margin<body-geom-margin>` to be ignored during the construction of
+23. Fixed a bug that was causing :ref:`geom margin<body-geom-margin>` to be ignored during the construction of
     midphase collision trees.
 
 

doc/includes/references.h

@@ -25,18 +25,20 @@ typedef enum mjtState_ {          // state elements
   mjSTATE_CTRL          = 1<<5,   // control
   mjSTATE_QFRC_APPLIED  = 1<<6,   // applied generalized force
   mjSTATE_XFRC_APPLIED  = 1<<7,   // applied Cartesian force/torque
-  mjSTATE_MOCAP_POS     = 1<<8,   // positions of mocap bodies
-  mjSTATE_MOCAP_QUAT    = 1<<9,   // orientations of mocap bodies
-  mjSTATE_USERDATA      = 1<<10,  // user data
-  mjSTATE_PLUGIN        = 1<<11,  // plugin state
+  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              = 12,     // 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_PHYSICS | mjSTATE_TIME | mjSTATE_PLUGIN,
   mjSTATE_USER          = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED |
-                          mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT | mjSTATE_USERDATA,
+                          mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT |
+                          mjSTATE_USERDATA,
   mjSTATE_INTEGRATION   = mjSTATE_FULLPHYSICS | mjSTATE_USER | mjSTATE_WARMSTART
 } mjtState;
 typedef enum mjtWarning_ {   // warning types
@@ -191,6 +193,7 @@ struct mjData_ {
   mjtNum* ctrl;              // control                                          (nu x 1)
   mjtNum* qfrc_applied;      // applied generalized force                        (nv x 1)
   mjtNum* xfrc_applied;      // applied Cartesian force/torque                   (nbody x 6)
+  mjtByte* eq_active;        // enable/disable constraints                       (neq x 1)
 
   // mocap data
   mjtNum* mocap_pos;         // positions of mocap bodies                        (nmocap x 3)
@@ -207,8 +210,8 @@ struct mjData_ {
   mjtNum* sensordata;        // sensor data array                                (nsensordata 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)
+  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)
 
   //-------------------- POSITION dependent
 
@@ -1124,7 +1127,7 @@ struct mjModel_ {
   int*      eq_type;              // constraint type (mjtEq)                  (neq x 1)
   int*      eq_obj1id;            // id of object 1                           (neq x 1)
   int*      eq_obj2id;            // id of object 2                           (neq x 1)
-  mjtByte*  eq_active;            // enable/disable constraint                (neq x 1)
+  mjtByte*  eq_active0;           // initial enable/disable constraint state  (neq x 1)
   mjtNum*   eq_solref;            // constraint solver reference              (neq x mjNREF)
   mjtNum*   eq_solimp;            // constraint solver impedance              (neq x mjNIMP)
   mjtNum*   eq_data;              // numeric data for constraint              (neq x mjNEQDATA)
@@ -2112,7 +2115,6 @@ struct mjvSceneState_ {
     int* eq_type;
     int* eq_obj1id;
     int* eq_obj2id;
-    mjtByte* eq_active;
     mjtNum* eq_data;
 
     int* tendon_num;
@@ -2170,6 +2172,7 @@ struct mjvSceneState_ {
 
     mjtNum* ctrl;
     mjtNum* xfrc_applied;
+    mjtByte* eq_active;
 
     mjtNum* sensordata;
 

include/mujoco/mjdata.h

@@ -33,18 +33,20 @@ typedef enum mjtState_ {          // state elements
   mjSTATE_CTRL          = 1<<5,   // control
   mjSTATE_QFRC_APPLIED  = 1<<6,   // applied generalized force
   mjSTATE_XFRC_APPLIED  = 1<<7,   // applied Cartesian force/torque
-  mjSTATE_MOCAP_POS     = 1<<8,   // positions of mocap bodies
-  mjSTATE_MOCAP_QUAT    = 1<<9,   // orientations of mocap bodies
-  mjSTATE_USERDATA      = 1<<10,  // user data
-  mjSTATE_PLUGIN        = 1<<11,  // plugin state
+  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              = 12,     // 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_PHYSICS | mjSTATE_TIME | mjSTATE_PLUGIN,
   mjSTATE_USER          = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED |
-                          mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT | mjSTATE_USERDATA,
+                          mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT |
+                          mjSTATE_USERDATA,
   mjSTATE_INTEGRATION   = mjSTATE_FULLPHYSICS | mjSTATE_USER | mjSTATE_WARMSTART
 } mjtState;
 
@@ -219,6 +221,7 @@ struct mjData_ {
   mjtNum* ctrl;              // control                                          (nu x 1)
   mjtNum* qfrc_applied;      // applied generalized force                        (nv x 1)
   mjtNum* xfrc_applied;      // applied Cartesian force/torque                   (nbody x 6)
+  mjtByte* eq_active;        // enable/disable constraints                       (neq x 1)
 
   // mocap data
   mjtNum* mocap_pos;         // positions of mocap bodies                        (nmocap x 3)
@@ -235,8 +238,8 @@ struct mjData_ {
   mjtNum* sensordata;        // sensor data array                                (nsensordata 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)
+  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)
 
   //-------------------- POSITION dependent
 

include/mujoco/mjmodel.h

@@ -856,7 +856,7 @@ struct mjModel_ {
   int*      eq_type;              // constraint type (mjtEq)                  (neq x 1)
   int*      eq_obj1id;            // id of object 1                           (neq x 1)
   int*      eq_obj2id;            // id of object 2                           (neq x 1)
-  mjtByte*  eq_active;            // enable/disable constraint                (neq x 1)
+  mjtByte*  eq_active0;           // initial enable/disable constraint state  (neq x 1)
   mjtNum*   eq_solref;            // constraint solver reference              (neq x mjNREF)
   mjtNum*   eq_solimp;            // constraint solver impedance              (neq x mjNIMP)
   mjtNum*   eq_data;              // numeric data for constraint              (neq x mjNEQDATA)

include/mujoco/mjvisualize.h

@@ -506,7 +506,6 @@ struct mjvSceneState_ {
     int* eq_type;
     int* eq_obj1id;
     int* eq_obj2id;
-    mjtByte* eq_active;
     mjtNum* eq_data;
 
     int* tendon_num;
@@ -564,6 +563,7 @@ struct mjvSceneState_ {
 
     mjtNum* ctrl;
     mjtNum* xfrc_applied;
+    mjtByte* eq_active;
 
     mjtNum* sensordata;
 

include/mujoco/mjxmacro.h

@@ -362,7 +362,7 @@
     XMJV( int,     eq_type,               neq,           1                    ) \
     XMJV( int,     eq_obj1id,             neq,           1                    ) \
     XMJV( int,     eq_obj2id,             neq,           1                    ) \
-    XMJV( mjtByte, eq_active,             neq,           1                    ) \
+    X   ( mjtByte, eq_active0,            neq,           1                    ) \
     X   ( mjtNum,  eq_solref,             neq,           mjNREF               ) \
     X   ( mjtNum,  eq_solimp,             neq,           mjNIMP               ) \
     XMJV( mjtNum,  eq_data,               neq,           mjNEQDATA            ) \
@@ -495,6 +495,7 @@
     XMJV( mjtNum,    ctrl,              nu,          1           ) \
     X   ( mjtNum,    qfrc_applied,      nv,          1           ) \
     XMJV( mjtNum,    xfrc_applied,      nbody,       6           ) \
+    XMJV( mjtByte,   eq_active,         neq,         1           ) \
     X   ( mjtNum,    mocap_pos,         nmocap,      3           ) \
     X   ( mjtNum,    mocap_quat,        nmocap,      4           ) \
     X   ( mjtNum,    qacc,              nv,          1           ) \

introspect/enums.py

@@ -394,15 +394,16 @@
              ('mjSTATE_CTRL', 32),
              ('mjSTATE_QFRC_APPLIED', 64),
              ('mjSTATE_XFRC_APPLIED', 128),
-             ('mjSTATE_MOCAP_POS', 256),
-             ('mjSTATE_MOCAP_QUAT', 512),
-             ('mjSTATE_USERDATA', 1024),
-             ('mjSTATE_PLUGIN', 2048),
-             ('mjNSTATE', 12),
+             ('mjSTATE_EQ_ACTIVE', 256),
+             ('mjSTATE_MOCAP_POS', 512),
+             ('mjSTATE_MOCAP_QUAT', 1024),
+             ('mjSTATE_USERDATA', 2048),
+             ('mjSTATE_PLUGIN', 4096),
+             ('mjNSTATE', 13),
              ('mjSTATE_PHYSICS', 14),
-             ('mjSTATE_FULLPHYSICS', 2063),
-             ('mjSTATE_USER', 2016),
-             ('mjSTATE_INTEGRATION', 4095),
+             ('mjSTATE_FULLPHYSICS', 4111),
+             ('mjSTATE_USER', 4064),
+             ('mjSTATE_INTEGRATION', 8191),
          ]),
      )),
     ('mjtWarning',

introspect/structs.py

@@ -2565,11 +2565,11 @@
                  doc='id of object 2                           (neq x 1)',
              ),
              StructFieldDecl(
-                 name='eq_active',
+                 name='eq_active0',
                  type=PointerType(
                      inner_type=ValueType(name='mjtByte'),
                  ),
-                 doc='enable/disable constraint                (neq x 1)',
+                 doc='initial enable/disable constraint state  (neq x 1)',
              ),
              StructFieldDecl(
                  name='eq_solref',
@@ -3826,6 +3826,13 @@
                  ),
                  doc='applied Cartesian force/torque                   (nbody x 6)',  # pylint: disable=line-too-long
              ),
+             StructFieldDecl(
+                 name='eq_active',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtByte'),
+                 ),
+                 doc='enable/disable constraints                       (neq x 1)',  # pylint: disable=line-too-long
+             ),
              StructFieldDecl(
                  name='mocap_pos',
                  type=PointerType(
@@ -6226,13 +6233,6 @@
                              ),
                              doc='',
                          ),
-                         StructFieldDecl(
-                             name='eq_active',
-                             type=PointerType(
-                                 inner_type=ValueType(name='mjtByte'),
-                             ),
-                             doc='',
-                         ),
                          StructFieldDecl(
                              name='eq_data',
                              type=PointerType(
@@ -6549,6 +6549,13 @@
                              ),
                              doc='',
                          ),
+                         StructFieldDecl(
+                             name='eq_active',
+                             type=PointerType(
+                                 inner_type=ValueType(name='mjtByte'),
+                             ),
+                             doc='',
+                         ),
                          StructFieldDecl(
                              name='sensordata',
                              type=PointerType(

python/mujoco/indexer_xmacro.h

@@ -78,14 +78,14 @@
   X( mjtNum, cam_, ipd,          ncam, 1               ) \
   X( mjtNum, cam_, user,         ncam, MJ_M(nuser_cam) )
 
-#define MJMODEL_EQUALITY                    \
-  X( int,     eq_, type,   neq, 1         ) \
-  X( int,     eq_, obj1id, neq, 1         ) \
-  X( int,     eq_, obj2id, neq, 1         ) \
-  X( mjtByte, eq_, active, neq, 1         ) \
-  X( mjtNum,  eq_, solref, neq, mjNREF    ) \
-  X( mjtNum,  eq_, solimp, neq, mjNIMP    ) \
-  X( mjtNum,  eq_, data,   neq, mjNEQDATA )
+#define MJMODEL_EQUALITY                     \
+  X( int,     eq_, type,    neq, 1         ) \
+  X( int,     eq_, obj1id,  neq, 1         ) \
+  X( int,     eq_, obj2id,  neq, 1         ) \
+  X( mjtByte, eq_, active0, neq, 1         ) \
+  X( mjtNum,  eq_, solref,  neq, mjNREF    ) \
+  X( mjtNum,  eq_, solimp,  neq, mjNIMP    ) \
+  X( mjtNum,  eq_, data,    neq, mjNEQDATA )
 
 #define MJMODEL_EXCLUDE                      \
   X( int, exclude_, signature, nexclude, 1 )

src/engine/engine_core_constraint.c

@@ -521,7 +521,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
 
   // find active equality constraints
   for (int i=0; i < m->neq; i++) {
-    if (m->eq_active[i]) {
+    if (d->eq_active[i]) {
       // get constraint data
       data = m->eq_data + mjNEQDATA*i;
       id[0] = m->eq_obj1id[i];
@@ -1481,7 +1481,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
 
   // find active equality constraints
   for (int i=0; i < neq; i++) {
-    if (m->eq_active[i]) {
+    if (d->eq_active[i]) {
       id[0] = m->eq_obj1id[i];
       id[1] = m->eq_obj2id[i];
       size = 0;

src/engine/engine_io.c

@@ -1549,6 +1549,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
   mju_zero(d->qvel, m->nv);
   mju_zero(d->act, m->na);
   mju_zero(d->ctrl, m->nu);
+  for (int i=0; i<m->neq; i++) d->eq_active[i] = m->eq_active0[i];
   mju_zero(d->qfrc_applied, m->nv);
   mju_zero(d->xfrc_applied, 6*m->nbody);
   mju_zero(d->qacc, m->nv);

src/engine/engine_print.c

@@ -880,6 +880,13 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
   printArray("CTRL", m->nu, 1, d->ctrl, fp, float_format);
   printArray("QFRC_APPLIED", m->nv, 1, d->qfrc_applied, fp, float_format);
   printArray("XFRC_APPLIED", m->nbody, 6, d->xfrc_applied, fp, float_format);
+  if (m->neq) {
+    fprintf(fp, NAME_FORMAT, "EQ_ACTIVE");
+    for (int c=0; c < m->neq; c++) {
+      fprintf(fp, " %d", d->eq_active[c]);
+    }
+    fprintf(fp, "\n\n");
+  }
   printArray("MOCAP_POS", m->nmocap, 3, d->mocap_pos, fp, float_format);
   printArray("MOCAP_QUAT", m->nmocap, 4, d->mocap_quat, fp, float_format);
   printArray("QACC", m->nv, 1, d->qacc, fp, float_format);