Implement blf-balancing-position-control application

cmake/BipedalLocomotionFrameworkDependencies.cmake

@@ -232,6 +232,10 @@ framework_dependent_option(FRAMEWORK_COMPILE_CalibrationDeltaUpdaterApplication
   "Compile calibration-delta-updater application?" ON
   "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_PYTHON_BINDINGS;FRAMEWORK_COMPILE_RobotInterface" OFF)
 
+framework_dependent_option(FRAMEWORK_COMPILE_BalancingPositionControlApplication
+  "Compile balancing-position-control application?" ON
+  "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_PYTHON_BINDINGS;FRAMEWORK_COMPILE_RobotInterface;FRAMEWORK_COMPILE_IK" OFF)
+
 framework_dependent_option(FRAMEWORK_COMPILE_YarpRobotLoggerDevice
   "Do you want to generate and compile the YarpRobotLoggerDevice?" ON
   "FRAMEWORK_COMPILE_RobotInterface;FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_Perception;FRAMEWORK_COMPILE_YarpUtilities;FRAMEWORK_USE_robometry" OFF)

utilities/CMakeLists.txt

@@ -8,3 +8,4 @@ add_subdirectory(joint-trajectory-player)
 add_subdirectory(mas-imu-test)
 add_subdirectory(realsense-test)
 add_subdirectory(calibration-delta-updater)
+add_subdirectory(balancing-position-control)

utilities/balancing-position-control/CMakeLists.txt

@@ -0,0 +1,17 @@
+# This software may be modified and distributed under the terms of the
+# BSD-3-Clause license.
+# Authors: Giulio Romualdi
+
+if(FRAMEWORK_COMPILE_BalancingPositionControlApplication)
+
+  install(FILES script/blf-balancing-position-control.py
+    PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ
+    DESTINATION "${CMAKE_INSTALL_BINDIR}")
+
+  install(DIRECTORY balancing_position_control
+    DESTINATION "${CMAKE_INSTALL_PREFIX}/share/BipedalLocomotionFramework/python")
+
+
+  install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/config)
+
+endif()

utilities/balancing-position-control/README.md

@@ -0,0 +1,26 @@
+# balancing-position-control
+The **balancing-position-control** is an application that allows a humanoid robot to move the center-of-mass (CoM) by
+following a given trajectory.
+
+## 🏃 How to use the application
+The fastest way to use the utility is to run the `python` application
+[`blf-balancing-position-control.py`](./script/blf-balancing-position-control.py).
+If you correctly installed the framework, you can run the application from any folder.
+
+The application will:
+- move the robot CoM following a trajectory specified by the following lists in
+  [blf-balancing-position-control-options.ini](./config/robots/ergoCubGazeboV1/blf-balancing-position-control-options.ini)
+  ```ini
+  com_knots_delta_x   (0.0, 0.0, 0.03, 0.03, -0.03, -0.03, 0.0, 0.0)
+  com_knots_delta_y   (0.0, 0.07, 0.07, -0.07, -0.07, 0.07, 0.07, 0.0)
+  com_knots_delta_z   (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+  ```
+  The `com_knots_delta_` represents the delta vertices of the polygon representing the desired CoM trajectory.
+  Given two vertices the application will generate a minimum jerk trajectory with given `motion_duration`.
+  Moreover, the controller will stop for `motion_timeout` once the vertex is reached.
+- open a port named `/balancing_controller/logger/data:o` containing the CoM trajectory and ZMP values structured as
+  [`VectorCollection`](../../src/YarpUtilities/thrifts/BipedalLocomotion/YarpUtilities/VectorsCollection.thrift) data.
+  The user may collect the data via [`YarpRobotLoggerDevice`](../../devices/YarpRobotLoggerDevice).
+
+Please if you want to run the application for a different robot remember to create a new folder in
+[`./config/robots/`](./config/robots). The name of the folder should match the name of the robot.

utilities/balancing-position-control/balancing_position_control/wbc.py

@@ -0,0 +1,11 @@
+# This software may be modified and distributed under the terms of the BSD-3-Clause license.
+# Authors: Giulio Romualdi
+
+import bipedal_locomotion_framework as blf
+import idyntree.swig as idyn
+
+
+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)

