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

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
+4 -2
View File
@@ -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];