Add the possibility to enable and disable the proportional controller…

… in IK::SE3Task

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

@@ -13,6 +13,7 @@
 #include <manif/manif.h>
 
 #include <BipedalLocomotion/IK/IKLinearTask.h>
+#include <BipedalLocomotion/System/ITaskControllerManager.h>
 
 #include <iDynTree/KinDynComputations.h>
 
@@ -47,7 +48,7 @@ namespace IK
  * @note SE3Task can be used to control also a subset of element of the linear part of the SE3.
  * Please refer to `mask` parameter in IK::SE3Task::initialize method.
  */
-class SE3Task : public IKLinearTask
+class SE3Task : public IKLinearTask, public BipedalLocomotion::System::ITaskControllerManager
 {
  LieGroupControllers::ProportionalControllerSO3d m_SO3Controller; /**< P Controller in SO(3) */
  LieGroupControllers::ProportionalControllerR3d m_R3Controller; /**< P Controller in R3 */
@@ -74,8 +75,10 @@ class SE3Task : public IKLinearTask
  std::size_t m_DoFs{m_spatialVelocitySize}; /**< DoFs associated to the entire task */
 
  Eigen::MatrixXd m_jacobian; /**< Jacobian matrix in MIXED representation */
-public:
 
+ System::ITaskControllerManager::Mode m_controllerMode; /**< State of the proportional controller
+ implemented in the task */
+public:
  /**
  * Initialize the task.
  * @param paramHandler pointer to the parameters handler.
@@ -145,6 +148,19 @@ class SE3Task : public IKLinearTask
  * @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
+ * 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 IK

src/IK/src/SE3Task.cpp

@@ -195,6 +195,13 @@ bool SE3Task::update()
 
  m_isValid = false;
 
+ auto getControllerState = [&](const auto& controller) {
+ if (m_controllerMode == Mode::Enable)
+ return controller.getControl().coeffs();
+ else
+ return controller.getFeedForward().coeffs();
+ };
+
  // set the state
  m_SO3Controller.setState(toManifRot(m_kinDyn->getWorldTransform(m_frameIndex).getRotation()));
  m_R3Controller.setState(toEigen(m_kinDyn->getWorldTransform(m_frameIndex).getPosition()));
@@ -204,12 +211,12 @@ bool SE3Task::update()
  m_R3Controller.computeControlLaw();
 
  // the angular part is always enabled
- m_b.tail<3>() = m_SO3Controller.getControl().coeffs();
+ m_b.tail<3>() = getControllerState(m_SO3Controller);
 
  // if we want to control all 6 DoF we avoid to lose performances
  if (m_linearDoFs == m_linearVelocitySize)
  {
- m_b.head<3>() = m_R3Controller.getControl().coeffs();
+ m_b.head<3>() = getControllerState(m_R3Controller);
 
  if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex,
  this->subA(m_robotVelocityVariable)))
@@ -234,7 +241,7 @@ bool SE3Task::update()
  {
  if (m_mask[i])
  {
- m_b(index) = m_R3Controller.getControl().coeffs()(i);
+ m_b(index) = getControllerState(m_R3Controller)(i);
  iDynTree::toEigen(this->subA(m_robotVelocityVariable)).row(index)
  = m_jacobian.row(i);
  index++;
@@ -278,3 +285,13 @@ bool SE3Task::isValid() const
 {
  return m_isValid;
 }
+
+void SE3Task::setTaskControllerMode(Mode mode)
+{
+ m_controllerMode = mode;
+}
+
+SE3Task::Mode SE3Task::getTaskControllerMode() const
+{
+ return m_controllerMode;
+}