From 548765545a5d240b2fdf85f1ff9ab45c7db61ed7 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 1 Nov 2021 20:06:16 +0100 Subject: [PATCH 1/5] Introduce the VariablesHandler::initialize python bindings --- bindings/python/System/src/VariablesHandler.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/bindings/python/System/src/VariablesHandler.cpp b/bindings/python/System/src/VariablesHandler.cpp index 81be10e227..2fb4a09ff1 100644 --- a/bindings/python/System/src/VariablesHandler.cpp +++ b/bindings/python/System/src/VariablesHandler.cpp @@ -35,6 +35,12 @@ void CreateVariablesHandler(pybind11::module& module) .def_static("invalid_variable", &VariablesHandler::VariableDescription::InvalidVariable); variablesHandler.def(py::init()) + .def( + "initialize", + [](VariablesHandler& impl, + std::shared_ptr + handler) -> bool { return impl.initialize(handler); }, + py::arg("param_handler")) .def("add_variable", py::overload_cast( &VariablesHandler::addVariable), @@ -46,8 +52,9 @@ void CreateVariablesHandler(pybind11::module& module) py::arg("name"), py::arg("elements_name")) .def("add_variable", - py::overload_cast& >( - &VariablesHandler::addVariable), + py::overload_cast&>(&VariablesHandler::addVariable), py::arg("name"), py::arg("size"), py::arg("elements_name")) From da826db1176c9bef716d50c2b69cee2e7d836091 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 1 Nov 2021 20:07:50 +0100 Subject: [PATCH 2/5] Add the utils module in the python bindings --- bindings/CMakeLists.txt | 9 ++- bindings/python/CMakeLists.txt | 3 + bindings/python/utils.py | 105 +++++++++++++++++++++++++++++++++ 3 files changed, 116 insertions(+), 1 deletion(-) create mode 100644 bindings/python/utils.py diff --git a/bindings/CMakeLists.txt b/bindings/CMakeLists.txt index 7bd3a3d051..24076b2423 100644 --- a/bindings/CMakeLists.txt +++ b/bindings/CMakeLists.txt @@ -4,6 +4,13 @@ if(FRAMEWORK_COMPILE_PYTHON_BINDINGS) + # define new line accordingly to the operating system + if (WIN32) + set(NEW_LINE "\n\r") + else() + set(NEW_LINE "\n") + endif() + option(FRAMEWORK_DETECT_ACTIVE_PYTHON_SITEPACKAGES "Do you want BLF to detect and use the active site-package directory? (it could be a system dir)" FALSE) @@ -28,7 +35,7 @@ if(FRAMEWORK_COMPILE_PYTHON_BINDINGS) # Create the __init__.py file file(GENERATE OUTPUT "${BLF_PYTHON_PACKAGE}/__init__.py" - CONTENT "from . import bindings") + CONTENT "from .bindings import *${NEW_LINE}from . import utils${NEW_LINE}") # Install the __init__.py file install(FILES "${BLF_PYTHON_PACKAGE}/__init__.py" diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 9c4a078777..32835afe0c 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -68,3 +68,6 @@ install(TARGETS pybind11_blf DESTINATION ${PYTHON_INSTDIR}) install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/all.py" DESTINATION ${PYTHON_INSTDIR}) +install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/utils.py" + DESTINATION ${PYTHON_INSTDIR}) + diff --git a/bindings/python/utils.py b/bindings/python/utils.py new file mode 100644 index 0000000000..321752ba18 --- /dev/null +++ b/bindings/python/utils.py @@ -0,0 +1,105 @@ +""" +File: utils.py +Authors: Giulio Romualdi +Copyright (C): 2021 Fondazione Istituto Italiano di Tecnologia +Licensed under either the GNU Lesser General Public License v3.0 : +https://www.gnu.org/licenses/lgpl-3.0.html +or the GNU Lesser General Public License v2.1 : +https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html +at your option. +""" +import bipedal_locomotion_framework as blf + + +def create_tsid(kindyn: blf.floating_base_estimators.KinDynComputationsDescriptor, + param_handler: blf.parameters_handler.IParametersHandler): + """create_tsid is a function that will help you to create a TSID problem. It creates a TSID object with all the + associated tasks. The function returns also the variable handler associated + + Parameters + ---------- + kindyn : bipedal_locomotion_framework.floating_base_estimators.KinDynComputationsDescriptor + a `kindyn` computation object. The object will be shared among all the tasks + param_handler : bipedal_locomotion_framework.parameters_handler.IParametersHandler + The handler containing all the required parameter. + The following configuration file can be used as example + + tasks = ["COM_TASK", "LF_TASK"] + + [TSID] + robot_acceleration_variable_name = "robot_acceleration" + joint_torques_variable_name = "joint_torques" + contact_wrench_variables_name = ["lf_wrench", "rf_wrench"] + + [VARIABLES] + variables_name = ["robot_acceleration", "joint_torques", "lf_wrench", "rf_wrench"] + variables_size = [29, 23, 6, 6] + + # the following parameter is optional and it can be used to give a name to each element of a specific variable + lf_wrench_elements_name = ["fx", "fy", "fz", "tx", "ty", "tz"] + + [COM_TASK] + name = "com" + type = "CoMTask" + priority = 1 + weight = [10.0, 10.0, 10.0] + + # The following parameters are required by the specific task + robot_acceleration_variable_name = "robot_acceleration" + kp_linear = 10.0 + kd_linear = 2.0 + + [LF_TASK] + name = "left_foot" + type = "SE3Task" + priority = 0 + + # The following parameters are required by the specific task + robot_acceleration_variable_name = "robot_acceleration" + frame_name = "left_sole_link" + kp_linear = 10.0 + kd_linear = 2.0 + kp_angular = 10.0 + kd_angular = 2.0 + + Returns + ------- + tuple + a tuple containing the solver, a dictionary of the tasks and the variables handler + """ + + solver = blf.tsid.QPTSID() + tasks = dict() + variables_handler = blf.system.VariablesHandler() + + # initialize the variable handler + variables_handler.initialize(param_handler.get_group("VARIABLES")) + + # initialize the solver + assert solver.initialize(param_handler.get_group("TSID")) + + # retrieve all the tasks + task_groups_name = param_handler.get_parameter_vector_string("tasks") + for task_group_name in task_groups_name: + task_group = param_handler.get_group(task_group_name) + name = task_group.get_parameter_string("name") + if name in tasks.keys(): + raise AssertionError("The task named ", name, " is not unique.") + + # instantiate and initialize the the task + tasks[name] = getattr(blf.tsid, task_group.get_parameter_string("type"))() + assert tasks[name].set_kin_dyn(kindyn.kindyn) + assert tasks[name].initialize(task_group) + + # add the task to the optimization problem + priority = task_group.get_parameter_int("priority") + assert priority == 1 or priority == 0 + if priority == 1: + assert solver.add_task(tasks[name], name, priority, task_group.get_parameter_vector_float("weight")) + else: + assert solver.add_task(tasks[name], name, priority) + + # All the tasks have been added. It's time to finalize the problem + assert solver.finalize(variables_handler) + + return solver, tasks, variables_handler From c7da8bb47e2624ca327511b2f4f87f1b5d123152 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 1 Nov 2021 20:16:24 +0100 Subject: [PATCH 3/5] Update the CHANGELOG --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index f70f5a550e..b04100d569 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,7 @@ All notable changes to this project are documented in this file. - Add the possibility to set the name of each element of a variable stored in the variables handler (https://github.com/ami-iit/bipedal-locomotion-framework/pull/429) - Develop the python bindings for toml implementation of the parameters handler (https://github.com/ami-iit/bipedal-locomotion-framework/pull/432) - Implement the VariableRegularizationTask in TSID (https://github.com/ami-iit/bipedal-locomotion-framework/pull/431) +- Implement `create_tsid` utility function for the python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/433) ### Changed ### Fix From 8a2793fb5884f430e3591b5f5b049308dbd1876d Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 1 Nov 2021 21:11:24 +0100 Subject: [PATCH 4/5] Copy the utils.py file in the correct folder for the tests --- bindings/python/CMakeLists.txt | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 32835afe0c..c3690f38d9 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -46,6 +46,13 @@ set_target_properties(pybind11_blf PROPERTIES if(FRAMEWORK_TEST_PYTHON_BINDINGS) add_subdirectory(tests) + # copy the utils.py file for the tests + add_custom_command(TARGET pybind11_blf POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy + ${CMAKE_CURRENT_SOURCE_DIR}/utils.py + ${BLF_PYTHON_PACKAGE}/utils.py) + + # For testing on Windows we copy the blf .dll in the same # directory of the bindings if(WIN32) @@ -61,7 +68,8 @@ endif() # bipedal_locomotion # |-- __init__.py (generated from main bindings CMake file) # `-- bindings. -# +# `-- utils.py + install(TARGETS pybind11_blf DESTINATION ${PYTHON_INSTDIR}) # Install the __init__.py file From 02b48200496b09eee5950dc40a6c51bd53c4ed38 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 2 Nov 2021 16:43:06 +0100 Subject: [PATCH 5/5] Avoid to call set_kin_dyn if not defined in for a task in create_tsid python method --- bindings/python/utils.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/bindings/python/utils.py b/bindings/python/utils.py index 321752ba18..08d8f94cf4 100644 --- a/bindings/python/utils.py +++ b/bindings/python/utils.py @@ -73,7 +73,7 @@ def create_tsid(kindyn: blf.floating_base_estimators.KinDynComputationsDescripto variables_handler = blf.system.VariablesHandler() # initialize the variable handler - variables_handler.initialize(param_handler.get_group("VARIABLES")) + assert variables_handler.initialize(param_handler.get_group("VARIABLES")) # initialize the solver assert solver.initialize(param_handler.get_group("TSID")) @@ -88,7 +88,11 @@ def create_tsid(kindyn: blf.floating_base_estimators.KinDynComputationsDescripto # instantiate and initialize the the task tasks[name] = getattr(blf.tsid, task_group.get_parameter_string("type"))() - assert tasks[name].set_kin_dyn(kindyn.kindyn) + + # a task may not have set_kin_dyn method + if hasattr(tasks[name], "set_kin_dyn") and callable(getattr(tasks[name], "set_kin_dyn")): + assert tasks[name].set_kin_dyn(kindyn.kindyn) + assert tasks[name].initialize(task_group) # add the task to the optimization problem