Add the possibility to control a subset of linear coordinates in `TSI…

…D::SE3Task` (#738)

CHANGELOG.md

@@ -3,6 +3,7 @@ All notable changes to this project are documented in this file.
 
 ## [Unreleased]
 - Added the ``DistanceTask`` and ``GravityTask`` to the IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/717)
+- Add the possibility to control a subset of linear coordinates in `TSID::SE3Task` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/738)
 
 ### 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)

src/IK/include/BipedalLocomotion/IK/SE3Task.h

@@ -66,6 +66,7 @@ class SE3Task : public IKLinearTask, public BipedalLocomotion::System::ITaskCont
 
     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. */
+    static constexpr std::size_t m_angularVelocitySize{3}; /**< Size of the angular velocity vector. */
 
     bool m_isInitialized{false}; /**< True if the task has been initialized. */
     bool m_isValid{false}; /**< True if the task is valid. */

src/IK/src/SE3Task.cpp

@@ -189,7 +189,7 @@ bool SE3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandl
         // compute the DoFs associated to the task
         m_linearDoFs = std::count(m_mask.begin(), m_mask.end(), true);
 
-        m_DoFs = m_linearDoFs + m_linearVelocitySize;
+        m_DoFs = m_linearDoFs + m_angularVelocitySize;
 
         // Update the mask description
         maskDescription.clear();

src/TSID/include/BipedalLocomotion/TSID/SE3Task.h

@@ -63,13 +63,23 @@ class SE3Task : public TSIDLinearTask, public BipedalLocomotion::System::ITaskCo
     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. */
+    static constexpr std::size_t m_angularVelocitySize{3}; /**< Size of the angular velocity vector. */
+
 
     bool m_isInitialized{false}; /**< True if the task has been initialized. */
     bool m_isValid{false}; /**< True if the task is valid. */
 
     std::shared_ptr<iDynTree::KinDynComputations> m_kinDyn; /**< Pointer to a KinDynComputations
                                                                object */
 
+    /** Mask used to select the DoFs controlled by the task */
+    std::array<bool, m_linearVelocitySize> 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 */
+
     /** State of the proportional derivative controller implemented in the task */
     Mode m_controllerMode{Mode::Enable};
 
@@ -86,6 +96,7 @@ class SE3Task : public TSIDLinearTask, public BipedalLocomotion::System::ITaskCo
      * |             `kd_linear`            | `double` or `vector<double>` |                         Gains of the linear velocity controller                         |    Yes    |
      * |            `kp_angular`            | `double` or `vector<double>` |                           Gain of the orientation controller                           |    Yes    |
      * |            `kd_angular`            | `double` or `vector<double>` |                         Gain of the angular velocity controller                        |    Yes    |
+     * |               `mask`               | `vector<bool>` |  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.
      */
     bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;

src/TSID/src/SE3Task.cpp

@@ -46,9 +46,10 @@ 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_robotAccelerationVariable.size);
 
     return true;
 }
@@ -72,6 +73,13 @@ bool SE3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandl
     std::string frameName = "Unknown";
     constexpr auto descriptionPrefix = "SE3Task Optimal Control Element - 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())
     {
         log()->error("{}, [{} {}] KinDynComputations object is not valid.",
@@ -187,8 +195,32 @@ bool SE3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandl
 
     m_SO3Controller.setGains(kpAngular, kdAngular);
 
-    // set the description
-    m_description = std::string(descriptionPrefix) + frameName + ".";
+    std::vector<bool> mask;
+    if (!ptr->getParameter("mask", mask) || (mask.size() != m_linearVelocitySize))
+    {
+        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_linearDoFs = std::count(m_mask.begin(), m_mask.end(), true);
+
+        m_DoFs = m_linearDoFs + m_angularVelocitySize;
+
+        // Update the mask description
+        maskDescription.clear();
+        for(const auto flag : m_mask)
+        {
+            maskDescription += boolToString(flag);
+        }
+    }
+
+    m_description = descriptionPrefix + frameName + " Mask:" + maskDescription + ".";
 
     m_isInitialized = true;
 
@@ -212,7 +244,7 @@ bool SE3Task::update()
             return controller.getFeedForward().coeffs();
     };
 
-    m_b = -iDynTree::toEigen(m_kinDyn->getFrameBiasAcc(m_frameIndex));
+    Eigen::Matrix<double, 6, 1> bFullDoF = -iDynTree::toEigen(m_kinDyn->getFrameBiasAcc(m_frameIndex));
 
     m_SO3Controller.setState(BipedalLocomotion::Conversions::toManifRot(
                                  m_kinDyn->getWorldTransform(m_frameIndex).getRotation()),
@@ -223,21 +255,51 @@ bool SE3Task::update()
                                 m_kinDyn->getWorldTransform(m_frameIndex).getPosition()),
                             iDynTree::toEigen(m_kinDyn->getFrameVel(m_frameIndex).getLinearVec3()));
 
-    // update the controller ouptut
+    // update the controller output
     m_SO3Controller.computeControlLaw();
     m_R3Controller.computeControlLaw();
 
     // get the output
-    m_b.head<3>() += getControllerOutupt(m_R3Controller);
-    m_b.tail<3>() += getControllerOutupt(m_SO3Controller);
+    bFullDoF.head<3>() += getControllerOutupt(m_R3Controller);
+    bFullDoF.tail<3>() += getControllerOutupt(m_SO3Controller);
 
