From 7a776b256581da2ee34de5539b10d1f54875c507 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 12 Jan 2021 11:27:35 +0100 Subject: [PATCH] Check that MIXED_REPRESENTATION is used in the SE3Task --- src/TSID/src/SE3Task.cpp | 11 +++++++++++ src/TSID/tests/SE3TaskTest.cpp | 3 +++ 2 files changed, 14 insertions(+) diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp index 0808846098..b95310d32a 100644 --- a/src/TSID/src/SE3Task.cpp +++ b/src/TSID/src/SE3Task.cpp @@ -30,6 +30,17 @@ bool SE3Task::initialize(std::weak_ptr 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 index b7fc57a874..48c7a2e362 100644 --- 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")