Add the possibility to disable the proportional controller in the IK::SE3Task

.github/workflows/ci.yml

@@ -19,7 +19,7 @@ env:
  CasADi_TAG: 3.5.5.2
  manif_TAG: 0.0.4
  matioCpp_TAG: v0.1.0
- LieGroupControllers_TAG: v0.0.1
+ LieGroupControllers_TAG: v0.1.1
  osqp_TAG: v0.6.2
  OsqpEigen_TAG: v0.6.3
  tomlplusplus_TAG: v2.4.0

CHANGELOG.md

@@ -11,6 +11,7 @@ All notable changes to this project are documented in this file.
 - Implement motor pwm, motor encoders, wbd joint torque estimates, pid reading in `YarpSensorBridge`(https://github.com/dic-iit/bipedal-locomotion-framework/pull/359).
 - Implement FeasibleContactWrenchTask for TSID component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/369).
 - Implement python bindings for QPInverseKinematics class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/303)
+- Implement `ControlTask` in for System component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/373).
 
 ### Changed
 - Add common Python files to gitignore (https://github.com/dic-iit/bipedal-locomotion-framework/pull/338)
@@ -20,6 +21,7 @@ All notable changes to this project are documented in this file.
 - Reduce the duplicate code in IK and TSID (https://github.com/dic-iit/bipedal-locomotion-framework/pull/364)
 - `QPFixedBaseTSID` now inherits from `QPTSID` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/366)
 - Enable the Current control in `RobotInterface` class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/375)
+- Add the possibility to disable and enable the PD controllers in `IK::SE3Task` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/373).
 
 ### Fix
 

cmake/BipedalLocomotionFrameworkDependencies.cmake

@@ -53,9 +53,9 @@ find_package(matioCpp QUIET)
 checkandset_dependency(matioCpp)
 dependency_classifier(matioCpp IS_USED ${FRAMEWORK_USE_matioCpp} PUBLIC)
 
-find_package(LieGroupControllers QUIET)
+find_package(LieGroupControllers 0.1.1 QUIET)
 checkandset_dependency(LieGroupControllers)
-dependency_classifier(LieGroupControllers IS_USED ${FRAMEWORK_USE_LieGroupControllers} PUBLIC)
+dependency_classifier(LieGroupControllers MINIMUM_VERSION 0.1.1 IS_USED ${FRAMEWORK_USE_LieGroupControllers} PUBLIC)
 
 find_package(OpenCV QUIET)
 checkandset_dependency(OpenCV)

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,11 @@ 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:
 
+ /** State of the proportional controller implemented in the task */
+ System::ITaskControllerManager::Mode m_controllerMode{Mode::Enable};
+
+public:
  /**
  * Initialize the task.
  * @param paramHandler pointer to the parameters handler.
@@ -145,6 +149,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;
+}

src/System/CMakeLists.txt

@@ -11,7 +11,7 @@ if(FRAMEWORK_COMPILE_System)
  add_bipedal_locomotion_library(
  NAME System
  PUBLIC_HEADERS ${H_PREFIX}/Advanceable.h ${H_PREFIX}/Source.h ${H_PREFIX}/Sink.h
- ${H_PREFIX}/VariablesHandler.h ${H_PREFIX}/LinearTask.h ${H_PREFIX}/ILinearTaskSolver.h
+ ${H_PREFIX}/VariablesHandler.h ${H_PREFIX}/LinearTask.h ${H_PREFIX}/ILinearTaskSolver.h ${H_PREFIX}/ITaskControllerManager.h
  ${H_PREFIX}/IClock.h ${H_PREFIX}/StdClock.h ${H_PREFIX}/Clock.h
  ${H_PREFIX}/SharedResource.h ${H_PREFIX}/AdvanceableRunner.h
  ${H_PREFIX}/QuitHandler.h

src/System/include/BipedalLocomotion/System/ITaskControllerManager.h

@@ -0,0 +1,47 @@
+/**
+ * @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_SYSTEM_I_TASK_CONTROLLER_MANAGER_H
+#define BIPEDAL_LOCOMOTION_SYSTEM_I_TASK_CONTROLLER_MANAGER_H
+
+namespace BipedalLocomotion
+{
+namespace System
+{
+
+/**
+ * ITaskControllerManager is an interface that can help you to handle tasks containing controllers.
+ * Please inherit it you want to disable or enable controllers embedded in a LinearTask
+ */
+struct ITaskControllerManager
+{
+ /**
+ * Mode representing the status of the task controller.
+ */
+ enum class Mode
+ {
+ Enable,
+ Disable
+ };
+
+ /**
+ * Set the task control mode.
+ * @param state state of the controller
+ */
+ virtual void setTaskControllerMode(Mode mode) = 0;
+
+ /**
+ * Get the task control mode.
+ * @return the state of the controller
+ */
+ virtual Mode getTaskControllerMode() const = 0;
+};
+
+} // namespace System
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_SYSTEM_I_TASK_CONTROLLER_MANAGER_H