From 64ac7bca4af64c09faad798c08afff54f40c7509 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 3 Aug 2023 10:31:00 +0200 Subject: [PATCH 1/4] Implement the python bindings for the SimplifiedModelControllers --- bindings/python/CMakeLists.txt | 1 + .../SimplifiedModelControllers/CMakeLists.txt | 16 +++++ .../CoMZMPController.h | 26 ++++++++ .../SimplifiedModelControllers/Module.h | 26 ++++++++ .../src/CoMZMPController.cpp | 65 +++++++++++++++++++ .../SimplifiedModelControllers/src/Module.cpp | 26 ++++++++ .../tests/test_com_zmp_controller.py | 26 ++++++++ .../bipedal_locomotion_framework.cpp.in | 9 +++ 8 files changed, 195 insertions(+) create mode 100644 bindings/python/SimplifiedModelControllers/CMakeLists.txt create mode 100644 bindings/python/SimplifiedModelControllers/include/BipedalLocomotion/bindings/SimplifiedModelControllers/CoMZMPController.h create mode 100644 bindings/python/SimplifiedModelControllers/include/BipedalLocomotion/bindings/SimplifiedModelControllers/Module.h create mode 100644 bindings/python/SimplifiedModelControllers/src/CoMZMPController.cpp create mode 100644 bindings/python/SimplifiedModelControllers/src/Module.cpp create mode 100644 bindings/python/SimplifiedModelControllers/tests/test_com_zmp_controller.py diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index e3e42af244..f9050a11b0 100644 --- a/bindings/python/CMakeLists.txt +++ b/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) diff --git a/bindings/python/SimplifiedModelControllers/CMakeLists.txt b/bindings/python/SimplifiedModelControllers/CMakeLists.txt new file mode 100644 index 0000000000..a455f14dbe --- /dev/null +++ b/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() diff --git a/bindings/python/SimplifiedModelControllers/include/BipedalLocomotion/bindings/SimplifiedModelControllers/CoMZMPController.h b/bindings/python/SimplifiedModelControllers/include/BipedalLocomotion/bindings/SimplifiedModelControllers/CoMZMPController.h new file mode 100644 index 0000000000..067208a8a5 --- /dev/null +++ b/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 + +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 diff --git a/bindings/python/SimplifiedModelControllers/include/BipedalLocomotion/bindings/SimplifiedModelControllers/Module.h b/bindings/python/SimplifiedModelControllers/include/BipedalLocomotion/bindings/SimplifiedModelControllers/Module.h new file mode 100644 index 0000000000..64b9c768c5 --- /dev/null +++ b/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 + +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 diff --git a/bindings/python/SimplifiedModelControllers/src/CoMZMPController.cpp b/bindings/python/SimplifiedModelControllers/src/CoMZMPController.cpp new file mode 100644 index 0000000000..33e78b7a97 --- /dev/null +++ b/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 +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace SimplifiedModelControllers +{ + +void CreateCoMZMPController(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::SimplifiedModelControllers; + using namespace BipedalLocomotion::System; + + py::class_(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(module, + "CoMZMPController"); + py::class_>(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 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 double>(&CoMZMPController::setFeedback), + py::arg("CoM_position"), + py::arg("ZMP_position"), + py::arg("angle")); +} + +} // namespace SimplifiedModelControllers +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/SimplifiedModelControllers/src/Module.cpp b/bindings/python/SimplifiedModelControllers/src/Module.cpp new file mode 100644 index 0000000000..5cba3627ff --- /dev/null +++ b/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 + +#include + +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 diff --git a/bindings/python/SimplifiedModelControllers/tests/test_com_zmp_controller.py b/bindings/python/SimplifiedModelControllers/tests/test_com_zmp_controller.py new file mode 100644 index 0000000000..80baae6c67 --- /dev/null +++ b/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() diff --git a/bindings/python/bipedal_locomotion_framework.cpp.in b/bindings/python/bipedal_locomotion_framework.cpp.in index 91aeaa4cde..b9ac560587 100644 --- a/bindings/python/bipedal_locomotion_framework.cpp.in +++ b/bindings/python/bipedal_locomotion_framework.cpp.in @@ -94,6 +94,10 @@ #include @endcmakeiftarget BipedalLocomotion::ReducedModelControllers +@cmakeiftarget BipedalLocomotion::SimplifiedModelControllers +#include +@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 } From c78da5ab27e26b59774a1c28ea1077896403bc40 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 31 Oct 2023 18:15:22 +0100 Subject: [PATCH 2/4] Implement the python bindings for the Wrench class --- bindings/python/Math/CMakeLists.txt | 2 +- .../BipedalLocomotion/bindings/Math/Wrench.h | 63 +++++++++++++++++++ bindings/python/Math/src/Module.cpp | 2 + 3 files changed, 66 insertions(+), 1 deletion(-) create mode 100644 bindings/python/Math/include/BipedalLocomotion/bindings/Math/Wrench.h diff --git a/bindings/python/Math/CMakeLists.txt b/bindings/python/Math/CMakeLists.txt index e20cd17186..9170ad9597 100644 --- a/bindings/python/Math/CMakeLists.txt +++ b/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 ) diff --git a/bindings/python/Math/include/BipedalLocomotion/bindings/Math/Wrench.h b/bindings/python/Math/include/BipedalLocomotion/bindings/Math/Wrench.h new file mode 100644 index 0000000000..ed1d59ac7b --- /dev/null +++ b/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 +#include + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace Math +{ + +template struct WrenchTrampoline +{ + BipedalLocomotion::Math::Wrench wrench; +}; + +template void CreateWrench(pybind11::module& module, const std::string& suffix) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::Math; + + py::class_>(module, ("Wrench" + suffix).c_str()) + .def(py::init<>()) + .def(py::init([](const Eigen::Matrix& vector) -> WrenchTrampoline { + WrenchTrampoline tmp; + tmp.wrench = vector; + return tmp; + })) + .def("get_local_cop", + [](const WrenchTrampoline& impl) -> Eigen::Matrix { + return impl.wrench.getLocalCoP(); + }) + .def_property( + "force", + [](const WrenchTrampoline& impl) + -> Eigen::Ref> { return impl.wrench.force(); }, + [](WrenchTrampoline& impl, const Eigen::Matrix& force) { + impl.wrench.force() = force; + }) + .def_property( + "torque", + [](const WrenchTrampoline& impl) + -> Eigen::Ref> { return impl.wrench.torque(); }, + [](WrenchTrampoline& impl, const Eigen::Matrix& torque) { + impl.wrench.torque() = torque; + }); +} +} // namespace Math +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_MATH_WRENCH_H diff --git a/bindings/python/Math/src/Module.cpp b/bindings/python/Math/src/Module.cpp index 77b1f7b342..5dbb22883a 100644 --- a/bindings/python/Math/src/Module.cpp +++ b/bindings/python/Math/src/Module.cpp @@ -10,6 +10,7 @@ #include #include #include +#include namespace BipedalLocomotion { @@ -23,6 +24,7 @@ void CreateModule(pybind11::module& module) CreateConstants(module); CreateSchmittTrigger(module); + CreateWrench(module, "d"); } } // namespace Math } // namespace bindings From a515af7f99f338a2e495292e9c2fec914daa5468 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 1 Aug 2023 12:37:15 +0200 Subject: [PATCH 3/4] =?UTF-8?q?Restructure=20the=20balancing=20position=20?= =?UTF-8?q?controller:=20-=20Add=20the=20possibility=20to=20enable=20and?= =?UTF-8?q?=20disable=20the=20CoM-ZMP=20controller=20in=20the=20balancing?= =?UTF-8?q?=20controller=20-=20Format=20the=20balancing=5Fposition=5Fcontr?= =?UTF-8?q?ol=20script=20with=20black=20-=20Use=20GlobalCoPEvaluator=20cla?= =?UTF-8?q?ss=20to=20compute=20the=20global=20CoP=20-=20=F0=9F=A4=96=20Upd?= =?UTF-8?q?ate=20the=20parameters=20for=20ergoCubSN000=20and=20ergoCubGaze?= =?UTF-8?q?boV1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../balancing-position-control/README.md | 32 +- .../balancing_position_control/wbc.py | 11 +- .../balancing_position_control/zmp.py | 42 -- ...blf-balancing-position-control-options.ini | 14 +- .../global_cop_evaluator.ini | 4 + ...blf-balancing-position-control-options.ini | 13 +- .../global_cop_evaluator.ini | 4 + .../robot_control.ini | 1 - .../script/blf-balancing-position-control.py | 428 ++++++++++++++---- 9 files changed, 393 insertions(+), 156 deletions(-) delete mode 100644 utilities/balancing-position-control/balancing_position_control/zmp.py create mode 100644 utilities/balancing-position-control/config/robots/ergoCubGazeboV1/blf_balancing_position_control/global_cop_evaluator.ini create mode 100644 utilities/balancing-position-control/config/robots/ergoCubSN000/blf_balancing_position_control/global_cop_evaluator.ini diff --git a/utilities/balancing-position-control/README.md b/utilities/balancing-position-control/README.md index e2a90db57d..7443052a51 100644 --- a/utilities/balancing-position-control/README.md +++ b/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 + ``` --- diff --git a/utilities/balancing-position-control/balancing_position_control/wbc.py b/utilities/balancing-position-control/balancing_position_control/wbc.py index 164f150434..fed27caa2f 100644 --- 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 deleted file mode 100644 index 0df3ee90be..0000000000 --- a/utilities/balancing-position-control/balancing_position_control/zmp.py +++ /dev/null @@ -1,42 +0,0 @@ -# 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 diff --git a/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/blf-balancing-position-control-options.ini b/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/blf-balancing-position-control-options.ini index fb3eeef875..e3ae441403 100644 --- a/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/blf-balancing-position-control-options.ini +++ b/utilities/balancing-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/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/blf_balancing_position_control/global_cop_evaluator.ini b/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/blf_balancing_position_control/global_cop_evaluator.ini new file mode 100644 index 0000000000..8da358b28f --- /dev/null +++ b/utilities/balancing-position-control/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/utilities/balancing-position-control/config/robots/ergoCubSN000/blf-balancing-position-control-options.ini b/utilities/balancing-position-control/config/robots/ergoCubSN000/blf-balancing-position-control-options.ini index fb3eeef875..3779dcacc3 100644 --- a/utilities/balancing-position-control/config/robots/ergoCubSN000/blf-balancing-position-control-options.ini +++ b/utilities/balancing-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/utilities/balancing-position-control/config/robots/ergoCubSN000/blf_balancing_position_control/global_cop_evaluator.ini b/utilities/balancing-position-control/config/robots/ergoCubSN000/blf_balancing_position_control/global_cop_evaluator.ini new file mode 100644 index 0000000000..8da358b28f --- /dev/null +++ b/utilities/balancing-position-control/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 diff --git a/utilities/balancing-position-control/config/robots/ergoCubSN000/blf_balancing_position_control/robot_control.ini b/utilities/balancing-position-control/config/robots/ergoCubSN000/blf_balancing_position_control/robot_control.ini index 41261a0c7c..c4257958fd 100644 --- a/utilities/balancing-position-control/config/robots/ergoCubSN000/blf_balancing_position_control/robot_control.ini +++ b/utilities/balancing-position-control/config/robots/ergoCubSN000/blf_balancing_position_control/robot_control.ini @@ -10,4 +10,3 @@ remote_control_boards ("torso", "left_arm", "right_arm", "left positioning_duration 3.0 #in seconds positioning_tolerance 0.05 #in seconds position_direct_max_admissible_error 0.1 #in seconds - diff --git a/utilities/balancing-position-control/script/blf-balancing-position-control.py b/utilities/balancing-position-control/script/blf-balancing-position-control.py index 26f7b5c75d..5d10d70048 100644 --- a/utilities/balancing-position-control/script/blf-balancing-position-control.py +++ b/utilities/balancing-position-control/script/blf-balancing-position-control.py @@ -8,7 +8,7 @@ import bipedal_locomotion_framework.bindings as blf import yarp -import idyntree.swig as idyn +import idyntree.bindings as idyn import manifpy as manif @@ -16,44 +16,79 @@ from pathlib import Path import sys -sys.path.extend([str(Path(__file__).parent.resolve() / ".." / "share" / "BipedalLocomotionFramework" / "python")]) +sys.path.extend( + [ + str( + Path(__file__).parent.resolve() + / ".." + / "share" + / "BipedalLocomotionFramework" + / "python" + ) + ] +) from balancing_position_control.wbc import WBC -from balancing_position_control.zmp import evaluate_local_zmp, evaluate_global_zmp - from datetime import timedelta -def build_remote_control_board_driver(param_handler: blf.parameters_handler.IParametersHandler, local_prefix: str): + +def build_remote_control_board_driver( + param_handler: blf.parameters_handler.IParametersHandler, local_prefix: str +): param_handler.set_parameter_string("local_prefix", local_prefix) return blf.robot_interface.construct_remote_control_board_remapper(param_handler) -def build_contact_wrench_driver(param_handler: blf.parameters_handler.IParametersHandler, local_prefix: str): +def build_contact_wrench_driver( + param_handler: blf.parameters_handler.IParametersHandler, local_prefix: str +): param_handler.set_parameter_string("local_prefix", local_prefix) return blf.robot_interface.construct_generic_sensor_client(param_handler) -def build_contact_wrenches_driver(params_contact_wrenches: blf.parameters_handler.IParametersHandler, - local_prefix: str): +def build_contact_wrenches_driver( + params_contact_wrenches: blf.parameters_handler.IParametersHandler, + local_prefix: str, +): # build contact wrenches polydrivers contact_wrenches_drivers = dict() contact_wrenches_names = dict() contact_wrenches_names["left_foot"] = [] contact_wrenches_names["right_foot"] = [] - for wrench_name in params_contact_wrenches.get_parameter_vector_string("left_contact_wrenches_group"): + for wrench_name in params_contact_wrenches.get_parameter_vector_string( + "left_contact_wrenches_group" + ): contact_wrenches_drivers[wrench_name] = build_contact_wrench_driver( - params_contact_wrenches.get_group(wrench_name), local_prefix) - assert contact_wrenches_drivers[wrench_name].is_valid() - contact_wrenches_names["left_foot"].append( - params_contact_wrenches.get_group(wrench_name).get_parameter_string("description")) + params_contact_wrenches.get_group(wrench_name), local_prefix + ) + if not contact_wrenches_drivers[wrench_name].is_valid(): + raise RuntimeError( + "Unable to create the contact wrench driver for " + wrench_name + ) - for wrench_name in params_contact_wrenches.get_parameter_vector_string("right_contact_wrenches_group"): + contact_wrenches_names["left_foot"].append( + params_contact_wrenches.get_group(wrench_name).get_parameter_string( + "description" + ) + ) + + for wrench_name in params_contact_wrenches.get_parameter_vector_string( + "right_contact_wrenches_group" + ): contact_wrenches_drivers[wrench_name] = build_contact_wrench_driver( - params_contact_wrenches.get_group(wrench_name), local_prefix) - assert contact_wrenches_drivers[wrench_name].is_valid() + params_contact_wrenches.get_group(wrench_name), local_prefix + ) + if not contact_wrenches_drivers[wrench_name].is_valid(): + raise RuntimeError( + "Unable to create the contact wrench driver for " + wrench_name + ) + contact_wrenches_names["right_foot"].append( - params_contact_wrenches.get_group(wrench_name).get_parameter_string("description")) + params_contact_wrenches.get_group(wrench_name).get_parameter_string( + "description" + ) + ) return contact_wrenches_drivers, contact_wrenches_names @@ -61,7 +96,9 @@ def build_contact_wrenches_driver(params_contact_wrenches: blf.parameters_handle def build_kin_dyn(param_handler): rf = yarp.ResourceFinder() robot_model_path = rf.findFile("model.urdf") - joint_list = param_handler.get_group("ROBOT_CONTROL").get_parameter_vector_string("joints_list") + joint_list = param_handler.get_group("ROBOT_CONTROL").get_parameter_vector_string( + "joints_list" + ) ml = idyn.ModelLoader() ml.loadReducedModelFromFile(robot_model_path, joint_list) @@ -74,7 +111,9 @@ def get_base_frame(base_frame: str, kindyn: idyn.KinDynComputations): frame_base_index = kindyn.model().getFrameIndex(base_frame) link_base_index = kindyn.model().getFrameLink(frame_base_index) base_frame_name = kindyn.model().getLinkName(link_base_index) - return base_frame_name, kindyn.getRelativeTransform(frame_base_index, link_base_index) + return base_frame_name, kindyn.getRelativeTransform( + frame_base_index, link_base_index + ) def create_new_spline(knots_positions, motion_duration: timedelta, dt: timedelta): @@ -89,13 +128,22 @@ def create_new_spline(knots_positions, motion_duration: timedelta, dt: timedelta def main(): # Before everything let use the YarpSink for the logger and the YarpClock as clock. These are functionalities # exposed by blf. - assert blf.text_logging.LoggerBuilder.set_factory(blf.text_logging.YarpLoggerFactory('balancing-position-control')) - assert blf.system.ClockBuilder.set_factory(blf.system.YarpClockFactory()) + if not blf.text_logging.LoggerBuilder.set_factory( + blf.text_logging.YarpLoggerFactory("balancing-position-control") + ): + raise RuntimeError("Unable to set the logger factory") + if not blf.system.ClockBuilder.set_factory(blf.system.YarpClockFactory()): + raise RuntimeError("Unable to set the clock factory") param_handler = blf.parameters_handler.YarpParametersHandler() - assert param_handler.set_from_filename("blf-balancing-position-control-options.ini") + if not param_handler.set_from_filename( + "blf-balancing-position-control-options.ini" + ): + raise RuntimeError("Unable to load the parameters") - contact_force_threshold = param_handler.get_parameter_float("contact_force_threshold") + contact_force_threshold = param_handler.get_parameter_float( + "contact_force_threshold" + ) dt = param_handler.get_parameter_datetime("dt") @@ -105,30 +153,40 @@ def main(): # Create the polydrivers poly_drivers, contact_wrenches_names = build_contact_wrenches_driver( params_contact_wrenches=param_handler.get_group("CONTACT_WRENCHES"), - local_prefix="balancing_controller") + local_prefix="balancing_controller", + ) poly_drivers["REMOTE_CONTROL_BOARD"] = build_remote_control_board_driver( param_handler=param_handler.get_group("ROBOT_CONTROL"), - local_prefix="balancing_controller") - assert poly_drivers["REMOTE_CONTROL_BOARD"].is_valid() + local_prefix="balancing_controller", + ) + if not poly_drivers["REMOTE_CONTROL_BOARD"].is_valid(): + raise RuntimeError("Unable to create the remote control board driver") # just to wait that everything is in place blf.log().info("Sleep for two seconds. Just to be sure the interfaces are on.") blf.clock().sleep_for(datetime.timedelta(seconds=2)) robot_control = blf.robot_interface.YarpRobotControl() - assert robot_control.initialize(param_handler.get_group("ROBOT_CONTROL")) - assert robot_control.set_driver(poly_drivers["REMOTE_CONTROL_BOARD"].poly) + if not robot_control.initialize(param_handler.get_group("ROBOT_CONTROL")): + raise RuntimeError("Unable to initialize the robot control") + if not robot_control.set_driver(poly_drivers["REMOTE_CONTROL_BOARD"].poly): + raise RuntimeError("Unable to set the driver for the robot control") # Create the sensor bridge sensor_bridge = blf.robot_interface.YarpSensorBridge() - assert sensor_bridge.initialize(param_handler.get_group("SENSOR_BRIDGE")) - assert sensor_bridge.set_drivers_list(list(poly_drivers.values())) + if not sensor_bridge.initialize(param_handler.get_group("SENSOR_BRIDGE")): + raise RuntimeError("Unable to initialize the sensor bridge") + if not sensor_bridge.set_drivers_list(list(poly_drivers.values())): + raise RuntimeError("Unable to set the drivers for the sensor bridge") # set the kindyn computations with the robot state - assert sensor_bridge.advance() - are_joint_ok, joint_positions, _ = sensor_bridge.get_joint_positions() - assert are_joint_ok + if not sensor_bridge.advance(): + raise RuntimeError("Unable to advance the sensor bridge") + + are_joints_ok, joint_positions, _ = sensor_bridge.get_joint_positions() + if not are_joints_ok: + raise RuntimeError("Unable to get the joint positions") base_frame = param_handler.get_parameter_string("base_frame") base_link, frame_T_link = get_base_frame(base_frame, kindyn=kindyn) @@ -136,27 +194,67 @@ def main(): right_contact_frame = param_handler.get_parameter_string("right_contact_frame") left_contact_frame = param_handler.get_parameter_string("left_contact_frame") - assert kindyn.setFloatingBase(base_link) - gravity = [0., 0., -blf.math.StandardAccelerationOfGravitation] + if not kindyn.setFloatingBase(base_link): + raise RuntimeError("Unable to set the floating base") + + # set the robot state for the kindyn computations object + gravity = [0.0, 0.0, -blf.math.StandardAccelerationOfGravitation] base_velocity = idyn.Twist() base_velocity.zero() - joint_velocity = joint_positions * 0 - assert kindyn.setRobotState(frame_T_link, joint_positions, base_velocity, joint_velocity, gravity) + desired_joint_velocities = joint_positions * 0 + joint_velocities = desired_joint_velocities.copy() + + if not kindyn.setRobotState( + frame_T_link, joint_positions, base_velocity, joint_velocities, gravity + ): + raise RuntimeError("Unable to set the robot state") initial_com_position = kindyn.getCenterOfMassPosition().toNumPy() - assert kindyn_with_measured.setFloatingBase(base_link) - assert kindyn_with_measured.setRobotState(frame_T_link, joint_positions, base_velocity, joint_velocity, gravity) + if not kindyn_with_measured.setFloatingBase(base_link): + raise RuntimeError("Unable to set the floating base") + if not kindyn_with_measured.setRobotState( + frame_T_link, joint_positions, base_velocity, joint_velocities, gravity + ): + raise RuntimeError("Unable to set the robot state") + + global_cop_evaluator = blf.contacts.GlobalCoPEvaluator() + if not global_cop_evaluator.initialize( + param_handler.get_group("GLOBAL_COP_EVALUATOR") + ): + raise RuntimeError("Unable to initialize the global cop evaluator") + + close_loop_with_zmp = param_handler.get_parameter_bool("close_loop_with_zmp") + + # create zmp-com controller if close_loop_with_zmp is true + com_zmp_controller = blf.simplified_model_controllers.CoMZMPController() + if close_loop_with_zmp: + if not com_zmp_controller.initialize( + param_handler.get_group("COM_ZMP_CONTROLLER") + ): + raise RuntimeError("Unable to initialize the zmp-com controller") # create and initialize the IK ik = WBC(param_handler=param_handler.get_group("IK"), kindyn=kindyn) I_H_r_sole = kindyn.getWorldTransform(right_contact_frame) I_H_l_sole = kindyn.getWorldTransform(left_contact_frame) - assert ik.tasks["right_foot_task"].set_set_point( - blf.conversions.to_manif_pose(I_H_r_sole.getRotation().toNumPy(), I_H_r_sole.getPosition().toNumPy())) - assert ik.tasks["left_foot_task"].set_set_point( - blf.conversions.to_manif_pose(I_H_l_sole.getRotation().toNumPy(), I_H_l_sole.getPosition().toNumPy())) - assert ik.tasks["joint_regularization_task"].set_set_point(joint_positions) - assert ik.tasks["torso_task"].set_set_point(manif.SO3.Identity()) + if not ik.tasks["right_foot_task"].set_set_point( + blf.conversions.to_manif_pose( + I_H_r_sole.getRotation().toNumPy(), I_H_r_sole.getPosition().toNumPy() + ) + ): + raise RuntimeError("Unable to set the set point for the right foot task") + if not ik.tasks["left_foot_task"].set_set_point( + blf.conversions.to_manif_pose( + I_H_l_sole.getRotation().toNumPy(), I_H_l_sole.getPosition().toNumPy() + ) + ): + raise RuntimeError("Unable to set the set point for the left foot task") + if not ik.tasks["joint_regularization_task"].set_set_point(joint_positions): + raise RuntimeError( + "Unable to set the set point for the joint regularization task" + ) + if not ik.tasks["torso_task"].set_set_point(manif.SO3.Identity()): + raise RuntimeError("Unable to set the set point for the torso task") desired_joint_positions = joint_positions.copy() @@ -167,76 +265,201 @@ def main(): motion_timeout = param_handler.get_parameter_datetime("motion_timeout") spline = create_new_spline( - [initial_com_position + np.array([com_knots_delta_x[0], com_knots_delta_y[0], com_knots_delta_z[0]]), - initial_com_position + np.array([com_knots_delta_x[1], com_knots_delta_y[1], com_knots_delta_z[1]])], - motion_duration, dt) + [ + initial_com_position + + np.array( + [com_knots_delta_x[0], com_knots_delta_y[0], com_knots_delta_z[0]] + ), + initial_com_position + + np.array( + [com_knots_delta_x[1], com_knots_delta_y[1], com_knots_delta_z[1]] + ), + ], + motion_duration, + dt, + ) index = 0 knot_index = 1 - port = blf.yarp_utilities.BufferedPortVectorsCollection() - port.open("/balancing_controller/logger/data:o") + lipm_omega_square = ( + blf.math.StandardAccelerationOfGravitation / initial_com_position[2] + ) - while True: + desired_com_position = initial_com_position.copy() - tic = blf.clock().now() + port = blf.yarp_utilities.BufferedPortVectorsCollection() + port.open("/balancing_position_controller/logger/data:o") - # get the feedback - assert sensor_bridge.advance() - are_joint_ok, joint_positions, _ = sensor_bridge.get_joint_positions() - assert are_joint_ok - assert kindyn.setRobotState(frame_T_link, desired_joint_positions, base_velocity, joint_velocity, gravity) - assert kindyn_with_measured.setRobotState(frame_T_link, joint_positions, base_velocity, joint_velocity, gravity) + # switch to position direct + robot_control.set_control_mode(blf.robot_interface.YarpRobotControl.PositionDirect) - # solve the IK - com_spline_output = spline.get_output() - assert ik.tasks["com_task"].set_set_point(com_spline_output.position, com_spline_output.velocity) - assert ik.solver.advance() - assert ik.solver.is_output_valid() + blf.log().info("Starting the balancing controller. Waiting for your input") + blf.log().info("Press enter to start the balancing controller") + input() + blf.log().info("Starting the balancing controller") - # integrate the system - desired_joint_positions += ik.solver.get_output().joint_velocity * dt.total_seconds() + while True: + tic = blf.clock().now() - # send the joint pose - assert robot_control.set_references(desired_joint_positions, - blf.robot_interface.YarpRobotControl.PositionDirect) + # get the feedback + if not sensor_bridge.advance(): + raise RuntimeError("Unable to advance the sensor bridge") + are_joints_ok, joint_positions, _ = sensor_bridge.get_joint_positions() + if not are_joints_ok: + raise RuntimeError("Unable to get the joint positions") + + are_joints_ok, joint_velocities, _ = sensor_bridge.get_joint_velocities() + if not are_joints_ok: + raise RuntimeError("Unable to get the joint velocities") + + if not kindyn.setRobotState( + frame_T_link, + desired_joint_positions, + base_velocity, + desired_joint_velocities, + gravity, + ): + raise RuntimeError("Unable to set the robot state") + if not kindyn_with_measured.setRobotState( + frame_T_link, joint_positions, base_velocity, joint_velocities, gravity + ): + raise RuntimeError("Unable to set the robot state") - # send the data left_wrench = np.zeros(6) for cartesian_wrench_name in contact_wrenches_names["left_foot"]: - is_ok, wrench, _ = sensor_bridge.get_cartesian_wrench(cartesian_wrench_name) - assert is_ok + _, wrench, _ = sensor_bridge.get_cartesian_wrench(cartesian_wrench_name) left_wrench += wrench right_wrench = np.zeros(6) for cartesian_wrench_name in contact_wrenches_names["right_foot"]: - is_ok, wrench, _ = sensor_bridge.get_cartesian_wrench(cartesian_wrench_name) - assert is_ok + _, wrench, _ = sensor_bridge.get_cartesian_wrench(cartesian_wrench_name) right_wrench += wrench - global_zmp = evaluate_global_zmp(left_wrench=left_wrench, right_wrench=right_wrench, - l_sole_frame=left_contact_frame, - r_sole_frame=right_contact_frame, - contact_force_threshold=contact_force_threshold, - kindyn=kindyn) - global_zmp_from_measured = evaluate_global_zmp(left_wrench=left_wrench, right_wrench=right_wrench, - l_sole_frame=left_contact_frame, - r_sole_frame=right_contact_frame, - contact_force_threshold=contact_force_threshold, - kindyn=kindyn_with_measured) - local_zmp_left, _ = evaluate_local_zmp(wrench=left_wrench, contact_force_threshold=contact_force_threshold) - local_zmp_right, _ = evaluate_local_zmp(wrench=right_wrench, contact_force_threshold=contact_force_threshold) + # evaluate the global CoP using the desired joint state + left_contact = blf.contacts.ContactWrench() + left_contact.wrench = left_wrench + left_contact.pose = blf.conversions.to_manif_pose( + kindyn.getWorldTransform(left_contact_frame) + ) + right_contact = blf.contacts.ContactWrench() + right_contact.wrench = right_wrench + right_contact.pose = blf.conversions.to_manif_pose( + kindyn.getWorldTransform(right_contact_frame) + ) + + if not global_cop_evaluator.set_input([left_contact, right_contact]): + raise RuntimeError("Unable to set the input for the global cop evaluator") + if not global_cop_evaluator.advance(): + raise RuntimeError("Unable to advance the global cop evaluator") + global_zmp = global_cop_evaluator.get_output() + + # evaluate the global CoP using the measured joint state + left_contact = blf.contacts.ContactWrench() + left_contact.wrench = left_wrench + left_contact.pose = blf.conversions.to_manif_pose( + kindyn_with_measured.getWorldTransform(left_contact_frame) + ) + right_contact = blf.contacts.ContactWrench() + right_contact.wrench = right_wrench + right_contact.pose = blf.conversions.to_manif_pose( + kindyn_with_measured.getWorldTransform(right_contact_frame) + ) + if not global_cop_evaluator.set_input([left_contact, right_contact]): + raise RuntimeError("Unable to set the input for the global cop evaluator") + if not global_cop_evaluator.advance(): + raise RuntimeError("Unable to advance the global cop evaluator") + global_zmp_from_measured = global_cop_evaluator.get_output() + + # use the CoM-ZMP controller + com_spline_output = spline.get_output() + # evaluate the desired ZMP using the LIP model + # ddx_com = omega^2 * (x_com - x_zmp) + desired_zmp = ( + com_spline_output.position[:2] + - com_spline_output.acceleration[:2] / lipm_omega_square + ) + desired_zmp = np.append(desired_zmp, 0.0) + + # set the desired ZMP and the feedback if close_loop_with_zmp is true + if close_loop_with_zmp: + com_zmp_controller.set_set_point( + com_spline_output.velocity[:2], + com_spline_output.position[:2], + desired_zmp[:2], + ) + com_zmp_controller.set_feedback( + kindyn_with_measured.getCenterOfMassPosition().toNumPy()[:2], + global_zmp_from_measured[:2], + 0, + ) + if not com_zmp_controller.advance(): + raise RuntimeError("Unable to advance the CoM-ZMP controller") + + # evaluate the desired CoM position + if close_loop_with_zmp: + desired_com_velocity = np.append( + com_zmp_controller.get_output(), com_spline_output.velocity[2] + ) + desired_com_position[0:2] += ( + com_zmp_controller.get_output() * dt.total_seconds() + ) + desired_com_position[2] = com_spline_output.position[2] + else: + desired_com_velocity = com_spline_output.velocity + desired_com_position = com_spline_output.position + + # solve the IK + if not ik.tasks["com_task"].set_set_point( + desired_com_position, desired_com_velocity + ): + raise RuntimeError("Unable to set the set point for the com task") + if not ik.solver.advance(): + raise RuntimeError("Unable to advance the solver") + if not ik.solver.is_output_valid(): + raise RuntimeError("The solver output is not valid") + + # integrate the system + desired_joint_positions += ( + ik.solver.get_output().joint_velocity * dt.total_seconds() + ) + desired_joint_velocities = ik.solver.get_output().joint_velocity + + # send the joint pose + if not robot_control.set_references( + desired_joint_positions, blf.robot_interface.YarpRobotControl.PositionDirect + ): + raise RuntimeError("Unable to set the references") + + left_wrench = blf.math.Wrenchd(left_wrench) + local_zmp_left = left_wrench.get_local_cop() + right_wrench = blf.math.Wrenchd(right_wrench) + local_zmp_right = right_wrench.get_local_cop() + com_from_desired = kindyn.getCenterOfMassPosition().toNumPy() com_from_measured = kindyn_with_measured.getCenterOfMassPosition().toNumPy() data = port.prepare() - data.vectors = {"global_zmp": global_zmp, "global_zmp_from_measured": global_zmp_from_measured, - "local_zmp_left": local_zmp_left, "local_zmp_right": local_zmp_right, - "com_from_desired": com_from_desired, "com_from_measured": com_from_measured} + data.vectors = { + "zmp::desired_planner": desired_zmp, + "zmp::measured::global::with_joint_desired": global_zmp, + "zmp::measured::global::with_joint_measured": global_zmp_from_measured, + "zmp::measured::local::left": local_zmp_left, + "zmp::measured::local::right": local_zmp_right, + "com::measured::with_joint_desired": com_from_desired, + "com::measured::with_joint_measured": com_from_measured, + "com::planned::position": com_spline_output.position, + "com::planned::velocity": com_spline_output.velocity, + "com::planned::acceleration": com_spline_output.acceleration, + "com::com_zmp::position": desired_com_position, + "com::com_zmp::velocity": desired_com_velocity, + "joints::desired::position": desired_joint_positions, + } port.write() # Final steps - assert spline.advance() + if not spline.advance(): + raise RuntimeError("Unable to advance the spline") if index * dt >= motion_duration + motion_timeout: if knot_index + 1 >= len(com_knots_delta_x): @@ -244,10 +467,27 @@ def main(): break spline = create_new_spline( - [initial_com_position + np.array( - [com_knots_delta_x[knot_index], com_knots_delta_y[knot_index], com_knots_delta_z[knot_index]]), - initial_com_position + np.array([com_knots_delta_x[knot_index + 1], com_knots_delta_y[knot_index + 1], - com_knots_delta_z[knot_index + 1]])], motion_duration, dt) + [ + initial_com_position + + np.array( + [ + com_knots_delta_x[knot_index], + com_knots_delta_y[knot_index], + com_knots_delta_z[knot_index], + ] + ), + initial_com_position + + np.array( + [ + com_knots_delta_x[knot_index + 1], + com_knots_delta_y[knot_index + 1], + com_knots_delta_z[knot_index + 1], + ] + ), + ], + motion_duration, + dt, + ) knot_index += 1 index = 0 @@ -262,6 +502,6 @@ def main(): port.close() -if __name__ == '__main__': +if __name__ == "__main__": network = yarp.Network() main() From 449ba7eb1f10e1abf80be54c55c7654ca6bba658 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 31 Oct 2023 18:29:45 +0100 Subject: [PATCH 4/4] Update the CHANGELOG.md --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5f576e5a26..f2c9f7b65e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,8 @@ 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) @@ -18,6 +20,7 @@ All notable changes to this project are documented in this file. - 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