Add the possibility to enable/disable the SE3Task controller in IK co…

…mponent
ami-iit · Apr 26, 2021 · ccdc259 · ccdc259
1 parent 738593b
commit ccdc259
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 6 deletions.
diff --git a/src/IK/include/BipedalLocomotion/IK/SE3Task.h b/src/IK/include/BipedalLocomotion/IK/SE3Task.h
@@ -65,6 +65,9 @@ class SE3Task : public System::LinearTask
     std::shared_ptr<iDynTree::KinDynComputations> m_kinDyn; /**< Pointer to a KinDynComputations
                                                                object */
 
+    double m_kpLinear;
+    double m_kpAngular;
+
 public:
 
     /**
@@ -118,6 +121,10 @@ class SE3Task : public System::LinearTask
      */
     bool setSetPoint(const manif::SE3d& I_H_F, const manif::SE3d::Tangent& mixedVelocity);
 
+    void enableControl();
+
+    void disableControl();
+
     /**
      * Get the size of the task. (I.e the number of rows of the vector b)
      * @return the size of the task.

diff --git a/src/IK/src/SE3Task.cpp b/src/IK/src/SE3Task.cpp
@@ -123,9 +123,8 @@ bool SE3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> pa
     }
 
     // set the gains for the controllers
-    double kpLinear;
-    double kpAngular;
-    if (!ptr->getParameter("kp_linear", kpLinear))
+
+    if (!ptr->getParameter("kp_linear", m_kpLinear))
     {
         log()->error("{}, [{} {}] Unable to get the proportional linear gain.",
                      errorPrefix,
@@ -134,7 +133,7 @@ bool SE3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> pa
         return false;
     }
 
-    if (!ptr->getParameter("kp_angular", kpAngular))
+    if (!ptr->getParameter("kp_angular", m_kpAngular))
     {
         log()->error("{}, [{} {}] Unable to get the proportional angular gain.",
                      errorPrefix,
@@ -143,8 +142,8 @@ bool SE3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> pa
         return false;
     }
 
-    m_R3Controller.setGains(kpLinear);
-    m_SO3Controller.setGains(kpAngular);
+    m_R3Controller.setGains(m_kpLinear);
+    m_SO3Controller.setGains(m_kpAngular);
 
     // set the description
     m_description = std::string(descriptionPrefix) + frameName + ".";
@@ -211,3 +210,15 @@ bool SE3Task::isValid() const
 {
     return m_isValid;
 }
+
+void SE3Task::enableControl()
+{
+    m_R3Controller.setGains(m_kpLinear);
+    m_SO3Controller.setGains(m_kpAngular);
+}
+
+void SE3Task::disableControl()
+{
+    m_R3Controller.setGains(0);
+    m_SO3Controller.setGains(0);
+}