utilities/balancing-position-control/balancing_position_control/zmp.py

@@ -0,0 +1,46 @@
+# This software may be modified and distributed under the terms of the BSD-3-Clause license.
+# Authors: Giulio Romualdi
+
+import idyntree.swig as idyn
+import numpy as np
+
+
+def evaluate_local_zmp(wrench):
+    tau_x = wrench[3]
+    tau_y = wrench[4]
+    f_z = wrench[2]
+    if f_z > 0.1:
+        return [-tau_y / f_z, tau_x / f_z, 0.0]
+    return [0.0, 0.0, 0.0]
+
+
+def evaluate_global_zmp(
+        left_wrench,
+        right_wrench,
+        kindyn: idyn.KinDynComputations,
+        l_sole_frame,
+        r_sole_frame,
+):
+    left_zmp = idyn.Position(np.array([0.0, 0.0, 0.0]))
+    zmp_left_defined = 0
+    if left_wrench[2] > 0.1:
+        left_zmp = idyn.Position(evaluate_local_zmp(left_wrench))
+        zmp_left_defined = 1
+
+    right_zmp = idyn.Position(np.array([0.0, 0.0, 0.0]))
+    zmp_right_defined = 0
+    if right_wrench[2] > 0.1:
+        right_zmp = idyn.Position(evaluate_local_zmp(right_wrench))
+        zmp_right_defined = 1
+
+    total_z = right_wrench[2] * zmp_right_defined + left_wrench[2] * zmp_left_defined
+
+    inertial_zmp_left = kindyn.getWorldTransform(l_sole_frame) * left_zmp
+    inertial_zmp_right = kindyn.getWorldTransform(r_sole_frame) * right_zmp
+
+    inertial_global_zmp = (
+            left_wrench[2] * zmp_left_defined * inertial_zmp_left.toNumPy() / total_z
+            + right_wrench[2] * zmp_right_defined * inertial_zmp_right.toNumPy() / total_z
+    )
+
+    return inertial_global_zmp

utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/contact_wrenches.ini

@@ -0,0 +1,22 @@
+left_contact_wrenches_group       ("LEFT_FOOT_FRONT", "LEFT_FOOT_REAR")
+right_contact_wrenches_group      ("RIGHT_FOOT_FRONT", "RIGHT_FOOT_REAR")
+
+[LEFT_FOOT_FRONT]
+description                     "left_foot_front"
+remote_port_name                "/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o"
+local_port_name_postfix         "/left_foot_front/cartesianEndEffectorWrench:i"
+
+[LEFT_FOOT_REAR]
+description                     "left_foot_rear"
+remote_port_name                "/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o"
+local_port_name_postfix         "/left_foot_rear/cartesianEndEffectorWrench:i"
+
+[RIGHT_FOOT_FRONT]
+description                     "right_foot_front"
+remote_port_name                "/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o"
+local_port_name_postfix         "/right_foot_front/cartesianEndEffectorWrench:i"
+
+[RIGHT_FOOT_REAR]
+description                     "right_foot_rear"
+remote_port_name                "/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o"
+local_port_name_postfix         "/right_foot_rear/cartesianEndEffectorWrench:i"

utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/ik.ini

