From 40b9eb1ca294e60f981d7c555fa32b9472b1d78b Mon Sep 17 00:00:00 2001 From: Davide Gorbani <101326942+davidegorbani@users.noreply.github.com> Date: Wed, 4 Sep 2024 13:24:37 +0200 Subject: [PATCH] Add joint velocity limits task (#879) --- CHANGELOG.md | 1 + bindings/python/IK/CMakeLists.txt | 4 +- .../bindings/IK/JointVelocityLimitsTask.h | 26 +++ .../python/IK/src/JointVelocityLimitsTask.cpp | 38 ++++ bindings/python/IK/src/Module.cpp | 2 + .../IK/tests/test_QP_inverse_kinematics.py | 20 ++ src/IK/CMakeLists.txt | 4 +- .../IK/JointVelocityLimitsTask.h | 118 ++++++++++++ src/IK/src/JointVelocityLimitsTask.cpp | 172 ++++++++++++++++++ src/IK/tests/CMakeLists.txt | 5 + src/IK/tests/JointVelocityLimitsTaskTest.cpp | 133 ++++++++++++++ 11 files changed, 519 insertions(+), 4 deletions(-) create mode 100644 bindings/python/IK/include/BipedalLocomotion/bindings/IK/JointVelocityLimitsTask.h create mode 100644 bindings/python/IK/src/JointVelocityLimitsTask.cpp create mode 100644 src/IK/include/BipedalLocomotion/IK/JointVelocityLimitsTask.h create mode 100644 src/IK/src/JointVelocityLimitsTask.cpp create mode 100644 src/IK/tests/JointVelocityLimitsTaskTest.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index cb25ed1e34..d67d58f7b9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -26,6 +26,7 @@ All notable changes to this project are documented in this file. - Add launch parameter to `blf-logger-with-audio.sh` script to set logger launch file (https://github.com/ami-iit/bipedal-locomotion-framework/pull/867) - Add `getJointLimits` function to set get actuated joints position limits (https://github.com/ami-iit/bipedal-locomotion-framework/pull/868) - Add the possibility to disable streaming of joint encoder acceleration measurements (https://github.com/ami-iit/bipedal-locomotion-framework/pull/876) +- Add a task in the IK to set the joint velocity limits (https://github.com/ami-iit/bipedal-locomotion-framework/pull/879) ### Changed - 🤖 [ergoCubSN001] Add logging of the wrist and fix the name of the waist imu (https://github.com/ami-iit/bipedal-locomotion-framework/pull/810) diff --git a/bindings/python/IK/CMakeLists.txt b/bindings/python/IK/CMakeLists.txt index 85d20e5e1e..f4ce2a90c9 100644 --- a/bindings/python/IK/CMakeLists.txt +++ b/bindings/python/IK/CMakeLists.txt @@ -8,8 +8,8 @@ if(TARGET BipedalLocomotion::IK) add_bipedal_locomotion_python_module( NAME IKBindings - SOURCES src/IntegrationBasedIK.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/JointLimitsTask.cpp src/AngularMomentumTask.cpp src/Module.cpp src/IKLinearTask.cpp src/DistanceTask.cpp src/GravityTask.cpp - HEADERS ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h ${H_PREFIX}/GravityTask.h + SOURCES src/IntegrationBasedIK.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/JointLimitsTask.cpp src/AngularMomentumTask.cpp src/Module.cpp src/IKLinearTask.cpp src/DistanceTask.cpp src/GravityTask.cpp src/JointVelocityLimitsTask.cpp + HEADERS ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h ${H_PREFIX}/GravityTask.h ${H_PREFIX}/JointVelocityLimitsTask.h LINK_LIBRARIES BipedalLocomotion::IK MANIF::manif TESTS tests/test_QP_inverse_kinematics.py TESTS_RUNTIME_CONDITIONS FRAMEWORK_USE_icub-models diff --git a/bindings/python/IK/include/BipedalLocomotion/bindings/IK/JointVelocityLimitsTask.h b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/JointVelocityLimitsTask.h new file mode 100644 index 0000000000..5dbd99bc7f --- /dev/null +++ b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/JointVelocityLimitsTask.h @@ -0,0 +1,26 @@ +/** + * @file JointVelocityLimitsTask.h + * @authors Davide Gorbani + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_JOINT_VELOCITY_LIMITS_TASK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_IK_JOINT_VELOCITY_LIMITS_TASK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace IK +{ + +void CreateJointVelocityLimitsTask(pybind11::module& module); + +} // namespace IK +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_JOINT_VELOCITY_LIMITS_TASK_H diff --git a/bindings/python/IK/src/JointVelocityLimitsTask.cpp b/bindings/python/IK/src/JointVelocityLimitsTask.cpp new file mode 100644 index 0000000000..56f022f40b --- /dev/null +++ b/bindings/python/IK/src/JointVelocityLimitsTask.cpp @@ -0,0 +1,38 @@ +/** + * @file JointVelocityLimitsTask.cpp + * @authors Davide Gorbani + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +#include +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace IK +{ + +void CreateJointVelocityLimitsTask(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::IK; + + py::class_, + IKLinearTask>(module, "JointVelocityLimitsTask") + .def(py::init()); +} + +} // namespace IK +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/IK/src/Module.cpp b/bindings/python/IK/src/Module.cpp index c2ae7aaf82..438f5c8cd3 100644 --- a/bindings/python/IK/src/Module.cpp +++ b/bindings/python/IK/src/Module.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ void CreateModule(pybind11::module& module) CreateAngularMomentumTask(module); CreateDistanceTask(module); CreateGravityTask(module); + CreateJointVelocityLimitsTask(module); CreateIntegrationBasedIK(module); CreateQPInverseKinematics(module); } diff --git a/bindings/python/IK/tests/test_QP_inverse_kinematics.py b/bindings/python/IK/tests/test_QP_inverse_kinematics.py index 74c267cef0..c0d9c94d23 100644 --- a/bindings/python/IK/tests/test_QP_inverse_kinematics.py +++ b/bindings/python/IK/tests/test_QP_inverse_kinematics.py @@ -260,6 +260,26 @@ def test_gravity_task(): assert gravity_task.set_feedforward_velocity_in_target_frame(feedforward_velocity=[np.random.uniform(-0.5,0.5) for _ in range(3)]) assert gravity_task.set_set_point(desired_gravity_direction=[np.random.uniform(-0.5,0.5) for _ in range(3)]) +def test_joint_velocity_limits_task(): + + # create KinDynComputationsDescriptor + joints_list, kindyn = get_kindyn() + + joint_values = [np.random.uniform(-0.5,0.5) for _ in range(kindyn.getNrOfDegreesOfFreedom())] + # Set the parameters + joint_velocity_limits_param_handler = blf.parameters_handler.StdParametersHandler() + joint_velocity_limits_param_handler.set_parameter_string(name="robot_velocity_variable_name", value="robotVelocity") + joint_velocity_limits_param_handler.set_parameter_vector_float(name="upper_limits",value=np.array(joint_values) + 0.01) + joint_velocity_limits_param_handler.set_parameter_vector_float(name="lower_limits",value=np.array(joint_values) - 0.01) + + # Initialize the task + joint_velocity_limits_task = blf.ik.JointVelocityLimitsTask() + assert joint_velocity_limits_task.set_kin_dyn(kindyn) + assert joint_velocity_limits_task.initialize(param_handler=joint_velocity_limits_param_handler) + joint_limits_var_handler = blf.system.VariablesHandler() + assert joint_limits_var_handler.add_variable("robotVelocity", 32) is True # robot velocity size = 26 (joints) + 6 (base) + assert joint_velocity_limits_task.set_variables_handler(variables_handler=joint_limits_var_handler) + def test_integration_based_ik_state(): state = blf.ik.IntegrationBasedIKState() diff --git a/src/IK/CMakeLists.txt b/src/IK/CMakeLists.txt index cdd27e57fc..9a91bb617f 100644 --- a/src/IK/CMakeLists.txt +++ b/src/IK/CMakeLists.txt @@ -10,9 +10,9 @@ if(FRAMEWORK_COMPILE_IK) NAME IK PUBLIC_HEADERS ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/QPFixedBaseInverseKinematics.h - ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h ${H_PREFIX}/GravityTask.h + ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h ${H_PREFIX}/GravityTask.h ${H_PREFIX}/JointVelocityLimitsTask.h SOURCES src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/CoMTask.cpp src/AngularMomentumTask.cpp src/JointLimitsTask.cpp - src/QPInverseKinematics.cpp src/QPFixedBaseInverseKinematics.cpp src/IKLinearTask.cpp src/IntegrationBasedIK.cpp src/DistanceTask.cpp src/GravityTask.cpp + src/QPInverseKinematics.cpp src/QPFixedBaseInverseKinematics.cpp src/IKLinearTask.cpp src/IntegrationBasedIK.cpp src/DistanceTask.cpp src/GravityTask.cpp src/JointVelocityLimitsTask.cpp PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System LieGroupControllers::LieGroupControllers diff --git a/src/IK/include/BipedalLocomotion/IK/JointVelocityLimitsTask.h b/src/IK/include/BipedalLocomotion/IK/JointVelocityLimitsTask.h new file mode 100644 index 0000000000..8a3584c94b --- /dev/null +++ b/src/IK/include/BipedalLocomotion/IK/JointVelocityLimitsTask.h @@ -0,0 +1,118 @@ +/** + * @file JointVelocityLimitsTask.h + * @authors Davide Gorbani + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_IK_JOINT_VELOCITY_LIMITS_TASK_H +#define BIPEDAL_LOCOMOTION_IK_JOINT_VELOCITY_LIMITS_TASK_H + +#include +#include +#include + +#include + +#include + +namespace BipedalLocomotion +{ +namespace IK +{ + +/** + * JointVelocityLimitsTask is a concrete implementation of the Task. Please use this element if you + * want to ensure that the generated joint velocity will consider the joints velocity limits. + * The task represents the following equation + * \f[ + * \dot{s}_{-} \preceq \begin{bmatrix} 0_6 & I_n \end{bmatrix} \nu \preceq \dot{s}_{+} + * \f] + * where \f$0_6\f$ and \f$I_n\f$ are the zero and the identity matrix. \f$\dot{s} _{-}\f$ and + * \f$\dot{s} _{+}\f$ are the lower and upper limits of the joints velocity. + */ +class JointVelocityLimitsTask : public IKLinearTask +{ + Eigen::VectorXd m_upperLimits; /**< Upper limits in radians. */ + Eigen::VectorXd m_lowerLimits; /**< Lower limits in radians. */ + + std::string m_robotVelocityVariableName; + + bool m_isInitialized{false}; /**< True if the task has been initialized. */ + bool m_isValid{false}; /**< True if the task is valid. */ + + std::shared_ptr m_kinDyn; /**< Pointer to a KinDynComputations + object */ + +public: + // clang-format off + /** + * Initialize the task. + * @param paramHandler pointer to the parameters handler. + * @note the following parameters are required by the class + * | Parameter Name | Type | Description | Mandatory | + * |:----------------------------------:|:--------:|:-----------------------------------------------------------------------------------------------------------:|:---------:| + * | `robot_velocity_variable_name` | `string` | Name of the variable contained in `VariablesHandler` describing the robot velocity | Yes | + * | `upper_limits` | `vector` | Vector containing the upper limit of the joints velocity | Yes | + * | `lower_limits` | `vector` | Vector containing the lower limit of the joints velocity | Yes | + * @return True in case of success, false otherwise. + * Where the generalized robot velocity is a vector containing the base spatial-velocity + * (expressed in mixed representation) and the joint velocities. + * @return True in case of success, false otherwise. + */ + // clang-format on + bool + initialize(std::weak_ptr paramHandler) override; + + /** + * 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) override; + + /** + * 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 + * `robot_velocity_variable_name` stored in the parameter handler. The variable represents the + * generalized velocity of the robot. Where the generalized robot velocity is a vector + * containing the base spatial-velocity (expressed in mixed representation) and the joints + * velocity. + * @return True in case of success, false otherwise. + */ + bool setVariablesHandler(const System::VariablesHandler& variablesHandler) override; + + /** + * Update the content of the element. + * For this task the matrices A and b are constant and do not need to be updated. + * @return True in case of success, false otherwise. + */ + bool update() override; + + /** + * 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 JointsLimitsTask is an inequality 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; +}; + +BLF_REGISTER_IK_TASK(JointVelocityLimitsTask); + +} // namespace IK +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_IK_JOINT_VELOCITY_LIMITS_TASK_H diff --git a/src/IK/src/JointVelocityLimitsTask.cpp b/src/IK/src/JointVelocityLimitsTask.cpp new file mode 100644 index 0000000000..253d67e0be --- /dev/null +++ b/src/IK/src/JointVelocityLimitsTask.cpp @@ -0,0 +1,172 @@ +/** + * @file JointVelocityLimitsTask.cpp + * @authors Davide Gorbani + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +#include + +#include +#include + +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::IK; + +bool JointVelocityLimitsTask::setKinDyn(std::shared_ptr kinDyn) +{ + if ((kinDyn == nullptr) || (!kinDyn->isValid())) + { + log()->error("[JointVelocityLimitsTask::setKinDyn] Invalid kinDyn object."); + return false; + } + + m_kinDyn = kinDyn; + + // populate the limits with the one retrieved from the model + m_upperLimits + = Eigen::VectorXd::Constant(m_kinDyn->getNrOfDegreesOfFreedom(), OsqpEigen::INFTY); + m_lowerLimits + = Eigen::VectorXd::Constant(m_kinDyn->getNrOfDegreesOfFreedom(), -OsqpEigen::INFTY); + + return true; +} + +bool JointVelocityLimitsTask::setVariablesHandler(const System::VariablesHandler& variablesHandler) +{ + constexpr auto errorPrefix = "[JointVelocityLimitsTask::setVariablesHandler]"; + + System::VariablesHandler::VariableDescription robotVelocityVariable; + + if (!m_isInitialized) + { + log()->error("{} The task is not initialized. Please call initialize method.", errorPrefix); + return false; + } + + if (!variablesHandler.getVariable(m_robotVelocityVariableName, robotVelocityVariable)) + { + log()->error("{} Error while retrieving the robot velocity variable.", errorPrefix); + return false; + } + + if (robotVelocityVariable.size != m_kinDyn->getNrOfDegreesOfFreedom() + 6) + { + log()->error("{} The size of the robot velocity variable does not match with the one " + "stored in kinDynComputations object. Expected: {}. Given: {}", + errorPrefix, + m_kinDyn->getNrOfDegreesOfFreedom() + 6, + robotVelocityVariable.size); + return false; + } + + // resize the matrices + m_A.resize(2 * m_kinDyn->getNrOfDegreesOfFreedom(), variablesHandler.getNumberOfVariables()); + m_A.setZero(); + m_b.resize(2 * m_kinDyn->getNrOfDegreesOfFreedom()); + m_b.head(m_kinDyn->getNrOfDegreesOfFreedom()).noalias() = m_upperLimits; + m_b.tail(m_kinDyn->getNrOfDegreesOfFreedom()).noalias() = -m_lowerLimits; + + // the submatrix associated to the robot velocity + // _ _ + // A = | 0_{6x6} +I_{nxn} | + // | 0_{6x6} -I_{nxn} | + // |_ _| + iDynTree::toEigen(this->subA(robotVelocityVariable)) + .topRightCorner(m_kinDyn->getNrOfDegreesOfFreedom(), m_kinDyn->getNrOfDegreesOfFreedom()) + .diagonal() + .setConstant(1); + iDynTree::toEigen(this->subA(robotVelocityVariable)) + .bottomRightCorner(m_kinDyn->getNrOfDegreesOfFreedom(), m_kinDyn->getNrOfDegreesOfFreedom()) + .diagonal() + .setConstant(-1); + + return true; +} + +bool JointVelocityLimitsTask::initialize( + std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[JointVelocityLimitsTask::initialize]"; + + if (m_kinDyn == nullptr || !m_kinDyn->isValid()) + { + log()->error("{} KinDynComputations object is not valid.", errorPrefix); + return false; + } + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + if (!ptr->getParameter("robot_velocity_variable_name", m_robotVelocityVariableName)) + { + log()->error("{} Error while retrieving the robot velocity variable.", errorPrefix); + return false; + } + + if (!ptr->getParameter("upper_limits", m_upperLimits) + || !ptr->getParameter("lower_limits", m_lowerLimits)) + { + log()->error("{} Error while retrieving the 'upper_limits' and/or the 'lower_limit' " + "parameters.", + errorPrefix); + return false; + } + + // set the description + m_description = "Joint velocity limits task"; + + m_isInitialized = true; + + return true; +} + +bool JointVelocityLimitsTask::update() +{ + constexpr auto errorPrefix = "[JointVelocityLimitsTask::update]"; + + m_isValid = false; + + m_b.head(m_kinDyn->getNrOfDegreesOfFreedom()).noalias() = m_upperLimits; + m_b.tail(m_kinDyn->getNrOfDegreesOfFreedom()).noalias() = -m_lowerLimits; + + m_isValid = true; + return m_isValid; +} + +std::size_t JointVelocityLimitsTask::size() const +{ + constexpr auto errorMessage = "[JointVelocityLimitsTask::size] Please call setKinDyn method " + "before. " + "A size equal to zero will be returned."; + + assert((m_kinDyn != nullptr) && errorMessage); + + if (m_kinDyn == nullptr) + { + log()->warn(errorMessage); + return 0; + } + return 2 * m_kinDyn->getNrOfDegreesOfFreedom(); +} + +JointVelocityLimitsTask::Type JointVelocityLimitsTask::type() const +{ + return Type::inequality; +} + +bool JointVelocityLimitsTask::isValid() const +{ + return m_isValid; +} diff --git a/src/IK/tests/CMakeLists.txt b/src/IK/tests/CMakeLists.txt index 8f63b0b735..713e9d3891 100644 --- a/src/IK/tests/CMakeLists.txt +++ b/src/IK/tests/CMakeLists.txt @@ -47,6 +47,11 @@ add_bipedal_test( SOURCES GravityTaskTest.cpp LINKS BipedalLocomotion::IK) + add_bipedal_test( + NAME JointVelocityLimitsTaskIK + SOURCES JointVelocityLimitsTaskTest.cpp + LINKS BipedalLocomotion::IK) + if (FRAMEWORK_COMPILE_ContinuousDynamicalSystem) add_bipedal_test( diff --git a/src/IK/tests/JointVelocityLimitsTaskTest.cpp b/src/IK/tests/JointVelocityLimitsTaskTest.cpp new file mode 100644 index 0000000000..6d3587dc83 --- /dev/null +++ b/src/IK/tests/JointVelocityLimitsTaskTest.cpp @@ -0,0 +1,133 @@ +/** + * @file JointVelocityLimitsTaskTest.cpp + * @authors Davide Gorbani + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +// Catch2 +#include + +// BipedalLocomotion +#include +#include +#include + +#include +#include + +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; + +TEST_CASE("Joint Velocity Limit task") +{ + using namespace std::chrono_literals; + const std::string robotVelocity = "robotVelocity"; + + 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); + + Eigen::VectorXd upperVelocityLimit(jointsPos.size()); + Eigen::VectorXd lowerVelocityLimit(jointsPos.size()); + upperVelocityLimit.setConstant(0.1); + lowerVelocityLimit.setConstant(-0.2); + const Eigen::VectorXd upperLimits = upperVelocityLimit; + const Eigen::VectorXd lowerLimits = lowerVelocityLimit; + parameterHandler->setParameter("upper_limits", upperLimits); + parameterHandler->setParameter("lower_limits", lowerLimits); + + JointVelocityLimitsTask task; + REQUIRE(task.setKinDyn(kinDyn)); + REQUIRE(task.initialize(parameterHandler)); + REQUIRE(task.setVariablesHandler(variablesHandler)); + + 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()); + REQUIRE(A.block(0, + variablesHandler.getVariable(robotVelocity).offset + 6, + model.getNrOfDOFs(), + model.getNrOfDOFs()) + .isIdentity()); + REQUIRE(A.block(model.getNrOfDOFs(), + variablesHandler.getVariable(robotVelocity).offset + 6, + model.getNrOfDOFs(), + model.getNrOfDOFs()) + .isDiagonal()); + REQUIRE(A.block(model.getNrOfDOFs(), + variablesHandler.getVariable(robotVelocity).offset + 6, + model.getNrOfDOFs(), + model.getNrOfDOFs()) + .diagonal() + .isConstant(-1)); + + // check the vector b + Eigen::VectorXd expectedB; + expectedB = upperVelocityLimit; + REQUIRE(b.head(model.getNrOfDOFs()).isApprox(expectedB)); + + expectedB = -lowerVelocityLimit; + REQUIRE(b.tail(model.getNrOfDOFs()).isApprox(expectedB)); + } + } +}