Implement create_tsid utility function for the python bindings

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

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"

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,10 +68,14 @@ endif()
 # bipedal_locomotion
 # |-- __init__.py (generated from main bindings CMake file)
 # `-- bindings.<cpython_extension>
-#
+# `-- utils.py
+
 install(TARGETS pybind11_blf DESTINATION ${PYTHON_INSTDIR})
 
 # Install the __init__.py file
 install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/all.py"
  DESTINATION ${PYTHON_INSTDIR})
 
+install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/utils.py"
+ DESTINATION ${PYTHON_INSTDIR})
+

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<const BipedalLocomotion::ParametersHandler::IParametersHandler>
+ handler) -> bool { return impl.initialize(handler); },
+ py::arg("param_handler"))
  .def("add_variable",
  py::overload_cast<const std::string&, const std::size_t&>(
  &VariablesHandler::addVariable),
@@ -46,8 +52,9 @@ void CreateVariablesHandler(pybind11::module& module)
  py::arg("name"),
  py::arg("elements_name"))
  .def("add_variable",
- py::overload_cast<const std::string&, const std::size_t&, const std::vector<std::string>& >(
- &VariablesHandler::addVariable),
+ py::overload_cast<const std::string&,
+ const std::size_t&,
+ const std::vector<std::string>&>(&VariablesHandler::addVariable),
  py::arg("name"),
  py::arg("size"),
  py::arg("elements_name"))

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