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

Don't normalize mocap quaternions in-place.

Browse Source 
PiperOrigin-RevId: 648394295
Change-Id: I348a0a045c1697f6ef782280d92c90a30759eb33
This commit is contained in:
Yuval Tassa
2024-07-01 09:29:02 -07:00
committed by Copybara-Service
parent c5cefdc2f6
commit 0c3698f278
3 changed files with 48 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+4 -7
View File
@@ -44,11 +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 mocap quaternions
for (int i=0; i < m->nmocap; i++) {
mju_normalize4(d->mocap_quat+4*i);
}
// compute global cartesian positions and orientations of all bodies
for (int i=1; i < m->nbody; i++) {
mjtNum xpos[3], xquat[4];
@@ -75,10 +70,12 @@ void mj_kinematics(const mjModel* m, mjData* d) {
int pid = m->body_parentid[i];
// get body pos and quat: from model or mocap
mjtNum *bodypos, *bodyquat;
mjtNum *bodypos, *bodyquat, quat[4];
if (m->body_mocapid[i] >= 0) {
bodypos = d->mocap_pos + 3*m->body_mocapid[i];
bodyquat = d->mocap_quat + 4*m->body_mocapid[i];
mju_copy4(quat, d->mocap_quat + 4*m->body_mocapid[i]);
mju_normalize4(quat);
bodyquat = quat;
} else {
bodypos = m->body_pos+3*i;
bodyquat = m->body_quat+4*i;