Merge pull request #654 from RiccardoGrieco/allow-vector-kp

Allow use of vectors for IK and TSID SO3 SE3 and R3 Tasks gains
ami-iit · Apr 18, 2023 · 1668701 · 1668701
2 parents 239ea0d + 5bf9199
commit 1668701
Show file tree

Hide file tree

Showing 14 changed files with 131 additions and 41 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -27,7 +27,7 @@ All notable changes to this project are documented in this file.
 - `SwingFootTrajectoryPlanner::advance()` must be called before getting the output (https://github.com/ami-iit/bipedal-locomotion-framework/pull/637)
 - Update the already existing classes in `ContinuousDynamicalSystem`to be compatible with the `System::NamedTuple` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/642)
 - Update the code to be compatible with LieGroupControllers v0.2.0 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/653)
-
+- Allow use of vectors for task gains (https://github.com/ami-iit/bipedal-locomotion-framework/pull/654)
 
 ### Fixed
 - Return an error if an invalid `KinDynComputations` object is passed to `QPInverseKinematics::build()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/622)

diff --git a/src/IK/include/BipedalLocomotion/IK/R3Task.h b/src/IK/include/BipedalLocomotion/IK/R3Task.h
@@ -83,7 +83,7 @@ class R3Task : public IKLinearTask, public BipedalLocomotion::System::ITaskContr
      * |:----------------------------------:|:--------:|:--------------------------------------------------------------------------------------:|:---------:|
      * |   `robot_velocity_variable_name`   | `string` |   Name of the variable contained in `VariablesHandler` describing the robot velocity   |    Yes    |
      * |            `frame_name`            | `string` |                       Name of the frame controlled by the SE3Task                      |    Yes    |
-     * |             `kp_linear`            | `double` |                             Gain of the position controller                            |    Yes    |
+     * |             `kp_linear`            | `double` or `vector<double>` |                             Gain of the position controller                            |    Yes    |
      * |               `mask`               | `vector<bool>` |  Mask representing the linear DoFs controlled. E.g. [1,0,1] will enable the control on the x and z coordinates only. (Default value, [1,1,1])   |    No    |
      * @return True in case of success, false otherwise.
      * Where the generalized robot velocity is a vector containing the base spatial-velocity

diff --git a/src/IK/include/BipedalLocomotion/IK/SE3Task.h b/src/IK/include/BipedalLocomotion/IK/SE3Task.h
@@ -96,9 +96,8 @@ class SE3Task : public IKLinearTask, public BipedalLocomotion::System::ITaskCont
      * |:----------------------------------:|:--------:|:--------------------------------------------------------------------------------------:|:---------:|
      * |   `robot_velocity_variable_name`   | `string` |   Name of the variable contained in `VariablesHandler` describing the robot velocity   |    Yes    |
      * |            `frame_name`            | `string` |                       Name of the frame controlled by the SE3Task                      |    Yes    |
-     * |             `kp_linear`            | `double` |                             Gain of the position controller                            |    Yes    |
-     * |            `kp_angular`            | `double` |                           Gain of the orientation controller                           |    Yes    |
-     * |               `mask`               | `vector<bool>` |  Mask representing the linear DoFs controlled. E.g. [1,0,1] will enable the control on the x and z coordinates only and the angular part. (Default value, [1,1,1])   |    No    |
+     * |             `kp_linear`            | `double` or `vector<double>` |                             Gain of the position controller                            |    Yes    |
+     * |            `kp_angular`            | `double` or `vector<double>` |                           Gain of the orientation controller                           |    Yes    |
      * |               `mask`               | `vector<bool>` |  Mask representing the linear DoFs controlled. E.g. [1,0,1] will enable the control on the x and z coordinates only and the angular part. (Default value, [1,1,1])   |    No    |
      * |  `use_position_exogenous_feedback` |  `bool`  |    If true the task will consider the frame position provided by the user as feedback. The feedback must be set using `setFeedback()`. (Default value `false`) |   No   |
      * |  `use_orientation_exogenous_feedback` |  `bool`  |    If true the task will consider the frame orientation provided by the user as feedback. The feedback must be set using `setFeedback()`. (Default value `false`) |   No   |

diff --git a/src/IK/include/BipedalLocomotion/IK/SO3Task.h b/src/IK/include/BipedalLocomotion/IK/SO3Task.h
@@ -71,7 +71,7 @@ class SO3Task : public IKLinearTask
      * |:----------------------------------:|:--------:|:--------------------------------------------------------------------------------------:|:---------:|
      * |    `robot_velocity_variable_name`  | `string` |   Name of the variable contained in `VariablesHandler` describing the robot velocity   |    Yes    |
      * |            `frame_name`            | `string` |                       Name of the frame controlled by the SO3Task                      |    Yes    |
-     * |            `kp_angular`            | `double` |                           Gain of the orientation controller                           |    Yes    |
+     * |            `kp_angular`            | `double` or `vector<double>` |                           Gain of the orientation controller                           |    Yes    |
      * Where the generalized robot velocity is a vector containing the base spatial-velocity
      * (expressed in mixed representation) and the joint velocities.
      * @return True in case of success, false otherwise.

diff --git a/src/IK/src/R3Task.cpp b/src/IK/src/R3Task.cpp
@@ -132,8 +132,13 @@ bool R3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandle
     }
 
     // set the gains for the controllers
-    double kpLinear;
-    if (!ptr->getParameter("kp_linear", kpLinear))
+    Eigen::Vector3d kpLinear;
+    double scalarBuffer;
+    if (ptr->getParameter("kp_linear", scalarBuffer))
+    {
+        kpLinear.setConstant(scalarBuffer);
+    }
+    else if(!ptr->getParameter("kp_linear", kpLinear))
     {
         log()->error("{} [{} {}] Unable to get the proportional linear gain.",
                      errorPrefix,

diff --git a/src/IK/src/SE3Task.cpp b/src/IK/src/SE3Task.cpp
@@ -133,10 +133,18 @@ bool SE3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandl
         return false;
     }
 
-    // set the gains for the controllers
-    double kpLinear;
-    double kpAngular;
-    if (!ptr->getParameter("kp_linear", kpLinear))
+    // set the gains for the R3 controller
+    double kpLinearScalar;
+    Eigen::Vector3d kpLinearVector;
+    if(ptr->getParameter("kp_linear", kpLinearScalar))
+    {
+        m_R3Controller.setGains(kpLinearScalar);
+    }
+    else if(ptr->getParameter("kp_linear", kpLinearVector))
+    {
+        m_R3Controller.setGains(kpLinearVector);
+    }
+    else
     {
         log()->error("{} [{} {}] Unable to get the proportional linear gain.",
                      errorPrefix,
@@ -145,7 +153,18 @@ bool SE3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandl
         return false;
     }
 
-    if (!ptr->getParameter("kp_angular", kpAngular))
+    // set gains for the SO3 controller
+    double kpAngularScalar;
+    Eigen::Vector3d kpAngularVector;
+    if(ptr->getParameter("kp_angular", kpAngularScalar))
+    {
+        m_SO3Controller.setGains(kpAngularScalar);
+    }
+    else if(ptr->getParameter("kp_angular", kpAngularVector))
+    {
+        m_SO3Controller.setGains(kpAngularVector);
+    }
+    else
     {
         log()->error("{} [{} {}] Unable to get the proportional angular gain.",
                      errorPrefix,
@@ -154,9 +173,6 @@ bool SE3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandl
         return false;
     }
 
-    m_R3Controller.setGains(kpLinear);
-    m_SO3Controller.setGains(kpAngular);
-
     std::vector<bool> mask;
     if (!ptr->getParameter("mask", mask) || (mask.size() != m_linearVelocitySize))
     {

diff --git a/src/IK/src/SO3Task.cpp b/src/IK/src/SO3Task.cpp
@@ -127,9 +127,18 @@ bool SO3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandl
     }
 
     // set the gains for the controllers
-    double kpAngular;
+    double kpAngularScalar;
+    Eigen::Vector3d kpAngularVector;
 
-    if (!ptr->getParameter("kp_angular", kpAngular))
+    if (ptr->getParameter("kp_angular", kpAngularScalar))
+    {
+        m_SO3Controller.setGains(kpAngularScalar);
+    }
+    else if(ptr->getParameter("kp_angular", kpAngularVector))
+    {
+        m_SO3Controller.setGains(kpAngularVector);
+    }
+    else
     {
         log()->error("{}, [{} {}] Unable to get the proportional angular gain.",
                      errorPrefix,
@@ -138,8 +147,6 @@ bool SO3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandl
         return false;
     }
 
-    m_SO3Controller.setGains(kpAngular);
-
     // set the description
     m_description = std::string(descriptionPrefix) + frameName + ".";
 

diff --git a/src/IK/tests/SE3TaskTest.cpp b/src/IK/tests/SE3TaskTest.cpp
@@ -24,6 +24,7 @@ using namespace BipedalLocomotion::IK;
 TEST_CASE("SE3 Task")
 {
     constexpr double kp = 1.0;
+    const std::vector<double> kpVector{kp, kp, kp};
     const std::string robotVelocity = "robotVelocity";
 
 
@@ -33,7 +34,7 @@ TEST_CASE("SE3 Task")
     parameterHandler->setParameter("robot_velocity_variable_name",
                                    robotVelocity);
 
-    parameterHandler->setParameter("kp_linear", kp);
+    parameterHandler->setParameter("kp_linear", kpVector);
     parameterHandler->setParameter("kp_angular", kp);
 
     // set the velocity representation

diff --git a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h
@@ -82,10 +82,10 @@ class SE3Task : public TSIDLinearTask, public BipedalLocomotion::System::ITaskCo
      * |:----------------------------------:|:--------:|:--------------------------------------------------------------------------------------:|:---------:|
      * | `robot_acceleration_variable_name` | `string` | Name of the variable contained in `VariablesHandler` describing the robot acceleration |    Yes    |
      * |            `frame_name`            | `string` |                       Name of the frame controlled by the SE3Task                      |    Yes    |
-     * |             `kp_linear`            | `double` |                             Gain of the position controller                            |    Yes    |
-     * |             `kd_linear`            | `double` |                         Gain of the linear velocity controller                         |    Yes    |
-     * |            `kp_angular`            | `double` |                           Gain of the orientation controller                           |    Yes    |
-     * |            `kd_angular`            | `double` |                         Gain of the angular velocity controller                        |    Yes    |
+     * |             `kp_linear`            | `double` or `vector<double>` |                             Gains of the position controller                            |    Yes    |
+     * |             `kd_linear`            | `double` or `vector<double>` |                         Gains of the linear velocity controller                         |    Yes    |
+     * |            `kp_angular`            | `double` or `vector<double>` |                           Gain of the orientation controller                           |    Yes    |
+     * |            `kd_angular`            | `double` or `vector<double>` |                         Gain of the angular velocity controller                        |    Yes    |
      * @return True in case of success, false otherwise.
      */
     bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;

diff --git a/src/TSID/include/BipedalLocomotion/TSID/SO3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SO3Task.h
@@ -69,8 +69,8 @@ class SO3Task : public TSIDLinearTask
      * |:----------------------------------:|:--------:|:--------------------------------------------------------------------------------------:|:---------:|
      * | `robot_acceleration_variable_name` | `string` | Name of the variable contained in `VariablesHandler` describing the robot acceleration |    Yes    |
      * |            `frame_name`            | `string` |                       Name of the frame controlled by the SO3Task                      |    Yes    |
-     * |            `kp_angular`            | `double` |                           Gain of the orientation controller                           |    Yes    |
-     * |            `kd_angular`            | `double` |                         Gain of the angular velocity controller                        |    Yes    |
+     * |            `kp_angular`            | `double` or `vector<double>` |                           Gain of the orientation controller                           |    Yes    |
+     * |            `kd_angular`            | `double` or `vector<double>` |                         Gain of the angular velocity controller                        |    Yes    |
      * @return True in case of success, false otherwise.
      */
     bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;

diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp
@@ -123,28 +123,68 @@ bool SE3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandl
         return false;
     }
 
-    // set the gains for the controllers
-    double kpLinear, kdLinear;
-    double kpAngular, kdAngular;
-    if (!ptr->getParameter("kp_linear", kpLinear) || !ptr->getParameter("kd_linear", kdLinear))
+    // set the gains for the R3 controller
+    Eigen::Vector3d kpLinear, kdLinear;
+    double bufferLinearScalar;
+
+    if(ptr->getParameter("kp_linear", bufferLinearScalar))
+    {
+        kpLinear.setConstant(bufferLinearScalar);
+    }
+    else if(!ptr->getParameter("kp_linear", kpLinear))
     {
-        log()->error("{}, [{} {}] Unable to get the proportional and derivative linear gain.",
+        log()->error("{}, [{} {}] Unable to get the proportional linear gain.",
                      errorPrefix,
                      descriptionPrefix,
                      frameName);
         return false;
     }
 
-    if (!ptr->getParameter("kp_angular", kpAngular) || !ptr->getParameter("kd_angular", kdAngular))
+    if(ptr->getParameter("kd_linear", bufferLinearScalar))
     {
-        log()->error("{}, [{} {}] Unable to get the proportional and derivative angular gain.",
+        kdLinear.setConstant(bufferLinearScalar);
+    }
+    else if(!ptr->getParameter("kd_linear", kdLinear))
+    {
+        log()->error("{}, [{} {}] Unable to get the derivative linear gain.",
                      errorPrefix,
                      descriptionPrefix,
                      frameName);
         return false;
     }
 
     m_R3Controller.setGains(kpLinear, kdLinear);
+
+    // set the gains for the SO3 controller
+    Eigen::Vector3d kpAngular, kdAngular;
+    double bufferAngularScalar;
+
+    if(ptr->getParameter("kp_angular", bufferAngularScalar))
+    {
+        kpAngular.setConstant(bufferAngularScalar);
+    }
+    else if(!ptr->getParameter("kp_angular", kpAngular))
+    {
+        log()->error("{}, [{} {}] Unable to get the proportional angular gain.",
+                     errorPrefix,
+                     descriptionPrefix,
+                     frameName);
+        return false;
+    }
+
+    if(ptr->getParameter("kd_angular", bufferAngularScalar))
+    {
+        kdAngular.setConstant(bufferAngularScalar);
+    }
+    else if(!ptr->getParameter("kd_angular", kdAngular))
+    {
+        log()->error("{}, [{} {}] Unable to get the derivative angular gain.",
+                     errorPrefix,
+                     descriptionPrefix,
+                     frameName);
+        return false;
+    }
+
     m_SO3Controller.setGains(kpAngular, kdAngular);
 
     // set the description

diff --git a/src/TSID/src/SO3Task.cpp b/src/TSID/src/SO3Task.cpp
@@ -125,11 +125,30 @@ bool SO3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandl
         return false;
     }
 
-    // set the gains for the controllers
-    double kpAngular, kdAngular;
-    if (!ptr->getParameter("kp_angular", kpAngular) || !ptr->getParameter("kd_angular", kdAngular))
+    // set the gains for the SO3 controller
+    Eigen::Vector3d kpAngular, kdAngular;
+    double scalarBuffer;
+
+    if (ptr->getParameter("kp_angular", scalarBuffer))
+    {
+        kpAngular.setConstant(scalarBuffer);
+    }
+    else if(!ptr->getParameter("kp_angular", kpAngular))
+    {
+        log()->error("{}, [{} {}] Unable to get the proportional angular gain.",
+                     errorPrefix,
+                     descriptionPrefix,
+                     frameName);
+        return false;
+    }
+
+    if (ptr->getParameter("kd_angular", scalarBuffer))
+    {
+        kdAngular.setConstant(scalarBuffer);
+    }
+    else if(!ptr->getParameter("kd_angular", kdAngular))
     {
-        log()->error("{}, [{} {}] Unable to get the proportional and derivative angular gain.",
+        log()->error("{}, [{} {}] Unable to get the derivative angular gain.",
                      errorPrefix,
                      descriptionPrefix,
                      frameName);

diff --git a/src/TSID/tests/SE3TaskTest.cpp b/src/TSID/tests/SE3TaskTest.cpp
@@ -24,7 +24,9 @@ using namespace BipedalLocomotion::TSID;
 TEST_CASE("SE3 Task")
 {
     constexpr double kp = 1.0;
+    const std::vector<double> kpVector{kp, kp, kp};
     constexpr double kd = 0.5;
+    const std::vector<double> kdVector{kd, kd, kd};
     const std::string robotAcceleration = "robotAcceleration";
 
 
@@ -34,8 +36,8 @@ TEST_CASE("SE3 Task")
     parameterHandler->setParameter("robot_acceleration_variable_name",
                                    robotAcceleration);
 
-    parameterHandler->setParameter("kp_linear", kp);
-    parameterHandler->setParameter("kd_linear", kd);
+    parameterHandler->setParameter("kp_linear", kpVector);
+    parameterHandler->setParameter("kd_linear", kdVector);
     parameterHandler->setParameter("kp_angular", kp);
     parameterHandler->setParameter("kd_angular", kd);
 

diff --git a/src/TSID/tests/SO3TaskTest.cpp b/src/TSID/tests/SO3TaskTest.cpp
@@ -24,6 +24,7 @@ using namespace BipedalLocomotion::TSID;
 TEST_CASE("SO3 Task")
 {
     constexpr double kp = 1.0;
+    std::vector<double> kpVector{kp, kp, kp};
     constexpr double kd = 0.5;
     const std::string robotAcceleration = "robotAcceleration";
 
@@ -32,7 +33,7 @@ TEST_CASE("SO3 Task")
 
     parameterHandler->setParameter("robot_acceleration_variable_name", robotAcceleration);
 
-    parameterHandler->setParameter("kp_angular", kp);
+    parameterHandler->setParameter("kp_angular", kpVector);
     parameterHandler->setParameter("kd_angular", kd);
 
     // set the velocity representation