chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update mj_energyPos.

Browse Source 
PiperOrigin-RevId: 757678945
Change-Id: If236b9e8f376a2673abc87e818326e9188f6c15a
This commit is contained in:
Taylor Howell
2025-05-12 03:12:10 -07:00
committed by Copybara-Service
parent b0b2c48190
commit 9accc7ae51
3 changed files with 29 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_passive.c
+1 -1
View File
@@ -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
+1 -3
View File
@@ -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
test/engine/engine_sensor_test.cc
+27
View File
@@ -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>