From 5baca6f8d2060225facfb9107d0266405a23e8d0 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 1 Aug 2023 12:30:23 +0200 Subject: [PATCH] Added GravityTask tests and bugfix Added GravityTask to QPInverseKinematics --- .../BipedalLocomotion/IK/GravityTask.h | 4 +- src/IK/src/GravityTask.cpp | 21 ++- src/IK/tests/CMakeLists.txt | 5 + src/IK/tests/GravityTaskTest.cpp | 132 ++++++++++++++++++ src/IK/tests/QPInverseKinematicsTest.cpp | 92 +++++++++++- 5 files changed, 239 insertions(+), 15 deletions(-) create mode 100644 src/IK/tests/GravityTaskTest.cpp diff --git a/src/IK/include/BipedalLocomotion/IK/GravityTask.h b/src/IK/include/BipedalLocomotion/IK/GravityTask.h index 4a0f8a07a7..0083d2bc23 100644 --- a/src/IK/include/BipedalLocomotion/IK/GravityTask.h +++ b/src/IK/include/BipedalLocomotion/IK/GravityTask.h @@ -119,7 +119,7 @@ class GravityTask : public IKLinearTask * * The input is normalized, unless the norm is too small. */ - void setDesiredGravityDirectionInTargetFrame( + bool setDesiredGravityDirectionInTargetFrame( const Eigen::Ref desiredGravityDirection); /** @@ -128,7 +128,7 @@ class GravityTask : public IKLinearTask * * Only the first two components are used. */ - void + bool setFeedForwardVelocityInTargetFrame(const Eigen::Ref feedforwardVelocity); /** diff --git a/src/IK/src/GravityTask.cpp b/src/IK/src/GravityTask.cpp index b1f38eadb1..490241c8ce 100644 --- a/src/IK/src/GravityTask.cpp +++ b/src/IK/src/GravityTask.cpp @@ -59,14 +59,19 @@ bool GravityTask::setVariablesHandler(const System::VariablesHandler& variablesH m_A.setZero(); m_b.resize(m_DoFs); m_b.setZero(); - m_jacobian.resize(m_spatialVelocitySize, m_kinDyn->getNrOfDegreesOfFreedom()); + m_jacobian.resize(m_spatialVelocitySize, + m_spatialVelocitySize + m_kinDyn->getNrOfDegreesOfFreedom()); m_jacobian.setZero(); m_desiredZDirectionBody.setZero(); m_feedForwardBody.setZero(); m_Am.resize(2, 3); - m_Am << 1, 0, 0, 0, 1, 0; m_bm.resize(2, 3); - m_bm << 0, -1, 0, 1, 0, 0; + // clang-format off + m_Am << 1, 0, 0, + 0, 1, 0; + m_bm << 0, -1, 0, + 1, 0, 0; + // clang-format on return true; } @@ -158,24 +163,26 @@ bool GravityTask::update() return m_isValid; } - m_A = m_Am * m_jacobian.bottomRows<3>(); - m_b << m_kp * m_bm * desiredZDirectionAbsolute + feedforwardAbsolute.topRows<2>(); + toEigen(this->subA(m_robotVelocityVariable)) = m_Am * m_jacobian.bottomRows<3>(); + m_b = m_kp * m_bm * desiredZDirectionAbsolute + feedforwardAbsolute.topRows<2>(); m_isValid = true; return m_isValid; } -void GravityTask::setDesiredGravityDirectionInTargetFrame( +bool GravityTask::setDesiredGravityDirectionInTargetFrame( const Eigen::Ref desiredGravityDirection) { m_desiredZDirectionBody = desiredGravityDirection; m_desiredZDirectionBody.normalize(); + return true; } -void GravityTask::setFeedForwardVelocityInTargetFrame( +bool GravityTask::setFeedForwardVelocityInTargetFrame( const Eigen::Ref feedforwardVelocity) { m_feedForwardBody = feedforwardVelocity; + return true; } std::size_t GravityTask::size() const diff --git a/src/IK/tests/CMakeLists.txt b/src/IK/tests/CMakeLists.txt index 66cc045b67..8f63b0b735 100644 --- a/src/IK/tests/CMakeLists.txt +++ b/src/IK/tests/CMakeLists.txt @@ -42,6 +42,11 @@ add_bipedal_test( SOURCES DistanceTaskTest.cpp LINKS BipedalLocomotion::IK) +add_bipedal_test( + NAME GravityTaskIK + SOURCES GravityTaskTest.cpp + LINKS BipedalLocomotion::IK) + if (FRAMEWORK_COMPILE_ContinuousDynamicalSystem) add_bipedal_test( diff --git a/src/IK/tests/GravityTaskTest.cpp b/src/IK/tests/GravityTaskTest.cpp new file mode 100644 index 0000000000..de79deff63 --- /dev/null +++ b/src/IK/tests/GravityTaskTest.cpp @@ -0,0 +1,132 @@ +/** + * @file JointTrackingTaskTest.cpp + * @authors Giulio Romualdi + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +// Catch2 +#include + +// BipedalLocomotion +#include +#include +#include + +#include +#include + +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; + +TEST_CASE("Distance task") +{ + const std::string robotVelocity = "robotVelocity"; + + Eigen::VectorXd kp; + + auto kinDyn = std::make_shared(); + auto parameterHandler = std::make_shared(); + + parameterHandler->setParameter("robot_velocity_variable_name", robotVelocity); + + // set the velocity representation + REQUIRE(kinDyn->setFrameVelocityRepresentation( + iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)); + + 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)); + + 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(robotVelocity, model.getNrOfDOFs() + 6); + variablesHandler.addVariable("dummy2", 15); + double kp = 2.0; + parameterHandler->setParameter("kp", kp); + + std::string targetName = iDynTree::getRandomLinkOfModel(model); + parameterHandler->setParameter("target_frame_name", targetName); + + GravityTask task; + REQUIRE(task.setKinDyn(kinDyn)); + REQUIRE(task.initialize(parameterHandler)); + REQUIRE(task.setVariablesHandler(variablesHandler)); + + Eigen::Vector3d desiredDirection({1.0, 2.0, 3.0}); + desiredDirection.normalize(); + REQUIRE(task.setDesiredGravityDirectionInTargetFrame(desiredDirection)); + Eigen::Vector3d feedforward({0.1, 0.2, 0.3}); + REQUIRE(task.setFeedForwardVelocityInTargetFrame(feedforward)); + + REQUIRE(task.update()); + REQUIRE(task.isValid()); + + // get A and b + Eigen::Ref A = task.getA(); + Eigen::Ref b = task.getB(); + + Eigen::Matrix3d targetRotation; + Eigen::MatrixXd jacobian; + jacobian.setZero(6, 6 + model.getNrOfDOFs()); + + targetRotation = iDynTree::toEigen(kinDyn->getWorldTransform(targetName).getRotation()); + kinDyn->getFrameFreeFloatingJacobian(targetName, jacobian); + + Eigen::MatrixXd expectedA = jacobian.middleRows<2>(3); + + // 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()); + + REQUIRE(A.middleCols(variablesHandler.getVariable(robotVelocity).offset, + model.getNrOfDOFs() + 6) + .isApprox(expectedA)); + + // check the vector b + Eigen::VectorXd expectedB(task.size()); + Eigen::Vector3d axisAbsolute, feedforwardAbsolute; + axisAbsolute = targetRotation * desiredDirection; + feedforwardAbsolute = targetRotation * feedforward; + expectedB(0) = -kp * axisAbsolute(1) + feedforwardAbsolute(0); + expectedB(1) = kp * axisAbsolute(0) + feedforwardAbsolute(1); + REQUIRE(b.isApprox(expectedB)); + } + } +} diff --git a/src/IK/tests/QPInverseKinematicsTest.cpp b/src/IK/tests/QPInverseKinematicsTest.cpp index dde67db0c1..cb96840ba8 100644 --- a/src/IK/tests/QPInverseKinematicsTest.cpp +++ b/src/IK/tests/QPInverseKinematicsTest.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -53,6 +54,7 @@ struct InverseKinematicsAndTasks std::shared_ptr regularizationTask; std::shared_ptr jointLimitsTask; std::shared_ptr distanceTask; + std::shared_ptr gravityTask; }; struct DesiredSetPoints @@ -63,6 +65,8 @@ struct DesiredSetPoints Eigen::VectorXd joints; std::string targetFrameDistance; double targetDistance; + std::string targetFrameGravity; + Eigen::Vector3d desiredBodyGravity; }; struct System @@ -85,7 +89,8 @@ std::shared_ptr createParameterHandler() "COM_TASK", "REGULARIZATION_TASK", "JOINT_LIMITS_TASK", - "DISTANCE_TASK"}); + "DISTANCE_TASK", + "GRAVITY_TASK"}); /////// SE3 Task auto SE3ParameterHandler = std::make_shared(); @@ -129,6 +134,14 @@ std::shared_ptr createParameterHandler() distanceTaskHandler->setParameter("kp", gain); parameterHandler->setGroup("DISTANCE_TASK", distanceTaskHandler); + /////// Gravity task + auto gravityTaskHandler = std::make_shared(); + gravityTaskHandler->setParameter("robot_velocity_variable_name", robotVelocity); + gravityTaskHandler->setParameter("type", "GravityTask"); + gravityTaskHandler->setParameter("priority", 0); + gravityTaskHandler->setParameter("kp", gain / 10); + parameterHandler->setGroup("GRAVITY_TASK", gravityTaskHandler); + return parameterHandler; } @@ -213,6 +226,11 @@ InverseKinematicsAndTasks createIK(std::shared_ptr handler, REQUIRE(out.distanceTask->initialize(handler->getGroup("DISTANCE_TASK"))); REQUIRE(out.ik->addTask(out.distanceTask, "distance_task", highPriority)); + out.gravityTask = std::make_shared(); + REQUIRE(out.gravityTask->setKinDyn(kinDyn)); + REQUIRE(out.gravityTask->initialize(handler->getGroup("GRAVITY_TASK"))); + REQUIRE(out.ik->addTask(out.gravityTask, "gravity_task", highPriority)); + REQUIRE(out.ik->finalize(variables)); return out; @@ -261,6 +279,22 @@ DesiredSetPoints getDesiredReference(std::shared_ptrgetWorldTransform(out.targetFrameDistance)).translation().norm(); + iDynTree::LinkIndex index; + // Find a frame sufficiently far away from those used for the distance and SE3 task + do + { + out.targetFrameGravity = iDynTree::getRandomLinkOfModel(kinDyn->model()); + index = kinDyn->model().getFrameIndex(out.targetFrameGravity); + } while (std::abs(index - kinDyn->model().getFrameIndex(out.endEffectorFrame)) < 10 + || std::abs(index - kinDyn->model().getFrameIndex(out.targetFrameDistance)) < 10); + + out.targetFrameGravity = kinDyn->getFloatingBase(); + + // The desired gravity in body frame is the world z axis in body frame, + // that is the third row of the body rotation matrix + out.desiredBodyGravity + = toManifPose(kinDyn->getWorldTransform(out.targetFrameGravity)).rotation().matrix().row(2); + return out; } @@ -306,7 +340,7 @@ TEST_CASE("QP-IK") auto kinDyn = std::make_shared(); auto parameterHandler = createParameterHandler(); - constexpr double tolerance = 1e-1; + constexpr double tolerance = 5e-1; // set the velocity representation REQUIRE(kinDyn->setFrameVelocityRepresentation( @@ -337,6 +371,10 @@ TEST_CASE("QP-IK") .lock() ->setParameter("target_frame_name", desiredSetPoints.targetFrameDistance); + parameterHandler->getGroup("GRAVITY_TASK") + .lock() + ->setParameter("target_frame_name", desiredSetPoints.targetFrameGravity); + // create the IK constexpr double jointLimitDelta = 0.5; finalizeParameterHandler(parameterHandler, @@ -354,8 +392,11 @@ TEST_CASE("QP-IK") REQUIRE(ikAndTasks.distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + REQUIRE(ikAndTasks.gravityTask->setDesiredGravityDirectionInTargetFrame( + desiredSetPoints.desiredBodyGravity)); + // propagate the inverse kinematics for - constexpr std::size_t iterations = 35; + constexpr std::size_t iterations = 30; Eigen::Vector3d gravity; gravity << 0, 0, -9.81; Eigen::Matrix4d baseTransform = Eigen::Matrix4d::Identity(); @@ -401,10 +442,22 @@ TEST_CASE("QP-IK") const manif::SE3d::Tangent error = endEffectorPose - desiredSetPoints.endEffectorPose; REQUIRE(error.coeffs().isZero(tolerance)); + // Check the distance error REQUIRE(toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameDistance)) .translation() .norm() == Catch::Approx(desiredSetPoints.targetDistance).epsilon(tolerance * 0.1)); + + // Check the gravity error + Eigen::Vector3d gravityError; + gravityError + = toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameGravity)) + .rotation() + .matrix() + .row(2) + .transpose() + - desiredSetPoints.desiredBodyGravity; + REQUIRE(gravityError.isZero(tolerance)); } } } @@ -443,6 +496,10 @@ TEST_CASE("QP-IK [With strict limits]") .lock() ->setParameter("target_frame_name", desiredSetPoints.targetFrameDistance); + parameterHandler->getGroup("GRAVITY_TASK") + .lock() + ->setParameter("target_frame_name", desiredSetPoints.targetFrameGravity); + // create the IK constexpr double jointLimitDelta = 0.5; Eigen::VectorXd limitsDelta @@ -462,8 +519,11 @@ TEST_CASE("QP-IK [With strict limits]") REQUIRE(ikAndTasks.distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + REQUIRE(ikAndTasks.gravityTask->setDesiredGravityDirectionInTargetFrame( + desiredSetPoints.desiredBodyGravity)); + // propagate the inverse kinematics for - constexpr std::size_t iterations = 35; + constexpr std::size_t iterations = 30; Eigen::Vector3d gravity; gravity << 0, 0, -9.81; Eigen::Matrix4d baseTransform = Eigen::Matrix4d::Identity(); @@ -513,7 +573,7 @@ TEST_CASE("QP-IK [With builder]") auto kinDyn = std::make_shared(); auto parameterHandler = createParameterHandler(); - constexpr double tolerance = 1e-1; + constexpr double tolerance = 5e-1; // set the velocity representation REQUIRE(kinDyn->setFrameVelocityRepresentation( @@ -552,6 +612,10 @@ TEST_CASE("QP-IK [With builder]") .lock() ->setParameter("target_frame_name", desiredSetPoints.targetFrameDistance); + parameterHandler->getGroup("GRAVITY_TASK") + .lock() + ->setParameter("target_frame_name", desiredSetPoints.targetFrameGravity); + // finalize the parameter handler constexpr double jointLimitDelta = 0.5; finalizeParameterHandler(parameterHandler, @@ -582,8 +646,13 @@ TEST_CASE("QP-IK [With builder]") REQUIRE_FALSE(distanceTask == nullptr); REQUIRE(distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + auto gravityTask = std::dynamic_pointer_cast(ik->getTask("GRAVITY_TASK").lock()); + REQUIRE_FALSE(gravityTask == nullptr); + REQUIRE( + gravityTask->setDesiredGravityDirectionInTargetFrame(desiredSetPoints.desiredBodyGravity)); + // propagate the inverse kinematics for - constexpr std::size_t iterations = 35; + constexpr std::size_t iterations = 30; Eigen::Vector3d gravity; gravity << 0, 0, -9.81; Eigen::Matrix4d baseTransform = Eigen::Matrix4d::Identity(); @@ -633,8 +702,19 @@ TEST_CASE("QP-IK [With builder]") const manif::SE3d::Tangent error = endEffectorPose - desiredSetPoints.endEffectorPose; REQUIRE(error.coeffs().isZero(tolerance)); + // Check the distance error REQUIRE(toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameDistance)) .translation() .norm() == Catch::Approx(desiredSetPoints.targetDistance).epsilon(tolerance * 0.1)); + + // Check the gravity error + Eigen::Vector3d gravityError; + gravityError = toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameGravity)) + .rotation() + .matrix() + .row(2) + .transpose() + - desiredSetPoints.desiredBodyGravity; + REQUIRE(gravityError.isZero(tolerance)); }