From 9accc7ae51764445658b594112e591ee37f258d8 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Mon, 12 May 2025 03:12:10 -0700 Subject: [PATCH] Update mj_energyPos. PiperOrigin-RevId: 757678945 Change-Id: If236b9e8f376a2673abc87e818326e9188f6c15a --- src/engine/engine_passive.c | 2 +- src/engine/engine_sensor.c | 4 +--- test/engine/engine_sensor_test.cc | 27 +++++++++++++++++++++++++++ 3 files changed, 29 insertions(+), 4 deletions(-) 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"(