From cf7c50d357cae3c2dcb732f51f7c4ae3afabb59a Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 14 Jul 2021 20:36:14 +0200 Subject: [PATCH 1/7] Require a const ParametersHandler in LinearizedFrictionCone and ContactWrenchCone class --- src/Math/include/BipedalLocomotion/Math/ContactWrenchCone.h | 2 +- .../include/BipedalLocomotion/Math/LinearizedFrictionCone.h | 2 +- src/Math/src/ContactWrenchCone.cpp | 2 +- src/Math/src/LinearizedFrictionCone.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Math/include/BipedalLocomotion/Math/ContactWrenchCone.h b/src/Math/include/BipedalLocomotion/Math/ContactWrenchCone.h index 414aefb926..39d2a8c5ed 100644 --- a/src/Math/include/BipedalLocomotion/Math/ContactWrenchCone.h +++ b/src/Math/include/BipedalLocomotion/Math/ContactWrenchCone.h @@ -78,7 +78,7 @@ class ContactWrenchCone * | `foot_limits_y` | `vector` | y coordinate of the foot limits w.r.t the frame attached to the surface | Yes | * @return true in case of success/false otherwise. */ - bool initialize(std::weak_ptr handler); + bool initialize(std::weak_ptr handler); /** * Get the matrix A. diff --git a/src/Math/include/BipedalLocomotion/Math/LinearizedFrictionCone.h b/src/Math/include/BipedalLocomotion/Math/LinearizedFrictionCone.h index ce13425bc4..e25c8eff04 100644 --- a/src/Math/include/BipedalLocomotion/Math/LinearizedFrictionCone.h +++ b/src/Math/include/BipedalLocomotion/Math/LinearizedFrictionCone.h @@ -49,7 +49,7 @@ class LinearizedFrictionCone * | `static_friction_coefficient` | `double` | Static friction coefficient. | * @return true in case of success/false otherwise. */ - bool initialize(std::weak_ptr handler); + bool initialize(std::weak_ptr handler); /** * Get the matrix A. diff --git a/src/Math/src/ContactWrenchCone.cpp b/src/Math/src/ContactWrenchCone.cpp index 0b4b972915..f8e91eb66d 100644 --- a/src/Math/src/ContactWrenchCone.cpp +++ b/src/Math/src/ContactWrenchCone.cpp @@ -17,7 +17,7 @@ using namespace BipedalLocomotion::Math; -bool ContactWrenchCone::initialize(std::weak_ptr handler) +bool ContactWrenchCone::initialize(std::weak_ptr handler) { constexpr auto errorPrefix = "[ContactWrenchCone::initialize]"; constexpr int wrenchSize = Wrench::SizeAtCompileTime; diff --git a/src/Math/src/LinearizedFrictionCone.cpp b/src/Math/src/LinearizedFrictionCone.cpp index 58bb4622c5..d043a5fe55 100644 --- a/src/Math/src/LinearizedFrictionCone.cpp +++ b/src/Math/src/LinearizedFrictionCone.cpp @@ -12,7 +12,7 @@ using namespace BipedalLocomotion::Math; -bool LinearizedFrictionCone::initialize(std::weak_ptr handler) +bool LinearizedFrictionCone::initialize(std::weak_ptr handler) { constexpr auto errorPrefix = "[LinearizedFrictionCone::initialize]"; From e2cb91d52e3c7da932f7f31488aab40e848d45ed Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 14 Jul 2021 20:36:37 +0200 Subject: [PATCH 2/7] Implement FeasibleContactWrenchTask for TSID component --- src/TSID/CMakeLists.txt | 2 + .../TSID/FeasibleContactWrenchTask.h | 132 +++++++++++ src/TSID/src/FeasibleContactWrenchTask.cpp | 215 ++++++++++++++++++ 3 files changed, 349 insertions(+) create mode 100644 src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h create mode 100644 src/TSID/src/FeasibleContactWrenchTask.cpp diff --git a/src/TSID/CMakeLists.txt b/src/TSID/CMakeLists.txt index 30dd7b586a..236616ead9 100644 --- a/src/TSID/CMakeLists.txt +++ b/src/TSID/CMakeLists.txt @@ -11,9 +11,11 @@ if(FRAMEWORK_COMPILE_TSID) PUBLIC_HEADERS ${H_PREFIX}/TSIDLinearTask.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/BaseDynamicsTask.h ${H_PREFIX}/JointDynamicsTask.h ${H_PREFIX}/TaskSpaceInverseDynamics.h + ${H_PREFIX}/FeasibleContactWrenchTask.h ${H_PREFIX}/QPFixedBaseTSID.h ${H_PREFIX}/QPTSID.h SOURCES src/SO3Task.cpp src/SE3Task.cpp src/JointTrackingTask.cpp src/CoMTask.cpp src/BaseDynamicsTask.cpp src/JointDynamicsTask.cpp + src/FeasibleContactWrenchTask.cpp src/QPFixedBaseTSID.cpp src/QPTSID.cpp PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::Contacts diff --git a/src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h b/src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h new file mode 100644 index 0000000000..b12883f9f8 --- /dev/null +++ b/src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h @@ -0,0 +1,132 @@ +/** + * @file FeasibleContactWrenchTask.h + * @authors Giulio Romualdi + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_TSID_FEASIBLE_CONTACT_WRENCH_TASK_H +#define BIPEDAL_LOCOMOTION_TSID_FEASIBLE_CONTACT_WRENCH_TASK_H + +#include + +#include + +#include +#include + + +#include + + +namespace BipedalLocomotion +{ +namespace TSID +{ + +/** + * FeasibleContactWrenchTask is a concrete implementation of the TSIDLinearTask. Please use this + * element if you want to ensure that the contact wrench satisfies the constraints implemented in + * BipedalLocomotion::Math::ContactWrenchCone. Differently from the + * BipedalLocomotion::Math::ContactWrenchCone class, FeasibleContactWrenchTask requires also the + * positivstellensatz of the normal force written in local coordinate. + * @note FeasibleContactWrenchTask::disable will force the wrench associated to the task to be equal + * to a null wrench. FeasibleContactWrenchTask::enable will allow the wrench to be different from + * zero. + */ +class FeasibleContactWrenchTask : public TSIDLinearTask +{ + struct ContactWrench + { + iDynTree::FrameIndex frameIndex; /**< Frame used to express the contact wrench */ + System::VariablesHandler::VariableDescription variable; /**< Variable describing the contact + wrench */ + Eigen::Matrix3d frame_R_inertial; + }; + + ContactWrench m_contactWrench; + + BipedalLocomotion::Math::ContactWrenchCone m_cone; + + bool m_isInitialized{false}; /**< True if the task has been initialized. */ + bool m_isValid{false}; /**< True if the task is valid. */ + + Eigen::MatrixXd m_AinBodyCoordinate; /**< Matrix A written in body frame */ + + std::shared_ptr m_kinDyn; /**< Pointer to a KinDynComputations + object */ +public: + + /** + * Initialize the task. + * @param handler pointer to the parameter handler. + * @note The following parameters are required: + * | Parameter Name | Type | Description | Mandatory | + * |:-----------------------------:|:----------------:|:-----------------------------------------------------------------------------:|:---------:| + * | `frame_name` | `string` | Name of the frame associated to the contact | Yes | + * | `variable_name` | `string` | Name of the variable contained in `VariablesHandler` describing the contact | Yes | + * | `number_of_slices` | `int` | Number of slices used to split 90 deg. | Yes | + * | `static_friction_coefficient` | `double` | Static friction coefficient. | Yes | + * | `foot_limits_x` | `vector` | x coordinate of the foot limits w.r.t the frame attached to the surface | Yes | + * | `foot_limits_y` | `vector` | y coordinate of the foot limits w.r.t the frame attached to the surface | Yes | + * @return true in case of success/false otherwise. + */ + bool initialize(std::weak_ptr paramHandler); + + /** + * Set the kinDynComputations object. + * @param kinDyn pointer to a kinDynComputations object. + * @return True in case of success, false otherwise. + */ + bool setKinDyn(std::shared_ptr kinDyn); + + /** + * Set the set of variables required by the task. The variables are stored in the + * System::VariablesHandler. + * @param variablesHandler reference to a variables handler. + * @note The handler must contain a variable named as the parameter + * `variable_name` stored in the parameter handler. The variable represents the contact wrench + * acting of the robot. + * @return True in case of success, false otherwise. + */ + bool setVariablesHandler(const System::VariablesHandler& variablesHandler) override; + + /** + * Update the content of the element. + * @return True in case of success, false otherwise. + */ + bool update() override; + + /** + * Enable the task. I.e. a contact wrench different from zero is allowed. + */ + void enable(); + + /** + * Disable the task. I.e. the contact wrench is forced to be equal to zero. + */ + void disable(); + + /** + * Get the size of the task. (I.e the number of rows of the vector b) + * @return the size of the task. + */ + std::size_t size() const override; + + /** + * The CoMTask is an equality task. + * @return the size of the task. + */ + Type type() const override; + + /** + * Determines the validity of the objects retrieved with getA() and getB() + * @return True if the objects are valid, false otherwise. + */ + bool isValid() const override; +}; + +} // namespace TSID +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_TSID_FEASIBLE_CONTACT_WRENCH_TASK_H diff --git a/src/TSID/src/FeasibleContactWrenchTask.cpp b/src/TSID/src/FeasibleContactWrenchTask.cpp new file mode 100644 index 0000000000..6930f9ee61 --- /dev/null +++ b/src/TSID/src/FeasibleContactWrenchTask.cpp @@ -0,0 +1,215 @@ +/** + * @file FeasibleContactWrenchTask.cpp + * @authors Giulio Romualdi + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include +#include +#include + +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::TSID; + +bool FeasibleContactWrenchTask::setKinDyn(std::shared_ptr kinDyn) +{ + if ((kinDyn == nullptr) || (!kinDyn->isValid())) + { + log()->error("[FeasibleContactWrenchTask::setKinDyn] Invalid kinDyn object."); + return false; + } + + m_kinDyn = kinDyn; + return true; +} + +bool FeasibleContactWrenchTask::setVariablesHandler(const System::VariablesHandler& variablesHandler) +{ + constexpr auto errorPrefix = "[FeasibleContactWrenchTask::setVariablesHandler]"; + + if (!m_isInitialized) + { + log()->error("{} The task is not initialized. Please call initialize method.", errorPrefix); + return false; + } + + if (!variablesHandler.getVariable(m_contactWrench.variable.name, m_contactWrench.variable)) + { + log()->error("{} Error while retrieving the contact variable named {}.", + errorPrefix, + m_contactWrench.variable.name); + return false; + } + + if (m_contactWrench.variable.size != BipedalLocomotion::Math::Wrenchd::SizeAtCompileTime) + { + log()->error("{} The variable size associated to the contact named {} is different " + "from {}.", + errorPrefix, + m_contactWrench.variable.size, + BipedalLocomotion::Math::Wrenchd::SizeAtCompileTime); + return false; + } + + // the additional constrains refer to the normal force + constexpr std::size_t normalForceFeasibilityConstraints = 2; + const auto rowsOfConeMatrix = m_cone.getA().rows(); + + // resize the matrices + m_A.resize(normalForceFeasibilityConstraints + rowsOfConeMatrix, + variablesHandler.getNumberOfVariables()); + m_A.setZero(); + m_b.resize(normalForceFeasibilityConstraints + rowsOfConeMatrix); + + // the vector b will contains the cone vector plus the constraint related to the contact normal + // force + // 0 <= fz <= max_normal_force + // [0 0 -1 0 0 0 ] * [fx fy fz toux tauy tauz]' <= 0 + // [0 0 1 0 0 0 ] * [fx fy fz toux tauy tauz]' <= max_normal_force + // if the task is enabled max_normal_force is equal to the max double. If the task is disable + // max_normal_force is equal to 0 + m_b.head(rowsOfConeMatrix) = m_cone.getB(); + m_b.tail<2>()(0) = 0; + m_b.tail<2>()(1) = std::numeric_limits::max(); + + // the matrix A in body coordinate is fixed + m_AinBodyCoordinate.resize(normalForceFeasibilityConstraints + rowsOfConeMatrix, + BipedalLocomotion::Math::Wrenchd::SizeAtCompileTime); + m_AinBodyCoordinate.topRows(rowsOfConeMatrix) = m_cone.getA(); + m_AinBodyCoordinate.bottomRows<2>() << 0, 0, -1, 0, 0, 0, + 0, 0, 1, 0, 0, 0; + + return true; +} + +bool FeasibleContactWrenchTask::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[FeasibleContactWrenchTask::initialize]"; + + if (m_kinDyn == nullptr || !m_kinDyn->isValid()) + { + log()->error("{} KinDynComputations object is not valid.", errorPrefix); + return false; + } + + if (m_kinDyn->getFrameVelocityRepresentation() + != iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION) + { + log()->error("{} The task supports only quantities expressed in MIXED representation. " + "Please provide a KinDynComputations with Frame velocity representation set " + "to MIXED_REPRESENTATION.", + errorPrefix); + return false; + } + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + std::string frameName; + if (!ptr->getParameter("frame_name", frameName) + || (m_contactWrench.frameIndex = m_kinDyn->model().getFrameIndex(frameName)) + == iDynTree::FRAME_INVALID_INDEX) + { + log()->error("{} Error while retrieving the frame associated to the contact.", errorPrefix); + return false; + } + + if (!ptr->getParameter("variable_name", m_contactWrench.variable.name)) + { + log()->error("{} Error while retrieving the variable name associated to the contact.", + errorPrefix); + return false; + } + + // initialize the cone + if (!m_cone.initialize(paramHandler)) + { + log()->error("{} Unable to initialize the cone.", errorPrefix); + return false; + } + + // set the description + m_description = "Feasible contact wrench - Frame: " + frameName + "."; + + m_isInitialized = true; + + return true; +} + +bool FeasibleContactWrenchTask::update() +{ + constexpr auto errorPrefix = "[FeasibleContactWrenchTask::update]"; + + m_isValid = false; + + if (m_kinDyn == nullptr) + { + log()->error("{} KinDynComputations object is not valid. Please call setKinDyn().", + errorPrefix); + return false; + } + + if (!m_contactWrench.variable.isValid()) + { + log()->error("{} The contact wrench variable is not valid. Please call " + "setVariablesHandler().", + errorPrefix); + return false; + } + + if (!m_isInitialized) + { + log()->error("{} The task is not initialized. Please call initialize().", errorPrefix); + return false; + } + + using namespace iDynTree; + + m_contactWrench.frame_R_inertial + = toEigen(m_kinDyn->getWorldTransform(m_contactWrench.frameIndex).getRotation().inverse()); + + toEigen(this->subA(m_contactWrench.variable)).leftCols<3>().noalias() + = m_AinBodyCoordinate.leftCols<3>() * m_contactWrench.frame_R_inertial; + toEigen(this->subA(m_contactWrench.variable)).rightCols<3>().noalias() + = m_AinBodyCoordinate.rightCols<3>() * m_contactWrench.frame_R_inertial; + + m_isValid = true; + + // no need to update b + return m_isValid; +} + +void FeasibleContactWrenchTask::enable() +{ + // the last element of the vector b can be used to disable / enable the task + m_b.tail<1>()(0) = std::numeric_limits::max(); +} + +void FeasibleContactWrenchTask::disable() +{ + // the last element of the vector b can be used to disable / enable the task + m_b.tail<1>()(0) = 0; +} + +std::size_t FeasibleContactWrenchTask::size() const +{ + return m_A.rows(); +} + +FeasibleContactWrenchTask::Type FeasibleContactWrenchTask::type() const +{ + return Type::inequality; +} + +bool FeasibleContactWrenchTask::isValid() const +{ + return m_isValid; +} From fc63fc7a4cd996c2826ccaab935e247da798b56a Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 14 Jul 2021 20:37:25 +0200 Subject: [PATCH 3/7] Implement FeasibleContactWrench task test --- src/TSID/tests/CMakeLists.txt | 5 + .../tests/FeasibleContactWrenchTaskTest.cpp | 151 ++++++++++++++++++ 2 files changed, 156 insertions(+) create mode 100644 src/TSID/tests/FeasibleContactWrenchTaskTest.cpp diff --git a/src/TSID/tests/CMakeLists.txt b/src/TSID/tests/CMakeLists.txt index 65ab7ea449..6162afecad 100644 --- a/src/TSID/tests/CMakeLists.txt +++ b/src/TSID/tests/CMakeLists.txt @@ -27,6 +27,11 @@ add_bipedal_test( SOURCES DynamicsTaskTest.cpp LINKS BipedalLocomotion::TSID) +add_bipedal_test( + NAME FeasibleContactWrenchTaskTSID + SOURCES FeasibleContactWrenchTaskTest.cpp + LINKS BipedalLocomotion::TSID) + add_bipedal_test( NAME QPFixedBaseTSID SOURCES QPFixedBaseTSIDTest.cpp diff --git a/src/TSID/tests/FeasibleContactWrenchTaskTest.cpp b/src/TSID/tests/FeasibleContactWrenchTaskTest.cpp new file mode 100644 index 0000000000..0c88a79d16 --- /dev/null +++ b/src/TSID/tests/FeasibleContactWrenchTaskTest.cpp @@ -0,0 +1,151 @@ +/** + * @file JointsTrackingTaskTest.cpp + * @authors Giulio Romualdi + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +// Catch2 +#include + +// BipedalLocomotion +#include +#include +#include +#include +#include + +#include +#include + +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::TSID; +using namespace BipedalLocomotion::Math; + +TEST_CASE("Joints and Base dynamics tasks") +{ + const std::string robotAcceleration = "robotAcceleration"; + const std::string jointsTorque = "jointsTorque"; + + auto kinDyn = std::make_shared(); + auto parameterHandler = std::make_shared(); + + std::vectorlimitsX{-0.05, 0.1}; + std::vectorlimitsY{-0.02, 0.01}; + parameterHandler->setParameter("number_of_slices", 2); + parameterHandler->setParameter("static_friction_coefficient", 0.3); + parameterHandler->setParameter("foot_limits_x", limitsX); + parameterHandler->setParameter("foot_limits_y", limitsY); + + // set the velocity representation + REQUIRE(kinDyn->setFrameVelocityRepresentation( + iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)); + + 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)); + + 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 : jointsVel) + { + joint = iDynTree::getRandomDouble(); + } + + for (auto& element : gravity) + { + element = iDynTree::getRandomDouble(); + } + + REQUIRE(kinDyn->setRobotState(worldBasePos, jointsPos, baseVel, jointsVel, gravity)); + + // Instantiate the handler + VariablesHandler variablesHandler; + variablesHandler.addVariable("dummy1", 10); + variablesHandler.addVariable("contact0", 6); + variablesHandler.addVariable("dummy2", 4); + + const std::string frameName = model.getFrameName(numberOfJoints); + parameterHandler->setParameter("variable_name", "contact0"); + parameterHandler->setParameter("frame_name", frameName); + + FeasibleContactWrenchTask task; + REQUIRE(task.setKinDyn(kinDyn)); + REQUIRE(task.initialize(parameterHandler)); + REQUIRE(task.setVariablesHandler(variablesHandler)); + REQUIRE(task.update()); + + // get A and b + Eigen::Ref A = task.getA(); + Eigen::Ref b = task.getB(); + + // check the matrix A + // + BipedalLocomotion::Math::ContactWrenchCone cone; + cone.initialize(parameterHandler); + const std::size_t numberOfConstraints = cone.getB().rows(); + Eigen::MatrixXd coneWithNormalForceA(numberOfConstraints + 2, 6); + + coneWithNormalForceA.topRows(numberOfConstraints) = cone.getA(); + coneWithNormalForceA.bottomRows(2) << 0, 0, -1, 0, 0, 0, + 0, 0, 1, 0, 0, 0; + + Eigen::VectorXd coneWithNormalForceB(numberOfConstraints + 2); + coneWithNormalForceB.head(numberOfConstraints) = cone.getB(); + coneWithNormalForceB.tail<2>() << 0, std::numeric_limits::max(); + + const auto frame_R_world = kinDyn->getWorldTransform(frameName).getRotation().inverse(); + const auto frame_T_world_rotationOnly + = iDynTree::Transform(frame_R_world, iDynTree::Position::Zero()); + const auto adjointMatrix = frame_T_world_rotationOnly.asAdjointTransform(); + + REQUIRE(A.middleCols(variablesHandler.getVariable("dummy1").offset, + variablesHandler.getVariable("dummy1").size) + .isZero()); + + REQUIRE(A.middleCols(variablesHandler.getVariable("dummy2").offset, + variablesHandler.getVariable("dummy2").size) + .isZero()); + + REQUIRE(A.middleCols(variablesHandler.getVariable("contact0").offset, + variablesHandler.getVariable("contact0").size) + .isApprox(coneWithNormalForceA * iDynTree::toEigen(adjointMatrix))); + + // check the vector b + REQUIRE(b.isApprox(coneWithNormalForceB)); + + // disable the task + task.disable(); + coneWithNormalForceB.tail(1)(0) = 0; + REQUIRE(b.isApprox(coneWithNormalForceB)); + + REQUIRE(A.middleCols(variablesHandler.getVariable("dummy1").offset, + variablesHandler.getVariable("dummy1").size) + .isZero()); + + REQUIRE(A.middleCols(variablesHandler.getVariable("dummy2").offset, + variablesHandler.getVariable("dummy2").size) + .isZero()); + + REQUIRE(A.middleCols(variablesHandler.getVariable("contact0").offset, + variablesHandler.getVariable("contact0").size) + .isApprox(coneWithNormalForceA * iDynTree::toEigen(adjointMatrix))); + } + } +} From 5bff02ec2dd5058d6dc4150e6f91f87481f53818 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 14 Jul 2021 21:53:49 +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 619b4a05ce..01635fbbaf 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ All notable changes to this project are documented in this file. - Implement `skew` function in Math component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/352) - Implement `QPTSID` class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/366) - Implement motor pwm, motor encoders, wbd joint torque estimates, pid reading in `YarpSensorBridge`(https://github.com/dic-iit/bipedal-locomotion-framework/pull/359). +- Implement FeasibleContactWrenchTask for TSID component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/369). ### Changed - Add common Python files to gitignore (https://github.com/dic-iit/bipedal-locomotion-framework/pull/338) From cdd7472051e84120891573dba2669e362f764c1c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 20 Jul 2021 19:19:57 +0200 Subject: [PATCH 5/7] Fix typos in FeasibleContactWrenchTask Co-authored-by: Stefano Dafarra --- src/TSID/src/FeasibleContactWrenchTask.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/TSID/src/FeasibleContactWrenchTask.cpp b/src/TSID/src/FeasibleContactWrenchTask.cpp index 6930f9ee61..73d87576b1 100644 --- a/src/TSID/src/FeasibleContactWrenchTask.cpp +++ b/src/TSID/src/FeasibleContactWrenchTask.cpp @@ -68,9 +68,9 @@ bool FeasibleContactWrenchTask::setVariablesHandler(const System::VariablesHandl // the vector b will contains the cone vector plus the constraint related to the contact normal // force // 0 <= fz <= max_normal_force - // [0 0 -1 0 0 0 ] * [fx fy fz toux tauy tauz]' <= 0 - // [0 0 1 0 0 0 ] * [fx fy fz toux tauy tauz]' <= max_normal_force - // if the task is enabled max_normal_force is equal to the max double. If the task is disable + // [0 0 -1 0 0 0 ] * [fx fy fz taux tauy tauz]' <= 0 + // [0 0 1 0 0 0 ] * [fx fy fz taux tauy tauz]' <= max_normal_force + // if the task is enabled max_normal_force is equal to the max double. If the task is disabled // max_normal_force is equal to 0 m_b.head(rowsOfConeMatrix) = m_cone.getB(); m_b.tail<2>()(0) = 0; From a2f89df4bbe9004c02b8ceea0a8f984f49b15426 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 20 Jul 2021 19:33:02 +0200 Subject: [PATCH 6/7] Implement FeasibleContactWrenchTask::setContactActive() --- .../TSID/FeasibleContactWrenchTask.h | 13 +++---------- src/TSID/src/FeasibleContactWrenchTask.cpp | 19 ++++++++++--------- 2 files changed, 13 insertions(+), 19 deletions(-) diff --git a/src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h b/src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h index b12883f9f8..3950b5866f 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h +++ b/src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h @@ -30,9 +30,6 @@ namespace TSID * BipedalLocomotion::Math::ContactWrenchCone. Differently from the * BipedalLocomotion::Math::ContactWrenchCone class, FeasibleContactWrenchTask requires also the * positivstellensatz of the normal force written in local coordinate. - * @note FeasibleContactWrenchTask::disable will force the wrench associated to the task to be equal - * to a null wrench. FeasibleContactWrenchTask::enable will allow the wrench to be different from - * zero. */ class FeasibleContactWrenchTask : public TSIDLinearTask { @@ -98,14 +95,10 @@ class FeasibleContactWrenchTask : public TSIDLinearTask bool update() override; /** - * Enable the task. I.e. a contact wrench different from zero is allowed. + * Set if the contact is active or not. + * @param isActive true if the contact is active, false otherwise. */ - void enable(); - - /** - * Disable the task. I.e. the contact wrench is forced to be equal to zero. - */ - void disable(); + void setContactActive(bool isActive); /** * Get the size of the task. (I.e the number of rows of the vector b) diff --git a/src/TSID/src/FeasibleContactWrenchTask.cpp b/src/TSID/src/FeasibleContactWrenchTask.cpp index 73d87576b1..f970c85a04 100644 --- a/src/TSID/src/FeasibleContactWrenchTask.cpp +++ b/src/TSID/src/FeasibleContactWrenchTask.cpp @@ -187,16 +187,17 @@ bool FeasibleContactWrenchTask::update() return m_isValid; } -void FeasibleContactWrenchTask::enable() +void FeasibleContactWrenchTask::setContactActive(bool isActive) { - // the last element of the vector b can be used to disable / enable the task - m_b.tail<1>()(0) = std::numeric_limits::max(); -} - -void FeasibleContactWrenchTask::disable() -{ - // the last element of the vector b can be used to disable / enable the task - m_b.tail<1>()(0) = 0; + if (isActive) + { + // the last element of the vector b can be used to disable / enable the task + m_b.tail<1>()(0) = std::numeric_limits::max(); + } else + { + // the last element of the vector b can be used to disable / enable the task + m_b.tail<1>()(0) = 0; + } } std::size_t FeasibleContactWrenchTask::size() const From 7ffb5254ada5d454234630e1d61b6f31f4e6eacc Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 20 Jul 2021 19:33:26 +0200 Subject: [PATCH 7/7] Update the FeasibleContactWrenchTask --- src/TSID/tests/FeasibleContactWrenchTaskTest.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/TSID/tests/FeasibleContactWrenchTaskTest.cpp b/src/TSID/tests/FeasibleContactWrenchTaskTest.cpp index 0c88a79d16..ad82fdccbe 100644 --- a/src/TSID/tests/FeasibleContactWrenchTaskTest.cpp +++ b/src/TSID/tests/FeasibleContactWrenchTaskTest.cpp @@ -131,7 +131,8 @@ TEST_CASE("Joints and Base dynamics tasks") REQUIRE(b.isApprox(coneWithNormalForceB)); // disable the task - task.disable(); + const bool isActive = false; + task.setContactActive(isActive); coneWithNormalForceB.tail(1)(0) = 0; REQUIRE(b.isApprox(coneWithNormalForceB));