diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 0ce354ec..6e5e2c20 100644 --- a/doc/XMLreference.rst +++ b/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` corresponds to this value, and is used to initialize + :ref:`mjData.eq_active`, 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` at runtime can be used to fix the body + temporarily. .. _equality-weld-relpose: diff --git a/doc/changelog.rst b/doc/changelog.rst index 02aecec6..44ef12c0 100644 --- a/doc/changelog.rst +++ b/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`, ``filterexact``, which updates first-order filter states with + 12. Added :ref:`mjData.eq_active` user input variable, for enabling/disabling the state of equality + constraints. Renamed ``mjModel.eq_active`` to :ref:`mjModel.eq_active0`, which now has the semantic of + "initial value of `mjData.eq_active`". Fixes `#876 `__ + + **Migration:** Replace uses of ``mjModel.eq_active`` with ``mjData.eq_active``. + +13. Added a new :ref:`dyntype`, ``filterexact``, which updates first-order filter states with the exact formula rather than with Euler integration. -13. Added an actuator attribute, :ref:`actearly`, which uses semi-implicit integration for +14. Added an actuator attribute, :ref:`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` and :ref:`actuatorfrclimited`, respectively. -15. Added the flag :ref:`eulerdamp`, which disables implicit integration of joint damping in the +16. Added the flag :ref:`eulerdamp`, which disables implicit integration of joint damping in the Euler integrator. See the :ref:`Numerical Integration` section for more details. -16. Added the flag :ref:`invdiscrete`, which enables discrete-time inverse dynamics for all +17. Added the flag :ref:`invdiscrete`, which enables discrete-time inverse dynamics for all :ref:`integrators` other than ``RK4``. See the flag documentation for more details. -17. Added :ref:`ls_iterations` and :ref:`ls_tolerance` options for adjusting +18. Added :ref:`ls_iterations` and :ref:`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 `__ . -19. Added camera :ref:`resolution` attribute and :ref:`camprojection` +20. Added camera :ref:`resolution` attribute and :ref:`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` calibration attributes: +21. Added :ref:`camera` calibration attributes: - The new attributes are :ref:`resolution`, :ref:`focal`, :ref:`focalpixel`, :ref:`principal`, @@ -119,13 +125,13 @@ General Python bindings ^^^^^^^^^^^^^^^ -21. Fixed `#870 `__ where calling ``update_scene`` with an invalid +22. Fixed `#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` to be ignored during the construction of +23. Fixed a bug that was causing :ref:`geom margin` to be ignored during the construction of midphase collision trees. diff --git a/doc/includes/references.h b/doc/includes/references.h index ec3bd3cd..8cc0691f 100644 --- a/doc/includes/references.h +++ b/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; diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index c705ffab..73ad99fa 100644 --- a/include/mujoco/mjdata.h +++ b/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 diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 0a6d71ba..cc06c92b 100644 --- a/include/mujoco/mjmodel.h +++ b/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) diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index 41d918b3..2433db5a 100644 --- a/include/mujoco/mjvisualize.h +++ b/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; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 7dea2823..30ecc6c2 100644 --- a/include/mujoco/mjxmacro.h +++ b/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 ) \ diff --git a/introspect/enums.py b/introspect/enums.py index 7760d68d..abbe45d0 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -394,15 +394,16 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('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', diff --git a/introspect/structs.py b/introspect/structs.py index a06e9a67..04a8a5e8 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -2565,11 +2565,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ 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 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), 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 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), - StructFieldDecl( - name='eq_active', - type=PointerType( - inner_type=ValueType(name='mjtByte'), - ), - doc='', - ), StructFieldDecl( name='eq_data', type=PointerType( @@ -6549,6 +6549,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='eq_active', + type=PointerType( + inner_type=ValueType(name='mjtByte'), + ), + doc='', + ), StructFieldDecl( name='sensordata', type=PointerType( diff --git a/python/mujoco/indexer_xmacro.h b/python/mujoco/indexer_xmacro.h index 9e4c32a7..b604f008 100644 --- a/python/mujoco/indexer_xmacro.h +++ b/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 ) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 4d8bfa61..cc1011b7 100644 --- a/src/engine/engine_core_constraint.c +++ b/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; diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 473a6475..49798389 100644 --- a/src/engine/engine_io.c +++ b/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; ineq; 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); diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 735a9c25..0446696a 100644 --- a/src/engine/engine_print.c +++ b/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); diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 734772e7..b89aebc9 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -105,6 +105,7 @@ static inline int mj_stateElemSize(const mjModel* m, mjtState spec) { case mjSTATE_CTRL: return m->nu; case mjSTATE_QFRC_APPLIED: return m->nv; case mjSTATE_XFRC_APPLIED: return 6*m->nbody; + case mjSTATE_EQ_ACTIVE: return m->neq; // mjtByte, stored as mjtNum in state vector case mjSTATE_MOCAP_POS: return 3*m->nmocap; case mjSTATE_MOCAP_QUAT: return 4*m->nmocap; case mjSTATE_USERDATA: return m->nuserdata; @@ -176,9 +177,21 @@ void mj_getState(const mjModel* m, const mjData* d, mjtNum* state, unsigned int mjtState element = 1<neq; + for (int j=0; j < neq; j++) { + state[adr++] = d->eq_active[j]; + } + } + + // regular state components (mjtNum) + else { + const mjtNum* ptr = mj_stateElemConstPtr(m, d, element); + mju_copy(state + adr, ptr, size); + adr += size; + } } } } @@ -196,9 +209,21 @@ void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, unsigned int mjtState element = 1<neq; + for (int j=0; j < neq; j++) { + d->eq_active[j] = state[adr++]; + } + } + + // regular state components (mjtNum) + else { + mjtNum* ptr = mj_stateElemPtr(m, d, element); + mju_copy(ptr, state + adr, size); + adr += size; + } } } } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index e1ebd6f8..7964bfed 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1908,7 +1908,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, if (vopt->flags[mjVIS_CONSTRAINT] && (category & catmask) && m->neq) { // connect or weld for (int i=0; i < m->neq; i++) { - if (m->eq_active[i] && (m->eq_type[i] == mjEQ_CONNECT || m->eq_type[i] == mjEQ_WELD)) { + if (d->eq_active[i] && (m->eq_type[i] == mjEQ_CONNECT || m->eq_type[i] == mjEQ_WELD)) { // compute endpoints in global coordinates int j = m->eq_obj1id[i], k = m->eq_obj2id[i]; mju_rotVecMat(vec, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_WELD), d->xmat+9*j); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 8cecdde7..e10cf370 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1950,7 +1950,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->eq_type[i] = peq->type; m->eq_obj1id[i] = peq->obj1id; m->eq_obj2id[i] = peq->obj2id; - m->eq_active[i] = peq->active; + m->eq_active0[i] = peq->active; copyvec(m->eq_solref+mjNREF*i, peq->solref, mjNREF); copyvec(m->eq_solimp+mjNIMP*i, peq->solimp, mjNIMP); copyvec(m->eq_data+mjNEQDATA*i, peq->data, mjNEQDATA); diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index a3f8abfa..dabebf9b 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -555,6 +555,57 @@ TEST_F(ForwardTest, gravcomp) { mj_deleteModel(model); } +// test disabling of equality constraints +TEST_F(ForwardTest, eq_active) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + + mjData* data = mj_makeData(model); + + // simulate for 1 second + while (data->time < 1) { + mj_step(model, data); + } + + // expect that the body has barely moved + EXPECT_LT(mju_abs(data->qpos[0]), 0.001); + + // turn the equality off, simulate for another second + data->eq_active[0] = 0; + while (data->time < 2) { + mj_step(model, data); + } + + // expect that the body has fallen about 5m + EXPECT_LT(data->qpos[0], -4.5); + EXPECT_GT(data->qpos[0], -5.5); + + // turn the equality back on, simulate for another second + data->eq_active[0] = 1; + while (data->time < 3) { + mj_step(model, data); + } + + // expect that the body has snapped back + EXPECT_LT(mju_abs(data->qpos[0]), 0.001); + + mj_deleteData(data); + mj_deleteModel(model); +} + // user defined 2nd-order activation dynamics: frequency-controlled oscillator // note that scalar mjcb_act_dyn callbacks are expected to return act_dot, but // since we have a vector output we write into act_dot directly diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 10c94734..a2b3acda 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -369,6 +369,7 @@ TEST_F(SupportTest, GetSetStateStepEqual) { // set controls and applied joint forces to random values for (int i=0; i < model->nu; i++) data->ctrl[i] = dist(rng); for (int i=0; i < model->nv; i++) data->qfrc_applied[i] = dist(rng); + for (int i=0; i < model->neq; i++) data->eq_active[i] = dist(rng) > 0; // take one step mj_step(model, data); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 4dcc8420..f2f395bd 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -4765,6 +4765,7 @@ public unsafe struct mjData_ { public double* ctrl; public double* qfrc_applied; public double* xfrc_applied; + public byte* eq_active; public double* mocap_pos; public double* mocap_quat; public double* qacc; @@ -5318,7 +5319,7 @@ public unsafe struct mjModel_ { public int* eq_type; public int* eq_obj1id; public int* eq_obj2id; - public byte* eq_active; + public byte* eq_active0; public double* eq_solref; public double* eq_solimp; public double* eq_data; @@ -6047,7 +6048,6 @@ public unsafe struct model { public int* eq_type; public int* eq_obj1id; public int* eq_obj2id; - public byte* eq_active; public double* eq_data; public int* tendon_num; public int* tendon_matid; @@ -6104,6 +6104,7 @@ public unsafe struct data { public double* act; public double* ctrl; public double* xfrc_applied; + public byte* eq_active; public double* sensordata; public double* xpos; public double* xquat;