Restructure the balancing-position-control script

CHANGELOG.md

@@ -11,13 +11,16 @@ All notable changes to this project are documented in this file.
 - Add the possibility to control a subset of coordinates in `TSID::CoMTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/724, https://github.com/ami-iit/bipedal-locomotion-framework/pull/727)
 - Add the possibility to set the maximum number of accepted deadline miss in `System::AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/726)
 - Add the `getControllerOutput` method to the `TSID::SE3Task` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/740)
+- Implement the python bindings for the `Wrench` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/716)
+- Implement the python bindings for the `SimplifiedModelControllers` components (https://github.com/ami-iit/bipedal-locomotion-framework/pull/716)
 
 ### Changed
 - Remove the possibility to disable the telemetry in `System::AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/726)
 - Change implementation of classes used in `RobotDynamicsEstimator` to optimize performance (https://github.com/ami-iit/bipedal-locomotion-framework/pull/731)
 - CMake: Permit to explictly specify Python installation directory by setting the `FRAMEWORK_PYTHON_INSTALL_DIR` CMake variable (https://github.com/ami-iit/bipedal-locomotion-framework/pull/741)
 - Remove outdated tests for `RobotDynamicsEstimator` library (https://github.com/ami-iit/bipedal-locomotion-framework/pull/742)
 - Modify CI to install `RobotDynamicsEstimator` library (https://github.com/ami-iit/bipedal-locomotion-framework/pull/746)
+- Restructure the balancing-position-control script (https://github.com/ami-iit/bipedal-locomotion-framework/pull/716)
 
 ## [0.15.0] - 2023-09-05
 ### Added

bindings/python/CMakeLists.txt

@@ -18,6 +18,7 @@ add_subdirectory(YarpUtilities)
 add_subdirectory(ContinuousDynamicalSystem)
 add_subdirectory(ReducedModelControllers)
 add_subdirectory(ML)
+add_subdirectory(SimplifiedModelControllers)
 
 include(ConfigureFileWithCMakeIfTarget)
 

bindings/python/Math/CMakeLists.txt

@@ -7,6 +7,6 @@ set(H_PREFIX include/BipedalLocomotion/bindings/Math)
 add_bipedal_locomotion_python_module(
  NAME MathBindings
  SOURCES src/Constants.cpp src/SchmittTrigger.cpp src/Module.cpp
- HEADERS ${H_PREFIX}/Constants.h ${H_PREFIX}/SchmittTrigger.h ${H_PREFIX}/Module.h
+ HEADERS ${H_PREFIX}/Constants.h ${H_PREFIX}/SchmittTrigger.h ${H_PREFIX}/Wrench.h ${H_PREFIX}/Module.h
  LINK_LIBRARIES BipedalLocomotion::Math
  )

bindings/python/Math/include/BipedalLocomotion/bindings/Math/Wrench.h

@@ -0,0 +1,63 @@
+/**
+ * @file Constants.h
+ * @authors Giulio Romualdi
+ * @copyright 2021 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_MATH_WRENCH_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_MATH_WRENCH_H
+
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+
+#include <BipedalLocomotion/Math/Wrench.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace Math
+{
+
+template <typename Scalar> struct WrenchTrampoline
+{
+ BipedalLocomotion::Math::Wrench<Scalar> wrench;
+};
+
+template <typename Scalar> void CreateWrench(pybind11::module& module, const std::string& suffix)
+{
+ namespace py = ::pybind11;
+ using namespace BipedalLocomotion::Math;
+
+ py::class_<WrenchTrampoline<Scalar>>(module, ("Wrench" + suffix).c_str())
+ .def(py::init<>())
+ .def(py::init([](const Eigen::Matrix<Scalar, 6, 1>& vector) -> WrenchTrampoline<Scalar> {
+ WrenchTrampoline<Scalar> tmp;
+ tmp.wrench = vector;
+ return tmp;
+ }))
+ .def("get_local_cop",
+ [](const WrenchTrampoline<Scalar>& impl) -> Eigen::Matrix<Scalar, 3, 1> {
+ return impl.wrench.getLocalCoP();
+ })
+ .def_property(
+ "force",
+ [](const WrenchTrampoline<Scalar>& impl)
+ -> Eigen::Ref<const Eigen::Matrix<Scalar, 3, 1>> { return impl.wrench.force(); },
+ [](WrenchTrampoline<Scalar>& impl, const Eigen::Matrix<Scalar, 3, 1>& force) {
+ impl.wrench.force() = force;
+ })
+ .def_property(
+ "torque",
+ [](const WrenchTrampoline<Scalar>& impl)
+ -> Eigen::Ref<const Eigen::Matrix<Scalar, 3, 1>> { return impl.wrench.torque(); },
+ [](WrenchTrampoline<Scalar>& impl, const Eigen::Matrix<Scalar, 3, 1>& torque) {
+ impl.wrench.torque() = torque;
+ });
+}
+} // namespace Math
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_MATH_WRENCH_H

bindings/python/Math/src/Module.cpp

@@ -10,6 +10,7 @@
 #include <BipedalLocomotion/bindings/Math/Constants.h>
 #include <BipedalLocomotion/bindings/Math/Module.h>
 #include <BipedalLocomotion/bindings/Math/SchmittTrigger.h>
+#include <BipedalLocomotion/bindings/Math/Wrench.h>
 
 namespace BipedalLocomotion
 {
@@ -23,6 +24,7 @@ void CreateModule(pybind11::module& module)
 
  CreateConstants(module);
  CreateSchmittTrigger(module);
+ CreateWrench<double>(module, "d");
 }
 } // namespace Math
 } // namespace bindings

bindings/python/SimplifiedModelControllers/CMakeLists.txt

@@ -0,0 +1,16 @@
+# Copyright (C) 2023 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# BSD-3-Clause license.
+
+if(TARGET BipedalLocomotion::SimplifiedModelControllers)
+
+ set(H_PREFIX include/BipedalLocomotion/bindings/SimplifiedModelControllers)
+
+ add_bipedal_locomotion_python_module(
+ NAME SimplifiedModelControllersBindings
+ SOURCES src/CoMZMPController.cpp src/Module.cpp
+ HEADERS ${H_PREFIX}/CoMZMPController.h ${H_PREFIX}/Module.h
+ LINK_LIBRARIES BipedalLocomotion::SimplifiedModelControllers
+ TESTS tests/test_com_zmp_controller.py)
+
+endif()

bindings/python/SimplifiedModelControllers/include/BipedalLocomotion/bindings/SimplifiedModelControllers/CoMZMPController.h

@@ -0,0 +1,26 @@
+/**
+ * @file CoMZMPController.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_SIMPLIFIED_MODEL_CONTROLLERS_COM_ZMP_CONTROLLER_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_SIMPLIFIED_MODEL_CONTROLLERS_COM_ZMP_CONTROLLER_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace SimplifiedModelControllers
+{
+
+void CreateCoMZMPController(pybind11::module& module);
+
+} // namespace SimplifiedModelControllers
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_SIMPLIFIED_MODEL_CONTROLLERS_COM_ZMP_CONTROLLER_H

bindings/python/SimplifiedModelControllers/include/BipedalLocomotion/bindings/SimplifiedModelControllers/Module.h

@@ -0,0 +1,26 @@
+/**
+ * @file Module.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_SIMPLIFIED_MODEL_CONTROLLERS_MODULE_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_SIMPLIFIED_MODEL_CONTROLLERS_MODULE_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace SimplifiedModelControllers
+{
+
+void CreateModule(pybind11::module& module);
+
+} // namespace Contacts
+} // namespace bindings
+} // namespace SimplifiedModelControllers
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_SIMPLIFIED_MODEL_CONTROLLERS_MODULE_H

bindings/python/SimplifiedModelControllers/src/CoMZMPController.cpp

@@ -0,0 +1,65 @@
+/**
+ * @file CoMZMPController.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 <BipedalLocomotion/SimplifiedModelControllers/CoMZMPController.h>
+#include <BipedalLocomotion/bindings/System/Advanceable.h>
+
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace SimplifiedModelControllers
+{
+
+void CreateCoMZMPController(pybind11::module& module)
+{
+ namespace py = ::pybind11;
+ using namespace BipedalLocomotion::SimplifiedModelControllers;
+ using namespace BipedalLocomotion::System;
+
+ py::class_<CoMZMPControllerInput>(module, "CoMZMPControllerInput")
+ .def(py::init())
+ .def_readwrite("desired_CoM_position", &CoMZMPControllerInput::desiredCoMPosition)
+ .def_readwrite("desired_CoM_velocity", &CoMZMPControllerInput::desiredCoMVelocity)
+ .def_readwrite("desired_ZMP_position", &CoMZMPControllerInput::desiredZMPPosition)
+ .def_readwrite("CoM_position", &CoMZMPControllerInput::CoMPosition)
+ .def_readwrite("ZMP_position", &CoMZMPControllerInput::ZMPPosition)
+ .def_readwrite("angle", &CoMZMPControllerInput::angle);
+
+ BipedalLocomotion::bindings::System::CreateAdvanceable<CoMZMPControllerInput,
+ Eigen::Vector2d>(module,
+ "CoMZMPController");
+ py::class_<CoMZMPController, //
+ Advanceable<CoMZMPControllerInput, Eigen::Vector2d>>(module, "CoMZMPController")
+ .def(py::init())
+ .def("set_set_point",
+ &CoMZMPController::setSetPoint,
+ py::arg("CoM_velocity"),
+ py::arg("CoM_position"),
+ py::arg("ZMP_position"))
+ .def("set_feedback",
+ py::overload_cast<Eigen::Ref<const Eigen::Vector2d>,
+ Eigen::Ref<const Eigen::Vector2d>,
+ const manif::SO2d&>(&CoMZMPController::setFeedback),
+ py::arg("CoM_position"),
+ py::arg("ZMP_position"),
+ py::arg("I_R_B"))
+ .def("set_feedback",
+ py::overload_cast<Eigen::Ref<const Eigen::Vector2d>,
+ Eigen::Ref<const Eigen::Vector2d>,
+ const double>(&CoMZMPController::setFeedback),
+ py::arg("CoM_position"),
+ py::arg("ZMP_position"),
+ py::arg("angle"));
+}
+
+} // namespace SimplifiedModelControllers
+} // namespace bindings
+} // namespace BipedalLocomotion

bindings/python/SimplifiedModelControllers/src/Module.cpp

@@ -0,0 +1,26 @@
+/**
+ * @file Module.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/SimplifiedModelControllers/CoMZMPController.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace SimplifiedModelControllers
+{
+void CreateModule(pybind11::module& module)
+{
+ module.doc() = "Simplified Model Controller module.";
+
+ CreateCoMZMPController(module);
+}
+} // namespace Contacts
+} // namespace bindings
+} // namespace SimplifiedModelControllers

bindings/python/SimplifiedModelControllers/tests/test_com_zmp_controller.py

@@ -0,0 +1,26 @@
+import pytest
+import bipedal_locomotion_framework.bindings as blf
+
+pytestmark = pytest.mark.CoMZMPController
+
+
+def test_com_zmp_controller():
+ controller = blf.simplified_model_controllers.CoMZMPController()
+
+ # Defining the param handler
+ controller_param_handler = blf.parameters_handler.StdParametersHandler()
+ controller_param_handler.set_parameter_vector_float("zmp_gain", [3.0, 4.0])
+ controller_param_handler.set_parameter_vector_float("com_gain", [1.0, 2.0])
+
+ # define the input
+ controller_input = blf.simplified_model_controllers.CoMZMPControllerInput()
+ controller_input.desired_CoM_position = [0.01, 0]
+ controller_input.desired_CoM_velocity = [0.0, 0]
+ controller_input.desired_ZMP_position = [0.01, 0]
+ controller_input.CoM_position = [0.01, 0]
+ controller_input.ZMP_position = [0.01, 0]
+ controller_input.angle = 0
+ assert controller.initialize(controller_param_handler)
+ assert controller.set_input(controller_input)
+ assert controller.advance()
+ assert controller.is_output_valid()

bindings/python/bipedal_locomotion_framework.cpp.in

@@ -94,6 +94,10 @@
 #include<BipedalLocomotion/bindings/ReducedModelControllers/Module.h>
 @endcmakeiftarget BipedalLocomotion::ReducedModelControllers
 
+@cmakeiftarget BipedalLocomotion::SimplifiedModelControllers
+#include<BipedalLocomotion/bindings/SimplifiedModelControllers/Module.h>
+@endcmakeiftarget BipedalLocomotion::SimplifiedModelControllers
+
 // Create the Python module
 PYBIND11_MODULE(bindings, m)
 {
@@ -208,4 +212,9 @@ PYBIND11_MODULE(bindings, m)
  py::module reducedModelControllersModule = m.def_submodule("reduced_model_controllers");
  bindings::ReducedModelControllers::CreateModule(reducedModelControllersModule);
  @endcmakeiftarget BipedalLocomotion::ReducedModelControllers
+
+ @cmakeiftarget BipedalLocomotion::SimplifiedModelControllers
+ py::module simplifiedModelControllersModule = m.def_submodule("simplified_model_controllers");
+ bindings::SimplifiedModelControllers::CreateModule(simplifiedModelControllersModule);
+ @endcmakeiftarget BipedalLocomotion::SimplifiedModelControllers
 }

utilities/balancing-position-control/README.md

@@ -8,7 +8,7 @@ The fastest way to use the utility is to run the `python` application
 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
+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)
@@ -20,25 +20,43 @@ The application will:
  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
+2. Open a port named `/balancing_position_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).
+3. Give you the possibility to close the loop with the ZMP thanks to the ZMP-CoM controller
+ presented in _G. Romualdi, S. Dafarra, Y. Hu, and D. Pucci, "A Benchmarking of DCM Based
+ Architectures for Position and Velocity Controlled Walking of Humanoid Robots," 2018 IEEE-RAS
+ 18th International Conference on Humanoid Robots (Humanoids), Beijing, China, 2018, pp. 1-9, doi:
+ 10.1109/HUMANOIDS.2018.8625025_. To enable the controller, set use_zmp_controller to true in
+ [`blf-balancing-position-control-options.ini``](./config/robots/ergoCubGazeboV1/blf-balancing-position-control-options.ini):
+ ```ini
+ close_loop_with_zmp true
+ ```
 
 ## 📝 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
+1. 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
+2. 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.
+3. The application can be used to stabilize the robot even if it is in single support
+ (i.e., only one foot is in contact with the ground). In this 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. Moreover, we suggest
+ activating the `ZMP-CoM` controller to close the loop with the ZMP and improve the overall
+ performance. In case of single support, the user must also set the `base_frame` parameter to the
+ name of the foot in contact with the ground. For instance, if the left foot is in contact with
+ the ground, the user must set in
+ [`blf-balancing-position-control-options.ini``](./config/robots/ergoCubGazeboV1/blf-balancing-position-control-options.ini):
+ ```ini
+ base_frame l_sole
+ ```
 
 ---
 

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
+ )