chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 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_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