Add joint velocity filter integration and profiling to balancing torque control

Author: GiulioRomualdi

utilities/balancing-torque-control/config/robots/lowerbodyqdd/blf-balancing-torque-control-options.ini

@@ -14,6 +14,7 @@ left_contact_frame       l_sole
 right_contact_frame      r_sole
 
 [include TSID "./blf_balancing_torque_control/tsid.ini"]
+[include JOINT_VELOCITY_FILTER "./blf_balancing_torque_control/joint_velocity_filter.ini"]
 [include ROBOT_CONTROL "./blf_balancing_torque_control/robot_control.ini"]
 [include SENSOR_BRIDGE "./blf_balancing_torque_control/sensor_bridge.ini"]
 [include DATA_LOGGING "./blf_balancing_torque_control/data_logging.ini"]

utilities/balancing-torque-control/config/robots/lowerbodyqdd/blf_balancing_torque_control/joint_velocity_filter.ini

@@ -0,0 +1,3 @@
+sampling_time 0.001
+cutoff_frequency 20.0
+order 2

utilities/balancing-torque-control/config/robots/lowerbodyqdd/blf_balancing_torque_control/tasks/base_dynamics.ini

@@ -5,6 +5,7 @@ priority                           0
 
 robot_acceleration_variable_name   "robot_acceleration"
 max_number_of_contacts             2
+consider_only_gravitational_term  true
 
 [CONTACT_0]
 variable_name                      "lf_wrench"

utilities/balancing-torque-control/config/robots/lowerbodyqdd/blf_balancing_torque_control/tasks/joint_dynamics.ini

@@ -5,6 +5,7 @@ priority                          0
 robot_acceleration_variable_name  "robot_acceleration"
 joint_torques_variable_name       "joint_torques"
 max_number_of_contacts            2
+consider_only_gravitational_term  true
 
 [CONTACT_0]
 variable_name                      "lf_wrench"

utilities/balancing-torque-control/config/robots/lowerbodyqdd/blf_balancing_torque_control/tsid.ini

@@ -68,6 +68,7 @@ number_of_slices                 2
 static_friction_coefficient      0.3
 foot_limits_x                    (-0.1, 0.1)
 foot_limits_y                    (-0.05, 0.05)
+max_admissible_normal_force      1000.0
 
 [RF_WRENCH_TASK]
 name                            "rf_feasibility_wrench_task"
@@ -80,6 +81,7 @@ number_of_slices                2
 static_friction_coefficient     0.3
 foot_limits_x                   (-0.1, 0.1)
 foot_limits_y                   (-0.05, 0.05)
+max_admissible_normal_force     1000.0
 
 [include BASE_DYNAMICS_TASK ./tasks/base_dynamics.ini]
 [include JOINT_DYNAMICS_TASK ./tasks/joint_dynamics.ini]

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

@@ -213,6 +213,15 @@ def __init__(self, config_file: str):
         self.index = 0
         self.knot_index = 1
 
+        self.joint_velocity_filter = blf.continuous_dynamical_system.ButterworthFilter()
+        if not self.joint_velocity_filter.initialize(
+            param_handler.get_group("JOINT_VELOCITY_FILTER")
+        ):
+            raise RuntimeError("Unable to initialize the joint velocity filter.")
+
+        # reset the filter with zero velocity
+        self.joint_velocity_filter.reset(np.zeros(len(self.joint_positions)))
+
         self.vectors_collection_server = blf.yarp_utilities.VectorsCollectionServer()
         if not self.vectors_collection_server.initialize(
             param_handler.get_group("DATA_LOGGING")
@@ -320,6 +329,17 @@ def advance(self):
             blf.log().error("Impossible to get the joint velocities.")
             return False
 
+        # filter the joint velocities
+        if not self.joint_velocity_filter.set_input(self.joint_velocities):
+            blf.log().error("Impossible to set the input of the joint velocity filter.")
+            return False
+
+        if not self.joint_velocity_filter.advance():
+            blf.log().error("Impossible to advance the joint velocity filter.")
+            return False
+
+        self.joint_velocities = self.joint_velocity_filter.get_output()
+
         if not self.kindyn.setRobotState(
             self.frame_T_link,
             self.desired_joint_positions,
@@ -469,11 +489,22 @@ def main():
 
     application = Application(config_file="blf-balancing-torque-control-options.ini")
 
+    profiler = blf.system.TimeProfiler()
+    profiler.set_period(10 / application.dt.total_seconds())
+    profiler.add_timer("Loop")
+
+    blf.log().info("Press enter to start the balancing torque control.")
+    input()
+
     while True:
         tic = blf.clock().now()
 
+        profiler.setInitTime("Loop")
         if not application.advance():
             return False
+        profiler.setFinalTime("Loop")
+
+        profiler.profile()
 
         toc = blf.clock().now()
         delta_time = toc - tic