Add sensors for potential and kinetic energy.

PiperOrigin-RevId: 716775375
Change-Id: Ic8ab7f1a51df970ab551dabe7c797cde13cd97fd

src/engine/engine_sensor.c

@@ -458,6 +458,16 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
         }
         break;
 
+      case mjSENS_E_POTENTIAL:                            // potential energy
+        mj_energyPos(m, d);
+        d->sensordata[adr] = d->energy[0];
+        break;
+
+      case mjSENS_E_KINETIC:                              // kinetic energy
+        mj_energyVel(m, d);
+        d->sensordata[adr] = d->energy[1];
+        break;
+
       case mjSENS_CLOCK:                                  // clock
         d->sensordata[adr] = d->time;
         break;
@@ -902,12 +912,6 @@ void mj_energyPos(const mjModel* m, mjData* d) {
   int padr;
   mjtNum dif[3], quat[4], stiffness;
 
-  // disabled: clear and return
-  if (!mjENABLED(mjENBL_ENERGY)) {
-    d->energy[0] = d->energy[1] = 0;
-    return;
-  }
-
   // init potential energy:  -sum_i body(i).mass * mju_dot(body(i).pos, gravity)
   d->energy[0] = 0;
   if (!mjDISABLED(mjDSBL_GRAVITY)) {
@@ -996,11 +1000,6 @@ void mj_energyPos(const mjModel* m, mjData* d) {
 
 // velocity-dependent energy (kinetic)
 void mj_energyVel(const mjModel* m, mjData* d) {
-  // return if disabled (already cleared in potential)
-  if (!mjENABLED(mjENBL_ENERGY)) {
-    return;
-  }
-
   mj_markStack(d);
   mjtNum *vec = mjSTACKALLOC(d, m->nv, mjtNum);