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

Don't normalize mjData->qpos quaternions in-place.

Browse Source 
PiperOrigin-RevId: 647927542
Change-Id: I13b0be55498d1da3af2cdc414cfed4c3908a6fe1
This commit is contained in:
Yuval Tassa
2024-06-29 03:06:07 -07:00
committed by Copybara-Service
parent 0e19722c61
commit 4d4b0bb2c3
10 changed files with 157 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+10 -6
View File
@@ -44,9 +44,6 @@ void mj_kinematics(const mjModel* m, mjData* d) {
d->xmat[0] = d->xmat[4] = d->xmat[8] = 1;
d->ximat[0] = d->ximat[4] = d->ximat[8] = 1;
// normalize all quaternions in qpos
mj_normalizeQuat(m, d->qpos);
// normalize mocap quaternions
for (int i=0; i < m->nmocap; i++) {
mju_normalize4(d->mocap_quat+4*i);
@@ -66,6 +63,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
// copy pos and quat from qpos
mju_copy3(xpos, d->qpos+qadr);
mju_copy4(xquat, d->qpos+qadr+3);
mju_normalize4(xquat);
// assign xanchor and xaxis
mju_copy3(d->xanchor+3*jntadr, xpos);
@@ -125,6 +123,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
mjtNum qloc[4];
if (jtype == mjJNT_BALL) {
mju_copy4(qloc, d->qpos+qadr);
mju_normalize4(qloc);
} else {
mju_axisAngle2Quat(qloc, m->jnt_axis+3*jid, d->qpos[qadr] - m->qpos0[qadr]);
}
@@ -890,13 +889,15 @@ void mj_transmission(const mjModel* m, mjData* d) {
int j = m->jnt_qposadr[id];
// axis: expmap representation of quaternion
mju_quat2Vel(axis, d->qpos+j, 1);
mju_copy4(quat, d->qpos+j);
mju_normalize4(quat);
mju_quat2Vel(axis, quat, 1);
// gearAxis: rotate to parent frame if necessary
if (m->actuator_trntype[i] == mjTRN_JOINT) {
mju_copy3(gearAxis, gear);
} else {
mju_negQuat(quat, d->qpos+j);
mju_negQuat(quat, quat);
mju_rotVecQuat(gearAxis, gear, quat);
}
@@ -923,12 +924,15 @@ void mj_transmission(const mjModel* m, mjData* d) {
// axis: expmap representation of quaternion
mju_quat2Vel(axis, d->qpos+j+3, 1);
mju_copy4(quat, d->qpos+j+3);
mju_normalize4(quat);
mju_quat2Vel(axis, quat, 1);
// gearAxis: rotate to world frame if necessary
if (m->actuator_trntype[i] == mjTRN_JOINT) {
mju_copy3(gearAxis, gear+3);
} else {
mju_negQuat(quat, d->qpos+j+3);
mju_negQuat(quat, quat);
mju_rotVecQuat(gearAxis, gear+3, quat);
}