@@ -0,0 +1,66 @@
+tasks ("COM_TASK", "RIGHT_FOOT_TASK", "LEFT_FOOT_TASK", "TORSO_TASK", "JOINT_REGULARIZATION_TASK")
+
+[VARIABLES]
+variables_name  ("robot_velocity")
+variables_size  (29)
+
+
+[IK]
+robot_velocity_variable_name    "robot_velocity"
+
+[COM_TASK]
+name                            com_task
+type                            CoMTask
+robot_velocity_variable_name    "robot_velocity"
+kp_linear                       2.0
+mask                            (true, true, true)
+priority 0
+
+[RIGHT_FOOT_TASK]
+name                            right_foot_task
+type                            SE3Task
+robot_velocity_variable_name    "robot_velocity"
+frame_name                      "r_sole"
+kp_linear                       7.0
+kp_angular                      5.0
+priority                        0
+
+[LEFT_FOOT_TASK]
+name                            left_foot_task
+type                            SE3Task
+robot_velocity_variable_name    "robot_velocity"
+frame_name                      "l_sole"
+kp_linear                       7.0
+kp_angular                      5.0
+priority                        0
+
+[TORSO_TASK]
+name                            torso_task
+
+robot_velocity_variable_name    "robot_velocity"
+frame_name                      "chest"
+kp_angular                      5.0
+
+type            SO3Task
+
+priority 1
+weight  (5.0, 5.0, 5.0)
+
+[JOINT_REGULARIZATION_TASK]
+name                            joint_regularization_task
+
+robot_velocity_variable_name    "robot_velocity"
+kp                              (5.0, 5.0, 5.0,
+                                 5.0, 5.0, 5.0, 5.0,
+                                 5.0, 5.0, 5.0, 5.0,
+                                 5.0, 5.0, 5.0, 5.0, 5.0, 5.0,
+                                 5.0, 5.0, 5.0, 5.0, 5.0, 5.0)
+
+type            JointTrackingTask
+
+priority 1
+weight  (1.0, 1.0, 1.0,
+         2.0, 2.0, 2.0, 2.0,
+         2.0, 2.0, 2.0, 2.0,
+         1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+         1.0, 1.0, 1.0, 1.0, 1.0, 1.0)

utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/robot_control.ini

@@ -0,0 +1,12 @@
+robot_name               ergocubSim
+
+joints_list             ("torso_pitch", "torso_roll", "torso_yaw",
+                        "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow",
+                        "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow",
+                        "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll",
+                        "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")
+
+remote_control_boards   ("torso", "left_arm", "right_arm", "left_leg", "right_leg")
+positioning_duration    3.0
+positioning_tolerance   0.05
+position_direct_max_admissible_error    0.1

utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/sensor_bridge.ini

@@ -0,0 +1,6 @@
+check_for_nan                                    false
+stream_joint_states                              true
+stream_cartesian_wrenches                        true
+
+[CartesianWrenches]
+cartesian_wrenches_list          ("left_foot_front", "left_foot_rear", "right_foot_front", "right_foot_rear")

utilities/balancing-position-control/config/robots/ergoCubGazeboV1/blf-balancing-position-control-options.ini

