diff --git a/CHANGELOG.md b/CHANGELOG.md index 8cac9377776..86e0c6f5838 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,12 @@ All notable changes to this project will be documented in this file. The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). +## [Unreleased] + +### Added +- Add the possibility to use `MatrixView` and `Span` as input/output objects for `InverseKinematics` class (https://github.com/robotology/idyntree/pull/822). +### Changed + ## [3.0.0] - 2020-02-03 ### Added diff --git a/src/high-level/src/KinDynComputations.cpp b/src/high-level/src/KinDynComputations.cpp index 6bd39d4418a..caa18c3244a 100644 --- a/src/high-level/src/KinDynComputations.cpp +++ b/src/high-level/src/KinDynComputations.cpp @@ -934,9 +934,7 @@ void KinDynComputations::getRobotState(iDynTree::VectorDynSize &s, iDynTree::VectorDynSize &s_dot, iDynTree::Vector3& world_gravity) { - world_gravity = pimpl->m_gravityAcc; - toEigen(s) = toEigen(this->pimpl->m_pos.jointPos()); - toEigen(s_dot) = toEigen(pimpl->m_vel.jointVel()); + this->getRobotState(make_span(s), make_span(s_dot), make_span(world_gravity)); } void KinDynComputations::getRobotState(iDynTree::Span s, diff --git a/src/inverse-kinematics/include/iDynTree/InverseKinematics.h b/src/inverse-kinematics/include/iDynTree/InverseKinematics.h index f02fa3b904f..3c66fa8c341 100644 --- a/src/inverse-kinematics/include/iDynTree/InverseKinematics.h +++ b/src/inverse-kinematics/include/iDynTree/InverseKinematics.h @@ -128,7 +128,7 @@ class iDynTree::InverseKinematics * @brief set the kinematic model to be used in the optimization * * All the degrees of freedom listed in the second parameters will be used as - * optimization variables. + * optimization variables. * If the vector is empty, all the joints will be used. * * @note you may want to simplify the model by calling @@ -139,7 +139,7 @@ class iDynTree::InverseKinematics */ bool setModel(const iDynTree::Model &model, const std::vector &consideredJoints = std::vector()); - + /*! * Set new joint limits * \author Yue Hu @@ -147,7 +147,7 @@ class iDynTree::InverseKinematics * @return true if successfull, false otherwise */ bool setJointLimits(std::vector >& jointLimits); - + /*! * Get current joint limits * \author Yue Hu @@ -180,6 +180,24 @@ class iDynTree::InverseKinematics bool setCurrentRobotConfiguration(const iDynTree::Transform& baseConfiguration, const iDynTree::VectorDynSize& jointConfiguration); + /*! + * Sets the robot current configuration + * + * + * @param baseConfiguration the 4x4 homogeneous transformation that transforms position vectors expressed in the base reference frame + * in position frames expressed in the world reference frame (i.e. pos_world = baseConfiguration*pos_base). + * @param robotConfiguration the robot configuration + * + * @note the size (and order) of jointConfiguration must match the joints in the model, not + * in the consideredJoints variable + * + * @warning the Span and the MatrixView objects should point an already existing memory. Memory allocation and resizing cannot be achieved with this kind of objects. + * @return true if successful, false otherwise. + */ + bool setCurrentRobotConfiguration(iDynTree::MatrixView baseConfiguration, + iDynTree::Span jointConfiguration); + + /*! * Set configuration for the specified joint * @@ -308,6 +326,22 @@ class iDynTree::InverseKinematics bool addFrameConstraint(const std::string& frameName, const iDynTree::Transform& constraintValue); + /*! + * Adds a (constancy) constraint for the specified frame + * + * The homogeneous trasformation of the specified frame w.r.t. the inertial frame + * will remain constant and equal to the specified second parameter + * + * @param frameName the name of the frame on which to attach the constraint + * @param constraintValue the 4x4 homogeneous transformation to associate to the constraint. + * + * @warning the Span and the MatrixView objects should point an already existing memory. Memory allocation and resizing cannot be achieved with this kind of objects. + * @return true if successful, false otherwise. + */ + bool addFrameConstraint(const std::string& frameName, + iDynTree::MatrixView constraintValue); + + /*! * Adds a (constancy) position constraint for the specified frame * @@ -320,6 +354,20 @@ class iDynTree::InverseKinematics bool addFramePositionConstraint(const std::string& frameName, const iDynTree::Position& constraintValue); + /*! + * Adds a (constancy) position constraint for the specified frame + * + * Only the position component of the frame is constrained + * @param frameName the name of the frame on which to attach the constraint + * @param constraintValue the position associated to the constraint + * + * @warning the Span and the MatrixView objects should point an already existing memory. Memory allocation and resizing cannot be achieved with this kind of objects. + * @return true if successful, false otherwise. + */ + bool addFramePositionConstraint(const std::string& frameName, + iDynTree::Span constraintValue); + + /*! * Adds a (constancy) position constraint for the specified frame * @@ -332,6 +380,20 @@ class iDynTree::InverseKinematics bool addFramePositionConstraint(const std::string& frameName, const iDynTree::Transform& constraintValue); + /*! + * Adds a (constancy) position constraint for the specified frame + * + * Only the position component of the frame is constrained + * @param frameName the name of the frame on which to attach the constraint + * @param constraintValue the 4x4 homogeneous transformation to associate to the constraint. + * + * @warning the Span and the MatrixView objects should point an already existing memory. Memory allocation and resizing cannot be achieved with this kind of objects. + * @return true if successful, false otherwise. + */ + bool addFramePositionConstraint(const std::string& frameName, + iDynTree::MatrixView constraintValue); + + /*! * Adds a (constancy) orientation constraint for the specified frame * @@ -344,6 +406,19 @@ class iDynTree::InverseKinematics bool addFrameRotationConstraint(const std::string& frameName, const iDynTree::Rotation& constraintValue); + /*! + * Adds a (constancy) orientation constraint for the specified frame + * + * Only the orientation component of the frame is constrained + * @param frameName the name of the frame on which to attach the constraint + * @param constraintValue if constraintValue is a 4x4 matrix is considered as homogeneous transformation, if it is a 3x3 is considered as rotation matrix. + * + * @warning the Span and the MatrixView objects should point an already existing memory. Memory allocation and resizing cannot be achieved with this kind of objects. + * @return true if successful, false otherwise. + */ + bool addFrameRotationConstraint(const std::string& frameName, + iDynTree::MatrixView constraintValue); + /*! * Adds a (constancy) orientation constraint for the specified frame * @@ -356,6 +431,25 @@ class iDynTree::InverseKinematics bool addFrameRotationConstraint(const std::string& frameName, const iDynTree::Transform& constraintValue); + /*! + * Activate a given constraint previously added with an addFrame**Constraint method. + * + * \note In this version of iDynTree, it is not possible to change the nature of the constraint + * (Full, Position or Rotation) when activating it again. + * + * @note This method returns true even if the frame constraint was already activate, it only + * returns false if the constraint was never added. + * @warning This method is not meant to be called at each IK loop, and it can increase the computational + * time of the next call to solve. + * + * @param frameName the name of the frame on which to attach the constraint + * @param newConstraintValue the 4x4 homogeneous transformation to associate to the pose of the constrained frame (r) in the world frame (w), i.e. ʷHᵣ. + * @warning the Span and the MatrixView objects should point an already existing memory. Memory allocation and resizing cannot be achieved with this kind of objects. + * @return true if successful, false otherwise. + */ + bool activateFrameConstraint(const std::string& frameName, + iDynTree::MatrixView newConstraintValue); + /*! * Activate a given constraint previously added with an addFrame**Constraint method. * @@ -373,6 +467,8 @@ class iDynTree::InverseKinematics */ bool activateFrameConstraint(const std::string& frameName, const Transform& newConstraintValue); + + /*! * Deactivate a given constraint previously added with an addFrame**Constraint method. * @@ -477,6 +573,24 @@ class iDynTree::InverseKinematics const double positionWeight=1.0, const double rotationWeight=1.0); + /*! + * Adds a target for the specified frame + * + * @param frameName the name of the frame which represents the target + * @param targetValue the 4x4 homogeneous transformation to associate to target. + * @param[in] positionWeight if the position part of the target is handled as + * a term in the cost function, this specify the weight + * of this term in the cost function. Default value is 1.0 + * @param[in] rotationWeight if the rotation part of the target is handled as + * a term in the cost function, this specify the weight + * of this term in the cost function. Default value is 1.0 + * @return true if successful, false otherwise. + */ + bool addTarget(const std::string& frameName, + iDynTree::MatrixView targetValue, + const double positionWeight=1.0, + const double rotationWeight=1.0); + /*! * Adds a position (3D) target for the specified frame * @@ -491,6 +605,21 @@ class iDynTree::InverseKinematics const iDynTree::Position& targetValue, const double positionWeight=1.0); + /*! + * Adds a position (3D) target for the specified frame + * + * @param frameName the name of the frame which represents the target + * @param targetValue value that the origin of the frame frameName should reach + * @param[in] positionWeight if the position part of the target is handled as + * a term in the cost function, this specify the weight + * of this term in the cost function. Default value is 1.0 + * @return true if successful, false otherwise. + */ + bool addPositionTarget(const std::string& frameName, + iDynTree::Span targetValue, + const double positionWeight=1.0); + + /*! * Adds a position (3D) target for the specified frame * @@ -507,6 +636,23 @@ class iDynTree::InverseKinematics const iDynTree::Transform& targetValue, const double positionWeight=1.0); + /*! + * Adds a target for the specified frame + * + * @param frameName the name of the frame which represents the target + * @param targetValue the 4x4 homogeneous transformation to associate to target. + * @param[in] positionWeight if the position part of the target is handled as + * a term in the cost function, this specify the weight + * of this term in the cost function. Default value is 1.0 + * @param[in] rotationWeight if the rotation part of the target is handled as + * a term in the cost function, this specify the weight + * of this term in the cost function. Default value is 1.0 + * @return true if successful, false otherwise. + */ + bool addPositionTarget(const std::string& frameName, + iDynTree::MatrixView targetValue, + const double positionWeight=1.0); + /*! * Adds an orientation target for the specified frame * @@ -546,6 +692,33 @@ class iDynTree::InverseKinematics const iDynTree::Transform& targetValue, const double rotationWeight=1.0); + /*! + * Adds an orientation target for the specified frame + * + * \note only the orientation component of the targetValue parameter + * will be considered + * + * This call is equivalent to call + * @code + * addRotationTarget(frameName, targetValue.rotation()); + * @endcode + * @see + * addRotationTarget(const std::string&, const iDynTree::Rotation&) + * addTarget(const std::string&, const iDynTree::Transform&) + * + * @param frameName the name of the frame which represents the target + * @param targetValue if constraintValue is a 4x4 matrix is considered as homogeneous + transformation, if it is a 3x3 is considered as rotation matrix. + * @param[in] rotationWeight if the rotation part of the target is handled as + * a term in the cost function, this specify the weight + * of this term in the cost function. Default value is 1.0 + * @return true if successful, false otherwise. + */ + bool addRotationTarget(const std::string& frameName, + iDynTree::MatrixView targetValue, + const double rotationWeight=1.0); + + /*! * Update the desired target and weights for the specified frame. * @@ -564,6 +737,24 @@ class iDynTree::InverseKinematics const double positionWeight=-1.0, const double rotationWeight=-1.0); + /*! + * Update the desired target and weights for the specified frame. + * + * @param frameName the name of the frame which represents the target + * @param targetValue 4x4 homogeneous transformation to associate to target. + * @param[in] positionWeight if the position part of the target is handled as + * a term in the cost function, this specify the weight + * of this term in the cost function. Default value is the the last one previously set. + * @param[in] rotationWeight if the rotation part of the target is handled as + * a term in the cost function, this specify the weight + * of this term in the cost function. Default value is the the last one previously set. + * @return true if successful, false otherwise, for example if the specified frame target was not previously added with addTarget . + */ + bool updateTarget(const std::string& frameName, + iDynTree::MatrixView targetValue, + const double positionWeight=-1.0, + const double rotationWeight=-1.0); + /*! * Update the position (3D) target for the specified frame * @@ -577,6 +768,22 @@ class iDynTree::InverseKinematics bool updatePositionTarget(const std::string& frameName, const iDynTree::Position& targetValue, const double positionWeight=-1.0); + + + /*! + * Update the position (3D) target for the specified frame + * + * @param frameName the name of the frame which represents the target + * @param targetValue value that the origin of the frame frameName should reach + * @param[in] positionWeight if the position part of the target is handled as + * a term in the cost function, this specify the weight + * of this term in the cost function. Default value is the last one previously set. + * @return true if successful, false otherwise, for example if the specified frame target was not previously added with addTarget or addPositionTarget . + */ + bool updatePositionTarget(const std::string& frameName, + iDynTree::Span targetValue, + const double positionWeight=-1.0); + /*! * Update an orientation target for the specified frame * @@ -591,6 +798,20 @@ class iDynTree::InverseKinematics const iDynTree::Rotation& targetValue, const double rotationWeight=-1.0); + /*! + * Update an orientation target for the specified frame + * + * @param frameName the name of the frame which represents the target + * @param targetValue 3x3 rotation matrix representing the orientation of the frame + frameName should reach + * @param[in] rotationWeight if the rotation part of the target is handled as + * a term in the cost function, this specify the weight + * of this term in the cost function. Default value is the last one previously set. + * @return true if successful, false otherwise, for example if the specified frame target was not previously added with addTarget or addRotationTarget . + */ + bool updateRotationTarget(const std::string& frameName, + iDynTree::MatrixView targetValue, + const double rotationWeight=-1.0); /*! * Specify the default method to solve all the specified targets @@ -603,7 +824,7 @@ class iDynTree::InverseKinematics * function. Existing targets will remain unchanged * * @param mode the target resolution mode - * + * * @return true if successful, false otherwise. */ void setDefaultTargetResolutionMode(enum iDynTree::InverseKinematicsTreatTargetAsConstraint mode); @@ -631,8 +852,8 @@ class iDynTree::InverseKinematics * @param targetName the name (frame) identified the target * @return true if successful, false otherwise, for example if the specified frame target was not previously added with addTarget or addRotationTarget . */ - bool setTargetResolutionMode(const std::string& targetName, enum InverseKinematicsTreatTargetAsConstraint mode); - + bool setTargetResolutionMode(const std::string& targetName, + enum InverseKinematicsTreatTargetAsConstraint mode); /*! * Return the target resolution mode @@ -660,6 +881,23 @@ class iDynTree::InverseKinematics */ bool setDesiredFullJointsConfiguration(const iDynTree::VectorDynSize& desiredJointConfiguration, double weight=-1.0); + /*! + * Sets a desired final configuration for all the robot joints. + * + * The solver will try to obtain solutions as similar to the specified configuration as possible + * + * @note the desiredJointConfiguration have the same serialisation of the joints in the specified model + * + * @param[in] desiredJointConfiguration configuration for the joints + * @param[in] weight weight for the joint configuration cost. + * If it is not passed, the previous passed value will be mantained. + * If the value was never passed, its value is 1e-6 . + * + * @return true if successful, false otherwise. + */ + bool setDesiredFullJointsConfiguration(iDynTree::Span desiredJointConfiguration, + double weight = -1.0); + /*! * Sets a desired final configuration for all the robot joints. * @@ -677,6 +915,42 @@ class iDynTree::InverseKinematics */ bool setDesiredFullJointsConfiguration(const iDynTree::VectorDynSize& desiredJointConfiguration, const iDynTree::VectorDynSize& weights); + /*! + * Sets a desired final configuration for all the robot joints. + * + * The solver will try to obtain solutions as similar to the specified configuration as possible + * + * @note the desiredJointConfiguration have the same serialisation of the joints in the specified model + * + * @param[in] desiredJointConfiguration configuration for the joints + * @param[in] weights Joint-wise weights for the joint configuration cost. + * This vector should have the same dimension of the desiredJointConfiguration. + * If one of its elements is negative, the previous value will be kept. + * If the value was never passed, its value is 1e-6, equal for all joints. + * + * @return true if successful, false otherwise. + */ + bool setDesiredFullJointsConfiguration(iDynTree::Span desiredJointConfiguration, + iDynTree::Span weights); + + + /*! + * Sets a desired final configuration for the set of considered joints. + * + * The solver will try to obtain solutions as similar to the specified configuration as possible + * + * @note the desiredJointConfiguration have the same order of the joints in the consideredJoints list. + * + * @param[in] desiredJointConfiguration configuration for the joints + * @param[in] weight weight for the joint configuration cost. + * If it is not passed, the previous passed value will be mantained. + * If the value was never passed, its value is 1e-6 . + * + * @return true if successful, false otherwise. + */ + bool setDesiredReducedJointConfiguration(const iDynTree::VectorDynSize& desiredJointConfiguration, + double weight=-1.0); + /*! * Sets a desired final configuration for the set of considered joints. * @@ -691,7 +965,9 @@ class iDynTree::InverseKinematics * * @return true if successful, false otherwise. */ - bool setDesiredReducedJointConfiguration(const iDynTree::VectorDynSize& desiredJointConfiguration, double weight=-1.0); + bool setDesiredReducedJointConfiguration(iDynTree::Span desiredJointConfiguration, + double weight=-1.0); + /*! * Sets a desired final configuration for the set of considered joints. @@ -708,7 +984,27 @@ class iDynTree::InverseKinematics * * @return true if successful, false otherwise. */ - bool setDesiredReducedJointConfiguration(const iDynTree::VectorDynSize& desiredJointConfiguration, const iDynTree::VectorDynSize& weights); + bool setDesiredReducedJointConfiguration(const iDynTree::VectorDynSize& desiredJointConfiguration, + const iDynTree::VectorDynSize& weights); + + /*! + * Sets a desired final configuration for the set of considered joints. + * + * The solver will try to obtain solutions as similar to the specified configuration as possible + * + * @note the desiredJointConfiguration have the same order of the joints in the consideredJoints list. + * + * @param[in] desiredJointConfiguration configuration for the joints + * @param[in] weights Joint-wise weights for the joint configuration cost. + * This vector should have the same dimension of the desiredJointConfiguration. + * If one of its elements is negative, the previous value will be kept. + * If the value was never passed, its value is 1e-6, equal for all joints. + * + * @return true if successful, false otherwise. + */ + bool setDesiredReducedJointConfiguration(iDynTree::Span desiredJointConfiguration, + iDynTree::Span weights); + bool setFullJointsInitialCondition(const iDynTree::Transform* baseTransform, const iDynTree::VectorDynSize* initialCondition); @@ -725,22 +1021,40 @@ class iDynTree::InverseKinematics void getFullJointsSolution(iDynTree::Transform& baseTransformSolution, iDynTree::VectorDynSize& shapeSolution); + bool getFullJointsSolution(iDynTree::MatrixView baseTransformSolution, + iDynTree::Span shapeSolution); + /*! * Return the last solution of the inverse kinematics problem * * This method returns in the shapeSolution variable only the joints that have been optimised - * viz. only the joints specified in the consideredJoints variable in the initialization + * viz. only the joints specified in the consideredJoints variable in the initialization * @param[out] baseTransformSolution solution for the base position * @param[out] shapeSolution solution for the shape (the internal configurations) */ void getReducedSolution(iDynTree::Transform& baseTransformSolution, iDynTree::VectorDynSize& shapeSolution); + /*! + * Return the last solution of the inverse kinematics problem + * + * This method returns in the shapeSolution variable only the joints that have been optimised + * viz. only the joints specified in the consideredJoints variable in the initialization + * @param[out] baseTransformSolution 4x4 homogeneous matrix solution for the base pose + * @param[out] shapeSolution solution for the shape (the internal configurations) + * @warning the Span and the MatrixView objects should point an already existing memory. Memory allocation and resizing cannot be achieved with this kind of objects. + * @return true if successful, false otherwise. + */ + bool getReducedSolution(iDynTree::MatrixView baseTransformSolution, + iDynTree::Span shapeSolution); + ///@} bool getPoseForFrame(const std::string& frameName, iDynTree::Transform& transform); + bool getPoseForFrame(const std::string& frameName, iDynTree::MatrixView transform); + /* Other iKin features: - set joint limits for each joint (different from the one loaded) @@ -757,6 +1071,8 @@ class iDynTree::InverseKinematics void setCOMTarget(iDynTree::Position& desiredPosition, double weight = 1.0); + bool setCOMTarget(iDynTree::Span desiredPosition, double weight = 1.0); + void setCOMAsConstraint(bool asConstraint = true); void setCOMAsConstraintTolerance(double tolerance = 1e-8); @@ -776,6 +1092,8 @@ class iDynTree::InverseKinematics */ void setCOMConstraintProjectionDirection(iDynTree::Vector3 direction); + bool setCOMConstraintProjectionDirection(iDynTree::Span direction); + private: void* m_pimpl; /*!< private implementation */ diff --git a/src/inverse-kinematics/include/private/InverseKinematicsData.h b/src/inverse-kinematics/include/private/InverseKinematicsData.h index 71fe0dac9ac..9c750c6a002 100644 --- a/src/inverse-kinematics/include/private/InverseKinematicsData.h +++ b/src/inverse-kinematics/include/private/InverseKinematicsData.h @@ -68,7 +68,7 @@ class internal::kinematics::InverseKinematicsData { InverseKinematicsInitialConditionPartial, InverseKinematicsInitialConditionFull }; - + public: /*! @name Model-related variables */ @@ -115,14 +115,14 @@ class internal::kinematics::InverseKinematicsData { iDynTree::Direction m_comHullConstraint_xAxisOfPlaneInWorld; iDynTree::Direction m_comHullConstraint_yAxisOfPlaneInWorld; iDynTree::Position m_comHullConstraint_originOfPlaneInWorld; - + //Preferred joints configuration for the optimization //Size: getNrOfDOFs of the considered model iDynTree::VectorDynSize m_preferredJointsConfiguration; iDynTree::VectorDynSize m_preferredJointsWeight; bool m_areBaseInitialConditionsSet; /*!< True if initial condition for the base pose are provided by the user */ - + InverseKinematicsInitialConditionType m_areJointsInitialConditionsSet; /*!< specify if the initial condition for the joints are provided by the user */ //These variables containts the initial condition @@ -325,10 +325,10 @@ class internal::kinematics::InverseKinematicsData { */ iDynTree::KinDynComputations& dynamics(); - void setCoMTarget(iDynTree::Position& desiredPosition, double weight); - + void setCoMTarget(const iDynTree::Position& desiredPosition, double weight); + void setCoMasConstraint(bool asConstraint); - + bool isCoMaConstraint(); void setCoMasConstraintTolerance(double TOL); diff --git a/src/inverse-kinematics/src/InverseKinematics.cpp b/src/inverse-kinematics/src/InverseKinematics.cpp index 458ec1ea1c6..1cab8b1031e 100644 --- a/src/inverse-kinematics/src/InverseKinematics.cpp +++ b/src/inverse-kinematics/src/InverseKinematics.cpp @@ -93,7 +93,7 @@ namespace iDynTree { return missingIpoptErrorReport(); #endif } - + bool InverseKinematics::setJointLimits(std::vector >& jointLimits) { #ifdef IDYNTREE_USES_IPOPT @@ -103,7 +103,7 @@ namespace iDynTree { return missingIpoptErrorReport(); #endif } - + bool InverseKinematics::getJointLimits(std::vector >& jointLimits) { #ifdef IDYNTREE_USES_IPOPT @@ -144,6 +144,21 @@ namespace iDynTree { #endif } + bool InverseKinematics::setCurrentRobotConfiguration(iDynTree::MatrixView baseConfiguration, + iDynTree::Span jointConfiguration) + { + constexpr int expected_transform_cols = 4; + constexpr int expected_transform_rows = 4; + bool ok = (baseConfiguration.rows() == expected_transform_rows) + && (baseConfiguration.cols() == expected_transform_cols); + if( !ok ) + { + reportError("InverseKinematics","getWorldBaseTransform","Wrong size in input world_T_base"); + return false; + } + + return this->setCurrentRobotConfiguration(Transform(baseConfiguration), jointConfiguration); + } bool InverseKinematics::setJointConfiguration(const std::string& jointName, const double jointConfiguration) { @@ -293,6 +308,7 @@ namespace iDynTree { #endif } + bool InverseKinematics::addFrameConstraint(const std::string& frameName, const iDynTree::Transform& constraintValue) { #ifdef IDYNTREE_USES_IPOPT @@ -303,6 +319,22 @@ namespace iDynTree { #endif } + bool InverseKinematics::addFrameConstraint(const std::string &frameName, + iDynTree::MatrixView constraintValue) + { + constexpr int expected_transform_cols = 4; + constexpr int expected_transform_rows = 4; + bool ok = (constraintValue.rows() == expected_transform_rows) + && (constraintValue.cols() == expected_transform_cols); + if( !ok ) + { + reportError("InverseKinematics","addFrameConstraint","Wrong size in input constraintValue"); + return false; + } + + return this->addFrameConstraint(frameName, Transform(constraintValue)); + } + bool InverseKinematics::addFramePositionConstraint(const std::string& frameName, const iDynTree::Position& constraintValue) { #ifdef IDYNTREE_USES_IPOPT @@ -313,6 +345,19 @@ namespace iDynTree { #endif } + bool InverseKinematics::addFramePositionConstraint(const std::string &frameName, iDynTree::Span constraintValue) + { + constexpr int expected_pos_size = 3; + bool ok = (constraintValue.size() == expected_pos_size); + if( !ok ) + { + reportError("InverseKinematics","addFramePositionConstraint","Wrong size in input constraintValue"); + return false; + } + + return this->addFramePositionConstraint(frameName, Position(constraintValue)); + } + bool InverseKinematics::addFramePositionConstraint(const std::string& frameName, const iDynTree::Transform& constraintValue) { #ifdef IDYNTREE_USES_IPOPT @@ -323,6 +368,23 @@ namespace iDynTree { #endif } + + bool InverseKinematics::addFramePositionConstraint(const std::string &frameName, + iDynTree::MatrixView constraintValue) + { + constexpr int expected_transform_cols = 4; + constexpr int expected_transform_rows = 4; + bool ok = (constraintValue.rows() == expected_transform_rows) + && (constraintValue.cols() == expected_transform_cols); + if( !ok ) + { + reportError("InverseKinematics","addFrameConstraint","Wrong size in input constraintValue"); + return false; + } + + return this->addFramePositionConstraint(frameName, Transform(constraintValue)); + } + bool InverseKinematics::addFrameRotationConstraint(const std::string& frameName, const iDynTree::Rotation& constraintValue) { #ifdef IDYNTREE_USES_IPOPT @@ -343,6 +405,32 @@ namespace iDynTree { #endif } + bool InverseKinematics::addFrameRotationConstraint(const std::string& frameName, + iDynTree::MatrixView constraintValue) + { + constexpr int expected_transform_cols = 4; + constexpr int expected_transform_rows = 4; + constexpr int expected_rotation_cols = 3; + constexpr int expected_rotation_rows = 3; + + if ((constraintValue.rows() == expected_transform_rows) + && (constraintValue.cols() == expected_transform_cols)) + { + return this->addFrameRotationConstraint(frameName, Transform(constraintValue)); + } + + if ((constraintValue.rows() == expected_rotation_rows) + && (constraintValue.cols() == expected_rotation_cols)) + { + return this->addFrameRotationConstraint(frameName, Rotation(constraintValue)); + } + + reportError("InverseKinematics", + "addFrameRotationConstraint", + "Wrong size in input constraintValue"); + return false; + } + bool InverseKinematics::activateFrameConstraint(const std::string& frameName, const Transform& newConstraintValue) { #ifdef IDYNTREE_USES_IPOPT @@ -371,6 +459,22 @@ namespace iDynTree { #endif } + bool InverseKinematics::activateFrameConstraint(const std::string& frameName, + iDynTree::MatrixView newConstraintValue) + { + constexpr int expected_transform_cols = 4; + constexpr int expected_transform_rows = 4; + bool ok = (newConstraintValue.rows() == expected_transform_rows) + && (newConstraintValue.cols() == expected_transform_cols); + if( !ok ) + { + reportError("InverseKinematics","activateFrameConstraint","Wrong size in input constraintValue"); + return false; + } + + return this->activateFrameConstraint(frameName, Transform(newConstraintValue)); + } + bool InverseKinematics::deactivateFrameConstraint(const std::string& frameName) { #ifdef IDYNTREE_USES_IPOPT @@ -582,6 +686,26 @@ namespace iDynTree { #endif } + bool InverseKinematics::addTarget(const std::string& frameName, + iDynTree::MatrixView targetValue, + const double positionWeight, + const double rotationWeight) + { + constexpr int expected_transform_cols = 4; + constexpr int expected_transform_rows = 4; + bool ok = (targetValue.rows() == expected_transform_rows) + && (targetValue.cols() == expected_transform_cols); + if (!ok) + { + reportError("InverseKinematics", + "addTarget", + "Wrong size in input targetValue"); + return false; + } + + return this->addTarget(frameName, Transform(targetValue), positionWeight, rotationWeight); + } + bool InverseKinematics::addPositionTarget(const std::string& frameName, const iDynTree::Position& constraintValue, const double positionWeight) { #ifdef IDYNTREE_USES_IPOPT @@ -592,6 +716,23 @@ namespace iDynTree { #endif } + bool InverseKinematics::addPositionTarget(const std::string& frameName, + iDynTree::Span targetValue, + const double positionWeight) + { + constexpr int expected_pos_size = 3; + bool ok = (targetValue.size() == expected_pos_size); + if (!ok) + { + reportError("InverseKinematics", + "addPositionTarget", + "Wrong size in input targetValue"); + return false; + } + + return this->addPositionTarget(frameName, Position(targetValue), positionWeight); + } + bool InverseKinematics::addPositionTarget(const std::string& frameName, const iDynTree::Transform& constraintValue, const double positionWeight) { #ifdef IDYNTREE_USES_IPOPT @@ -602,6 +743,25 @@ namespace iDynTree { #endif } + bool InverseKinematics::addPositionTarget(const std::string& frameName, + iDynTree::MatrixView targetValue, + const double positionWeight) + { + constexpr int expected_transform_cols = 4; + constexpr int expected_transform_rows = 4; + bool ok = (targetValue.rows() == expected_transform_rows) + && (targetValue.cols() == expected_transform_cols); + if (!ok) + { + reportError("InverseKinematics", + "addPositionTarget", + "Wrong size in input targetValue"); + return false; + } + + return this->addPositionTarget(frameName, Transform(targetValue), positionWeight); + } + bool InverseKinematics::addRotationTarget(const std::string& frameName, const iDynTree::Rotation& constraintValue, const double rotationWeight) { #ifdef IDYNTREE_USES_IPOPT @@ -622,6 +782,33 @@ namespace iDynTree { #endif } + bool InverseKinematics::addRotationTarget(const std::string& frameName, + iDynTree::MatrixView targetValue, + const double rotationWeight) + { + constexpr int expected_transform_cols = 4; + constexpr int expected_transform_rows = 4; + constexpr int expected_rotation_cols = 3; + constexpr int expected_rotation_rows = 3; + + if ((targetValue.rows() == expected_transform_rows) + && (targetValue.cols() == expected_transform_cols)) + { + return this->addRotationTarget(frameName, Transform(targetValue), rotationWeight); + } + + if ((targetValue.rows() == expected_rotation_rows) + && (targetValue.cols() == expected_rotation_cols)) + { + return this->addRotationTarget(frameName,Rotation(targetValue),rotationWeight); + } + + reportError("InverseKinematics", + "addRotationTarget", + "Wrong size in input targetValue"); + return false; + } + bool InverseKinematics::updateTarget(const std::string& frameName, const Transform& targetValue, const double positionWeight, @@ -646,6 +833,26 @@ namespace iDynTree { #endif } + bool InverseKinematics::updateTarget(const std::string& frameName, + iDynTree::MatrixView targetValue, + const double positionWeight, + const double rotationWeight) + { + constexpr int expected_transform_cols = 4; + constexpr int expected_transform_rows = 4; + bool ok = (targetValue.rows() == expected_transform_rows) + && (targetValue.cols() == expected_transform_cols); + if (!ok) + { + reportError("InverseKinematics", + "updateTarget", + "Wrong size in input targetValue"); + return false; + } + + return this->updateTarget(frameName, Transform(targetValue), positionWeight, rotationWeight); + } + bool InverseKinematics::updatePositionTarget(const std::string& frameName, const Position& targetValue, const double positionWeight) @@ -668,6 +875,23 @@ namespace iDynTree { #endif } + bool InverseKinematics::updatePositionTarget(const std::string& frameName, + iDynTree::Span targetValue, + const double positionWeight) + { + constexpr int expected_pos_size = 3; + bool ok = (targetValue.size() == expected_pos_size); + if (!ok) + { + reportError("InverseKinematics", + "updatePositionTarget", + "Wrong size in input targetValue"); + return false; + } + + return this->updatePositionTarget(frameName, Position(targetValue), positionWeight); + } + bool InverseKinematics::updateRotationTarget(const std::string& frameName, const Rotation& targetValue, const double rotationWeight) @@ -690,7 +914,29 @@ namespace iDynTree { #endif } - bool InverseKinematics::setDesiredFullJointsConfiguration(const iDynTree::VectorDynSize& desiredJointConfiguration, double weight) + bool InverseKinematics::updateRotationTarget(const std::string& frameName, + iDynTree::MatrixView targetValue, + const double rotationWeight) + { + constexpr int expected_rotation_cols = 3; + constexpr int expected_rotation_rows = 3; + + const bool ok = ((targetValue.rows() == expected_rotation_rows) + && (targetValue.cols() == expected_rotation_cols)); + + if (!ok) + { + reportError("InverseKinematics", + "updateRotationTarget", + "Wrong size in input targetValue"); + return false; + } + + return this->updateRotationTarget(frameName,Rotation(targetValue),rotationWeight); + } + + bool InverseKinematics::setDesiredFullJointsConfiguration(iDynTree::Span desiredJointConfiguration, + double weight) { #ifdef IDYNTREE_USES_IPOPT assert(m_pimpl); @@ -705,7 +951,14 @@ namespace iDynTree { #endif } - bool InverseKinematics::setDesiredFullJointsConfiguration(const VectorDynSize &desiredJointConfiguration, const VectorDynSize &weights) + bool InverseKinematics::setDesiredFullJointsConfiguration(const iDynTree::VectorDynSize& desiredJointConfiguration, + double weight) + { + return this->setDesiredFullJointsConfiguration(make_span(desiredJointConfiguration), weight); + } + + bool InverseKinematics::setDesiredFullJointsConfiguration(iDynTree::Span desiredJointConfiguration, + iDynTree::Span weights) { #ifdef IDYNTREE_USES_IPOPT assert(m_pimpl); @@ -720,8 +973,8 @@ namespace iDynTree { assert(IK_PIMPL(m_pimpl)->m_preferredJointsWeight.size() == weights.size()); for (unsigned int i = 0; i < weights.size(); ++i){ - if (weights(i) >= 0.0) { - IK_PIMPL(m_pimpl)->m_preferredJointsWeight(i) = weights(i); + if (weights[i] >= 0.0) { + IK_PIMPL(m_pimpl)->m_preferredJointsWeight(i) = weights[i]; } } return true; @@ -730,13 +983,22 @@ namespace iDynTree { #endif } - bool InverseKinematics::setDesiredReducedJointConfiguration(const iDynTree::VectorDynSize& desiredJointConfiguration, double weight) + bool InverseKinematics::setDesiredFullJointsConfiguration( + const iDynTree::VectorDynSize& desiredJointConfiguration, + const iDynTree::VectorDynSize& weights) + { + return this->setDesiredFullJointsConfiguration(make_span(desiredJointConfiguration), + make_span(weights)); + } + + bool InverseKinematics::setDesiredReducedJointConfiguration(iDynTree::Span desiredJointConfiguration, + double weight) { #ifdef IDYNTREE_USES_IPOPT assert(m_pimpl); assert(IK_PIMPL(m_pimpl)->m_reducedVariablesInfo.modelJointsToOptimisedJoints.size() == desiredJointConfiguration.size()); for (size_t i = 0; i < desiredJointConfiguration.size(); ++i) { - IK_PIMPL(m_pimpl)->m_preferredJointsConfiguration(IK_PIMPL(m_pimpl)->m_reducedVariablesInfo.modelJointsToOptimisedJoints[i]) = desiredJointConfiguration(i); + IK_PIMPL(m_pimpl)->m_preferredJointsConfiguration(IK_PIMPL(m_pimpl)->m_reducedVariablesInfo.modelJointsToOptimisedJoints[i]) = desiredJointConfiguration[i]; } if (weight >= 0.0) { @@ -748,7 +1010,14 @@ namespace iDynTree { #endif } - bool InverseKinematics::setDesiredReducedJointConfiguration(const VectorDynSize &desiredJointConfiguration, const VectorDynSize &weights) + bool InverseKinematics::setDesiredReducedJointConfiguration(const iDynTree::VectorDynSize& desiredJointConfiguration, + double weight) + { + return this->setDesiredReducedJointConfiguration(make_span(desiredJointConfiguration), weight); + } + + bool InverseKinematics::setDesiredReducedJointConfiguration(iDynTree::Span desiredJointConfiguration, + iDynTree::Span weights) { #ifdef IDYNTREE_USES_IPOPT assert(m_pimpl); @@ -760,9 +1029,9 @@ namespace iDynTree { } for (size_t i = 0; i < desiredJointConfiguration.size(); ++i) { - IK_PIMPL(m_pimpl)->m_preferredJointsConfiguration(IK_PIMPL(m_pimpl)->m_reducedVariablesInfo.modelJointsToOptimisedJoints[i]) = desiredJointConfiguration(i); + IK_PIMPL(m_pimpl)->m_preferredJointsConfiguration(IK_PIMPL(m_pimpl)->m_reducedVariablesInfo.modelJointsToOptimisedJoints[i]) = desiredJointConfiguration[i]; if (weights(i) >= 0.0) { - IK_PIMPL(m_pimpl)->m_preferredJointsWeight(IK_PIMPL(m_pimpl)->m_reducedVariablesInfo.modelJointsToOptimisedJoints[i]) = weights(i); + IK_PIMPL(m_pimpl)->m_preferredJointsWeight(IK_PIMPL(m_pimpl)->m_reducedVariablesInfo.modelJointsToOptimisedJoints[i]) = weights[i]; } } @@ -772,6 +1041,13 @@ namespace iDynTree { #endif } + bool InverseKinematics::setDesiredReducedJointConfiguration(const VectorDynSize &desiredJointConfiguration, + const VectorDynSize &weights) + { + return this->setDesiredReducedJointConfiguration(make_span(desiredJointConfiguration), + make_span(weights)); + } + bool InverseKinematics::setFullJointsInitialCondition(const iDynTree::Transform* baseTransform, const iDynTree::VectorDynSize* initialCondition) { @@ -902,6 +1178,39 @@ namespace iDynTree { #endif } + bool InverseKinematics::getFullJointsSolution(iDynTree::MatrixView baseTransformSolution, + iDynTree::Span shapeSolution) + { +#ifdef IDYNTREE_USES_IPOPT + bool ok = shapeSolution.size() == IK_PIMPL(m_pimpl)->m_dofs; + if (!ok) + { + reportError("InveseKineamtics", + "getFullJointsSolution", + "Invalid size of the shapeSolution vector"); + return false; + } + + constexpr int expected_transform_cols = 4; + constexpr int expected_transform_rows = 4; + ok = (baseTransformSolution.rows() == expected_transform_rows) + && (baseTransformSolution.cols() == expected_transform_cols); + if (!ok) + { + reportError("InverseKinematics", + "getFullJointsSolution", + "Invalid size of the baseTransformSolution vector"); + return false; + } + + toEigen(baseTransformSolution) = toEigen(IK_PIMPL(m_pimpl)->m_baseResults.asHomogeneousTransform()); + toEigen(shapeSolution) = toEigen(IK_PIMPL(m_pimpl)->m_jointsResults); + return true; +#else + return missingIpoptErrorReport(); +#endif + } + void InverseKinematics::getReducedSolution(iDynTree::Transform & baseTransformSolution, iDynTree::VectorDynSize & shapeSolution) { @@ -918,6 +1227,44 @@ namespace iDynTree { #endif } + bool InverseKinematics::getReducedSolution(iDynTree::MatrixView baseTransformSolution, + iDynTree::Span shapeSolution) + { +#ifdef IDYNTREE_USES_IPOPT + bool ok = shapeSolution.size() == IK_PIMPL(m_pimpl)->m_reducedVariablesInfo.modelJointsToOptimisedJoints.size(); + if (!ok) + { + reportError("InveseKineamtics", + "getReducedSolution", + "Invalid size of the shapeSolution vector"); + return false; + } + + constexpr int expected_transform_cols = 4; + constexpr int expected_transform_rows = 4; + ok = (baseTransformSolution.rows() == expected_transform_rows) + && (baseTransformSolution.cols() == expected_transform_cols); + if (!ok) + { + reportError("InverseKinematics", + "getReducedSolution", + "Invalid size of the baseTransformSolution vector"); + return false; + } + + assert(m_pimpl); + toEigen(baseTransformSolution) = toEigen(IK_PIMPL(m_pimpl)->m_baseResults.asHomogeneousTransform()); + assert(shapeSolution.size() == IK_PIMPL(m_pimpl)->m_reducedVariablesInfo.modelJointsToOptimisedJoints.size()); + for (size_t i = 0; i < shapeSolution.size(); ++i) { + shapeSolution(i) = IK_PIMPL(m_pimpl)->m_jointsResults(IK_PIMPL(m_pimpl)->m_reducedVariablesInfo.modelJointsToOptimisedJoints[i]); + } + return true; +#else + return missingIpoptErrorReport(); +#endif + } + + bool InverseKinematics::getPoseForFrame(const std::string& frameName, iDynTree::Transform& transform) { @@ -930,6 +1277,31 @@ namespace iDynTree { #endif } + bool InverseKinematics::getPoseForFrame(const std::string& frameName, + iDynTree::MatrixView transform) + { + constexpr int expected_transform_cols = 4; + constexpr int expected_transform_rows = 4; + const bool ok = (transform.rows() == expected_transform_rows) + && (transform.cols() == expected_transform_cols); + if (!ok) + { + reportError("InverseKinematics", + "getPoseForFrame", + "Invalid size of the transform matrix"); + return false; + } + +#ifdef IDYNTREE_USES_IPOPT + assert(m_pimpl); + toEigen(transform) = toEigen(IK_PIMPL(m_pimpl)->dynamics().getWorldTransform(frameName).asHomogeneousTransform()); + return true; +#else + return missingIpoptErrorReport(); +#endif + } + + const Model& InverseKinematics::fullModel() const { #ifdef IDYNTREE_USES_IPOPT @@ -960,7 +1332,7 @@ namespace iDynTree { return missingIpoptErrorReport(); #endif } - + void InverseKinematics::setCOMAsConstraint(bool asConstraint) { #ifdef IDYNTREE_USES_IPOPT @@ -970,7 +1342,7 @@ namespace iDynTree { #endif } - + bool InverseKinematics::isCOMAConstraint() { #ifdef IDYNTREE_USES_IPOPT @@ -980,6 +1352,7 @@ namespace iDynTree { #endif } + // this should be const reference but we keep it like this to avoid breaking the API. Please change me in iDynTree 4.0 void InverseKinematics::setCOMTarget(Position& desiredPosition, double weight) { #ifdef IDYNTREE_USES_IPOPT @@ -989,6 +1362,23 @@ namespace iDynTree { #endif } + bool InverseKinematics::setCOMTarget(iDynTree::Span desiredPosition, double weight) + { + constexpr int expected_pos_size = 3; + bool ok = (desiredPosition.size() == expected_pos_size); + if( !ok ) + { + reportError("InverseKinematics","setCOMTarget","Wrong size in input desiredPosition"); + return false; + } + + // TODO please remove me in iDynTree 4.0 + Position tmp(desiredPosition); + this->setCOMTarget(tmp, weight); + + return true; + } + void InverseKinematics::setCOMAsConstraintTolerance(double tolerance) { #ifdef IDYNTREE_USES_IPOPT @@ -1008,15 +1398,32 @@ namespace iDynTree { #endif } + // this should be const reference but we keep it like this to avoid breaking the API. Please change me in iDynTree 4.0 void InverseKinematics::setCOMConstraintProjectionDirection(iDynTree::Vector3 direction) { + this->setCOMConstraintProjectionDirection(make_span(direction)); + } + + bool InverseKinematics::setCOMConstraintProjectionDirection(iDynTree::Span direction) + { + constexpr int expected_pos_size = 3; + bool ok = (direction.size() == expected_pos_size); + if( !ok ) + { + reportError("InverseKinematics", + "setCOMConstraintProjectionDirection", + "Wrong size in input direction"); + return false; + } + #ifdef IDYNTREE_USES_IPOPT // define the projection matrix 'Pdirection' in the class 'ConvexHullProjectionConstraint' IK_PIMPL(m_pimpl)->m_comHullConstraint_projDirection = direction; IK_PIMPL(m_pimpl)->m_comHullConstraint.setProjectionAlongDirection(direction); + + return true; #else - missingIpoptErrorReport(); + return missingIpoptErrorReport(); #endif } - } diff --git a/src/inverse-kinematics/src/InverseKinematicsData.cpp b/src/inverse-kinematics/src/InverseKinematicsData.cpp index 110ea6ce12c..7dbe490a409 100644 --- a/src/inverse-kinematics/src/InverseKinematicsData.cpp +++ b/src/inverse-kinematics/src/InverseKinematicsData.cpp @@ -157,7 +157,7 @@ namespace kinematics { m_areBaseInitialConditionsSet = false; m_areJointsInitialConditionsSet = internal::kinematics::InverseKinematicsData::InverseKinematicsInitialConditionNotSet; - + m_comTarget.isActive = false; m_comTarget.weight = 0; m_comTarget.desiredPosition.zero(); @@ -396,15 +396,15 @@ namespace kinematics { return false; } } - - void InverseKinematicsData::setCoMTarget(iDynTree::Position& desiredPosition, double weight){ + + void InverseKinematicsData::setCoMTarget(const iDynTree::Position& desiredPosition, double weight){ this->m_comTarget.desiredPosition = desiredPosition; if (!this->m_comTarget.isActive && m_defaultTargetResolutionMode & iDynTree::InverseKinematicsTreatTargetAsConstraintPositionOnly) { this->m_comTarget.isConstraint = true; } - + if (!(weight < 0)) { this->m_comTarget.weight = weight; } @@ -538,10 +538,10 @@ namespace kinematics { return; } - + bool InverseKinematicsData::setJointLimits(std::vector >& jointLimits) - { - // check that the dimension of the vector is correct + { + // check that the dimension of the vector is correct if(jointLimits.size() != m_jointLimits.size()) return false; @@ -551,16 +551,16 @@ namespace kinematics { return true; } - + bool InverseKinematicsData::getJointLimits(std::vector >& jointLimits) - { - // check that the dimension of the vector is correct + { + // check that the dimension of the vector is correct if(jointLimits.size() != m_jointLimits.size()) return false; - + // return the current limits jointLimits = m_jointLimits; - + return true; } diff --git a/src/inverse-kinematics/tests/CMakeLists.txt b/src/inverse-kinematics/tests/CMakeLists.txt index 9d2ecfda62b..05963fbbc5d 100644 --- a/src/inverse-kinematics/tests/CMakeLists.txt +++ b/src/inverse-kinematics/tests/CMakeLists.txt @@ -13,4 +13,4 @@ endmacro() add_ik_test(ConvexHullHelpers) add_ik_test(InverseKinematics) - +add_ik_test(InverseKinematicsMatrixViewAndSpan) diff --git a/src/inverse-kinematics/tests/InverseKinematicsMatrixViewAndSpanUnitTest.cpp b/src/inverse-kinematics/tests/InverseKinematicsMatrixViewAndSpanUnitTest.cpp new file mode 100644 index 00000000000..67f849fb307 --- /dev/null +++ b/src/inverse-kinematics/tests/InverseKinematicsMatrixViewAndSpanUnitTest.cpp @@ -0,0 +1,677 @@ +/* + * Copyright (C) 2021 Fondazione Istituto Italiano di Tecnologia + * + * Licensed under either the GNU Lesser General Public License v3.0 : + * https://www.gnu.org/licenses/lgpl-3.0.html + * or the GNU Lesser General Public License v2.1 : + * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html + * at your option. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "testModels.h" + +#include +#include +#include +#include + +#include + +using namespace iDynTree; + +/** + * Return the current time in seconds, with respect + * to an arbitrary point in time. + */ +inline double clockInSec() +{ + clock_t ret = clock(); + return ((double)ret)/((double)CLOCKS_PER_SEC); +} + +inline double clockDurationInSeconds(clock_t duration) +{ + return static_cast(duration) / static_cast(CLOCKS_PER_SEC); +} + +iDynTree::JointPosDoubleArray getRandomJointPositions(const iDynTree::Model & model) +{ + iDynTree::JointPosDoubleArray sRandom(model); + getRandomJointPositions(sRandom,model); + return sRandom; +} + + +inline iDynTree::JointPosDoubleArray getRandomJointPositionsCloseTo(const iDynTree::Model& model, iDynTree::Span s, double maxDelta) +{ + iDynTree::JointPosDoubleArray vec(model); + + assert(vec.size() == model.getNrOfPosCoords()); + for(iDynTree::JointIndex jntIdx=0; jntIdx < model.getNrOfJoints(); jntIdx++) + { + iDynTree::IJointConstPtr jntPtr = model.getJoint(jntIdx); + if( jntPtr->hasPosLimits() ) + { + for(int i=0; i < jntPtr->getNrOfPosCoords(); i++) + { + int dofIndex = jntPtr->getDOFsOffset()+i; + double max = std::min(jntPtr->getMaxPosLimit(i),s(dofIndex)+maxDelta); + double min = std::max(jntPtr->getMinPosLimit(i),s(dofIndex)-maxDelta); + vec(dofIndex) = iDynTree::getRandomDouble(min,max); + } + } + else + { + for(int i=0; i < jntPtr->getNrOfPosCoords(); i++) + { + int dofIndex = jntPtr->getDOFsOffset()+i; + vec(jntPtr->getDOFsOffset()+i) = iDynTree::getRandomDouble(s(dofIndex)-maxDelta,s(dofIndex)+maxDelta); + } + } + } + + return vec; +} + +struct simpleChainIKOptions +{ + iDynTree::InverseKinematicsRotationParametrization rotationParametrization; + bool useDesiredJointPositionsToActualValue; + bool useDesiredJointPositionsToRandomValue; +}; + +void simpleChainIK(int minNrOfJoints, int maxNrOfJoints, const iDynTree::InverseKinematicsRotationParametrization rotationParametrization) +{ + // Solve a simple IK problem for a chain, with no constraints + for (int i = minNrOfJoints; i <= maxNrOfJoints; i++) + { + assert(i >= 2); + + std::cerr << "~~~~~~~> simpleChainIK with " << i << " dofs " << std::endl; + bool noFixedJoints = true; + iDynTree::Model chain = iDynTree::getRandomChain(i, 10, noFixedJoints); + + ASSERT_EQUAL_DOUBLE(i, chain.getNrOfDOFs()); + + // Name of the targetFrame the leaf added by getRandomChain + std::string targetFrame = "link" + iDynTree::int2string(i - 1); + + // Create IK + iDynTree::InverseKinematics ik; + ik.setVerbosity(0); + bool ok = ik.setModel(chain); + ASSERT_IS_TRUE(ok); + // Always express the target as cost + ik.setDefaultTargetResolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraintNone); + + // Use the requested parametrization + ik.setRotationParametrization(rotationParametrization); + + ik.setCostTolerance(1e-6); + ik.setConstraintsTolerance(1e-7); + + // Create also a KinDyn object to perform forward kinematics for the desired values and the optimized ones + iDynTree::KinDynComputations kinDynDes; + ok = kinDynDes.loadRobotModel(ik.fullModel()); + ASSERT_IS_TRUE(ok); + iDynTree::KinDynComputations kinDynOpt; + ok = kinDynOpt.loadRobotModel(ik.fullModel()); + ASSERT_IS_TRUE(ok); + + // Create a random vector of internal joint positions + // used to get reasonable values for the constraints and the targets + Eigen::VectorXd s = toEigen(getRandomJointPositions(kinDynDes.model())); + ok = kinDynDes.setJointPos(s); + ASSERT_IS_TRUE(ok); + + // Add the fixed base constraint + ok = ik.addFrameConstraint("link1", kinDynDes.getWorldTransform("link1")); + //ok = ik.addFrameRotationConstraint("link1", iDynTree::Transform::Identity().getRotation()); + //ok = ik.addFramePositionConstraint("link1", iDynTree::Transform::Identity().getPosition()); + ASSERT_IS_TRUE(ok); + + // Add target + ok = ik.addPositionTarget(targetFrame, MatrixView(kinDynDes.getWorldTransform(targetFrame).asHomogeneousTransform())); + ASSERT_IS_TRUE(ok); + + + // Add a random initial guess + iDynTree::Transform baseDes = kinDynDes.getWorldBaseTransform(); + // Set a max delta to avoid local minima in testing + double maxDelta = 0.01; + iDynTree::JointPosDoubleArray sInitial = getRandomJointPositionsCloseTo(ik.fullModel(), s, maxDelta); + ik.setFullJointsInitialCondition(&(baseDes), &(sInitial)); + + // Add a random desired position + Eigen::VectorXd sDesired = toEigen(getRandomJointPositions(ik.fullModel())); + ik.setDesiredFullJointsConfiguration(sDesired, 1e-12); + + // Start from the initial one + clock_t tic = clock(); + ok = ik.solve(); + std::cerr << "IK Solved in " << clockDurationInSeconds(clock() - tic) << "s" << std::endl; + + ASSERT_IS_TRUE(ok); + + // Get the solution + Eigen::Matrix4d baseOpt; + Eigen::VectorXd sOpt(ik.fullModel().getNrOfDOFs()); + ok = ik.getFullJointsSolution(baseOpt, sOpt); + + ASSERT_IS_TRUE(ok); + + Eigen::VectorXd dummyVel(6); + dummyVel.setZero(); + Eigen::Vector3d dummyGrav; + dummyGrav.setZero(); + Eigen::VectorXd dummyJointVel(ik.fullModel().getNrOfDOFs()); + dummyJointVel.setZero(); + + kinDynOpt.setRobotState(baseOpt, sOpt, dummyVel, dummyJointVel, dummyGrav); + double tolConstraints = 1e-6; + double tolTargets = 1e-3; + + // Check if the base link constraint is still valid + ASSERT_EQUAL_TRANSFORM_TOL(kinDynOpt.getWorldTransform("link1"), kinDynDes.getWorldTransform("link1"), tolConstraints); + + // Check if the target is realized + ASSERT_EQUAL_VECTOR_TOL(kinDynDes.getWorldTransform(targetFrame).getPosition(), kinDynOpt.getWorldTransform(targetFrame).getPosition(), tolTargets); + } + +} + +// Check the consistency of a simple humanoid wholebody IK test case +void simpleHumanoidWholeBodyIKConsistency(const iDynTree::InverseKinematicsRotationParametrization rotationParametrization) +{ + iDynTree::InverseKinematics ik; + + bool ok = ik.loadModelFromFile(getAbsModelPath("iCubGenova02.urdf")); + ASSERT_IS_TRUE(ok); + + //ik.setFloatingBaseOnFrameNamed("l_foot"); + + ik.setRotationParametrization(rotationParametrization); + + // Create also a KinDyn object to perform forward kinematics for the desired values + iDynTree::KinDynComputations kinDynDes; + ok = kinDynDes.loadRobotModel(ik.fullModel()); + ASSERT_IS_TRUE(ok); + + // Create a random vector of internal joint positions + // used to get reasonable values for the constraints and the targets + const auto s = getRandomJointPositions(kinDynDes.model()); + ok = kinDynDes.setJointPos(s); + ASSERT_IS_TRUE(ok); + + // Create a simple IK problem with the foot constraint + + // The l_sole frame is our world absolute frame (i.e. {}^A H_{l_sole} = identity + ok = ik.addFrameConstraint("l_foot", MatrixView(kinDynDes.getWorldTransform("l_foot").asHomogeneousTransform())); + ASSERT_IS_TRUE(ok); + + std::cerr << "kinDynDes.getWorldTransform(l_foot) : " << kinDynDes.getWorldTransform("l_foot").toString() << std::endl; + + // The relative position of the r_sole should be the initial one + ok = ik.addFrameConstraint("r_sole", MatrixView(kinDynDes.getWorldTransform("r_sole").asHomogeneousTransform())); + ASSERT_IS_TRUE(ok); + + // The two cartesian targets should be reasonable values + ik.setDefaultTargetResolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraintNone); + ok = ik.addPositionTarget("l_elbow_1", MatrixView(kinDynDes.getWorldTransform("l_elbow_1").asHomogeneousTransform())); + ASSERT_IS_TRUE(ok); + + //ok = ik.addPositionTarget("r_elbow_1",kinDynDes.getRelativeTransform("l_sole","r_elbow_1").getPosition()); + //ASSERT_IS_TRUE(ok); + + iDynTree::Transform initialH = kinDynDes.getWorldBaseTransform(); + + ik.setFullJointsInitialCondition(&initialH, &s); + ik.setDesiredFullJointsConfiguration(s, 1e-15); + + // Solve the optimization problem + clock_t tic = clock(); + ok = ik.solve(); + std::cerr << "IK Solved in " << clockDurationInSeconds(clock() - tic) << "s" << std::endl; + ASSERT_IS_TRUE(ok); + + Eigen::Matrix4d basePosOptimized; + Eigen::VectorXd sOptimized(ik.fullModel().getNrOfDOFs()); + sOptimized.setZero(); + + ik.getFullJointsSolution(basePosOptimized, sOptimized); + + // We create a new KinDyn object to perform forward kinematics for the optimized values + iDynTree::KinDynComputations kinDynOpt; + kinDynOpt.loadRobotModel(ik.fullModel()); + Eigen::VectorXd dummyVel(6); + dummyVel.setZero(); + Eigen::Vector3d dummyGrav; + dummyGrav.setZero(); + Eigen::VectorXd dummyJointVel(ik.fullModel().getNrOfDOFs()); + dummyJointVel.setZero(); + kinDynOpt.setRobotState(basePosOptimized, sOptimized, dummyVel, dummyJointVel, dummyGrav); + + // Check that the contraint and the targets are respected + double tolConstraints = 1e-7; + double tolTargets = 1e-6; + ASSERT_EQUAL_TRANSFORM_TOL(kinDynDes.getWorldTransform("l_foot"), kinDynOpt.getWorldTransform("l_foot"), tolConstraints); + ASSERT_EQUAL_TRANSFORM_TOL(kinDynDes.getWorldTransform("r_sole"), kinDynOpt.getWorldTransform("r_sole"), tolConstraints); + ASSERT_EQUAL_VECTOR_TOL(kinDynDes.getWorldTransform("l_elbow_1").getPosition(), + kinDynOpt.getWorldTransform("l_elbow_1").getPosition(), tolTargets); + + return; +} + +// Check the consistency of a simple humanoid wholebody IK test case, setting a CoM target. +void simpleHumanoidWholeBodyIKCoMConsistency(const iDynTree::InverseKinematicsRotationParametrization rotationParametrization, + const iDynTree::InverseKinematicsTreatTargetAsConstraint targetResolutionMode, + size_t dofsToBeRemoved = 0) +{ + iDynTree::InverseKinematics ik; + + ik.setVerbosity(0); + + std::vector consideredJoints; + iDynTree::ModelLoader loader; + loader.loadModelFromFile(getAbsModelPath("iCubGenova02.urdf")); + if (dofsToBeRemoved > 0) { + consideredJoints.reserve(loader.model().getNrOfDOFs()); + // TODO: 1 to 1 joints to dofs + // Also.. I assume 0 dofs joints are at the end. + //fill all the joints + for (int index = 0; index < loader.model().getNrOfDOFs(); ++index) { + consideredJoints.push_back(loader.model().getJointName(index)); + } + } + std::vector toBeRemoved; + toBeRemoved.reserve(dofsToBeRemoved); + for (size_t index = 0; index < dofsToBeRemoved; ++index) { + int indexToBeRemoved = -1; + do { + indexToBeRemoved = rand() % loader.model().getNrOfDOFs(); + } while (std::find(toBeRemoved.begin(), toBeRemoved.end(), indexToBeRemoved) != toBeRemoved.end()); + toBeRemoved.push_back(indexToBeRemoved); + } + std::sort(toBeRemoved.begin(), toBeRemoved.end()); + + for (std::vector::const_reverse_iterator rit = toBeRemoved.rbegin(); rit != toBeRemoved.rend(); ++rit) { + std::cerr << "Removing DOF " << *(consideredJoints.begin() + (*rit)) << "(" << *rit << ")" <(kinDynDes.getWorldTransform("l_foot").asHomogeneousTransform())); + ASSERT_IS_TRUE(ok); + + std::cerr << "kinDynDes.getWorldTransform(l_foot) : " << kinDynDes.getWorldTransform("l_foot").toString() << std::endl; + + // The relative position of the r_sole should be the initial one + ok = ik.addFrameConstraint("r_sole",MatrixView(kinDynDes.getWorldTransform("r_sole").asHomogeneousTransform())); + ASSERT_IS_TRUE(ok); + + // The two cartesian targets should be reasonable values + Eigen::Vector3d comDes; + ok = kinDynDes.getCenterOfMassPosition(comDes); + ASSERT_IS_TRUE(ok); + + ok = ik.setCOMTarget(comDes, 10); + ASSERT_IS_TRUE(ok); + + ik.setCOMAsConstraintTolerance(1e-8); + + //ok = ik.addPositionTarget("r_elbow_1",kinDynDes.getRelativeTransform("l_sole","r_elbow_1").getPosition()); + //ASSERT_IS_TRUE(ok); + + ik.setFullJointsInitialCondition(&initialH, &s); + ik.setDesiredFullJointsConfiguration(s, 1e-15); + + // Solve the optimization problem + clock_t tic = clock(); + ok = ik.solve(); + std::cerr << "IK Solved in " << clockDurationInSeconds(clock() - tic) << "s" << std::endl; + ASSERT_IS_TRUE(ok); + + Eigen::Matrix4d basePosOptimized; + Eigen::VectorXd sOptimized(ik.fullModel().getNrOfDOFs()), sOptimizedFromReduced(ik.fullModel().getNrOfDOFs()); + sOptimized.setZero(); + sOptimizedFromReduced.setZero(); + Eigen::VectorXd sReducedOptimized(ik.reducedModel().getNrOfDOFs()); + sReducedOptimized.setZero(); + + ok = ik.getFullJointsSolution(basePosOptimized, sOptimized); + ASSERT_IS_TRUE(ok); + + ok = ik.getReducedSolution(basePosOptimized, sReducedOptimized); + ASSERT_IS_TRUE(ok); + // now map sReduced to sOptimizedFromReduced + + + // We create a new KinDyn object to perform forward kinematics for the optimized values + iDynTree::KinDynComputations kinDynOpt; + kinDynOpt.loadRobotModel(ik.fullModel()); + Eigen::VectorXd dummyVel(6); + dummyVel.setZero(); + Eigen::Vector3d dummyGrav; + dummyGrav.setZero(); + Eigen::VectorXd dummyJointVel(ik.fullModel().getNrOfDOFs()); + dummyJointVel.setZero(); + kinDynOpt.setRobotState(basePosOptimized, sOptimized, dummyVel, dummyJointVel, dummyGrav); + + // Check that the contraint and the targets are respected + double tolConstraints = 1e-7; + double tolTargets = 1e-6; + ASSERT_EQUAL_TRANSFORM_TOL(kinDynDes.getWorldTransform("l_foot"), kinDynOpt.getWorldTransform("l_foot"), tolConstraints); + ASSERT_EQUAL_TRANSFORM_TOL(kinDynDes.getWorldTransform("r_sole"), kinDynOpt.getWorldTransform("r_sole"), tolConstraints); + ASSERT_EQUAL_VECTOR_TOL(kinDynDes.getCenterOfMassPosition(), + kinDynOpt.getCenterOfMassPosition(), tolTargets); + + return; +} + + +// Check the consistency of a simple humanoid wholebody IK test case, setting a CoM target. +void simpleHumanoidWholeBodyIKCoMandChestConsistency(const iDynTree::InverseKinematicsRotationParametrization rotationParametrization, + const iDynTree::InverseKinematicsTreatTargetAsConstraint targetResolutionMode) +{ + iDynTree::InverseKinematics ik; + + ik.setMaxIterations(500); + ik.setVerbosity(0); + + bool ok = ik.loadModelFromFile(getAbsModelPath("iCubGenova02.urdf")); + ASSERT_IS_TRUE(ok); + + //ik.setFloatingBaseOnFrameNamed("l_foot"); + + ik.setRotationParametrization(rotationParametrization); + ik.setDefaultTargetResolutionMode(targetResolutionMode); + + // Create also a KinDyn object to perform forward kinematics for the desired values + iDynTree::KinDynComputations kinDynDes; + ok = kinDynDes.loadRobotModel(ik.fullModel()); + ASSERT_IS_TRUE(ok); + + // Create a random vector of internal joint positions + // used to get reasonable values for the constraints and the targets + iDynTree::JointPosDoubleArray s = getRandomJointPositions(kinDynDes.model()); + ok = kinDynDes.setJointPos(s); + ASSERT_IS_TRUE(ok); + + // Create a simple IK problem with the foot constraint + + // The l_sole frame is our world absolute frame (i.e. {}^A H_{l_sole} = identity + Eigen::Matrix4d lFootTransform; + ok = kinDynDes.getWorldTransform("l_foot", lFootTransform); + ASSERT_IS_TRUE(ok); + + ok = ik.addFrameConstraint("l_foot", lFootTransform); + ASSERT_IS_TRUE(ok); + + std::cerr << "kinDynDes.getWorldTransform(l_foot) : " << kinDynDes.getWorldTransform("l_foot").toString() << std::endl; + + // The relative position of the r_sole should be the initial one + Eigen::Matrix4d rSoleTransform; + ok = kinDynDes.getWorldTransform("r_sole", rSoleTransform); + ASSERT_IS_TRUE(ok); + + ok = ik.addFrameConstraint("r_sole", rSoleTransform); + ASSERT_IS_TRUE(ok); + + // The two cartesian targets should be reasonable values + Eigen::Vector3d comDes; + ok = kinDynDes.getCenterOfMassPosition(comDes); + ASSERT_IS_TRUE(ok); + + ok = ik.setCOMTarget(comDes, 1); + ASSERT_IS_TRUE(ok); + + ik.setCOMAsConstraintTolerance(1e-8); + + Eigen::Matrix3d chestRotation = toEigen(kinDynDes.getWorldTransform("chest").getRotation()); + ok = ik.addRotationTarget("chest", chestRotation, 100); + ASSERT_IS_TRUE(ok); + + ik.setTargetResolutionMode("chest", iDynTree::InverseKinematicsTreatTargetAsConstraintNone); + + iDynTree::Transform initialH = kinDynDes.getWorldBaseTransform(); + + ik.setFullJointsInitialCondition(&initialH, &s); + ik.setDesiredFullJointsConfiguration(s, 1e-15); + + // Solve the optimization problem + clock_t tic = clock(); + ok = ik.solve(); + std::cerr << "IK Solved in " << clockDurationInSeconds(clock() - tic) << "s" << std::endl; + ASSERT_IS_TRUE(ok); + + Eigen::Matrix4d basePosOptimized; + Eigen::VectorXd sOptimized(ik.fullModel().getNrOfDOFs()); + sOptimized.setZero(); + + ik.getFullJointsSolution(basePosOptimized, sOptimized); + + // We create a new KinDyn object to perform forward kinematics for the optimized values + iDynTree::KinDynComputations kinDynOpt; + kinDynOpt.loadRobotModel(ik.fullModel()); + Eigen::VectorXd dummyVel(6); + dummyVel.setZero(); + Eigen::Vector3d dummyGrav; + dummyGrav.setZero(); + Eigen::VectorXd dummyJointVel(ik.fullModel().getNrOfDOFs()); + dummyJointVel.setZero(); + kinDynOpt.setRobotState(basePosOptimized, sOptimized, dummyVel, dummyJointVel, dummyGrav); + + // Check that the contraint and the targets are respected + double tolConstraints = 1e-7; + double tolTargets = 1e-6; + ASSERT_EQUAL_TRANSFORM_TOL(kinDynDes.getWorldTransform("l_foot"), kinDynOpt.getWorldTransform("l_foot"), tolConstraints); + ASSERT_EQUAL_TRANSFORM_TOL(kinDynDes.getWorldTransform("r_sole"), kinDynOpt.getWorldTransform("r_sole"), tolConstraints); + ASSERT_EQUAL_VECTOR_TOL(kinDynDes.getCenterOfMassPosition(), + kinDynOpt.getCenterOfMassPosition(), tolTargets); + ASSERT_EQUAL_MATRIX_TOL(kinDynDes.getWorldTransform("chest").getRotation(), kinDynOpt.getWorldTransform("chest").getRotation(), tolTargets); + + return; +} + +void COMConvexHullConstraintWithSwitchingConstraints() +{ + // Load an humanoid model + iDynTree::InverseKinematics ik; + + ik.setMaxIterations(500); + ik.setVerbosity(0); + + // Use the requested parametrization + ik.setRotationParametrization(iDynTree::InverseKinematicsRotationParametrizationRollPitchYaw); + + //ik.setCostTolerance(1e-4); + //ik.setConstraintsTolerance(1e-5); + + bool ok = ik.loadModelFromFile(getAbsModelPath("iCubGenova02.urdf")); + ASSERT_IS_TRUE(ok); + + // Also create a KinDynComputation object for some forward kinematics + KinDynComputations kinDynRef; + ok = kinDynRef.loadRobotModel(ik.fullModel()); + ASSERT_IS_TRUE(ok); + KinDynComputations kinDynsol; + ok = kinDynsol.loadRobotModel(ik.fullModel()); + ASSERT_IS_TRUE(ok); + + + // We use the joints all set to 0 as a reference position + Transform identityTransform = Transform::Identity(); + VectorDynSize refereceJointPos(ik.fullModel().getNrOfDOFs()); + refereceJointPos.zero(); + + // For the elbow we set another position as 0 is outside the limits + const Model& model = ik.fullModel(); + int lElbowDofIndex = model.getJoint(model.getJointIndex("l_elbow"))->getDOFsOffset(); + refereceJointPos(lElbowDofIndex) = 0.5; + int rElbowDofIndex = model.getJoint(model.getJointIndex("r_elbow"))->getDOFsOffset(); + refereceJointPos(rElbowDofIndex) = 0.5; + + kinDynRef.setJointPos(refereceJointPos); + + // Add a postural "task" to the IK + ik.setDesiredFullJointsConfiguration(refereceJointPos, 1e-2); + + // Add two frames constraints on both soles (assuming initially that l_sole == world) + const Eigen::Matrix4d identity = Eigen::Matrix4d::Identity(); + ik.addFrameConstraint("l_sole", identity); + ik.addFrameConstraint("r_sole", MatrixView(kinDynRef.getRelativeTransform("l_sole", "r_sole").asHomogeneousTransform())); + + // Add the COM convex hull constraint, where the polygon are just a long strip of 2 centimeters at the origin of the frames + // The COM projection plane will always be the world XY plane, so the world plane need to be always coincident with a support sole + iDynTree::Direction xAxis(1.0, 0.0, 0.0); + iDynTree::Direction yAxis(0.0, 1.0, 0.0); + // The strips are long on the x direction so we don't need to care where the COM is actually in the X direction + double stripWidth = 0.02; + ik.addCenterOfMassProjectionConstraint("l_sole", iDynTree::Polygon::XYRectangleFromOffsets(1, 1, stripWidth/2, stripWidth/2), + "r_sole", iDynTree::Polygon::XYRectangleFromOffsets(1, 1, stripWidth/2, stripWidth/2), + xAxis, yAxis); + + // Solve the problem in this case: as the com projection of all the joint set to 0 is in in between the soles + // the inverse kinematics should give the joints all to zero as the solution + clock_t tic = clock(); + Transform initialCondition = kinDynRef.getRelativeTransform("l_sole", "root_link"); + ik.setFullJointsInitialCondition(&initialCondition, &refereceJointPos); + //ik.setVerbosity(5); + ok = ik.solve(); + ASSERT_IS_TRUE(ok); + std::cerr << "COMConvexHullConstraintWithSwitchingConstraints: IK with both constraints solved in " << clockDurationInSeconds(clock() - tic) << "s" << std::endl; + + Eigen::Matrix4d baseSolution; + Eigen::VectorXd jointPosSolution(ik.fullModel().getNrOfDOFs()); + ik.getFullJointsSolution(baseSolution, jointPosSolution); + + ASSERT_EQUAL_VECTOR_TOL(refereceJointPos, + jointPosSolution, 1e-4); + + // We deactivate both constraints. The solution should be still all the joints to zero + ok = ik.deactivateFrameConstraint("l_sole"); + ASSERT_IS_TRUE(ok); + ASSERT_IS_FALSE(ik.isFrameConstraintActive("l_sole")); + + ok = ik.deactivateFrameConstraint("r_sole"); + ASSERT_IS_TRUE(ok); + ASSERT_IS_FALSE(ik.isFrameConstraintActive("r_sole")); + + tic = clock(); + ik.setFullJointsInitialCondition(&identityTransform, &refereceJointPos); + ok = ik.solve(); + std::cerr << "COMConvexHullConstraintWithSwitchingConstraints: IK with no constraints solved in " << clockDurationInSeconds(clock() - tic) << "s" << std::endl; + ASSERT_IS_TRUE(ok); + + ik.getFullJointsSolution(baseSolution, jointPosSolution); + + ASSERT_EQUAL_VECTOR_TOL(refereceJointPos, + jointPosSolution, 1e-3); + + // We only activate the left sole constraint, placing the "world" on the left_sole + ok = ik.activateFrameConstraint("l_sole", identity); + ASSERT_IS_TRUE(ok); + ASSERT_IS_TRUE(ik.isFrameConstraintActive("l_sole")); + + ok = ik.deactivateFrameConstraint("r_sole"); + ASSERT_IS_TRUE(ok); + ASSERT_IS_FALSE(ik.isFrameConstraintActive("r_sole")); + + // We solve the problem and verify that the y-component of com projected in the l_sole frame is less than stripWidth/2 + tic = clock(); + ik.setFullJointsInitialCondition(&identityTransform, &refereceJointPos); + ok = ik.solve(); + std::cerr << "COMConvexHullConstraintWithSwitchingConstraints: IK with l_sole constraint solved in " << clockDurationInSeconds(clock() - tic) << "s" << std::endl; + ASSERT_IS_TRUE(ok); + + ik.getFullJointsSolution(baseSolution, jointPosSolution); + kinDynsol.setJointPos(jointPosSolution); + Position com_l_sole = kinDynsol.getWorldTransform("l_sole").inverse()*(kinDynsol.getCenterOfMassPosition()); + ASSERT_IS_TRUE(std::abs(com_l_sole(1)) < stripWidth/2.0 + 1e-9); + + // We only activate the right sole constraint, placing the "world" on the left_sole + ok = ik.activateFrameConstraint("r_sole", identity); + ASSERT_IS_TRUE(ok); + ASSERT_IS_TRUE(ik.isFrameConstraintActive("r_sole")); + + ok = ik.deactivateFrameConstraint("l_sole"); + ASSERT_IS_TRUE(ok); + ASSERT_IS_FALSE(ik.isFrameConstraintActive("l_sole")); + + // We solve the problem and verify that the y-component of com projected in the l_sole frame is less than stripWidth/2 + tic = clock(); + ik.setFullJointsInitialCondition(&identityTransform, &refereceJointPos); + ok = ik.solve(); + std::cerr << "COMConvexHullConstraintWithSwitchingConstraints: IK with r_sole constraint solved in " << clockDurationInSeconds(clock() - tic) << "s" << std::endl; + ASSERT_IS_TRUE(ok); + + ik.getFullJointsSolution(baseSolution, jointPosSolution); + kinDynsol.setJointPos(jointPosSolution); + Position com_r_sole = kinDynsol.getWorldTransform("r_sole").inverse()*(kinDynsol.getCenterOfMassPosition()); + ASSERT_IS_TRUE(std::abs(com_r_sole(1)) < stripWidth/2.0 + 1e-9); + + // Check that getting the margin works fine + double margin = ik.getCenterOfMassProjectionMargin(); + ASSERT_IS_FALSE(std::isnan(margin)); + +} + +int main() +{ + // Improve repetability (at least in the same platform) + srand(1); + + simpleChainIK(2, 13, iDynTree::InverseKinematicsRotationParametrizationRollPitchYaw); + + // This is not working at the moment, there is some problem with quaternion constraints + //simpleChainIK(10,iDynTree::InverseKinematicsRotationParametrizationQuaternion); + simpleHumanoidWholeBodyIKConsistency(iDynTree::InverseKinematicsRotationParametrizationRollPitchYaw); + simpleHumanoidWholeBodyIKCoMConsistency(iDynTree::InverseKinematicsRotationParametrizationRollPitchYaw, iDynTree::InverseKinematicsTreatTargetAsConstraintNone); + simpleHumanoidWholeBodyIKCoMConsistency(iDynTree::InverseKinematicsRotationParametrizationRollPitchYaw, iDynTree::InverseKinematicsTreatTargetAsConstraintFull); + simpleHumanoidWholeBodyIKCoMandChestConsistency(iDynTree::InverseKinematicsRotationParametrizationRollPitchYaw, iDynTree::InverseKinematicsTreatTargetAsConstraintFull); + + for (size_t i = 1; i < 10; i++) { + std::cerr << "Removing " << i << " Dofs" << std::endl; + simpleHumanoidWholeBodyIKCoMConsistency(iDynTree::InverseKinematicsRotationParametrizationRollPitchYaw, iDynTree::InverseKinematicsTreatTargetAsConstraintFull, i); + } + + COMConvexHullConstraintWithSwitchingConstraints(); + + + return EXIT_SUCCESS; +}