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

Implement sleeping in engine

Browse Source 
PiperOrigin-RevId: 829361787
Change-Id: I6f64d8e25c4248cf32c18cd94d37ff5def78946e
This commit is contained in:
Yuval Tassa
2025-11-07 03:32:03 -08:00
committed by Copybara-Service
parent 1e0226d360
commit 769f37b653
55 changed files with 3602 additions and 677 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_support.c
+38 -27
View File
@@ -561,40 +561,51 @@ void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt,
}
// integrate qpos with given qvel
void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum dt) {
// loop over joints
for (int j=0; j < m->njnt; j++) {
// get addresses in qpos and qvel
int padr = m->jnt_qposadr[j];
int vadr = m->jnt_dofadr[j];
// integrate qpos with given qvel for given body indices
void mj_integratePosInd(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum dt,
const int* index, int nbody) {
for (int b=1; b < nbody; b++) {
int k = index ? index[b] : b;
int start = m->body_jntadr[k];
int end = start + m->body_jntnum[k];
for (int j=start; j < end; j++) {
// get addresses in qpos and qvel
int padr = m->jnt_qposadr[j];
int vadr = m->jnt_dofadr[j];
switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_FREE:
// position update
for (int i=0; i < 3; i++) {
qpos[padr+i] += dt * qvel[vadr+i];
switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_FREE:
// position update
for (int i=0; i < 3; i++) {
qpos[padr+i] += dt * qvel[vadr+i];
}
padr += 3;
vadr += 3;
// continue with rotation update
mjFALLTHROUGH;
case mjJNT_BALL:
// quaternion update
mju_quatIntegrate(qpos+padr, qvel+vadr, dt);
break;
case mjJNT_HINGE:
case mjJNT_SLIDE:
// scalar update: same for rotation and translation
qpos[padr] += dt * qvel[vadr];
}
padr += 3;
vadr += 3;
// continue with rotation update
mjFALLTHROUGH;
case mjJNT_BALL:
// quaternion update
mju_quatIntegrate(qpos+padr, qvel+vadr, dt);
break;
case mjJNT_HINGE:
case mjJNT_SLIDE:
// scalar update: same for rotation and translation
qpos[padr] += dt * qvel[vadr];
}
}
}
// integrate qpos with given qvel
void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum dt) {
mj_integratePosInd(m, qpos, qvel, dt, NULL, m->nbody);
}
// normalize all quaternions in qpos-type vector
void mj_normalizeQuat(const mjModel* m, mjtNum* qpos) {
// find quaternion fields and normalize