@@ -0,0 +1,16 @@
+com_knots_delta_x   (0.0, 0.0, 0.03, 0.03, -0.03, -0.03, 0.0, 0.0)
+com_knots_delta_y   (0.0, 0.07, 0.07, -0.07, -0.07, 0.07, 0.07, 0.0)
+com_knots_delta_z   (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+motion_duration     10.0
+motion_timeout      10.0
+base_frame          l_sole
+dt                  0.01
+left_contact_frame  l_sole
+right_contact_frame r_sole
+
+
+[include IK "./balancing_control/ik.ini"]
+[include ROBOT_CONTROL "./balancing_control/robot_control.ini"]
+[include SENSOR_BRIDGE "./balancing_control/sensor_bridge.ini"]
+[include CONTACT_WRENCHES "./balancing_control/contact_wrenches.ini"]
+

utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/contact_wrenches.ini

@@ -0,0 +1,22 @@
+left_contact_wrenches_group       ("LEFT_FOOT_FRONT", "LEFT_FOOT_REAR")
+right_contact_wrenches_group      ("RIGHT_FOOT_FRONT", "RIGHT_FOOT_REAR")
+
+[LEFT_FOOT_FRONT]
+description                     "left_foot_front"
+remote_port_name                "/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o"
+local_port_name_postfix         "/left_foot_front/cartesianEndEffectorWrench:i"
+
+[LEFT_FOOT_REAR]
+description                     "left_foot_rear"
+remote_port_name                "/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o"
+local_port_name_postfix         "/left_foot_rear/cartesianEndEffectorWrench:i"
+
+[RIGHT_FOOT_FRONT]
+description                     "right_foot_front"
+remote_port_name                "/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o"
+local_port_name_postfix         "/right_foot_front/cartesianEndEffectorWrench:i"
+
+[RIGHT_FOOT_REAR]
+description                     "right_foot_rear"
+remote_port_name                "/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o"
+local_port_name_postfix         "/right_foot_rear/cartesianEndEffectorWrench:i"

utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/ik.ini

@@ -0,0 +1,66 @@
+tasks ("COM_TASK", "RIGHT_FOOT_TASK", "LEFT_FOOT_TASK", "TORSO_TASK", "JOINT_REGULARIZATION_TASK")
+
+[VARIABLES]
+variables_name  ("robot_velocity")
+variables_size  (29)
+
+
+[IK]
+robot_velocity_variable_name    "robot_velocity"
+
+[COM_TASK]
+name                            com_task
+type                            CoMTask
+robot_velocity_variable_name    "robot_velocity"
+kp_linear                       2.0
+mask                            (true, true, true)
+priority 0
+
+[RIGHT_FOOT_TASK]
+name                            right_foot_task
+type                            SE3Task
+robot_velocity_variable_name    "robot_velocity"
+frame_name                      "r_sole"
+kp_linear                       7.0
+kp_angular                      5.0
+priority                        0
+
+[LEFT_FOOT_TASK]
+name                            left_foot_task
+type                            SE3Task
+robot_velocity_variable_name    "robot_velocity"
+frame_name                      "l_sole"
+kp_linear                       7.0
+kp_angular                      5.0
+priority                        0
+
+[TORSO_TASK]
+name                            torso_task
+
+robot_velocity_variable_name    "robot_velocity"
+frame_name                      "chest"
+kp_angular                      5.0
+
+type            SO3Task
+
+priority 1
+weight  (5.0, 5.0, 5.0)
+
+[JOINT_REGULARIZATION_TASK]
+name                            joint_regularization_task
+
+robot_velocity_variable_name    "robot_velocity"
+kp                              (5.0, 5.0, 5.0,
+                                 5.0, 5.0, 5.0, 5.0,
+                                 5.0, 5.0, 5.0, 5.0,
+                                 5.0, 5.0, 5.0, 5.0, 5.0, 5.0,
+                                 5.0, 5.0, 5.0, 5.0, 5.0, 5.0)
+
+type            JointTrackingTask
+
+priority 1
+weight  (1.0, 1.0, 1.0,
+         2.0, 2.0, 2.0, 2.0,
+         2.0, 2.0, 2.0, 2.0,
+         1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+         1.0, 1.0, 1.0, 1.0, 1.0, 1.0)

utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/robot_control.ini

@@ -0,0 +1,12 @@
+robot_name               ergocub
+
+joints_list             ("torso_pitch", "torso_roll", "torso_yaw",
+                        "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow",
+                        "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow",
+                        "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll",
+                        "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")
+
+remote_control_boards   ("torso", "left_arm", "right_arm", "left_leg", "right_leg")
+positioning_duration    3.0
+positioning_tolerance   0.05
+position_direct_max_admissible_error    0.1

utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/sensor_bridge.ini

@@ -0,0 +1,6 @@
+check_for_nan                                    false
+stream_joint_states                              true
+stream_cartesian_wrenches                        true
+
+[CartesianWrenches]
+cartesian_wrenches_list          ("left_foot_front", "left_foot_rear", "right_foot_front", "right_foot_rear")

utilities/balancing-position-control/config/robots/ergoCubSN000/blf-balancing-position-control-options.ini

@@ -0,0 +1,16 @@
+com_knots_delta_x   (0.0, 0.0, 0.03, 0.03, -0.03, -0.03, 0.0, 0.0)
+com_knots_delta_y   (0.0, 0.07, 0.07, -0.07, -0.07, 0.07, 0.07, 0.0)
+com_knots_delta_z   (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+motion_duration     10.0
+motion_timeout      10.0
+base_frame          l_sole
+dt                  0.01
+left_contact_frame  l_sole
+right_contact_frame r_sole
+
+
+[include IK "./balancing_control/ik.ini"]
+[include ROBOT_CONTROL "./balancing_control/robot_control.ini"]
+[include SENSOR_BRIDGE "./balancing_control/sensor_bridge.ini"]
+[include CONTACT_WRENCHES "./balancing_control/contact_wrenches.ini"]
+