-    if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex,
+    m_b.tail<3>() = bFullDoF.tail<3>();
+
+    if (m_DoFs == m_linearVelocitySize)
+    {
+        m_b.head<3>() = bFullDoF.head<3>();
+
+        if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex,
                                                 this->subA(m_robotAccelerationVariable)))
+        {
+            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;
+        if(!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex, m_jacobian))
+        {
+            log()->error("[SE3Task::update] Unable to get the jacobian.");
+            return m_isValid;
+        }
+
+        int index = 0;
+        for (std::size_t i = 0; i < m_linearVelocitySize; i++)
+        {
+            if (m_mask[i])
+            {
+                m_b(index) = bFullDoF(i);
+                toEigen(this->subA(m_robotAccelerationVariable)).row(index) = m_jacobian.row(i);
+                index++;
+            }
+        }
+
+        // take the all angular part
+        iDynTree::toEigen(this->subA(m_robotAccelerationVariable)).bottomRows<3>() = m_jacobian.bottomRows<3>();
     }
 
+
+
     m_isValid = true;
     return m_isValid;
 }
@@ -258,7 +320,7 @@ bool SE3Task::setSetPoint(const manif::SE3d& I_H_F,
 
 std::size_t SE3Task::size() const
 {
-    return m_spatialVelocitySize;
+    return m_DoFs;
 }
 
 SE3Task::Type SE3Task::type() const

src/TSID/tests/SE3TaskTest.cpp

@@ -46,44 +46,44 @@ TEST_CASE("SE3 Task")
 
     for (std::size_t numberOfJoints = 6; numberOfJoints < 40; 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(robotAcceleration, model.getNrOfDOFs() + 6);
-            variablesHandler.addVariable("dummy2", 15);
+        // Instantiate the handler
+        VariablesHandler variablesHandler;
+        variablesHandler.addVariable("dummy1", 10);
+        variablesHandler.addVariable(robotAcceleration, 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")
+        {
             SE3Task task;
             REQUIRE(task.setKinDyn(kinDyn));
             REQUIRE(task.initialize(parameterHandler));
@@ -149,5 +149,106 @@ TEST_CASE("SE3 Task")
 
             REQUIRE(b.isApprox(expectedB));
         }
+
+        DYNAMIC_SECTION("Model with " << numberOfJoints << " joints - [mask]")
+        {
+            const auto mask = std::vector<bool>{true, false, true};
+            const auto DoFs = std::count(mask.begin(), mask.end(), true) + 3;
+
+            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();
+            const auto desiredAcceleration = manif::SE3d::Tangent::Random();
+
+            REQUIRE(task.setSetPoint(desiredPose, desiredVelocity, desiredAcceleration));
+
+            REQUIRE(task.update());
+            REQUIRE(task.isValid());
+
+            // get A and b
+            Eigen::Ref<const Eigen::MatrixXd> A = task.getA();
+            Eigen::Ref<const Eigen::VectorXd> 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 < 3; i++)
+            {
+                if (mask[i])
+                {
+                    jacobianWithMask.row(index) = jacobian.row(i);
+                    index++;
+                }
+            }
+
+            jacobianWithMask.bottomRows(3) = jacobian.bottomRows(3);
+            REQUIRE(A.middleCols(variablesHandler.getVariable(robotAcceleration).offset,
+                        variablesHandler.getVariable(robotAcceleration).size)
+                        .isApprox(jacobianWithMask));
+
+            // check the vector b
+            LieGroupControllers::ProportionalDerivativeControllerSO3d SO3Controller;
+            LieGroupControllers::ProportionalDerivativeControllerR3d R3Controller;
+            SO3Controller.setGains(kp, kd);
+            R3Controller.setGains(kp, kd);
+
+            SO3Controller.setFeedForward(desiredAcceleration.ang());
+            R3Controller.setFeedForward(desiredAcceleration.lin());
+            SO3Controller.setDesiredState(desiredPose.quat(), desiredVelocity.ang());;
+            R3Controller.setDesiredState(desiredPose.translation(), desiredVelocity.lin());
+
+            SO3Controller.setState(BipedalLocomotion::Conversions::toManifRot(
+                                       kinDyn->getWorldTransform(controlledFrame).getRotation()),
+                                   iDynTree::toEigen(
+                                       kinDyn->getFrameVel(controlledFrame).getAngularVec3()));
+
+            R3Controller.setState(iDynTree::toEigen(
+                                      kinDyn->getWorldTransform(controlledFrame).getPosition()),
+                                  iDynTree::toEigen(
+                                      kinDyn->getFrameVel(controlledFrame).getLinearVec3()));
+
+            SO3Controller.computeControlLaw();
+            R3Controller.computeControlLaw();
+
+            Eigen::VectorXd expectedBFull(6);
+            expectedBFull = -iDynTree::toEigen(kinDyn->getFrameBiasAcc(controlledFrame));
+            expectedBFull.head<3>() += R3Controller.getControl().coeffs();
+            expectedBFull.tail<3>() += SO3Controller.getControl().coeffs();
+
+            Eigen::VectorXd expectedB(DoFs);
+
+            index = 0;
+            for(int i = 0; i < 3; i++)
+            {
+                if(mask[i])
+                {
+                    expectedB(index) = expectedBFull[i];
+                    index++;
+                }
+            }
+
+            expectedB.bottomRows(3) = expectedBFull.bottomRows(3);
+
+            REQUIRE(b.isApprox(expectedB));
+        }
     }
 }