Check that MIXED_REPRESENTATION is used in the SE3Task

ami-iit · Jan 12, 2021 · 7a776b2 · 7a776b2
1 parent e2bb904
commit 7a776b2
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 0 deletions.
diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp
@@ -30,6 +30,17 @@ bool SE3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> pa
         return false;
     }
 
+    if (m_kinDyn->getFrameVelocityRepresentation()
+        != iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)
+    {
+        std::cerr << errorPrefix << descriptionPrefix << frameName
+                  << " - The task supports only quantities expressed in MIXED representation. "
+                     "Please provide a KinDynComputations with Frame velocity representation set "
+                     "to MIXED_REPRESENTATION."
+                  << std::endl;
+        return false;
+    }
+
     auto ptr = paramHandler.lock();
     if(ptr == nullptr)
     {

diff --git a/src/TSID/tests/SE3TaskTest.cpp b/src/TSID/tests/SE3TaskTest.cpp
@@ -39,6 +39,9 @@ TEST_CASE("SE3 Task")
     parameterHandler->setParameter("kp_angular", kp);
     parameterHandler->setParameter("kd_angular", kd);
 
+    // set the velocity representation
+    REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION));
+
     for (std::size_t numberOfJoints = 6; numberOfJoints < 1000; numberOfJoints += 15)
     {
         DYNAMIC_SECTION("Model with " << numberOfJoints << " joints")