Add the getControllerOutput method to the TSID::SE3Task class (#740)

ami-iit · Oct 19, 2023 · 8bdf35b · 8bdf35b
1 parent 72459c3
commit 8bdf35b
Show file tree

Hide file tree

Showing 6 changed files with 44 additions and 4 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -8,6 +8,7 @@ All notable changes to this project are documented in this file.
 ### Added
 - Add the possibility to control a subset of coordinates in `TSID::CoMTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/724, https://github.com/ami-iit/bipedal-locomotion-framework/pull/727)
 - Add the possibility to set the maximum number of accepted deadline miss in `System::AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/726)
+- Add the `getControllerOutput` method to the `TSID::SE3Task` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/740)
 
 ### Changed
 - Remove the possibility to disable the telemetry in `System::AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/726)

diff --git a/bindings/python/TSID/src/SE3Task.cpp b/bindings/python/TSID/src/SE3Task.cpp
@@ -41,7 +41,10 @@ void CreateSE3Task(pybind11::module& module)
              &SE3Task::setSetPoint,
              py::arg("I_H_F"),
              py::arg("mixed_velocity"),
-             py::arg("mixed_acceleration"));
+             py::arg("mixed_acceleration"))
+        .def("get_controller_output",
+             &SE3Task::getControllerOutput);
+
 }
 
 } // namespace TSID

diff --git a/bindings/python/TSID/tests/test_TSID.py b/bindings/python/TSID/tests/test_TSID.py
@@ -137,6 +137,11 @@ def test_se3_task():
     assert se3_task.set_set_point(I_H_F=I_H_F,
                                   mixed_velocity=mixed_velocity,
                                   mixed_acceleration=mixed_acceleration)
+    # Update the task
+    assert se3_task.update()
+
+    # Check get_controller_output
+    assert se3_task.get_controller_output().size == 6
 
 def test_so3_task():
 

diff --git a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h
@@ -79,6 +79,7 @@ class SE3Task : public TSIDLinearTask, public BipedalLocomotion::System::ITaskCo
     std::size_t m_DoFs{m_spatialVelocitySize}; /**< DoFs associated to the entire task */
 
     Eigen::MatrixXd m_jacobian; /**< Jacobian matrix in MIXED representation */
+    Eigen::VectorXd m_controllerOutput; /**< Controller output */
 
     /** State of the proportional derivative controller implemented in the task */
     Mode m_controllerMode{Mode::Enable};
@@ -168,6 +169,14 @@ class SE3Task : public TSIDLinearTask, public BipedalLocomotion::System::ITaskCo
      * @return the state of the controller
      */
     Mode getTaskControllerMode() const override;
+
+    /**
+     * Get the controller output.
+     * @note the value of the controller output is changed by the update method.
+     * @return a const reference to the controller output.
+     */
+    Eigen::Ref<const Eigen::VectorXd> getControllerOutput() const;
+
 };
 
 } // namespace TSID

diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp
@@ -50,6 +50,7 @@ bool SE3Task::setVariablesHandler(const System::VariablesHandler& variablesHandl
     m_A.setZero();
     m_b.resize(m_DoFs);
     m_jacobian.resize(m_spatialVelocitySize, m_robotAccelerationVariable.size);
+    m_controllerOutput.resize(m_spatialVelocitySize);
 
     return true;
 }
@@ -237,7 +238,7 @@ bool SE3Task::update()
         return m_isValid;
     }
 
-    auto getControllerOutupt = [&](const auto& controller) {
+    auto getGenericControllerOutput = [&](const auto& controller) {
         if (m_controllerMode == Mode::Enable)
             return controller.getControl().coeffs();
         else
@@ -260,8 +261,9 @@ bool SE3Task::update()
     m_R3Controller.computeControlLaw();
 
     // get the output
-    bFullDoF.head<3>() += getControllerOutupt(m_R3Controller);
-    bFullDoF.tail<3>() += getControllerOutupt(m_SO3Controller);
+    m_controllerOutput.head<3>() = getGenericControllerOutput(m_R3Controller);
+    m_controllerOutput.tail<3>() = getGenericControllerOutput(m_SO3Controller);
+    bFullDoF += m_controllerOutput;
 
     m_b.tail<3>() = bFullDoF.tail<3>();
 
@@ -342,3 +344,8 @@ SE3Task::Mode SE3Task::getTaskControllerMode() const
 {
     return m_controllerMode;
 }
+
+Eigen::Ref<const Eigen::VectorXd> SE3Task::getControllerOutput() const
+{
+    return m_controllerOutput;
+}
diff --git a/src/TSID/tests/SE3TaskTest.cpp b/src/TSID/tests/SE3TaskTest.cpp
@@ -101,6 +101,7 @@ TEST_CASE("SE3 Task")
             // get A and b
             Eigen::Ref<const Eigen::MatrixXd> A = task.getA();
             Eigen::Ref<const Eigen::VectorXd> b = task.getB();
+            Eigen::Ref<const Eigen::VectorXd> controllerOutput = task.getControllerOutput();
 
             // check the matrix A
             REQUIRE(A.middleCols(variablesHandler.getVariable("dummy1").offset,
@@ -143,11 +144,16 @@ TEST_CASE("SE3 Task")
             R3Controller.computeControlLaw();
 
             Eigen::VectorXd expectedB(6);
+            Eigen::VectorXd expectedControllerOutput(6);
             expectedB = -iDynTree::toEigen(kinDyn->getFrameBiasAcc(controlledFrame));
             expectedB.head<3>() += R3Controller.getControl().coeffs();
             expectedB.tail<3>() += SO3Controller.getControl().coeffs();
 
+            expectedControllerOutput.head<3>() = R3Controller.getControl().coeffs();
+            expectedControllerOutput.tail<3>() = SO3Controller.getControl().coeffs();
+
             REQUIRE(b.isApprox(expectedB));
+            REQUIRE(controllerOutput.isApprox(expectedControllerOutput));
         }
 
         DYNAMIC_SECTION("Model with " << numberOfJoints << " joints - [mask]")
@@ -175,6 +181,7 @@ TEST_CASE("SE3 Task")
             // get A and b
             Eigen::Ref<const Eigen::MatrixXd> A = task.getA();
             Eigen::Ref<const Eigen::VectorXd> b = task.getB();
+            Eigen::Ref<const Eigen::VectorXd> controllerOutput = task.getControllerOutput();
 
             // check the matrix A
             REQUIRE(A.middleCols(variablesHandler.getVariable("dummy1").offset,
@@ -230,10 +237,18 @@ TEST_CASE("SE3 Task")
             R3Controller.computeControlLaw();
 
             Eigen::VectorXd expectedBFull(6);
+            Eigen::VectorXd expectedControllerOutput(6);
+
+
             expectedBFull = -iDynTree::toEigen(kinDyn->getFrameBiasAcc(controlledFrame));
             expectedBFull.head<3>() += R3Controller.getControl().coeffs();
             expectedBFull.tail<3>() += SO3Controller.getControl().coeffs();
 
+            expectedControllerOutput.head<3>() = R3Controller.getControl().coeffs();
+            expectedControllerOutput.tail<3>() = SO3Controller.getControl().coeffs();
+
+            REQUIRE(controllerOutput.isApprox(expectedControllerOutput));
+
             Eigen::VectorXd expectedB(DoFs);
 
             index = 0;