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) 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]"; 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..3950b5866f --- /dev/null +++ b/src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h @@ -0,0 +1,125 @@ +/** + * @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. + */ +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; + + /** + * Set if the contact is active or not. + * @param isActive true if the contact is active, false otherwise. + */ + void setContactActive(bool isActive); + + /** + * 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..f970c85a04 --- /dev/null +++ b/src/TSID/src/FeasibleContactWrenchTask.cpp @@ -0,0 +1,216 @@ +/** + * @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 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; + 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::setContactActive(bool isActive) +{ + 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 +{ + return m_A.rows(); +} + +FeasibleContactWrenchTask::Type FeasibleContactWrenchTask::type() const +{ + return Type::inequality; +} + +bool FeasibleContactWrenchTask::isValid() const +{ + return m_isValid; +} 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..ad82fdccbe --- /dev/null +++ b/src/TSID/tests/FeasibleContactWrenchTaskTest.cpp @@ -0,0 +1,152 @@ +/** + * @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 + const bool isActive = false; + task.setContactActive(isActive); + 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))); + } + } +}