From 41ba0a6a52d86bb3b8e03cb1349defedb2322548 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 1 Aug 2023 13:06:45 +0200 Subject: [PATCH] Log the desired ZMP and the desired CoM quantities in the balancing-position-control script --- .../script/blf-balancing-position-control.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/utilities/balancing-position-control/script/blf-balancing-position-control.py b/utilities/balancing-position-control/script/blf-balancing-position-control.py index d54478cdf4..e65670df1b 100644 --- a/utilities/balancing-position-control/script/blf-balancing-position-control.py +++ b/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()