Add the possibility to set the task controller mode for the SE3Task in the TSID component and update the python bindings

CHANGELOG.md

@@ -13,6 +13,8 @@ All notable changes to this project are documented in this file.
 - Implement `ContactWrench` python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/441)
 - Implement AngularMomentum task in the IK component and the associated bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/443)
 - Implement `create_ik` utility function for the python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/444)
+- Add the possibility to set the task controller mode for the SE3Task in the TSID component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/445)
+- Expose the `ITaskControlMode` class in the python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/445)
 
 ### Changed
 - Inherits all the `Eigen::Matrix` constructors in the `Wrenchd` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/441)

bindings/python/IK/src/SE3Task.cpp

@@ -11,6 +11,8 @@
 
 #include <BipedalLocomotion/IK/IKLinearTask.h>
 #include <BipedalLocomotion/IK/SE3Task.h>
+#include <BipedalLocomotion/System/ITaskControllerManager.h>
+
 #include <BipedalLocomotion/bindings/IK/SE3Task.h>
 
 namespace BipedalLocomotion
@@ -25,7 +27,10 @@ void CreateSE3Task(pybind11::module& module)
  namespace py = ::pybind11;
  using namespace BipedalLocomotion::IK;
 
- py::class_<SE3Task, std::shared_ptr<SE3Task>, IKLinearTask>(module, "SE3Task")
+ py::class_<SE3Task,
+ std::shared_ptr<SE3Task>,
+ IKLinearTask,
+ BipedalLocomotion::System::ITaskControllerManager>(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"));

bindings/python/System/CMakeLists.txt

@@ -6,8 +6,8 @@ if(FRAMEWORK_COMPILE_System)
 
  add_bipedal_locomotion_python_module(
  NAME System
- SOURCES src/VariablesHandler.cpp src/LinearTask.cpp src/Module.cpp
- HEADERS include/BipedalLocomotion/bindings/System/VariablesHandler.h include/BipedalLocomotion/bindings/System/LinearTask.h
+ SOURCES src/VariablesHandler.cpp src/LinearTask.cpp src/Module.cpp src/ITaskControllerManager.cpp
+ HEADERS include/BipedalLocomotion/bindings/System/VariablesHandler.h include/BipedalLocomotion/bindings/System/LinearTask.h include/BipedalLocomotion/bindings/System/ITaskControllerManager.h
  LINK_LIBRARIES BipedalLocomotion::System
  TESTS tests/test_variables_handler.py
  )

bindings/python/System/include/BipedalLocomotion/bindings/System/ITaskControllerManager.h

@@ -0,0 +1,26 @@
+/**
+ * @file ITaskControllerManager.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_SYSTEM_I_TASK_CONTROLLER_MANAGER_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_I_TASK_CONTROLLER_MANAGER_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace System
+{
+
+void CreateITaskControllerManager(pybind11::module& module);
+
+} // namespace System
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_I_TASK_CONTROLLER_MANAGER_H

bindings/python/System/src/ITaskControllerManager.cpp

@@ -0,0 +1,46 @@
+/**
+ * @file ITaskControllerManager.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 <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/System/ITaskControllerManager.h>
+#include <BipedalLocomotion/bindings/System/ITaskControllerManager.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace System
+{
+
+void CreateITaskControllerManager(pybind11::module& module)
+{
+ namespace py = ::pybind11;
+
+ using namespace BipedalLocomotion::System;
+
+ py::class_<ITaskControllerManager, std::shared_ptr<ITaskControllerManager>>
+ ITaskControllerManager(module, "ITaskControllerManager");
+ py::enum_<ITaskControllerManager::Mode>(ITaskControllerManager, "ITaskControllerManagerMode")
+ .value("Enable", ITaskControllerManager::Mode::Enable)
+ .value("Disable", ITaskControllerManager::Mode::Disable)
+ .export_values();
+
+ ITaskControllerManager
+ .def("set_task_controller_mode",
+ &ITaskControllerManager::setTaskControllerMode,
+ py::arg("mode"))
+ .def("get_task_controller_mode", &ITaskControllerManager::getTaskControllerMode)
+ .def_property("task_controller_mode",
+ &ITaskControllerManager::getTaskControllerMode,
+ &ITaskControllerManager::setTaskControllerMode);
+}
+
+} // namespace System
+} // namespace bindings
+} // namespace BipedalLocomotion

bindings/python/System/src/Module.cpp

@@ -10,6 +10,7 @@
 #include <BipedalLocomotion/bindings/System/LinearTask.h>
 #include <BipedalLocomotion/bindings/System/Module.h>
 #include <BipedalLocomotion/bindings/System/VariablesHandler.h>
+#include <BipedalLocomotion/bindings/System/ITaskControllerManager.h>
 
 namespace BipedalLocomotion
 {
@@ -23,6 +24,7 @@ void CreateModule(pybind11::module& module)
 
  CreateVariablesHandler(module);
  CreateLinearTask(module);
+ CreateITaskControllerManager(module);
 }
 } // namespace System
 } // namespace bindings

bindings/python/TSID/src/SE3Task.cpp

@@ -9,8 +9,10 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
-#include <BipedalLocomotion/TSID/TSIDLinearTask.h>
+#include <BipedalLocomotion/System/ITaskControllerManager.h>
 #include <BipedalLocomotion/TSID/SE3Task.h>
+#include <BipedalLocomotion/TSID/TSIDLinearTask.h>
+
 #include <BipedalLocomotion/bindings/TSID/SE3Task.h>
 
 namespace BipedalLocomotion
@@ -25,7 +27,8 @@ void CreateSE3Task(pybind11::module& module)
  namespace py = ::pybind11;
  using namespace BipedalLocomotion::TSID;
 
- py::class_<SE3Task, std::shared_ptr<SE3Task>, TSIDLinearTask>(module, "SE3Task")
+ py::class_<SE3Task, std::shared_ptr<SE3Task>, TSIDLinearTask,
+ BipedalLocomotion::System::ITaskControllerManager>(module, "SE3Task")
  .def(py::init())
  .def("set_kin_dyn", &SE3Task::setKinDyn, py::arg("kin_dyn"))
  .def("set_set_point",

src/IK/include/BipedalLocomotion/IK/SE3Task.h

@@ -50,6 +50,10 @@ namespace IK
  */
 class SE3Task : public IKLinearTask, public BipedalLocomotion::System::ITaskControllerManager
 {
+public:
+ using Mode = System::ITaskControllerManager::Mode;
+
+private:
  LieGroupControllers::ProportionalControllerSO3d m_SO3Controller; /**< P Controller in SO(3) */
  LieGroupControllers::ProportionalControllerR3d m_R3Controller; /**< P Controller in R3 */
 
@@ -77,7 +81,7 @@ class SE3Task : public IKLinearTask, public BipedalLocomotion::System::ITaskCont
  Eigen::MatrixXd m_jacobian; /**< Jacobian matrix in MIXED representation */
 
  /** State of the proportional controller implemented in the task */
- System::ITaskControllerManager::Mode m_controllerMode{Mode::Enable};
+ Mode m_controllerMode{Mode::Enable};
 
 public:
  /**

src/TSID/include/BipedalLocomotion/TSID/SE3Task.h

@@ -10,6 +10,7 @@
 
 #include <manif/manif.h>
 
+#include <BipedalLocomotion/System/ITaskControllerManager.h>
 #include <BipedalLocomotion/TSID/TSIDLinearTask.h>
 
 #include <iDynTree/KinDynComputations.h>
@@ -43,8 +44,12 @@ namespace TSID
  * representation is used to define the 6d-velocity. You can find further details in Section 2.3.4
  * of https://traversaro.github.io/phd-thesis/traversaro-phd-thesis.pdf.
  */
-class SE3Task : public TSIDLinearTask
+class SE3Task : public TSIDLinearTask, public BipedalLocomotion::System::ITaskControllerManager
 {
+public:
+ using Mode = System::ITaskControllerManager::Mode;
+
+private:
  LieGroupControllers::ProportionalDerivativeControllerSO3d m_SO3Controller; /**< PD Controller in
  SO(3) */
  LieGroupControllers::ProportionalDerivativeControllerR3d m_R3Controller; /**< PD Controller in
@@ -65,6 +70,9 @@ class SE3Task : public TSIDLinearTask
  std::shared_ptr<iDynTree::KinDynComputations> m_kinDyn; /**< Pointer to a KinDynComputations
  object */
 
+ /** State of the proportional derivative controller implemented in the task */
+ Mode m_controllerMode{Mode::Enable};
+
 public:
  /**
  * Initialize the planner.
@@ -136,6 +144,19 @@ class SE3Task : public TSIDLinearTask
  * @return True if the objects are valid, false otherwise.
  */
  bool isValid() const override;
+
+ /**
+ * Set the task controller mode. Please use this method to disable/enable the Proportional
+ * Derivative controller implemented in this task.
+ * @param state state of the controller
+ */
+ void setTaskControllerMode(Mode mode) override;
+
+ /**
+ * Get the task controller mode.
+ * @return the state of the controller
+ */
+ Mode getTaskControllerMode() const override;
 };
 
 } // namespace TSID

src/TSID/src/SE3Task.cpp

@@ -165,6 +165,13 @@ bool SE3Task::update()
  return m_isValid;
  }
 
