Restructure the balancing position controller:

- Add the possibility to enable and disable the CoM-ZMP controller in the balancing controller - Format the balancing_position_control script with black - Use GlobalCoPEvaluator class to compute the global CoP - 🤖 Update the parameters for ergoCubSN000 and ergoCubGazeboV1
ami-iit · Oct 31, 2023 · 4152ba7 · 4152ba7
1 parent 4b444f7
commit 4152ba7
Show file tree

Hide file tree

Showing 7 changed files with 368 additions and 148 deletions.
diff --git a/utilities/balancing-position-control/balancing_position_control/wbc.py b/utilities/balancing-position-control/balancing_position_control/wbc.py
@@ -6,6 +6,11 @@
 
 
 class WBC:
-    def __init__(self, param_handler: blf.parameters_handler.IParametersHandler, kindyn: idyn.KinDynComputations):
-        self.solver, self.tasks, self.variables_handler = blf.utils.create_ik(kindyn=kindyn,
-                                                                              param_handler=param_handler)
+    def __init__(
+        self,
+        param_handler: blf.parameters_handler.IParametersHandler,
+        kindyn: idyn.KinDynComputations,
+    ):
+        self.solver, self.tasks, self.variables_handler = blf.utils.create_ik(
+            kindyn=kindyn, param_handler=param_handler
+        )
diff --git a/utilities/balancing-position-control/balancing_position_control/zmp.py b/utilities/balancing-position-control/balancing_position_control/zmp.py
diff --git a/...position-control/config/robots/ergoCubGazeboV1/blf-balancing-position-control-options.ini b/...position-control/config/robots/ergoCubGazeboV1/blf-balancing-position-control-options.ini
@@ -1,19 +1,23 @@
-dt                       "00:00:00.010000"   # Time expressed in RFC 3339 (0.01 seconds)
+dt                       0.01   # in seconds
 contact_force_threshold  0.1    # in Newton
 
-
 com_knots_delta_x        (0.0, 0.0,  0.03,  0.03, -0.03, -0.03, 0.0,  0.0)    # in meter
 com_knots_delta_y        (0.0, 0.07, 0.07, -0.07, -0.07,  0.07, 0.07, 0.0)    # in meter
 com_knots_delta_z        (0.0, 0.0,  0.0,   0.0,   0.0,   0.0,  0.0,  0.0)    # in meter
-motion_duration          "00:00:10.000000"   # Time expressed in RFC 3339 (10 seconds)
-motion_timeout           "00:00:10.000000"   # Time expressed in RFC 3339 (10 seconds)
+motion_duration          10.0   # in seconds
+motion_timeout           10.0   # in seconds
 
 base_frame               l_sole
 left_contact_frame       l_sole
 right_contact_frame      r_sole
+close_loop_with_zmp      false
+
+[COM_ZMP_CONTROLLER]
+com_gain                        (5.0, 5.0)
+zmp_gain                        (1.0, 1.0)
 
 [include IK "./blf_balancing_position_control/ik.ini"]
 [include ROBOT_CONTROL "./blf_balancing_position_control/robot_control.ini"]
 [include SENSOR_BRIDGE "./blf_balancing_position_control/sensor_bridge.ini"]
 [include CONTACT_WRENCHES "./blf_balancing_position_control/contact_wrenches.ini"]
-
+[include GLOBAL_COP_EVALUATOR "./blf_balancing_position_control/global_cop_evaluator.ini"]
diff --git a/...rol/config/robots/ergoCubGazeboV1/blf_balancing_position_control/global_cop_evaluator.ini b/...rol/config/robots/ergoCubGazeboV1/blf_balancing_position_control/global_cop_evaluator.ini
@@ -0,0 +1,4 @@
+minimum_normal_force        10.0
+cop_admissible_limits       (0.10, 0.08)
+constant_cop_tolerance      0.01
+constant_cop_max_counter    -1
diff --git a/...ng-position-control/config/robots/ergoCubSN000/blf-balancing-position-control-options.ini b/...ng-position-control/config/robots/ergoCubSN000/blf-balancing-position-control-options.ini
@@ -1,19 +1,24 @@
-dt                       "00:00:00.010000"   # Time expressed in RFC 3339 (0.01 seconds)
+dt                       0.01   # in seconds
 contact_force_threshold  0.1    # in Newton
 
 
 com_knots_delta_x        (0.0, 0.0,  0.03,  0.03, -0.03, -0.03, 0.0,  0.0)    # in meter
 com_knots_delta_y        (0.0, 0.07, 0.07, -0.07, -0.07,  0.07, 0.07, 0.0)    # in meter
 com_knots_delta_z        (0.0, 0.0,  0.0,   0.0,   0.0,   0.0,  0.0,  0.0)    # in meter
-motion_duration          "00:00:10.000000"   # Time expressed in RFC 3339 (10 seconds)
-motion_timeout           "00:00:10.000000"   # Time expressed in RFC 3339 (10 seconds)
+motion_duration          10.0   # in seconds
+motion_timeout           10.0   # in seconds
 
 base_frame               l_sole
 left_contact_frame       l_sole
 right_contact_frame      r_sole
+close_loop_with_zmp      false
+
+[COM_ZMP_CONTROLLER]
+com_gain                        (5.0, 5.0)
+zmp_gain                        (1.0, 1.0)
 
 [include IK "./blf_balancing_position_control/ik.ini"]
 [include ROBOT_CONTROL "./blf_balancing_position_control/robot_control.ini"]
 [include SENSOR_BRIDGE "./blf_balancing_position_control/sensor_bridge.ini"]
 [include CONTACT_WRENCHES "./blf_balancing_position_control/contact_wrenches.ini"]
-
+[include GLOBAL_COP_EVALUATOR "./blf_balancing_position_control/global_cop_evaluator.ini"]
diff --git a/...ontrol/config/robots/ergoCubSN000/blf_balancing_position_control/global_cop_evaluator.ini b/...ontrol/config/robots/ergoCubSN000/blf_balancing_position_control/global_cop_evaluator.ini
@@ -0,0 +1,4 @@
+minimum_normal_force        10.0
+cop_admissible_limits       (0.10, 0.08)
+constant_cop_tolerance      0.01
+constant_cop_max_counter    -1