Update mj_energyPos.

PiperOrigin-RevId: 757678945
Change-Id: If236b9e8f376a2673abc87e818326e9188f6c15a

src/engine/engine_passive.c

@@ -84,7 +84,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
 
     case mjJNT_BALL:
       {
-        // convert quatertion difference into angular "velocity"
+        // convert quaternion difference into angular "velocity"
         mjtNum dif[3], quat[4];
         mju_copy4(quat, d->qpos+padr);
         mju_normalize4(quat);

src/engine/engine_sensor.c

@@ -938,9 +938,7 @@ void mj_energyPos(const mjModel* m, mjData* d) {
 
       switch ((mjtJoint) m->jnt_type[i]) {
       case mjJNT_FREE:
-        mju_copy4(quat, d->qpos+padr);
+        mju_sub3(dif, d->qpos+padr, m->qpos_spring+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

test/engine/engine_sensor_test.cc

@@ -454,6 +454,33 @@ TEST_F(SensorTest, PotentialEnergy) {
   mj_deleteModel(model);
 }
 
+TEST_F(SensorTest, PotentialEnergyFreeJointSpring) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <option gravity="0 0 0"/>
+    <worldbody>
+      <body>
+        <geom size="1" mass="3"/>
+        <joint type="free" stiffness="2"/>
+      </body>
+    </worldbody>
+    <sensor>
+      <e_potential/>
+    </sensor>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml);
+  mjData* data = mj_makeData(model);
+  data->qpos[0] = 1;
+  data->qpos[1] = 2;
+  data->qpos[2] = 3;
+  mj_forward(model, data);
+  EXPECT_EQ(data->sensordata[0], 0.5*2*14);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 TEST_F(SensorTest, KineticEnergy) {
   constexpr char xml[] = R"(
   <mujoco>