Merge pull request #428 from ami-iit/pybind11/tsid

Introduce the python bindings for TSID component
ami-iit · Oct 20, 2021 · 89281ae · 89281ae
2 parents 5d0bcbd + 2e4cc44
commit 89281ae
Show file tree

Hide file tree

Showing 66 changed files with 1,137 additions and 104 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -3,6 +3,8 @@ All notable changes to this project are documented in this file.
 
 ## [Unreleased]
 ### Added
+- Implement Python bindings for the TSID component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/428)
+
 ### Changed
 ### Fix
 

diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt
@@ -10,6 +10,7 @@ add_subdirectory(RobotInterface)
 add_subdirectory(Math)
 add_subdirectory(FloatingBaseEstimators)
 add_subdirectory(IK)
+add_subdirectory(TSID)
 add_subdirectory(TextLogging)
 
 include(ConfigureFileWithCMakeIf)

diff --git a/bindings/python/IK/include/BipedalLocomotion/bindings/IK/CoMTask.h b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/CoMTask.h
@@ -5,8 +5,8 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_IK_COM_TASK_H
-#define BIPEDAL_LOCOMOTION_IK_COM_TASK_H
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_COM_TASK_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_IK_COM_TASK_H
 
 #include <pybind11/pybind11.h>
 
@@ -23,4 +23,4 @@ void CreateCoMTask(pybind11::module& module);
 } // namespace bindings
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_IK_COM_TASK_H
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_COM_TASK_H
diff --git a/bindings/python/IK/include/BipedalLocomotion/bindings/IK/IKLinearTask.h b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/IKLinearTask.h
@@ -5,8 +5,8 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_IK_LINEAR_TASK_H
-#define BIPEDAL_LOCOMOTION_IK_LINEAR_TASK_H
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_LINEAR_TASK_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_IK_LINEAR_TASK_H
 
 #include <pybind11/pybind11.h>
 
@@ -23,4 +23,4 @@ void CreateIKLinearTask(pybind11::module& module);
 } // namespace bindings
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_IK_LINEAR_TASK_H
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_LINEAR_TASK_H
diff --git a/bindings/python/IK/include/BipedalLocomotion/bindings/IK/IntegrationBasedIK.h b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/IntegrationBasedIK.h
@@ -5,8 +5,8 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_IK_INTEGRATION_BASE_IK_H
-#define BIPEDAL_LOCOMOTION_IK_INTEGRATION_BASE_IK_H
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_INTEGRATION_BASE_IK_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_IK_INTEGRATION_BASE_IK_H
 
 #include <pybind11/pybind11.h>
 
@@ -23,4 +23,5 @@ void CreateIntegrationBasedIK(pybind11::module& module);
 } // namespace bindings
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_IK_INTEGRATION_BASE_IK_H
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_INTEGRATION_BASE_IK_H
+
diff --git a/bindings/python/IK/include/BipedalLocomotion/bindings/IK/JointTrackingTask.h b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/JointTrackingTask.h
@@ -5,8 +5,8 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_IK_JOINT_TRACKING_TASK_H
-#define BIPEDAL_LOCOMOTION_IK_JOINT_TRACKING_TASK_H
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_JOINT_TRACKING_TASK_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_IK_JOINT_TRACKING_TASK_H
 
 #include <pybind11/pybind11.h>
 
@@ -23,4 +23,4 @@ void CreateJointTrackingTask(pybind11::module& module);
 } // namespace bindings
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_IK_JOINT_TRACKING_TASK_H
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_JOINT_TRACKING_TASK_H
diff --git a/bindings/python/IK/include/BipedalLocomotion/bindings/IK/QPInverseKinematics.h b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/QPInverseKinematics.h
@@ -5,8 +5,8 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_IK_QP_INVERSE_KINEMATICS_H
-#define BIPEDAL_LOCOMOTION_IK_QP_INVERSE_KINEMATICS_H
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_QP_INVERSE_KINEMATICS_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_IK_QP_INVERSE_KINEMATICS_H
 
 #include <pybind11/pybind11.h>
 
@@ -23,4 +23,4 @@ void CreateQPInverseKinematics(pybind11::module& module);
 } // namespace bindings
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_IK_QP_INVERSE_KINEMATICS_H
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_QP_INVERSE_KINEMATICS_H
diff --git a/bindings/python/IK/include/BipedalLocomotion/bindings/IK/SE3Task.h b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/SE3Task.h
@@ -5,8 +5,8 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_IK_SE3_TASK_H
-#define BIPEDAL_LOCOMOTION_IK_SE3_TASK_H
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_SE3_TASK_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_IK_SE3_TASK_H
 
 #include <pybind11/pybind11.h>
 
@@ -23,4 +23,4 @@ void CreateSE3Task(pybind11::module& module);
 } // namespace bindings
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_IK_SE3_TASK_H
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_SE3_TASK_H
diff --git a/bindings/python/IK/include/BipedalLocomotion/bindings/IK/SO3Task.h b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/SO3Task.h
@@ -5,8 +5,8 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_IK_SO3_TASK_H
-#define BIPEDAL_LOCOMOTION_IK_SO3_TASK_H
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_SO3_TASK_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_IK_SO3_TASK_H
 
 #include <pybind11/pybind11.h>
 
@@ -23,4 +23,4 @@ void CreateSO3Task(pybind11::module& module);
 } // namespace bindings
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_IK_SO3_TASK_H
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_SO3_TASK_H
diff --git a/bindings/python/IK/src/CoMTask.cpp b/bindings/python/IK/src/CoMTask.cpp
@@ -27,14 +27,7 @@ void CreateCoMTask(pybind11::module& module)
 
     py::class_<CoMTask, std::shared_ptr<CoMTask>, IKLinearTask>(module, "CoMTask")
         .def(py::init())
-        .def(
-            "initialize",
-            [](CoMTask& impl,
-               std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler>
-                   paramHandler) -> bool { return impl.initialize(paramHandler); },
-            py::arg("param_handler"))
         .def("set_kin_dyn", &CoMTask::setKinDyn, py::arg("kin_dyn"))
-        .def("set_variables_handler", &CoMTask::setVariablesHandler, py::arg("variables_handler"))
         .def("set_set_point", &CoMTask::setSetPoint, py::arg("position"), py::arg("velocity"));
 }
 

diff --git a/bindings/python/IK/src/IntegrationBasedIK.cpp b/bindings/python/IK/src/IntegrationBasedIK.cpp
@@ -10,6 +10,7 @@
 #include <pybind11/stl.h>
 
 #include <BipedalLocomotion/IK/IntegrationBasedIK.h>
+#include <BipedalLocomotion/System/ILinearTaskSolver.h>
 #include <BipedalLocomotion/System/Source.h>
 #include <BipedalLocomotion/bindings/IK/IntegrationBasedIK.h>
 
@@ -33,7 +34,33 @@ void CreateIntegrationBasedIK(pybind11::module& module)
 
     py::class_<Source<IntegrationBasedIKState>>(module, "IntegrationBasedIKStateSource");
 
-    py::class_<IntegrationBasedIK, Source<IntegrationBasedIKState>>(module, "IntegrationBasedIK");
+    py::class_<ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>,
+               Source<IntegrationBasedIKState>>(module, "ILinearTaskSolverIK")
+        .def("add_task",
+             &ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::addTask,
+             py::arg("task"),
+             py::arg("task_name"),
+             py::arg("priority"),
+             py::arg("weight") = Eigen::VectorXd())
+        .def("get_task_names",
+             &ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::getTaskNames)
+        .def("finalize",
+             &ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::finalize,
+             py::arg("handler"))
+        .def("advance", &ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::advance)
+        .def("get_output", &ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::getOutput)
+        .def("is_output_valid",
+             &ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::isOutputValid)
+        .def(
+            "initialize",
+            [](ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>& impl,
+               std::shared_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler>
+                   handler) -> bool { return impl.initialize(handler); },
+            py::arg("handler"));
+
+    py::class_<IntegrationBasedIK, //
+               ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>>(module,
+                                                                         "IntegrationBasedIK");
 }
 
 } // namespace IK

diff --git a/bindings/python/IK/src/JointTrackingTask.cpp b/bindings/python/IK/src/JointTrackingTask.cpp
@@ -28,18 +28,8 @@ void CreateJointTrackingTask(pybind11::module& module)
     py::class_<JointTrackingTask, std::shared_ptr<JointTrackingTask>, IKLinearTask>( //
         module,
         "JointTrackingTask")
-
         .def(py::init())
-        .def(
-            "initialize",
-            [](JointTrackingTask& impl,
-               std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler>
-                   paramHandler) -> bool { return impl.initialize(paramHandler); },
-            py::arg("param_handler"))
         .def("set_kin_dyn", &JointTrackingTask::setKinDyn, py::arg("kin_dyn"))
-        .def("set_variables_handler",
-             &JointTrackingTask::setVariablesHandler,
-             py::arg("variables_handler"))
         .def("set_set_point",
              py::overload_cast<Eigen::Ref<const Eigen::VectorXd>>(&JointTrackingTask::setSetPoint),
              py::arg("joint_position"))

diff --git a/bindings/python/IK/src/QPInverseKinematics.cpp b/bindings/python/IK/src/QPInverseKinematics.cpp
@@ -5,7 +5,6 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#include <pybind11/eigen.h>
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
@@ -27,24 +26,7 @@ void CreateQPInverseKinematics(pybind11::module& module)
     using namespace BipedalLocomotion::IK;
 
     py::class_<QPInverseKinematics, IntegrationBasedIK>(module, "QPInverseKinematics")
-        .def(py::init())
-        .def(
-            "initialize",
-            [](QPInverseKinematics& impl,
-               std::shared_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler>
-                   handler) -> bool { return impl.initialize(handler); },
-            py::arg("handler"))
-        .def("add_task",
-             &QPInverseKinematics::addTask,
-             py::arg("task"),
-             py::arg("taskName"),
-             py::arg("priority"),
-             py::arg("weight") = Eigen::VectorXd())
-        .def("get_task_names", &QPInverseKinematics::getTaskNames)
-        .def("finalize", &QPInverseKinematics::finalize, py::arg("handler"))
-        .def("advance", &QPInverseKinematics::advance)
-        .def("get_output", &QPInverseKinematics::getOutput)
-        .def("is_output_valid", &QPInverseKinematics::isOutputValid);
+        .def(py::init());
 }
 
 } // namespace IK

diff --git a/bindings/python/IK/src/SE3Task.cpp b/bindings/python/IK/src/SE3Task.cpp
@@ -27,14 +27,7 @@ void CreateSE3Task(pybind11::module& module)
 
     py::class_<SE3Task, std::shared_ptr<SE3Task>, IKLinearTask>(module, "SE3Task")
         .def(py::init())
-        .def(
-            "initialize",
-            [](SE3Task& impl,
-               std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler>
-                   paramHandler) -> bool { return impl.initialize(paramHandler); },
-            py::arg("param_handler"))
         .def("set_kin_dyn", &SE3Task::setKinDyn, py::arg("kin_dyn"))
-        .def("set_variables_handler", &SE3Task::setVariablesHandler, py::arg("variables_handler"))
         .def("set_set_point", &SE3Task::setSetPoint, py::arg("I_H_F"), py::arg("mixed_velocity"));
 }
 

diff --git a/bindings/python/IK/src/SO3Task.cpp b/bindings/python/IK/src/SO3Task.cpp
@@ -27,14 +27,7 @@ void CreateSO3Task(pybind11::module& module)
 
     py::class_<SO3Task, std::shared_ptr<SO3Task>, IKLinearTask>(module, "SO3Task")
         .def(py::init())
-        .def(
-            "initialize",
-            [](SO3Task& impl,
-               std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler>
-                   paramHandler) -> bool { return impl.initialize(paramHandler); },
-            py::arg("param_handler"))
         .def("set_kin_dyn", &SO3Task::setKinDyn, py::arg("kin_dyn"))
-        .def("set_variables_handler", &SO3Task::setVariablesHandler, py::arg("variables_handler"))
         .def("set_set_point", &SO3Task::setSetPoint, py::arg("I_R_F"), py::arg("angular_velocity"));
 }
 

diff --git a/bindings/python/IK/tests/test_QP_inverse_kinematics.py b/bindings/python/IK/tests/test_QP_inverse_kinematics.py
@@ -235,7 +235,7 @@ def test_qp_inverse_kinematics():
     assert com_task.set_set_point(position=np.array([0.,0.,0.53]), velocity=np.array([0.,0.,0.]))
 
     # Add CoM task as hard constraint
-    assert qp_ik.add_task(task=com_task, taskName="Com_task", priority=0)
+    assert qp_ik.add_task(task=com_task, task_name="Com_task", priority=0)
 
     # Configure SE3 task (right foot)
     se3_param_handler = blf.parameters_handler.StdParametersHandler()
@@ -254,7 +254,7 @@ def test_qp_inverse_kinematics():
     assert se3_task.set_set_point(I_H_F=I_H_F, mixed_velocity=mixed_velocity)
 
     # Add SE3 task as hard constraint
-    assert qp_ik.add_task(task=se3_task, taskName="se3_task", priority=0)
+    assert qp_ik.add_task(task=se3_task, task_name="se3_task", priority=0)
 
     # Configure joint tracking task (regularization)
     joint_tracking_param_handler = blf.parameters_handler.StdParametersHandler()
@@ -277,7 +277,7 @@ def test_qp_inverse_kinematics():
     assert joint_tracking_task.set_set_point(joint_position=np.add(joint_values,joint_values_delta))
 
     # Add joint tracking task as soft constraint
-    assert qp_ik.add_task(task=joint_tracking_task, taskName="joint_tracking_task", priority=1, weight=[1.0]*kindyn_desc.kindyn.get_nr_of_dofs())
+    assert qp_ik.add_task(task=joint_tracking_task, task_name="joint_tracking_task", priority=1, weight=[1.0]*kindyn_desc.kindyn.get_nr_of_dofs())
 
     # Check that all the tasks have been added
     task_names = qp_ik.get_task_names()

diff --git a/bindings/python/System/include/BipedalLocomotion/bindings/System/LinearTask.h b/bindings/python/System/include/BipedalLocomotion/bindings/System/LinearTask.h
@@ -1,12 +1,12 @@
 /**
  * @file LinearTask.h
- * @authors Paolo Maria Viceconte
+ * @authors Paolo Maria Viceconte, Giulio Romualdi
  * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_SYSTEM_LINEAR_TASK_H
-#define BIPEDAL_LOCOMOTION_SYSTEM_LINEAR_TASK_H
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_LINEAR_TASK_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_LINEAR_TASK_H
 
 #include <pybind11/pybind11.h>
 
@@ -23,4 +23,4 @@ void CreateLinearTask(pybind11::module& module);
 } // namespace bindings
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_SYSTEM_LINEAR_TASK_H
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_LINEAR_TASK_H
diff --git a/bindings/python/System/src/LinearTask.cpp b/bindings/python/System/src/LinearTask.cpp
@@ -5,6 +5,8 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
+#include <pybind11/cast.h>
+#include <pybind11/eigen.h>
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
@@ -24,7 +26,32 @@ void CreateLinearTask(pybind11::module& module)
 
     using namespace BipedalLocomotion::System;
 
-    py::class_<LinearTask, std::shared_ptr<LinearTask>>(module, "LinearTask");
+    py::class_<LinearTask, std::shared_ptr<LinearTask>> linearTask(module, "LinearTask");
+
+    py::enum_<LinearTask::Type>(linearTask, "Type")
+        .value("equality", LinearTask::Type::equality)
+        .value("inequality", LinearTask::Type::inequality)
+        .export_values();
+
+    linearTask
+        .def(
+            "initialize",
+            [](LinearTask& impl,
+               std::shared_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler>
+                   handler) -> bool { return impl.initialize(handler); },
+            py::arg("param_handler"))
+        .def("set_variables_handler",
+             &LinearTask::setVariablesHandler,
+             py::arg("variables_handler"))
+        .def("update", &LinearTask::update)
+        .def("get_A", &LinearTask::getA)
+        .def("get_b", &LinearTask::getB)
+        .def("size", &LinearTask::size)
+        .def("get_description", &LinearTask::getDescription)
+        .def("type", &LinearTask::type)
+        .def("is_valid", &LinearTask::isValid)
+        .def("__repr__", &LinearTask::getDescription)
+        .def("__len__", &LinearTask::size);
 }
 
 } // namespace System

diff --git a/bindings/python/TSID/CMakeLists.txt b/bindings/python/TSID/CMakeLists.txt
@@ -0,0 +1,17 @@
+# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+if(FRAMEWORK_COMPILE_TSID)
+
+  set(H_PREFIX include/BipedalLocomotion/bindings/TSID)
+
+  add_bipedal_locomotion_python_module(
+    NAME TSIDBindings
+    SOURCES src/BaseDynamicsTask.cpp src/CoMTask.cpp src/FeasibleContactWrenchTask.cpp src/JointDynamicsTask.cpp src/JointTrackingTask.cpp src/Module.cpp src/QPTSID.cpp src/SE3Task.cpp src/SO3Task.cpp src/TaskSpaceInverseDynamics.cpp src/TSIDLinearTask.cpp
+    HEADERS ${H_PREFIX}/BaseDynamicsTask.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/FeasibleContactWrenchTask.h ${H_PREFIX}/JointDynamicsTask.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/QPTSID.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/TaskSpaceInverseDynamics.h ${H_PREFIX}/TSIDLinearTask.h
+    LINK_LIBRARIES BipedalLocomotion::TSID
+    TESTS tests/test_TSID.py
+    )
+
+endif()