diff --git a/CHANGELOG.md b/CHANGELOG.md index 9f56367440..6e78f3806d 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/src/IK/include/BipedalLocomotion/IK/SE3Task.h b/src/IK/include/BipedalLocomotion/IK/SE3Task.h index 7b85c1eb5e..680b1b0ae7 100644 --- a/src/IK/include/BipedalLocomotion/IK/SE3Task.h +++ b/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. */ diff --git a/src/IK/src/SE3Task.cpp b/src/IK/src/SE3Task.cpp index 175dca0b90..78949d593a 100644 --- a/src/IK/src/SE3Task.cpp +++ b/src/IK/src/SE3Task.cpp @@ -189,7 +189,7 @@ bool SE3Task::initialize(std::weak_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}; + 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` | Gains of the linear velocity controller | Yes | * | `kp_angular` | `double` or `vector` | Gain of the orientation controller | Yes | * | `kd_angular` | `double` or `vector` | Gain of the angular velocity controller | Yes | + * | `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. */ bool initialize(std::weak_ptr paramHandler) override; diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp index 45c85f211c..4c347d776b 100644 --- a/src/TSID/src/SE3Task.cpp +++ b/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_ptrisValid()) { log()->error("{}, [{} {}] KinDynComputations object is not valid.", @@ -187,8 +195,32 @@ bool SE3Task::initialize(std::weak_ptr 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 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 diff --git a/src/TSID/tests/SE3TaskTest.cpp b/src/TSID/tests/SE3TaskTest.cpp index 50a86bd846..44a64a6a07 100644 --- a/src/TSID/tests/SE3TaskTest.cpp +++ b/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{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 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 < 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)); + } } }