diff --git a/.gitignore b/.gitignore index d5aaa3ef7f..9725c751a9 100644 --- a/.gitignore +++ b/.gitignore @@ -11,6 +11,7 @@ build* # IDEs .idea* +.vscode* # ====== # Python (https://github.com/github/gitignore/blob/master/Python.gitignore) diff --git a/CHANGELOG.md b/CHANGELOG.md index 01bbc468e3..9f56367440 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,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) ### 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/bindings/python/IK/CMakeLists.txt b/bindings/python/IK/CMakeLists.txt index 9a468ef12e..85d20e5e1e 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 - 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 + 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 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/DistanceTask.h b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/DistanceTask.h new file mode 100644 index 0000000000..6be8d70592 --- /dev/null +++ b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/DistanceTask.h @@ -0,0 +1,26 @@ +/** + * @file DistanceTask.h + * @authors Stefano Dafarra + * @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_DISTANCE_TASK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_IK_DISTANCE_TASK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace IK +{ + +void CreateDistanceTask(pybind11::module& module); + +} // namespace IK +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_DISTANCE_TASK_H diff --git a/bindings/python/IK/include/BipedalLocomotion/bindings/IK/GravityTask.h b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/GravityTask.h new file mode 100644 index 0000000000..7b16d41881 --- /dev/null +++ b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/GravityTask.h @@ -0,0 +1,26 @@ +/** + * @file GravityTask.h + * @authors Stefano Dafarra + * @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_GRAVITY_TASK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_IK_GRAVITY_TASK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace IK +{ + +void CreateGravityTask(pybind11::module& module); + +} // namespace IK +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_GRAVITY_TASK_H diff --git a/bindings/python/IK/src/DistanceTask.cpp b/bindings/python/IK/src/DistanceTask.cpp new file mode 100644 index 0000000000..d26f3aaf66 --- /dev/null +++ b/bindings/python/IK/src/DistanceTask.cpp @@ -0,0 +1,39 @@ +/** + * @file DistanceTask.cpp + * @authors Stefano Dafarra + * @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 CreateDistanceTask(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::IK; + + py::class_, IKLinearTask>(module, "DistanceTask") + .def(py::init()) + .def("set_desired_distance", &DistanceTask::setDesiredDistance, py::arg("desired_distance")) + .def("set_set_point", &DistanceTask::setSetPoint, py::arg("desired_distance")) + .def("get_distance", &DistanceTask::getDistance); +} + +} // namespace IK +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/IK/src/GravityTask.cpp b/bindings/python/IK/src/GravityTask.cpp new file mode 100644 index 0000000000..e0e39cd2cf --- /dev/null +++ b/bindings/python/IK/src/GravityTask.cpp @@ -0,0 +1,46 @@ +/** + * @file GravityTask.cpp + * @authors Stefano Dafarra + * @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 CreateGravityTask(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::IK; + + py::class_, IKLinearTask>(module, "GravityTask") + .def(py::init()) + .def("set_desired_gravity_direction_in_target_frame", + &GravityTask::setDesiredGravityDirectionInTargetFrame, + py::arg("desired_gravity_direction")) + .def("set_feedforward_velocity_in_target_frame", + &GravityTask::setFeedForwardVelocityInTargetFrame, + py::arg("feedforward_velocity")) + .def("set_set_point", + &GravityTask::setSetPoint, + py::arg("desired_gravity_direction"), + py::arg("feedforward_velocity") = Eigen::Vector3d::Zero()); +} + +} // namespace IK +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/IK/src/Module.cpp b/bindings/python/IK/src/Module.cpp index 189aaadc5a..c2ae7aaf82 100644 --- a/bindings/python/IK/src/Module.cpp +++ b/bindings/python/IK/src/Module.cpp @@ -9,6 +9,8 @@ #include #include +#include +#include #include #include #include @@ -37,6 +39,8 @@ void CreateModule(pybind11::module& module) CreateJointTrackingTask(module); CreateJointLimitsTask(module); CreateAngularMomentumTask(module); + CreateDistanceTask(module); + CreateGravityTask(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 8158d42b7a..74c267cef0 100644 --- a/bindings/python/IK/tests/test_QP_inverse_kinematics.py +++ b/bindings/python/IK/tests/test_QP_inverse_kinematics.py @@ -213,6 +213,53 @@ def test_angular_momentum_task(): assert angular_momentum_task.set_variables_handler(variables_handler=angular_momentum_var_handler) assert angular_momentum_task.set_set_point([0., 0., 0.]) +def test_distance_task(): + # create KinDynComputationsDescriptor + joints_list, kindyn = get_kindyn() + + # Set the parameters + distance_task_param_handler = blf.parameters_handler.StdParametersHandler() + distance_task_param_handler.set_parameter_string(name="robot_velocity_variable_name", value="robotVelocity") + distance_task_param_handler.set_parameter_string(name="target_frame_name", value="chest") + distance_task_param_handler.set_parameter_string(name="reference_frame_name", value="root_link") + distance_task_param_handler.set_parameter_float(name="kp",value=5.0) + + # Initialize the task + distance_task = blf.ik.DistanceTask() + assert distance_task.set_kin_dyn(kindyn) + assert distance_task.initialize(param_handler=distance_task_param_handler) + distance_task_var_handler = blf.system.VariablesHandler() + assert distance_task_var_handler.add_variable("robotVelocity", 32) is True # robot velocity size = 26 (joints) + 6 (base) + assert distance_task.set_variables_handler(variables_handler=distance_task_var_handler) + + # Set desired distance + assert distance_task.set_desired_distance(desired_distance=np.random.uniform(-0.5,0.5)) + assert distance_task.set_set_point(desired_distance=np.random.uniform(-0.5,0.5)) + +def test_gravity_task(): + # create KinDynComputationsDescriptor + joints_list, kindyn = get_kindyn() + + # Set the parameters + gravity_task_param_handler = blf.parameters_handler.StdParametersHandler() + gravity_task_param_handler.set_parameter_string(name="robot_velocity_variable_name", value="robotVelocity") + gravity_task_param_handler.set_parameter_string(name="target_frame_name", value="chest") + gravity_task_param_handler.set_parameter_string(name="base_frame_name", value="root_link") + gravity_task_param_handler.set_parameter_float(name="kp",value=5.0) + + # Initialize the task + gravity_task = blf.ik.GravityTask() + assert gravity_task.set_kin_dyn(kindyn) + assert gravity_task.initialize(param_handler=gravity_task_param_handler) + gravity_task_var_handler = blf.system.VariablesHandler() + assert gravity_task_var_handler.add_variable("robotVelocity", 32) is True # robot velocity size = 26 (joints) + 6 (base) + assert gravity_task.set_variables_handler(variables_handler=gravity_task_var_handler) + + # Set desired distance + assert gravity_task.set_desired_gravity_direction_in_target_frame(desired_gravity_direction=[np.random.uniform(-0.5,0.5) for _ in range(3)]) + 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_integration_based_ik_state(): state = blf.ik.IntegrationBasedIKState() diff --git a/src/IK/CMakeLists.txt b/src/IK/CMakeLists.txt index f160c6d339..cdd27e57fc 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}/QPInverseKinematics.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h ${H_PREFIX}/GravityTask.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/QPInverseKinematics.cpp src/QPFixedBaseInverseKinematics.cpp src/IKLinearTask.cpp src/IntegrationBasedIK.cpp src/DistanceTask.cpp src/GravityTask.cpp PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System LieGroupControllers::LieGroupControllers diff --git a/src/IK/include/BipedalLocomotion/IK/DistanceTask.h b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h new file mode 100644 index 0000000000..068e57f101 --- /dev/null +++ b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h @@ -0,0 +1,163 @@ +/** + * @file DistanceTask.h + * @authors Ehsan Ranjbari + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef DISTANCE_TASK_H +#define DISTANCE_TASK_H + +#include + +#include + +#include + +namespace BipedalLocomotion +{ +namespace IK +{ + +// clang-format off +/** + * The DistanceTask class controls the distance of a frame with respect the world origin, or another frame. + * The task assumes perfect control of the robot velocity \f$\nu\f$ that contains the base + * linear and angular velocity expressed in mixed representation and the joint velocities. + * The task represents the following equation + * \f[ + * \frac{1}{d} p ^ \top J_p \nu = k (d ^ * - d) + * \f] + * where \f$p\f$ is the position of the target frame wrt the base or world frame. + * \f$d\f$ is the current distance. + * \f$J_p\f$ is the linear part of the robot jacobian. + * \f$k\f$ is the controller gain and \f$d ^ *\f$ is the desired distance. + */ +// clang-format on +class DistanceTask : public IKLinearTask +{ + + System::VariablesHandler::VariableDescription m_robotVelocityVariable; /**< Variable describing + the robot velocity + (base + joint) */ + + static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. + */ + static constexpr std::size_t m_DoFs{1}; /**< DoFs associated to the task */ + + 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 */ + + Eigen::MatrixXd m_jacobian, m_relativeJacobian; /**< Internal buffers to store the jacobian. */ + Eigen::Vector3d m_framePosition; /**< Internal buffer to store the frame position. */ + double m_kp; /**< Controller gain. */ + double m_desiredDistance{0.0}; /**< Desired distance. */ + + std::string m_referenceName; /**< Reference frame name. */ + std::string m_targetFrameName; /**< Target frame name. */ + iDynTree::FrameIndex m_referenceFrameIndex, m_targetFrameIndex; /**< Base and target frame name + indexes. */ + + double m_computedDistance{0.0}; /**< Computed distance. */ + double m_distanceNumericThreshold{0.001}; /**< Numeric threshold when inverting the computed + distance in the Jacobian computation. */ + +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 | + * | `kp` | `double` | Gain of the distance controller | Yes | + * | `target_frame_name` | `string` | Name of the frame of which computing the distance | Yes | + * | `reference_frame_name` | `string` | Name of the frame with respect to which computing the distance. If empty, the world frame will be used (Default = "") | No | + * | `distance_numeric_threshold` | `double` | Lowerbound for the computed distance when inverting it in the task Jacobian (Default = 0.001) | No | + * @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 task. + * @return True in case of success, false otherwise. + */ + bool update() override; + + /** + * @brief setDesiredDistance Set the desired distance between the target and the reference frame + * @param desiredDistance The desired distance expressed in meters + * @note The desired distance is considered without sign. + * @return True in case of success, false otherwise. + */ + bool setDesiredDistance(double desiredDistance); + + /** + * @brief setSetPoint Set the desired distance between the target and the reference frame + * @param desiredDistance The desired distance expressed in meters + * + * It is equivalent to setDesiredDistance + * + * @note The desired distance is considered without sign. + * @return True in case of success, false otherwise. + */ + bool setSetPoint(double desiredDistance); + + /** + * @brief Get the computed distance between the target frame and either the world origin, or the + * base origin. + * @return The computed distance + */ + double getDistance() const; + + /** + * 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 DistanceTask 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; +}; + +BLF_REGISTER_IK_TASK(DistanceTask); + +} // namespace IK +} // namespace BipedalLocomotion + +#endif // DISTANCE_TASK_H diff --git a/src/IK/include/BipedalLocomotion/IK/GravityTask.h b/src/IK/include/BipedalLocomotion/IK/GravityTask.h new file mode 100644 index 0000000000..44604d8166 --- /dev/null +++ b/src/IK/include/BipedalLocomotion/IK/GravityTask.h @@ -0,0 +1,232 @@ +/** + * @file DistanceTask.h + * @authors Ehsan Ranjbari + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef GRAVITY_TASK_H +#define GRAVITY_TASK_H + +#include + +#include + +#include + +#include + +namespace BipedalLocomotion +{ +namespace IK +{ +// clang-format off +/** + * The GravityTask class aligns the z axis of a frame to a desired gravity direction. + * The task assumes perfect control of the robot velocity \f$\nu\f$ that contains the base + * linear and angular velocity expressed in mixed representation and the joint velocities. + * The task represents the following equation + * \f[ + * \begin{bmatrix} + * 1 & 0 & 0 \\ + * 0 & 1 & 0 + * \end{bmatrix} J_\omega \nu = -k \begin{bmatrix} + * 0 &-1 & 0 \\ + * 1 & 0 & 0 + * \end{bmatrix} {} ^ W R_f {} ^ f g ^ * + {} ^ W R_f {} ^ f \omega_{ff} + * \f] + * where \f$J_\omega\f$ is the angular part of the frame jacobian. + * \f$k\f$ is the controller gain. + * \f${} ^ W R_f\f$ is the frame rotation. + * \f${} ^ W g ^ *\f$ is the desired z axis direction expressed in the frame coordinates. + * \f${} ^ W \omega_{ff}\f$ is the feed-forward velocity expressed in the frame coordinates. + * The explanation is as follows. + * + * We define ${}^wR_b$ as the orientation of the frame $b$ we want to move, + * and ${}^b\hat{V}$ as the unitary vector fixed in frame $b$ that we want to align to ${}^wz = e_3 = [0 0 1]^\top$. + * + * We define the following Lyapunov function: + * \f[ + * V = 1 - \cos(\theta) \geq 0. + * \f] + * + * Since \f$ a^\top b = ||a|| ~||b|| \cos(\theta) \f$, we have: + * \f[ + * V = 1 - \left({}^wR_b{}^b\hat{V}\right)^\top e_3, + * \f] + * given that both vectors are unitary. + * + * The time derivative of the Lyapunov function is as follows: + * \f[ + * \dot{V} = - \left({}^w\dot{R}_b{}^b\hat{V}\right)^\top e_3, + * \f] + * where \f$ {}^w\dot{R}_b = S({}^w\omega){}^w{R}_b \f$, with \f$ S(\cdot) \f$ being a skew symmetric matrix, + * and \f$ {}^w\omega \in \mathbb{R}^3 \f$ is the right-trivialized angular velocity. Hence, + * \f[ + * \dot{V} = - \left(S({}^w\omega){}^w{R}_b{}^b\hat{V}\right)^\top e_3. + * \f] + * + * Exploiting \f$ S(\cdot)^\top = -S(\cdot) \f$, we have: + * \f[ + * \dot{V} = {}^b\hat{V}^\top{}^w{R}_b^\top S({}^w\omega) e_3, + * \f] + * and \f$ S(a)b = -S(b)a \f$: + * \f[ + * \dot{V} = -{}^b\hat{V}^\top{}^w{R}_b^\top S(e_3){}^w\omega. + * \f] + * + * Then, if we choose: + * \f[ + * {}^w\omega = k \left({}^b\hat{V}^\top{}^w{R}_b^\top S(e_3)\right)^\top + \lambda e_3, + * \f] + * with \f$ k, \lambda \in \mathbb{R}^3 \f$, we have that \f$ \dot{V} \leq 0 \f$. Then, + * \f[ + * {}^w\omega = -k S(e_3) {}^w{R}_b{}^b\hat{V} + \lambda e_3. + * \f] + * + * We can introduce the right-trivialized Jacobian \f$ {}^wJ \f$ such that \f$ {}^wJ\dot{\nu} = {}^w\omega \f$: + * \f[ + * {}^wJ\dot{\nu} = -k S(e_3) {}^w{R}_b{}^b\hat{V} + \lambda e_3. + * \f] + * + * We can filter out the rotation around the world Z-axis, thus obtaining: + * \f[ + * \begin{bmatrix} + * e_1 & e_2 + * \end{bmatrix}^\top {}^wJ\dot{\nu} = -k \begin{bmatrix} + * 0 & -1 & 0\\ + * 1 & 0 & 0 + * \end{bmatrix} {}^w{R}_b{}^b\hat{V}. + * \f] + */ +// clang-format on +class GravityTask : public IKLinearTask +{ + + System::VariablesHandler::VariableDescription m_robotVelocityVariable; /**< Variable describing + the robot velocity + (base + joint) */ + + static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. + */ + static constexpr std::size_t m_DoFs{2}; /**< DoFs associated to the task */ + + 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 */ + + double m_kp; /**< Controller gain. */ + Eigen::MatrixXd m_jacobian; /**< Internal buffer to store the jacobian. */ + Eigen::Vector3d m_desiredZDirectionBody; /**< Desired gravity direction in the target frame. */ + Eigen::Vector3d m_feedForwardBody; /**< Feedforward term expressed in body direction. */ + Eigen::MatrixXd m_Am; /**< Matrix filtering jacobian rows. */ + Eigen::MatrixXd m_bm; /**< Matrix filtering acceleration. */ + + iDynTree::FrameIndex m_targetFrameIndex; /**< Index of the target frame. */ + +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 | + * | `kp` | `double` | Gain of the distance controller | Yes | + * | `target_frame_name` | `string` | Name of the frame to which apply the gravity task | Yes | + * @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 task. + * @return True in case of success, false otherwise. + */ + bool update() override; + + /** + * @brief Set the desired gravity direction expressed the target frame. + * @param desiredGravityDirection The desired gravity direction. + * @return True in case of success, false otherwise. + * + * The input is normalized, unless the norm is too small. + */ + bool setDesiredGravityDirectionInTargetFrame( + const Eigen::Ref desiredGravityDirection); + + /** + * @brief Set the feedforward angular velocity expressed in the target frame. + * @param feedforwardVelocity The desired feedforward velocity in rad/s. + * @return True in case of success, false otherwise. + * Only the first two components are used. + */ + bool + setFeedForwardVelocityInTargetFrame(const Eigen::Ref feedforwardVelocity); + + /** + * @brief Set the desired gravity direction expressed the target frame and the feedforward + * velocity. + * @param desiredGravityDirection The desired gravity direction in target frame. + * @param feedforwardVelocity The desired feedforward velocity in rad/s expressed in the target + * frame. + * @return True in case of success, false otherwise. + * + * The desiredGravityDirection is normalized, unless the norm is too small. + * Only the first two components of the feedforwardVelocity are used. + * This is equivalent of using setDesiredGravityDirectionInTargetFrame and + * setFeedForwardVelocityInTargetFrame + */ + bool setSetPoint(const Eigen::Ref desiredGravityDirection, + const Eigen::Ref feedforwardVelocity + = Eigen::Vector3d::Zero()); + + /** + * 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 DistanceTask 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; +}; + +BLF_REGISTER_IK_TASK(GravityTask); + +} // namespace IK +} // namespace BipedalLocomotion + +#endif // GRAVITY_TASK_H diff --git a/src/IK/src/DistanceTask.cpp b/src/IK/src/DistanceTask.cpp new file mode 100644 index 0000000000..67c6eb3fa2 --- /dev/null +++ b/src/IK/src/DistanceTask.cpp @@ -0,0 +1,251 @@ +/** + * @file DistanceTask.cpp + * @authors Ehsan Ranjbari + * @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 + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; + +bool DistanceTask::setKinDyn(std::shared_ptr kinDyn) +{ + if ((kinDyn == nullptr) || (!kinDyn->isValid())) + { + log()->error("[DistanceTask::setKinDyn] Invalid kinDyn object."); + return false; + } + + m_kinDyn = kinDyn; + return true; +} + +bool DistanceTask::setVariablesHandler(const System::VariablesHandler& variablesHandler) +{ + if (!m_isInitialized) + { + log()->error("[DistanceTask::setVariablesHandler] The task is not initialized. Please call " + "initialize method."); + return false; + } + + // get the variable + if (!variablesHandler.getVariable(m_robotVelocityVariable.name, m_robotVelocityVariable)) + { + log()->error("[DistanceTask::setVariablesHandler] Unable to get the variable named {}.", + m_robotVelocityVariable.name); + return false; + } + + // get the variable + if (m_robotVelocityVariable.size != m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize) + { + log()->error("[DistanceTask::setVariablesHandler] The size of the robot velocity variable " + "is different from the one expected. Expected size: {}. Given size: {}.", + m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize, + m_robotVelocityVariable.size); + return false; + } + + // resize the matrices + m_A.resize(m_DoFs, variablesHandler.getNumberOfVariables()); + m_A.setZero(); + m_b.resize(m_DoFs); + m_b.setZero(); + m_jacobian.resize(m_spatialVelocitySize, + m_spatialVelocitySize + m_kinDyn->getNrOfDegreesOfFreedom()); + m_jacobian.setZero(); + m_relativeJacobian.resize(m_spatialVelocitySize, m_kinDyn->getNrOfDegreesOfFreedom()); + m_relativeJacobian.setZero(); + m_framePosition.setZero(); + + return true; +} + +bool DistanceTask::initialize( + std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[DistanceTask::initialize]"; + + m_description = "DistanceTask"; + + if (m_kinDyn == nullptr || !m_kinDyn->isValid()) + { + log()->error("{} [{}] KinDynComputations object is not valid.", errorPrefix, m_description); + + return false; + } + + if (m_kinDyn->getFrameVelocityRepresentation() + != iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION) + { + log()->error("{} [{}] task supports only quantities expressed in MIXED " + "representation. Please provide a KinDynComputations with Frame velocity " + "representation set to MIXED_REPRESENTATION.", + errorPrefix, + m_description); + return false; + } + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} [{}] The parameter handler is not valid.", errorPrefix, m_description); + return false; + } + + if (!ptr->getParameter("robot_velocity_variable_name", m_robotVelocityVariable.name)) + { + log()->error("{} [{}] Failed to retrieve the robot velocity variable.", + errorPrefix, + m_description); + return false; + } + + // set the gains for the controllers + if (!ptr->getParameter("kp", m_kp)) + { + log()->error("{} [{}] Failed to get the proportional linear gain.", + errorPrefix, + m_description); + return false; + } + + // set the base frame name + if (!ptr->getParameter("reference_frame_name", m_referenceName)) + { + log()->debug("{} [{}] No base_name specified. Using default \"\"", + errorPrefix, + m_description); + } + + // set the finger tip frame Name + if (!ptr->getParameter("target_frame_name", m_targetFrameName)) + { + log()->error("{} [{}] Failed to get the end effector frame name.", + errorPrefix, + m_description); + return false; + } + + // set the gains for the controllers + if (!ptr->getParameter("distance_numeric_threshold", m_distanceNumericThreshold)) + { + log()->debug("{} [{}] No distance_numeric_threshold specified. Using default {}", + errorPrefix, + m_description, + m_distanceNumericThreshold); + } + + if (m_referenceName != "") + { + m_referenceFrameIndex = m_kinDyn->getFrameIndex(m_referenceName); + + if (m_referenceFrameIndex == iDynTree::FRAME_INVALID_INDEX) + { + log()->error("{} [{}] The specified base name ({}) does not seem to exist.", + errorPrefix, + m_description, + m_referenceName); + return false; + } + } + + m_targetFrameIndex = m_kinDyn->getFrameIndex(m_targetFrameName); + + if (m_targetFrameIndex == iDynTree::FRAME_INVALID_INDEX) + { + log()->error("{} [{}] The specified target name ({}) does not seem to exist.", + errorPrefix, + m_description, + m_targetFrameName); + return false; + } + + m_isInitialized = true; + + return true; +} + +bool DistanceTask::update() +{ + using namespace BipedalLocomotion::Conversions; + using namespace iDynTree; + + m_isValid = false; + + if (m_referenceName == "") + { + m_framePosition = toEigen(m_kinDyn->getWorldTransform(m_targetFrameIndex).getPosition()); + + if (!m_kinDyn->getFrameFreeFloatingJacobian(m_targetFrameIndex, m_jacobian)) + { + log()->error("[DistanceTask::update] Unable to get the jacobian."); + return m_isValid; + } + + } else + { + m_framePosition = toEigen( + m_kinDyn->getRelativeTransform(m_referenceFrameIndex, m_targetFrameIndex).getPosition()); + + if (!m_kinDyn->getRelativeJacobian(m_referenceFrameIndex, + m_targetFrameIndex, + m_relativeJacobian)) + { + log()->error("[DistanceTask::update] Unable to get the relative jacobian."); + return m_isValid; + } + + m_jacobian.rightCols(m_kinDyn->getNrOfDegreesOfFreedom()) = m_relativeJacobian; + } + + m_computedDistance = m_framePosition.norm(); + + toEigen(this->subA(m_robotVelocityVariable)).noalias() + = (m_framePosition.transpose() * m_jacobian.topRows<3>()) + / (std::max(m_distanceNumericThreshold, m_computedDistance)); + m_b(0) = m_kp * (m_desiredDistance - m_computedDistance); + + m_isValid = true; + return m_isValid; +} + +bool DistanceTask::setDesiredDistance(double desiredDistance) +{ + m_desiredDistance = std::abs(desiredDistance); + return true; +} + +bool DistanceTask::setSetPoint(double desiredDistance) +{ + return setDesiredDistance(desiredDistance); +} + +double DistanceTask::getDistance() const +{ + return m_computedDistance; +} + +std::size_t DistanceTask::size() const +{ + return m_DoFs; +} + +DistanceTask::Type DistanceTask::type() const +{ + return Type::equality; +} + +bool DistanceTask::isValid() const +{ + return m_isValid; +} diff --git a/src/IK/src/GravityTask.cpp b/src/IK/src/GravityTask.cpp new file mode 100644 index 0000000000..7de70b466f --- /dev/null +++ b/src/IK/src/GravityTask.cpp @@ -0,0 +1,209 @@ +/** + * @file GravityTask.cpp + * @authors Ehsan Ranjbari + * @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 + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; + +bool GravityTask::setKinDyn(std::shared_ptr kinDyn) +{ + if ((kinDyn == nullptr) || (!kinDyn->isValid())) + { + log()->error("[GravityTask::setKinDyn] Invalid kinDyn object."); + return false; + } + + m_kinDyn = kinDyn; + return true; +} + +bool GravityTask::setVariablesHandler(const System::VariablesHandler& variablesHandler) +{ + if (!m_isInitialized) + { + log()->error("[GravityTask::setVariablesHandler] The task is not initialized. Please call " + "initialize method."); + return false; + } + + // get the variable + if (!variablesHandler.getVariable(m_robotVelocityVariable.name, m_robotVelocityVariable)) + { + log()->error("[GravityTask::setVariablesHandler] Unable to get the variable named {}.", + m_robotVelocityVariable.name); + return false; + } + + // get the variable + if (m_robotVelocityVariable.size != m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize) + { + log()->error("[GravityTask::setVariablesHandler] The size of the robot velocity variable " + "is different from the one expected. Expected size: {}. Given size: {}.", + m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize, + m_robotVelocityVariable.size); + return false; + } + + // resize the matrices + m_A.resize(m_DoFs, variablesHandler.getNumberOfVariables()); + m_A.setZero(); + m_b.resize(m_DoFs); + m_b.setZero(); + 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_bm.resize(2, 3); + // clang-format off + m_Am << 1, 0, 0, + 0, 1, 0; + m_bm << 0, -1, 0, + 1, 0, 0; + // clang-format on + + return true; +} + +bool GravityTask::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[GravityTask::initialize]"; + + m_description = "GravityTask"; + + if (m_kinDyn == nullptr || !m_kinDyn->isValid()) + { + log()->error("{} [{}] KinDynComputations object is not valid.", errorPrefix, m_description); + + return false; + } + + if (m_kinDyn->getFrameVelocityRepresentation() + != iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION) + { + log()->error("{} [{}] task supports only quantities expressed in MIXED " + "representation. Please provide a KinDynComputations with Frame velocity " + "representation set to MIXED_REPRESENTATION.", + errorPrefix, + m_description); + return false; + } + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} [{}] The parameter handler is not valid.", errorPrefix, m_description); + return false; + } + + std::string robotVelocityVariableName; + if (!ptr->getParameter("robot_velocity_variable_name", m_robotVelocityVariable.name)) + { + log()->error("{} [{}] while retrieving the robot velocity variable.", + errorPrefix, + m_description); + return false; + } + + // set the gains for the controllers + if (!ptr->getParameter("kp", m_kp)) + { + log()->error("{} [{}] to get the proportional linear gain.", errorPrefix, m_description); + return false; + } + + std::string targetFrameName; + if (!ptr->getParameter("target_frame_name", targetFrameName)) + { + log()->error("{} [{}] to get the end effector frame name.", errorPrefix, m_description); + return false; + } + + m_targetFrameIndex = m_kinDyn->getFrameIndex(targetFrameName); + + if (m_targetFrameIndex == iDynTree::FRAME_INVALID_INDEX) + { + log()->error("{} [{}] The specified target name ({}) does not seem to exist.", + errorPrefix, + m_description, + targetFrameName); + return false; + } + + m_isInitialized = true; + + return true; +} + +bool GravityTask::update() +{ + using namespace iDynTree; + + m_isValid = false; + + auto targetRotation = toEigen(m_kinDyn->getWorldTransform(m_targetFrameIndex).getRotation()); + + Eigen::Vector3d desiredZDirectionAbsolute = targetRotation * m_desiredZDirectionBody; + Eigen::Vector3d feedforwardAbsolute = targetRotation * m_feedForwardBody; + + if (!m_kinDyn->getFrameFreeFloatingJacobian(m_targetFrameIndex, m_jacobian)) + { + log()->error("[GravityTask::update] Unable to get the frame jacobian."); + return m_isValid; + } + + toEigen(this->subA(m_robotVelocityVariable)) = m_Am * m_jacobian.bottomRows<3>(); + m_b = feedforwardAbsolute.topRows<2>(); + m_b.noalias() -= m_kp * m_bm * desiredZDirectionAbsolute; + + m_isValid = true; + return m_isValid; +} + +bool GravityTask::setDesiredGravityDirectionInTargetFrame( + const Eigen::Ref desiredGravityDirection) +{ + m_desiredZDirectionBody = desiredGravityDirection; + m_desiredZDirectionBody.normalize(); + return true; +} + +bool GravityTask::setFeedForwardVelocityInTargetFrame( + const Eigen::Ref feedforwardVelocity) +{ + m_feedForwardBody = feedforwardVelocity; + return true; +} + +bool GravityTask::setSetPoint(const Eigen::Ref desiredGravityDirection, + const Eigen::Ref feedforwardVelocity) +{ + return setDesiredGravityDirectionInTargetFrame(desiredGravityDirection) + && setFeedForwardVelocityInTargetFrame(feedforwardVelocity); +} + +std::size_t GravityTask::size() const +{ + return m_DoFs; +} + +GravityTask::Type GravityTask::type() const +{ + return Type::equality; +} + +bool GravityTask::isValid() const +{ + return m_isValid; +} diff --git a/src/IK/tests/CMakeLists.txt b/src/IK/tests/CMakeLists.txt index ff076c220e..8f63b0b735 100644 --- a/src/IK/tests/CMakeLists.txt +++ b/src/IK/tests/CMakeLists.txt @@ -37,6 +37,16 @@ add_bipedal_test( SOURCES AngularMomentumTaskTest.cpp LINKS BipedalLocomotion::IK) +add_bipedal_test( + NAME DistanceTaskIK + 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/DistanceTaskTest.cpp b/src/IK/tests/DistanceTaskTest.cpp new file mode 100644 index 0000000000..0d12ae98cd --- /dev/null +++ b/src/IK/tests/DistanceTaskTest.cpp @@ -0,0 +1,164 @@ +/** + * @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); + + bool relative = numberOfJoints % 2 == 0; + std::string baseName; + + if (relative) + { + do + { + baseName = iDynTree::getRandomLinkOfModel(model); + } while (baseName == targetName); + + parameterHandler->setParameter("reference_frame_name", baseName); + } else + { + parameterHandler->setParameter("reference_frame_name", ""); + } + + DistanceTask task; + REQUIRE(task.setKinDyn(kinDyn)); + REQUIRE(task.initialize(parameterHandler)); + REQUIRE(task.setVariablesHandler(variablesHandler)); + + double desiredDistance = 1.0; + REQUIRE(task.setSetPoint(desiredDistance)); + + REQUIRE(task.update()); + REQUIRE(task.isValid()); + + // get A and b + Eigen::Ref A = task.getA(); + Eigen::Ref b = task.getB(); + + Eigen::Vector3d targetPosition; + Eigen::MatrixXd jacobian; + jacobian.setZero(6, 6 + model.getNrOfDOFs()); + + if (relative) + { + targetPosition = iDynTree::toEigen( + kinDyn->getRelativeTransform(baseName, targetName).getPosition()); + Eigen::MatrixXd relativeJacobian; + relativeJacobian.resize(6, model.getNrOfDOFs()); + kinDyn->getRelativeJacobian(model.getFrameIndex(baseName), + model.getFrameIndex(targetName), + relativeJacobian); + jacobian.rightCols(model.getNrOfDOFs()) = relativeJacobian; + + } else + { + targetPosition + = iDynTree::toEigen(kinDyn->getWorldTransform(targetName).getPosition()); + kinDyn->getFrameFreeFloatingJacobian(targetName, jacobian); + } + Eigen::VectorXd distance(1); + distance << targetPosition.norm(); + + Eigen::VectorXd measuredDistance(1); + measuredDistance << task.getDistance(); + + REQUIRE(distance.isApprox(measuredDistance)); + + Eigen::MatrixXd expectedA + = targetPosition.transpose() * jacobian.topRows<3>() / distance(0); + + // 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()); + expectedB(0) = kp * (desiredDistance - distance(0)); + REQUIRE(b.isApprox(expectedB)); + } + } +} diff --git a/src/IK/tests/GravityTaskTest.cpp b/src/IK/tests/GravityTaskTest.cpp new file mode 100644 index 0000000000..38837130e5 --- /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(); + Eigen::Vector3d feedforward({0.1, 0.2, 0.3}); + + REQUIRE(task.setSetPoint(desiredDirection, 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 563a3aaa5f..f90e55915d 100644 --- a/src/IK/tests/QPInverseKinematicsTest.cpp +++ b/src/IK/tests/QPInverseKinematicsTest.cpp @@ -6,6 +6,7 @@ */ // Catch2 +#include #include // std @@ -21,9 +22,11 @@ #include #include +#include +#include #include -#include #include +#include #include #include @@ -43,20 +46,26 @@ using namespace std::chrono_literals; constexpr auto robotVelocity = "robotVelocity"; constexpr std::chrono::nanoseconds dT = 10ms; -struct InverseKinematicsAndTasks +struct InverseKinematicsTasks { - std::shared_ptr ik; std::shared_ptr se3Task; std::shared_ptr comTask; std::shared_ptr regularizationTask; std::shared_ptr jointLimitsTask; + std::shared_ptr distanceTask; + std::shared_ptr gravityTask; }; struct DesiredSetPoints { Eigen::Vector3d CoMPosition; + std::string endEffectorFrame; manif::SE3d endEffectorPose; Eigen::VectorXd joints; + std::string targetFrameDistance; + double targetDistance; + std::string targetFrameGravity; + Eigen::Vector3d desiredBodyGravity; }; struct System @@ -69,6 +78,7 @@ std::shared_ptr createParameterHandler() { constexpr double gain = 5.0; + constexpr double additionalTasksWeight = 10.0; auto parameterHandler = std::make_shared(); parameterHandler->setParameter("robot_velocity_variable_name", robotVelocity); @@ -78,7 +88,9 @@ std::shared_ptr createParameterHandler() std::vector{"SE3_TASK", "COM_TASK", "REGULARIZATION_TASK", - "JOINT_LIMITS_TASK"}); + "JOINT_LIMITS_TASK", + "DISTANCE_TASK", + "GRAVITY_TASK"}); /////// SE3 Task auto SE3ParameterHandler = std::make_shared(); @@ -105,7 +117,7 @@ std::shared_ptr createParameterHandler() jointRegularizationHandler->setParameter("priority", 1); parameterHandler->setGroup("REGULARIZATION_TASK", jointRegularizationHandler); - + /////// Joint limits task auto jointLimitsHandler = std::make_shared(); jointLimitsHandler->setParameter("robot_velocity_variable_name", robotVelocity); jointLimitsHandler->setParameter("sampling_time", dT); @@ -114,6 +126,27 @@ std::shared_ptr createParameterHandler() jointLimitsHandler->setParameter("priority", 0); parameterHandler->setGroup("JOINT_LIMITS_TASK", jointLimitsHandler); + /////// Distance task + auto distanceTaskHandler = std::make_shared(); + distanceTaskHandler->setParameter("robot_velocity_variable_name", robotVelocity); + distanceTaskHandler->setParameter("type", "DistanceTask"); + distanceTaskHandler->setParameter("priority", 1); + distanceTaskHandler->setParameter("kp", gain * 5); + Eigen::VectorXd distanceWeight(1); + distanceWeight << additionalTasksWeight; + distanceTaskHandler->setParameter("weight", distanceWeight); + 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", 1); + gravityTaskHandler->setParameter("kp", gain * 10); + const Eigen::Vector2d gravityWeight = additionalTasksWeight * Eigen::Vector2d::Ones(); + gravityTaskHandler->setParameter("weight", gravityWeight); + parameterHandler->setGroup("GRAVITY_TASK", gravityTaskHandler); + return parameterHandler; } @@ -140,57 +173,35 @@ void finalizeParameterHandler(std::shared_ptr handler, handler->getGroup("JOINT_LIMITS_TASK").lock()->setParameter("lower_limits", lowerLimits); } -InverseKinematicsAndTasks createIK(std::shared_ptr handler, - std::shared_ptr kinDyn, - const VariablesHandler& variables) +InverseKinematicsTasks createIKTasks(std::shared_ptr handler, + std::shared_ptr kinDyn) { - InverseKinematicsAndTasks out; - - constexpr std::size_t highPriority = 0; - constexpr std::size_t lowPriority = 1; - Eigen::VectorXd kpRegularization; - Eigen::VectorXd weightRegularization; - - out.ik = std::make_shared(); - REQUIRE(out.ik->initialize(handler)); + InverseKinematicsTasks out; out.se3Task = std::make_shared(); REQUIRE(out.se3Task->setKinDyn(kinDyn)); REQUIRE(out.se3Task->initialize(handler->getGroup("SE3_TASK"))); - REQUIRE(out.ik->addTask(out.se3Task, "se3_task", highPriority)); out.comTask = std::make_shared(); REQUIRE(out.comTask->setKinDyn(kinDyn)); REQUIRE(out.comTask->initialize(handler->getGroup("COM_TASK"))); - REQUIRE(out.ik->addTask(out.comTask, "com_task", highPriority)); - REQUIRE(handler->getGroup("REGULARIZATION_TASK").lock()->getParameter("kp", kpRegularization)); - REQUIRE(handler->getGroup("REGULARIZATION_TASK").lock()->getParameter("weight", weightRegularization)); out.regularizationTask = std::make_shared(); REQUIRE(out.regularizationTask->setKinDyn(kinDyn)); REQUIRE(out.regularizationTask->initialize(handler->getGroup("REGULARIZATION_TASK"))); - REQUIRE(out.ik->addTask(out.regularizationTask, - "regularization_task", - lowPriority, - weightRegularization)); out.jointLimitsTask = std::make_shared(); REQUIRE(out.jointLimitsTask->setKinDyn(kinDyn)); REQUIRE(out.jointLimitsTask->initialize(handler->getGroup("JOINT_LIMITS_TASK"))); - REQUIRE(out.ik->addTask(out.jointLimitsTask, "joint_limits_task", highPriority)); - const Eigen::VectorXd newWeight = 10 * weightRegularization; - auto newWeightProvider = std::make_shared(newWeight); - REQUIRE(out.ik->setTaskWeight("regularization_task", newWeightProvider)); - - auto outWeightProvider = out.ik->getTaskWeightProvider("regularization_task").lock(); - REQUIRE(outWeightProvider); - REQUIRE(outWeightProvider->getOutput().isApprox(newWeight)); - - REQUIRE(out.ik->setTaskWeight("regularization_task", weightRegularization)); + out.distanceTask = std::make_shared(); + REQUIRE(out.distanceTask->setKinDyn(kinDyn)); + REQUIRE(out.distanceTask->initialize(handler->getGroup("DISTANCE_TASK"))); - REQUIRE(out.ik->finalize(variables)); + out.gravityTask = std::make_shared(); + REQUIRE(out.gravityTask->setKinDyn(kinDyn)); + REQUIRE(out.gravityTask->initialize(handler->getGroup("GRAVITY_TASK"))); return out; } @@ -226,10 +237,31 @@ DesiredSetPoints getDesiredReference(std::shared_ptrgetCenterOfMassPosition()); - const std::string controlledFrame = kinDyn->model().getFrameName(numberOfJoints); - out.endEffectorPose = toManifPose(kinDyn->getWorldTransform(controlledFrame)); + out.endEffectorFrame = kinDyn->model().getFrameName(numberOfJoints); + out.endEffectorPose = toManifPose(kinDyn->getWorldTransform(out.endEffectorFrame)); out.joints.resize(jointsPos.size()); out.joints = iDynTree::toEigen(jointsPos); + iDynTree::LinkIndex indexDistance, indexGravity; + + // Find a frame sufficiently far away from those used for the distance and SE3 task + do + { + out.targetFrameGravity = iDynTree::getRandomLinkOfModel(kinDyn->model()); + out.targetFrameDistance = iDynTree::getRandomLinkOfModel(kinDyn->model()); + indexGravity = kinDyn->model().getFrameIndex(out.targetFrameGravity); + indexDistance = kinDyn->model().getFrameIndex(out.targetFrameDistance); + } while (std::abs(indexGravity - kinDyn->model().getFrameIndex(out.endEffectorFrame)) < 5 + || std::abs(indexDistance - kinDyn->model().getFrameIndex(out.endEffectorFrame)) < 5 + || std::abs(indexGravity - indexDistance) < 5 || indexGravity < 3 + || indexDistance < 3); + + out.targetDistance + = toManifPose(kinDyn->getWorldTransform(out.targetFrameDistance)).translation().norm(); + + // 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; } @@ -250,7 +282,8 @@ System getSystem(std::shared_ptr kinDyn) Eigen::VectorXd jointPositions(kinDyn->getNrOfDegreesOfFreedom()); Eigen::Vector3d gravity; - REQUIRE(kinDyn->getRobotState(basePose, jointPositions, baseVelocity, jointVelocities, gravity)); + REQUIRE( + kinDyn->getRobotState(basePose, jointPositions, baseVelocity, jointVelocities, gravity)); // perturb the joint position for (int i = 0; i < kinDyn->getNrOfDegreesOfFreedom(); i++) @@ -258,15 +291,23 @@ System getSystem(std::shared_ptr kinDyn) jointPositions[i] += distribution(generator); } + Eigen::AngleAxisd baseAngleAxis(basePose.topLeftCorner<3, 3>()); + + double newAngle = baseAngleAxis.angle() + distribution(generator); + + Eigen::AngleAxisd newBaseRotation(newAngle, baseAngleAxis.axis()); + out.dynamics = std::make_shared(); - out.dynamics->setState({basePose.topRightCorner<3, 1>(), - toManifRot(basePose.topLeftCorner<3, 3>()), - jointPositions}); + out.dynamics->setState( + {basePose.topRightCorner<3, 1>(), toManifRot(newBaseRotation.matrix()), jointPositions}); out.integrator = std::make_shared>(); REQUIRE(out.integrator->setIntegrationStep(dT)); out.integrator->setDynamicalSystem(out.dynamics); + REQUIRE( + kinDyn->setRobotState(basePose, jointPositions, baseVelocity, jointVelocities, gravity)); + return out; } @@ -275,7 +316,9 @@ TEST_CASE("QP-IK") auto kinDyn = std::make_shared(); auto parameterHandler = createParameterHandler(); - constexpr double tolerance = 1e-1; + constexpr double tolerance = 2e-1; + constexpr std::size_t highPriority = 0; + constexpr std::size_t lowPriority = 1; // set the velocity representation REQUIRE(kinDyn->setFrameVelocityRepresentation( @@ -298,10 +341,17 @@ TEST_CASE("QP-IK") auto system = getSystem(kinDyn); // Set the frame name - const std::string controlledFrame = model.getFrameName(numberOfJoints); parameterHandler->getGroup("SE3_TASK") .lock() - ->setParameter("frame_name", controlledFrame); + ->setParameter("frame_name", desiredSetPoints.endEffectorFrame); + + parameterHandler->getGroup("DISTANCE_TASK") + .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; @@ -310,13 +360,42 @@ TEST_CASE("QP-IK") system, Eigen::VectorXd::Constant(kinDyn->model().getNrOfDOFs(), jointLimitDelta)); - auto ikAndTasks = createIK(parameterHandler, kinDyn, variablesHandler); + auto ikTasks = createIKTasks(parameterHandler, kinDyn); + + Eigen::VectorXd weightRegularization; - REQUIRE(ikAndTasks.se3Task->setSetPoint(desiredSetPoints.endEffectorPose, - manif::SE3d::Tangent::Zero())); - REQUIRE(ikAndTasks.comTask->setSetPoint(desiredSetPoints.CoMPosition, - Eigen::Vector3d::Zero())); - REQUIRE(ikAndTasks.regularizationTask->setSetPoint(desiredSetPoints.joints)); + auto ik = std::make_shared(); + REQUIRE(ik->initialize(parameterHandler)); + REQUIRE(ik->addTask(ikTasks.se3Task, "se3_task", highPriority)); + REQUIRE(ik->addTask(ikTasks.comTask, "com_task", highPriority)); + + REQUIRE(parameterHandler->getGroup("REGULARIZATION_TASK") + .lock() + ->getParameter("weight", weightRegularization)); + REQUIRE(ik->addTask(ikTasks.regularizationTask, + "regularization_task", + lowPriority, + weightRegularization)); + REQUIRE(ik->addTask(ikTasks.jointLimitsTask, "joint_limits_task", highPriority)); + + const Eigen::VectorXd newWeight = 10 * weightRegularization; + auto newWeightProvider + = std::make_shared(newWeight); + REQUIRE(ik->setTaskWeight("regularization_task", newWeightProvider)); + + auto outWeightProvider = ik->getTaskWeightProvider("regularization_task").lock(); + REQUIRE(outWeightProvider); + REQUIRE(outWeightProvider->getOutput().isApprox(newWeight)); + + REQUIRE(ik->setTaskWeight("regularization_task", weightRegularization)); + + REQUIRE(ik->finalize(variablesHandler)); + + REQUIRE(ikTasks.se3Task->setSetPoint(desiredSetPoints.endEffectorPose, + manif::SE3d::Tangent::Zero())); + REQUIRE(ikTasks.comTask->setSetPoint(desiredSetPoints.CoMPosition, + Eigen::Vector3d::Zero())); + REQUIRE(ikTasks.regularizationTask->setSetPoint(desiredSetPoints.joints)); // propagate the inverse kinematics for constexpr std::size_t iterations = 30; @@ -341,11 +420,11 @@ TEST_CASE("QP-IK") gravity)); // solve the IK - REQUIRE(ikAndTasks.ik->advance()); + REQUIRE(ik->advance()); // get the output of the IK - baseVelocity = ikAndTasks.ik->getOutput().baseVelocity.coeffs(); - jointVelocity = ikAndTasks.ik->getOutput().jointVelocity; + baseVelocity = ik->getOutput().baseVelocity.coeffs(); + jointVelocity = ik->getOutput().jointVelocity; // propagate the dynamical system system.dynamics->setControlInput({baseVelocity, jointVelocity}); @@ -353,11 +432,13 @@ TEST_CASE("QP-IK") } // check the CoM position - REQUIRE(desiredSetPoints.CoMPosition.isApprox(toEigen(kinDyn->getCenterOfMassPosition()), - tolerance)); + REQUIRE( + desiredSetPoints.CoMPosition.isApprox(toEigen(kinDyn->getCenterOfMassPosition()), + tolerance)); // Check the end-effector pose error - const manif::SE3d endEffectorPose = toManifPose(kinDyn->getWorldTransform(controlledFrame)); + const manif::SE3d endEffectorPose + = toManifPose(kinDyn->getWorldTransform(desiredSetPoints.endEffectorFrame)); // please read it as (log(desiredSetPoints.endEffectorPose^-1 * endEffectorPose))^v const manif::SE3d::Tangent error = endEffectorPose - desiredSetPoints.endEffectorPose; @@ -366,13 +447,14 @@ TEST_CASE("QP-IK") } } - TEST_CASE("QP-IK [With strict limits]") { auto kinDyn = std::make_shared(); auto parameterHandler = createParameterHandler(); constexpr double tolerance = 1e-2; + constexpr std::size_t highPriority = 0; + constexpr std::size_t lowPriority = 1; // set the velocity representation REQUIRE(kinDyn->setFrameVelocityRepresentation( @@ -393,8 +475,17 @@ TEST_CASE("QP-IK [With strict limits]") auto system = getSystem(kinDyn); // Set the frame name - const std::string controlledFrame = model.getFrameName(numberOfJoints); - parameterHandler->getGroup("SE3_TASK").lock()->setParameter("frame_name", controlledFrame); + parameterHandler->getGroup("SE3_TASK") + .lock() + ->setParameter("frame_name", desiredSetPoints.endEffectorFrame); + + parameterHandler->getGroup("DISTANCE_TASK") + .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; @@ -406,12 +497,41 @@ TEST_CASE("QP-IK [With strict limits]") limitsDelta(3) = 0; finalizeParameterHandler(parameterHandler, kinDyn, system, limitsDelta); - auto ikAndTasks = createIK(parameterHandler, kinDyn, variablesHandler); + auto ikTasks = createIKTasks(parameterHandler, kinDyn); + + Eigen::VectorXd weightRegularization; + + auto ik = std::make_shared(); + REQUIRE(ik->initialize(parameterHandler)); + REQUIRE(ik->addTask(ikTasks.se3Task, "se3_task", highPriority)); + REQUIRE(ik->addTask(ikTasks.comTask, "com_task", highPriority)); + + REQUIRE(parameterHandler->getGroup("REGULARIZATION_TASK") + .lock() + ->getParameter("weight", weightRegularization)); + REQUIRE(ik->addTask(ikTasks.regularizationTask, + "regularization_task", + lowPriority, + weightRegularization)); + REQUIRE(ik->addTask(ikTasks.jointLimitsTask, "joint_limits_task", highPriority)); + + const Eigen::VectorXd newWeight = 10 * weightRegularization; + auto newWeightProvider + = std::make_shared(newWeight); + REQUIRE(ik->setTaskWeight("regularization_task", newWeightProvider)); - REQUIRE(ikAndTasks.se3Task->setSetPoint(desiredSetPoints.endEffectorPose, - manif::SE3d::Tangent::Zero())); - REQUIRE(ikAndTasks.comTask->setSetPoint(desiredSetPoints.CoMPosition, Eigen::Vector3d::Zero())); - REQUIRE(ikAndTasks.regularizationTask->setSetPoint(desiredSetPoints.joints)); + auto outWeightProvider = ik->getTaskWeightProvider("regularization_task").lock(); + REQUIRE(outWeightProvider); + REQUIRE(outWeightProvider->getOutput().isApprox(newWeight)); + + REQUIRE(ik->setTaskWeight("regularization_task", weightRegularization)); + + REQUIRE(ik->finalize(variablesHandler)); + + REQUIRE(ikTasks.se3Task->setSetPoint(desiredSetPoints.endEffectorPose, + manif::SE3d::Tangent::Zero())); + REQUIRE(ikTasks.comTask->setSetPoint(desiredSetPoints.CoMPosition, Eigen::Vector3d::Zero())); + REQUIRE(ikTasks.regularizationTask->setSetPoint(desiredSetPoints.joints)); // propagate the inverse kinematics for constexpr std::size_t iterations = 30; @@ -442,11 +562,11 @@ TEST_CASE("QP-IK [With strict limits]") gravity)); // solve the IK - REQUIRE(ikAndTasks.ik->advance()); + REQUIRE(ik->advance()); // get the output of the IK - baseVelocity = ikAndTasks.ik->getOutput().baseVelocity.coeffs(); - jointVelocity = ikAndTasks.ik->getOutput().jointVelocity; + baseVelocity = ik->getOutput().baseVelocity.coeffs(); + jointVelocity = ik->getOutput().jointVelocity; // given the joint constraint specified we check that the velocity is satisfied. This should // be zero @@ -459,13 +579,12 @@ TEST_CASE("QP-IK [With strict limits]") } } - TEST_CASE("QP-IK [With builder]") { auto kinDyn = std::make_shared(); auto parameterHandler = createParameterHandler(); - constexpr double tolerance = 1e-1; + constexpr double tolerance = 2e-1; // set the velocity representation REQUIRE(kinDyn->setFrameVelocityRepresentation( @@ -477,7 +596,7 @@ TEST_CASE("QP-IK [With builder]") const iDynTree::Model model = iDynTree::getRandomModel(numberOfJoints); REQUIRE(kinDyn->loadRobotModel(model)); - /////// VariableHandler and IK params + // VariableHandler and IK params auto variablesParameterHandler = std::make_shared(); variablesParameterHandler->setParameter("variables_name", std::vector{robotVelocity}); @@ -496,8 +615,17 @@ TEST_CASE("QP-IK [With builder]") auto system = getSystem(kinDyn); // Set the frame name - const std::string controlledFrame = model.getFrameName(numberOfJoints); - parameterHandler->getGroup("SE3_TASK").lock()->setParameter("frame_name", controlledFrame); + parameterHandler->getGroup("SE3_TASK") + .lock() + ->setParameter("frame_name", desiredSetPoints.endEffectorFrame); + + parameterHandler->getGroup("DISTANCE_TASK") + .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; @@ -511,7 +639,6 @@ TEST_CASE("QP-IK [With builder]") auto [variablesHandler, weights, ik] = QPInverseKinematics::build(parameterHandler, kinDyn); REQUIRE_FALSE(ik == nullptr); - auto se3Task = std::dynamic_pointer_cast(ik->getTask("SE3_TASK").lock()); REQUIRE_FALSE(se3Task == nullptr); REQUIRE(se3Task->setSetPoint(desiredSetPoints.endEffectorPose, manif::SE3d::Tangent::Zero())); @@ -525,6 +652,16 @@ TEST_CASE("QP-IK [With builder]") REQUIRE_FALSE(regularizationTask == nullptr); REQUIRE(regularizationTask->setSetPoint(desiredSetPoints.joints)); + auto distanceTask + = std::dynamic_pointer_cast(ik->getTask("DISTANCE_TASK").lock()); + 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 = 30; Eigen::Vector3d gravity; @@ -551,7 +688,7 @@ TEST_CASE("QP-IK [With builder]") // get the output of the IK baseVelocity = ik->getOutput().baseVelocity.coeffs(); - jointVelocity =ik->getOutput().jointVelocity; + jointVelocity = ik->getOutput().jointVelocity; // propagate the dynamical system system.dynamics->setControlInput({baseVelocity, jointVelocity}); @@ -562,7 +699,6 @@ TEST_CASE("QP-IK [With builder]") = std::dynamic_pointer_cast( ik->getTaskWeightProvider("REGULARIZATION_TASK").lock()); - REQUIRE(weightProvider != nullptr); // check the CoM position @@ -570,9 +706,151 @@ TEST_CASE("QP-IK [With builder]") tolerance)); // Check the end-effector pose error - const manif::SE3d endEffectorPose = toManifPose(kinDyn->getWorldTransform(controlledFrame)); + const manif::SE3d endEffectorPose + = toManifPose(kinDyn->getWorldTransform(desiredSetPoints.endEffectorFrame)); // please read it as (log(desiredSetPoints.endEffectorPose^-1 * endEffectorPose))^v 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)); +} + +TEST_CASE("QP-IK [Distance and Gravity tasks]") +{ + auto kinDyn = std::make_shared(); + auto parameterHandler = createParameterHandler(); + + constexpr double tolerance = 2e-2; + constexpr std::size_t highPriority = 0; + constexpr std::size_t lowPriority = 1; + + // set the velocity representation + REQUIRE(kinDyn->setFrameVelocityRepresentation( + iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)); + + constexpr std::size_t numberOfJoints = 30; + + // create the model + const iDynTree::Model model = iDynTree::getRandomModel(numberOfJoints); + REQUIRE(kinDyn->loadRobotModel(model)); + + const auto desiredSetPoints = getDesiredReference(kinDyn, numberOfJoints); + + // Instantiate the handler + VariablesHandler variablesHandler; + variablesHandler.addVariable(robotVelocity, model.getNrOfDOFs() + 6); + + auto system = getSystem(kinDyn); + + // Set the frame name + parameterHandler->getGroup("SE3_TASK") + .lock() + ->setParameter("frame_name", desiredSetPoints.endEffectorFrame); + + parameterHandler->getGroup("DISTANCE_TASK") + .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, + kinDyn, + system, + Eigen::VectorXd::Constant(kinDyn->model().getNrOfDOFs(), + jointLimitDelta)); + auto ikTasks = createIKTasks(parameterHandler, kinDyn); + + Eigen::VectorXd weightRegularization; + + auto ik = std::make_shared(); + REQUIRE(ik->initialize(parameterHandler)); + + REQUIRE(parameterHandler->getGroup("REGULARIZATION_TASK") + .lock() + ->getParameter("weight", weightRegularization)); + REQUIRE(ik->addTask(ikTasks.regularizationTask, + "regularization_task", + lowPriority, + weightRegularization)); + REQUIRE(ik->addTask(ikTasks.distanceTask, "distance_task", highPriority)); + + REQUIRE(ik->addTask(ikTasks.gravityTask, "gravity_task", highPriority)); + + REQUIRE(ik->finalize(variablesHandler)); + + REQUIRE(ikTasks.regularizationTask->setSetPoint(desiredSetPoints.joints)); + + REQUIRE(ikTasks.distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + + REQUIRE(ikTasks.gravityTask->setDesiredGravityDirectionInTargetFrame( + desiredSetPoints.desiredBodyGravity)); + + // propagate the inverse kinematics for + constexpr std::size_t iterations = 30; + Eigen::Vector3d gravity; + gravity << 0, 0, -9.81; + Eigen::Matrix4d baseTransform = Eigen::Matrix4d::Identity(); + Eigen::Matrix baseVelocity = Eigen::Matrix::Zero(); + Eigen::VectorXd jointVelocity = Eigen::VectorXd::Zero(model.getNrOfDOFs()); + + for (std::size_t iteration = 0; iteration < iterations; iteration++) + { + // get the solution of the integrator + const auto& [basePosition, baseRotation, jointPosition] = system.integrator->getSolution(); + + // update the KinDynComputations object + baseTransform.topLeftCorner<3, 3>() = baseRotation.rotation(); + baseTransform.topRightCorner<3, 1>() = basePosition; + REQUIRE(kinDyn->setRobotState(baseTransform, + jointPosition, + baseVelocity, + jointVelocity, + gravity)); + + // solve the IK + REQUIRE(ik->advance()); + + // get the output of the IK + baseVelocity = ik->getOutput().baseVelocity.coeffs(); + jointVelocity = ik->getOutput().jointVelocity; + + // propagate the dynamical system + system.dynamics->setControlInput({baseVelocity, jointVelocity}); + system.integrator->integrate(0s, dT); + } + + // Check the distance error + REQUIRE(toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameDistance)) + .translation() + .norm() + == Catch::Approx(desiredSetPoints.targetDistance).epsilon(tolerance)); + + // Check the gravity error + Eigen::Vector3d gravityError; + gravityError = toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameGravity)) + .rotation() + .matrix() + .row(2) + .transpose() + - desiredSetPoints.desiredBodyGravity; + REQUIRE(gravityError.isZero(tolerance)); }