Log the desired ZMP and the desired CoM quantities in the balancing-p…

…osition-control script

utilities/balancing-position-control/script/blf-balancing-position-control.py

@@ -264,6 +264,8 @@ def main():
     index = 0
     knot_index = 1
 
+    lipm_omega_square = blf.math.StandardAccelerationOfGravitation / initial_com_position[2]
+
     port = blf.yarp_utilities.BufferedPortVectorsCollection()
     port.open("/balancing_controller/logger/data:o")
 
@@ -347,14 +349,23 @@ def main():
         com_from_desired = kindyn.getCenterOfMassPosition().toNumPy()
         com_from_measured = kindyn_with_measured.getCenterOfMassPosition().toNumPy()
 
+        # evaluate the desired ZMP using the LIP model
+        # ddx_com = omega^2 * (x_com - x_zmp)
+        desired_zmp = com_spline_output.position[:2] - com_spline_output.acceleration[:2] / lipm_omega_square
+        desired_zmp = np.append(desired_zmp, 0.0)
+
         data = port.prepare()
         data.vectors = {
+            "desired_zmp": desired_zmp,
             "global_zmp": global_zmp,
             "global_zmp_from_measured": global_zmp_from_measured,
             "local_zmp_left": local_zmp_left,
             "local_zmp_right": local_zmp_right,
-            "com_from_desired": com_from_desired,
-            "com_from_measured": com_from_measured,
+            "estimated_com_from_desired": com_from_desired,
+            "estimated_com_from_measured": com_from_measured,
+            "com_desired_position": com_spline_output.position,
+            "com_desired_velocity": com_spline_output.velocity,
+            "com_desired_acceleration": com_spline_output.acceleration,
         }
         port.write()