chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 4 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
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;
}
}
}
}