Implement blf-balancing-position-control application and YarpTextLogging python bindings

CHANGELOG.md

@@ -12,6 +12,8 @@ All notable changes to this project are documented in this file.
 - Add the possibity to call the advanceable capabilities of the `QuinticSpline` from the python (https://github.com/ami-iit/bipedal-locomotion-framework/pull/609)
 - Implement the `CubicSpline` python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/609)
 - Implement the python bindings for the iDynTree to manif conversions (https://github.com/ami-iit/bipedal-locomotion-framework/pull/610)
+- Implement `blf-balancing-position-control` application (https://github.com/ami-iit/bipedal-locomotion-framework/pull/611)
+- Implement the python bindings for `YarpTextLogging` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/611)
 
 ### Changed
 - Ask for `toml++ v3.0.1` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/581)

bindings/python/TextLogging/CMakeLists.txt

@@ -25,3 +25,14 @@ if(FRAMEWORK_COMPILE_RosImplementation)
     LINK_LIBRARIES BipedalLocomotion::TextLoggingRosImplementation)
 
 endif()
+
+
+if(FRAMEWORK_COMPILE_YarpImplementation)
+
+  add_bipedal_locomotion_python_module(
+    NAME  TextLoggingYarpImplementation
+    SOURCES src/YarpLogger.cpp src/YarpModule.cpp
+    HEADERS ${H_PREFIX}/YarpLogger.h ${H_PREFIX}/YarpModule.h
+    LINK_LIBRARIES BipedalLocomotion::TextLoggingYarpImplementation)
+
+endif()

bindings/python/TextLogging/include/BipedalLocomotion/bindings/TextLogging/YarpLogger.h