+ auto getControllerOutupt = [&](const auto& controller) {
+ if (m_controllerMode == Mode::Enable)
+ return controller.getControl().coeffs();
+ else
+ return controller.getFeedForward().coeffs();
+ };
+
  m_b = -iDynTree::toEigen(m_kinDyn->getFrameBiasAcc(m_frameIndex));
 
  m_SO3Controller.setState(
@@ -176,11 +183,13 @@ bool SE3Task::update()
  {iDynTree::toEigen(m_kinDyn->getWorldTransform(m_frameIndex).getPosition()),
  iDynTree::toEigen(m_kinDyn->getFrameVel(m_frameIndex).getLinearVec3())});
 
+ // update the controller ouptut
  m_SO3Controller.computeControlLaw();
  m_R3Controller.computeControlLaw();
 
- m_b.head<3>() += m_R3Controller.getControl().coeffs();
- m_b.tail<3>() += m_SO3Controller.getControl().coeffs();
+ // get the output
+ m_b.head<3>() += getControllerOutupt(m_R3Controller);
+ m_b.tail<3>() += getControllerOutupt(m_SO3Controller);
 
  if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex,
  this->subA(m_robotAccelerationVariable)))
@@ -221,3 +230,13 @@ bool SE3Task::isValid() const
 {
  return m_isValid;
 }
+
+void SE3Task::setTaskControllerMode(Mode mode)
+{
+ m_controllerMode = mode;
+}
+
+SE3Task::Mode SE3Task::getTaskControllerMode() const
+{
+ return m_controllerMode;
+}