From bfa5eaa1f2d7611911eda7681d6e5960c241078d Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 12 Jan 2021 14:17:44 +0100 Subject: [PATCH] Avoid to use magic numbers in SE3Task class --- src/TSID/include/BipedalLocomotion/TSID/SE3Task.h | 2 ++ src/TSID/src/SE3Task.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h index 529e750b9f..a40579c6d2 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h +++ b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h @@ -55,6 +55,8 @@ class SE3Task : public Task iDynTree::FrameIndex m_frameIndex; /**< Frame controlled by the OptimalControlElement */ + static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. */ + public: /** * Initialize the planner. diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp index b95310d32a..8397a8e820 100644 --- a/src/TSID/src/SE3Task.cpp +++ b/src/TSID/src/SE3Task.cpp @@ -59,7 +59,8 @@ bool SE3Task::initialize(std::weak_ptr pa return false; } - if (m_robotAccelerationVariable.size != m_kinDyn->getNrOfDegreesOfFreedom() + 6) + if (m_robotAccelerationVariable.size + != m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize) { std::cerr << errorPrefix << descriptionPrefix << frameName << " - Error while retrieving the robot acceleration variable." << std::endl; @@ -101,9 +102,9 @@ bool SE3Task::initialize(std::weak_ptr pa m_description = std::string(descriptionPrefix) + frameName + "."; // resize the matrices - m_A.resize(6, variablesHandler.getNumberOfVariables()); + m_A.resize(m_spatialVelocitySize, variablesHandler.getNumberOfVariables()); m_A.setZero(); - m_b.resize(6); + m_b.resize(m_spatialVelocitySize); return true; }