diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c
index 40296906..3c025804 100644
--- a/src/engine/engine_passive.c
+++ b/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);
diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c
index 0ff91a09..485474c1 100644
--- a/src/engine/engine_sensor.c
+++ b/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_normalize4(quat);
- mju_sub3(dif, quat, m->qpos_spring+padr);
+ mju_sub3(dif, d->qpos+padr, m->qpos_spring+padr);
d->energy[0] += 0.5*stiffness*mju_dot3(dif, dif);
// continue with rotations
diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc
index 5aa2aa5b..b5f0125a 100644
--- a/test/engine/engine_sensor_test.cc
+++ b/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"(
+
+
+
+
+
+
+
+
+
+
+
+
+ )";
+ 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"(