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

PiperOrigin-RevId: 647927542
Change-Id: I13b0be55498d1da3af2cdc414cfed4c3908a6fe1

src/engine/engine_core_constraint.c

@@ -818,7 +818,10 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
       // BALL joint
       else if (m->jnt_type[i] == mjJNT_BALL) {
         // convert joint quaternion to axis-angle
-        mju_quat2Vel(angleAxis, d->qpos+m->jnt_qposadr[i], 1);
+        int adr = m->jnt_qposadr[i];
+        mjtNum quat[4] = {d->qpos[adr], d->qpos[adr+1], d->qpos[adr+2], d->qpos[adr+3]};
+        mju_normalize4(quat);
+        mju_quat2Vel(angleAxis, quat, 1);
 
         // get rotation angle, normalize
         value = mju_normalize3(angleAxis);
@@ -1767,7 +1770,10 @@ static int mj_nl(const mjModel* m, const mjData* d, int *nnz) {
     }
     else if (m->jnt_type[i] == mjJNT_BALL) {
       mjtNum angleAxis[3];
-      mju_quat2Vel(angleAxis, d->qpos+m->jnt_qposadr[i], 1);
+      int adr = m->jnt_qposadr[i];
+      mjtNum quat[4] = {d->qpos[adr], d->qpos[adr+1], d->qpos[adr+2], d->qpos[adr+3]};
+      mju_normalize4(quat);
+      mju_quat2Vel(angleAxis, quat, 1);
       value = mju_normalize3(angleAxis);
       dist = mju_max(m->jnt_range[2*i], m->jnt_range[2*i+1]) - value;
       if (dist < margin) {

src/engine/engine_core_smooth.c

@@ -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);
         }
 

src/engine/engine_passive.c

@@ -64,9 +64,11 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
 
     case mjJNT_BALL:
       {
-        mjtNum dif[3];
         // convert quatertion difference into angular "velocity"
-        mju_subQuat(dif, d->qpos + padr, m->qpos_spring + padr);
+        mjtNum dif[3], quat[4];
+        mju_copy4(quat, d->qpos+padr);
+        mju_normalize4(quat);
+        mju_subQuat(dif, quat, m->qpos_spring + padr);
 
         // apply torque
         d->qfrc_spring[dadr+0] = -stiffness*dif[0];

src/engine/engine_sensor.c

@@ -281,6 +281,7 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
 
       case mjSENS_BALLQUAT:                               // ballquat
         mju_copy4(d->sensordata+adr, d->qpos+m->jnt_qposadr[objid]);
+        mju_normalize4(d->sensordata+adr);
         break;
 
       case mjSENS_JOINTLIMITPOS:                          // jointlimitpos
@@ -899,7 +900,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
 // position-dependent energy (potential)
 void mj_energyPos(const mjModel* m, mjData* d) {
   int padr;
-  mjtNum dif[3], stiffness;
+  mjtNum dif[3], quat[4], stiffness;
 
   // disabled: clear and return
   if (!mjENABLED(mjENBL_ENERGY)) {
@@ -923,7 +924,9 @@ void mj_energyPos(const mjModel* m, mjData* d) {
 
       switch ((mjtJoint) m->jnt_type[i]) {
       case mjJNT_FREE:
-        mju_sub3(dif, d->qpos+padr, m->qpos_spring+padr);
+        mju_copy4(quat, d->qpos+padr);
+        mju_normalize4(quat);
+        mju_sub3(dif, quat, m->qpos_spring+padr);
         d->energy[0] += 0.5*stiffness*mju_dot3(dif, dif);
 
         // continue with rotations
@@ -932,6 +935,8 @@ void mj_energyPos(const mjModel* m, mjData* d) {
 
       case mjJNT_BALL:
         // covert quatertion difference into angular "velocity"
+        mju_copy4(quat, d->qpos+padr);
+        mju_normalize4(quat);
         mju_subQuat(dif, d->qpos + padr, m->qpos_spring + padr);
         d->energy[0] += 0.5*stiffness*mju_dot3(dif, dif);
         break;

src/engine/engine_util_spatial.c

@@ -247,8 +247,8 @@ void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale) {
   mju_copy3(tmp, vel);
   angle = scale * mju_normalize3(tmp);
   mju_axisAngle2Quat(qrot, tmp, angle);
-  mju_mulQuat(quat, quat, qrot);
   mju_normalize4(quat);
+  mju_mulQuat(quat, quat, qrot);
 }