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

Added mjData.eq_active user input variable, for enabling/disabling the state of equality constraints.

Browse Source 
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
This commit is contained in:
Yuval Tassa
2023-10-03 09:29:45 -07:00
committed by Copybara-Service
parent 177aec68dc
commit ee78b8f76b
19 changed files with 180 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+4 -3
View File
@@ -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
+17 -11
View File
@@ -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
+13 -10
View File
@@ -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
+11 -8
View File
@@ -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
+1 -1
View File
@@ -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
+1 -1
View File
@@ -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
+2 -1
View File
@@ -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
+9 -8
View File
@@ -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',
introspect/structs.py
+16 -9
View File
@@ -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(
python/mujoco/indexer_xmacro.h
+8 -8
View File
@@ -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
+2 -2
View File
@@ -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
+1
View File
@@ -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
+7
View File
@@ -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);
src/engine/engine_support.c
+31 -6
View File
@@ -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<<i;
if (element & spec) {
int size = mj_stateElemSize(m, element);
const mjtNum* ptr = mj_stateElemConstPtr(m, d, element);
mju_copy(state + adr, ptr, size);
adr += size;
// special handling of eq_active (mjtByte)
if (element == mjSTATE_EQ_ACTIVE) {
int neq = m->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<<i;
if (element & spec) {
int size = mj_stateElemSize(m, element);
mjtNum* ptr = mj_stateElemPtr(m, d, element);
mju_copy(ptr, state + adr, size);
adr += size;
// special handling of eq_active (mjtByte)
if (element == mjSTATE_EQ_ACTIVE) {
int neq = m->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;
}
}
}
}
src/engine/engine_vis_visualize.c
+1 -1
View File
@@ -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);
src/user/user_model.cc
+1 -1
View File
@@ -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);
test/engine/engine_forward_test.cc
+51
View File
@@ -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"(
<mujoco>
<worldbody>
<body>
<joint name="vertical" type="slide" axis="0 0 1"/>
<geom size="1"/>
</body>
</worldbody>
<equality>
<joint joint1="vertical"/>
</equality>
</mujoco>
)";
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
test/engine/engine_support_test.cc
+1
View File
@@ -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);
unity/Runtime/Bindings/MjBindings.cs
+3 -2
View File
@@ -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;