From 2a54d6b1698d667bf29ef6029c6bd3df84d0873c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 6 Jul 2021 15:28:56 +0200 Subject: [PATCH 1/7] Add the possibility to control a subset of coordinates in IK::SE3Task class --- src/IK/include/BipedalLocomotion/IK/SE3Task.h | 8 ++ src/IK/src/SE3Task.cpp | 99 ++++++++++++++++--- 2 files changed, 93 insertions(+), 14 deletions(-) diff --git a/src/IK/include/BipedalLocomotion/IK/SE3Task.h b/src/IK/include/BipedalLocomotion/IK/SE3Task.h index dde6523112..1b3aae2a5e 100644 --- a/src/IK/include/BipedalLocomotion/IK/SE3Task.h +++ b/src/IK/include/BipedalLocomotion/IK/SE3Task.h @@ -44,6 +44,8 @@ namespace IK * different definitions of exponential maps and logarithm maps. Please consider that here the MIXED * representation is used to define the 6d-velocity. You can find further details in Section 2.3.4 * of https://traversaro.github.io/phd-thesis/traversaro-phd-thesis.pdf. + * @note SE3Task can be used to control also a subset of element of SE3. Please refer to `mask` + * parameter in IK::SE3Task::initialize method. */ class SE3Task : public System::LinearTask { @@ -65,6 +67,11 @@ class SE3Task : public System::LinearTask std::shared_ptr m_kinDyn; /**< Pointer to a KinDynComputations object */ + /** Mask used to select the DoFs controlled by the task */ + std::array m_mask{true, true, true, true, true, true}; + std::size_t m_DoFs{m_spatialVelocitySize}; /**< DoFs associated to the task */ + + Eigen::MatrixXd m_jacobian; /**< Jacobian matrix in MIXED representation */ public: /** @@ -77,6 +84,7 @@ class SE3Task : public System::LinearTask * | `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` | Mask representing the DoFs controlled. E.g. [T,F,T,F,F,F] will enable the control on the x and z coordinates only. (Default value, [T,T,T,T,T,T]) | No | * @return True in case of success, false otherwise. * Where the generalized robot velocity is a vector containing the base spatial-velocity * (expressed in mixed representation) and the joint velocities. diff --git a/src/IK/src/SE3Task.cpp b/src/IK/src/SE3Task.cpp index c7de848b06..9b575e1d35 100644 --- a/src/IK/src/SE3Task.cpp +++ b/src/IK/src/SE3Task.cpp @@ -57,19 +57,28 @@ bool SE3Task::setVariablesHandler(const System::VariablesHandler& variablesHandl } // resize the matrices - m_A.resize(m_spatialVelocitySize, variablesHandler.getNumberOfVariables()); + m_A.resize(m_DoFs, variablesHandler.getNumberOfVariables()); m_A.setZero(); - m_b.resize(m_spatialVelocitySize); + m_b.resize(m_DoFs); + m_jacobian.resize(m_spatialVelocitySize, m_robotVelocityVariable.size); return true; } bool SE3Task::initialize(std::weak_ptr paramHandler) { - constexpr std::string_view errorPrefix = "[SE3Task::initialize] "; + constexpr auto errorPrefix = "[SE3Task::initialize] "; std::string frameName = "Unknown"; - constexpr std::string_view descriptionPrefix = "SE3Task Optimal Control Element - Frame name: "; + constexpr auto descriptionPrefix = "IK-SE3Task - Frame name: "; + + std::string maskDescription = ""; + auto boolToString = [](bool b) { return b ? " true" : " false"; }; + for(const auto flag : m_mask) + { + maskDescription += boolToString(flag); + } + if (m_kinDyn == nullptr || !m_kinDyn->isValid()) { @@ -146,8 +155,31 @@ bool SE3Task::initialize(std::weak_ptr pa m_R3Controller.setGains(kpLinear); m_SO3Controller.setGains(kpAngular); - // set the description - m_description = std::string(descriptionPrefix) + frameName + "."; + std::vector mask; + if (!ptr->getParameter("mask", mask) || (mask.size() != m_spatialVelocitySize)) + { + log()->info("{} [{} {}] Unable to find the mask parameter. The default value is used:{}.", + errorPrefix, + descriptionPrefix, + frameName, + maskDescription); + } + else + { + // covert an std::vector in a std::array + std::copy(mask.begin(), mask.end(), m_mask.begin()); + // compute the DoFs associated to the task + m_DoFs = std::count(m_mask.begin(), m_mask.end(), true); + + // Update the mask description + maskDescription.clear(); + for(const auto flag : m_mask) + { + maskDescription += boolToString(flag); + } + } + + m_description = descriptionPrefix + frameName + " Mask:" + maskDescription + "."; m_isInitialized = true; @@ -169,14 +201,53 @@ bool SE3Task::update() m_SO3Controller.computeControlLaw(); m_R3Controller.computeControlLaw(); - m_b.head<3>() = m_R3Controller.getControl().coeffs(); - m_b.tail<3>() = m_SO3Controller.getControl().coeffs(); - - if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex, - this->subA(m_robotVelocityVariable))) + // if we want to control all 6 DoF we avoid to lose performances + if (m_DoFs == m_spatialVelocitySize) + { + m_b.head<3>() = m_R3Controller.getControl().coeffs(); + m_b.tail<3>() = m_SO3Controller.getControl().coeffs(); + + if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex, + this->subA(m_robotVelocityVariable))) + { + log()->error("[SE3Task::update] Unable to get the jacobian."); + return m_isValid; + } + } else { - log()->error("[SE3Task::update] Unable to get the jacobian."); - return m_isValid; + // store the jacobian associated to the given frame + if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex, m_jacobian)) + { + log()->error("[SE3Task::update] Unable to get the jacobian."); + return m_isValid; + } + + // take only the required components + std::size_t index = 0; + + // linear components + for (std::size_t i = 0; i < 3; i++) + { + if (m_mask[i]) + { + m_b(index) = m_R3Controller.getControl().coeffs()(i); + iDynTree::toEigen(this->subA(m_robotVelocityVariable)).row(index) + = m_jacobian.row(i); + index++; + } + } + + // angular components + for (std::size_t i = 0; i < 3; i++) + { + if (m_mask[i + 3]) + { + m_b(index) = m_SO3Controller.getControl().coeffs()(i); + iDynTree::toEigen(this->subA(m_robotVelocityVariable)).row(index) + = m_jacobian.row(i + 3); + index++; + } + } } m_isValid = true; @@ -199,7 +270,7 @@ bool SE3Task::setSetPoint(const manif::SE3d& I_H_F, const manif::SE3d::Tangent& std::size_t SE3Task::size() const { - return m_spatialVelocitySize; + return m_DoFs; } SE3Task::Type SE3Task::type() const From 41ea76ff5fef4ac3b207a6070d99c8058efa7a94 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 6 Jul 2021 15:29:23 +0200 Subject: [PATCH 2/7] Improve the readability of the errors in IK::SE3Task class --- src/IK/src/SE3Task.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/IK/src/SE3Task.cpp b/src/IK/src/SE3Task.cpp index 9b575e1d35..ba89da3db2 100644 --- a/src/IK/src/SE3Task.cpp +++ b/src/IK/src/SE3Task.cpp @@ -82,7 +82,7 @@ bool SE3Task::initialize(std::weak_ptr pa if (m_kinDyn == nullptr || !m_kinDyn->isValid()) { - log()->error("{}, [{} {}] KinDynComputations object is not valid.", + log()->error("{} [{} {}] KinDynComputations object is not valid.", errorPrefix, descriptionPrefix, frameName); @@ -92,7 +92,7 @@ bool SE3Task::initialize(std::weak_ptr pa if (m_kinDyn->getFrameVelocityRepresentation() != iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION) { - log()->error("{}, [{} {}] The task supports only quantities expressed in MIXED " + log()->error("{} [{} {}] The task supports only quantities expressed in MIXED " "representation. Please provide a KinDynComputations with Frame velocity " "representation set to MIXED_REPRESENTATION.", errorPrefix, @@ -104,7 +104,7 @@ bool SE3Task::initialize(std::weak_ptr pa auto ptr = paramHandler.lock(); if (ptr == nullptr) { - log()->error("{}, [{} {}] The parameter handler is not valid.", + log()->error("{} [{} {}] The parameter handler is not valid.", errorPrefix, descriptionPrefix, frameName); @@ -113,7 +113,7 @@ bool SE3Task::initialize(std::weak_ptr pa if (!ptr->getParameter("robot_velocity_variable_name", m_robotVelocityVariable.name)) { - log()->error("{}, [{} {}] Error while retrieving the robot velocity variable.", + log()->error("{} [{} {}] Error while retrieving the robot velocity variable.", errorPrefix, descriptionPrefix, frameName); @@ -124,7 +124,7 @@ bool SE3Task::initialize(std::weak_ptr pa || (m_frameIndex = m_kinDyn->model().getFrameIndex(frameName)) == iDynTree::FRAME_INVALID_INDEX) { - log()->error("{}, [{} {}] Error while retrieving the frame that should be controlled.", + log()->error("{} [{} {}] Error while retrieving the frame that should be controlled.", errorPrefix, descriptionPrefix, frameName); @@ -136,7 +136,7 @@ bool SE3Task::initialize(std::weak_ptr pa double kpAngular; if (!ptr->getParameter("kp_linear", kpLinear)) { - log()->error("{}, [{} {}] Unable to get the proportional linear gain.", + log()->error("{} [{} {}] Unable to get the proportional linear gain.", errorPrefix, descriptionPrefix, frameName); @@ -145,7 +145,7 @@ bool SE3Task::initialize(std::weak_ptr pa if (!ptr->getParameter("kp_angular", kpAngular)) { - log()->error("{}, [{} {}] Unable to get the proportional angular gain.", + log()->error("{} [{} {}] Unable to get the proportional angular gain.", errorPrefix, descriptionPrefix, frameName); From 6c177f260c5d5c7577eb6e9acb949b061fad5b53 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 6 Jul 2021 15:30:35 +0200 Subject: [PATCH 3/7] Implement the test for the IK::SE3Task mask feature --- src/IK/tests/SE3TaskTest.cpp | 166 ++++++++++++++++++++++++++++------- 1 file changed, 132 insertions(+), 34 deletions(-) diff --git a/src/IK/tests/SE3TaskTest.cpp b/src/IK/tests/SE3TaskTest.cpp index ff5b35e804..d12e6bba95 100644 --- a/src/IK/tests/SE3TaskTest.cpp +++ b/src/IK/tests/SE3TaskTest.cpp @@ -41,48 +41,49 @@ TEST_CASE("SE3 Task") for (std::size_t numberOfJoints = 6; numberOfJoints < 200; numberOfJoints += 15) { - DYNAMIC_SECTION("Model with " << numberOfJoints << " joints") - { - // create the model - const iDynTree::Model model = iDynTree::getRandomModel(numberOfJoints); - REQUIRE(kinDyn->loadRobotModel(model)); + // create the model + const iDynTree::Model model = iDynTree::getRandomModel(numberOfJoints); + REQUIRE(kinDyn->loadRobotModel(model)); - const auto worldBasePos = iDynTree::getRandomTransform(); - const auto baseVel = iDynTree::getRandomTwist(); - iDynTree::VectorDynSize jointsPos(model.getNrOfDOFs()); - iDynTree::VectorDynSize jointsVel(model.getNrOfDOFs()); - iDynTree::Vector3 gravity; + const auto worldBasePos = iDynTree::getRandomTransform(); + const auto baseVel = iDynTree::getRandomTwist(); + iDynTree::VectorDynSize jointsPos(model.getNrOfDOFs()); + iDynTree::VectorDynSize jointsVel(model.getNrOfDOFs()); + iDynTree::Vector3 gravity; - for (auto& joint : jointsPos) - { - joint = iDynTree::getRandomDouble(); - } + for (auto& joint : jointsPos) + { + joint = iDynTree::getRandomDouble(); + } - for (auto& joint : jointsVel) - { - joint = iDynTree::getRandomDouble(); - } + for (auto& joint : jointsVel) + { + joint = iDynTree::getRandomDouble(); + } - for (auto& element : gravity) - { - element = iDynTree::getRandomDouble(); - } + for (auto& element : gravity) + { + element = iDynTree::getRandomDouble(); + } - REQUIRE(kinDyn->setRobotState(worldBasePos, jointsPos, baseVel, jointsVel, gravity)); + REQUIRE(kinDyn->setRobotState(worldBasePos, jointsPos, baseVel, jointsVel, gravity)); - // Instantiate the handler - VariablesHandler variablesHandler; - variablesHandler.addVariable("dummy1", 10); - variablesHandler.addVariable(robotVelocity, model.getNrOfDOFs() + 6); - variablesHandler.addVariable("dummy2", 15); + // Instantiate the handler + VariablesHandler variablesHandler; + variablesHandler.addVariable("dummy1", 10); + variablesHandler.addVariable(robotVelocity, model.getNrOfDOFs() + 6); + variablesHandler.addVariable("dummy2", 15); - const std::string controlledFrame = model.getFrameName(numberOfJoints); - parameterHandler->setParameter("frame_name", controlledFrame); + const std::string controlledFrame = model.getFrameName(numberOfJoints); + parameterHandler->setParameter("frame_name", controlledFrame); + DYNAMIC_SECTION("Model with " << numberOfJoints << " joints - [All DoF]") + { SE3Task task; REQUIRE(task.setKinDyn(kinDyn)); REQUIRE(task.initialize(parameterHandler)); REQUIRE(task.setVariablesHandler(variablesHandler)); + REQUIRE(task.size() == 6); const auto desiredPose = manif::SE3d::Random(); const auto desiredVelocity = manif::SE3d::Tangent::Random(); @@ -99,18 +100,18 @@ TEST_CASE("SE3 Task") // check the matrix A REQUIRE(A.middleCols(variablesHandler.getVariable("dummy1").offset, variablesHandler.getVariable("dummy1").size) - .isZero()); + .isZero()); REQUIRE(A.middleCols(variablesHandler.getVariable("dummy2").offset, variablesHandler.getVariable("dummy2").size) - .isZero()); + .isZero()); Eigen::MatrixXd jacobian(6, model.getNrOfDOFs() + 6); REQUIRE(kinDyn->getFrameFreeFloatingJacobian(controlledFrame, jacobian)); REQUIRE(A.middleCols(variablesHandler.getVariable(robotVelocity).offset, variablesHandler.getVariable(robotVelocity).size) - .isApprox(jacobian)); + .isApprox(jacobian)); // check the vector b LieGroupControllers::ProportionalControllerSO3d SO3Controller; @@ -124,7 +125,7 @@ TEST_CASE("SE3 Task") R3Controller.setDesiredState(desiredPose.translation()); SO3Controller.setState(BipedalLocomotion::Conversions::toManifRot( - kinDyn->getWorldTransform(controlledFrame).getRotation())); + kinDyn->getWorldTransform(controlledFrame).getRotation())); R3Controller.setState( iDynTree::toEigen(kinDyn->getWorldTransform(controlledFrame).getPosition())); @@ -138,5 +139,102 @@ TEST_CASE("SE3 Task") REQUIRE(b.isApprox(expectedB)); } + + + DYNAMIC_SECTION("Model with " << numberOfJoints << " joints - [mask]") + { + const auto mask = std::vector{true, false, true, false, false, false}; + const auto DoFs = std::count(mask.begin(), mask.end(), true); + + parameterHandler->setParameter("mask", mask); + + SE3Task task; + REQUIRE(task.setKinDyn(kinDyn)); + REQUIRE(task.initialize(parameterHandler)); + REQUIRE(task.setVariablesHandler(variablesHandler)); + REQUIRE(task.size() == DoFs); + + const auto desiredPose = manif::SE3d::Random(); + const auto desiredVelocity = manif::SE3d::Tangent::Random(); + + REQUIRE(task.setSetPoint(desiredPose, desiredVelocity)); + + REQUIRE(task.update()); + REQUIRE(task.isValid()); + + // get A and b + Eigen::Ref A = task.getA(); + Eigen::Ref b = task.getB(); + + // check the matrix A + REQUIRE(A.middleCols(variablesHandler.getVariable("dummy1").offset, + variablesHandler.getVariable("dummy1").size) + .isZero()); + + REQUIRE(A.middleCols(variablesHandler.getVariable("dummy2").offset, + variablesHandler.getVariable("dummy2").size) + .isZero()); + + Eigen::MatrixXd jacobian(6, model.getNrOfDOFs() + 6); + REQUIRE(kinDyn->getFrameFreeFloatingJacobian(controlledFrame, jacobian)); + + // extract the + Eigen::MatrixXd jacobianWithMask(DoFs, model.getNrOfDOFs() + 6); + std::size_t index = 0; + for (std::size_t i = 0; i < 6; i++) + { + if (mask[i]) + { + jacobianWithMask.row(index) = jacobian.row(i); + index++; + } + } + + REQUIRE(A.middleCols(variablesHandler.getVariable(robotVelocity).offset, + variablesHandler.getVariable(robotVelocity).size) + .isApprox(jacobianWithMask)); + + // check the vector b + LieGroupControllers::ProportionalControllerSO3d SO3Controller; + LieGroupControllers::ProportionalControllerR3d R3Controller; + SO3Controller.setGains(kp); + R3Controller.setGains(kp); + + SO3Controller.setFeedForward(desiredVelocity.ang()); + R3Controller.setFeedForward(desiredVelocity.lin()); + SO3Controller.setDesiredState(desiredPose.quat()); + R3Controller.setDesiredState(desiredPose.translation()); + + SO3Controller.setState(BipedalLocomotion::Conversions::toManifRot( + kinDyn->getWorldTransform(controlledFrame).getRotation())); + + R3Controller.setState( + iDynTree::toEigen(kinDyn->getWorldTransform(controlledFrame).getPosition())); + + SO3Controller.computeControlLaw(); + R3Controller.computeControlLaw(); + + Eigen::VectorXd expectedB(DoFs); + index = 0; + for(int i = 0; i < 3; i++) + { + if(mask[i]) + { + expectedB(index) = R3Controller.getControl().coeffs()[i]; + index++; + } + } + + for(int i = 0; i < 3; i++) + { + if(mask[i + 3]) + { + expectedB(index) = SO3Controller.getControl().coeffs()[i]; + index++; + } + } + + REQUIRE(b.isApprox(expectedB)); + } } } From 4a6a9ea6c9a43bf8946b8f160cfc5e23eab688f1 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 6 Jul 2021 15:58:05 +0200 Subject: [PATCH 4/7] Update the CHANGELOG --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index eae4b6eb7d..2c1567115a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,7 @@ All notable changes to this project are documented in this file. ### Changed - Add common Python files to gitignore (https://github.com/dic-iit/bipedal-locomotion-framework/pull/338) +- Add the possibility to control a subset of coordinates in `IK::SE3Task` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/356) ### Fix From 8583f7c3c34b5741b4d5057e0222cd197ab51234 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 12 Jul 2021 13:43:29 +0200 Subject: [PATCH 5/7] Remove the possibility to use the mask for the angular part of the SE3Task in IK component --- src/IK/include/BipedalLocomotion/IK/SE3Task.h | 12 +++++---- src/IK/src/SE3Task.cpp | 26 ++++++++----------- 2 files changed, 18 insertions(+), 20 deletions(-) diff --git a/src/IK/include/BipedalLocomotion/IK/SE3Task.h b/src/IK/include/BipedalLocomotion/IK/SE3Task.h index 1b3aae2a5e..6fe6d97ec5 100644 --- a/src/IK/include/BipedalLocomotion/IK/SE3Task.h +++ b/src/IK/include/BipedalLocomotion/IK/SE3Task.h @@ -44,8 +44,8 @@ namespace IK * different definitions of exponential maps and logarithm maps. Please consider that here the MIXED * representation is used to define the 6d-velocity. You can find further details in Section 2.3.4 * of https://traversaro.github.io/phd-thesis/traversaro-phd-thesis.pdf. - * @note SE3Task can be used to control also a subset of element of SE3. Please refer to `mask` - * parameter in IK::SE3Task::initialize method. + * @note SE3Task can be used to control also a subset of element of the linear part of the SE3. + * Please refer to `mask` parameter in IK::SE3Task::initialize method. */ class SE3Task : public System::LinearTask { @@ -60,6 +60,7 @@ class SE3Task : public System::LinearTask iDynTree::FrameIndex m_frameIndex; /**< Frame controlled by the OptimalControlElement */ static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. */ + static constexpr std::size_t m_linearVelocitySize{3}; /**< Size of the linear velocity vector. */ bool m_isInitialized{false}; /**< True if the task has been initialized. */ bool m_isValid{false}; /**< True if the task is valid. */ @@ -68,8 +69,9 @@ class SE3Task : public System::LinearTask object */ /** Mask used to select the DoFs controlled by the task */ - std::array m_mask{true, true, true, true, true, true}; - std::size_t m_DoFs{m_spatialVelocitySize}; /**< DoFs associated to the task */ + std::array m_mask{true, true, true}; + std::size_t m_linearDoFs{m_linearVelocitySize}; /**< DoFs associated to the linear task */ + std::size_t m_DoFs{m_spatialVelocitySize}; /**< DoFs associated to the entire task */ Eigen::MatrixXd m_jacobian; /**< Jacobian matrix in MIXED representation */ public: @@ -84,7 +86,7 @@ class SE3Task : public System::LinearTask * | `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` | Mask representing the DoFs controlled. E.g. [T,F,T,F,F,F] will enable the control on the x and z coordinates only. (Default value, [T,T,T,T,T,T]) | No | + * | `mask` | `vector` | Mask representing the linear DoFs controlled. E.g. [T,F,T] will enable the control on the x and z coordinates only and the angular part. (Default value, [T,T,T]) | No | * @return True in case of success, false otherwise. * Where the generalized robot velocity is a vector containing the base spatial-velocity * (expressed in mixed representation) and the joint velocities. diff --git a/src/IK/src/SE3Task.cpp b/src/IK/src/SE3Task.cpp index ba89da3db2..aca86d5322 100644 --- a/src/IK/src/SE3Task.cpp +++ b/src/IK/src/SE3Task.cpp @@ -156,7 +156,7 @@ bool SE3Task::initialize(std::weak_ptr pa m_SO3Controller.setGains(kpAngular); std::vector mask; - if (!ptr->getParameter("mask", mask) || (mask.size() != m_spatialVelocitySize)) + if (!ptr->getParameter("mask", mask) || (mask.size() != m_linearVelocitySize)) { log()->info("{} [{} {}] Unable to find the mask parameter. The default value is used:{}.", errorPrefix, @@ -169,7 +169,9 @@ bool SE3Task::initialize(std::weak_ptr pa // covert an std::vector in a std::array std::copy(mask.begin(), mask.end(), m_mask.begin()); // compute the DoFs associated to the task - m_DoFs = std::count(m_mask.begin(), m_mask.end(), true); + m_linearDoFs = std::count(m_mask.begin(), m_mask.end(), true); + + m_DoFs = m_linearDoFs + m_linearVelocitySize; // Update the mask description maskDescription.clear(); @@ -201,11 +203,13 @@ bool SE3Task::update() m_SO3Controller.computeControlLaw(); m_R3Controller.computeControlLaw(); + // the angular part is always enabled + m_b.tail<3>() = m_SO3Controller.getControl().coeffs(); + // if we want to control all 6 DoF we avoid to lose performances - if (m_DoFs == m_spatialVelocitySize) + if (m_linearDoFs == m_linearVelocitySize) { m_b.head<3>() = m_R3Controller.getControl().coeffs(); - m_b.tail<3>() = m_SO3Controller.getControl().coeffs(); if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex, this->subA(m_robotVelocityVariable))) @@ -237,17 +241,9 @@ bool SE3Task::update() } } - // angular components - for (std::size_t i = 0; i < 3; i++) - { - if (m_mask[i + 3]) - { - m_b(index) = m_SO3Controller.getControl().coeffs()(i); - iDynTree::toEigen(this->subA(m_robotVelocityVariable)).row(index) - = m_jacobian.row(i + 3); - index++; - } - } + // take the all angular part + iDynTree::toEigen(this->subA(m_robotVelocityVariable)).bottomRows<3>() + = m_jacobian.bottomRows<3>(); } m_isValid = true; From 44ee05833e2ec10078d390be1c44b1bb175419d2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 12 Jul 2021 13:44:27 +0200 Subject: [PATCH 6/7] Update IK unit test SE3TasK accordingly to 8583f7c3c34b5741b4d5057e0222cd197ab51234 --- src/IK/tests/SE3TaskTest.cpp | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/src/IK/tests/SE3TaskTest.cpp b/src/IK/tests/SE3TaskTest.cpp index d12e6bba95..392be41612 100644 --- a/src/IK/tests/SE3TaskTest.cpp +++ b/src/IK/tests/SE3TaskTest.cpp @@ -143,8 +143,8 @@ TEST_CASE("SE3 Task") DYNAMIC_SECTION("Model with " << numberOfJoints << " joints - [mask]") { - const auto mask = std::vector{true, false, true, false, false, false}; - const auto DoFs = std::count(mask.begin(), mask.end(), true); + const auto mask = std::vector{true, false, true}; + const auto DoFs = std::count(mask.begin(), mask.end(), true) + 3; parameterHandler->setParameter("mask", mask); @@ -181,7 +181,7 @@ TEST_CASE("SE3 Task") // extract the Eigen::MatrixXd jacobianWithMask(DoFs, model.getNrOfDOFs() + 6); std::size_t index = 0; - for (std::size_t i = 0; i < 6; i++) + for (std::size_t i = 0; i < 3; i++) { if (mask[i]) { @@ -190,6 +190,8 @@ TEST_CASE("SE3 Task") } } + jacobianWithMask.bottomRows(3) = jacobian.bottomRows(3); + REQUIRE(A.middleCols(variablesHandler.getVariable(robotVelocity).offset, variablesHandler.getVariable(robotVelocity).size) .isApprox(jacobianWithMask)); @@ -225,14 +227,7 @@ TEST_CASE("SE3 Task") } } - for(int i = 0; i < 3; i++) - { - if(mask[i + 3]) - { - expectedB(index) = SO3Controller.getControl().coeffs()[i]; - index++; - } - } + expectedB.bottomRows(3) = SO3Controller.getControl().coeffs(); REQUIRE(b.isApprox(expectedB)); } From 87bf8ce58b995db022bc37ce9923092965989ee1 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 12 Jul 2021 16:54:38 +0200 Subject: [PATCH 7/7] Uses 1 and 0 instead of T and F in the SE3Task.h --- src/IK/include/BipedalLocomotion/IK/SE3Task.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/IK/include/BipedalLocomotion/IK/SE3Task.h b/src/IK/include/BipedalLocomotion/IK/SE3Task.h index 6fe6d97ec5..429eba8395 100644 --- a/src/IK/include/BipedalLocomotion/IK/SE3Task.h +++ b/src/IK/include/BipedalLocomotion/IK/SE3Task.h @@ -86,7 +86,7 @@ class SE3Task : public System::LinearTask * | `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` | Mask representing the linear DoFs controlled. E.g. [T,F,T] will enable the control on the x and z coordinates only and the angular part. (Default value, [T,T,T]) | No | + * | `mask` | `vector` | 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 | * @return True in case of success, false otherwise. * Where the generalized robot velocity is a vector containing the base spatial-velocity * (expressed in mixed representation) and the joint velocities.