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

equivalent but clearer quat-diff call in mj_differentiatePos

Browse Source 
PiperOrigin-RevId: 530075836
Change-Id: I3bbe5dec17ccdf53c372bb81087dddde30a24c5c
This commit is contained in:
Yuval Tassa
2023-05-07 02:20:51 -07:00
committed by Copybara-Service
parent ff1fa84d32
commit 49066e1794
2 changed files with 78 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_support.c
+5 -8
View File
@@ -1224,14 +1224,11 @@ void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]
// compute velocity by finite-differencing two positions
void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt,
const mjtNum* qpos1, const mjtNum* qpos2) {
int padr, vadr;
mjtNum neg[4], dif[4];
// loop over joints
for (int j=0; j<m->njnt; j++) {
// get addresses in qpos and qvel
padr = m->jnt_qposadr[j];
vadr = m->jnt_dofadr[j];
int padr = m->jnt_qposadr[j];
int vadr = m->jnt_dofadr[j];
switch (m->jnt_type[j]) {
case mjJNT_FREE:
@@ -1245,9 +1242,9 @@ void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt,
mjFALLTHROUGH;
case mjJNT_BALL:
mju_negQuat(neg, qpos1+padr); // solve: qpos1 * dif = qpos2
mju_mulQuat(dif, neg, qpos2+padr);
mju_quat2Vel(qvel+vadr, dif, dt);
// solve: qpos1 * quat(qvel * dt) = qpos2
mju_subQuat(qvel+vadr, qpos2+padr, qpos1+padr);
mju_scl3(qvel+vadr, qvel+vadr, 1/dt);
break;
case mjJNT_HINGE: