From dd8595555bbdffae7fcddbd5db5a34c0070c72bc Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 19 Oct 2021 23:24:49 +0200 Subject: [PATCH 01/13] Add initialize method in System::LinearTask class --- src/System/include/BipedalLocomotion/System/LinearTask.h | 8 ++++++++ src/System/src/LinearTask.cpp | 5 +++++ 2 files changed, 13 insertions(+) diff --git a/src/System/include/BipedalLocomotion/System/LinearTask.h b/src/System/include/BipedalLocomotion/System/LinearTask.h index 373e00179c..ff94c9e254 100644 --- a/src/System/include/BipedalLocomotion/System/LinearTask.h +++ b/src/System/include/BipedalLocomotion/System/LinearTask.h @@ -68,6 +68,14 @@ class LinearTask */ virtual bool setVariablesHandler(const System::VariablesHandler& variablesHandler); + /** + * Initialize the task. + * @param paramHandler pointer to the parameters handler. + * @return True in case of success, false otherwise. + */ + virtual bool + initialize(std::weak_ptr paramHandler); + /** * Update the content of the task. * @return True in case of success, false otherwise. diff --git a/src/System/src/LinearTask.cpp b/src/System/src/LinearTask.cpp index d3e9e447ed..b0ac9f7d04 100644 --- a/src/System/src/LinearTask.cpp +++ b/src/System/src/LinearTask.cpp @@ -33,6 +33,11 @@ bool LinearTask::setVariablesHandler(const VariablesHandler& variablesHandler) return true; } +bool LinearTask::initialize(std::weak_ptr paramHandler) +{ + return true; +} + bool LinearTask::update() { return true; From e0631361a2e684eff93921edaf81aec36814ace4 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 19 Oct 2021 23:26:04 +0200 Subject: [PATCH 02/13] Update the IK component accordingly to the dd8595555bbdffae7fcddbd5db5a34c0070c72bc --- src/IK/include/BipedalLocomotion/IK/CoMTask.h | 2 +- src/IK/include/BipedalLocomotion/IK/JointTrackingTask.h | 2 +- src/IK/include/BipedalLocomotion/IK/SE3Task.h | 2 +- src/IK/include/BipedalLocomotion/IK/SO3Task.h | 2 +- src/IK/src/CoMTask.cpp | 2 +- src/IK/src/JointTrackingTask.cpp | 2 +- src/IK/src/SE3Task.cpp | 2 +- src/IK/src/SO3Task.cpp | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/IK/include/BipedalLocomotion/IK/CoMTask.h b/src/IK/include/BipedalLocomotion/IK/CoMTask.h index 5c9f29e065..63850a25a9 100644 --- a/src/IK/include/BipedalLocomotion/IK/CoMTask.h +++ b/src/IK/include/BipedalLocomotion/IK/CoMTask.h @@ -84,7 +84,7 @@ class CoMTask : public IKLinearTask * (expressed in mixed representation) and the joint velocities. * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler); + bool initialize(std::weak_ptr paramHandler) override; /** * Set the kinDynComputations object. diff --git a/src/IK/include/BipedalLocomotion/IK/JointTrackingTask.h b/src/IK/include/BipedalLocomotion/IK/JointTrackingTask.h index ecdbadf6f9..ccd928f9d5 100644 --- a/src/IK/include/BipedalLocomotion/IK/JointTrackingTask.h +++ b/src/IK/include/BipedalLocomotion/IK/JointTrackingTask.h @@ -62,7 +62,7 @@ class JointTrackingTask : public IKLinearTask * (expressed in mixed representation) and the joint velocities. * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler); + bool initialize(std::weak_ptr paramHandler) override; /** * Set the kinDynComputations object. diff --git a/src/IK/include/BipedalLocomotion/IK/SE3Task.h b/src/IK/include/BipedalLocomotion/IK/SE3Task.h index 84b4522c67..c78797dfed 100644 --- a/src/IK/include/BipedalLocomotion/IK/SE3Task.h +++ b/src/IK/include/BipedalLocomotion/IK/SE3Task.h @@ -96,7 +96,7 @@ class SE3Task : public IKLinearTask, public BipedalLocomotion::System::ITaskCont * (expressed in mixed representation) and the joint velocities. * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler); + bool initialize(std::weak_ptr paramHandler) override; /** * Set the kinDynComputations object. diff --git a/src/IK/include/BipedalLocomotion/IK/SO3Task.h b/src/IK/include/BipedalLocomotion/IK/SO3Task.h index c296c0f08b..0644afe946 100644 --- a/src/IK/include/BipedalLocomotion/IK/SO3Task.h +++ b/src/IK/include/BipedalLocomotion/IK/SO3Task.h @@ -76,7 +76,7 @@ class SO3Task : public IKLinearTask * (expressed in mixed representation) and the joint velocities. * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler); + bool initialize(std::weak_ptr paramHandler) override; /** * Set the kinDynComputations object. diff --git a/src/IK/src/CoMTask.cpp b/src/IK/src/CoMTask.cpp index 488e929acc..faf38c506c 100644 --- a/src/IK/src/CoMTask.cpp +++ b/src/IK/src/CoMTask.cpp @@ -65,7 +65,7 @@ bool CoMTask::setVariablesHandler(const System::VariablesHandler& variablesHandl return true; } -bool CoMTask::initialize(std::weak_ptr paramHandler) +bool CoMTask::initialize(std::weak_ptr paramHandler) { constexpr auto errorPrefix = "[CoMTask::initialize]"; diff --git a/src/IK/src/JointTrackingTask.cpp b/src/IK/src/JointTrackingTask.cpp index 49102b1b3c..2ba6f7248e 100644 --- a/src/IK/src/JointTrackingTask.cpp +++ b/src/IK/src/JointTrackingTask.cpp @@ -71,7 +71,7 @@ bool JointTrackingTask::setVariablesHandler(const System::VariablesHandler& vari return true; } -bool JointTrackingTask::initialize(std::weak_ptr paramHandler) +bool JointTrackingTask::initialize(std::weak_ptr paramHandler) { constexpr auto errorPrefix = "[JointTrackingTask::initialize] "; diff --git a/src/IK/src/SE3Task.cpp b/src/IK/src/SE3Task.cpp index 9a8ea2e51a..b82b4945e2 100644 --- a/src/IK/src/SE3Task.cpp +++ b/src/IK/src/SE3Task.cpp @@ -65,7 +65,7 @@ bool SE3Task::setVariablesHandler(const System::VariablesHandler& variablesHandl return true; } -bool SE3Task::initialize(std::weak_ptr paramHandler) +bool SE3Task::initialize(std::weak_ptr paramHandler) { constexpr auto errorPrefix = "[SE3Task::initialize] "; diff --git a/src/IK/src/SO3Task.cpp b/src/IK/src/SO3Task.cpp index 0469561005..5c0f7242a2 100644 --- a/src/IK/src/SO3Task.cpp +++ b/src/IK/src/SO3Task.cpp @@ -65,7 +65,7 @@ bool SO3Task::setVariablesHandler(const System::VariablesHandler& variablesHandl return true; } -bool SO3Task::initialize(std::weak_ptr paramHandler) +bool SO3Task::initialize(std::weak_ptr paramHandler) { constexpr std::string_view errorPrefix = "[SO3Task::initialize] "; From 1cb4f18fe04a00813ef49208efd8a7d38b319f39 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 19 Oct 2021 23:28:02 +0200 Subject: [PATCH 03/13] Update the python bindings associated to the LinearTask and the IK accordingly to dd8595555bbdffae7fcddbd5db5a34c0070c72bc and e0631361a2e684eff93921edaf81aec36814ace4 --- bindings/python/IK/src/CoMTask.cpp | 7 ----- bindings/python/IK/src/JointTrackingTask.cpp | 10 ------- bindings/python/IK/src/SE3Task.cpp | 7 ----- bindings/python/IK/src/SO3Task.cpp | 7 ----- bindings/python/System/src/LinearTask.cpp | 28 +++++++++++++++++++- 5 files changed, 27 insertions(+), 32 deletions(-) diff --git a/bindings/python/IK/src/CoMTask.cpp b/bindings/python/IK/src/CoMTask.cpp index 0b734a291a..b0ba833af5 100644 --- 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_, IKLinearTask>(module, "CoMTask") .def(py::init()) - .def( - "initialize", - [](CoMTask& impl, - std::shared_ptr - 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/JointTrackingTask.cpp b/bindings/python/IK/src/JointTrackingTask.cpp index 42bfbbf097..b72a9cd062 100644 --- 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_, IKLinearTask>( // module, "JointTrackingTask") - .def(py::init()) - .def( - "initialize", - [](JointTrackingTask& impl, - std::shared_ptr - 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>(&JointTrackingTask::setSetPoint), py::arg("joint_position")) diff --git a/bindings/python/IK/src/SE3Task.cpp b/bindings/python/IK/src/SE3Task.cpp index 4bca184eec..530a14df23 100644 --- 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_, IKLinearTask>(module, "SE3Task") .def(py::init()) - .def( - "initialize", - [](SE3Task& impl, - std::shared_ptr - 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 index 56082c8dbc..00e085e8bc 100644 --- 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_, IKLinearTask>(module, "SO3Task") .def(py::init()) - .def( - "initialize", - [](SO3Task& impl, - std::shared_ptr - 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/System/src/LinearTask.cpp b/bindings/python/System/src/LinearTask.cpp index 8bdc497f35..a156c7676e 100644 --- a/bindings/python/System/src/LinearTask.cpp +++ b/bindings/python/System/src/LinearTask.cpp @@ -5,6 +5,7 @@ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. */ +#include #include #include @@ -24,7 +25,32 @@ void CreateLinearTask(pybind11::module& module) using namespace BipedalLocomotion::System; - py::class_>(module, "LinearTask"); + py::class_> linearTask(module, "LinearTask"); + + py::enum_(linearTask, "Type") + .value("equality", LinearTask::Type::equality) + .value("inequality", LinearTask::Type::inequality) + .export_values(); + + linearTask + .def( + "initialize", + [](LinearTask& impl, + std::shared_ptr + 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 From 1d5d2fc7a23ef6e3c986be3c37495c5cf3c5841e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 19 Oct 2021 23:36:40 +0200 Subject: [PATCH 04/13] Update the TSID component accordingly to the dd8595555bbdffae7fcddbd5db5a34c0070c72bc --- src/TSID/include/BipedalLocomotion/TSID/BaseDynamicsTask.h | 2 +- src/TSID/include/BipedalLocomotion/TSID/CoMTask.h | 2 +- .../include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h | 2 +- src/TSID/include/BipedalLocomotion/TSID/JointDynamicsTask.h | 2 +- src/TSID/include/BipedalLocomotion/TSID/JointTrackingTask.h | 2 +- src/TSID/include/BipedalLocomotion/TSID/SE3Task.h | 2 +- src/TSID/include/BipedalLocomotion/TSID/SO3Task.h | 2 +- src/TSID/src/BaseDynamicsTask.cpp | 2 +- src/TSID/src/JointDynamicsTask.cpp | 2 +- src/TSID/src/JointTrackingTask.cpp | 2 +- src/TSID/src/SE3Task.cpp | 2 +- src/TSID/src/SO3Task.cpp | 2 +- 12 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/TSID/include/BipedalLocomotion/TSID/BaseDynamicsTask.h b/src/TSID/include/BipedalLocomotion/TSID/BaseDynamicsTask.h index 709d03ae40..62f8551b61 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/BaseDynamicsTask.h +++ b/src/TSID/include/BipedalLocomotion/TSID/BaseDynamicsTask.h @@ -82,7 +82,7 @@ class BaseDynamicsTask : public TSIDLinearTask * | `frame_name` | `string` | Name of the frame associated to the contact | Yes | * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler); + bool initialize(std::weak_ptr paramHandler) override; /** * Set the kinDynComputations object. diff --git a/src/TSID/include/BipedalLocomotion/TSID/CoMTask.h b/src/TSID/include/BipedalLocomotion/TSID/CoMTask.h index c82991c2ad..f4a159052e 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/CoMTask.h +++ b/src/TSID/include/BipedalLocomotion/TSID/CoMTask.h @@ -73,7 +73,7 @@ class CoMTask : public TSIDLinearTask * (expressed in mixed representation) and the joint velocities. * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler); + bool initialize(std::weak_ptr paramHandler) override; /** * Set the kinDynComputations object. diff --git a/src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h b/src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h index 3950b5866f..5bb9b3e98e 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h +++ b/src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h @@ -68,7 +68,7 @@ class FeasibleContactWrenchTask : public TSIDLinearTask * | `foot_limits_y` | `vector` | y coordinate of the foot limits w.r.t the frame attached to the surface | Yes | * @return true in case of success/false otherwise. */ - bool initialize(std::weak_ptr paramHandler); + bool initialize(std::weak_ptr paramHandler) override; /** * Set the kinDynComputations object. diff --git a/src/TSID/include/BipedalLocomotion/TSID/JointDynamicsTask.h b/src/TSID/include/BipedalLocomotion/TSID/JointDynamicsTask.h index 99daed74df..de8bdf7e89 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/JointDynamicsTask.h +++ b/src/TSID/include/BipedalLocomotion/TSID/JointDynamicsTask.h @@ -85,7 +85,7 @@ class JointDynamicsTask : public TSIDLinearTask * | `frame_name` | `string` | Name of the frame associated to the contact | Yes | * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler); + bool initialize(std::weak_ptr paramHandler) override; /** * Set the kinDynComputations object. diff --git a/src/TSID/include/BipedalLocomotion/TSID/JointTrackingTask.h b/src/TSID/include/BipedalLocomotion/TSID/JointTrackingTask.h index 2ac59ac7b7..c4581ed405 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/JointTrackingTask.h +++ b/src/TSID/include/BipedalLocomotion/TSID/JointTrackingTask.h @@ -63,7 +63,7 @@ class JointTrackingTask : public TSIDLinearTask * | `kd` | `vector` | Derivative Gain of the controller. if not specified \f$k_d = 2 \sqrt{k_p}\f$ | No | * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler); + bool initialize(std::weak_ptr paramHandler) override; /** * Set the kinDynComputations object. diff --git a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h index af2dd29d44..1525219e0d 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h +++ b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h @@ -80,7 +80,7 @@ class SE3Task : public TSIDLinearTask * | `kd_angular` | `double` | Gain of the angular velocity controller | Yes | * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler); + bool initialize(std::weak_ptr paramHandler) override; /** * Set the kinDynComputations object. diff --git a/src/TSID/include/BipedalLocomotion/TSID/SO3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SO3Task.h index f96674e64d..cc77fca364 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/SO3Task.h +++ b/src/TSID/include/BipedalLocomotion/TSID/SO3Task.h @@ -73,7 +73,7 @@ class SO3Task : public TSIDLinearTask * | `kd_angular` | `double` | Gain of the angular velocity controller | Yes | * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler); + bool initialize(std::weak_ptr paramHandler) override; /** * Set the kinDynComputations object. diff --git a/src/TSID/src/BaseDynamicsTask.cpp b/src/TSID/src/BaseDynamicsTask.cpp index 1585346ee4..1d839d6431 100644 --- a/src/TSID/src/BaseDynamicsTask.cpp +++ b/src/TSID/src/BaseDynamicsTask.cpp @@ -83,7 +83,7 @@ bool BaseDynamicsTask::setVariablesHandler(const System::VariablesHandler& varia return true; } -bool BaseDynamicsTask::initialize(std::weak_ptr paramHandler) +bool BaseDynamicsTask::initialize(std::weak_ptr paramHandler) { constexpr auto errorPrefix = "[BaseDynamicsTask::initialize]"; diff --git a/src/TSID/src/JointDynamicsTask.cpp b/src/TSID/src/JointDynamicsTask.cpp index 350bee951c..0e28322cf3 100644 --- a/src/TSID/src/JointDynamicsTask.cpp +++ b/src/TSID/src/JointDynamicsTask.cpp @@ -101,7 +101,7 @@ bool JointDynamicsTask::setVariablesHandler(const System::VariablesHandler& vari return true; } -bool JointDynamicsTask::initialize(std::weak_ptr paramHandler) +bool JointDynamicsTask::initialize(std::weak_ptr paramHandler) { constexpr auto errorPrefix = "[JointDynamicsTask::initialize]"; diff --git a/src/TSID/src/JointTrackingTask.cpp b/src/TSID/src/JointTrackingTask.cpp index a5ebef12dd..9eedf2c250 100644 --- a/src/TSID/src/JointTrackingTask.cpp +++ b/src/TSID/src/JointTrackingTask.cpp @@ -69,7 +69,7 @@ bool JointTrackingTask::setVariablesHandler(const System::VariablesHandler& vari return true; } -bool JointTrackingTask::initialize(std::weak_ptr paramHandler) +bool JointTrackingTask::initialize(std::weak_ptr paramHandler) { constexpr auto errorPrefix = "[JointTrackingTask::initialize]"; diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp index 0a2eaea2ea..aea50a72ba 100644 --- a/src/TSID/src/SE3Task.cpp +++ b/src/TSID/src/SE3Task.cpp @@ -65,7 +65,7 @@ bool SE3Task::setKinDyn(std::shared_ptr kinDyn) return true; } -bool SE3Task::initialize(std::weak_ptr paramHandler) +bool SE3Task::initialize(std::weak_ptr paramHandler) { constexpr std::string_view errorPrefix = "[SE3Task::initialize] "; diff --git a/src/TSID/src/SO3Task.cpp b/src/TSID/src/SO3Task.cpp index 38104d1a30..7a2d58ab7d 100644 --- a/src/TSID/src/SO3Task.cpp +++ b/src/TSID/src/SO3Task.cpp @@ -67,7 +67,7 @@ bool SO3Task::setKinDyn(std::shared_ptr kinDyn) return true; } -bool SO3Task::initialize(std::weak_ptr paramHandler) +bool SO3Task::initialize(std::weak_ptr paramHandler) { constexpr std::string_view errorPrefix = "[SO3Task::initialize] "; From 341bb669e550d563e0ff352973c3a692cb4bd750 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 20 Oct 2021 00:12:26 +0200 Subject: [PATCH 05/13] Fix declare barrier in the python bindings include files --- .../IK/include/BipedalLocomotion/bindings/IK/CoMTask.h | 6 +++--- .../include/BipedalLocomotion/bindings/IK/IKLinearTask.h | 6 +++--- .../BipedalLocomotion/bindings/IK/IntegrationBasedIK.h | 7 ++++--- .../BipedalLocomotion/bindings/IK/JointTrackingTask.h | 6 +++--- .../BipedalLocomotion/bindings/IK/QPInverseKinematics.h | 6 +++--- .../IK/include/BipedalLocomotion/bindings/IK/SE3Task.h | 6 +++--- .../IK/include/BipedalLocomotion/bindings/IK/SO3Task.h | 6 +++--- .../BipedalLocomotion/bindings/System/LinearTask.h | 8 ++++---- 8 files changed, 26 insertions(+), 25 deletions(-) diff --git a/bindings/python/IK/include/BipedalLocomotion/bindings/IK/CoMTask.h b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/CoMTask.h index 4596fd1568..4282358d45 100644 --- 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 @@ -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 index b3bbddf0ce..5ad9cb992f 100644 --- 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 @@ -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 index 213ccdec0c..67218efd6f 100644 --- 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 @@ -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 index ee7e7ff758..55f095986f 100644 --- 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 @@ -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 index 04966b2742..e89e872fc1 100644 --- 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 @@ -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 index 051bf81ef7..d54bb8ebd9 100644 --- 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 @@ -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 index c679fbef8d..b0aee847bc 100644 --- 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 @@ -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/System/include/BipedalLocomotion/bindings/System/LinearTask.h b/bindings/python/System/include/BipedalLocomotion/bindings/System/LinearTask.h index 38b8e0b8bd..bc90af4acf 100644 --- 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 @@ -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 From a57b318e198035cbe7b70ca194483c6bc4d72dfd Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 20 Oct 2021 01:59:53 +0200 Subject: [PATCH 06/13] Fix the implementation of QPInverseKinematics bindings --- bindings/python/IK/src/IntegrationBasedIK.cpp | 29 ++++++++++++++++++- .../python/IK/src/QPInverseKinematics.cpp | 20 +------------ .../IK/tests/test_QP_inverse_kinematics.py | 6 ++-- 3 files changed, 32 insertions(+), 23 deletions(-) diff --git a/bindings/python/IK/src/IntegrationBasedIK.cpp b/bindings/python/IK/src/IntegrationBasedIK.cpp index 2a9ebec3ba..ece0e3e10d 100644 --- a/bindings/python/IK/src/IntegrationBasedIK.cpp +++ b/bindings/python/IK/src/IntegrationBasedIK.cpp @@ -10,6 +10,7 @@ #include #include +#include #include #include @@ -33,7 +34,33 @@ void CreateIntegrationBasedIK(pybind11::module& module) py::class_>(module, "IntegrationBasedIKStateSource"); - py::class_>(module, "IntegrationBasedIK"); + py::class_, + Source>(module, "ILinearTaskSolverIK") + .def("add_task", + &ILinearTaskSolver::addTask, + py::arg("task"), + py::arg("task_name"), + py::arg("priority"), + py::arg("weight") = Eigen::VectorXd()) + .def("get_task_names", + &ILinearTaskSolver::getTaskNames) + .def("finalize", + &ILinearTaskSolver::finalize, + py::arg("handler")) + .def("advance", &ILinearTaskSolver::advance) + .def("get_output", &ILinearTaskSolver::getOutput) + .def("is_output_valid", + &ILinearTaskSolver::isOutputValid) + .def( + "initialize", + [](ILinearTaskSolver& impl, + std::shared_ptr + handler) -> bool { return impl.initialize(handler); }, + py::arg("handler")); + + py::class_>(module, + "IntegrationBasedIK"); } } // namespace IK diff --git a/bindings/python/IK/src/QPInverseKinematics.cpp b/bindings/python/IK/src/QPInverseKinematics.cpp index 411bd71bb6..96db3d0365 100644 --- 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 #include #include @@ -27,24 +26,7 @@ void CreateQPInverseKinematics(pybind11::module& module) using namespace BipedalLocomotion::IK; py::class_(module, "QPInverseKinematics") - .def(py::init()) - .def( - "initialize", - [](QPInverseKinematics& impl, - std::shared_ptr - 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/tests/test_QP_inverse_kinematics.py b/bindings/python/IK/tests/test_QP_inverse_kinematics.py index 8cd4d66264..7b57b75e77 100644 --- 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() From fa493c16a804de59cb16998e9aaf3d36b06dffbe Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 20 Oct 2021 02:00:49 +0200 Subject: [PATCH 07/13] Implement the bindings for TSID --- bindings/python/CMakeLists.txt | 1 + bindings/python/TSID/CMakeLists.txt | 20 ++++++ .../bindings/TSID/BaseDynamicsTask.h | 26 +++++++ .../BipedalLocomotion/bindings/TSID/CoMTask.h | 26 +++++++ .../bindings/TSID/FeasibleContactWrenchTask.h | 26 +++++++ .../bindings/TSID/JointDynamicsTask.h | 26 +++++++ .../bindings/TSID/JointTrackingTask.h | 26 +++++++ .../BipedalLocomotion/bindings/TSID/Module.h | 26 +++++++ .../BipedalLocomotion/bindings/TSID/QPTSID.h | 28 ++++++++ .../BipedalLocomotion/bindings/TSID/SE3Task.h | 26 +++++++ .../BipedalLocomotion/bindings/TSID/SO3Task.h | 26 +++++++ .../bindings/TSID/TSIDLinearTask.h | 26 +++++++ .../bindings/TSID/TaskSpaceInverseDynamics.h | 26 +++++++ bindings/python/TSID/src/BaseDynamicsTask.cpp | 37 ++++++++++ bindings/python/TSID/src/CoMTask.cpp | 40 +++++++++++ .../TSID/src/FeasibleContactWrenchTask.cpp | 40 +++++++++++ .../python/TSID/src/JointDynamicsTask.cpp | 37 ++++++++++ .../python/TSID/src/JointTrackingTask.cpp | 55 +++++++++++++++ bindings/python/TSID/src/Module.cpp | 45 ++++++++++++ bindings/python/TSID/src/QPTSID.cpp | 44 ++++++++++++ bindings/python/TSID/src/SE3Task.cpp | 40 +++++++++++ bindings/python/TSID/src/SO3Task.cpp | 40 +++++++++++ bindings/python/TSID/src/TSIDLinearTask.cpp | 35 ++++++++++ .../TSID/src/TaskSpaceInverseDynamics.cpp | 68 +++++++++++++++++++ .../bipedal_locomotion_framework.cpp.in | 9 +++ 25 files changed, 799 insertions(+) create mode 100644 bindings/python/TSID/CMakeLists.txt create mode 100644 bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/BaseDynamicsTask.h create mode 100644 bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/CoMTask.h create mode 100644 bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/FeasibleContactWrenchTask.h create mode 100644 bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/JointDynamicsTask.h create mode 100644 bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/JointTrackingTask.h create mode 100644 bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/Module.h create mode 100644 bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/QPTSID.h create mode 100644 bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/SE3Task.h create mode 100644 bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/SO3Task.h create mode 100644 bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/TSIDLinearTask.h create mode 100644 bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/TaskSpaceInverseDynamics.h create mode 100644 bindings/python/TSID/src/BaseDynamicsTask.cpp create mode 100644 bindings/python/TSID/src/CoMTask.cpp create mode 100644 bindings/python/TSID/src/FeasibleContactWrenchTask.cpp create mode 100644 bindings/python/TSID/src/JointDynamicsTask.cpp create mode 100644 bindings/python/TSID/src/JointTrackingTask.cpp create mode 100644 bindings/python/TSID/src/Module.cpp create mode 100644 bindings/python/TSID/src/QPTSID.cpp create mode 100644 bindings/python/TSID/src/SE3Task.cpp create mode 100644 bindings/python/TSID/src/SO3Task.cpp create mode 100644 bindings/python/TSID/src/TSIDLinearTask.cpp create mode 100644 bindings/python/TSID/src/TaskSpaceInverseDynamics.cpp diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 8c66b3b850..9c4a078777 100644 --- 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/TSID/CMakeLists.txt b/bindings/python/TSID/CMakeLists.txt new file mode 100644 index 0000000000..6a06728284 --- /dev/null +++ b/bindings/python/TSID/CMakeLists.txt @@ -0,0 +1,20 @@ +# 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. + + # src/IntegrationBasedTSID.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/Module.cpp src/TSIDLinearTask.cpp + +# ${H_PREFIX}/IntegrationBasedTSID.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/TSIDLinearTask.h + +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 + ) + +endif() diff --git a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/BaseDynamicsTask.h b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/BaseDynamicsTask.h new file mode 100644 index 0000000000..10936cc191 --- /dev/null +++ b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/BaseDynamicsTask.h @@ -0,0 +1,26 @@ +/** + * @file BaseDynamicsTask.h + * @authors 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_BINDINGS_TSID_BASE_DYNAMICS_TASK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_TSID_BASE_DYNAMICS_TASK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateBaseDynamicsTask(pybind11::module& module); + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_TSID_BASE_DYNAMICS_TASK_H diff --git a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/CoMTask.h b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/CoMTask.h new file mode 100644 index 0000000000..5950373e04 --- /dev/null +++ b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/CoMTask.h @@ -0,0 +1,26 @@ +/** + * @file CoMTask.h + * @authors 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_BINDINGS_TSID_COM_TASK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_TSID_COM_TASK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateCoMTask(pybind11::module& module); + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_TSID_COM_TASK_H diff --git a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/FeasibleContactWrenchTask.h b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/FeasibleContactWrenchTask.h new file mode 100644 index 0000000000..d6016e22e5 --- /dev/null +++ b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/FeasibleContactWrenchTask.h @@ -0,0 +1,26 @@ +/** + * @file FeasibleContactWrenchTask.h + * @authors 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_BINDINGS_TSID_FEASIBLE_CONTACT_WRENCH_TASK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_TSID_FEASIBLE_CONTACT_WRENCH_TASK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateFeasibleContactWrenchTask(pybind11::module& module); + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_TSID_FEASIBLE_CONTACT_WRENCH_TASK_H diff --git a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/JointDynamicsTask.h b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/JointDynamicsTask.h new file mode 100644 index 0000000000..adb4158627 --- /dev/null +++ b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/JointDynamicsTask.h @@ -0,0 +1,26 @@ +/** + * @file JointDynamicsTask.h + * @authors 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_BINDINGS_TSID_JOINT_DYNAMICS_TASK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_TSID_JOINT_DYNAMICS_TASK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateJointDynamicsTask(pybind11::module& module); + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_TSID_JOINT_DYNAMICS_TASK_H diff --git a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/JointTrackingTask.h b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/JointTrackingTask.h new file mode 100644 index 0000000000..df624a71b1 --- /dev/null +++ b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/JointTrackingTask.h @@ -0,0 +1,26 @@ +/** + * @file JointTrackingTask.h + * @authors 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_BINDINGS_TSID_JOINT_TRACKING_TASK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_TSID_JOINT_TRACKING_TASK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateJointTrackingTask(pybind11::module& module); + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_TSID_JOINT_TRACKING_TASK_H diff --git a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/Module.h b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/Module.h new file mode 100644 index 0000000000..cd771b39a0 --- /dev/null +++ b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/Module.h @@ -0,0 +1,26 @@ +/** + * @file Module.h + * @authors 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_BINDINGS_TSID_MODULE_H +#define BIPEDAL_LOCOMOTION_BINDINGS_TSID_MODULE_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateModule(pybind11::module& module); + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_TSID_MODULE_H diff --git a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/QPTSID.h b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/QPTSID.h new file mode 100644 index 0000000000..1072420f47 --- /dev/null +++ b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/QPTSID.h @@ -0,0 +1,28 @@ +/** + * @file QPTSID.h + * @authors 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_BINDINGS_TSID_QP_TSID_H +#define BIPEDAL_LOCOMOTION_BINDINGS_TSID_QP_TSID_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateQPTSID(pybind11::module& module); + +void CreateQPFixedBaseTSID(pybind11::module& module); + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_TSID_QP_TSID_H diff --git a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/SE3Task.h b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/SE3Task.h new file mode 100644 index 0000000000..b89be3ffce --- /dev/null +++ b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/SE3Task.h @@ -0,0 +1,26 @@ +/** + * @file SE3Task.h + * @authors 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_BINDINGS_TSID_SE3_TASK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_TSID_SE3_TASK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateSE3Task(pybind11::module& module); + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_TSID_SE3_TASK_H diff --git a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/SO3Task.h b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/SO3Task.h new file mode 100644 index 0000000000..209ddd7800 --- /dev/null +++ b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/SO3Task.h @@ -0,0 +1,26 @@ +/** + * @file SO3Task.h + * @authors 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_BINDINGS_TSID_SO3_TASK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_TSID_SO3_TASK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateSO3Task(pybind11::module& module); + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_TSID_SO3_TASK_H diff --git a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/TSIDLinearTask.h b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/TSIDLinearTask.h new file mode 100644 index 0000000000..3a304fb8bb --- /dev/null +++ b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/TSIDLinearTask.h @@ -0,0 +1,26 @@ +/** + * @file TSIDLinearTask.h + * @authors 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_BINDINGS_TSID_LINEAR_TASK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_TSID_LINEAR_TASK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateTSIDLinearTask(pybind11::module& module); + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_TSID_LINEAR_TASK_H diff --git a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/TaskSpaceInverseDynamics.h b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/TaskSpaceInverseDynamics.h new file mode 100644 index 0000000000..ef13df7344 --- /dev/null +++ b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/TaskSpaceInverseDynamics.h @@ -0,0 +1,26 @@ +/** + * @file TaskSpaceInverseDynamics.h + * @authors 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_BINDINGS_TSID_TASK_SPACE_INVERSE_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_BINDINGS_TSID_TASK_SPACE_INVERSE_DYNAMICS_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateTaskSpaceInverseDynamics(pybind11::module& module); + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_TSID_TASK_SPACE_INVERSE_DYNAMICS_H diff --git a/bindings/python/TSID/src/BaseDynamicsTask.cpp b/bindings/python/TSID/src/BaseDynamicsTask.cpp new file mode 100644 index 0000000000..56be268f6e --- /dev/null +++ b/bindings/python/TSID/src/BaseDynamicsTask.cpp @@ -0,0 +1,37 @@ +/** + * @file BaseDynamicsTask.cpp + * @authors 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. + */ + +#include +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateBaseDynamicsTask(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::TSID; + + py::class_, // + TSIDLinearTask>(module, "BaseDynamicsTask") + .def(py::init()) + .def("set_kin_dyn", &BaseDynamicsTask::setKinDyn, py::arg("kin_dyn")); +} + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/TSID/src/CoMTask.cpp b/bindings/python/TSID/src/CoMTask.cpp new file mode 100644 index 0000000000..ab9d02107a --- /dev/null +++ b/bindings/python/TSID/src/CoMTask.cpp @@ -0,0 +1,40 @@ +/** + * @file CoMTask.cpp + * @authors Paolo Maria Viceconte + * @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. + */ + +#include +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateCoMTask(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::TSID; + + py::class_, TSIDLinearTask>(module, "CoMTask") + .def(py::init()) + .def("set_kin_dyn", &CoMTask::setKinDyn, py::arg("kin_dyn")) + .def("set_set_point", + &CoMTask::setSetPoint, + py::arg("position"), + py::arg("velocity"), + py::arg("acceleration")); +} + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/TSID/src/FeasibleContactWrenchTask.cpp b/bindings/python/TSID/src/FeasibleContactWrenchTask.cpp new file mode 100644 index 0000000000..73f93ad617 --- /dev/null +++ b/bindings/python/TSID/src/FeasibleContactWrenchTask.cpp @@ -0,0 +1,40 @@ +/** + * @file BaseDynamicsTask.cpp + * @authors Paolo Maria Viceconte + * @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. + */ + +#include +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateFeasibleContactWrenchTask(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::TSID; + + py::class_, // + TSIDLinearTask>(module, "FeasibleContactWrenchTask") + .def(py::init()) + .def("set_kin_dyn", &FeasibleContactWrenchTask::setKinDyn, py::arg("kin_dyn")) + .def("set_contact_active", + &FeasibleContactWrenchTask::setContactActive, + py::arg("is_active")); +} + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/TSID/src/JointDynamicsTask.cpp b/bindings/python/TSID/src/JointDynamicsTask.cpp new file mode 100644 index 0000000000..c6fde4c829 --- /dev/null +++ b/bindings/python/TSID/src/JointDynamicsTask.cpp @@ -0,0 +1,37 @@ +/** + * @file JointDynamicsTask.cpp + * @authors 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. + */ + +#include +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateJointDynamicsTask(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::TSID; + + py::class_, // + TSIDLinearTask>(module, "JointDynamicsTask") + .def(py::init()) + .def("set_kin_dyn", &JointDynamicsTask::setKinDyn, py::arg("kin_dyn")); +} + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/TSID/src/JointTrackingTask.cpp b/bindings/python/TSID/src/JointTrackingTask.cpp new file mode 100644 index 0000000000..601d413595 --- /dev/null +++ b/bindings/python/TSID/src/JointTrackingTask.cpp @@ -0,0 +1,55 @@ +/** + * @file JointTrackingTask.cpp + * @authors 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. + */ + +#include +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateJointTrackingTask(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::TSID; + + py::class_, TSIDLinearTask>( // + module, + "JointTrackingTask") + .def(py::init()) + .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>(&JointTrackingTask::setSetPoint), + py::arg("joint_position")) + .def("set_set_point", + py::overload_cast, Eigen::Ref>( + &JointTrackingTask::setSetPoint), + py::arg("joint_position"), + py::arg("joint_velocity")) + .def("set_set_point", + py::overload_cast, + Eigen::Ref, + Eigen::Ref>(&JointTrackingTask::setSetPoint), + py::arg("joint_position"), + py::arg("joint_velocity"), + py::arg("joint_acceleration")); +} + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/TSID/src/Module.cpp b/bindings/python/TSID/src/Module.cpp new file mode 100644 index 0000000000..7e0aa995ca --- /dev/null +++ b/bindings/python/TSID/src/Module.cpp @@ -0,0 +1,45 @@ +/** + * @file Module.cpp + * @authors 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. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ +void CreateModule(pybind11::module& module) +{ + module.doc() = "TSID module."; + + CreateTSIDLinearTask(module); + CreateCoMTask(module); + CreateSE3Task(module); + CreateSO3Task(module); + CreateJointTrackingTask(module); + CreateBaseDynamicsTask(module); + CreateFeasibleContactWrenchTask(module); + CreateTaskSpaceInverseDynamics(module); + CreateQPTSID(module); + CreateQPFixedBaseTSID(module); +} +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/TSID/src/QPTSID.cpp b/bindings/python/TSID/src/QPTSID.cpp new file mode 100644 index 0000000000..c48cbd225d --- /dev/null +++ b/bindings/python/TSID/src/QPTSID.cpp @@ -0,0 +1,44 @@ +/** + * @file QPInverseKinematics.cpp + * @authors 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. + */ + +#include +#include +#include + +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateQPTSID(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::TSID; + + py::class_(module, "QPTSID").def(py::init()); +} + +void CreateQPFixedBaseTSID(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::TSID; + + py::class_(module, "QPFixedBaseTSID") + .def(py::init()) + .def("set_kin_dyn", &QPFixedBaseTSID::setKinDyn, py::arg("kin_dyn")); +} + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/TSID/src/SE3Task.cpp b/bindings/python/TSID/src/SE3Task.cpp new file mode 100644 index 0000000000..33bcad7339 --- /dev/null +++ b/bindings/python/TSID/src/SE3Task.cpp @@ -0,0 +1,40 @@ +/** + * @file SE3Task.cpp + * @authors Paolo Maria Viceconte + * @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. + */ + +#include +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateSE3Task(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::TSID; + + py::class_, TSIDLinearTask>(module, "SE3Task") + .def(py::init()) + .def("set_kin_dyn", &SE3Task::setKinDyn, py::arg("kin_dyn")) + .def("set_set_point", + &SE3Task::setSetPoint, + py::arg("I_H_F"), + py::arg("mixed_velocity"), + py::arg("mixed_acceleration")); +} + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/TSID/src/SO3Task.cpp b/bindings/python/TSID/src/SO3Task.cpp new file mode 100644 index 0000000000..836f7fb7ed --- /dev/null +++ b/bindings/python/TSID/src/SO3Task.cpp @@ -0,0 +1,40 @@ +/** + * @file SO3Task.cpp + * @authors 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. + */ + +#include +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateSO3Task(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::TSID; + + py::class_, TSIDLinearTask>(module, "SO3Task") + .def(py::init()) + .def("set_kin_dyn", &SO3Task::setKinDyn, py::arg("kin_dyn")) + .def("set_set_point", + &SO3Task::setSetPoint, + py::arg("I_R_F"), + py::arg("angular_velocity"), + py::arg("angular_acceleration")); +} + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/TSID/src/TSIDLinearTask.cpp b/bindings/python/TSID/src/TSIDLinearTask.cpp new file mode 100644 index 0000000000..aa53a81fef --- /dev/null +++ b/bindings/python/TSID/src/TSIDLinearTask.cpp @@ -0,0 +1,35 @@ +/** + * @file SE3Task.cpp + * @authors Paolo Maria Viceconte + * @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. + */ + +#include +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateTSIDLinearTask(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::TSID; + using namespace BipedalLocomotion::System; + + py::class_, LinearTask>(module, + "TSIDLinearTask"); +} + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/TSID/src/TaskSpaceInverseDynamics.cpp b/bindings/python/TSID/src/TaskSpaceInverseDynamics.cpp new file mode 100644 index 0000000000..42fac6beee --- /dev/null +++ b/bindings/python/TSID/src/TaskSpaceInverseDynamics.cpp @@ -0,0 +1,68 @@ +/** + * @file TaskSpaceInverseDynamics.cpp + * @authors 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. + */ + +#include +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace TSID +{ + +void CreateTaskSpaceInverseDynamics(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::TSID; + using namespace BipedalLocomotion::System; + + py::class_(module, "TSIDState") + .def(py::init()) + .def_readwrite("base_acceleration", &TSIDState::baseAcceleration) + .def_readwrite("joint_accelerations", &TSIDState::jointAccelerations) + .def_readwrite("joint_torques", &TSIDState::jointTorques) + .def_readwrite("contact_wrenches", &TSIDState::contactWrenches); + + py::class_>(module, "TSIDStateSource"); + + py::class_, + Source>(module, "ILinearTaskSolverTSID") + .def("add_task", + &ILinearTaskSolver::addTask, + py::arg("task"), + py::arg("task_name"), + py::arg("priority"), + py::arg("weight") = Eigen::VectorXd()) + .def("get_task_names", + &ILinearTaskSolver::getTaskNames) + .def("finalize", + &ILinearTaskSolver::finalize, + py::arg("handler")) + .def("advance", &ILinearTaskSolver::advance) + .def("get_output", &ILinearTaskSolver::getOutput) + .def("is_output_valid", + &ILinearTaskSolver::isOutputValid) + .def( + "initialize", + [](ILinearTaskSolver& impl, + std::shared_ptr + handler) -> bool { return impl.initialize(handler); }, + py::arg("handler")); + + py::class_>(module, "TaskSpaceInverseDynamics"); +} + +} // namespace TSID +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/bipedal_locomotion_framework.cpp.in b/bindings/python/bipedal_locomotion_framework.cpp.in index e1d8133bf8..be0b7300b3 100644 --- a/bindings/python/bipedal_locomotion_framework.cpp.in +++ b/bindings/python/bipedal_locomotion_framework.cpp.in @@ -40,6 +40,10 @@ #include @endcmakeif FRAMEWORK_COMPILE_IK +@cmakeif FRAMEWORK_COMPILE_TSID +#include +@endcmakeif FRAMEWORK_COMPILE_TSID + // Create the Python module PYBIND11_MODULE(bindings, m) @@ -95,4 +99,9 @@ PYBIND11_MODULE(bindings, m) py::module ikModule = m.def_submodule("ik"); bindings::IK::CreateModule(ikModule); @endcmakeif FRAMEWORK_COMPILE_IK + + @cmakeif FRAMEWORK_COMPILE_TSID + py::module tsidModule = m.def_submodule("tsid"); + bindings::TSID::CreateModule(tsidModule); + @endcmakeif FRAMEWORK_COMPILE_TSID } From c58026c1df6f9dae1e816d8857b1ef4c81c25428 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 20 Oct 2021 14:08:17 +0200 Subject: [PATCH 08/13] Fix missing include in LinearTask python bindings --- bindings/python/System/src/LinearTask.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/bindings/python/System/src/LinearTask.cpp b/bindings/python/System/src/LinearTask.cpp index a156c7676e..adced219e6 100644 --- a/bindings/python/System/src/LinearTask.cpp +++ b/bindings/python/System/src/LinearTask.cpp @@ -6,6 +6,7 @@ */ #include +#include #include #include From 951dbad3a5d0a11fe64f801a489197e470048a4a Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 20 Oct 2021 14:08:51 +0200 Subject: [PATCH 09/13] Add joint dynamics task in the TSID bindings --- bindings/python/TSID/src/Module.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/bindings/python/TSID/src/Module.cpp b/bindings/python/TSID/src/Module.cpp index 7e0aa995ca..1b21991318 100644 --- a/bindings/python/TSID/src/Module.cpp +++ b/bindings/python/TSID/src/Module.cpp @@ -35,6 +35,7 @@ void CreateModule(pybind11::module& module) CreateSO3Task(module); CreateJointTrackingTask(module); CreateBaseDynamicsTask(module); + CreateJointDynamicsTask(module); CreateFeasibleContactWrenchTask(module); CreateTaskSpaceInverseDynamics(module); CreateQPTSID(module); From de0c3cc7f261a1502b4f4e0db540335ad3f1df04 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 20 Oct 2021 14:10:26 +0200 Subject: [PATCH 10/13] Fix FeasibleContactWrenchTaskTest.cpp --- src/TSID/tests/FeasibleContactWrenchTaskTest.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/TSID/tests/FeasibleContactWrenchTaskTest.cpp b/src/TSID/tests/FeasibleContactWrenchTaskTest.cpp index ad82fdccbe..f618cb5eab 100644 --- a/src/TSID/tests/FeasibleContactWrenchTaskTest.cpp +++ b/src/TSID/tests/FeasibleContactWrenchTaskTest.cpp @@ -25,11 +25,8 @@ using namespace BipedalLocomotion::System; using namespace BipedalLocomotion::TSID; using namespace BipedalLocomotion::Math; -TEST_CASE("Joints and Base dynamics tasks") +TEST_CASE("Fesasible contact wrench Test") { - const std::string robotAcceleration = "robotAcceleration"; - const std::string jointsTorque = "jointsTorque"; - auto kinDyn = std::make_shared(); auto parameterHandler = std::make_shared(); From 5fc97b181ee2ad097a6ba9819dd5c5be59b0b7da Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 20 Oct 2021 14:10:58 +0200 Subject: [PATCH 11/13] Add the tsid python bindings test --- bindings/python/TSID/CMakeLists.txt | 5 +- bindings/python/TSID/tests/test_TSID.py | 214 ++++++++++++++++++++++++ 2 files changed, 215 insertions(+), 4 deletions(-) create mode 100644 bindings/python/TSID/tests/test_TSID.py diff --git a/bindings/python/TSID/CMakeLists.txt b/bindings/python/TSID/CMakeLists.txt index 6a06728284..989e8360c7 100644 --- a/bindings/python/TSID/CMakeLists.txt +++ b/bindings/python/TSID/CMakeLists.txt @@ -2,10 +2,6 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. - # src/IntegrationBasedTSID.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/Module.cpp src/TSIDLinearTask.cpp - -# ${H_PREFIX}/IntegrationBasedTSID.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/TSIDLinearTask.h - if(FRAMEWORK_COMPILE_TSID) set(H_PREFIX include/BipedalLocomotion/bindings/TSID) @@ -15,6 +11,7 @@ if(FRAMEWORK_COMPILE_TSID) 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() diff --git a/bindings/python/TSID/tests/test_TSID.py b/bindings/python/TSID/tests/test_TSID.py new file mode 100644 index 0000000000..fc0894ad90 --- /dev/null +++ b/bindings/python/TSID/tests/test_TSID.py @@ -0,0 +1,214 @@ +import pytest +pytestmark = pytest.mark.tsid + +import bipedal_locomotion_framework.bindings as blf +import manifpy as manif +import numpy as np +import tempfile +import urllib.request + +def get_kindyn(): + + # retrieve the model + model_url = 'https://raw.githubusercontent.com/robotology/icub-models/master/iCub/robots/iCubGazeboV2_5/model.urdf' + model = urllib.request.urlopen(model_url) + temp = tempfile.NamedTemporaryFile() + temp.write(model.read()) + + # create KinDynComputationsDescriptor + kindyn_handler = blf.parameters_handler.StdParametersHandler() + kindyn_handler.set_parameter_string("model_file_name", temp.name) + joints_list = ["neck_pitch", "neck_roll", "neck_yaw", + "torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw","l_elbow", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw","r_elbow", + "l_hip_pitch", "l_hip_roll", "l_hip_yaw","l_knee", "l_ankle_pitch", "l_ankle_roll", + "r_hip_pitch", "r_hip_roll", "r_hip_yaw","r_knee", "r_ankle_pitch", "r_ankle_roll"] + kindyn_handler.set_parameter_vector_string("joints_list", joints_list) + kindyn_desc = blf.floating_base_estimators.construct_kindyncomputations_descriptor(kindyn_handler) + assert kindyn_desc.is_valid() + + return joints_list, kindyn_desc + +def test_com_task(): + + kp = 1.0 + kd = 0.5 + + # get kindyn + joints_list, kindyn_desc = get_kindyn() + + # Set the parameters + com_param_handler = blf.parameters_handler.StdParametersHandler() + com_param_handler.set_parameter_string(name="robot_acceleration_variable_name", \ + value="robotAcceleration") + com_param_handler.set_parameter_float(name="kp_linear", value=kp) + com_param_handler.set_parameter_float(name="kd_linear", value=kd) + + # Initialize the task + com_task = blf.tsid.CoMTask() + assert com_task.set_kin_dyn(kindyn_desc.kindyn) + assert com_task.initialize(param_handler=com_param_handler) + + com_var_handler = blf.system.VariablesHandler() + + # robot velocity size = 26 (joints) + 6 (base) + assert com_var_handler.add_variable("robotAcceleration", len(joints_list) + 6) is True + assert com_task.set_variables_handler(variables_handler=com_var_handler) + assert com_task.set_set_point(position=np.array([1.,1.,1.5]), + velocity=np.array([0.,0.5,0.5]), + acceleration=np.array([0.,-0.1,0.1])) + +def test_se3_task(): + + # get kindyn + joints_list, kindyn_desc = get_kindyn() + + # Set the parameters + se3_param_handler = blf.parameters_handler.StdParametersHandler() + se3_param_handler.set_parameter_string(name="robot_acceleration_variable_name", value="robotAcceleration") + se3_param_handler.set_parameter_string(name="frame_name", value="r_sole") + se3_param_handler.set_parameter_float(name="kp_linear", value=10.0) + se3_param_handler.set_parameter_float(name="kp_angular", value=10.0) + se3_param_handler.set_parameter_float(name="kd_linear", value=5.0) + se3_param_handler.set_parameter_float(name="kd_angular", value=5.0) + + # Initialize the task + se3_task = blf.tsid.SE3Task() + assert se3_task.set_kin_dyn(kindyn_desc.kindyn) + assert se3_task.initialize(param_handler=se3_param_handler) + se3_var_handler = blf.system.VariablesHandler() + assert se3_var_handler.add_variable("robotAcceleration", len(joints_list) + 6) is True + assert se3_task.set_variables_handler(variables_handler=se3_var_handler) + + # Set desiderata + I_H_F = manif.SE3(position=[1.0, -2.0, 3.3], quaternion=[0, 0, -1, 0]) + mixed_velocity = manif.SE3Tangent([0.0]*6) # TODO: proper assignment + mixed_acceleration = manif.SE3Tangent([0.0]*6) # TODO: proper assignment + assert se3_task.set_set_point(I_H_F=I_H_F, + mixed_velocity=mixed_velocity, + mixed_acceleration=mixed_acceleration) + +def test_so3_task(): + + # get kindyn + joints_list, kindyn_desc = get_kindyn() + + # Set the parameters + so3_param_handler = blf.parameters_handler.StdParametersHandler() + so3_param_handler.set_parameter_string(name="robot_acceleration_variable_name", value="robotAcceleration") + so3_param_handler.set_parameter_string(name="frame_name", value="chest") + so3_param_handler.set_parameter_float(name="kp_angular", value=5.0) + so3_param_handler.set_parameter_float(name="kd_angular", value=1.0) + + # Initialize the task + so3_task = blf.tsid.SO3Task() + assert so3_task.set_kin_dyn(kindyn_desc.kindyn) + assert so3_task.initialize(param_handler=so3_param_handler) + so3_var_handler = blf.system.VariablesHandler() + assert so3_var_handler.add_variable("robotAcceleration", len(joints_list) + 6) is True + assert so3_task.set_variables_handler(variables_handler=so3_var_handler) + + # Set desiderata + I_R_F = manif.SO3(quaternion=[0, 0, -1, 0]) + angular_velocity = manif.SO3Tangent([0.0]*3) # TODO: proper assignment + angular_acceleration = manif.SO3Tangent([0.0]*3) # TODO: proper assignment + assert so3_task.set_set_point(I_R_F=I_R_F, + angular_velocity=angular_velocity, + angular_acceleration=angular_acceleration) + +def test_joint_tracking_task(): + + # get kindyn + joints_list, kindyn_desc = get_kindyn() + + # Set the parameters + joint_tracking_param_handler = blf.parameters_handler.StdParametersHandler() + joint_tracking_param_handler.set_parameter_string(name="robot_acceleration_variable_name", value="robotAcceleration") + joint_tracking_param_handler.set_parameter_vector_float(name="kp",value=[5.0]*kindyn_desc.kindyn.get_nr_of_dofs()) + joint_tracking_param_handler.set_parameter_vector_float(name="kd",value=[1.0]*kindyn_desc.kindyn.get_nr_of_dofs()) + + # Initialize the task + joint_tracking_task = blf.tsid.JointTrackingTask() + assert joint_tracking_task.set_kin_dyn(kindyn_desc.kindyn) + assert joint_tracking_task.initialize(param_handler=joint_tracking_param_handler) + joint_tracking_var_handler = blf.system.VariablesHandler() + + # robot velocity size = 26 (joints) + 6 (base) + assert joint_tracking_var_handler.add_variable("robotAcceleration", len(joints_list) + 6) is True + assert joint_tracking_task.set_variables_handler(variables_handler=joint_tracking_var_handler) + + # Set desired joint pos + joint_values = [np.random.uniform(-0.5,0.5) for _ in range(kindyn_desc.kindyn.get_nr_of_dofs())] + assert joint_tracking_task.set_set_point(joint_position=joint_values) + + # Set desired joint pos and vel + joint_values = [np.random.uniform(-0.5,0.5) for _ in range(kindyn_desc.kindyn.get_nr_of_dofs())] + joint_velocities = [np.random.uniform(-0.5,0.5) for _ in range(kindyn_desc.kindyn.get_nr_of_dofs())] + assert joint_tracking_task.set_set_point(joint_position=joint_values,joint_velocity=joint_velocities) + +def test_feasible_contact_wrench(): + + # get kindyn + joints_list, kindyn_desc = get_kindyn() + + # Set the parameters + feasible_contact_wrench_param_handler = blf.parameters_handler.StdParametersHandler() + feasible_contact_wrench_param_handler.set_parameter_int(name="number_of_slices", value=2) + feasible_contact_wrench_param_handler.set_parameter_float(name="static_friction_coefficient", + value=0.3) + feasible_contact_wrench_param_handler.set_parameter_vector_float(name="foot_limits_x", + value=[-0.05, 0.1]) + feasible_contact_wrench_param_handler.set_parameter_vector_float(name="foot_limits_y", + value=[-0.02, 0.01]) + feasible_contact_wrench_param_handler.set_parameter_string(name="variable_name", + value="left_foot_contact") + feasible_contact_wrench_param_handler.set_parameter_string(name="frame_name", + value="l_sole") + + # Initialize the task + feasible_contact_wrench_task = blf.tsid.FeasibleContactWrenchTask() + assert feasible_contact_wrench_task.set_kin_dyn(kindyn_desc.kindyn) + assert feasible_contact_wrench_task.initialize(param_handler=feasible_contact_wrench_param_handler) + feasible_contact_wrench_var_handler = blf.system.VariablesHandler() + + assert feasible_contact_wrench_var_handler.add_variable("left_foot_contact", 6) is True + assert feasible_contact_wrench_task.set_variables_handler(variables_handler=feasible_contact_wrench_var_handler) + +def test_dynamics(): + + # get kindyn + joints_list, kindyn_desc = get_kindyn() + + # Set the parameters + param_handler = blf.parameters_handler.StdParametersHandler() + param_handler.set_parameter_string(name="robot_acceleration_variable_name", + value="robotAcceleration") + param_handler.set_parameter_string(name="joint_torques_variable_name", + value="jointsTorque") + param_handler.set_parameter_int(name="max_number_of_contacts", value=1) + + contact_group = blf.parameters_handler.StdParametersHandler() + contact_group.set_parameter_string(name="variable_name", + value="left_foot_contact") + contact_group.set_parameter_string(name="frame_name", + value="l_sole") + assert param_handler.set_group("CONTACT_0", contact_group) + + var_handler = blf.system.VariablesHandler() + var_handler.add_variable("robotAcceleration", len(joints_list) + 6) + var_handler.add_variable("jointsTorque", len(joints_list)) + var_handler.add_variable("left_foot_contact", 6) + + + # Initialize the task + base_dynamics_task = blf.tsid.BaseDynamicsTask() + assert base_dynamics_task.set_kin_dyn(kindyn_desc.kindyn) + assert base_dynamics_task.initialize(param_handler=param_handler) + assert base_dynamics_task.set_variables_handler(variables_handler=var_handler) + + # Initialize the task + joint_dynamics_task = blf.tsid.JointDynamicsTask() + assert joint_dynamics_task.set_kin_dyn(kindyn_desc.kindyn) + assert joint_dynamics_task.initialize(param_handler=param_handler) + assert joint_dynamics_task.set_variables_handler(variables_handler=var_handler) From 6b259fd5f010bb5435f3be2a392adf29d54e523b Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 20 Oct 2021 14:12:11 +0200 Subject: [PATCH 12/13] Update the CHANGELOG --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 48fdc5954c..2cf80e89f4 100644 --- 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 From 2e4cc44532d1f587b44106da4a07ae248e1704b6 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 20 Oct 2021 18:09:28 +0200 Subject: [PATCH 13/13] Fix test_TSID.py on Windows --- bindings/python/TSID/tests/test_TSID.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/bindings/python/TSID/tests/test_TSID.py b/bindings/python/TSID/tests/test_TSID.py index fc0894ad90..0cac46b760 100644 --- a/bindings/python/TSID/tests/test_TSID.py +++ b/bindings/python/TSID/tests/test_TSID.py @@ -6,14 +6,16 @@ import numpy as np import tempfile import urllib.request +import os def get_kindyn(): # retrieve the model model_url = 'https://raw.githubusercontent.com/robotology/icub-models/master/iCub/robots/iCubGazeboV2_5/model.urdf' model = urllib.request.urlopen(model_url) - temp = tempfile.NamedTemporaryFile() + temp = tempfile.NamedTemporaryFile('wb', delete=False) temp.write(model.read()) + temp.close() # create KinDynComputationsDescriptor kindyn_handler = blf.parameters_handler.StdParametersHandler() @@ -28,6 +30,8 @@ def get_kindyn(): kindyn_desc = blf.floating_base_estimators.construct_kindyncomputations_descriptor(kindyn_handler) assert kindyn_desc.is_valid() + os.unlink(temp.name) + return joints_list, kindyn_desc def test_com_task():