@@ -0,0 +1,26 @@
+/**
+ * @file YarpLogger.h
+ * @authors Giulio Romualdi
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_YARP_LOGGER_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_YARP_LOGGER_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace TextLogging
+{
+
+void CreateYarpLoggerFactory(pybind11::module& module);
+
+} // namespace TextLogging
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_YARP_LOGGER_H

bindings/python/TextLogging/include/BipedalLocomotion/bindings/TextLogging/YarpModule.h

@@ -0,0 +1,18 @@
+/**
+ * @file YarpModule.h
+ * @authors Giulio Romualdi
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_TEXT_LOGGING_YARP_MODULE_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_TEXT_LOGGING_YARP_MODULE_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion::bindings::TextLogging
+{
+void CreateYarpModule(pybind11::module& module);
+} // namespace BipedalLocomotion::bindings::TextLogging
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_TEXT_LOGGING_YARP_MODULE_H

bindings/python/TextLogging/src/RosLogger.cpp

@@ -31,7 +31,7 @@ void CreateRosLoggerFactory(pybind11::module& module)
                LoggerFactory,
                std::shared_ptr<RosLoggerFactory>>(module,
                                                   "RosLoggerFactory")
-        .def(py::init<const std::string_view&>(), py::arg("name"));
+        .def(py::init<const std::string_view&>(), py::arg("name") = "blf");
 }
 
 } // namespace TextLogging

bindings/python/TextLogging/src/YarpLogger.cpp

@@ -0,0 +1,38 @@
+/**
+ * @file YarpLoggerFactory.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <pybind11/cast.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/TextLogging/YarpLogger.h>
+#include <BipedalLocomotion/bindings/TextLogging/YarpLogger.h>
+
+#include <string_view>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace TextLogging
+{
+
+void CreateYarpLoggerFactory(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+    using namespace BipedalLocomotion::TextLogging;
+
+    py::class_<YarpLoggerFactory, //
+               LoggerFactory,
+               std::shared_ptr<YarpLoggerFactory>>(module,
+                                                  "YarpLoggerFactory")
+        .def(py::init<const std::string_view&>(), py::arg("name") = "blf");
+}
+
+} // namespace TextLogging
+} // namespace bindings
+} // namespace BipedalLocomotion

bindings/python/TextLogging/src/YarpModule.cpp

@@ -0,0 +1,19 @@
+/**
+ * @file YarpModule.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <pybind11/pybind11.h>
+
+#include <BipedalLocomotion/bindings/TextLogging/YarpModule.h>
+#include <BipedalLocomotion/bindings/TextLogging/YarpLogger.h>
+
+namespace BipedalLocomotion::bindings::TextLogging
+{
+void CreateYarpModule(pybind11::module& module)
+{
+    CreateYarpLoggerFactory(module);
+}
+} // namespace BipedalLocomotion::bindings::TextLogging

bindings/python/bipedal_locomotion_framework.cpp.in

@@ -17,6 +17,10 @@
 #include <BipedalLocomotion/bindings/TextLogging/RosModule.h>
 @endcmakeif FRAMEWORK_USE_rclcpp
 
+@cmakeif FRAMEWORK_COMPILE_YarpImplementation
+#include <BipedalLocomotion/bindings/TextLogging/YarpModule.h>
+@endcmakeif FRAMEWORK_COMPILE_YarpImplementation
+
 @cmakeif FRAMEWORK_COMPILE_YarpImplementation
 #include <BipedalLocomotion/bindings/ParametersHandler/YarpModule.h>
 @endcmakeif FRAMEWORK_COMPILE_YarpImplementation
@@ -94,6 +98,10 @@ PYBIND11_MODULE(bindings, m)
     bindings::TextLogging::CreateRosModule(textLoggingModule);
     @endcmakeif FRAMEWORK_USE_rclcpp
 
+    @cmakeif FRAMEWORK_COMPILE_YarpImplementation
+    bindings::TextLogging::CreateYarpModule(textLoggingModule);
+    @endcmakeif FRAMEWORK_COMPILE_YarpImplementation
+
     py::module parametersHandlerModule = m.def_submodule("parameters_handler");
     bindings::ParametersHandler::CreateModule(parametersHandlerModule);
 

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,46 @@
+# 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:
+1. 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 above lists represent the coordinate written in a frame placed in the CoM position at `t=0s`
+   with the `x` axis pointing forward, `z` upward.
+   Given two adjacent knots described by the lists `com_knots_delta_<>`, the planner generates a
+   minimum jerk trajectory that lasts `motion_duration` seconds. Once the knot is reached the planner
+   will wait for `motion_timeout` seconds before starting a new minimum jerk trajectory.
+2. 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).
+
+## 📝 Some additional information
+Before running the application, please notice that:
+1. **balancing-position-control** does not consider the measured zero moment point (ZMP) to generate
+   the CoM trajectory. This means that a user can also run the application when the robot is placed
+   on external support.
+2. The `com_knots_delta_<>` lists represent the coordinate in the CoM frame at `t=0s`this means
+   that the one may also run the application when the robot is in single support. However, in that
+   case, the user must be sure that the CoM trajectory is always within the support polygon and that
+   the joint tracking performance is sufficiently accurate to prevent the robot from falling.
+3. The application solves an inverse kinematics (IK) to generate the joint trajectory. The inverse
+   kinematics considers the feet' position and orientation (pose) and the CoM position as high
+   priority tasks while regularizing the chest orientation and the joint position to a given
+   configuration. The desired pose of the feet, the orientation of the torso, and joint regularization
+   are set equal to the initial values.
+
+---
+
+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,42 @@
+# This software may be modified and distributed under the terms of the BSD-3-Clause license.
+# Authors: Giulio Romualdi
+
+import idyntree.swig as idyn
+
+
+def evaluate_local_zmp(wrench, contact_force_threshold):
+    tau_x = wrench[3]
+    tau_y = wrench[4]
+    f_z = wrench[2]
+    if f_z >= contact_force_threshold:
+        return [-tau_y / f_z, tau_x / f_z, 0.0], True
+    return [0.0, 0.0, 0.0], False
+
+
+def evaluate_global_zmp(
+        left_wrench,
+        right_wrench,
+        kindyn: idyn.KinDynComputations,
+        l_sole_frame,
+        r_sole_frame,
+        contact_force_threshold
+):
+    def to_int(is_defined):
+        if is_defined:
+            return 1
+        return 0
+
+    left_zmp, zmp_left_defined = evaluate_local_zmp(left_wrench, contact_force_threshold)
+    right_zmp, zmp_right_defined = evaluate_local_zmp(right_wrench, contact_force_threshold)
+
+    total_z = right_wrench[2] * to_int(zmp_right_defined) + left_wrench[2] * to_int(zmp_left_defined)
+
+    inertial_zmp_left = kindyn.getWorldTransform(l_sole_frame) * idyn.Position(left_zmp)
+    inertial_zmp_right = kindyn.getWorldTransform(r_sole_frame) * idyn.Position(right_zmp)
+
+    inertial_global_zmp = (
+            left_wrench[2] * to_int(zmp_left_defined) * inertial_zmp_left.toNumPy() / total_z
+            + right_wrench[2] * to_int(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  #in seconds
+positioning_tolerance                   0.05 #in seconds
+position_direct_max_admissible_error    0.1  #in seconds