From 82ff0a72fc9ba1265e12984a093c03f412a82ce4 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 26 Sep 2019 17:16:58 +0200 Subject: [PATCH 01/58] replace the m_jointRegularizationHessian and m_jointRegularizationGradient variables with m_jointRegularizationWeights --- .../QPInverseKinematics.h | 2 + .../src/QPInverseKinematics.cpp | 43 +++++++------------ 2 files changed, 18 insertions(+), 27 deletions(-) diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h index 25f5ad28..7924fbef 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h @@ -101,6 +101,8 @@ namespace WalkingControllers double m_kAttHand; /**< Gain related to the desired hand attitude. */ double m_kNeck; /**< Gain related to the desired neck attitude. */ double m_kCom; /**< Gain related to the desired CoM position. */ + + iDynTree::VectorDynSize m_jointRegularizationWeights; /**< Weight related to the the regularization term */ iDynTree::Vector3 m_comWeight; /**< CoM weight. */ double m_neckWeight; /**< Neck weight matrix. */ iDynSparseMatrix m_jointRegularizationHessian; /**< Contains a constant matrix that can be useful diff --git a/src/WholeBodyControllers/src/QPInverseKinematics.cpp b/src/WholeBodyControllers/src/QPInverseKinematics.cpp index b80a167b..4479e909 100644 --- a/src/WholeBodyControllers/src/QPInverseKinematics.cpp +++ b/src/WholeBodyControllers/src/QPInverseKinematics.cpp @@ -150,23 +150,14 @@ bool WalkingQPIK::initializeMatrices(const yarp::os::Searchable& config) } // set the matrix related to the joint regularization - iDynTree::VectorDynSize jointRegularizationWeights(m_actuatedDOFs); - if(!YarpUtilities::getVectorFromSearchable(config, "joint_regularization_weights", jointRegularizationWeights)) + + m_jointRegularizationWeights.resize(m_actuatedDOFs); + if(!YarpUtilities::getVectorFromSearchable(config, "joint_regularization_weights", m_jointRegularizationWeights)) { yError() << "Initialization failed while reading jointRegularizationWeights vector."; return false; } - // m_jointRegulatizationHessian = H' \lamda H - m_jointRegularizationHessian.resize(m_numberOfVariables, m_numberOfVariables); - for(int i = 0; i < m_actuatedDOFs; i++) - m_jointRegularizationHessian(i + 6, i + 6) = jointRegularizationWeights(i); - - // evaluate constant sub-matrix of the gradient matrix - m_jointRegularizationGradient.resize(m_numberOfVariables, m_actuatedDOFs); - for(int i = 0; i < m_actuatedDOFs; i++) - m_jointRegularizationGradient(i + 6, i) = jointRegularizationWeights(i); - m_jointRegularizationGains.resize(m_actuatedDOFs); if(!YarpUtilities::getVectorFromSearchable(config, "joint_regularization_gains", m_jointRegularizationGains)) @@ -506,13 +497,12 @@ void WalkingQPIK::evaluateHessianMatrix() m_neckWeight * iDynTree::toEigen(m_neckJacobian); - if(!m_enableHandRetargeting) - hessianDense += iDynTree::toEigen(m_jointRegularizationHessian); - else + hessianDense.bottomRightCorner(m_actuatedDOFs, m_actuatedDOFs) += iDynTree::toEigen(m_jointRegularizationWeights).asDiagonal(); + + if(m_enableHandRetargeting) { // think about the possibility to project in the null space the joint regularization - hessianDense += iDynTree::toEigen(m_jointRegularizationHessian) - + iDynTree::toEigen(m_leftHandJacobian).transpose() + hessianDense += iDynTree::toEigen(m_leftHandJacobian).transpose() * iDynTree::toEigen(m_handWeightSmoother->getPos()).asDiagonal() * iDynTree::toEigen(m_leftHandJacobian) + iDynTree::toEigen(m_rightHandJacobian).transpose() @@ -533,7 +523,7 @@ void WalkingQPIK::evaluateGradientVector() auto neckJacobian(iDynTree::toEigen(m_neckJacobian)); - auto jointRegularizationGradient(iDynTree::toEigen(m_jointRegularizationGradient)); + auto jointRegularizationWeights(iDynTree::toEigen(m_jointRegularizationWeights)); auto jointRegularizationGains(iDynTree::toEigen(m_jointRegularizationGains)); auto jointPosition(iDynTree::toEigen(m_jointPosition)); auto regularizationTerm(iDynTree::toEigen(m_regularizationTerm)); @@ -549,11 +539,13 @@ void WalkingQPIK::evaluateGradientVector() gradient = -neckJacobian.transpose() * m_neckWeight * (-m_kNeck * iDynTree::unskew(iDynTree::toEigen(errorNeckAttitude))); - // if the hand retargeting is not enable the regularization term should be used as usual - if(!m_enableHandRetargeting) - gradient += -jointRegularizationGradient * jointRegularizationGains.asDiagonal() - * (regularizationTerm - jointPosition); - else + // g = Weight * K_p * (regularizationTerm - jointPosition) + // Weight and K_p are two diagonal matrices so their product can be also evaluated multiplying component-wise + // the elements of the vectors and then generating the diagonal matrix + gradient.tail(m_actuatedDOFs) += (-jointRegularizationWeights.cwiseProduct(jointRegularizationGains)).asDiagonal() + * (regularizationTerm - jointPosition); + + if(m_enableHandRetargeting) { auto leftHandCorrectionLinear(iDynTree::toEigen(m_leftHandCorrection.getLinearVec3())); auto leftHandCorrectionAngular(iDynTree::toEigen(m_leftHandCorrection.getAngularVec3())); @@ -579,10 +571,7 @@ void WalkingQPIK::evaluateGradientVector() rightHandCorrectionAngular = m_kAttHand * (iDynTree::unskew(iDynTree::toEigen(rightHandAttitudeError))); - gradient += - -jointRegularizationGradient * jointRegularizationGains.asDiagonal() - * (regularizationTerm - jointPosition) - - iDynTree::toEigen(m_leftHandJacobian).transpose() + gradient += - iDynTree::toEigen(m_leftHandJacobian).transpose() * iDynTree::toEigen(m_handWeightSmoother->getPos()).asDiagonal() * (-iDynTree::toEigen(m_leftHandCorrection)) - iDynTree::toEigen(m_rightHandJacobian).transpose() * iDynTree::toEigen(m_handWeightSmoother->getPos()).asDiagonal() * (-iDynTree::toEigen(m_rightHandCorrection)); From 2a3739dd0088c65af66ffa7f2fa7e8bd8dc045bb Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 26 Sep 2019 19:46:44 +0200 Subject: [PATCH 02/58] implement the joint retargeting in the QP inverse kinematics --- .../QPInverseKinematics.h | 604 ++++++++++-------- .../src/QPInverseKinematics.cpp | 225 ++++++- 2 files changed, 514 insertions(+), 315 deletions(-) diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h index 7924fbef..2f19445e 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h @@ -81,6 +81,7 @@ namespace WalkingControllers desiredNeckOrientation rotation matrix). */ iDynTree::VectorDynSize m_regularizationTerm; /**< Desired joint position (regularization term).*/ + iDynTree::VectorDynSize m_retargetingJointValue; /**< Desired joint retargeting position.*/ iDynTree::Position m_comPosition; /**< Desired Linear velocity of the CoM. */ iDynTree::Transform m_leftFootToWorldTransform; /**< Actual left foot to world transformation.*/ @@ -95,6 +96,7 @@ namespace WalkingControllers int m_actuatedDOFs; /**< Number of actuated actuated DoF. */ iDynTree::VectorDynSize m_jointRegularizationGains; /**< Gain related to the joint regularization. */ + iDynTree::VectorDynSize m_jointRetargetingGains; /**< Gain related to the joint regularization. */ double m_kPosFoot; /**< Gain related to the desired foot position. */ double m_kAttFoot; /**< Gain related to the desired foot attitude. */ double m_kPosHand; /**< Gain related to the desired Hand position. */ @@ -110,289 +112,323 @@ namespace WalkingControllers iDynSparseMatrix m_jointRegularizationGradient; /**< Contains a constant matrix that can be useful in the gradient evaluation ($-\lambda H'$). */ double m_kJointLimitsUpperBound; /**< Gain related to the the joint upper bound */ - double m_kJointLimitsLowerBound; /**< Gain related to the the joint lower bound */ - iDynTree::VectorDynSize m_jointVelocitiesBounds; /**< Bounds on the joint velocities*/ - iDynTree::VectorDynSize m_jointPositionsUpperBounds; /**< Upper Bounds on the joint position*/ - iDynTree::VectorDynSize m_jointPositionsLowerBounds; /**< Lower Bounds on the joint position*/ - - // gain scheduling - std::unique_ptr m_handWeightSmoother; /**< Minimum jerk trajectory - for the hand weight matrix. */ - yarp::sig::Vector m_handWeightWalkingVector; /**< Weight matrix (only the diagonal) used for - the hand retargeting during walking. */ - yarp::sig::Vector m_handWeightStanceVector; /**< Weight matrix (only the diagonal) used for - the hand retargeting during stance. */ - - bool m_useCoMAsConstraint; /**< True if the CoM is added as a constraint. */ - bool m_useJointsLimitsConstraint; /**< True if the CoM is added as a constraint. */ - bool m_enableHandRetargeting; /**< True if the hand retargeting is used */ - - iDynTree::MatrixDynSize m_hessianDense; /**< Hessian matrix */ - iDynTree::VectorDynSize m_gradient; /**< Gradient vector */ - iDynSparseMatrix m_constraintsMatrixSparse; /**< Constraint matrix */ - iDynTree::VectorDynSize m_lowerBound; /**< Lower bound */ - iDynTree::VectorDynSize m_upperBound; /**< Upper bound */ - iDynTree::VectorDynSize m_solution; /**< Solution of the optimization problem */ - iDynTree::VectorDynSize m_desiredJointVelocitiesOutput; /**< Joint velocities required by the optimization problem */ - - /** - * Initialize all the constant matrix from the configuration file. - * @param config configuration parameters - * @return true/false in case of success/failure. - */ - bool initializeMatrices(const yarp::os::Searchable& config); - - /** - * Initialize hand retargeting. - * @param config configuration parameters - * @return true/false in case of success/failure. - */ - bool initializeHandRetargeting(const yarp::os::Searchable& config); - - /** - * Instantiate the solver - */ - virtual void instantiateSolver() = 0; - - /** - * Evaluate the Hessian matrix. - */ - void evaluateHessianMatrix(); - - /** - * Evaluate the gradient vector. - */ - void evaluateGradientVector(); - - /** - * Evaluate the Linear constraint matrix. - */ - void evaluateLinearConstraintMatrix(); - - /** - * Evaluate Lower and upper bounds - */ - void evaluateBounds(); - - /** - * Set the number of constraints (it may change according to the solver used) - */ - virtual void setNumberOfConstraints(){;}; - - /** - * Initialize matrices that depends on the solver used - */ - virtual void initializeSolverSpecificMatrices(){;}; - - /** - * Set joint velocities bounds - */ - virtual void setJointVelocitiesBounds() = 0; - - public: - /** - * Initialize the QP-IK problem. - * @param config config of the QP-IK solver - * @param actuatedDOFs number of the actuated DoF - * @param minJointsPosition is a vector containing the min joints position limit - * @param minJointsPosition is a vector containing the max joints position limit - * @param maxJointsVelocity is a vector containing the max joints velocity limit. - * @return true/false in case of success/failure. - */ - bool initialize(const yarp::os::Searchable &config, - const int &actuatedDOFs, - const iDynTree::VectorDynSize& maxJointsVelocity, - const iDynTree::VectorDynSize& maxJointsPosition, - const iDynTree::VectorDynSize& minJointsPosition); - - /** - * Set the robot state. - * @param jointPosition vector of joint positions (in rad); - * @param leftFootToWorldTransform transformation between the inertial frame and the left foot; - * @param rightFootToWorldTransform transformation between the inertial frame and the right foot; - * @param leftHandToWorldTransform transformation between the inertial frame and the left foot; - * @param rightHandToWorldTransform transformation between the inertial frame and the right foot; - * @param neckOrientation rotation between the inertial frame and the neck; - * @param comPosition position of the CoM - * @return true/false in case of success/failure. - */ - bool setRobotState(const iDynTree::VectorDynSize& jointPosition, - const iDynTree::Transform& leftFootToWorldTransform, - const iDynTree::Transform& rightFootToWorldTransform, - const iDynTree::Transform& leftHandToWorldTransform, - const iDynTree::Transform& rightHandToWorldTransform, - const iDynTree::Rotation& neckOrientation, - const iDynTree::Position& comPosition); - - /** - * Set the Jacobian of the CoM - * @param comJacobian jacobian of the CoM (mixed representation) - * @return true/false in case of success/failure. - */ - bool setCoMJacobian(const iDynTree::MatrixDynSize& comJacobian); - - /** - * Set the Jacobian of the left foot - * @param leftFootJacobian jacobian of the left foot (mixed representation) - * @return true/false in case of success/failure. - */ - bool setLeftFootJacobian(const iDynTree::MatrixDynSize& leftFootJacobian); - - /** - * Set the Jacobian of the right foot - * @param rightFootJacobian jacobian of the right foot (mixed representation) - * @return true/false in case of success/failure. - */ - bool setRightFootJacobian(const iDynTree::MatrixDynSize& rightFootJacobian); - - /** - * Set the Jacobian of the left hand - * @param leftHandJacobian jacobian of the left hand (mixed representation) - * @return true/false in case of success/failure. - */ - bool setLeftHandJacobian(const iDynTree::MatrixDynSize& leftHandJacobian); - - /** - * Set the Jacobian of the right hand - * @param rightHandJacobian jacobian of the right hand (mixed representation) - * @return true/false in case of success/failure. - */ - bool setRightHandJacobian(const iDynTree::MatrixDynSize& rightHandJacobian); - - /** - * Set the Jacobian of the neck - * @param neckJacobian jacobian of the neck (mixed representation) - * @return true/false in case of success/failure. - */ - bool setNeckJacobian(const iDynTree::MatrixDynSize& neckJacobian); - - /** - * Set the desired joint position. - * Please use this term as regularization term. - * @param regularizationTerm vector of the desired joint position. - * @return true/false in case of success/failure. - */ - bool setDesiredJointPosition(const iDynTree::VectorDynSize& regularizationTerm); - - /** - * Set the desired twist of both feet - * @param leftFootTwist contain the desired twist of the left foot (MIXED representation); - * @param rightFootTwist contain the desired twist of the right foot (MIXED representation). - */ - void setDesiredFeetTwist(const iDynTree::Twist& leftFootTwist, - const iDynTree::Twist& rightFootTwist); - - /** - * Set the desired twist of both feet - * @param leftHandTwist contain the desired twist of the left hand (MIXED representation); - * @param rightHandTwist contain the desired twist of the right hand (MIXED representation). - */ - void setDesiredHandsTwist(const iDynTree::Twist& leftHandTwist, - const iDynTree::Twist& rightHandTwist); - - - /** - * Set the desired CoMVelocity - * @param comVelocity contain the desired CoM velocity. - */ - void setDesiredCoMVelocity(const iDynTree::Vector3& comVelocity); - - /** - * Set the desired feet transformation - * @param desiredLeftFootToWorldTransform desired transformation between the left foot and the world frame; - * @param desiredRightFootToWorldTransform desired transformation between the right foot and the world frame. - */ - void setDesiredFeetTransformation(const iDynTree::Transform& desiredLeftFootToWorldTransform, - const iDynTree::Transform& desiredRightFootToWorldTransform); - - /** - * Set the desired hand transformation - * @param desiredLeftHandToWorldTransform desired transformation between the left hand and the world frame; - * @param desireRightHandToWorldTransform desired transformation between the right hand and the world frame. - */ - void setDesiredHandsTransformation(const iDynTree::Transform& desiredLeftHandToWorldTransform, - const iDynTree::Transform& desiredRightHandToWorldTransform); - - /** - * Set the desired orientation of the neck - * @param desiredNeckOrientation rotation matrix between the neck and the world frame. - */ - void setDesiredNeckOrientation(const iDynTree::Rotation& desiredNeckOrientation); - - /** - * Set the desired CoMPosition - * @param desiredComPosition contain the desired CoM position. - */ - void setDesiredCoMPosition(const iDynTree::Position& desiredComPosition); - - /** - * Set the robot phase. This is used by the minimum jerk trajectory. - * @param isStancePhase true if the robot is in the stance phase - */ - void setPhase(const bool& isStancePhase); - - /** - * Solve the optimization problem. - * @return true/false in case of success/failure. - */ - virtual bool solve() = 0; - - /** - * Get the solution of the optimization problem (base velocity + joint velocities) - * @return the solution of the optimization problem - */ - const iDynTree::VectorDynSize& getSolution() const; - - /** - * Get the solution of the optimization problem (only joint velocities) - * @return the solution of the optimization problem - */ - const iDynTree::VectorDynSize& getDesiredJointVelocities() const; - - /** - * Get the error seen by the QP problem for the left foot. - * @note it can be useful for debug - * @return the error - */ - iDynTree::VectorDynSize getLeftFootError(); - - /** - * Get the error seen by the QP problem for the right foot. - * @note it can be useful for debug - * @return the error - */ - iDynTree::VectorDynSize getRightFootError(); - - /** - * Get the hessian matrix - * @return the hessian matrix - */ - const iDynTree::MatrixDynSize& getHessianMatrix() const; - - /** - * Get the gradient vector - * @return the gradient vector - */ - const iDynTree::VectorDynSize& getGradient() const; - - /** - * Get the Constraint Matrix - * @return the constraint matrix - */ - const iDynSparseMatrix& getConstraintMatrix() const; - - /** - * Get the upper bound - * @return the upper bound - */ - const iDynTree::VectorDynSize& getUpperBound() const; - - /** - * Get the lower bound - * @return the lower bound - */ - const iDynTree::VectorDynSize& getLowerBound() const; - }; -}; + double m_kJointLimitsLowerBound; /**< Gain related to the the joint lower bound */ + iDynTree::VectorDynSize m_jointVelocitiesBounds; /**< Bounds on the joint velocities*/ + iDynTree::VectorDynSize m_jointPositionsUpperBounds; /**< Upper Bounds on the joint position*/ + iDynTree::VectorDynSize m_jointPositionsLowerBounds; /**< Lower Bounds on the joint position*/ + + // gain scheduling + std::unique_ptr m_handWeightSmoother; /**< Minimum jerk trajectory + for the hand weight matrix. */ + yarp::sig::Vector m_handWeightWalkingVector; /**< Weight matrix (only the diagonal) used for + the hand retargeting during walking. */ + yarp::sig::Vector m_handWeightStanceVector; /**< Weight matrix (only the diagonal) used for + the hand retargeting during stance. */ + + // gain scheduling in case of joint retargeting + std::unique_ptr m_jointRetargetingWeightSmoother; /**< Minimum jerk trajectory + for the Joint retargeting weight matrix. */ + yarp::sig::Vector m_jointRetargetingWeightWalking; /**< Weight matrix (only the diagonal) used for + the joint retargeting during walking. */ + yarp::sig::Vector m_jointRetargetingWeightStance; /**< Weight matrix (only the diagonal) used for + the joint retargeting during stance. */ + + std::unique_ptr m_jointRegularizationWeightSmoother; /**< Minimum jerk trajectory + for the Joint regularization weight matrix. */ + yarp::sig::Vector m_jointRegularizationWeightWalking; /**< Weight matrix (only the diagonal) used for + the joint regularization during walking. */ + yarp::sig::Vector m_jointRegularizationWeightStance; /**< Weight matrix (only the diagonal) used for + the joint regularization during stance. */ + + std::unique_ptr m_torsoWeightSmoother; /**< Minimum jerk trajectory for the torso weight matrix. */ + double m_torsoWeightWalking; /**< Weight matrix (only the diagonal) used for the torso during walking. */ + double m_torsoWeightStance; /**< Weight matrix (only the diagonal) used for the torso during stance. */ + + bool m_useCoMAsConstraint; /**< True if the CoM is added as a constraint. */ + bool m_useJointsLimitsConstraint; /**< True if the CoM is added as a constraint. */ + bool m_enableHandRetargeting; /**< True if the hand retargeting is used */ + bool m_enableJointRetargeting; /**< True if the joint retargeting is used */ + + iDynTree::MatrixDynSize m_hessianDense; /**< Hessian matrix */ + iDynTree::VectorDynSize m_gradient; /**< Gradient vector */ + iDynSparseMatrix m_constraintsMatrixSparse; /**< Constraint matrix */ + iDynTree::VectorDynSize m_lowerBound; /**< Lower bound */ + iDynTree::VectorDynSize m_upperBound; /**< Upper bound */ + iDynTree::VectorDynSize m_solution; /**< Solution of the optimization problem */ + iDynTree::VectorDynSize m_desiredJointVelocitiesOutput; /**< Joint velocities required by the optimization problem */ + + /** + * Initialize all the constant matrix from the configuration file. + * @param config configuration parameters + * @return true/false in case of success/failure. + */ + bool initializeMatrices(const yarp::os::Searchable& config); + + /** + * Initialize hand retargeting. + * @param config configuration parameters + * @return true/false in case of success/failure. + */ + bool initializeHandRetargeting(const yarp::os::Searchable& config); + + /** + * Initialize joint retargeting. + * @param config configuration parameters + * @return true/false in case of success/failure. + */ + bool initializeJointRetargeting(const yarp::os::Searchable& config); + + + /** + * Instantiate the solver + */ + virtual void instantiateSolver() = 0; + + /** + * Evaluate the Hessian matrix. + */ + void evaluateHessianMatrix(); + + /** + * Evaluate the gradient vector. + */ + void evaluateGradientVector(); + + /** + * Evaluate the Linear constraint matrix. + */ + void evaluateLinearConstraintMatrix(); + + /** + * Evaluate Lower and upper bounds + */ + void evaluateBounds(); + + /** + * Set the number of constraints (it may change according to the solver used) + */ + virtual void setNumberOfConstraints(){;}; + + /** + * Initialize matrices that depends on the solver used + */ + virtual void initializeSolverSpecificMatrices(){;}; + + /** + * Set joint velocities bounds + */ + virtual void setJointVelocitiesBounds() = 0; + + public: + /** + * Initialize the QP-IK problem. + * @param config config of the QP-IK solver + * @param actuatedDOFs number of the actuated DoF + * @param minJointsPosition is a vector containing the min joints position limit + * @param minJointsPosition is a vector containing the max joints position limit + * @param maxJointsVelocity is a vector containing the max joints velocity limit. + * @return true/false in case of success/failure. + */ + bool initialize(const yarp::os::Searchable &config, + const int &actuatedDOFs, + const iDynTree::VectorDynSize& maxJointsVelocity, + const iDynTree::VectorDynSize& maxJointsPosition, + const iDynTree::VectorDynSize& minJointsPosition); + + /** + * Set the robot state. + * @param jointPosition vector of joint positions (in rad); + * @param leftFootToWorldTransform transformation between the inertial frame and the left foot; + * @param rightFootToWorldTransform transformation between the inertial frame and the right foot; + * @param leftHandToWorldTransform transformation between the inertial frame and the left foot; + * @param rightHandToWorldTransform transformation between the inertial frame and the right foot; + * @param neckOrientation rotation between the inertial frame and the neck; + * @param comPosition position of the CoM + * @return true/false in case of success/failure. + */ + bool setRobotState(const iDynTree::VectorDynSize& jointPosition, + const iDynTree::Transform& leftFootToWorldTransform, + const iDynTree::Transform& rightFootToWorldTransform, + const iDynTree::Transform& leftHandToWorldTransform, + const iDynTree::Transform& rightHandToWorldTransform, + const iDynTree::Rotation& neckOrientation, + const iDynTree::Position& comPosition); + + /** + * Set the Jacobian of the CoM + * @param comJacobian jacobian of the CoM (mixed representation) + * @return true/false in case of success/failure. + */ + bool setCoMJacobian(const iDynTree::MatrixDynSize& comJacobian); + + /** + * Set the Jacobian of the left foot + * @param leftFootJacobian jacobian of the left foot (mixed representation) + * @return true/false in case of success/failure. + */ + bool setLeftFootJacobian(const iDynTree::MatrixDynSize& leftFootJacobian); + + /** + * Set the Jacobian of the right foot + * @param rightFootJacobian jacobian of the right foot (mixed representation) + * @return true/false in case of success/failure. + */ + bool setRightFootJacobian(const iDynTree::MatrixDynSize& rightFootJacobian); + + /** + * Set the Jacobian of the left hand + * @param leftHandJacobian jacobian of the left hand (mixed representation) + * @return true/false in case of success/failure. + */ + bool setLeftHandJacobian(const iDynTree::MatrixDynSize& leftHandJacobian); + + /** + * Set the Jacobian of the right hand + * @param rightHandJacobian jacobian of the right hand (mixed representation) + * @return true/false in case of success/failure. + */ + bool setRightHandJacobian(const iDynTree::MatrixDynSize& rightHandJacobian); + + /** + * Set the Jacobian of the neck + * @param neckJacobian jacobian of the neck (mixed representation) + * @return true/false in case of success/failure. + */ + bool setNeckJacobian(const iDynTree::MatrixDynSize& neckJacobian); + + /** + * Set the desired joint position. + * Please use this term as regularization term. + * @param regularizationTerm vector of the desired joint position. + * @return true/false in case of success/failure. + */ + bool setDesiredJointPosition(const iDynTree::VectorDynSize& regularizationTerm); + + /** + * Set the desired twist of both feet + * @param leftFootTwist contain the desired twist of the left foot (MIXED representation); + * @param rightFootTwist contain the desired twist of the right foot (MIXED representation). + */ + void setDesiredFeetTwist(const iDynTree::Twist& leftFootTwist, + const iDynTree::Twist& rightFootTwist); + + /** + * Set the desired twist of both feet + * @param leftHandTwist contain the desired twist of the left hand (MIXED representation); + * @param rightHandTwist contain the desired twist of the right hand (MIXED representation). + */ + void setDesiredHandsTwist(const iDynTree::Twist& leftHandTwist, + const iDynTree::Twist& rightHandTwist); + + /** + * Set the desired twist of both feet + * @param jointPosition contain the desired joint position used in the retargeting. + * @return true/false in case of success/failure. + */ + bool setDesiredRetargetingJoint(const iDynTree::VectorDynSize& jointPosition); + + /** + * Set the desired CoMVelocity + * @param comVelocity contain the desired CoM velocity. + */ + void setDesiredCoMVelocity(const iDynTree::Vector3& comVelocity); + + /** + * Set the desired feet transformation + * @param desiredLeftFootToWorldTransform desired transformation between the left foot and the world frame; + * @param desiredRightFootToWorldTransform desired transformation between the right foot and the world frame. + */ + void setDesiredFeetTransformation(const iDynTree::Transform& desiredLeftFootToWorldTransform, + const iDynTree::Transform& desiredRightFootToWorldTransform); + + /** + * Set the desired hand transformation + * @param desiredLeftHandToWorldTransform desired transformation between the left hand and the world frame; + * @param desireRightHandToWorldTransform desired transformation between the right hand and the world frame. + */ + void setDesiredHandsTransformation(const iDynTree::Transform& desiredLeftHandToWorldTransform, + const iDynTree::Transform& desiredRightHandToWorldTransform); + + /** + * Set the desired orientation of the neck + * @param desiredNeckOrientation rotation matrix between the neck and the world frame. + */ + void setDesiredNeckOrientation(const iDynTree::Rotation& desiredNeckOrientation); + + /** + * Set the desired CoMPosition + * @param desiredComPosition contain the desired CoM position. + */ + void setDesiredCoMPosition(const iDynTree::Position& desiredComPosition); + + /** + * Set the robot phase. This is used by the minimum jerk trajectory. + * @param isStancePhase true if the robot is in the stance phase + */ + void setPhase(const bool& isStancePhase); + + /** + * Solve the optimization problem. + * @return true/false in case of success/failure. + */ + virtual bool solve() = 0; + + /** + * Get the solution of the optimization problem (base velocity + joint velocities) + * @return the solution of the optimization problem + */ + const iDynTree::VectorDynSize& getSolution() const; + + /** + * Get the solution of the optimization problem (only joint velocities) + * @return the solution of the optimization problem + */ + const iDynTree::VectorDynSize& getDesiredJointVelocities() const; + + /** + * Get the error seen by the QP problem for the left foot. + * @note it can be useful for debug + * @return the error + */ + iDynTree::VectorDynSize getLeftFootError(); + + /** + * Get the error seen by the QP problem for the right foot. + * @note it can be useful for debug + * @return the error + */ + iDynTree::VectorDynSize getRightFootError(); + + /** + * Get the hessian matrix + * @return the hessian matrix + */ + const iDynTree::MatrixDynSize& getHessianMatrix() const; + + /** + * Get the gradient vector + * @return the gradient vector + */ + const iDynTree::VectorDynSize& getGradient() const; + + /** + * Get the Constraint Matrix + * @return the constraint matrix + */ + const iDynSparseMatrix& getConstraintMatrix() const; + + /** + * Get the upper bound + * @return the upper bound + */ + const iDynTree::VectorDynSize& getUpperBound() const; + + /** + * Get the lower bound + * @return the lower bound + */ + const iDynTree::VectorDynSize& getLowerBound() const; + }; + }; #endif diff --git a/src/WholeBodyControllers/src/QPInverseKinematics.cpp b/src/WholeBodyControllers/src/QPInverseKinematics.cpp index 4479e909..19ced2fa 100644 --- a/src/WholeBodyControllers/src/QPInverseKinematics.cpp +++ b/src/WholeBodyControllers/src/QPInverseKinematics.cpp @@ -43,6 +43,14 @@ bool WalkingQPIK::initialize(const yarp::os::Searchable &config, m_useCoMAsConstraint = config.check("use_com_as_constraint", yarp::os::Value(false)).asBool(); m_enableHandRetargeting = config.check("use_hand_retargeting", yarp::os::Value(false)).asBool(); + m_enableJointRetargeting = config.check("use_joint_retargeting", yarp::os::Value(false)).asBool(); + + if(m_enableHandRetargeting && m_enableJointRetargeting) + { + yError() << "[initialize] You cannot enable both hand and joint retargeting."; + return false; + } + m_useJointsLimitsConstraint = config.check("use_joint_limits_constraint", yarp::os::Value(false)).asBool(); // TODO in the future the number of constraints should be added inside @@ -79,6 +87,7 @@ bool WalkingQPIK::initialize(const yarp::os::Searchable &config, m_rightHandJacobian.resize(6, m_numberOfVariables); m_regularizationTerm.resize(m_actuatedDOFs); + m_retargetingJointValue.resize(m_actuatedDOFs); m_jointPosition.resize(m_actuatedDOFs); // preprare constant matrix necessary for the QP problem @@ -97,6 +106,13 @@ bool WalkingQPIK::initialize(const yarp::os::Searchable &config, return false; } + if(m_enableJointRetargeting) + if(!initializeJointRetargeting(config)) + { + yError() << "[initialize] Unable to Initialize the joint retargeting."; + return false; + } + if(!setJointsBounds(maxJointsVelocity, maxJointsPosition, minJointsPosition)) { yError() << "[initialize] Unable to set the joint bounds."; @@ -143,19 +159,22 @@ bool WalkingQPIK::initializeMatrices(const yarp::os::Searchable& config) } } - if(!YarpUtilities::getNumberFromSearchable(config, "neck_weight", m_neckWeight)) + // if the joint retargeting is enabled the weights of the cost function are time-variant. + if(!m_enableJointRetargeting) { - yError() << "Initialization failed while reading neck_weight vector."; - return false; - } - - // set the matrix related to the joint regularization + if(!YarpUtilities::getNumberFromSearchable(config, "neck_weight", m_neckWeight)) + { + yError() << "Initialization failed while reading neck_weight vector."; + return false; + } - m_jointRegularizationWeights.resize(m_actuatedDOFs); - if(!YarpUtilities::getVectorFromSearchable(config, "joint_regularization_weights", m_jointRegularizationWeights)) - { - yError() << "Initialization failed while reading jointRegularizationWeights vector."; - return false; + // set the matrix related to the joint regularization + m_jointRegularizationWeights.resize(m_actuatedDOFs); + if(!YarpUtilities::getVectorFromSearchable(config, "joint_regularization_weights", m_jointRegularizationWeights)) + { + yError() << "Initialization failed while reading jointRegularizationWeights vector."; + return false; + } } m_jointRegularizationGains.resize(m_actuatedDOFs); @@ -242,7 +261,6 @@ bool WalkingQPIK::initializeHandRetargeting(const yarp::os::Searchable& config) return false; } - m_handWeightStanceVector.resize(6); if(!YarpUtilities::getVectorFromSearchable(config, "hand_weight_stance", m_handWeightStanceVector)) { @@ -271,6 +289,85 @@ bool WalkingQPIK::initializeHandRetargeting(const yarp::os::Searchable& config) return true; } +bool WalkingQPIK::initializeJointRetargeting(const yarp::os::Searchable& config) +{ + double smoothingTime; + if(!YarpUtilities::getNumberFromSearchable(config, "smoothing_time", smoothingTime)) + { + yError() << "[initializeJointRetargeting] Initialization failed while reading smoothingTime."; + return false; + } + + double dT; + if(!YarpUtilities::getNumberFromSearchable(config, "sampling_time", dT)) + { + yError() << "Initialization failed while a double."; + return false; + } + + // joint retargeting + m_jointRetargetingGains.resize(m_actuatedDOFs); + if(!YarpUtilities::getVectorFromSearchable(config, "joint_retargeting_gains", m_jointRetargetingGains)) + { + yError() << "[initializeJointRetargeting] Initialization failed while reading jointRetargetingGains vector."; + return false; + } + + // if the joint retargeting is enable the weights of the cost function are time variant + m_jointRetargetingWeightWalking.resize(m_actuatedDOFs); + if(!YarpUtilities::getVectorFromSearchable(config, "joint_retargeting_weight_walking", m_jointRetargetingWeightWalking)) + { + yError() << "[initializeJointRetargeting] Initialization failed while reading the yarp vector."; + return false; + } + + m_jointRetargetingWeightStance.resize(m_actuatedDOFs); + if(!YarpUtilities::getVectorFromSearchable(config, "joint_retargeting_weight_stance", m_jointRetargetingWeightStance)) + { + yError() << "[initializeJointRetargeting] Initialization failed while reading the yarp vector."; + return false; + } + + m_jointRetargetingWeightSmoother = std::make_unique(m_actuatedDOFs, dT, smoothingTime); + m_jointRetargetingWeightSmoother->init(m_jointRetargetingWeightStance); + + // joint regularization + m_jointRegularizationWeightWalking.resize(m_actuatedDOFs); + if(!YarpUtilities::getVectorFromSearchable(config, "joint_regularization_weight_walking", m_jointRegularizationWeightWalking)) + { + yError() << "[initializeJointRetargeting] Initialization failed while reading the yarp vector."; + return false; + } + + m_jointRegularizationWeightStance.resize(m_actuatedDOFs); + if(!YarpUtilities::getVectorFromSearchable(config, "joint_regularization_weight_stance", m_jointRegularizationWeightStance)) + { + yError() << "[initializeJointRetargeting] Initialization failed while reading the yarp vector."; + return false; + } + + m_jointRegularizationWeightSmoother = std::make_unique(m_actuatedDOFs, dT, smoothingTime); + m_jointRegularizationWeightSmoother->init(m_jointRegularizationWeightStance); + + // torso + if(!YarpUtilities::getNumberFromSearchable(config, "torso_weight_walking", m_torsoWeightWalking)) + { + yError() << "[initializeJointRetargeting] Initialization failed while reading the double."; + return false; + } + + if(!YarpUtilities::getNumberFromSearchable(config, "torso_weight_stance", m_torsoWeightStance)) + { + yError() << "[initializeJointRetargeting] Initialization failed while reading the double."; + return false; + } + + m_torsoWeightSmoother = std::make_unique(1, dT, smoothingTime); + m_torsoWeightSmoother->init(yarp::sig::Vector(1, m_torsoWeightStance)); + + return true; +} + bool WalkingQPIK::setRobotState(const iDynTree::VectorDynSize& jointPosition, const iDynTree::Transform& leftFootToWorldTransform, const iDynTree::Transform& rightFootToWorldTransform, @@ -304,13 +401,30 @@ bool WalkingQPIK::setRobotState(const iDynTree::VectorDynSize& jointPosition, void WalkingQPIK::setPhase(const bool& isStancePhase) { - if(!m_enableHandRetargeting) - return; - if(isStancePhase) - m_handWeightSmoother->computeNextValues(m_handWeightStanceVector); + { + if(m_enableHandRetargeting) + m_handWeightSmoother->computeNextValues(m_handWeightStanceVector); + + if(m_enableJointRetargeting) + { + m_torsoWeightSmoother->computeNextValues(yarp::sig::Vector(1, m_torsoWeightStance)); + m_jointRetargetingWeightSmoother->computeNextValues(m_jointRetargetingWeightStance); + m_jointRegularizationWeightSmoother->computeNextValues(m_jointRegularizationWeightStance); + } + } else - m_handWeightSmoother->computeNextValues(m_handWeightWalkingVector); + { + if(m_enableHandRetargeting) + m_handWeightSmoother->computeNextValues(m_handWeightWalkingVector); + + if(m_enableJointRetargeting) + { + m_torsoWeightSmoother->computeNextValues(yarp::sig::Vector(1, m_torsoWeightWalking)); + m_jointRetargetingWeightSmoother->computeNextValues(m_jointRetargetingWeightWalking); + m_jointRegularizationWeightSmoother->computeNextValues(m_jointRegularizationWeightWalking); + } + } } void WalkingQPIK::setDesiredNeckOrientation(const iDynTree::Rotation& desiredNeckOrientation) @@ -470,6 +584,24 @@ void WalkingQPIK::setDesiredHandsTransformation(const iDynTree::Transform& desir m_desiredRightHandToWorldTransform = desiredRightHandToWorldTransform; } +bool WalkingQPIK::setDesiredRetargetingJoint(const iDynTree::VectorDynSize& jointPosition) +{ + // if the joint retargeting is not used return + if(!m_enableJointRetargeting) + return true; + + if(jointPosition.size() != m_actuatedDOFs) + { + yError() << "[WalkingQPIK::setDesiredRetargetingJoint] The size of the jointPosition vector is not coherent with the " + << "number of the actuated Joint"; + return false; + } + + m_retargetingJointValue = jointPosition; + + return true; +} + void WalkingQPIK::setDesiredHandsTwist(const iDynTree::Twist& leftHandTwist, const iDynTree::Twist& rightHandTwist) { @@ -493,11 +625,21 @@ void WalkingQPIK::evaluateHessianMatrix() // the joint angle auto hessianDense(iDynTree::toEigen(m_hessianDense)); - hessianDense = iDynTree::toEigen(m_neckJacobian).transpose() * - m_neckWeight * - iDynTree::toEigen(m_neckJacobian); - - hessianDense.bottomRightCorner(m_actuatedDOFs, m_actuatedDOFs) += iDynTree::toEigen(m_jointRegularizationWeights).asDiagonal(); + // if the joint retargeting is enable the weights of the cost function are time variant + if(!m_enableJointRetargeting) + { + hessianDense = iDynTree::toEigen(m_neckJacobian).transpose() * m_neckWeight * iDynTree::toEigen(m_neckJacobian); + hessianDense.bottomRightCorner(m_actuatedDOFs, m_actuatedDOFs) += iDynTree::toEigen(m_jointRegularizationWeights).asDiagonal(); + } + else + { + hessianDense = iDynTree::toEigen(m_neckJacobian).transpose() * + m_torsoWeightSmoother->getPos()(0) * + iDynTree::toEigen(m_neckJacobian); + hessianDense.bottomRightCorner(m_actuatedDOFs, m_actuatedDOFs) += + (iDynTree::toEigen(m_jointRegularizationWeightSmoother->getPos()) + + iDynTree::toEigen(m_jointRetargetingWeightSmoother->getPos())).asDiagonal(); + } if(m_enableHandRetargeting) { @@ -523,7 +665,6 @@ void WalkingQPIK::evaluateGradientVector() auto neckJacobian(iDynTree::toEigen(m_neckJacobian)); - auto jointRegularizationWeights(iDynTree::toEigen(m_jointRegularizationWeights)); auto jointRegularizationGains(iDynTree::toEigen(m_jointRegularizationGains)); auto jointPosition(iDynTree::toEigen(m_jointPosition)); auto regularizationTerm(iDynTree::toEigen(m_regularizationTerm)); @@ -536,14 +677,36 @@ void WalkingQPIK::evaluateGradientVector() // Neck orientation iDynTree::Matrix3x3 errorNeckAttitude = iDynTreeUtilities::Rotation::skewSymmetric(m_neckOrientation * m_desiredNeckOrientation.inverse()); - gradient = -neckJacobian.transpose() * m_neckWeight * (-m_kNeck - * iDynTree::unskew(iDynTree::toEigen(errorNeckAttitude))); - - // g = Weight * K_p * (regularizationTerm - jointPosition) - // Weight and K_p are two diagonal matrices so their product can be also evaluated multiplying component-wise - // the elements of the vectors and then generating the diagonal matrix - gradient.tail(m_actuatedDOFs) += (-jointRegularizationWeights.cwiseProduct(jointRegularizationGains)).asDiagonal() - * (regularizationTerm - jointPosition); + + if(!m_enableJointRetargeting) + { + auto jointRegularizationWeights(iDynTree::toEigen(m_jointRegularizationWeights)); + + gradient = -neckJacobian.transpose() * m_neckWeight * (-m_kNeck * iDynTree::unskew(iDynTree::toEigen(errorNeckAttitude))); + + // g = Weight * K_p * (regularizationTerm - jointPosition) + // Weight and K_p are two diagonal matrices so their product can be also evaluated multiplying component-wise + // the elements of the vectors and then generating the diagonal matrix + gradient.tail(m_actuatedDOFs) += (-jointRegularizationWeights.cwiseProduct(jointRegularizationGains)).asDiagonal() + * (regularizationTerm - jointPosition); + } + else + { + auto jointRetargetingGains(iDynTree::toEigen(m_jointRetargetingGains)); + auto jointRetargetingValues(iDynTree::toEigen(m_retargetingJointValue)); + + gradient = -neckJacobian.transpose() * m_torsoWeightSmoother->getPos()(0) + * (-m_kNeck * iDynTree::unskew(iDynTree::toEigen(errorNeckAttitude))); + + // g = Weight * K_p * (regularizationTerm - jointPosition) + // Weight and K_p are two diagonal matrices so their product can be also evaluated multiplying component-wise + // the elements of the vectors and then generating the diagonal matrix + gradient.tail(m_actuatedDOFs) += (-jointRegularizationGains.cwiseProduct( + iDynTree::toEigen(m_jointRegularizationWeightSmoother->getPos()))).asDiagonal() + * (regularizationTerm - jointPosition) + + (-jointRetargetingGains.cwiseProduct(iDynTree::toEigen(m_jointRetargetingWeightSmoother->getPos()))).asDiagonal() + * (jointRetargetingValues - jointPosition); + } if(m_enableHandRetargeting) { From 3d23bde63addda77ddd041c491a4b35d2b8c8218 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 26 Sep 2019 19:47:41 +0200 Subject: [PATCH 03/58] handle the joint retargeting in the Retargeting Client --- .../RetargetingHelper/Helper.h | 19 ++- src/RetargetingHelper/src/Helper.cpp | 134 +++++++++++++----- src/WalkingModule/src/Module.cpp | 7 +- 3 files changed, 124 insertions(+), 36 deletions(-) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index bcbb99c5..d29052d1 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -36,6 +36,7 @@ namespace WalkingControllers bool m_useHandRetargeting; /**< True if the hand retargeting is used */ bool m_useVirtualizer; /**< True if the virtualizer is used */ + bool m_useJointRetargeting; /**< True if the joint retargeting is used */ iDynTree::Transform m_leftHandTransform; /**< Left hand transform head_T_leftHand */ iDynTree::Transform m_rightHandTransform; /**< Right hand transform head_T_rightHand*/ @@ -54,6 +55,11 @@ namespace WalkingControllers yarp::os::BufferedPort m_robotOrientationPort; /**< Average orientation of the robot.*/ + std::vector m_retargetJointsIndex; /**< Vector containing the indices of the retarget joints. */ + yarp::os::BufferedPort m_jointRetargetingPort; /**< joint retargeting port. */ + iDynTree::VectorDynSize m_retargetJoints; /**< Values of the retarget Joints. */ + + /** * Convert a yarp vector containing position + rpy into an iDynTree homogeneous transform * @param vector a 6d yarp vector @@ -69,19 +75,23 @@ namespace WalkingControllers * @param config configuration parameters * @param name name of the module * @param period period of the module + * @param controlledJointsName name of the controlled joints * @return true/false in case of success/failure */ bool initialize(const yarp::os::Searchable &config, const std::string &name, - const double &period); + const double &period, + const std::vector& controlledJointsName); /** * Reset the client * @param leftHandTransform head_T_leftHand transform * @param rightHandTransform head_T_rightHand transform + * @param jointValues joint values [rad] */ void reset(const iDynTree::Transform& leftHandTransform, - const iDynTree::Transform& rightHandTransform); + const iDynTree::Transform& rightHandTransform, + const iDynTree::VectorDynSize& jointValues); /** * Close the client @@ -103,6 +113,11 @@ namespace WalkingControllers */ const iDynTree::Transform& rightHandTransform() const; + /** + * Get the value of the retarget joints + */ + const iDynTree::VectorDynSize& jointValues() const; + /** * Get the homogeneous transform of the right hand w.r.t. the head frame head_T_rightHand */ diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index 670aaa3e..dd1c03f7 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -26,19 +26,30 @@ void RetargetingClient::convertYarpVectorPoseIntoTransform(const yarp::sig::Vect bool RetargetingClient::initialize(const yarp::os::Searchable &config, const std::string &name, - const double &period) + const double &period, + const std::vector& controlledJointsName) { if(config.isNull()) { yInfo() << "[RetargetingClient::initialize] the hand retargeting is disable"; m_useHandRetargeting = false; m_useVirtualizer = false; + m_useJointRetargeting = false; return true; } m_useHandRetargeting = config.check("use_hand_retargeting", yarp::os::Value(false)).asBool(); + m_useJointRetargeting = config.check("use_joint_retargeting", yarp::os::Value(false)).asBool(); m_useVirtualizer = config.check("use_virtualizer", yarp::os::Value(false)).asBool(); + if(m_useJointRetargeting && m_useHandRetargeting) + { + yError() << "[RetargetingClient::initialize] You cannot enable the joint retargeting along with the hand retargeting."; + return false; + } + + m_retargetJoints.resize(controlledJointsName.size()); + std::string portName; if(m_useHandRetargeting) @@ -75,6 +86,41 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, m_rightHandSmoother = std::make_unique(6, period, smoothingTime); } + if(m_useJointRetargeting) + { + std::vector retargetJointNames; + + yarp::os::Value *retargetJointNamesYarp; + if(!config.check("retargeting_joint_list", retargetJointNamesYarp)) + { + yError() << "[RetargetingClient::initialize] Unable to find joints_list into config file."; + return false; + } + if(!YarpUtilities::yarpListToStringVector(retargetJointNamesYarp, retargetJointNames)) + { + yError() << "[RetargetingClient::initialize] Unable to convert yarp list into a vector of strings."; + return false; + } + + // find the indices + for(const std::string& joint : retargetJointNames) + { + int index = std::distance(controlledJointsName.begin(), std::find(controlledJointsName.begin(), + controlledJointsName.end(), + joint)); + + m_retargetJointsIndex.push_back(index); + } + + if(!YarpUtilities::getStringFromSearchable(config, "joint_retargeting_port_name", + portName)) + { + yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; + return false; + } + m_jointRetargetingPort.open("/" + name + portName); + } + if(m_useVirtualizer) { if(!YarpUtilities::getStringFromSearchable(config, "robot_orientation_port_name", @@ -90,48 +136,63 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, } void RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, - const iDynTree::Transform& rightHandTransform) + const iDynTree::Transform& rightHandTransform, + const iDynTree::VectorDynSize& jointValues) { - if(!m_useHandRetargeting) - return; + if(m_useHandRetargeting) + { + m_leftHandTransform = leftHandTransform; + m_rightHandTransform = rightHandTransform; - m_leftHandTransform = leftHandTransform; - m_rightHandTransform = rightHandTransform; + iDynTree::toEigen(m_leftHandTransformYarp).segment<3>(0) = + iDynTree::toEigen(m_leftHandTransform.getPosition()); - iDynTree::toEigen(m_leftHandTransformYarp).segment<3>(0) = - iDynTree::toEigen(m_leftHandTransform.getPosition()); + iDynTree::toEigen(m_leftHandTransformYarp).segment<3>(3) = + iDynTree::toEigen(m_leftHandTransform.getRotation().asRPY()); - iDynTree::toEigen(m_leftHandTransformYarp).segment<3>(3) = - iDynTree::toEigen(m_leftHandTransform.getRotation().asRPY()); + iDynTree::toEigen(m_rightHandTransformYarp).segment<3>(0) = + iDynTree::toEigen(m_rightHandTransform.getPosition()); - iDynTree::toEigen(m_rightHandTransformYarp).segment<3>(0) = - iDynTree::toEigen(m_rightHandTransform.getPosition()); + iDynTree::toEigen(m_rightHandTransformYarp).segment<3>(3) = + iDynTree::toEigen(m_rightHandTransform.getRotation().asRPY()); - iDynTree::toEigen(m_rightHandTransformYarp).segment<3>(3) = - iDynTree::toEigen(m_rightHandTransform.getRotation().asRPY()); + m_leftHandSmoother->init(m_leftHandTransformYarp); + m_rightHandSmoother->init(m_rightHandTransformYarp); + } - m_leftHandSmoother->init(m_leftHandTransformYarp); - m_rightHandSmoother->init(m_rightHandTransformYarp); + m_retargetJoints = jointValues; } void RetargetingClient::getFeedback() { - if(!m_useHandRetargeting) - return; + if(m_useHandRetargeting) + { + auto desiredLeftHandPose = m_leftHandTransformPort.read(false); + if(desiredLeftHandPose != nullptr) + m_leftHandTransformYarp = *desiredLeftHandPose; - auto desiredLeftHandPose = m_leftHandTransformPort.read(false); - if(desiredLeftHandPose != nullptr) - m_leftHandTransformYarp = *desiredLeftHandPose; + m_leftHandSmoother->computeNextValues(m_leftHandTransformYarp); + convertYarpVectorPoseIntoTransform(m_leftHandSmoother->getPos(), m_leftHandTransform); - m_leftHandSmoother->computeNextValues(m_leftHandTransformYarp); - convertYarpVectorPoseIntoTransform(m_leftHandSmoother->getPos(), m_leftHandTransform); + auto desiredRightHandPose = m_rightHandTransformPort.read(false); + if(desiredRightHandPose != nullptr) + m_rightHandTransformYarp = *desiredRightHandPose; - auto desiredRightHandPose = m_rightHandTransformPort.read(false); - if(desiredRightHandPose != nullptr) - m_rightHandTransformYarp = *desiredRightHandPose; + m_rightHandSmoother->computeNextValues(m_rightHandTransformYarp); + convertYarpVectorPoseIntoTransform(m_rightHandSmoother->getPos(), m_rightHandTransform); + } + + if(m_useJointRetargeting) + { + auto desiredJoint = m_jointRetargetingPort.read(false); + if(desiredJoint != nullptr) + { + for(int i =0; i < desiredJoint->size(); i++) + m_retargetJoints(m_retargetJointsIndex[i]) = (*desiredJoint)(i); + } + } - m_rightHandSmoother->computeNextValues(m_rightHandTransformYarp); - convertYarpVectorPoseIntoTransform(m_rightHandSmoother->getPos(), m_rightHandTransform); + return; } const iDynTree::Transform& RetargetingClient::leftHandTransform() const @@ -144,19 +205,28 @@ const iDynTree::Transform& RetargetingClient::rightHandTransform() const return m_rightHandTransform; } +const iDynTree::VectorDynSize& RetargetingClient::jointValues() const +{ + return m_retargetJoints; +} + void RetargetingClient::close() { - if(!m_useHandRetargeting) - return; + if(m_useHandRetargeting) + { + m_leftHandTransformPort.close(); + m_rightHandTransformPort.close(); + } - m_leftHandTransformPort.close(); - m_rightHandTransformPort.close(); + if(m_useJointRetargeting) + m_jointRetargetingPort.close(); } void RetargetingClient::setRobotBaseOrientation(const iDynTree::Rotation& rotation) { if(!m_useVirtualizer) return; + yarp::sig::Vector& output = m_robotOrientationPort.prepare(); output.clear(); output.push_back(rotation.asRPY()(2)); diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 6e3d5c0a..53815dad 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -284,7 +284,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) yarp::os::Bottle retargetingOptions = rf.findGroup("RETARGETING"); retargetingOptions.append(generalOptions); m_retargetingClient = std::make_unique(); - if (!m_retargetingClient->initialize(retargetingOptions, getName(), m_dT)) + if (!m_retargetingClient->initialize(retargetingOptions, getName(), m_dT, m_robotControlHelper->getAxesList())) { yError() << "[WalkingModule::configure] Failed to configure the retargeting"; return false; @@ -405,6 +405,8 @@ bool WalkingModule::solveQPIK(const std::unique_ptr& solver, const solver->setDesiredHandsTransformation(m_FKSolver->getHeadToWorldTransform() * m_retargetingClient->leftHandTransform(), m_FKSolver->getHeadToWorldTransform() * m_retargetingClient->rightHandTransform()); + ok &= solver->setDesiredRetargetingJoint(m_retargetingClient->jointValues()); + // set jacobians iDynTree::MatrixDynSize jacobian, comJacobian; jacobian.resize(6, m_robotControlHelper->getActuatedDoFs() + 6); @@ -505,7 +507,8 @@ bool WalkingModule::updateModule() m_retargetingClient->reset(m_FKSolver->getHeadToWorldTransform().inverse() * m_FKSolver->getLeftHandToWorldTransform(), m_FKSolver->getHeadToWorldTransform().inverse() - * m_FKSolver->getRightHandToWorldTransform()); + * m_FKSolver->getRightHandToWorldTransform(), + m_robotControlHelper->getJointPosition()); m_robotState = WalkingFSM::Prepared; From 69ca702dfd75da3b9452b1f9acbf231e201fe755 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 30 Sep 2019 11:48:24 +0200 Subject: [PATCH 04/58] tune gains on iCubGazeboV2_5 --- .../joint_retargeting/inverseKinematics.ini | 28 +++++++ .../joint_retargeting/jointRetargeting.ini | 8 ++ .../joint_retargeting/qpInverseKinematics.ini | 78 +++++++++++++++++++ .../joint_retargeting/robotControl.ini | 20 +++++ .../dcm_walking_joint_retargeting.ini | 60 ++++++++++++++ 5 files changed, 194 insertions(+) create mode 100644 app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/inverseKinematics.ini create mode 100644 app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini create mode 100644 app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematics.ini create mode 100644 app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini create mode 100644 app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini diff --git a/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/inverseKinematics.ini b/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/inverseKinematics.ini new file mode 100644 index 00000000..5ff6edbf --- /dev/null +++ b/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/inverseKinematics.ini @@ -0,0 +1,28 @@ +#The left_foot_frame is supposed to have the same orientation of the Inertia frame, which have the z axis pointing upwards, +#the x axis pointing forward and and the y concludes a right-handed frame +left_foot_frame l_sole +right_foot_frame r_sole + +#Remove the following line in order not to consider the +#additional frame +additional_frame neck_2 + +#The additional rotation is defined (by rows) in the Inertia frame. +#Remove the following line to keep the identity as additional rotation +additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) + +# solver paramenters +solver-verbosity 0 +solver_name ma27 +max-cpu-time 20 + +#DEGREES +jointRegularization (0, 0, 0, + 15, 0, 0, + -7, 22, 11, 30, 0, 0, 0, + -7, 22, 11, 30, 0, 0, 0, + 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, + 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) + +#jointRegularization (0, 15, 0, 0, -29.794, 29.97, 0.00, 44.977, -29.794, 29.97, 0.00, 44.977, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) +#jointRegularization (15, 0, 0, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) diff --git a/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini b/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini new file mode 100644 index 00000000..c434f32b --- /dev/null +++ b/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini @@ -0,0 +1,8 @@ +retargeting_joint_list ("neck_pitch", "neck_roll", "neck_yaw", + "torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw") + +joint_retargeting_port_name /jointPosition:i + +robot_orientation_port_name /torsoYaw:o diff --git a/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematics.ini b/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematics.ini new file mode 100644 index 00000000..93cd706a --- /dev/null +++ b/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematics.ini @@ -0,0 +1,78 @@ +# CoM +use_com_as_constraint 1 + +# torso +torso_weight_walking 5.0 +torso_weight_stance 0.0 +additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) + +## Regularization +joint_regularization (0,0,0, + 15, 0, 0, + -7, 22, 11, 30, 0, 0, 0, + -7, 22, 11, 30, 0, 0, 0, + 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, + 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) + +joint_regularization_gains (5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +joint_regularization_weight_stance (0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + +joint_regularization_weight_walking (1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, + 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +## Retargeting +joint_retargeting_gains (5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +joint_retargeting_weight_walking (1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, + 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +joint_retargeting_weight_stance (1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, + 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + + +#Gains +k_posCom 1.0 +k_posFoot 7.0 +k_attFoot 5.0 +k_neck 5.0 + +# Hand retargeting +k_posHand 2.5 +k_attHand 1.0 + +# use gain scheduling for hand retargeting +smoothing_time 0.5 + +# joint limits +k_joint_limit_lower_bound 1.0 +k_joint_limit_upper_bound 1.0 + +# use_joint_limits_constraint 1 diff --git a/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini b/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini new file mode 100644 index 00000000..b9fafe16 --- /dev/null +++ b/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini @@ -0,0 +1,20 @@ +robot icubSim + + +joints_list ("neck_pitch", "neck_roll", "neck_yaw", + "torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", + "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", + "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + + +remote_control_boards ("head", "torso", "left_arm", "right_arm", "left_leg", "right_leg") + +# filters +# if use_*_filter is equal to 0 the low pass filters are not used +use_joint_velocity_filter 0 +joint_velocity_cut_frequency 10.0 + +use_wrench_filter 0 +wrench_cut_frequency 10.0 diff --git a/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini b/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini new file mode 100644 index 00000000..444a20f8 --- /dev/null +++ b/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini @@ -0,0 +1,60 @@ +# Remove this line if you don't want to use the MPC +# use_mpc 1 + +# Remove this line if you don't want to use the QP-IK +use_QP-IK 1 + +# Remove this line if you don't want to use osqp to +# solve QP-IK. In this case qpOASES will be used +# use_osqp 1 + +# remove this line if you don't want to save data of the experiment +# dump_data 1 + +# general parameters +[GENERAL] +name walking-coordinator +# height of the com +com_height 0.53 +# sampling time +sampling_time 0.01 +# enable hand retargeting +use_joint_retargeting 1 +# enable the virtualizer +use_virtualizer 1 + +# include robot control parameters +[include ROBOT_CONTROL "./dcm_walking/joint_retargeting/robotControl.ini"] + +# include trajectory planner parameters +[include TRAJECTORY_PLANNER "./dcm_walking/common/plannerParams.ini"] + +# include MPC parameters +[include DCM_MPC_CONTROLLER "./dcm_walking/common/controllerParams.ini"] + +# include MPC parameters +[include DCM_REACTIVE_CONTROLLER "./dcm_walking/common/dcmReactiveControllerParams.ini"] + +# include MPC parameters +[include ZMP_CONTROLLER "./dcm_walking/common/zmpControllerParams.ini"] + +# include inverse kinematcs parameters +[include INVERSE_KINEMATICS_SOLVER "./dcm_walking/joint_retargeting/inverseKinematics.ini"] + +# include qp inverse kinematcs parameters +[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/joint_retargeting/qpInverseKinematics.ini"] + +# include inverse kinematcs parameters +[include FORWARD_KINEMATICS_SOLVER "./dcm_walking/common/forwardKinematics.ini"] + +# include FT sensors parameters +[include FT_SENSORS "./dcm_walking/common/forceTorqueSensors.ini"] + +# include Logger parameters +[include WALKING_LOGGER "./dcm_walking/common/walkingLogger.ini"] + +# include lower PID parameters +[include PID "./dcm_walking/common/pidParams.ini"] + +# retargeting +[include RETARGETING "./dcm_walking/joint_retargeting/jointRetargeting.ini"] From 340e68ae9c06540a7bfe2cdb205e0dfa4ee454b0 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 30 Sep 2019 12:07:42 +0200 Subject: [PATCH 05/58] rename iCubGazeboV2_5/dcm_walking_retargeting.ini into iCubGazeboV2_5/dcm_walking_hand_retargeting.ini --- ...m_walking_retargeting.ini => dcm_walking_hand_retargeting.ini} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/WalkingModule/app/robots/iCubGazeboV2_5/{dcm_walking_retargeting.ini => dcm_walking_hand_retargeting.ini} (100%) diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_hand_retargeting.ini similarity index 100% rename from src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_retargeting.ini rename to src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_hand_retargeting.ini From 91199360590f158be1c2497249ab84e3e0bffc96 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 30 Sep 2019 12:11:39 +0200 Subject: [PATCH 06/58] add parameters of iCubGenova04 (joint retargeting) --- .../joint_retargeting/inverseKinematics.ini | 28 +++++++ .../joint_retargeting/jointRetargeting.ini | 8 ++ .../joint_retargeting/plannerParams.ini | 55 +++++++++++++ .../joint_retargeting/qpInverseKinematics.ini | 78 +++++++++++++++++++ .../joint_retargeting/robotControl.ini | 20 +++++ .../joint_retargeting/zmpControllerParams.ini | 12 +++ .../dcm_walking_joint_retargeting.ini | 60 ++++++++++++++ 7 files changed, 261 insertions(+) create mode 100644 app/robots/iCubGenova04/dcm_walking/joint_retargeting/inverseKinematics.ini create mode 100644 app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini create mode 100644 app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini create mode 100644 app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini create mode 100644 app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini create mode 100644 app/robots/iCubGenova04/dcm_walking/joint_retargeting/zmpControllerParams.ini create mode 100644 app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/inverseKinematics.ini b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/inverseKinematics.ini new file mode 100644 index 00000000..5ff6edbf --- /dev/null +++ b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/inverseKinematics.ini @@ -0,0 +1,28 @@ +#The left_foot_frame is supposed to have the same orientation of the Inertia frame, which have the z axis pointing upwards, +#the x axis pointing forward and and the y concludes a right-handed frame +left_foot_frame l_sole +right_foot_frame r_sole + +#Remove the following line in order not to consider the +#additional frame +additional_frame neck_2 + +#The additional rotation is defined (by rows) in the Inertia frame. +#Remove the following line to keep the identity as additional rotation +additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) + +# solver paramenters +solver-verbosity 0 +solver_name ma27 +max-cpu-time 20 + +#DEGREES +jointRegularization (0, 0, 0, + 15, 0, 0, + -7, 22, 11, 30, 0, 0, 0, + -7, 22, 11, 30, 0, 0, 0, + 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, + 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) + +#jointRegularization (0, 15, 0, 0, -29.794, 29.97, 0.00, 44.977, -29.794, 29.97, 0.00, 44.977, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) +#jointRegularization (15, 0, 0, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini new file mode 100644 index 00000000..c434f32b --- /dev/null +++ b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini @@ -0,0 +1,8 @@ +retargeting_joint_list ("neck_pitch", "neck_roll", "neck_yaw", + "torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw") + +joint_retargeting_port_name /jointPosition:i + +robot_orientation_port_name /torsoYaw:o diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini new file mode 100644 index 00000000..95afc70c --- /dev/null +++ b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini @@ -0,0 +1,55 @@ +##Timings +plannerHorizon 5.0 + +##Unicycle Related Quantities +unicycleGain 10.0 +referencePosition (0.1 0.0) +timeWeight 2.5 +positionWeight 1.0 +slowWhenTurningGain 5.0 + +##Bounds +#Step length +maxStepLength 0.20 +minStepLength 0.02 +#Width +minWidth 0.15 +#Angle Variations in DEGREES +#maxAngleVariation 12.0 +maxAngleVariation 20.0 +minAngleVariation 8.0 +#Timings +maxStepDuration 0.85 +minStepDuration 0.75 + +##Nominal Values +#Width +nominalWidth 0.16 +#Height +stepHeight 0.02 +stepLandingVelocity 0.00 +footApexTime 0.8 +comHeightDelta 0.0 +#Timings +nominalDuration 0.8 +lastStepSwitchTime 0.8 +switchOverSwingRatio 0.7 + +#ZMP Delta +leftZMPDelta (0.03 -0.005) +rightZMPDelta (0.03 0.005) + +#MergePoint +mergePointRatio 0.4 + +# pitch delta +# pitchDelta -3.0 +#pitchDelta 0.0 + +##Should be the first step with the left foot? +swingLeft 1 +startAlwaysSameFoot 1 + +##Remove this line if you don't want to use the minimum jerk trajectory in feet interpolation +#useMinimumJerkFootTrajectory 1 + diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini new file mode 100644 index 00000000..93cd706a --- /dev/null +++ b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini @@ -0,0 +1,78 @@ +# CoM +use_com_as_constraint 1 + +# torso +torso_weight_walking 5.0 +torso_weight_stance 0.0 +additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) + +## Regularization +joint_regularization (0,0,0, + 15, 0, 0, + -7, 22, 11, 30, 0, 0, 0, + -7, 22, 11, 30, 0, 0, 0, + 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, + 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) + +joint_regularization_gains (5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +joint_regularization_weight_stance (0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + +joint_regularization_weight_walking (1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, + 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +## Retargeting +joint_retargeting_gains (5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +joint_retargeting_weight_walking (1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, + 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +joint_retargeting_weight_stance (1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, + 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + + +#Gains +k_posCom 1.0 +k_posFoot 7.0 +k_attFoot 5.0 +k_neck 5.0 + +# Hand retargeting +k_posHand 2.5 +k_attHand 1.0 + +# use gain scheduling for hand retargeting +smoothing_time 0.5 + +# joint limits +k_joint_limit_lower_bound 1.0 +k_joint_limit_upper_bound 1.0 + +# use_joint_limits_constraint 1 diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini new file mode 100644 index 00000000..620878d9 --- /dev/null +++ b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini @@ -0,0 +1,20 @@ +robot icub + + +joints_list ("neck_pitch", "neck_roll", "neck_yaw", + "torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", + "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", + "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + + +remote_control_boards ("head", "torso", "left_arm", "right_arm", "left_leg", "right_leg") + +# filters +# if use_*_filter is equal to 0 the low pass filters are not used +use_joint_velocity_filter 0 +joint_velocity_cut_frequency 10.0 + +use_wrench_filter 0 +wrench_cut_frequency 10.0 diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/zmpControllerParams.ini b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/zmpControllerParams.ini new file mode 100644 index 00000000..5bdca487 --- /dev/null +++ b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/zmpControllerParams.ini @@ -0,0 +1,12 @@ +# remove the following line if you do not want the gain scheduling +useGainScheduling 1 + +smoothingTime 0.1 + +# if the gain scheduling is off only k*_walking parameters are used +kZMP_walking 3.0 +kCoM_walking 10.0 + +kZMP_stance 1.2 +kCoM_stance 6.0 + diff --git a/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini b/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini new file mode 100644 index 00000000..752f347e --- /dev/null +++ b/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini @@ -0,0 +1,60 @@ +# Remove this line if you don't want to use the MPC +# use_mpc 1 + +# Remove this line if you don't want to use the QP-IK +use_QP-IK 1 + +# Remove this line if you don't want to use osqp to +# solve QP-IK. In this case qpOASES will be used +# use_osqp 1 + +# remove this line if you don't want to save data of the experiment +# dump_data 1 + +# general parameters +[GENERAL] +name walking-coordinator +# height of the com +com_height 0.53 +# sampling time +sampling_time 0.01 +# enable joint retargeting +use_joint_retargeting 1 +# enable the virtualizer +use_virtualizer 1 + +# include robot control parameters +[include ROBOT_CONTROL "./dcm_walking/joint_retargeting/robotControl.ini"] + +# include trajectory planner parameters +[include TRAJECTORY_PLANNER "./dcm_walking/joint_retargeting/plannerParams.ini"] + +# include MPC parameters +[include DCM_MPC_CONTROLLER "./dcm_walking/common/controllerParams.ini"] + +# include MPC parameters +[include DCM_REACTIVE_CONTROLLER "./dcm_walking/common/dcmReactiveControllerParams.ini"] + +# include MPC parameters +[include ZMP_CONTROLLER "./dcm_walking/joint_retargeting/zmpControllerParams.ini"] + +# include inverse kinematcs parameters +[include INVERSE_KINEMATICS_SOLVER "./dcm_walking/joint_retargeting/inverseKinematics.ini"] + +# include qp inverse kinematcs parameters +[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/joint_retargeting/qpInverseKinematics.ini"] + +# include inverse kinematcs parameters +[include FORWARD_KINEMATICS_SOLVER "./dcm_walking/common/forwardKinematics.ini"] + +# include FT sensors parameters +[include FT_SENSORS "./dcm_walking/common/forceTorqueSensors.ini"] + +# include Logger parameters +[include WALKING_LOGGER "./dcm_walking/common/walkingLogger.ini"] + +# include lower PID parameters +[include PID "./dcm_walking/common/pidParams.ini"] + +# retargeting +[include RETARGETING "./dcm_walking/joint_retargeting/jointRetargeting.ini"] From 4fa140997c432afa2835d2b6d4b4beae6e02b1a7 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 30 Sep 2019 12:13:17 +0200 Subject: [PATCH 07/58] rename iCubGenova04/dcm_walking_retargeting.ini into iCubGenova04/dcm_walking_hand_retargeting.ini --- ...m_walking_retargeting.ini => dcm_walking_hand_retargeting.ini} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/WalkingModule/app/robots/iCubGenova04/{dcm_walking_retargeting.ini => dcm_walking_hand_retargeting.ini} (100%) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_hand_retargeting.ini similarity index 100% rename from src/WalkingModule/app/robots/iCubGenova04/dcm_walking_retargeting.ini rename to src/WalkingModule/app/robots/iCubGenova04/dcm_walking_hand_retargeting.ini From f29cf01687c46f78f2d159a13ca320ecd2abc54c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 10 Oct 2019 14:16:16 +0000 Subject: [PATCH 08/58] tune parameters on iCubGenova04 --- .../joint_retargeting/qpInverseKinematics.ini | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini index 93cd706a..5c4b65af 100644 --- a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini +++ b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini @@ -30,8 +30,8 @@ joint_regularization_weight_stance (0.0, 0.0, 0.0, joint_regularization_weight_walking (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, - 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, - 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) @@ -44,16 +44,16 @@ joint_retargeting_gains (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) joint_retargeting_weight_walking (1.0, 1.0, 1.0, - 1.0, 1.0, 1.0, - 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, - 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 0.0, 0.0, 0.0 + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) joint_retargeting_weight_stance (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, - 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, - 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) From 41b597628f06ffd337c4c828992a6b125b155eff Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 15 Oct 2019 14:47:27 +0200 Subject: [PATCH 09/58] add the lastStepDCMOffsetPercentage parameter please refer to robotology/unicycle-footstep-planner@6b889e42b730042af9ab9b4c06883ab10d142f39 --- .../dcm_walking/joint_retargeting/plannerParams.ini | 7 ++++++- src/TrajectoryPlanner/src/TrajectoryGenerator.cpp | 2 ++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini index 95afc70c..9fd7e27f 100644 --- a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini +++ b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini @@ -39,6 +39,12 @@ switchOverSwingRatio 0.7 leftZMPDelta (0.03 -0.005) rightZMPDelta (0.03 0.005) +# Last Step DCM Offset +# If it is 0.5 the final DCM will be in the middle of the two footsteps; +# If it is 0 the DCM position coincides with the stance foot ZMP; +# If it is 1 the DCM position coincides with the next foot ZMP position. +lastStepDCMOffset 0.1 + #MergePoint mergePointRatio 0.4 @@ -52,4 +58,3 @@ startAlwaysSameFoot 1 ##Remove this line if you don't want to use the minimum jerk trajectory in feet interpolation #useMinimumJerkFootTrajectory 1 - diff --git a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp index debbd022..b778ec80 100644 --- a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp +++ b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp @@ -99,6 +99,7 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) double switchOverSwingRatio = config.check("switchOverSwingRatio", yarp::os::Value(0.4)).asDouble(); double mergePointRatio = config.check("mergePointRatio", yarp::os::Value(0.5)).asDouble(); + double lastStepDCMOffset = config.check("lastStepDCMOffset", yarp::os::Value(0.0)).asDouble(); m_nominalWidth = config.check("nominalWidth", yarp::os::Value(0.04)).asDouble(); @@ -150,6 +151,7 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) m_dcmGenerator = m_trajectoryGenerator.addDCMTrajectoryGenerator(); m_dcmGenerator->setFootOriginOffset(leftZMPDelta, rightZMPDelta); m_dcmGenerator->setOmega(sqrt(9.81/comHeight)); + ok = ok && m_dcmGenerator->setLastStepDCMOffsetPercentage(lastStepDCMOffset); m_correctLeft = true; From 6c56b8dabd3f05373e02972f60e76e785ffa2311 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 15 Oct 2019 15:12:55 +0200 Subject: [PATCH 10/58] check the worst error before setting the position command only for the danegerous_joints --- .../joint_retargeting/robotControl.ini | 8 +++++ .../RobotInterface/Helper.h | 3 +- src/RobotInterface/src/Helper.cpp | 32 +++++++++++++------ .../hand_retargeting/robotControl.ini | 5 +++ .../joypad_control/robotControl.ini | 7 ++++ .../hand_retargeting/robotControl.ini | 5 +++ .../joypad_control/robotControl.ini | 7 ++++ .../WalkingControllers/YarpUtilities/Helper.h | 8 +++++ src/YarpUtilities/src/Helper.cpp | 26 +++++++++++++++ 9 files changed, 90 insertions(+), 11 deletions(-) diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini index 620878d9..57f1277b 100644 --- a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini +++ b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini @@ -9,6 +9,14 @@ joints_list ("neck_pitch", "neck_roll", "neck_yaw", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +dangerous_joints (0, 0, 0, + 1, 1, 1, + 1, 1, 1, 1, 0, 0, 0, + 1, 1, 1, 1, 0, 0, 0, + 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1) + + remote_control_boards ("head", "torso", "left_arm", "right_arm", "left_leg", "right_leg") # filters diff --git a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h index c349f9de..f6eb5f1f 100644 --- a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h +++ b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h @@ -35,6 +35,7 @@ namespace WalkingControllers { yarp::dev::PolyDriver m_robotDevice; /**< Main robot device. */ std::vector m_axesList; /**< Vector containing the name of the controlled joints. */ + std::vector m_dangerousJoints; /**< Vector containing the the information related to the importance of the joint. */ unsigned int m_actuatedDOFs; /**< Number of the actuated DoFs. */ // YARP Interfaces exposed by the remotecontrolboardremapper @@ -94,7 +95,7 @@ namespace WalkingControllers * @return true in case of success and false otherwise. */ bool getWorstError(const iDynTree::VectorDynSize& desiredJointPositionsRad, - std::pair& worstError); + std::pair& worstError); /** * Switch the control mode. diff --git a/src/RobotInterface/src/Helper.cpp b/src/RobotInterface/src/Helper.cpp index a354edae..20aa36f9 100644 --- a/src/RobotInterface/src/Helper.cpp +++ b/src/RobotInterface/src/Helper.cpp @@ -9,7 +9,7 @@ using namespace WalkingControllers; bool RobotInterface::getWorstError(const iDynTree::VectorDynSize& desiredJointPositionsRad, - std::pair& worstError) + std::pair& worstError) { if(!m_encodersInterface) { @@ -24,7 +24,7 @@ bool RobotInterface::getWorstError(const iDynTree::VectorDynSize& desiredJointPo } // clear the std::pair - worstError.first = ""; + worstError.first = 0; worstError.second = 0.0; double currentJointPositionRad; double absoluteJointErrorRad; @@ -35,7 +35,7 @@ bool RobotInterface::getWorstError(const iDynTree::VectorDynSize& desiredJointPo desiredJointPositionsRad(i))); if(absoluteJointErrorRad > worstError.second) { - worstError.first = m_axesList[i]; + worstError.first = i; worstError.second = absoluteJointErrorRad; } } @@ -186,6 +186,18 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) m_actuatedDOFs = m_axesList.size(); + yarp::os::Value *dangerousJoints; + if(!config.check("dangerous_joints", dangerousJoints)) + { + yError() << "[configureRobot] Unable to find dangerous_joints into config file."; + return false; + } + if(!YarpUtilities::yarpListToBoolVector(dangerousJoints, m_dangerousJoints)) + { + yError() << "[configureRobot] Unable to convert yarp list into a vector of bool."; + return false; + } + // open the device if(!m_robotDevice.open(options)) { @@ -501,7 +513,7 @@ bool RobotInterface::setPositionReferences(const iDynTree::VectorDynSize& desire m_desiredJointPositionRad = desiredJointPositionsRad; - std::pair worstError("", 0.0); + std::pair worstError(0, 0.0); if(!getWorstError(desiredJointPositionsRad, worstError)) { @@ -571,7 +583,7 @@ bool RobotInterface::checkMotionDone(bool& motionDone) bool checkMotionDone = false; m_positionInterface->checkMotionDone(&checkMotionDone); - std::pair worstError; + std::pair worstError; if (!getWorstError(m_desiredJointPositionRad, worstError)) { yError() << "[RobotInterface::checkMotionDone] Unable to get the worst error."; @@ -583,7 +595,7 @@ bool RobotInterface::checkMotionDone(bool& motionDone) if (now - m_startingPositionControlTime > m_positioningTime + timeThreshold) { yError() << "[RobotInterface::checkMotionDone] The timer is expired but the joint " - << worstError.first << " has an error of " << worstError.second + << m_axesList[worstError.first] << " has an error of " << worstError.second << " radians"; return false; } @@ -623,7 +635,7 @@ bool RobotInterface::setDirectPositionReferences(const iDynTree::VectorDynSize& return false; } - std::pair worstError("", 0.0); + std::pair worstError(0, 0.0); if(!getWorstError(desiredPositionRad, worstError)) { @@ -631,11 +643,11 @@ bool RobotInterface::setDirectPositionReferences(const iDynTree::VectorDynSize& return false; } - if(worstError.second > 0.5) + if(worstError.second > 0.5 && m_dangerousJoints[worstError.first]) { yError() << "[RobotInterface::setDirectPositionReferences] The worst error between the current and the " - << "desired position of the " << worstError.first - << "-th joint is greater than 0.5 rad."; + << "desired position of the " << m_axesList[worstError.first] + << " joint is greater than 0.5 rad."; return false; } diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/robotControl.ini index a76b3b73..d9de4585 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/robotControl.ini @@ -7,6 +7,11 @@ joints_list ("torso_pitch", "torso_roll", "torso_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +dangerous_joints (1, 1, 1, + 1, 1, 1, 1, 0, 0, 0, + 1, 1, 1, 1, 0, 0, 0, + 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1) remote_control_boards ("torso", "left_arm", "right_arm", "left_leg", "right_leg") diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/robotControl.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/robotControl.ini index b9b39d18..6fe6ef93 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/robotControl.ini @@ -6,6 +6,13 @@ joints_list ("torso_pitch", "torso_roll", "torso_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +dangerous_joints (1, 1, 1, + 1, 1, 1, 1, + 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1) + + remote_control_boards ("torso", "left_arm", "right_arm", "left_leg", "right_leg") # filters diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/robotControl.ini index ccd2bcb0..269bb19d 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/robotControl.ini @@ -7,6 +7,11 @@ joints_list ("torso_pitch", "torso_roll", "torso_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +dangerous_joints (1, 1, 1, + 1, 1, 1, 1, 0, 0, 0, + 1, 1, 1, 1, 0, 0, 0, + 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1) remote_control_boards ("torso", "left_arm", "right_arm", "left_leg", "right_leg") diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/robotControl.ini index ab34b573..7371e212 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/robotControl.ini @@ -6,6 +6,13 @@ joints_list ("torso_pitch", "torso_roll", "torso_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +dangerous_joints (1, 1, 1, + 1, 1, 1, 1, + 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1) + + remote_control_boards ("torso", "left_arm", "right_arm", "left_leg", "right_leg") # filters diff --git a/src/YarpUtilities/include/WalkingControllers/YarpUtilities/Helper.h b/src/YarpUtilities/include/WalkingControllers/YarpUtilities/Helper.h index b7701ede..89845739 100644 --- a/src/YarpUtilities/include/WalkingControllers/YarpUtilities/Helper.h +++ b/src/YarpUtilities/include/WalkingControllers/YarpUtilities/Helper.h @@ -45,6 +45,14 @@ namespace WalkingControllers */ bool yarpListToStringVector(yarp::os::Value*& input, std::vector& output); + /** + * Convert a yarp list into a vector of bool + * @param input is the pointer of a yarp value; + * @param output is the vector of bool. + * @return true/false in case of success/failure + */ + bool yarpListToBoolVector(yarp::os::Value*& input, std::vector& output); + /** * Extract a string from a searchable object. * @param config is the searchable object; diff --git a/src/YarpUtilities/src/Helper.cpp b/src/YarpUtilities/src/Helper.cpp index c64b5787..c4710b4d 100644 --- a/src/YarpUtilities/src/Helper.cpp +++ b/src/YarpUtilities/src/Helper.cpp @@ -77,6 +77,32 @@ bool YarpUtilities::getStringFromSearchable(const yarp::os::Searchable& config, return true; } +bool YarpUtilities::yarpListToBoolVector(yarp::os::Value*& input, std::vector& output) +{ + // clear the std::vector + output.clear(); + + // check if the yarp value is a list + if(!input->isList()) + { + yError() << "[yarpListToBoolVector] The input is not a list."; + return false; + } + + yarp::os::Bottle *bottle = input->asList(); + for(int i = 0; i < bottle->size(); i++) + { + // check if the elements of the bottle are strings + if(!bottle->get(i).isString()) + { + yError() << "[yarpListToBoolVector] There is a field that is not a bool."; + return false; + } + output.push_back(bottle->get(i).asBool()); + } + return true; +} + bool YarpUtilities::getNumberFromSearchable(const yarp::os::Searchable& config, const std::string& key, double& number) { From e99ff376eb95c93d67a48b27c253aad19e1d3cba Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 15 Oct 2019 15:57:29 +0000 Subject: [PATCH 11/58] bugfix in Utilis.cpp --- src/YarpUtilities/src/Helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/YarpUtilities/src/Helper.cpp b/src/YarpUtilities/src/Helper.cpp index c4710b4d..5a2eccff 100644 --- a/src/YarpUtilities/src/Helper.cpp +++ b/src/YarpUtilities/src/Helper.cpp @@ -92,8 +92,8 @@ bool YarpUtilities::yarpListToBoolVector(yarp::os::Value*& input, std::vectorasList(); for(int i = 0; i < bottle->size(); i++) { - // check if the elements of the bottle are strings - if(!bottle->get(i).isString()) + // check if the elements of the bottle are bool or integer + if(!bottle->get(i).isBool() && !bottle->get(i).isInt()) { yError() << "[yarpListToBoolVector] There is a field that is not a bool."; return false; From a10371c04a604103a2f9b7d90fc31ff61f0ac913 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 15 Oct 2019 16:02:01 +0000 Subject: [PATCH 12/58] tune lastStepDCMOffset on iCubGenova04 --- .../dcm_walking/joint_retargeting/plannerParams.ini | 2 +- .../dcm_walking/joypad_control/plannerParams.ini | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini index 9fd7e27f..d4b579bf 100644 --- a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini +++ b/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini @@ -43,7 +43,7 @@ rightZMPDelta (0.03 0.005) # If it is 0.5 the final DCM will be in the middle of the two footsteps; # If it is 0 the DCM position coincides with the stance foot ZMP; # If it is 1 the DCM position coincides with the next foot ZMP position. -lastStepDCMOffset 0.1 +lastStepDCMOffset 0.25 #MergePoint mergePointRatio 0.4 diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/plannerParams.ini index 422d14c7..ea52baf5 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/plannerParams.ini @@ -39,6 +39,13 @@ switchOverSwingRatio 0.7 leftZMPDelta (0.03 -0.005) rightZMPDelta (0.03 0.005) +# Last Step DCM Offset +# If it is 0.5 the final DCM will be in the middle of the two footsteps; +# If it is 0 the DCM position coincides with the stance foot ZMP; +# If it is 1 the DCM position coincides with the next foot ZMP position. +lastStepDCMOffset 0.25 + + #MergePoint mergePointRatio 0.4 From 10df05aa64bce4048e4e82052d89233fba3466c9 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 26 Oct 2019 19:51:03 +0200 Subject: [PATCH 13/58] tune parameters on iCubGazeboV2_5 --- .../dcm_walking/joint_retargeting/inverseKinematics.ini | 0 .../dcm_walking/joint_retargeting/jointRetargeting.ini | 0 .../dcm_walking/joint_retargeting/qpInverseKinematics.ini | 0 .../dcm_walking/joint_retargeting/robotControl.ini | 6 ++++++ .../robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini | 0 5 files changed, 6 insertions(+) rename {app => src/WalkingModule/app}/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/inverseKinematics.ini (100%) rename {app => src/WalkingModule/app}/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini (100%) rename {app => src/WalkingModule/app}/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematics.ini (100%) rename {app => src/WalkingModule/app}/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini (80%) rename {app => src/WalkingModule/app}/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini (100%) diff --git a/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/inverseKinematics.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/inverseKinematics.ini similarity index 100% rename from app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/inverseKinematics.ini rename to src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/inverseKinematics.ini diff --git a/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini similarity index 100% rename from app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini rename to src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini diff --git a/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematics.ini similarity index 100% rename from app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematics.ini rename to src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematics.ini diff --git a/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini similarity index 80% rename from app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini rename to src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini index b9fafe16..17bf6679 100644 --- a/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini @@ -8,6 +8,12 @@ joints_list ("neck_pitch", "neck_roll", "neck_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +dangerous_joints (0, 0, 0, + 1, 1, 1, + 1, 1, 1, 1, 0, 0, 0, + 1, 1, 1, 1, 0, 0, 0, + 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1) remote_control_boards ("head", "torso", "left_arm", "right_arm", "left_leg", "right_leg") diff --git a/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini similarity index 100% rename from app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini rename to src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini From dd9a586ec0b550301920ffd53246d7d8f7b3df70 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 26 Oct 2019 19:51:15 +0200 Subject: [PATCH 14/58] tune parameters on iCubGenova04 --- .../dcm_walking/hand_retargeting/plannerParams.ini | 7 ++++++- .../dcm_walking/joint_retargeting/inverseKinematics.ini | 0 .../dcm_walking/joint_retargeting/jointRetargeting.ini | 0 .../dcm_walking/joint_retargeting/plannerParams.ini | 7 +------ .../dcm_walking/joint_retargeting/qpInverseKinematics.ini | 0 .../dcm_walking/joint_retargeting/robotControl.ini | 0 .../dcm_walking/joint_retargeting/zmpControllerParams.ini | 0 .../robots/iCubGenova04/dcm_walking_joint_retargeting.ini | 0 8 files changed, 7 insertions(+), 7 deletions(-) rename {app => src/WalkingModule/app}/robots/iCubGenova04/dcm_walking/joint_retargeting/inverseKinematics.ini (100%) rename {app => src/WalkingModule/app}/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini (100%) rename {app => src/WalkingModule/app}/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini (82%) rename {app => src/WalkingModule/app}/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini (100%) rename {app => src/WalkingModule/app}/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini (100%) rename {app => src/WalkingModule/app}/robots/iCubGenova04/dcm_walking/joint_retargeting/zmpControllerParams.ini (100%) rename {app => src/WalkingModule/app}/robots/iCubGenova04/dcm_walking_joint_retargeting.ini (100%) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini index 95afc70c..d4b579bf 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini @@ -39,6 +39,12 @@ switchOverSwingRatio 0.7 leftZMPDelta (0.03 -0.005) rightZMPDelta (0.03 0.005) +# Last Step DCM Offset +# If it is 0.5 the final DCM will be in the middle of the two footsteps; +# If it is 0 the DCM position coincides with the stance foot ZMP; +# If it is 1 the DCM position coincides with the next foot ZMP position. +lastStepDCMOffset 0.25 + #MergePoint mergePointRatio 0.4 @@ -52,4 +58,3 @@ startAlwaysSameFoot 1 ##Remove this line if you don't want to use the minimum jerk trajectory in feet interpolation #useMinimumJerkFootTrajectory 1 - diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/inverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/inverseKinematics.ini similarity index 100% rename from app/robots/iCubGenova04/dcm_walking/joint_retargeting/inverseKinematics.ini rename to src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/inverseKinematics.ini diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini similarity index 100% rename from app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini rename to src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini similarity index 82% rename from app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini rename to src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini index d4b579bf..95afc70c 100644 --- a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini @@ -39,12 +39,6 @@ switchOverSwingRatio 0.7 leftZMPDelta (0.03 -0.005) rightZMPDelta (0.03 0.005) -# Last Step DCM Offset -# If it is 0.5 the final DCM will be in the middle of the two footsteps; -# If it is 0 the DCM position coincides with the stance foot ZMP; -# If it is 1 the DCM position coincides with the next foot ZMP position. -lastStepDCMOffset 0.25 - #MergePoint mergePointRatio 0.4 @@ -58,3 +52,4 @@ startAlwaysSameFoot 1 ##Remove this line if you don't want to use the minimum jerk trajectory in feet interpolation #useMinimumJerkFootTrajectory 1 + diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini similarity index 100% rename from app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini rename to src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini similarity index 100% rename from app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini rename to src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini diff --git a/app/robots/iCubGenova04/dcm_walking/joint_retargeting/zmpControllerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/zmpControllerParams.ini similarity index 100% rename from app/robots/iCubGenova04/dcm_walking/joint_retargeting/zmpControllerParams.ini rename to src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/zmpControllerParams.ini diff --git a/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini similarity index 100% rename from app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini rename to src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini From 439abb8b39f8c9d7576d63a3b8d7b0bc73a5eae6 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 7 Nov 2019 16:50:43 +0100 Subject: [PATCH 15/58] implement first version of the comheight retargeting --- .../RetargetingHelper/Helper.h | 27 ++++- src/RetargetingHelper/src/Helper.cpp | 106 +++++++++++++++++- src/WalkingModule/src/Module.cpp | 25 +++-- 3 files changed, 142 insertions(+), 16 deletions(-) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index d29052d1..99300d12 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -11,10 +11,12 @@ // std #include +#include // iDyntree #include #include +#include // yarp #include @@ -37,15 +39,18 @@ namespace WalkingControllers bool m_useHandRetargeting; /**< True if the hand retargeting is used */ bool m_useVirtualizer; /**< True if the virtualizer is used */ bool m_useJointRetargeting; /**< True if the joint retargeting is used */ + bool m_useCoMHeightRetargeting; /**< True if the com retargeting is used */ iDynTree::Transform m_leftHandTransform; /**< Left hand transform head_T_leftHand */ iDynTree::Transform m_rightHandTransform; /**< Right hand transform head_T_rightHand*/ yarp::sig::Vector m_leftHandTransformYarp; /**< Left hand position + rpy head_T_leftHand */ yarp::sig::Vector m_rightHandTransformYarp; /**< Right hand position + rpy head_T_rightHand*/ + yarp::sig::Vector m_comHeightYarp; /**< Left hand position + rpy head_T_leftHand */ yarp::os::BufferedPort m_leftHandTransformPort; /**< Right hand position + rpy head_T_rightHand*/ yarp::os::BufferedPort m_rightHandTransformPort; /**< Right hand position + rpy head_T_rightHand*/ + yarp::os::BufferedPort m_comHeightPort; /**< CoMHeight */ std::unique_ptr m_leftHandSmoother; /**< Minimum jerk trajectory for the left hand. */ @@ -53,12 +58,21 @@ namespace WalkingControllers std::unique_ptr m_rightHandSmoother; /**< Minimum jerk trajectory for the right hand. */ + std::unique_ptr m_comHeightSmoother; /**< CoM Height smooter. */ + + yarp::os::BufferedPort m_robotOrientationPort; /**< Average orientation of the robot.*/ std::vector m_retargetJointsIndex; /**< Vector containing the indices of the retarget joints. */ yarp::os::BufferedPort m_jointRetargetingPort; /**< joint retargeting port. */ iDynTree::VectorDynSize m_retargetJoints; /**< Values of the retarget Joints. */ + double m_comHeightInput; + double m_comHeight; + double m_comHeightVelocity; + double m_comConstantHeight; + + bool m_isStancePhase{true}; /** * Convert a yarp vector containing position + rpy into an iDynTree homogeneous transform @@ -88,10 +102,13 @@ namespace WalkingControllers * @param leftHandTransform head_T_leftHand transform * @param rightHandTransform head_T_rightHand transform * @param jointValues joint values [rad] + * @param comHeight height of the CoM + * @return true/false in case of success/failure */ - void reset(const iDynTree::Transform& leftHandTransform, + bool reset(const iDynTree::Transform& leftHandTransform, const iDynTree::Transform& rightHandTransform, - const iDynTree::VectorDynSize& jointValues); + const iDynTree::VectorDynSize& jointValues, + const double& comHeight); /** * Close the client @@ -118,10 +135,16 @@ namespace WalkingControllers */ const iDynTree::VectorDynSize& jointValues() const; + double comHeight() const; + + double comHeightVelocity() const; + /** * Get the homogeneous transform of the right hand w.r.t. the head frame head_T_rightHand */ void setRobotBaseOrientation(const iDynTree::Rotation& rotation); + + bool setPhase(bool isStancePhase); }; }; #endif diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index dd1c03f7..7ca02269 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -35,12 +35,14 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, m_useHandRetargeting = false; m_useVirtualizer = false; m_useJointRetargeting = false; + m_useCoMHeightRetargeting = false; return true; } m_useHandRetargeting = config.check("use_hand_retargeting", yarp::os::Value(false)).asBool(); m_useJointRetargeting = config.check("use_joint_retargeting", yarp::os::Value(false)).asBool(); m_useVirtualizer = config.check("use_virtualizer", yarp::os::Value(false)).asBool(); + m_useCoMHeightRetargeting = config.check("use_com_retargeting", yarp::os::Value(false)).asBool(); if(m_useJointRetargeting && m_useHandRetargeting) { @@ -132,18 +134,38 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, m_robotOrientationPort.open("/" + name + portName); } + if(m_useCoMHeightRetargeting) + { + if(!YarpUtilities::getStringFromSearchable(config, "com_height_retargeting_port_name", portName)) + { + yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; + return false; + } + m_comHeightPort.open("/" + name + portName); + + double smoothingTime; + if(!YarpUtilities::getNumberFromSearchable(config, "smoothing_time", smoothingTime)) + { + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + return false; + } + + m_comHeightSmoother = std::make_unique(1, period, smoothingTime); + } + return true; } -void RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, +bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, const iDynTree::Transform& rightHandTransform, - const iDynTree::VectorDynSize& jointValues) + const iDynTree::VectorDynSize& jointValues, + const double& comHeight) { + m_leftHandTransform = leftHandTransform; + m_rightHandTransform = rightHandTransform; + if(m_useHandRetargeting) { - m_leftHandTransform = leftHandTransform; - m_rightHandTransform = rightHandTransform; - iDynTree::toEigen(m_leftHandTransformYarp).segment<3>(0) = iDynTree::toEigen(m_leftHandTransform.getPosition()); @@ -161,6 +183,43 @@ void RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, } m_retargetJoints = jointValues; + + m_comHeight = comHeight; + m_comHeightVelocity = 0; + m_comConstantHeight = comHeight; + + if(m_useCoMHeightRetargeting) + { + m_comHeightYarp(0) = comHeight; + m_comHeightSmoother->init(m_comHeightYarp); + + // let's read the port to reset the comHeightInput + bool okCoMHeight = false; + unsigned int attempt = 0; + do + { + if(!okCoMHeight) + { + auto desiredCoMHeight = m_comHeightPort.read(false); + if(desiredCoMHeight != nullptr) + { + m_comHeightInput = (*desiredCoMHeight)(2); + okCoMHeight = true; + } + } + + if(okCoMHeight) + return true; + + yarp::os::Time::delay(0.001); + attempt++; + } while (attempt < 100); + + yError() << "[RetargetingClient::reset] The retargeting port is not streaming data"; + return false; + } + + return true; } void RetargetingClient::getFeedback() @@ -192,6 +251,28 @@ void RetargetingClient::getFeedback() } } + + if(m_useCoMHeightRetargeting) + { + if(!m_isStancePhase) + m_comHeightYarp(0) = m_comConstantHeight; + else + { + auto desiredCoMHeight = m_comHeightPort.read(false); + if(desiredCoMHeight != nullptr) + { + m_comHeightYarp(0) += (*desiredCoMHeight)(2) - m_comHeightInput; + m_comHeightInput = (*desiredCoMHeight)(2); + } + + } + + m_comHeightSmoother->computeNextValues(m_comHeightYarp); + m_comHeight = m_comHeightSmoother->getPos()(0); + m_comHeightVelocity = m_comHeightSmoother->getVel()(0); + + } + return; } @@ -210,6 +291,16 @@ const iDynTree::VectorDynSize& RetargetingClient::jointValues() const return m_retargetJoints; } +double RetargetingClient::comHeight() const +{ + return m_comHeight; +} + +double RetargetingClient::comHeightVelocity() const +{ + return m_comHeightVelocity; +} + void RetargetingClient::close() { if(m_useHandRetargeting) @@ -232,3 +323,8 @@ void RetargetingClient::setRobotBaseOrientation(const iDynTree::Rotation& rotati output.push_back(rotation.asRPY()(2)); m_robotOrientationPort.write(false); } + +bool RetargetingClient::setPhase(bool isStancePhase) +{ + m_isStancePhase = isStancePhase; +} diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 53815dad..f56ee713 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -504,11 +504,17 @@ bool WalkingModule::updateModule() return false; } - m_retargetingClient->reset(m_FKSolver->getHeadToWorldTransform().inverse() - * m_FKSolver->getLeftHandToWorldTransform(), - m_FKSolver->getHeadToWorldTransform().inverse() - * m_FKSolver->getRightHandToWorldTransform(), - m_robotControlHelper->getJointPosition()); + if(!m_retargetingClient->reset(m_FKSolver->getHeadToWorldTransform().inverse() + * m_FKSolver->getLeftHandToWorldTransform(), + m_FKSolver->getHeadToWorldTransform().inverse() + * m_FKSolver->getRightHandToWorldTransform(), + m_robotControlHelper->getJointPosition(), + m_comHeightTrajectory.front())) + { + yError() << "[WalkingModule::updateModule] Unable to reset the retargeting client."; + return false; + + } m_robotState = WalkingFSM::Prepared; @@ -589,6 +595,9 @@ bool WalkingModule::updateModule() return false; } + double threshold = 0.001; + bool stancePhase = iDynTree::toEigen(m_DCMVelocityDesired.front()).norm() < threshold; + m_retargetingClient->setPhase(stancePhase); m_retargetingClient->getFeedback(); if(!updateFKSolver()) @@ -659,8 +668,6 @@ bool WalkingModule::updateModule() // inner COM-ZMP controller // if the the norm of desired DCM velocity is lower than a threshold then the robot // is stopped - double threshold = 0.001; - bool stancePhase = iDynTree::toEigen(m_DCMVelocityDesired.front()).norm() < threshold; m_walkingZMPController->setPhase(stancePhase); iDynTree::Vector2 desiredZMP; @@ -694,13 +701,13 @@ bool WalkingModule::updateModule() iDynTree::Position desiredCoMPosition; desiredCoMPosition(0) = outputZMPCoMControllerPosition(0); desiredCoMPosition(1) = outputZMPCoMControllerPosition(1); - desiredCoMPosition(2) = m_comHeightTrajectory.front(); + desiredCoMPosition(2) = m_retargetingClient->comHeight(); iDynTree::Vector3 desiredCoMVelocity; desiredCoMVelocity(0) = outputZMPCoMControllerVelocity(0); desiredCoMVelocity(1) = outputZMPCoMControllerVelocity(1); - desiredCoMVelocity(2) = m_comHeightVelocity.front(); + desiredCoMVelocity(2) = m_retargetingClient->comHeightVelocity(); // evaluate desired neck transformation double yawLeft = m_leftTrajectory.front().getRotation().asRPY()(2); From 14d01152c6a8d0e112161ee1f76d8330dfa3613d Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 12 Nov 2019 10:49:54 +0100 Subject: [PATCH 16/58] change the logic behind the com height retargeting --- .../WalkingControllers/RetargetingHelper/Helper.h | 4 ++-- src/RetargetingHelper/src/Helper.cpp | 12 ++++-------- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index 99300d12..e5375679 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -67,7 +67,7 @@ namespace WalkingControllers yarp::os::BufferedPort m_jointRetargetingPort; /**< joint retargeting port. */ iDynTree::VectorDynSize m_retargetJoints; /**< Values of the retarget Joints. */ - double m_comHeightInput; + double m_comHeightInputZero; double m_comHeight; double m_comHeightVelocity; double m_comConstantHeight; @@ -144,7 +144,7 @@ namespace WalkingControllers */ void setRobotBaseOrientation(const iDynTree::Rotation& rotation); - bool setPhase(bool isStancePhase); + void setPhase(bool isStancePhase); }; }; #endif diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index 7ca02269..8af85695 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -203,7 +203,7 @@ bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, auto desiredCoMHeight = m_comHeightPort.read(false); if(desiredCoMHeight != nullptr) { - m_comHeightInput = (*desiredCoMHeight)(2); + m_comHeightInputZero = (*desiredCoMHeight)(2); okCoMHeight = true; } } @@ -260,17 +260,13 @@ void RetargetingClient::getFeedback() { auto desiredCoMHeight = m_comHeightPort.read(false); if(desiredCoMHeight != nullptr) - { - m_comHeightYarp(0) += (*desiredCoMHeight)(2) - m_comHeightInput; - m_comHeightInput = (*desiredCoMHeight)(2); - } - + m_comHeightYarp(0) = (*desiredCoMHeight)(2) - m_comHeightInputZero + + m_comConstantHeight; } m_comHeightSmoother->computeNextValues(m_comHeightYarp); m_comHeight = m_comHeightSmoother->getPos()(0); m_comHeightVelocity = m_comHeightSmoother->getVel()(0); - } return; @@ -324,7 +320,7 @@ void RetargetingClient::setRobotBaseOrientation(const iDynTree::Rotation& rotati m_robotOrientationPort.write(false); } -bool RetargetingClient::setPhase(bool isStancePhase) +void RetargetingClient::setPhase(bool isStancePhase) { m_isStancePhase = isStancePhase; } From 9bef46666124dcb0dfbb14485b0d50aaaa0a8b83 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 12 Nov 2019 17:22:19 +0000 Subject: [PATCH 17/58] Bugfix in RetargetingClient --- src/RetargetingHelper/src/Helper.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index 8af85695..2be7389e 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -136,6 +136,8 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, if(m_useCoMHeightRetargeting) { + m_comHeightYarp.resize(1); + if(!YarpUtilities::getStringFromSearchable(config, "com_height_retargeting_port_name", portName)) { yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; From 89faa2a2ad29da872b537aa2ef1dde21b13ffe46 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 Nov 2019 17:31:10 +0100 Subject: [PATCH 18/58] change the PID sign according to robotology/robots-configuration@cae6f4407a4e54f3d564ec6a9725e4540d04683a --- .../dcm_walking/common/pidParams.ini | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/pidParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/pidParams.ini index bdc5b912..17036470 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/pidParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/pidParams.ini @@ -16,27 +16,27 @@ smoothingTime 0.05 [DEFAULT] #NAME P I D -torso_yaw -6000.0 -7111.0 0.0 -torso_roll -9000.0 -10666.0 0.0 -torso_pitch -9000.0 -14222.0 0.0 +torso_yaw 6000.0 7111.0 0.0 +torso_roll 9000.0 10666.0 0.0 +torso_pitch 9000.0 14222.0 0.0 l_shoulder_pitch 2000.0 7111.0 0.0 #l_shoulder_roll 2166.0 10666.0 0.0 l_shoulder_yaw 1711.1 7111.0 0.0 -r_shoulder_pitch -2000.0 -7111.0 0.0 -#r_shoulder_roll -2066.0 -10666.0 0.0 -r_shoulder_yaw -1711.1 -7111.0 0.0 -l_hip_pitch -4000.0 -7000.0 -100.0 +r_shoulder_pitch 2000.0 7111.0 0.0 +#r_shoulder_roll 2066.0 10666.0 0.0 +r_shoulder_yaw 1711.1 7111.0 0.0 +l_hip_pitch 4000.0 7000.0 100.0 l_hip_roll 4500.0 8000.0 0.0 -l_hip_yaw -3011.0 -100.0 0.0 -l_knee -9000.0 -800.0 0.0 +l_hip_yaw 3011.0 100.0 0.0 +l_knee 9000.0 800.0 0.0 l_ankle_pitch 4000.0 200.0 0.0 l_ankle_roll 6000.0 200.0 0.0 r_hip_pitch 4000.0 7000.0 100.0 -r_hip_roll -4500.0 -8000.0 0.0 +r_hip_roll 4500.0 8000.0 0.0 r_hip_yaw 3011.0 100.0 0.0 r_knee 9000.0 800.0 0.0 -r_ankle_pitch -4000.0 -200.0 0.0 -r_ankle_roll -6000.0 -200.0 0.0 +r_ankle_pitch 4000.0 200.0 0.0 +r_ankle_roll 6000.0 200.0 0.0 [PREIMPACT_LEFT] activationPhase SWING_LEFT @@ -49,8 +49,8 @@ l_ankle_roll 2000.0 100.0 0.0 activationPhase SWING_RIGHT activationOffset 0.3 #NAME P I D -r_ankle_pitch -1000.0 -100.0 0.0 -r_ankle_roll -2000.0 -100.0 0.0 +r_ankle_pitch 1000.0 100.0 0.0 +r_ankle_roll 2000.0 100.0 0.0 # The next reset the original gains. [AFTER_IMPACT] From d957a2f538b3df8f9f7af3bed4be256bf8df112e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 Nov 2019 18:50:15 +0100 Subject: [PATCH 19/58] dump joints data --- src/WalkingModule/src/Module.cpp | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index f56ee713..3bf3a936 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -811,13 +811,14 @@ bool WalkingModule::updateModule() auto rightFoot = m_FKSolver->getRightFootToWorldTransform(); m_walkingLogger->sendData(m_FKSolver->getDCM(), m_DCMPositionDesired.front(), m_DCMVelocityDesired.front(), measuredZMP, desiredZMP, m_FKSolver->getCoMPosition(), - m_stableDCMModel->getCoMPosition(), - m_stableDCMModel->getCoMVelocity(), + m_stableDCMModel->getCoMPosition(), yarp::sig::Vector(1, m_retargetingClient->comHeight()), + m_stableDCMModel->getCoMVelocity(), yarp::sig::Vector(1, m_retargetingClient->comHeightVelocity()), leftFoot.getPosition(), leftFoot.getRotation().asRPY(), rightFoot.getPosition(), rightFoot.getRotation().asRPY(), m_leftTrajectory.front().getPosition(), m_leftTrajectory.front().getRotation().asRPY(), m_rightTrajectory.front().getPosition(), m_rightTrajectory.front().getRotation().asRPY(), - errorL, errorR); + m_robotControlHelper->getJointPosition(), + m_retargetingClient->jointValues()); } propagateTime(); @@ -1180,8 +1181,8 @@ bool WalkingModule::startWalking() "zmp_x", "zmp_y", "zmp_des_x", "zmp_des_y", "com_x", "com_y", "com_z", - "com_des_x", "com_des_y", - "com_des_dx", "com_des_dy", + "com_des_x", "com_des_y", "com_des_z", + "com_des_dx", "com_des_dy", "com_des_dz", "lf_x", "lf_y", "lf_z", "lf_roll", "lf_pitch", "lf_yaw", "rf_x", "rf_y", "rf_z", @@ -1190,10 +1191,18 @@ bool WalkingModule::startWalking() "lf_des_roll", "lf_des_pitch", "lf_des_yaw", "rf_des_x", "rf_des_y", "rf_des_z", "rf_des_roll", "rf_des_pitch", "rf_des_yaw", - "lf_err_x", "lf_err_y", "lf_err_z", - "lf_err_roll", "lf_err_pitch", "lf_err_yaw", - "rf_err_x", "rf_err_y", "rf_err_z", - "rf_err_roll", "rf_err_pitch", "rf_err_yaw"}); + "neck_pitch", "neck_roll", "neck_yaw", + "torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", + "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", + "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll", + "neck_pitch_des", "neck_roll_des", "neck_yaw_des", + "torso_pitch_des", "torso_roll_des", "torso_yaw_des", + "l_shoulder_pitch_des", "l_shoulder_roll_des", "l_shoulder_yaw_des", "l_elbow_des", "l_wrist_prosup_des", "l_wrist_pitch_des", "l_wrist_yaw_des", + "r_shoulder_pitch_des", "r_shoulder_roll_des", "r_shoulder_yaw_des", "r_elbow_des", "r_wrist_prosup_des", "r_wrist_pitch_des", "r_wrist_yaw_des", + "l_hip_pitch_des", "l_hip_roll_des", "l_hip_yaw_des", "l_knee_des", "l_ankle_pitch_des", "l_ankle_roll_des", + "r_hip_pitch_des", "r_hip_roll_des", "r_hip_yaw_des", "r_knee_des", "r_ankle_pitch_des", "r_ankle_roll_des"}); } // if the robot was only prepared the filters has to be reseted From 7f29ec4c9ab9c6fa7dbfdbb3afadb0498c2ea07e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 14 Nov 2019 12:45:28 +0100 Subject: [PATCH 20/58] -add minimum jerk trajectory for the joint retargeting -handle the parameters in the retargeting helper using the groups --- .../RetargetingHelper/Helper.h | 3 ++ src/RetargetingHelper/src/Helper.cpp | 49 ++++++++++++++----- 2 files changed, 40 insertions(+), 12 deletions(-) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index e5375679..270ce1af 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -66,6 +66,9 @@ namespace WalkingControllers std::vector m_retargetJointsIndex; /**< Vector containing the indices of the retarget joints. */ yarp::os::BufferedPort m_jointRetargetingPort; /**< joint retargeting port. */ iDynTree::VectorDynSize m_retargetJoints; /**< Values of the retarget Joints. */ + yarp::sig::Vector m_retargetJointsYARP; /**< Values of the retarget Joints (YARP). */ + std::unique_ptr m_jointRetargetingSmoother; /**< Minimum jerk trajectory + for the joint retargeting. */ double m_comHeightInputZero; double m_comHeight; diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index 2be7389e..262996fa 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -51,14 +51,17 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, } m_retargetJoints.resize(controlledJointsName.size()); + m_retargetJointsYARP.resize(controlledJointsName.size()); std::string portName; if(m_useHandRetargeting) { + const yarp::os::Bottle& option = config.findGroup("HAND_RETARGETING"); + // open left hand port - if(!YarpUtilities::getStringFromSearchable(config, "left_hand_transform_port_name", - portName)) + if(!YarpUtilities::getStringFromSearchable(option, "left_hand_transform_port_name", + portName)) { yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; return false; @@ -66,7 +69,7 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, m_leftHandTransformPort.open("/" + name + portName); // open right hand port - if(!YarpUtilities::getStringFromSearchable(config, "right_hand_transform_port_name", + if(!YarpUtilities::getStringFromSearchable(option, "right_hand_transform_port_name", portName)) { yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; @@ -78,7 +81,7 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, m_rightHandTransformYarp.resize(6); double smoothingTime; - if(!YarpUtilities::getNumberFromSearchable(config, "smoothing_time", smoothingTime)) + if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time", smoothingTime)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; @@ -90,10 +93,11 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, if(m_useJointRetargeting) { - std::vector retargetJointNames; + const yarp::os::Bottle& option = config.findGroup("JOINT_RETARGETING"); + std::vector retargetJointNames; yarp::os::Value *retargetJointNamesYarp; - if(!config.check("retargeting_joint_list", retargetJointNamesYarp)) + if(!option.check("retargeting_joint_list", retargetJointNamesYarp)) { yError() << "[RetargetingClient::initialize] Unable to find joints_list into config file."; return false; @@ -114,19 +118,31 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, m_retargetJointsIndex.push_back(index); } - if(!YarpUtilities::getStringFromSearchable(config, "joint_retargeting_port_name", + if(!YarpUtilities::getStringFromSearchable(option, "joint_retargeting_port_name", portName)) { yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; return false; } m_jointRetargetingPort.open("/" + name + portName); + + double smoothingTime; + if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time", smoothingTime)) + { + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + return false; + } + + m_jointRetargetingSmoother = std::make_unique(controlledJointsName.size(), period, smoothingTime); + } if(m_useVirtualizer) { - if(!YarpUtilities::getStringFromSearchable(config, "robot_orientation_port_name", - portName)) + const yarp::os::Bottle& option = config.findGroup("VIRTUALIZER"); + + if(!YarpUtilities::getStringFromSearchable(option, "robot_orientation_port_name", + portName)) { yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; return false; @@ -136,9 +152,11 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, if(m_useCoMHeightRetargeting) { + const yarp::os::Bottle& option = config.findGroup("COM_RETARGETING"); + m_comHeightYarp.resize(1); - if(!YarpUtilities::getStringFromSearchable(config, "com_height_retargeting_port_name", portName)) + if(!YarpUtilities::getStringFromSearchable(option, "com_height_retargeting_port_name", portName)) { yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; return false; @@ -146,7 +164,7 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, m_comHeightPort.open("/" + name + portName); double smoothingTime; - if(!YarpUtilities::getNumberFromSearchable(config, "smoothing_time", smoothingTime)) + if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time", smoothingTime)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; @@ -184,7 +202,11 @@ bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, m_rightHandSmoother->init(m_rightHandTransformYarp); } + // joint retargeting m_retargetJoints = jointValues; + iDynTree::toEigen(m_retargetJointsYARP) = iDynTree::toEigen(jointValues); + if (m_useJointRetargeting) + m_jointRetargetingSmoother->init(m_retargetJointsYARP); m_comHeight = comHeight; m_comHeightVelocity = 0; @@ -249,8 +271,11 @@ void RetargetingClient::getFeedback() if(desiredJoint != nullptr) { for(int i =0; i < desiredJoint->size(); i++) - m_retargetJoints(m_retargetJointsIndex[i]) = (*desiredJoint)(i); + m_retargetJointsYARP(m_retargetJointsIndex[i]) = (*desiredJoint)(i); } + + m_jointRetargetingSmoother->computeNextValues(m_retargetJointsYARP); + iDynTree::toEigen(m_retargetJoints) = iDynTree::toEigen(m_retargetJointsYARP); } From 2a170cb03edfd8db29f2186f756ab410525ba2ac Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 14 Nov 2019 14:11:54 +0100 Subject: [PATCH 21/58] update the retargeting configuration files for iCubGazeboV2_5 --- .../hand_retargeting/handRetargeting.ini | 2 ++ .../joint_retargeting/jointRetargeting.ini | 16 ++++++++++++++-- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/handRetargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/handRetargeting.ini index 8e2d4c0b..1782591a 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/handRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/handRetargeting.ini @@ -1,6 +1,8 @@ +[HAND_RETARGETING] left_hand_transform_port_name /leftHandDesiredPose:i right_hand_transform_port_name /rightHandDesiredPose:i smoothing_time 0.1 +[VIRTUALIZER] robot_orientation_port_name /torsoYaw:o diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini index c434f32b..680c2352 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini @@ -1,8 +1,20 @@ +[HAND_RETARGETING] + + +[JOINT_RETARGETING] retargeting_joint_list ("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", - "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", - "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw") + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", + "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", + "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") joint_retargeting_port_name /jointPosition:i +smoothing_time 1.5 +[VIRTUALIZER] robot_orientation_port_name /torsoYaw:o + +[COM_RETARGETING] +com_height_retargeting_port_name /CoM:i +smoothing_time 0.5 From 6f7ad07669e31bc9a918200100d17708cb1b66d9 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 14 Nov 2019 14:12:44 +0100 Subject: [PATCH 22/58] update the retargeting configuration files for iCubGenova04 --- .../hand_retargeting/handRetargeting.ini | 5 ++++- .../joint_retargeting/jointRetargeting.ini | 15 ++++++++++++++- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/handRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/handRetargeting.ini index fefb8e95..f7a13fde 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/handRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/handRetargeting.ini @@ -1,6 +1,9 @@ +[HAND_RETARGETING] + left_hand_transform_port_name /leftHandDesiredPose:i right_hand_transform_port_name /rightHandDesiredPose:i smoothing_time 0.1 -robot_orientation_port_name /torsoYaw:o \ No newline at end of file +[VIRTUALIZER] +robot_orientation_port_name /torsoYaw:o diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini index c434f32b..a351817c 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini @@ -1,8 +1,21 @@ +[HAND_RETARGETING] + + +[JOINT_RETARGETING] retargeting_joint_list ("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", - "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw") + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", + "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", + "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") joint_retargeting_port_name /jointPosition:i +smoothing_time 0.5 + +[VIRTUALIZER] robot_orientation_port_name /torsoYaw:o + +[COM_RETARGETING] +com_height_retargeting_port_name /CoM:i +smoothing_time 1.5 From 51bf4831bc95a4ba060d24946da3867e1b77811f Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 15 Nov 2019 11:51:50 +0100 Subject: [PATCH 23/58] reorganize the RetargetingHelper --- .../RetargetingHelper/Helper.h | 48 ++++----- src/RetargetingHelper/src/Helper.cpp | 101 +++++++++--------- 2 files changed, 74 insertions(+), 75 deletions(-) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index 270ce1af..5a962c41 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -36,44 +36,38 @@ namespace WalkingControllers { private: + typedef struct + { + yarp::sig::Vector yarpVector; + std::unique_ptr smoother; + yarp::os::BufferedPort port; + double smoothingTimeInApproaching; + double smoothingTimeInWalking; + + } retargetingElement; + bool m_useHandRetargeting; /**< True if the hand retargeting is used */ bool m_useVirtualizer; /**< True if the virtualizer is used */ bool m_useJointRetargeting; /**< True if the joint retargeting is used */ bool m_useCoMHeightRetargeting; /**< True if the com retargeting is used */ - iDynTree::Transform m_leftHandTransform; /**< Left hand transform head_T_leftHand */ - iDynTree::Transform m_rightHandTransform; /**< Right hand transform head_T_rightHand*/ - - yarp::sig::Vector m_leftHandTransformYarp; /**< Left hand position + rpy head_T_leftHand */ - yarp::sig::Vector m_rightHandTransformYarp; /**< Right hand position + rpy head_T_rightHand*/ - yarp::sig::Vector m_comHeightYarp; /**< Left hand position + rpy head_T_leftHand */ - - yarp::os::BufferedPort m_leftHandTransformPort; /**< Right hand position + rpy head_T_rightHand*/ - yarp::os::BufferedPort m_rightHandTransformPort; /**< Right hand position + rpy head_T_rightHand*/ - yarp::os::BufferedPort m_comHeightPort; /**< CoMHeight */ + iDynTree::Transform m_leftHandTransform; + retargetingElement m_leftHand; - std::unique_ptr m_leftHandSmoother; /**< Minimum jerk trajectory - for the left hand. */ + iDynTree::Transform m_rightHandTransform; + retargetingElement m_rightHand; - std::unique_ptr m_rightHandSmoother; /**< Minimum jerk trajectory - for the right hand. */ - - std::unique_ptr m_comHeightSmoother; /**< CoM Height smooter. */ - - - yarp::os::BufferedPort m_robotOrientationPort; /**< Average orientation of the robot.*/ + double m_comHeightValue; + double m_comHeightInputZero; + double m_comHeightVelocity; + double m_comConstantHeight; + retargetingElement m_comHeight; std::vector m_retargetJointsIndex; /**< Vector containing the indices of the retarget joints. */ - yarp::os::BufferedPort m_jointRetargetingPort; /**< joint retargeting port. */ iDynTree::VectorDynSize m_retargetJoints; /**< Values of the retarget Joints. */ - yarp::sig::Vector m_retargetJointsYARP; /**< Values of the retarget Joints (YARP). */ - std::unique_ptr m_jointRetargetingSmoother; /**< Minimum jerk trajectory - for the joint retargeting. */ + retargetingElement m_jointRetargeting; - double m_comHeightInputZero; - double m_comHeight; - double m_comHeightVelocity; - double m_comConstantHeight; + yarp::os::BufferedPort m_robotOrientationPort; /**< Average orientation of the robot.*/ bool m_isStancePhase{true}; diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index 262996fa..0ed65b68 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -51,7 +51,7 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, } m_retargetJoints.resize(controlledJointsName.size()); - m_retargetJointsYARP.resize(controlledJointsName.size()); + m_jointRetargeting.yarpVector.resize(controlledJointsName.size()); std::string portName; @@ -66,19 +66,19 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; return false; } - m_leftHandTransformPort.open("/" + name + portName); + m_leftHand.port.open("/" + name + portName); // open right hand port if(!YarpUtilities::getStringFromSearchable(option, "right_hand_transform_port_name", - portName)) + portName)) { yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; return false; } - m_rightHandTransformPort.open("/" + name + portName); + m_rightHand.port.open("/" + name + portName); - m_leftHandTransformYarp.resize(6); - m_rightHandTransformYarp.resize(6); + m_leftHand.yarpVector.resize(6); + m_rightHand.yarpVector.resize(6); double smoothingTime; if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time", smoothingTime)) @@ -87,8 +87,8 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, return false; } - m_leftHandSmoother = std::make_unique(6, period, smoothingTime); - m_rightHandSmoother = std::make_unique(6, period, smoothingTime); + m_leftHand.smoother = std::make_unique(6, period, smoothingTime); + m_rightHand.smoother = std::make_unique(6, period, smoothingTime); } if(m_useJointRetargeting) @@ -114,7 +114,6 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, int index = std::distance(controlledJointsName.begin(), std::find(controlledJointsName.begin(), controlledJointsName.end(), joint)); - m_retargetJointsIndex.push_back(index); } @@ -124,7 +123,7 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; return false; } - m_jointRetargetingPort.open("/" + name + portName); + m_jointRetargeting.port.open("/" + name + portName); double smoothingTime; if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time", smoothingTime)) @@ -133,7 +132,7 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, return false; } - m_jointRetargetingSmoother = std::make_unique(controlledJointsName.size(), period, smoothingTime); + m_jointRetargeting.smoother = std::make_unique(controlledJointsName.size(), period, smoothingTime); } @@ -154,14 +153,14 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, { const yarp::os::Bottle& option = config.findGroup("COM_RETARGETING"); - m_comHeightYarp.resize(1); + m_comHeight.yarpVector.resize(1); if(!YarpUtilities::getStringFromSearchable(option, "com_height_retargeting_port_name", portName)) { yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; return false; } - m_comHeightPort.open("/" + name + portName); + m_comHeight.port.open("/" + name + portName); double smoothingTime; if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time", smoothingTime)) @@ -170,7 +169,7 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, return false; } - m_comHeightSmoother = std::make_unique(1, period, smoothingTime); + m_comHeight.smoother = std::make_unique(1, period, smoothingTime); } return true; @@ -186,36 +185,36 @@ bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, if(m_useHandRetargeting) { - iDynTree::toEigen(m_leftHandTransformYarp).segment<3>(0) = + iDynTree::toEigen(m_leftHand.yarpVector).segment<3>(0) = iDynTree::toEigen(m_leftHandTransform.getPosition()); - iDynTree::toEigen(m_leftHandTransformYarp).segment<3>(3) = + iDynTree::toEigen(m_leftHand.yarpVector).segment<3>(3) = iDynTree::toEigen(m_leftHandTransform.getRotation().asRPY()); - iDynTree::toEigen(m_rightHandTransformYarp).segment<3>(0) = + iDynTree::toEigen(m_rightHand.yarpVector).segment<3>(0) = iDynTree::toEigen(m_rightHandTransform.getPosition()); - iDynTree::toEigen(m_rightHandTransformYarp).segment<3>(3) = + iDynTree::toEigen(m_rightHand.yarpVector).segment<3>(3) = iDynTree::toEigen(m_rightHandTransform.getRotation().asRPY()); - m_leftHandSmoother->init(m_leftHandTransformYarp); - m_rightHandSmoother->init(m_rightHandTransformYarp); + m_leftHand.smoother->init(m_leftHand.yarpVector); + m_rightHand.smoother->init(m_rightHand.yarpVector); } // joint retargeting m_retargetJoints = jointValues; - iDynTree::toEigen(m_retargetJointsYARP) = iDynTree::toEigen(jointValues); + iDynTree::toEigen(m_jointRetargeting.yarpVector) = iDynTree::toEigen(jointValues); if (m_useJointRetargeting) - m_jointRetargetingSmoother->init(m_retargetJointsYARP); + m_jointRetargeting.smoother->init(m_jointRetargeting.yarpVector); - m_comHeight = comHeight; + m_comHeightValue = comHeight; m_comHeightVelocity = 0; m_comConstantHeight = comHeight; if(m_useCoMHeightRetargeting) { - m_comHeightYarp(0) = comHeight; - m_comHeightSmoother->init(m_comHeightYarp); + m_comHeight.yarpVector(0) = comHeight; + m_comHeight.smoother->init(m_comHeight.yarpVector); // let's read the port to reset the comHeightInput bool okCoMHeight = false; @@ -224,7 +223,7 @@ bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, { if(!okCoMHeight) { - auto desiredCoMHeight = m_comHeightPort.read(false); + auto desiredCoMHeight = m_comHeight.port.read(false); if(desiredCoMHeight != nullptr) { m_comHeightInputZero = (*desiredCoMHeight)(2); @@ -250,50 +249,50 @@ void RetargetingClient::getFeedback() { if(m_useHandRetargeting) { - auto desiredLeftHandPose = m_leftHandTransformPort.read(false); + auto desiredLeftHandPose = m_leftHand.port.read(false); if(desiredLeftHandPose != nullptr) - m_leftHandTransformYarp = *desiredLeftHandPose; + m_leftHand.yarpVector = *desiredLeftHandPose; - m_leftHandSmoother->computeNextValues(m_leftHandTransformYarp); - convertYarpVectorPoseIntoTransform(m_leftHandSmoother->getPos(), m_leftHandTransform); + m_leftHand.smoother->computeNextValues(m_leftHand.yarpVector); + convertYarpVectorPoseIntoTransform(m_leftHand.smoother->getPos(), m_leftHandTransform); - auto desiredRightHandPose = m_rightHandTransformPort.read(false); + auto desiredRightHandPose = m_rightHand.port.read(false); if(desiredRightHandPose != nullptr) - m_rightHandTransformYarp = *desiredRightHandPose; + m_rightHand.yarpVector = *desiredRightHandPose; - m_rightHandSmoother->computeNextValues(m_rightHandTransformYarp); - convertYarpVectorPoseIntoTransform(m_rightHandSmoother->getPos(), m_rightHandTransform); + m_rightHand.smoother->computeNextValues(m_rightHand.yarpVector); + convertYarpVectorPoseIntoTransform(m_rightHand.smoother->getPos(), m_rightHandTransform); } if(m_useJointRetargeting) { - auto desiredJoint = m_jointRetargetingPort.read(false); + auto desiredJoint = m_jointRetargeting.port.read(false); if(desiredJoint != nullptr) { for(int i =0; i < desiredJoint->size(); i++) - m_retargetJointsYARP(m_retargetJointsIndex[i]) = (*desiredJoint)(i); + m_jointRetargeting.yarpVector(m_retargetJointsIndex[i]) = (*desiredJoint)(i); } - m_jointRetargetingSmoother->computeNextValues(m_retargetJointsYARP); - iDynTree::toEigen(m_retargetJoints) = iDynTree::toEigen(m_retargetJointsYARP); + m_jointRetargeting.smoother->computeNextValues(m_jointRetargeting.yarpVector); + iDynTree::toEigen(m_retargetJoints) = iDynTree::toEigen(m_jointRetargeting.smoother->getPos()); } if(m_useCoMHeightRetargeting) { if(!m_isStancePhase) - m_comHeightYarp(0) = m_comConstantHeight; + m_comHeight.yarpVector(0) = m_comConstantHeight; else { - auto desiredCoMHeight = m_comHeightPort.read(false); + auto desiredCoMHeight = m_comHeight.port.read(false); if(desiredCoMHeight != nullptr) - m_comHeightYarp(0) = (*desiredCoMHeight)(2) - m_comHeightInputZero + m_comHeight.yarpVector(0) = (*desiredCoMHeight)(2) - m_comHeightInputZero + m_comConstantHeight; } - m_comHeightSmoother->computeNextValues(m_comHeightYarp); - m_comHeight = m_comHeightSmoother->getPos()(0); - m_comHeightVelocity = m_comHeightSmoother->getVel()(0); + m_comHeight.smoother->computeNextValues(m_comHeight.yarpVector); + m_comHeightValue = m_comHeight.smoother->getPos()(0); + m_comHeightVelocity = m_comHeight.smoother->getVel()(0); } return; @@ -316,7 +315,7 @@ const iDynTree::VectorDynSize& RetargetingClient::jointValues() const double RetargetingClient::comHeight() const { - return m_comHeight; + return m_comHeightValue; } double RetargetingClient::comHeightVelocity() const @@ -328,12 +327,18 @@ void RetargetingClient::close() { if(m_useHandRetargeting) { - m_leftHandTransformPort.close(); - m_rightHandTransformPort.close(); + m_leftHand.port.close(); + m_rightHand.port.close(); } if(m_useJointRetargeting) - m_jointRetargetingPort.close(); + m_jointRetargeting.port.close(); + + if(m_useCoMHeightRetargeting) + m_comHeight.port.close(); + + if(m_useVirtualizer) + m_robotOrientationPort.close(); } void RetargetingClient::setRobotBaseOrientation(const iDynTree::Rotation& rotation) From 2fe2d70c12ded0b8849526a97de78b47570c6f23 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 15 Nov 2019 14:14:00 +0100 Subject: [PATCH 24/58] - implement the approaching strategy on the RetargetingHelper - update the configuration files for iCubGenova04 and iCubGazeboV2_5 --- .../RetargetingHelper/Helper.h | 44 ++++-- src/RetargetingHelper/src/Helper.cpp | 133 +++++++++++++++--- .../hand_retargeting/handRetargeting.ini | 6 +- .../joint_retargeting/jointRetargeting.ini | 8 +- .../hand_retargeting/handRetargeting.ini | 5 +- .../joint_retargeting/jointRetargeting.ini | 9 +- src/WalkingModule/src/Module.cpp | 17 ++- 7 files changed, 189 insertions(+), 33 deletions(-) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index 5a962c41..1dfcd3b0 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -43,7 +43,6 @@ namespace WalkingControllers yarp::os::BufferedPort port; double smoothingTimeInApproaching; double smoothingTimeInWalking; - } retargetingElement; bool m_useHandRetargeting; /**< True if the hand retargeting is used */ @@ -51,11 +50,11 @@ namespace WalkingControllers bool m_useJointRetargeting; /**< True if the joint retargeting is used */ bool m_useCoMHeightRetargeting; /**< True if the com retargeting is used */ - iDynTree::Transform m_leftHandTransform; - retargetingElement m_leftHand; + iDynTree::Transform m_leftHandTransform; /**< Desired left hand transform */ + retargetingElement m_leftHand; /**< Left hand retargeting element */ - iDynTree::Transform m_rightHandTransform; - retargetingElement m_rightHand; + iDynTree::Transform m_rightHandTransform; /**< Desired right hand transform */ + retargetingElement m_rightHand; /**< Right hand retargeting element */ double m_comHeightValue; double m_comHeightInputZero; @@ -64,12 +63,16 @@ namespace WalkingControllers retargetingElement m_comHeight; std::vector m_retargetJointsIndex; /**< Vector containing the indices of the retarget joints. */ - iDynTree::VectorDynSize m_retargetJoints; /**< Values of the retarget Joints. */ - retargetingElement m_jointRetargeting; + iDynTree::VectorDynSize m_jointRetargetingValue; /**< Values of the retarget Joints. */ + retargetingElement m_jointRetargeting; /**< Joint retargeting element */ yarp::os::BufferedPort m_robotOrientationPort; /**< Average orientation of the robot.*/ - bool m_isStancePhase{true}; + bool m_isStancePhase{true}; /**< True if the robot is not walking */ + + bool m_isApproachingPhase; /**< True if the robot is in the approaching phase */ + double m_startingApproachingPhaseTime; /**< Initial time of the approaching phase (seconds) */ + double m_approachPhaseDuration; /**< Duration of the approaching phase (seconds) */ /** * Convert a yarp vector containing position + rpy into an iDynTree homogeneous transform @@ -79,6 +82,11 @@ namespace WalkingControllers void convertYarpVectorPoseIntoTransform(const yarp::sig::Vector& vector, iDynTree::Transform& transform); + /** + * Terminate the approaching phase + */ + void stopApproachingPhase(); + public: /** @@ -132,8 +140,16 @@ namespace WalkingControllers */ const iDynTree::VectorDynSize& jointValues() const; + /** + * Get the CoM height + * @return height of the CoM + */ double comHeight() const; + /** + * Get the CoM height velocity + * @return height velocity of the CoM + */ double comHeightVelocity() const; /** @@ -141,7 +157,19 @@ namespace WalkingControllers */ void setRobotBaseOrientation(const iDynTree::Rotation& rotation); + void setPhase(bool isStancePhase); + + /** + * Start the approaching phase + */ + void startApproachingPhase(); + + /** + * Check if the approaching phase is running + * @return true if the approaching phase is running + */ + bool isApproachingPhase() const; }; }; #endif diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index 0ed65b68..3f9047cc 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -44,17 +44,32 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, m_useVirtualizer = config.check("use_virtualizer", yarp::os::Value(false)).asBool(); m_useCoMHeightRetargeting = config.check("use_com_retargeting", yarp::os::Value(false)).asBool(); + // The approaching phase is set only if the startApproacingPhase method is called + m_isApproachingPhase = false; + if(m_useJointRetargeting && m_useHandRetargeting) { yError() << "[RetargetingClient::initialize] You cannot enable the joint retargeting along with the hand retargeting."; return false; } - m_retargetJoints.resize(controlledJointsName.size()); + m_jointRetargetingValue.resize(controlledJointsName.size()); m_jointRetargeting.yarpVector.resize(controlledJointsName.size()); - std::string portName; + if(!m_useHandRetargeting && !m_useVirtualizer && + !m_useJointRetargeting && !m_useCoMHeightRetargeting) + { + return true; + } + + // get the approaching phase duration + if(!YarpUtilities::getNumberFromSearchable(config, "approaching_phase_duration", m_approachPhaseDuration)) + { + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + return false; + } + std::string portName; if(m_useHandRetargeting) { const yarp::os::Bottle& option = config.findGroup("HAND_RETARGETING"); @@ -80,15 +95,27 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, m_leftHand.yarpVector.resize(6); m_rightHand.yarpVector.resize(6); - double smoothingTime; - if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time", smoothingTime)) + double smoothingTimeApproching; + if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_approaching", smoothingTimeApproching)) + { + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + return false; + } + m_leftHand.smoothingTimeInApproaching = smoothingTimeApproching; + m_rightHand.smoothingTimeInApproaching = smoothingTimeApproching; + + + double smoothingTimeWalking; + if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_walking", smoothingTimeWalking)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; } + m_leftHand.smoothingTimeInWalking = smoothingTimeWalking; + m_rightHand.smoothingTimeInWalking = smoothingTimeWalking; - m_leftHand.smoother = std::make_unique(6, period, smoothingTime); - m_rightHand.smoother = std::make_unique(6, period, smoothingTime); + m_leftHand.smoother = std::make_unique(6, period, m_leftHand.smoothingTimeInApproaching); + m_rightHand.smoother = std::make_unique(6, period, m_rightHand.smoothingTimeInApproaching); } if(m_useJointRetargeting) @@ -125,14 +152,21 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, } m_jointRetargeting.port.open("/" + name + portName); - double smoothingTime; - if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time", smoothingTime)) + if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_approaching", + m_jointRetargeting.smoothingTimeInApproaching)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; } - m_jointRetargeting.smoother = std::make_unique(controlledJointsName.size(), period, smoothingTime); + if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_walking", m_jointRetargeting.smoothingTimeInWalking)) + { + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + return false; + } + + m_jointRetargeting.smoother = std::make_unique(controlledJointsName.size(), period, + m_jointRetargeting.smoothingTimeInApproaching); } @@ -162,14 +196,19 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, } m_comHeight.port.open("/" + name + portName); - double smoothingTime; - if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time", smoothingTime)) + if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_approaching", m_comHeight.smoothingTimeInApproaching)) + { + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + return false; + } + + if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_walking", m_comHeight.smoothingTimeInWalking)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; } - m_comHeight.smoother = std::make_unique(1, period, smoothingTime); + m_comHeight.smoother = std::make_unique(1, period, m_comHeight.smoothingTimeInApproaching); } return true; @@ -202,7 +241,7 @@ bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, } // joint retargeting - m_retargetJoints = jointValues; + m_jointRetargetingValue = jointValues; iDynTree::toEigen(m_jointRetargeting.yarpVector) = iDynTree::toEigen(jointValues); if (m_useJointRetargeting) m_jointRetargeting.smoother->init(m_jointRetargeting.yarpVector); @@ -238,7 +277,8 @@ bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, attempt++; } while (attempt < 100); - yError() << "[RetargetingClient::reset] The retargeting port is not streaming data"; + if(!okCoMHeight) + yError() << "[RetargetingClient::reset] The CoM height is not coming from the yarp port."; return false; } @@ -274,7 +314,7 @@ void RetargetingClient::getFeedback() } m_jointRetargeting.smoother->computeNextValues(m_jointRetargeting.yarpVector); - iDynTree::toEigen(m_retargetJoints) = iDynTree::toEigen(m_jointRetargeting.smoother->getPos()); + iDynTree::toEigen(m_jointRetargetingValue) = iDynTree::toEigen(m_jointRetargeting.smoother->getPos()); } @@ -295,6 +335,14 @@ void RetargetingClient::getFeedback() m_comHeightVelocity = m_comHeight.smoother->getVel()(0); } + // check if the approaching phase is finished + if(m_isApproachingPhase) + { + double now = yarp::os::Time::now(); + if(now - m_startingApproachingPhaseTime > m_approachPhaseDuration) + stopApproachingPhase(); + } + return; } @@ -310,7 +358,7 @@ const iDynTree::Transform& RetargetingClient::rightHandTransform() const const iDynTree::VectorDynSize& RetargetingClient::jointValues() const { - return m_retargetJoints; + return m_jointRetargetingValue; } double RetargetingClient::comHeight() const @@ -356,3 +404,56 @@ void RetargetingClient::setPhase(bool isStancePhase) { m_isStancePhase = isStancePhase; } + +void RetargetingClient::stopApproachingPhase() +{ + if(m_useHandRetargeting) + { + m_leftHand.smoother->setT(m_leftHand.smoothingTimeInWalking); + m_rightHand.smoother->setT(m_rightHand.smoothingTimeInWalking); + } + + if(m_useJointRetargeting) + { + m_jointRetargeting.smoother->setT(m_jointRetargeting.smoothingTimeInWalking); + } + + if(m_useCoMHeightRetargeting) + { + m_comHeight.smoother->setT(m_comHeight.smoothingTimeInWalking); + } + + m_isApproachingPhase = false; +} + +void RetargetingClient::startApproachingPhase() +{ + // if the retargeting is not used the approaching phase is not required + if(!m_useHandRetargeting && !m_useJointRetargeting && !m_useCoMHeightRetargeting) + return; + + m_startingApproachingPhaseTime = yarp::os::Time::now(); + + if(m_useHandRetargeting) + { + m_leftHand.smoother->setT(m_leftHand.smoothingTimeInApproaching); + m_rightHand.smoother->setT(m_rightHand.smoothingTimeInApproaching); + } + + if(m_useJointRetargeting) + { + m_jointRetargeting.smoother->setT(m_jointRetargeting.smoothingTimeInApproaching); + } + + if(m_useCoMHeightRetargeting) + { + m_comHeight.smoother->setT(m_comHeight.smoothingTimeInApproaching); + } + + m_isApproachingPhase = true; +} + +bool RetargetingClient::isApproachingPhase() const +{ + return m_isApproachingPhase; +} diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/handRetargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/handRetargeting.ini index 1782591a..94ad288f 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/handRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/handRetargeting.ini @@ -1,8 +1,12 @@ +approaching_phase_duration 1.0 + + [HAND_RETARGETING] left_hand_transform_port_name /leftHandDesiredPose:i right_hand_transform_port_name /rightHandDesiredPose:i -smoothing_time 0.1 +smoothing_time_approaching 0.1 +smoothing_time_walking 0.1 [VIRTUALIZER] robot_orientation_port_name /torsoYaw:o diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini index 680c2352..87884bc0 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini @@ -1,3 +1,5 @@ +approaching_phase_duration 4.0 + [HAND_RETARGETING] @@ -10,11 +12,13 @@ retargeting_joint_list ("neck_pitch", "neck_roll", "neck_yaw", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") joint_retargeting_port_name /jointPosition:i -smoothing_time 1.5 +smoothing_time_approaching 2.0 +smoothing_time_walking 1.0 [VIRTUALIZER] robot_orientation_port_name /torsoYaw:o [COM_RETARGETING] com_height_retargeting_port_name /CoM:i -smoothing_time 0.5 +smoothing_time_approaching 2.0 +smoothing_time_walking 1.0 diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/handRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/handRetargeting.ini index f7a13fde..2020cb4e 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/handRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/handRetargeting.ini @@ -1,9 +1,12 @@ +approaching_phase_duration 1.0 + [HAND_RETARGETING] left_hand_transform_port_name /leftHandDesiredPose:i right_hand_transform_port_name /rightHandDesiredPose:i -smoothing_time 0.1 +smoothing_time_approaching 0.5 +smoothing_time_walking 0.1 [VIRTUALIZER] robot_orientation_port_name /torsoYaw:o diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini index a351817c..d84b002f 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini @@ -1,3 +1,5 @@ +approaching_phase_duration 4.0 + [HAND_RETARGETING] @@ -10,12 +12,15 @@ retargeting_joint_list ("neck_pitch", "neck_roll", "neck_yaw", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") joint_retargeting_port_name /jointPosition:i - -smoothing_time 0.5 +smoothing_time_approaching 2.0 +smoothing_time_walking 1.0 [VIRTUALIZER] robot_orientation_port_name /torsoYaw:o [COM_RETARGETING] com_height_retargeting_port_name /CoM:i +smoothing_time_approaching 2.0 +smoothing_time_walking 1.5 + smoothing_time 1.5 diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 3bf3a936..b64d3673 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -821,10 +821,14 @@ bool WalkingModule::updateModule() m_retargetingClient->jointValues()); } - propagateTime(); + // in the approaching phase the robot should not move and the trajectories should not advance + if(!m_retargetingClient->isApproachingPhase()) + { + propagateTime(); - // advance all the signals - advanceReferenceSignals(); + // advance all the signals + advanceReferenceSignals(); + } m_retargetingClient->setRobotBaseOrientation(yawRotation.inverse()); } @@ -1209,6 +1213,7 @@ bool WalkingModule::startWalking() if(m_robotState == WalkingFSM::Prepared) m_robotControlHelper->resetFilters(); + m_retargetingClient->startApproachingPhase(); m_robotState = WalkingFSM::Walking; return true; @@ -1216,6 +1221,12 @@ bool WalkingModule::startWalking() bool WalkingModule::setPlannerInput(double x, double y) { + // in the approaching phase the robot should not move + // as soon as the approaching phase is finished the user + // can move the robot + if(m_retargetingClient->isApproachingPhase()) + return true; + // the trajectory was already finished the new trajectory will be attached as soon as possible if(m_mergePoints.empty()) { From 169988c9d84b71e662a6f080dfa9442fab91a386 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 15 Nov 2019 14:14:32 +0100 Subject: [PATCH 25/58] enable CoM retargeting on iCubGazeboV2_5 --- .../app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini index 444a20f8..ad306305 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini @@ -22,6 +22,8 @@ sampling_time 0.01 use_joint_retargeting 1 # enable the virtualizer use_virtualizer 1 +# enable the com retargeting +use_com_retargeting 1 # include robot control parameters [include ROBOT_CONTROL "./dcm_walking/joint_retargeting/robotControl.ini"] From 8a0a7266d20b13eec245bf82cddd9c1a7ad0f79c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 15 Nov 2019 17:56:10 +0000 Subject: [PATCH 26/58] tune parameters on iCubGenova04 --- src/RetargetingHelper/src/Helper.cpp | 3 +- .../hand_retargeting/inverseKinematics.ini | 5 ++-- .../hand_retargeting/qpInverseKinematics.ini | 12 ++++---- .../hand_retargeting/robotControl.ini | 4 +-- .../joint_retargeting/inverseKinematics.ini | 4 +-- .../joint_retargeting/jointRetargeting.ini | 13 ++++----- .../joint_retargeting/plannerParams.ini | 13 ++++++--- .../joint_retargeting/qpInverseKinematics.ini | 28 +++++++++---------- .../joint_retargeting/robotControl.ini | 8 +++--- .../dcm_walking_joint_retargeting.ini | 5 ++-- src/WalkingModule/src/Module.cpp | 8 +++--- 11 files changed, 53 insertions(+), 50 deletions(-) diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index 3f9047cc..95229e13 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -326,7 +326,8 @@ void RetargetingClient::getFeedback() { auto desiredCoMHeight = m_comHeight.port.read(false); if(desiredCoMHeight != nullptr) - m_comHeight.yarpVector(0) = (*desiredCoMHeight)(2) - m_comHeightInputZero + + m_comHeight.yarpVector(0) = ((*desiredCoMHeight)(2) - m_comHeightInputZero) * 0.5 + m_comConstantHeight; } diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/inverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/inverseKinematics.ini index c8661a7a..6871ccf8 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/inverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/inverseKinematics.ini @@ -18,11 +18,10 @@ max-cpu-time 20 #DEGREES jointRegularization (15, 0, 0, - -7, 22, 11, 30, 0, 0, 0, - -7, 22, 11, 30, 0, 0, 0, + -7, 22, 11, 30, 0, + -7, 22, 11, 30, 0, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) #jointRegularization (0, 15, 0, 0, -29.794, 29.97, 0.00, 44.977, -29.794, 29.97, 0.00, 44.977, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) #jointRegularization (15, 0, 0, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) - diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/qpInverseKinematics.ini index 96128512..7f7d394c 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/qpInverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/qpInverseKinematics.ini @@ -7,20 +7,20 @@ neck_weight 5.0 additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) joint_regularization (15, 0, 0, - -7, 22, 11, 30, 0, 0, 0, - -7, 22, 11, 30, 0, 0, 0, + -7, 22, 11, 30, 0, + -7, 22, 11, 30, 0, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) joint_regularization_weights (1.0, 1.0, 1.0, - 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, - 0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25, + 0.15, 0.25, 0.25, 0.15, 0.25, + 0.15, 0.25, 0.25, 0.15, 0.25, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) joint_regularization_gains (5.0, 5.0, 5.0, - 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, - 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/robotControl.ini index 269bb19d..eb9cb5a5 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/robotControl.ini @@ -2,8 +2,8 @@ robot icub joints_list ("torso_pitch", "torso_roll", "torso_yaw", - "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", - "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/inverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/inverseKinematics.ini index 5ff6edbf..50bed910 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/inverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/inverseKinematics.ini @@ -19,8 +19,8 @@ max-cpu-time 20 #DEGREES jointRegularization (0, 0, 0, 15, 0, 0, - -7, 22, 11, 30, 0, 0, 0, - -7, 22, 11, 30, 0, 0, 0, + -7, 22, 11, 30, 0, + -7, 22, 11, 30, 0, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini index d84b002f..43a4d70f 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini @@ -6,14 +6,13 @@ approaching_phase_duration 4.0 [JOINT_RETARGETING] retargeting_joint_list ("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", - "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", - "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", - "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", - "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup") + joint_retargeting_port_name /jointPosition:i smoothing_time_approaching 2.0 -smoothing_time_walking 1.0 +smoothing_time_walking 0.2 [VIRTUALIZER] robot_orientation_port_name /torsoYaw:o @@ -21,6 +20,4 @@ robot_orientation_port_name /torsoYaw:o [COM_RETARGETING] com_height_retargeting_port_name /CoM:i smoothing_time_approaching 2.0 -smoothing_time_walking 1.5 - -smoothing_time 1.5 +smoothing_time_walking 0.2 diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini index 95afc70c..d0ba4205 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini @@ -19,8 +19,8 @@ minWidth 0.15 maxAngleVariation 20.0 minAngleVariation 8.0 #Timings -maxStepDuration 0.85 -minStepDuration 0.75 +maxStepDuration 0.9 +minStepDuration 0.8 ##Nominal Values #Width @@ -31,7 +31,7 @@ stepLandingVelocity 0.00 footApexTime 0.8 comHeightDelta 0.0 #Timings -nominalDuration 0.8 +nominalDuration 0.85 lastStepSwitchTime 0.8 switchOverSwingRatio 0.7 @@ -42,6 +42,12 @@ rightZMPDelta (0.03 0.005) #MergePoint mergePointRatio 0.4 +# Last Step DCM Offset +# If it is 0.5 the final DCM will be in the middle of the two footsteps; +# If it is 0 the DCM position coincides with the stance foot ZMP; +# If it is 1 the DCM position coincides with the next foot ZMP position. +lastStepDCMOffset 0.25 + # pitch delta # pitchDelta -3.0 #pitchDelta 0.0 @@ -52,4 +58,3 @@ startAlwaysSameFoot 1 ##Remove this line if you don't want to use the minimum jerk trajectory in feet interpolation #useMinimumJerkFootTrajectory 1 - diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini index 5c4b65af..8baac938 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini @@ -9,51 +9,51 @@ additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) ## Regularization joint_regularization (0,0,0, 15, 0, 0, - -7, 22, 11, 30, 0, 0, 0, - -7, 22, 11, 30, 0, 0, 0, + -7, 22, 11, 30, 0, + -7, 22, 11, 30, 0, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) joint_regularization_gains (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, - 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, - 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) joint_regularization_weight_stance (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) joint_regularization_weight_walking (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) ## Retargeting joint_retargeting_gains (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, - 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, - 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) joint_retargeting_weight_walking (1.0, 1.0, 1.0, 0.0, 0.0, 0.0 - 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, - 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) joint_retargeting_weight_stance (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, - 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, - 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini index 57f1277b..ef60dfa7 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini @@ -3,16 +3,16 @@ robot icub joints_list ("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", - "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", - "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") dangerous_joints (0, 0, 0, 1, 1, 1, - 1, 1, 1, 1, 0, 0, 0, - 1, 1, 1, 1, 0, 0, 0, + 1, 1, 1, 1, 0, + 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini index 752f347e..00a3f4db 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini @@ -1,6 +1,6 @@ # Remove this line if you don't want to use the MPC # use_mpc 1 - +1;5202;0c # Remove this line if you don't want to use the QP-IK use_QP-IK 1 @@ -9,7 +9,7 @@ use_QP-IK 1 # use_osqp 1 # remove this line if you don't want to save data of the experiment -# dump_data 1 +dump_data 1 # general parameters [GENERAL] @@ -22,6 +22,7 @@ sampling_time 0.01 use_joint_retargeting 1 # enable the virtualizer use_virtualizer 1 +use_com_retargeting 1 # include robot control parameters [include ROBOT_CONTROL "./dcm_walking/joint_retargeting/robotControl.ini"] diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index b64d3673..569da9bf 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -1197,14 +1197,14 @@ bool WalkingModule::startWalking() "rf_des_roll", "rf_des_pitch", "rf_des_yaw", "neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", - "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", - "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll", "neck_pitch_des", "neck_roll_des", "neck_yaw_des", "torso_pitch_des", "torso_roll_des", "torso_yaw_des", - "l_shoulder_pitch_des", "l_shoulder_roll_des", "l_shoulder_yaw_des", "l_elbow_des", "l_wrist_prosup_des", "l_wrist_pitch_des", "l_wrist_yaw_des", - "r_shoulder_pitch_des", "r_shoulder_roll_des", "r_shoulder_yaw_des", "r_elbow_des", "r_wrist_prosup_des", "r_wrist_pitch_des", "r_wrist_yaw_des", + "l_shoulder_pitch_des", "l_shoulder_roll_des", "l_shoulder_yaw_des", "l_elbow_des", "l_wrist_prosup_des", + "r_shoulder_pitch_des", "r_shoulder_roll_des", "r_shoulder_yaw_des", "r_elbow_des", "r_wrist_prosup_des", "l_hip_pitch_des", "l_hip_roll_des", "l_hip_yaw_des", "l_knee_des", "l_ankle_pitch_des", "l_ankle_roll_des", "r_hip_pitch_des", "r_hip_roll_des", "r_hip_yaw_des", "r_knee_des", "r_ankle_pitch_des", "r_ankle_roll_des"}); } From 150979bc4b84ab3fbbcad8351c034a1524be7a2f Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 18 Nov 2019 08:37:36 +0100 Subject: [PATCH 27/58] update stategy for switching from stance and walking phase --- .../WalkingControllers/WalkingModule/Module.h | 3 ++ src/WalkingModule/src/Module.cpp | 29 ++++++++++++++----- 2 files changed, 25 insertions(+), 7 deletions(-) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 496d216d..8fa6bda5 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -129,6 +129,9 @@ namespace WalkingControllers // debug std::unique_ptr m_velocityIntegral{nullptr}; + bool m_isStancePhase{true}; /**< if true the robot is not walking */ + bool m_isStancePhaseStarting{false}; + /** * Get the robot model from the resource finder and set it. * @param rf is the reference to a resource finder object. diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 569da9bf..9e79fc2b 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -378,9 +378,7 @@ bool WalkingModule::solveQPIK(const std::unique_ptr& solver, const iDynTree::VectorDynSize &output) { bool ok = true; - double threshold = 0.001; - bool stancePhase = iDynTree::toEigen(m_DCMVelocityDesired.front()).norm() < threshold; - solver->setPhase(stancePhase); + solver->setPhase(m_isStancePhase); ok &= solver->setRobotState(m_robotControlHelper->getJointPosition(), m_FKSolver->getLeftFootToWorldTransform(), @@ -588,6 +586,18 @@ bool WalkingModule::updateModule() } } + // check if the stance phase is started + double treshold = 0.01; + if(m_isStancePhaseStarting && + iDynTree::toEigen(m_DCMVelocityDesired.front()).norm() < treshold) + { + m_isStancePhase = true; + m_isStancePhaseStarting = false; + } + + // TODO remove me + yInfo() << "m_isStancePhase " << m_isStancePhase << " isStancePhaseStarting " << m_isStancePhaseStarting; + // get feedbacks and evaluate useful quantities if(!m_robotControlHelper->getFeedbacks(10)) { @@ -595,9 +605,7 @@ bool WalkingModule::updateModule() return false; } - double threshold = 0.001; - bool stancePhase = iDynTree::toEigen(m_DCMVelocityDesired.front()).norm() < threshold; - m_retargetingClient->setPhase(stancePhase); + m_retargetingClient->setPhase(m_isStancePhase); m_retargetingClient->getFeedback(); if(!updateFKSolver()) @@ -668,7 +676,7 @@ bool WalkingModule::updateModule() // inner COM-ZMP controller // if the the norm of desired DCM velocity is lower than a threshold then the robot // is stopped - m_walkingZMPController->setPhase(stancePhase); + m_walkingZMPController->setPhase(m_isStancePhase); iDynTree::Vector2 desiredZMP; if(m_useMPC) @@ -1269,6 +1277,13 @@ bool WalkingModule::setPlannerInput(double x, double y) m_newTrajectoryRequired = true; + double treshold = 0.01; + // the robot is about to move + if(iDynTree::toEigen(m_desiredPosition).norm() > treshold) + m_isStancePhase = false; + else + m_isStancePhaseStarting = true; + return true; } From 8ce32d6f4b1c2ca364aa5d509d90276d34455613 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 18 Nov 2019 11:23:43 +0100 Subject: [PATCH 28/58] Delay the transitio between the stance and walking phase --- .../dcm_walking_joint_retargeting.ini | 2 ++ .../dcm_walking_joint_retargeting.ini | 4 ++- .../WalkingControllers/WalkingModule/Module.h | 2 ++ src/WalkingModule/src/Module.cpp | 30 +++++++++++++++---- 4 files changed, 31 insertions(+), 7 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini index ad306305..4245b091 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini @@ -11,6 +11,8 @@ use_QP-IK 1 # remove this line if you don't want to save data of the experiment # dump_data 1 +stance_phase_time_out 1 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini index 00a3f4db..709f7dda 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini @@ -1,6 +1,6 @@ # Remove this line if you don't want to use the MPC # use_mpc 1 -1;5202;0c + # Remove this line if you don't want to use the QP-IK use_QP-IK 1 @@ -11,6 +11,8 @@ use_QP-IK 1 # remove this line if you don't want to save data of the experiment dump_data 1 +stance_phase_time_out 1 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 8fa6bda5..16ac38aa 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -131,6 +131,8 @@ namespace WalkingControllers bool m_isStancePhase{true}; /**< if true the robot is not walking */ bool m_isStancePhaseStarting{false}; + int m_stancePhaseCounter; + int m_stancePhaseMaxCounter; /** * Get the robot model from the resource finder and set it. diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 9e79fc2b..79bb091d 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -322,6 +322,14 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) m_qDesired.resize(m_robotControlHelper->getActuatedDoFs()); m_dqDesired.resize(m_robotControlHelper->getActuatedDoFs()); + // TODO please remove me. Counters are evil + double stancePhaseTimeOutSeconds = rf.check("stance_phase_time_out",yarp::os::Value(0.5)).asDouble(); + + m_stancePhaseMaxCounter = (int) std::round(stancePhaseTimeOutSeconds/m_dT); + + yInfo() << "m_stancePhaseMaxCounter " << m_stancePhaseMaxCounter + << " stancePhaseTimeOutSeconds " << stancePhaseTimeOutSeconds; + yInfo() << "[WalkingModule::configure] Ready to play!"; return true; @@ -591,12 +599,16 @@ bool WalkingModule::updateModule() if(m_isStancePhaseStarting && iDynTree::toEigen(m_DCMVelocityDesired.front()).norm() < treshold) { - m_isStancePhase = true; - m_isStancePhaseStarting = false; + m_stancePhaseCounter--; + if(m_stancePhaseCounter == 0) + { + m_isStancePhase = true; + m_isStancePhaseStarting = false; + } } // TODO remove me - yInfo() << "m_isStancePhase " << m_isStancePhase << " isStancePhaseStarting " << m_isStancePhaseStarting; + yInfo() << "m_isStancePhase " << m_isStancePhase << " isStancePhaseStarting " << m_isStancePhaseStarting << " m_stancePhaseCounter " << m_stancePhaseCounter; // get feedbacks and evaluate useful quantities if(!m_robotControlHelper->getFeedbacks(10)) @@ -832,10 +844,10 @@ bool WalkingModule::updateModule() // in the approaching phase the robot should not move and the trajectories should not advance if(!m_retargetingClient->isApproachingPhase()) { - propagateTime(); + propagateTime(); - // advance all the signals - advanceReferenceSignals(); + // advance all the signals + advanceReferenceSignals(); } m_retargetingClient->setRobotBaseOrientation(yawRotation.inverse()); @@ -1280,9 +1292,15 @@ bool WalkingModule::setPlannerInput(double x, double y) double treshold = 0.01; // the robot is about to move if(iDynTree::toEigen(m_desiredPosition).norm() > treshold) + { m_isStancePhase = false; + m_isStancePhaseStarting = false; + } else + { m_isStancePhaseStarting = true; + m_stancePhaseCounter = m_stancePhaseMaxCounter; + } return true; } From 766443b7e6e4fe52fcec7e206763f7d59b3a3d2c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 18 Nov 2019 12:40:45 +0100 Subject: [PATCH 29/58] bugfix in WalkingModule --- src/WalkingModule/src/Module.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 79bb091d..dbe280cb 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -1298,8 +1298,11 @@ bool WalkingModule::setPlannerInput(double x, double y) } else { + // if the stance phase has not started reset the counter + if(!m_isStancePhaseStarting) + m_stancePhaseCounter = m_stancePhaseMaxCounter; + m_isStancePhaseStarting = true; - m_stancePhaseCounter = m_stancePhaseMaxCounter; } return true; From 887f9145800b79a89a4c521a6c9e69a1765216b1 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 25 Nov 2019 13:05:32 +0000 Subject: [PATCH 30/58] tune parameters on iCubGenova04 --- .../dcm_walking/joint_retargeting/jointRetargeting.ini | 2 +- .../joint_retargeting/qpInverseKinematics.ini | 10 +++++----- .../joint_retargeting/zmpControllerParams.ini | 3 +-- .../iCubGenova04/dcm_walking_joint_retargeting.ini | 2 +- src/WalkingModule/src/Module.cpp | 2 +- 5 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini index 43a4d70f..b4129722 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini @@ -20,4 +20,4 @@ robot_orientation_port_name /torsoYaw:o [COM_RETARGETING] com_height_retargeting_port_name /CoM:i smoothing_time_approaching 2.0 -smoothing_time_walking 0.2 +smoothing_time_walking 1.0 \ No newline at end of file diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini index 8baac938..39469251 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini @@ -28,10 +28,10 @@ joint_regularization_weight_stance (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) -joint_regularization_weight_walking (1.0, 1.0, 1.0, +joint_regularization_weight_walking (0.0, 0.0, 0.0, 1.0, 1.0, 1.0, - 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, + 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) @@ -45,8 +45,8 @@ joint_retargeting_gains (5.0, 5.0, 5.0, joint_retargeting_weight_walking (1.0, 1.0, 1.0, 0.0, 0.0, 0.0 - 1.0, 1.0, 1.0, 1.0, 1.0, - 1.0, 1.0, 1.0, 1.0, 1.0, + 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/zmpControllerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/zmpControllerParams.ini index 5bdca487..9b512e52 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/zmpControllerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/zmpControllerParams.ini @@ -7,6 +7,5 @@ smoothingTime 0.1 kZMP_walking 3.0 kCoM_walking 10.0 -kZMP_stance 1.2 +kZMP_stance 1.0 kCoM_stance 6.0 - diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini index 709f7dda..7c3f2d62 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini @@ -9,7 +9,7 @@ use_QP-IK 1 # use_osqp 1 # remove this line if you don't want to save data of the experiment -dump_data 1 +#dump_data 1 stance_phase_time_out 1 diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index dbe280cb..d06b2f69 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -498,7 +498,7 @@ bool WalkingModule::updateModule() m_stableDCMModel->reset(m_DCMPositionDesired.front()); // reset the retargeting - if(!m_robotControlHelper->getFeedbacks(10)) + if(!m_robotControlHelper->getFeedbacks(100)) { yError() << "[WalkingModule::updateModule] Unable to get the feedback."; return false; From 9e90d2c3a3b46d8cdc8aef39963eb717e89902fb Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 9 Apr 2020 12:15:49 +0200 Subject: [PATCH 31/58] Fix indentation in QPInverseKinematics.h --- .../QPInverseKinematics.h | 636 +++++++++--------- 1 file changed, 318 insertions(+), 318 deletions(-) diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h index 2f19445e..5b0d4866 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h @@ -112,323 +112,323 @@ namespace WalkingControllers iDynSparseMatrix m_jointRegularizationGradient; /**< Contains a constant matrix that can be useful in the gradient evaluation ($-\lambda H'$). */ double m_kJointLimitsUpperBound; /**< Gain related to the the joint upper bound */ - double m_kJointLimitsLowerBound; /**< Gain related to the the joint lower bound */ - iDynTree::VectorDynSize m_jointVelocitiesBounds; /**< Bounds on the joint velocities*/ - iDynTree::VectorDynSize m_jointPositionsUpperBounds; /**< Upper Bounds on the joint position*/ - iDynTree::VectorDynSize m_jointPositionsLowerBounds; /**< Lower Bounds on the joint position*/ - - // gain scheduling - std::unique_ptr m_handWeightSmoother; /**< Minimum jerk trajectory - for the hand weight matrix. */ - yarp::sig::Vector m_handWeightWalkingVector; /**< Weight matrix (only the diagonal) used for - the hand retargeting during walking. */ - yarp::sig::Vector m_handWeightStanceVector; /**< Weight matrix (only the diagonal) used for - the hand retargeting during stance. */ - - // gain scheduling in case of joint retargeting - std::unique_ptr m_jointRetargetingWeightSmoother; /**< Minimum jerk trajectory - for the Joint retargeting weight matrix. */ - yarp::sig::Vector m_jointRetargetingWeightWalking; /**< Weight matrix (only the diagonal) used for - the joint retargeting during walking. */ - yarp::sig::Vector m_jointRetargetingWeightStance; /**< Weight matrix (only the diagonal) used for - the joint retargeting during stance. */ - - std::unique_ptr m_jointRegularizationWeightSmoother; /**< Minimum jerk trajectory - for the Joint regularization weight matrix. */ - yarp::sig::Vector m_jointRegularizationWeightWalking; /**< Weight matrix (only the diagonal) used for - the joint regularization during walking. */ - yarp::sig::Vector m_jointRegularizationWeightStance; /**< Weight matrix (only the diagonal) used for - the joint regularization during stance. */ - - std::unique_ptr m_torsoWeightSmoother; /**< Minimum jerk trajectory for the torso weight matrix. */ - double m_torsoWeightWalking; /**< Weight matrix (only the diagonal) used for the torso during walking. */ - double m_torsoWeightStance; /**< Weight matrix (only the diagonal) used for the torso during stance. */ - - bool m_useCoMAsConstraint; /**< True if the CoM is added as a constraint. */ - bool m_useJointsLimitsConstraint; /**< True if the CoM is added as a constraint. */ - bool m_enableHandRetargeting; /**< True if the hand retargeting is used */ - bool m_enableJointRetargeting; /**< True if the joint retargeting is used */ - - iDynTree::MatrixDynSize m_hessianDense; /**< Hessian matrix */ - iDynTree::VectorDynSize m_gradient; /**< Gradient vector */ - iDynSparseMatrix m_constraintsMatrixSparse; /**< Constraint matrix */ - iDynTree::VectorDynSize m_lowerBound; /**< Lower bound */ - iDynTree::VectorDynSize m_upperBound; /**< Upper bound */ - iDynTree::VectorDynSize m_solution; /**< Solution of the optimization problem */ - iDynTree::VectorDynSize m_desiredJointVelocitiesOutput; /**< Joint velocities required by the optimization problem */ - - /** - * Initialize all the constant matrix from the configuration file. - * @param config configuration parameters - * @return true/false in case of success/failure. - */ - bool initializeMatrices(const yarp::os::Searchable& config); - - /** - * Initialize hand retargeting. - * @param config configuration parameters - * @return true/false in case of success/failure. - */ - bool initializeHandRetargeting(const yarp::os::Searchable& config); - - /** - * Initialize joint retargeting. - * @param config configuration parameters - * @return true/false in case of success/failure. - */ - bool initializeJointRetargeting(const yarp::os::Searchable& config); - - - /** - * Instantiate the solver - */ - virtual void instantiateSolver() = 0; - - /** - * Evaluate the Hessian matrix. - */ - void evaluateHessianMatrix(); - - /** - * Evaluate the gradient vector. - */ - void evaluateGradientVector(); - - /** - * Evaluate the Linear constraint matrix. - */ - void evaluateLinearConstraintMatrix(); - - /** - * Evaluate Lower and upper bounds - */ - void evaluateBounds(); - - /** - * Set the number of constraints (it may change according to the solver used) - */ - virtual void setNumberOfConstraints(){;}; - - /** - * Initialize matrices that depends on the solver used - */ - virtual void initializeSolverSpecificMatrices(){;}; - - /** - * Set joint velocities bounds - */ - virtual void setJointVelocitiesBounds() = 0; - - public: - /** - * Initialize the QP-IK problem. - * @param config config of the QP-IK solver - * @param actuatedDOFs number of the actuated DoF - * @param minJointsPosition is a vector containing the min joints position limit - * @param minJointsPosition is a vector containing the max joints position limit - * @param maxJointsVelocity is a vector containing the max joints velocity limit. - * @return true/false in case of success/failure. - */ - bool initialize(const yarp::os::Searchable &config, - const int &actuatedDOFs, - const iDynTree::VectorDynSize& maxJointsVelocity, - const iDynTree::VectorDynSize& maxJointsPosition, - const iDynTree::VectorDynSize& minJointsPosition); - - /** - * Set the robot state. - * @param jointPosition vector of joint positions (in rad); - * @param leftFootToWorldTransform transformation between the inertial frame and the left foot; - * @param rightFootToWorldTransform transformation between the inertial frame and the right foot; - * @param leftHandToWorldTransform transformation between the inertial frame and the left foot; - * @param rightHandToWorldTransform transformation between the inertial frame and the right foot; - * @param neckOrientation rotation between the inertial frame and the neck; - * @param comPosition position of the CoM - * @return true/false in case of success/failure. - */ - bool setRobotState(const iDynTree::VectorDynSize& jointPosition, - const iDynTree::Transform& leftFootToWorldTransform, - const iDynTree::Transform& rightFootToWorldTransform, - const iDynTree::Transform& leftHandToWorldTransform, - const iDynTree::Transform& rightHandToWorldTransform, - const iDynTree::Rotation& neckOrientation, - const iDynTree::Position& comPosition); - - /** - * Set the Jacobian of the CoM - * @param comJacobian jacobian of the CoM (mixed representation) - * @return true/false in case of success/failure. - */ - bool setCoMJacobian(const iDynTree::MatrixDynSize& comJacobian); - - /** - * Set the Jacobian of the left foot - * @param leftFootJacobian jacobian of the left foot (mixed representation) - * @return true/false in case of success/failure. - */ - bool setLeftFootJacobian(const iDynTree::MatrixDynSize& leftFootJacobian); - - /** - * Set the Jacobian of the right foot - * @param rightFootJacobian jacobian of the right foot (mixed representation) - * @return true/false in case of success/failure. - */ - bool setRightFootJacobian(const iDynTree::MatrixDynSize& rightFootJacobian); - - /** - * Set the Jacobian of the left hand - * @param leftHandJacobian jacobian of the left hand (mixed representation) - * @return true/false in case of success/failure. - */ - bool setLeftHandJacobian(const iDynTree::MatrixDynSize& leftHandJacobian); - - /** - * Set the Jacobian of the right hand - * @param rightHandJacobian jacobian of the right hand (mixed representation) - * @return true/false in case of success/failure. - */ - bool setRightHandJacobian(const iDynTree::MatrixDynSize& rightHandJacobian); - - /** - * Set the Jacobian of the neck - * @param neckJacobian jacobian of the neck (mixed representation) - * @return true/false in case of success/failure. - */ - bool setNeckJacobian(const iDynTree::MatrixDynSize& neckJacobian); - - /** - * Set the desired joint position. - * Please use this term as regularization term. - * @param regularizationTerm vector of the desired joint position. - * @return true/false in case of success/failure. - */ - bool setDesiredJointPosition(const iDynTree::VectorDynSize& regularizationTerm); - - /** - * Set the desired twist of both feet - * @param leftFootTwist contain the desired twist of the left foot (MIXED representation); - * @param rightFootTwist contain the desired twist of the right foot (MIXED representation). - */ - void setDesiredFeetTwist(const iDynTree::Twist& leftFootTwist, - const iDynTree::Twist& rightFootTwist); - - /** - * Set the desired twist of both feet - * @param leftHandTwist contain the desired twist of the left hand (MIXED representation); - * @param rightHandTwist contain the desired twist of the right hand (MIXED representation). - */ - void setDesiredHandsTwist(const iDynTree::Twist& leftHandTwist, - const iDynTree::Twist& rightHandTwist); - - /** - * Set the desired twist of both feet - * @param jointPosition contain the desired joint position used in the retargeting. - * @return true/false in case of success/failure. - */ - bool setDesiredRetargetingJoint(const iDynTree::VectorDynSize& jointPosition); - - /** - * Set the desired CoMVelocity - * @param comVelocity contain the desired CoM velocity. - */ - void setDesiredCoMVelocity(const iDynTree::Vector3& comVelocity); - - /** - * Set the desired feet transformation - * @param desiredLeftFootToWorldTransform desired transformation between the left foot and the world frame; - * @param desiredRightFootToWorldTransform desired transformation between the right foot and the world frame. - */ - void setDesiredFeetTransformation(const iDynTree::Transform& desiredLeftFootToWorldTransform, - const iDynTree::Transform& desiredRightFootToWorldTransform); - - /** - * Set the desired hand transformation - * @param desiredLeftHandToWorldTransform desired transformation between the left hand and the world frame; - * @param desireRightHandToWorldTransform desired transformation between the right hand and the world frame. - */ - void setDesiredHandsTransformation(const iDynTree::Transform& desiredLeftHandToWorldTransform, - const iDynTree::Transform& desiredRightHandToWorldTransform); - - /** - * Set the desired orientation of the neck - * @param desiredNeckOrientation rotation matrix between the neck and the world frame. - */ - void setDesiredNeckOrientation(const iDynTree::Rotation& desiredNeckOrientation); - - /** - * Set the desired CoMPosition - * @param desiredComPosition contain the desired CoM position. - */ - void setDesiredCoMPosition(const iDynTree::Position& desiredComPosition); - - /** - * Set the robot phase. This is used by the minimum jerk trajectory. - * @param isStancePhase true if the robot is in the stance phase - */ - void setPhase(const bool& isStancePhase); - - /** - * Solve the optimization problem. - * @return true/false in case of success/failure. - */ - virtual bool solve() = 0; - - /** - * Get the solution of the optimization problem (base velocity + joint velocities) - * @return the solution of the optimization problem - */ - const iDynTree::VectorDynSize& getSolution() const; - - /** - * Get the solution of the optimization problem (only joint velocities) - * @return the solution of the optimization problem - */ - const iDynTree::VectorDynSize& getDesiredJointVelocities() const; - - /** - * Get the error seen by the QP problem for the left foot. - * @note it can be useful for debug - * @return the error - */ - iDynTree::VectorDynSize getLeftFootError(); - - /** - * Get the error seen by the QP problem for the right foot. - * @note it can be useful for debug - * @return the error - */ - iDynTree::VectorDynSize getRightFootError(); - - /** - * Get the hessian matrix - * @return the hessian matrix - */ - const iDynTree::MatrixDynSize& getHessianMatrix() const; - - /** - * Get the gradient vector - * @return the gradient vector - */ - const iDynTree::VectorDynSize& getGradient() const; - - /** - * Get the Constraint Matrix - * @return the constraint matrix - */ - const iDynSparseMatrix& getConstraintMatrix() const; - - /** - * Get the upper bound - * @return the upper bound - */ - const iDynTree::VectorDynSize& getUpperBound() const; - - /** - * Get the lower bound - * @return the lower bound - */ - const iDynTree::VectorDynSize& getLowerBound() const; - }; - }; + double m_kJointLimitsLowerBound; /**< Gain related to the the joint lower bound */ + iDynTree::VectorDynSize m_jointVelocitiesBounds; /**< Bounds on the joint velocities*/ + iDynTree::VectorDynSize m_jointPositionsUpperBounds; /**< Upper Bounds on the joint position*/ + iDynTree::VectorDynSize m_jointPositionsLowerBounds; /**< Lower Bounds on the joint position*/ + + // gain scheduling + std::unique_ptr m_handWeightSmoother; /**< Minimum jerk trajectory + for the hand weight matrix. */ + yarp::sig::Vector m_handWeightWalkingVector; /**< Weight matrix (only the diagonal) used for + the hand retargeting during walking. */ + yarp::sig::Vector m_handWeightStanceVector; /**< Weight matrix (only the diagonal) used for + the hand retargeting during stance. */ + + // gain scheduling in case of joint retargeting + std::unique_ptr m_jointRetargetingWeightSmoother; /**< Minimum jerk trajectory + for the Joint retargeting weight matrix. */ + yarp::sig::Vector m_jointRetargetingWeightWalking; /**< Weight matrix (only the diagonal) used for + the joint retargeting during walking. */ + yarp::sig::Vector m_jointRetargetingWeightStance; /**< Weight matrix (only the diagonal) used for + the joint retargeting during stance. */ + + std::unique_ptr m_jointRegularizationWeightSmoother; /**< Minimum jerk trajectory + for the Joint regularization weight matrix. */ + yarp::sig::Vector m_jointRegularizationWeightWalking; /**< Weight matrix (only the diagonal) used for + the joint regularization during walking. */ + yarp::sig::Vector m_jointRegularizationWeightStance; /**< Weight matrix (only the diagonal) used for + the joint regularization during stance. */ + + std::unique_ptr m_torsoWeightSmoother; /**< Minimum jerk trajectory for the torso weight matrix. */ + double m_torsoWeightWalking; /**< Weight matrix (only the diagonal) used for the torso during walking. */ + double m_torsoWeightStance; /**< Weight matrix (only the diagonal) used for the torso during stance. */ + + bool m_useCoMAsConstraint; /**< True if the CoM is added as a constraint. */ + bool m_useJointsLimitsConstraint; /**< True if the CoM is added as a constraint. */ + bool m_enableHandRetargeting; /**< True if the hand retargeting is used */ + bool m_enableJointRetargeting; /**< True if the joint retargeting is used */ + + iDynTree::MatrixDynSize m_hessianDense; /**< Hessian matrix */ + iDynTree::VectorDynSize m_gradient; /**< Gradient vector */ + iDynSparseMatrix m_constraintsMatrixSparse; /**< Constraint matrix */ + iDynTree::VectorDynSize m_lowerBound; /**< Lower bound */ + iDynTree::VectorDynSize m_upperBound; /**< Upper bound */ + iDynTree::VectorDynSize m_solution; /**< Solution of the optimization problem */ + iDynTree::VectorDynSize m_desiredJointVelocitiesOutput; /**< Joint velocities required by the optimization problem */ + + /** + * Initialize all the constant matrix from the configuration file. + * @param config configuration parameters + * @return true/false in case of success/failure. + */ + bool initializeMatrices(const yarp::os::Searchable& config); + + /** + * Initialize hand retargeting. + * @param config configuration parameters + * @return true/false in case of success/failure. + */ + bool initializeHandRetargeting(const yarp::os::Searchable& config); + + /** + * Initialize joint retargeting. + * @param config configuration parameters + * @return true/false in case of success/failure. + */ + bool initializeJointRetargeting(const yarp::os::Searchable& config); + + + /** + * Instantiate the solver + */ + virtual void instantiateSolver() = 0; + + /** + * Evaluate the Hessian matrix. + */ + void evaluateHessianMatrix(); + + /** + * Evaluate the gradient vector. + */ + void evaluateGradientVector(); + + /** + * Evaluate the Linear constraint matrix. + */ + void evaluateLinearConstraintMatrix(); + + /** + * Evaluate Lower and upper bounds + */ + void evaluateBounds(); + + /** + * Set the number of constraints (it may change according to the solver used) + */ + virtual void setNumberOfConstraints(){;}; + + /** + * Initialize matrices that depends on the solver used + */ + virtual void initializeSolverSpecificMatrices(){;}; + + /** + * Set joint velocities bounds + */ + virtual void setJointVelocitiesBounds() = 0; + + public: + /** + * Initialize the QP-IK problem. + * @param config config of the QP-IK solver + * @param actuatedDOFs number of the actuated DoF + * @param minJointsPosition is a vector containing the min joints position limit + * @param minJointsPosition is a vector containing the max joints position limit + * @param maxJointsVelocity is a vector containing the max joints velocity limit. + * @return true/false in case of success/failure. + */ + bool initialize(const yarp::os::Searchable &config, + const int &actuatedDOFs, + const iDynTree::VectorDynSize& maxJointsVelocity, + const iDynTree::VectorDynSize& maxJointsPosition, + const iDynTree::VectorDynSize& minJointsPosition); + + /** + * Set the robot state. + * @param jointPosition vector of joint positions (in rad); + * @param leftFootToWorldTransform transformation between the inertial frame and the left foot; + * @param rightFootToWorldTransform transformation between the inertial frame and the right foot; + * @param leftHandToWorldTransform transformation between the inertial frame and the left foot; + * @param rightHandToWorldTransform transformation between the inertial frame and the right foot; + * @param neckOrientation rotation between the inertial frame and the neck; + * @param comPosition position of the CoM + * @return true/false in case of success/failure. + */ + bool setRobotState(const iDynTree::VectorDynSize& jointPosition, + const iDynTree::Transform& leftFootToWorldTransform, + const iDynTree::Transform& rightFootToWorldTransform, + const iDynTree::Transform& leftHandToWorldTransform, + const iDynTree::Transform& rightHandToWorldTransform, + const iDynTree::Rotation& neckOrientation, + const iDynTree::Position& comPosition); + + /** + * Set the Jacobian of the CoM + * @param comJacobian jacobian of the CoM (mixed representation) + * @return true/false in case of success/failure. + */ + bool setCoMJacobian(const iDynTree::MatrixDynSize& comJacobian); + + /** + * Set the Jacobian of the left foot + * @param leftFootJacobian jacobian of the left foot (mixed representation) + * @return true/false in case of success/failure. + */ + bool setLeftFootJacobian(const iDynTree::MatrixDynSize& leftFootJacobian); + + /** + * Set the Jacobian of the right foot + * @param rightFootJacobian jacobian of the right foot (mixed representation) + * @return true/false in case of success/failure. + */ + bool setRightFootJacobian(const iDynTree::MatrixDynSize& rightFootJacobian); + + /** + * Set the Jacobian of the left hand + * @param leftHandJacobian jacobian of the left hand (mixed representation) + * @return true/false in case of success/failure. + */ + bool setLeftHandJacobian(const iDynTree::MatrixDynSize& leftHandJacobian); + + /** + * Set the Jacobian of the right hand + * @param rightHandJacobian jacobian of the right hand (mixed representation) + * @return true/false in case of success/failure. + */ + bool setRightHandJacobian(const iDynTree::MatrixDynSize& rightHandJacobian); + + /** + * Set the Jacobian of the neck + * @param neckJacobian jacobian of the neck (mixed representation) + * @return true/false in case of success/failure. + */ + bool setNeckJacobian(const iDynTree::MatrixDynSize& neckJacobian); + + /** + * Set the desired joint position. + * Please use this term as regularization term. + * @param regularizationTerm vector of the desired joint position. + * @return true/false in case of success/failure. + */ + bool setDesiredJointPosition(const iDynTree::VectorDynSize& regularizationTerm); + + /** + * Set the desired twist of both feet + * @param leftFootTwist contain the desired twist of the left foot (MIXED representation); + * @param rightFootTwist contain the desired twist of the right foot (MIXED representation). + */ + void setDesiredFeetTwist(const iDynTree::Twist& leftFootTwist, + const iDynTree::Twist& rightFootTwist); + + /** + * Set the desired twist of both feet + * @param leftHandTwist contain the desired twist of the left hand (MIXED representation); + * @param rightHandTwist contain the desired twist of the right hand (MIXED representation). + */ + void setDesiredHandsTwist(const iDynTree::Twist& leftHandTwist, + const iDynTree::Twist& rightHandTwist); + + /** + * Set the desired twist of both feet + * @param jointPosition contain the desired joint position used in the retargeting. + * @return true/false in case of success/failure. + */ + bool setDesiredRetargetingJoint(const iDynTree::VectorDynSize& jointPosition); + + /** + * Set the desired CoMVelocity + * @param comVelocity contain the desired CoM velocity. + */ + void setDesiredCoMVelocity(const iDynTree::Vector3& comVelocity); + + /** + * Set the desired feet transformation + * @param desiredLeftFootToWorldTransform desired transformation between the left foot and the world frame; + * @param desiredRightFootToWorldTransform desired transformation between the right foot and the world frame. + */ + void setDesiredFeetTransformation(const iDynTree::Transform& desiredLeftFootToWorldTransform, + const iDynTree::Transform& desiredRightFootToWorldTransform); + + /** + * Set the desired hand transformation + * @param desiredLeftHandToWorldTransform desired transformation between the left hand and the world frame; + * @param desireRightHandToWorldTransform desired transformation between the right hand and the world frame. + */ + void setDesiredHandsTransformation(const iDynTree::Transform& desiredLeftHandToWorldTransform, + const iDynTree::Transform& desiredRightHandToWorldTransform); + + /** + * Set the desired orientation of the neck + * @param desiredNeckOrientation rotation matrix between the neck and the world frame. + */ + void setDesiredNeckOrientation(const iDynTree::Rotation& desiredNeckOrientation); + + /** + * Set the desired CoMPosition + * @param desiredComPosition contain the desired CoM position. + */ + void setDesiredCoMPosition(const iDynTree::Position& desiredComPosition); + + /** + * Set the robot phase. This is used by the minimum jerk trajectory. + * @param isStancePhase true if the robot is in the stance phase + */ + void setPhase(const bool& isStancePhase); + + /** + * Solve the optimization problem. + * @return true/false in case of success/failure. + */ + virtual bool solve() = 0; + + /** + * Get the solution of the optimization problem (base velocity + joint velocities) + * @return the solution of the optimization problem + */ + const iDynTree::VectorDynSize& getSolution() const; + + /** + * Get the solution of the optimization problem (only joint velocities) + * @return the solution of the optimization problem + */ + const iDynTree::VectorDynSize& getDesiredJointVelocities() const; + + /** + * Get the error seen by the QP problem for the left foot. + * @note it can be useful for debug + * @return the error + */ + iDynTree::VectorDynSize getLeftFootError(); + + /** + * Get the error seen by the QP problem for the right foot. + * @note it can be useful for debug + * @return the error + */ + iDynTree::VectorDynSize getRightFootError(); + + /** + * Get the hessian matrix + * @return the hessian matrix + */ + const iDynTree::MatrixDynSize& getHessianMatrix() const; + + /** + * Get the gradient vector + * @return the gradient vector + */ + const iDynTree::VectorDynSize& getGradient() const; + + /** + * Get the Constraint Matrix + * @return the constraint matrix + */ + const iDynSparseMatrix& getConstraintMatrix() const; + + /** + * Get the upper bound + * @return the upper bound + */ + const iDynTree::VectorDynSize& getUpperBound() const; + + /** + * Get the lower bound + * @return the lower bound + */ + const iDynTree::VectorDynSize& getLowerBound() const; + }; +}; #endif From b4bc664bd02e03df7ae63b9c3f9b1b929c2cc460 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 22 Apr 2020 20:47:49 +0200 Subject: [PATCH 32/58] Fix indentation in RobotInterface helper files --- .../RobotInterface/Helper.h | 10 ++-- src/RobotInterface/src/Helper.cpp | 58 +++++++++---------- 2 files changed, 34 insertions(+), 34 deletions(-) diff --git a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h index 540bc113..2c8c6d61 100644 --- a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h +++ b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h @@ -67,9 +67,9 @@ namespace WalkingControllers iDynTree::VectorDynSize m_jointVelocitiesBounds; /**< Joint Velocity bounds [rad/s]. */ iDynTree::VectorDynSize m_jointPositionsUpperBounds; /**< Joint Position upper bound [rad]. */ iDynTree::VectorDynSize m_jointPositionsLowerBounds; /**< Joint Position lower bound [rad]. */ - std::vector m_isJointModeStiffVector;/**< Joint is in the stiff or compliance mode */ - std::vector m_JointModeStiffVectorDefult;/**< All the joints are in the stiff mode */ - std::vector m_currentModeofJoints;/**< Joint is in the stiff or compliance mode based on the walking architecture phases */ + std::vector m_isJointModeStiffVector;/**< Joint is in the stiff or compliance mode */ + std::vector m_JointModeStiffVectorDefult;/**< All the joints are in the stiff mode */ + std::vector m_currentModeofJoints;/**< Joint is in the stiff or compliance mode based on the walking architecture phases */ // yarp::sig::Vector m_positionFeedbackDegFiltered; yarp::sig::Vector m_velocityFeedbackDegFiltered; /**< Vector containing the filtered joint velocity [deg/s]. */ @@ -242,9 +242,9 @@ namespace WalkingControllers const iDynTree::Twist& getBaseTwist() const; /** - * Set the height of the offset coming from the base estimation + * Set the height of the offset coming from the base estimation * @param offset of the height of the base in meters - */ + */ void setHeightOffset(const double& offset); /** diff --git a/src/RobotInterface/src/Helper.cpp b/src/RobotInterface/src/Helper.cpp index d4650465..c79cbaaa 100644 --- a/src/RobotInterface/src/Helper.cpp +++ b/src/RobotInterface/src/Helper.cpp @@ -32,7 +32,7 @@ bool RobotInterface::getWorstError(const iDynTree::VectorDynSize& desiredJointPo { currentJointPositionRad = iDynTree::deg2rad(m_positionFeedbackDeg[i]); absoluteJointErrorRad = std::fabs(iDynTreeUtilities::shortestAngularDistance(currentJointPositionRad, - desiredJointPositionsRad(i))); + desiredJointPositionsRad(i))); if(m_currentModeofJoints.at(i)==yarp::dev::InteractionModeEnum::VOCAB_IM_STIFF) { if(absoluteJointErrorRad > worstError.second) @@ -295,8 +295,8 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) if(!m_robotDevice.view(m_InteractionInterface) || !m_InteractionInterface) { - yError() << "[configureRobot] Cannot obtain IInteractionMode interface"; - return false; + yError() << "[configureRobot] Cannot obtain IInteractionMode interface"; + return false; } // resize the buffers @@ -385,25 +385,25 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) } m_useExternalRobotBase = config.check("use_external_robot_base", yarp::os::Value("False")).asBool(); - if(m_useExternalRobotBase) - { - m_robotBasePort.open("/" + name + "/robotBase:i"); - // connect port + if(m_useExternalRobotBase) + { + m_robotBasePort.open("/" + name + "/robotBase:i"); + // connect port - std::string floatingBasePortName; - if(!YarpUtilities::getStringFromSearchable(config, "floating_base_port_name", floatingBasePortName)) - { - yError() << "[RobotHelper::configureForceTorqueSensors] Unable to get the string from searchable."; - return false; - } + std::string floatingBasePortName; + if(!YarpUtilities::getStringFromSearchable(config, "floating_base_port_name", floatingBasePortName)) + { + yError() << "[RobotHelper::configureForceTorqueSensors] Unable to get the string from searchable."; + return false; + } - if(!yarp::os::Network::connect(floatingBasePortName, "/" + name + "/robotBase:i")) - { - yError() << "Unable to connect to port " << "/" + name + "/robotBase:i"; - return false; - } + if(!yarp::os::Network::connect(floatingBasePortName, "/" + name + "/robotBase:i")) + { + yError() << "Unable to connect to port " << "/" + name + "/robotBase:i"; + return false; } - m_heightOffset = 0; + } + m_heightOffset = 0; return true; } @@ -431,9 +431,9 @@ bool RobotInterface::configureForceTorqueSensors(const yarp::os::Searchable& con // open and connect left foot wrench if(!YarpUtilities::getStringFromSearchable(config, "leftFootWrenchInputPort_name", portInput)) { - yError() << "[RobotInterface::configureForceTorqueSensors] Unable to get " - "the string from searchable."; - return false; + yError() << "[RobotInterface::configureForceTorqueSensors] Unable to get " + "the string from searchable."; + return false; } if(!YarpUtilities::getStringFromSearchable(config, "leftFootWrenchOutputPort_name", portOutput)) { @@ -570,7 +570,7 @@ bool RobotInterface::switchToControlMode(const int& controlMode) } bool RobotInterface::setPositionReferences(const iDynTree::VectorDynSize& desiredJointPositionsRad, - const double& positioningTimeSec) + const double& positioningTimeSec) { if(m_controlMode != VOCAB_CM_POSITION) { @@ -591,10 +591,10 @@ bool RobotInterface::setPositionReferences(const iDynTree::VectorDynSize& desire } if(m_InteractionInterface == nullptr) - { - yError() << "[RobotInterface::setPositionReferences] IInteractionMode interface is not ready."; - return false; - } + { + yError() << "[RobotInterface::setPositionReferences] IInteractionMode interface is not ready."; + return false; + } m_desiredJointPositionRad = desiredJointPositionsRad; @@ -632,7 +632,7 @@ bool RobotInterface::setPositionReferences(const iDynTree::VectorDynSize& desire { currentJointPositionRad = iDynTree::deg2rad(m_positionFeedbackDeg[i]); absoluteJointErrorRad = std::fabs(iDynTreeUtilities::shortestAngularDistance(currentJointPositionRad, - desiredJointPositionsRad(i))); + desiredJointPositionsRad(i))); refSpeeds[i] = std::max(3.0, iDynTree::rad2deg(absoluteJointErrorRad) / positioningTimeSec); } @@ -884,7 +884,7 @@ bool RobotInterface::setInteractionMode() return false; } - m_currentModeofJoints = m_isJointModeStiffVector; + m_currentModeofJoints = m_isJointModeStiffVector; return true; } From c754e44b216fb062fe9275aad8a9aa1b94e11343 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 23 Apr 2020 12:47:03 +0200 Subject: [PATCH 33/58] Clean the RobotInterface to be compliant with the code style and rename several variables --- .../RobotInterface/Helper.h | 22 +++-- src/RobotInterface/src/Helper.cpp | 97 ++++++++++++------- src/WalkingModule/src/Module.cpp | 2 +- 3 files changed, 73 insertions(+), 48 deletions(-) diff --git a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h index 2c8c6d61..288f07a7 100644 --- a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h +++ b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h @@ -38,7 +38,11 @@ namespace WalkingControllers { yarp::dev::PolyDriver m_robotDevice; /**< Main robot device. */ std::vector m_axesList; /**< Vector containing the name of the controlled joints. */ - std::vector m_dangerousJoints; /**< Vector containing the the information related to the importance of the joint. */ + + std::vector m_jointInteractionMode;/**< Joint is in the stiff or compliance mode */ + std::vector m_currentJointInteractionMode;/**< Joint is in the stiff or compliance mode based on the walking architecture phases */ + + std::vector m_isGoodTrackingRequired; /**< Vector containing the the information related to the importance of the joint. */ unsigned int m_actuatedDOFs; /**< Number of the actuated DoFs. */ // YARP Interfaces exposed by the remotecontrolboardremapper @@ -48,7 +52,7 @@ namespace WalkingControllers yarp::dev::IVelocityControl *m_velocityInterface{nullptr}; /**< Position control interface. */ yarp::dev::IControlMode *m_controlModeInterface{nullptr}; /**< Control mode interface. */ yarp::dev::IControlLimits *m_limitsInterface{nullptr}; /**< Encorders interface. */ - yarp::dev::IInteractionMode *m_InteractionInterface{nullptr}; /**< Stiff/compliant mode interface. */ + yarp::dev::IInteractionMode *m_interactionInterface{nullptr}; /**< Stiff/compliant mode interface. */ std::unique_ptr m_PIDHandler; /**< Pointer to the PID handler object. */ @@ -67,10 +71,6 @@ namespace WalkingControllers iDynTree::VectorDynSize m_jointVelocitiesBounds; /**< Joint Velocity bounds [rad/s]. */ iDynTree::VectorDynSize m_jointPositionsUpperBounds; /**< Joint Position upper bound [rad]. */ iDynTree::VectorDynSize m_jointPositionsLowerBounds; /**< Joint Position lower bound [rad]. */ - std::vector m_isJointModeStiffVector;/**< Joint is in the stiff or compliance mode */ - std::vector m_JointModeStiffVectorDefult;/**< All the joints are in the stiff mode */ - std::vector m_currentModeofJoints;/**< Joint is in the stiff or compliance mode based on the walking architecture phases */ - // yarp::sig::Vector m_positionFeedbackDegFiltered; yarp::sig::Vector m_velocityFeedbackDegFiltered; /**< Vector containing the filtered joint velocity [deg/s]. */ std::unique_ptr m_positionFilter; /**< Joint position low pass filter .*/ @@ -89,8 +89,6 @@ namespace WalkingControllers std::unique_ptr m_rightWrenchFilter; /**< Right wrench low pass filter.*/ bool m_useWrenchFilter; /**< True if the wrench filter is used. */ - std::vector m_jointModes; /**< True if the joint is in the stiff mode */ - double m_startingPositionControlTime; bool m_positionMoveSkipped; @@ -118,6 +116,10 @@ namespace WalkingControllers * @return true in case of success and false otherwise. */ bool switchToControlMode(const int& controlMode); + + bool setInteractionMode(yarp::dev::InteractionModeEnum interactionMode); + + bool setInteractionMode(std::vector& interactionModes); public: /** @@ -224,10 +226,10 @@ namespace WalkingControllers WalkingPIDHandler& getPIDHandler(); /** - * Set the intraction mode of the joints(stiff/compliant). + * Set the intraction mode stored in the configuration * @return true in case of success and false otherwise. */ - bool setInteractionMode(); + bool loadCustomInteractionMode(); /** * Get the base Transform from external software. diff --git a/src/RobotInterface/src/Helper.cpp b/src/RobotInterface/src/Helper.cpp index c79cbaaa..5e4ab193 100644 --- a/src/RobotInterface/src/Helper.cpp +++ b/src/RobotInterface/src/Helper.cpp @@ -30,14 +30,15 @@ bool RobotInterface::getWorstError(const iDynTree::VectorDynSize& desiredJointPo double absoluteJointErrorRad; for(int i = 0; i < m_actuatedDOFs; i++) { - currentJointPositionRad = iDynTree::deg2rad(m_positionFeedbackDeg[i]); - absoluteJointErrorRad = std::fabs(iDynTreeUtilities::shortestAngularDistance(currentJointPositionRad, - desiredJointPositionsRad(i))); - if(m_currentModeofJoints.at(i)==yarp::dev::InteractionModeEnum::VOCAB_IM_STIFF) + if (m_currentJointInteractionMode[i] == yarp::dev::InteractionModeEnum::VOCAB_IM_STIFF + && m_isGoodTrackingRequired[i]) { + currentJointPositionRad = iDynTree::deg2rad(m_positionFeedbackDeg[i]); + absoluteJointErrorRad = std::abs(iDynTreeUtilities::shortestAngularDistance(currentJointPositionRad, + desiredJointPositionsRad(i))); if(absoluteJointErrorRad > worstError.second) { - worstError.first =i; + worstError.first = i; worstError.second = absoluteJointErrorRad; } } @@ -212,42 +213,38 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) yarp::os::Property& remoteControlBoardsOpts = options.addGroup("REMOTE_CONTROLBOARD_OPTIONS"); remoteControlBoardsOpts.put("writeStrict", "on"); + // get the actuated DoFs m_actuatedDOFs = m_axesList.size(); - yarp::os::Value *dangerousJoints; - if(!config.check("dangerous_joints", dangerousJoints)) - { - yError() << "[configureRobot] Unable to find dangerous_joints into config file."; - return false; - } - if(!YarpUtilities::yarpListToBoolVector(dangerousJoints, m_dangerousJoints)) + m_isGoodTrackingRequired.resize(m_actuatedDOFs); + if(!YarpUtilities::getVectorOfBooleanFromSearchable(config, "good_tracking_required", + m_isGoodTrackingRequired)) { - yError() << "[configureRobot] Unable to convert yarp list into a vector of bool."; + yError() << "[RobotInterface::configureRobot] Unable to find is_good_tracking_required into config file."; return false; } - m_isJointModeStiffVector.resize(m_actuatedDOFs); - m_JointModeStiffVectorDefult.resize(m_actuatedDOFs); - m_jointModes.resize(m_actuatedDOFs); - if(!YarpUtilities::getVectorOfBooleanFromSearchable(config,"joint_is_stiff_mode",m_jointModes)) + m_jointInteractionMode.resize(m_actuatedDOFs); + m_currentJointInteractionMode.resize(m_actuatedDOFs); + std::vector isJointInStiffMode(m_actuatedDOFs); + if(!YarpUtilities::getVectorOfBooleanFromSearchable(config, "joint_is_stiff_mode", + isJointInStiffMode)) { yError() << "[RobotInterface::configureRobot] Unable to find joint_is_stiff_mode into config file."; return false; } - for (unsigned int i=0;igetInteractionModes(m_currentJointInteractionMode.data())) + { + yError() << "[RobotHelper::configure] Unable to get the interaction mode."; + return false; + } + if(!setInteractionMode(yarp::dev::InteractionModeEnum::VOCAB_IM_STIFF)) + { + yError() << "[RobotInterface::configureRobot] Unable to set the stiff control mode for all joints."; + return false; + } + return true; } @@ -569,6 +579,27 @@ bool RobotInterface::switchToControlMode(const int& controlMode) return true; } +bool RobotInterface::setInteractionMode(yarp::dev::InteractionModeEnum interactionMode) +{ + std::vector interactionModes(m_actuatedDOFs, interactionMode); + + return setInteractionMode(interactionModes); +} + +bool RobotInterface::setInteractionMode(std::vector& interactionModes) +{ + if(m_currentJointInteractionMode != interactionModes) + { + bool ok = m_interactionInterface->setInteractionModes(interactionModes.data()); + if (ok) + m_currentJointInteractionMode = interactionModes; + + return ok; + } + + return true; +} + bool RobotInterface::setPositionReferences(const iDynTree::VectorDynSize& desiredJointPositionsRad, const double& positioningTimeSec) { @@ -590,7 +621,7 @@ bool RobotInterface::setPositionReferences(const iDynTree::VectorDynSize& desire return false; } - if(m_InteractionInterface == nullptr) + if(m_interactionInterface == nullptr) { yError() << "[RobotInterface::setPositionReferences] IInteractionMode interface is not ready."; return false; @@ -728,7 +759,7 @@ bool RobotInterface::setDirectPositionReferences(const iDynTree::VectorDynSize& return false; } - if(worstError.second > 0.5 && m_dangerousJoints[worstError.first]) + if(worstError.second > 0.5) { yError() << "[RobotInterface::setDirectPositionReferences] The worst error between the current and the " << "desired position of the " << m_axesList[worstError.first] @@ -797,7 +828,7 @@ bool RobotInterface::close() m_leftWrenchPort.close(); switchToControlMode(VOCAB_CM_POSITION); m_controlMode = VOCAB_CM_POSITION; - m_InteractionInterface->setInteractionModes(m_JointModeStiffVectorDefult.data()); + setInteractionMode(yarp::dev::InteractionModeEnum::VOCAB_IM_STIFF); if(!m_robotDevice.close()) { yError() << "[RobotInterface::close] Unable to close the device."; @@ -876,16 +907,8 @@ bool RobotInterface::isExternalRobotBaseUsed() return m_useExternalRobotBase; } -bool RobotInterface::setInteractionMode() +bool RobotInterface::loadCustomInteractionMode() { - if(!m_InteractionInterface->setInteractionModes(m_isJointModeStiffVector.data())) - { - yError() << "[RobotInterface::setInteractionMode] Error while setting the interaction modes of the joints"; - return false; - } - - m_currentModeofJoints = m_isJointModeStiffVector; - - return true; + return setInteractionMode(m_jointInteractionMode); } diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 97af4706..b8d59c19 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -1272,7 +1272,7 @@ bool WalkingModule::startWalking() } - if (!m_robotControlHelper->setInteractionMode()) + if (!m_robotControlHelper->loadCustomInteractionMode()) { yError() << "[WalkingModule::startWalking] Unable to set the intraction mode of the joints"; return false; From 9760be0556d233e940c1faba357e1d48f3f54cf8 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 23 Apr 2020 12:51:09 +0200 Subject: [PATCH 34/58] Move the logic behind the isStancePhase flag from WalkingModule to TrajectoryGenerator --- .../TrajectoryPlanner/TrajectoryGenerator.h | 10 ++++ .../src/TrajectoryGenerator.cpp | 47 +++++++++++++++++ .../WalkingControllers/WalkingModule/Module.h | 6 +-- src/WalkingModule/src/Module.cpp | 50 ++++--------------- 4 files changed, 67 insertions(+), 46 deletions(-) diff --git a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h index 869f2bbf..5beca5e4 100644 --- a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h +++ b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h @@ -46,6 +46,7 @@ namespace WalkingControllers double m_dT; /**< Sampling time of the planner. */ double m_plannerHorizon; /**< Horizon of the planner. */ + std::size_t m_stancePhaseDelay; /**< Delay in ticks of the beginning of the stance phase. */ double m_nominalWidth; /**< Nominal width between two feet. */ double m_initTime; /**< Init time of the current trajectory. */ @@ -226,6 +227,15 @@ namespace WalkingControllers */ bool getWeightPercentage(std::vector &weightInLeft, std::vector &weightInRight); + /** + * Get the weight percentage for the left and right foot + * @param weightInLeft vector containing the weight on the left foot (0 in case in case of + * stance foot during SS, 1 in case of swing foot) + * @param weightInRight vector containing the weight on the right foot (0 in case in case of + * stance foot during SS, 1 in case of swing foot) + * @return true/false in case of success/failure. + */ + bool getIsStancePhase(std::vector& isStancePhase); /** * Reset the planner diff --git a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp index e139ad31..ce1e4cef 100644 --- a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp +++ b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp @@ -56,6 +56,11 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) m_plannerHorizon = config.check("plannerHorizon", yarp::os::Value(20.0)).asDouble(); double unicycleGain = config.check("unicycleGain", yarp::os::Value(10.0)).asDouble(); + // TODO please remove me. Counters are evil + double stancePhaseDelaySeconds = config.check("stance_phase_delay",yarp::os::Value(0.0)).asDouble(); + + m_stancePhaseDelay = (std::size_t) std::round(stancePhaseDelaySeconds / m_dT); + if(!YarpUtilities::getVectorFromSearchable(config, "referencePosition", m_referencePointDistance)) { yError() << "[configurePlanner] Initialization failed while reading referencePosition vector."; @@ -633,3 +638,45 @@ void TrajectoryGenerator::reset() // change the state of the generator m_generatorState = GeneratorState::FirstStep; } + +bool TrajectoryGenerator::getIsStancePhase(std::vector& isStancePhase) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getDCMVelocityTrajectory] No trajectories are available"; + return false; + } + + const auto & DCMVelocityTrajectory = m_dcmGenerator->getDCMVelocity(); + isStancePhase.resize(DCMVelocityTrajectory.size()); + + double threshold = 0.001; + + // here there is the assumption that each trajectory begins with a stance phase + std::size_t stancePhaseDelayCounter = 0; + for(std::size_t i = 0; i < DCMVelocityTrajectory.size(); i++) + { + // in this case the robot is moving + if(iDynTree::toEigen(DCMVelocityTrajectory[i]).norm() > threshold) + { + isStancePhase[i] = false; + // reset the counter for the beginning of the next stance phase. + // If m_stancePhaseDelay is equal to zero, the stance phase will not be delayed + stancePhaseDelayCounter = m_stancePhaseDelay; + } + else + { + // decreased the counter only if it is different from zero. + // it is required to add a delay in the beginning of the stance phase + stancePhaseDelayCounter = stancePhaseDelayCounter == 0 ? 0 : stancePhaseDelayCounter--; + + // the delay expired the robot can be considered stance + if(stancePhaseDelayCounter == 0) + isStancePhase[i] = true; + else + isStancePhase[i] = false; + } + } + + return true; +} diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 16ac38aa..0c1f221c 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -103,6 +103,7 @@ namespace WalkingControllers std::deque m_comHeightTrajectory; /**< Deque containing the CoM height trajectory. */ std::deque m_comHeightVelocity; /**< Deque containing the CoM height velocity. */ std::deque m_mergePoints; /**< Deque containing the time position of the merge points. */ + std::deque m_isStancePhase; /**< if true the robot is not walking */ std::deque m_isLeftFixedFrame; /**< Deque containing when the main frame of the left foot is the fixed frame In general a main frame of a foot is the fix frame only during the @@ -129,11 +130,6 @@ namespace WalkingControllers // debug std::unique_ptr m_velocityIntegral{nullptr}; - bool m_isStancePhase{true}; /**< if true the robot is not walking */ - bool m_isStancePhaseStarting{false}; - int m_stancePhaseCounter; - int m_stancePhaseMaxCounter; - /** * Get the robot model from the resource finder and set it. * @param rf is the reference to a resource finder object. diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index b8d59c19..a869c19e 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -322,14 +322,6 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) m_qDesired.resize(m_robotControlHelper->getActuatedDoFs()); m_dqDesired.resize(m_robotControlHelper->getActuatedDoFs()); - // TODO please remove me. Counters are evil - double stancePhaseTimeOutSeconds = rf.check("stance_phase_time_out",yarp::os::Value(0.5)).asDouble(); - - m_stancePhaseMaxCounter = (int) std::round(stancePhaseTimeOutSeconds/m_dT); - - yInfo() << "m_stancePhaseMaxCounter " << m_stancePhaseMaxCounter - << " stancePhaseTimeOutSeconds " << stancePhaseTimeOutSeconds; - yInfo() << "[WalkingModule::configure] Ready to play!"; return true; @@ -386,7 +378,7 @@ bool WalkingModule::solveQPIK(const std::unique_ptr& solver, const iDynTree::VectorDynSize &output) { bool ok = true; - solver->setPhase(m_isStancePhase); + solver->setPhase(m_isStancePhase.front()); ok &= solver->setRobotState(m_robotControlHelper->getJointPosition(), m_FKSolver->getLeftFootToWorldTransform(), @@ -601,22 +593,6 @@ bool WalkingModule::updateModule() } } - // check if the stance phase is started - double treshold = 0.01; - if(m_isStancePhaseStarting && - iDynTree::toEigen(m_DCMVelocityDesired.front()).norm() < treshold) - { - m_stancePhaseCounter--; - if(m_stancePhaseCounter == 0) - { - m_isStancePhase = true; - m_isStancePhaseStarting = false; - } - } - - // TODO remove me - yInfo() << "m_isStancePhase " << m_isStancePhase << " isStancePhaseStarting " << m_isStancePhaseStarting << " m_stancePhaseCounter " << m_stancePhaseCounter; - // get feedbacks and evaluate useful quantities if(!m_robotControlHelper->getFeedbacks(100)) { @@ -624,7 +600,7 @@ bool WalkingModule::updateModule() return false; } - m_retargetingClient->setPhase(m_isStancePhase); + m_retargetingClient->setPhase(m_isStancePhase.front()); m_retargetingClient->getFeedback(); if(!updateFKSolver()) @@ -695,7 +671,7 @@ bool WalkingModule::updateModule() // inner COM-ZMP controller // if the the norm of desired DCM velocity is lower than a threshold then the robot // is stopped - m_walkingZMPController->setPhase(m_isStancePhase); + m_walkingZMPController->setPhase(m_isStancePhase.front()); iDynTree::Vector2 desiredZMP; if(m_useMPC) @@ -1143,6 +1119,7 @@ bool WalkingModule::updateTrajectories(const size_t& mergePoint) std::vector comHeightVelocity; std::vector mergePoints; std::vector isLeftFixedFrame; + std::vector isStancePhase; // get dcm position and velocity m_trajectoryGenerator->getDCMPositionTrajectory(DCMPositionDesired); @@ -1161,6 +1138,9 @@ bool WalkingModule::updateTrajectories(const size_t& mergePoint) // get merge points m_trajectoryGenerator->getMergePoints(mergePoints); + // get stance phase flags + m_trajectoryGenerator->getIsStancePhase(isStancePhase); + // append vectors to deques StdUtilities::appendVectorToDeque(leftTrajectory, m_leftTrajectory, mergePoint); StdUtilities::appendVectorToDeque(rightTrajectory, m_rightTrajectory, mergePoint); @@ -1177,6 +1157,8 @@ bool WalkingModule::updateTrajectories(const size_t& mergePoint) StdUtilities::appendVectorToDeque(comHeightTrajectory, m_comHeightTrajectory, mergePoint); StdUtilities::appendVectorToDeque(comHeightVelocity, m_comHeightVelocity, mergePoint); + StdUtilities::appendVectorToDeque(isStancePhase, m_isStancePhase, mergePoint); + m_mergePoints.assign(mergePoints.begin(), mergePoints.end()); // the first merge point is always equal to 0 @@ -1335,20 +1317,6 @@ bool WalkingModule::setPlannerInput(double x, double y) m_newTrajectoryRequired = true; double treshold = 0.01; - // the robot is about to move - if(iDynTree::toEigen(m_desiredPosition).norm() > treshold) - { - m_isStancePhase = false; - m_isStancePhaseStarting = false; - } - else - { - // if the stance phase has not started reset the counter - if(!m_isStancePhaseStarting) - m_stancePhaseCounter = m_stancePhaseMaxCounter; - - m_isStancePhaseStarting = true; - } return true; } From dbbb6b1c8391896c0ab0773cd4250fa4f0b74c11 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 23 Apr 2020 14:17:35 +0200 Subject: [PATCH 35/58] Update the parameters according to the 4a79c51f6a7f06980b2bed1eceeba5c777536758 and 21fb7bdf8cd9159e790e769bf54e5e0088b16711 --- .../hand_retargeting/robotControl.ini | 26 +++++++++------- .../joint_retargeting/robotControl.ini | 23 +++++++++----- .../joypad_control/robotControl.ini | 28 ++++++++--------- .../hand_retargeting/plannerParams.ini | 4 +++ .../hand_retargeting/robotControl.ini | 27 ++++++++-------- .../joint_retargeting/plannerParams.ini | 4 +++ .../joint_retargeting/robotControl.ini | 31 ++++++++++++------- .../joypad_control/robotControl.ini | 27 ++++++++-------- .../dcm_walking_joint_retargeting.ini | 2 -- 9 files changed, 99 insertions(+), 73 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/robotControl.ini index a8265334..275d136a 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/robotControl.ini @@ -7,18 +7,6 @@ joints_list ("torso_pitch", "torso_roll", "torso_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") -dangerous_joints (1, 1, 1, - 1, 1, 1, 1, 0, 0, 0, - 1, 1, 1, 1, 0, 0, 0, - 1, 1, 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1) - -# if 1 the joint is in stiff mode if 0 the joint is in compliant mode -joint_is_stiff_mode (true, true, true, - true, true, true, true, true, true,true, - true, true, true, true, true, true,true, - true, true, true, true, true, true, - true, true, true, true, true, true) remote_control_boards ("torso", "left_arm", "right_arm", "left_leg", "right_leg") # filters @@ -28,3 +16,17 @@ joint_velocity_cut_frequency 10.0 use_wrench_filter 0 wrench_cut_frequency 10.0 + +# if true the joint is in stiff mode if false the joint is in compliant mode +joint_is_stiff_mode (true, true, true, + true, true, true, true, true, true, true, + true, true, true, true, true, true, true, + true, true, true, true, true, true, + true, true, true, true, true, true) + +# if true a good joint traking is considered mandatory +good_tracking_required (true, true, true, + true, true, true, true, false, false, false, + true, true, true, true, false, false, false, + true, true, true, true, true, true, + true, true, true, true, true, true) diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini index 17bf6679..6a9b80b6 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/robotControl.ini @@ -8,13 +8,6 @@ joints_list ("neck_pitch", "neck_roll", "neck_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") -dangerous_joints (0, 0, 0, - 1, 1, 1, - 1, 1, 1, 1, 0, 0, 0, - 1, 1, 1, 1, 0, 0, 0, - 1, 1, 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1) - remote_control_boards ("head", "torso", "left_arm", "right_arm", "left_leg", "right_leg") # filters @@ -24,3 +17,19 @@ joint_velocity_cut_frequency 10.0 use_wrench_filter 0 wrench_cut_frequency 10.0 + +# if true the joint is in stiff mode if false the joint is in compliant mode +joint_is_stiff_mode (true, true, true, + true, true, true, + true, true, true, true, true, true, true, + true, true, true, true, true, true, true, + true, true, true, true, true, true, + true, true, true, true, true, true) + +# if true a good joint traking is considered mandatory +good_tracking_required (false, false, false, + true, true, true, + true, true, true, true, false, false, false, + true, true, true, true, false, false, false, + true, true, true, true, true, true, + true, true, true, true, true, true) diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/robotControl.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/robotControl.ini index cc4dbc84..c448c336 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/robotControl.ini @@ -6,20 +6,6 @@ joints_list ("torso_pitch", "torso_roll", "torso_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") -# if 1 the joint is in stiff mode if 0 the joint is in compliant mode -joint_is_stiff_mode (true, true, true, - true, true, true, true, - true, true, true, true, - true, true, true, true, true, true, - true, true, true, true, true, true) - -dangerous_joints (1, 1, 1, - 1, 1, 1, 1, - 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1) - - remote_control_boards ("torso", "left_arm", "right_arm", "left_leg", "right_leg") # Base data robot port name of Gazebo @@ -32,3 +18,17 @@ joint_velocity_cut_frequency 10.0 use_wrench_filter 0 wrench_cut_frequency 10.0 + +# if true the joint is in stiff mode if false the joint is in compliant mode +joint_is_stiff_mode (true, true, true, + true, true, true, true, + true, true, true, true, + true, true, true, true, true, true, + true, true, true, true, true, true) + +# if true a good joint traking is considered mandatory +good_tracking_required (true, true, true, + true, true, true, true, + true, true, true, true, + true, true, true, true, true, true, + true, true, true, true, true, true) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini index d4b579bf..d9157373 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini @@ -58,3 +58,7 @@ startAlwaysSameFoot 1 ##Remove this line if you don't want to use the minimum jerk trajectory in feet interpolation #useMinimumJerkFootTrajectory 1 + +## Delay of the beginning of the stance phase in seconds. If you remove the +## following line the delay will be zero +stance_phase_delay 1.0 diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/robotControl.ini index e50c7ad1..b1381671 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/robotControl.ini @@ -7,19 +7,6 @@ joints_list ("torso_pitch", "torso_roll", "torso_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") -dangerous_joints (1, 1, 1, - 1, 1, 1, 1, 0, 0, 0, - 1, 1, 1, 1, 0, 0, 0, - 1, 1, 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1) - -# if 1 the joint is in stiff mode if 0 the joint is in compliant mode -joint_is_stiff_mode (true, true, true, - true, true, true, true, true, true,true, - true, true, true, true, true, true,true, - true, true, true, true, true, true, - true, true, true, true, true, true) - remote_control_boards ("torso", "left_arm", "right_arm", "left_leg", "right_leg") # filters @@ -29,3 +16,17 @@ joint_velocity_cut_frequency 10.0 use_wrench_filter 0 wrench_cut_frequency 10.0 + +# if true the joint is in stiff mode if false the joint is in compliant mode +joint_is_stiff_mode (true, true, true, + true, true, true, true, true, true, true, + true, true, true, true, true, true, true, + true, true, true, true, true, true, + true, true, true, true, true, true) + +# if true a good joint traking is considered mandatory +good_tracking_required (true, true, true, + true, true, true, true, false, false, false, + true, true, true, true, false, false, false, + true, true, true, true, true, true, + true, true, true, true, true, true) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini index d0ba4205..00c43364 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini @@ -58,3 +58,7 @@ startAlwaysSameFoot 1 ##Remove this line if you don't want to use the minimum jerk trajectory in feet interpolation #useMinimumJerkFootTrajectory 1 + +## Delay of the beginning of the stance phase in seconds. If you remove the +## following line the delay will be zero +stance_phase_delay 1.0 diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini index ef60dfa7..ab075edd 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/robotControl.ini @@ -2,21 +2,11 @@ robot icub joints_list ("neck_pitch", "neck_roll", "neck_yaw", - "torso_pitch", "torso_roll", "torso_yaw", - "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", - "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") - -dangerous_joints (0, 0, 0, - 1, 1, 1, - 1, 1, 1, 1, 0, - 1, 1, 1, 1, 0, - 1, 1, 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1) - - remote_control_boards ("head", "torso", "left_arm", "right_arm", "left_leg", "right_leg") # filters @@ -26,3 +16,20 @@ joint_velocity_cut_frequency 10.0 use_wrench_filter 0 wrench_cut_frequency 10.0 + + +# if true the joint is in stiff mode if false the joint is in compliant mode +joint_is_stiff_mode (true, true, true, + true, true, true, + true, true, true, true, true, true, true, + true, true, true, true, true, true, true, + true, true, true, true, true, true, + true, true, true, true, true, true) + +# if true a good joint traking is considered mandatory +good_tracking_required (false, false, false, + true, true, true, + true, true, true, true, false, false, false, + true, true, true, true, false, false, false, + true, true, true, true, true, true, + true, true, true, true, true, true) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/robotControl.ini index 236a2de4..68515325 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/robotControl.ini @@ -6,22 +6,9 @@ joints_list ("torso_pitch", "torso_roll", "torso_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") -dangerous_joints (1, 1, 1, - 1, 1, 1, 1, - 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1) - remote_control_boards ("torso", "left_arm", "right_arm", "left_leg", "right_leg") -# if 1 the joint is in stiff mode if 0 the joint is in compliant mode -joint_is_stiff_mode (true, true, true, - true, true, true, true, - true, true, true, true, - true, true, true, true, true, true, - true, true, true, true, true, true) - # filters # if use_*_filter is equal to 0 the low pass filters are not used use_joint_velocity_filter 0 @@ -29,3 +16,17 @@ joint_velocity_cut_frequency 10.0 use_wrench_filter 0 wrench_cut_frequency 10.0 + +# if true the joint is in stiff mode if false the joint is in compliant mode +joint_is_stiff_mode (true, true, true, + true, true, true, true, + true, true, true, true, + true, true, true, true, true, true, + true, true, true, true, true, true) + +# if true a good joint traking is considered mandatory +good_tracking_required (true, true, true, + true, true, true, true, + true, true, true, true, + true, true, true, true, true, true, + true, true, true, true, true, true) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini index 7c3f2d62..3d5d6ec0 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini @@ -11,8 +11,6 @@ use_QP-IK 1 # remove this line if you don't want to save data of the experiment #dump_data 1 -stance_phase_time_out 1 - # general parameters [GENERAL] name walking-coordinator From f0eccade2f144e25beb621ae75610685e5678ba9 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 23 Apr 2020 17:21:48 +0200 Subject: [PATCH 36/58] Fix code style in RetargetingHelper/Helper.h --- .../WalkingControllers/RetargetingHelper/Helper.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index 1dfcd3b0..98bc26a7 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -36,14 +36,14 @@ namespace WalkingControllers { private: - typedef struct + struct RetargetingElement { yarp::sig::Vector yarpVector; std::unique_ptr smoother; yarp::os::BufferedPort port; double smoothingTimeInApproaching; double smoothingTimeInWalking; - } retargetingElement; + }; bool m_useHandRetargeting; /**< True if the hand retargeting is used */ bool m_useVirtualizer; /**< True if the virtualizer is used */ @@ -51,20 +51,20 @@ namespace WalkingControllers bool m_useCoMHeightRetargeting; /**< True if the com retargeting is used */ iDynTree::Transform m_leftHandTransform; /**< Desired left hand transform */ - retargetingElement m_leftHand; /**< Left hand retargeting element */ + RetargetingElement m_leftHand; /**< Left hand retargeting element */ iDynTree::Transform m_rightHandTransform; /**< Desired right hand transform */ - retargetingElement m_rightHand; /**< Right hand retargeting element */ + RetargetingElement m_rightHand; /**< Right hand retargeting element */ double m_comHeightValue; double m_comHeightInputZero; double m_comHeightVelocity; double m_comConstantHeight; - retargetingElement m_comHeight; + RetargetingElement m_comHeight; std::vector m_retargetJointsIndex; /**< Vector containing the indices of the retarget joints. */ iDynTree::VectorDynSize m_jointRetargetingValue; /**< Values of the retarget Joints. */ - retargetingElement m_jointRetargeting; /**< Joint retargeting element */ + RetargetingElement m_jointRetargeting; /**< Joint retargeting element */ yarp::os::BufferedPort m_robotOrientationPort; /**< Average orientation of the robot.*/ From 02da2513df4056f2bc32fa2339f4ff79c2e6b60c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 23 Apr 2020 17:25:59 +0200 Subject: [PATCH 37/58] Revert "change the PID sign according to robotology/robots-configuration@cae6f4407a4e54f3d564ec6a9725e4540d04683a" This reverts commit 89faa2a2ad29da872b537aa2ef1dde21b13ffe46. --- .../dcm_walking/common/pidParams.ini | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/pidParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/pidParams.ini index 17036470..bdc5b912 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/pidParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/pidParams.ini @@ -16,27 +16,27 @@ smoothingTime 0.05 [DEFAULT] #NAME P I D -torso_yaw 6000.0 7111.0 0.0 -torso_roll 9000.0 10666.0 0.0 -torso_pitch 9000.0 14222.0 0.0 +torso_yaw -6000.0 -7111.0 0.0 +torso_roll -9000.0 -10666.0 0.0 +torso_pitch -9000.0 -14222.0 0.0 l_shoulder_pitch 2000.0 7111.0 0.0 #l_shoulder_roll 2166.0 10666.0 0.0 l_shoulder_yaw 1711.1 7111.0 0.0 -r_shoulder_pitch 2000.0 7111.0 0.0 -#r_shoulder_roll 2066.0 10666.0 0.0 -r_shoulder_yaw 1711.1 7111.0 0.0 -l_hip_pitch 4000.0 7000.0 100.0 +r_shoulder_pitch -2000.0 -7111.0 0.0 +#r_shoulder_roll -2066.0 -10666.0 0.0 +r_shoulder_yaw -1711.1 -7111.0 0.0 +l_hip_pitch -4000.0 -7000.0 -100.0 l_hip_roll 4500.0 8000.0 0.0 -l_hip_yaw 3011.0 100.0 0.0 -l_knee 9000.0 800.0 0.0 +l_hip_yaw -3011.0 -100.0 0.0 +l_knee -9000.0 -800.0 0.0 l_ankle_pitch 4000.0 200.0 0.0 l_ankle_roll 6000.0 200.0 0.0 r_hip_pitch 4000.0 7000.0 100.0 -r_hip_roll 4500.0 8000.0 0.0 +r_hip_roll -4500.0 -8000.0 0.0 r_hip_yaw 3011.0 100.0 0.0 r_knee 9000.0 800.0 0.0 -r_ankle_pitch 4000.0 200.0 0.0 -r_ankle_roll 6000.0 200.0 0.0 +r_ankle_pitch -4000.0 -200.0 0.0 +r_ankle_roll -6000.0 -200.0 0.0 [PREIMPACT_LEFT] activationPhase SWING_LEFT @@ -49,8 +49,8 @@ l_ankle_roll 2000.0 100.0 0.0 activationPhase SWING_RIGHT activationOffset 0.3 #NAME P I D -r_ankle_pitch 1000.0 100.0 0.0 -r_ankle_roll 2000.0 100.0 0.0 +r_ankle_pitch -1000.0 -100.0 0.0 +r_ankle_roll -2000.0 -100.0 0.0 # The next reset the original gains. [AFTER_IMPACT] From 2fdcdfaffe01255cd707c2ffd89a78128bd4615e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 24 Apr 2020 00:08:37 +0200 Subject: [PATCH 38/58] Remove duplicated code in RetargetingHelper component --- .../RetargetingHelper/Helper.h | 21 +-- src/RetargetingHelper/src/Helper.cpp | 136 +++++++++--------- 2 files changed, 76 insertions(+), 81 deletions(-) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index 98bc26a7..af157593 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -36,6 +36,7 @@ namespace WalkingControllers { private: + template struct RetargetingElement { yarp::sig::Vector yarpVector; @@ -43,6 +44,7 @@ namespace WalkingControllers yarp::os::BufferedPort port; double smoothingTimeInApproaching; double smoothingTimeInWalking; + Data data; }; bool m_useHandRetargeting; /**< True if the hand retargeting is used */ @@ -50,21 +52,20 @@ namespace WalkingControllers bool m_useJointRetargeting; /**< True if the joint retargeting is used */ bool m_useCoMHeightRetargeting; /**< True if the com retargeting is used */ - iDynTree::Transform m_leftHandTransform; /**< Desired left hand transform */ - RetargetingElement m_leftHand; /**< Left hand retargeting element */ + RetargetingElement m_leftHand; /**< Left hand retargeting element */ + RetargetingElement m_rightHand; /**< Right hand retargeting element */ - iDynTree::Transform m_rightHandTransform; /**< Desired right hand transform */ - RetargetingElement m_rightHand; /**< Right hand retargeting element */ - - double m_comHeightValue; double m_comHeightInputZero; - double m_comHeightVelocity; double m_comConstantHeight; - RetargetingElement m_comHeight; + struct SecondOrder + { + double position; + double velocity; + }; + RetargetingElement m_comHeight; std::vector m_retargetJointsIndex; /**< Vector containing the indices of the retarget joints. */ - iDynTree::VectorDynSize m_jointRetargetingValue; /**< Values of the retarget Joints. */ - RetargetingElement m_jointRetargeting; /**< Joint retargeting element */ + RetargetingElement m_jointRetargeting; /**< Joint retargeting element */ yarp::os::BufferedPort m_robotOrientationPort; /**< Average orientation of the robot.*/ diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index 95229e13..850120e5 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -53,7 +53,7 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, return false; } - m_jointRetargetingValue.resize(controlledJointsName.size()); + m_jointRetargeting.data.resize(controlledJointsName.size()); m_jointRetargeting.yarpVector.resize(controlledJointsName.size()); if(!m_useHandRetargeting && !m_useVirtualizer && @@ -70,52 +70,48 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, } std::string portName; + double smoothingTimeApproching; + double smoothingTimeWalking; if(m_useHandRetargeting) { const yarp::os::Bottle& option = config.findGroup("HAND_RETARGETING"); - // open left hand port - if(!YarpUtilities::getStringFromSearchable(option, "left_hand_transform_port_name", - portName)) + auto initializeHand = [&](auto& hand, const std::string& portNameLabel) -> bool { - yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; - return false; - } - m_leftHand.port.open("/" + name + portName); + // open left hand port + if(!YarpUtilities::getStringFromSearchable(config, portNameLabel, + portName)) + { + yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; + return false; + } + hand.port.open("/" + name + portName); - // open right hand port - if(!YarpUtilities::getStringFromSearchable(option, "right_hand_transform_port_name", - portName)) - { - yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; - return false; - } - m_rightHand.port.open("/" + name + portName); - m_leftHand.yarpVector.resize(6); - m_rightHand.yarpVector.resize(6); + if(!YarpUtilities::getNumberFromSearchable(config, "smoothing_time_approaching", smoothingTimeApproching)) + { + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + return false; + } + hand.smoothingTimeInApproaching = smoothingTimeApproching; - double smoothingTimeApproching; - if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_approaching", smoothingTimeApproching)) - { - yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; - return false; - } - m_leftHand.smoothingTimeInApproaching = smoothingTimeApproching; - m_rightHand.smoothingTimeInApproaching = smoothingTimeApproching; + if(!YarpUtilities::getNumberFromSearchable(config, "smoothing_time_walking", smoothingTimeWalking)) + { + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + return false; + } + hand.smoothingTimeInWalking = smoothingTimeWalking; - double smoothingTimeWalking; - if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_walking", smoothingTimeWalking)) - { - yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; - return false; - } - m_leftHand.smoothingTimeInWalking = smoothingTimeWalking; - m_rightHand.smoothingTimeInWalking = smoothingTimeWalking; + hand.yarpVector.resize(6); + hand.smoother = std::make_unique(6, period, hand.smoothingTimeInApproaching); + + return true; + }; - m_leftHand.smoother = std::make_unique(6, period, m_leftHand.smoothingTimeInApproaching); - m_rightHand.smoother = std::make_unique(6, period, m_rightHand.smoothingTimeInApproaching); + if(!initializeHand(m_leftHand, "left_hand_transform_port_name") + || !initializeHand(m_rightHand, "right_hand_transform_port_name")) + return false; } if(m_useJointRetargeting) @@ -219,35 +215,34 @@ bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, const iDynTree::VectorDynSize& jointValues, const double& comHeight) { - m_leftHandTransform = leftHandTransform; - m_rightHandTransform = rightHandTransform; + m_leftHand.data = leftHandTransform; + m_rightHand.data = rightHandTransform; if(m_useHandRetargeting) { - iDynTree::toEigen(m_leftHand.yarpVector).segment<3>(0) = - iDynTree::toEigen(m_leftHandTransform.getPosition()); - - iDynTree::toEigen(m_leftHand.yarpVector).segment<3>(3) = - iDynTree::toEigen(m_leftHandTransform.getRotation().asRPY()); + auto resetHandSmoother = [](auto& hand) + { + iDynTree::toEigen(hand.yarpVector).template segment<3>(0) = + iDynTree::toEigen(hand.data.getPosition()); - iDynTree::toEigen(m_rightHand.yarpVector).segment<3>(0) = - iDynTree::toEigen(m_rightHandTransform.getPosition()); + iDynTree::toEigen(hand.yarpVector).template segment<3>(3) = + iDynTree::toEigen(hand.data.getRotation().asRPY()); - iDynTree::toEigen(m_rightHand.yarpVector).segment<3>(3) = - iDynTree::toEigen(m_rightHandTransform.getRotation().asRPY()); + hand.smoother->init(hand.yarpVector); + }; - m_leftHand.smoother->init(m_leftHand.yarpVector); - m_rightHand.smoother->init(m_rightHand.yarpVector); + resetHandSmoother(m_leftHand); + resetHandSmoother(m_rightHand); } // joint retargeting - m_jointRetargetingValue = jointValues; + m_jointRetargeting.data = jointValues; iDynTree::toEigen(m_jointRetargeting.yarpVector) = iDynTree::toEigen(jointValues); if (m_useJointRetargeting) m_jointRetargeting.smoother->init(m_jointRetargeting.yarpVector); - m_comHeightValue = comHeight; - m_comHeightVelocity = 0; + m_comHeight.data.position = comHeight; + m_comHeight.data.velocity = 0; m_comConstantHeight = comHeight; if(m_useCoMHeightRetargeting) @@ -289,19 +284,18 @@ void RetargetingClient::getFeedback() { if(m_useHandRetargeting) { - auto desiredLeftHandPose = m_leftHand.port.read(false); - if(desiredLeftHandPose != nullptr) - m_leftHand.yarpVector = *desiredLeftHandPose; - - m_leftHand.smoother->computeNextValues(m_leftHand.yarpVector); - convertYarpVectorPoseIntoTransform(m_leftHand.smoother->getPos(), m_leftHandTransform); + auto getHandFeedback = [this](auto& hand) + { + auto desiredHandPose = hand.port.read(false); + if(desiredHandPose != nullptr) + hand.yarpVector = *desiredHandPose; - auto desiredRightHandPose = m_rightHand.port.read(false); - if(desiredRightHandPose != nullptr) - m_rightHand.yarpVector = *desiredRightHandPose; + hand.smoother->computeNextValues(hand.yarpVector); + convertYarpVectorPoseIntoTransform(hand.smoother->getPos(), hand.data); + }; - m_rightHand.smoother->computeNextValues(m_rightHand.yarpVector); - convertYarpVectorPoseIntoTransform(m_rightHand.smoother->getPos(), m_rightHandTransform); + getHandFeedback(m_leftHand); + getHandFeedback(m_rightHand); } if(m_useJointRetargeting) @@ -314,7 +308,7 @@ void RetargetingClient::getFeedback() } m_jointRetargeting.smoother->computeNextValues(m_jointRetargeting.yarpVector); - iDynTree::toEigen(m_jointRetargetingValue) = iDynTree::toEigen(m_jointRetargeting.smoother->getPos()); + iDynTree::toEigen(m_jointRetargeting.data) = iDynTree::toEigen(m_jointRetargeting.smoother->getPos()); } @@ -332,8 +326,8 @@ void RetargetingClient::getFeedback() } m_comHeight.smoother->computeNextValues(m_comHeight.yarpVector); - m_comHeightValue = m_comHeight.smoother->getPos()(0); - m_comHeightVelocity = m_comHeight.smoother->getVel()(0); + m_comHeight.data.position = m_comHeight.smoother->getPos()(0); + m_comHeight.data.velocity = m_comHeight.smoother->getVel()(0); } // check if the approaching phase is finished @@ -349,27 +343,27 @@ void RetargetingClient::getFeedback() const iDynTree::Transform& RetargetingClient::leftHandTransform() const { - return m_leftHandTransform; + return m_leftHand.data; } const iDynTree::Transform& RetargetingClient::rightHandTransform() const { - return m_rightHandTransform; + return m_rightHand.data; } const iDynTree::VectorDynSize& RetargetingClient::jointValues() const { - return m_jointRetargetingValue; + return m_jointRetargeting.data; } double RetargetingClient::comHeight() const { - return m_comHeightValue; + return m_comHeight.data.position; } double RetargetingClient::comHeightVelocity() const { - return m_comHeightVelocity; + return m_comHeight.data.velocity; } void RetargetingClient::close() From 9f400ea02338a8457b87fba0879bf5b0aa8e6b5c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Apr 2020 11:24:09 +0200 Subject: [PATCH 39/58] Fix typos spotted by @S-Dafarra Co-Authored-By: Stefano Dafarra --- .../include/WalkingControllers/RetargetingHelper/Helper.h | 2 +- .../include/WalkingControllers/RobotInterface/Helper.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index af157593..83e5b9b1 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -64,7 +64,7 @@ namespace WalkingControllers }; RetargetingElement m_comHeight; - std::vector m_retargetJointsIndex; /**< Vector containing the indices of the retarget joints. */ + std::vector m_retargetJointsIndex; /**< Vector containing the indices of the retargeted joints. */ RetargetingElement m_jointRetargeting; /**< Joint retargeting element */ yarp::os::BufferedPort m_robotOrientationPort; /**< Average orientation of the robot.*/ diff --git a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h index 288f07a7..57f5fd8d 100644 --- a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h +++ b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h @@ -226,7 +226,7 @@ namespace WalkingControllers WalkingPIDHandler& getPIDHandler(); /** - * Set the intraction mode stored in the configuration + * Set the interaction mode stored in the configuration file * @return true in case of success and false otherwise. */ bool loadCustomInteractionMode(); From c3143c8eb96cdd9880c63bfca60135a759e7fe87 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Apr 2020 12:47:37 +0200 Subject: [PATCH 40/58] Fix typo in RetargetingHelper component Co-Authored-By: Stefano Dafarra --- src/RetargetingHelper/src/Helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index 850120e5..2152e84f 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -44,7 +44,7 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, m_useVirtualizer = config.check("use_virtualizer", yarp::os::Value(false)).asBool(); m_useCoMHeightRetargeting = config.check("use_com_retargeting", yarp::os::Value(false)).asBool(); - // The approaching phase is set only if the startApproacingPhase method is called + // The approaching phase is set only if the startApproachingPhase method is called m_isApproachingPhase = false; if(m_useJointRetargeting && m_useHandRetargeting) From a4621db3a494f27670b39fbab73f763d4fe67025 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Apr 2020 13:34:09 +0200 Subject: [PATCH 41/58] Throw a warning if the joint is in compliant mode, but it is also requested to have good tracking in RobotInterface component --- src/RobotInterface/src/Helper.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/RobotInterface/src/Helper.cpp b/src/RobotInterface/src/Helper.cpp index 5e4ab193..caa6eb7b 100644 --- a/src/RobotInterface/src/Helper.cpp +++ b/src/RobotInterface/src/Helper.cpp @@ -246,6 +246,16 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) } } + for (unsigned int i = 0; i < m_actuatedDOFs; i++) + { + if(m_jointInteractionMode[i] == yarp::dev::InteractionModeEnum::VOCAB_IM_COMPLIANT + && m_isGoodTrackingRequired[i]) + { + yWarning() << "[configureRobot] The control mode of the the joint " << m_axesList[i] + << " is set to COMPLIANT. It is not possible to guarantee a good tracking."; + } + } + // open the device if(!m_robotDevice.open(options)) { @@ -911,4 +921,3 @@ bool RobotInterface::loadCustomInteractionMode() { return setInteractionMode(m_jointInteractionMode); } - From 8026c26afcf2c23bbdad12e1f8556614cf194469 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Apr 2020 13:41:38 +0200 Subject: [PATCH 42/58] Fix documentation in TrajectoryGenerator class --- .../TrajectoryPlanner/TrajectoryGenerator.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h index 5beca5e4..1ed20519 100644 --- a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h +++ b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h @@ -228,11 +228,10 @@ namespace WalkingControllers bool getWeightPercentage(std::vector &weightInLeft, std::vector &weightInRight); /** - * Get the weight percentage for the left and right foot - * @param weightInLeft vector containing the weight on the left foot (0 in case in case of - * stance foot during SS, 1 in case of swing foot) - * @param weightInRight vector containing the weight on the right foot (0 in case in case of - * stance foot during SS, 1 in case of swing foot) + * Get the desired current state of the robot. If the robot is not walking (i.e. the DCM + * velocity is almost zero), it is considered in "stance" phase. + * @param isStancePhase vector containing if the robot is in the stance phase during the + * entire horizon. * @return true/false in case of success/failure. */ bool getIsStancePhase(std::vector& isStancePhase); From 0acf4fa3f1772b3986c4af5f423cc1aaa5649bac Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Apr 2020 15:03:34 +0200 Subject: [PATCH 43/58] Update RetargetingHelper component according to @S-Dafarra's suggestion --- .../RetargetingHelper/Helper.h | 19 +++++-- src/RetargetingHelper/src/Helper.cpp | 55 +++++++++++-------- 2 files changed, 45 insertions(+), 29 deletions(-) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index 83e5b9b1..5e8dc74f 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -39,7 +39,7 @@ namespace WalkingControllers template struct RetargetingElement { - yarp::sig::Vector yarpVector; + yarp::sig::Vector yarpReadBuffer; std::unique_ptr smoother; yarp::os::BufferedPort port; double smoothingTimeInApproaching; @@ -55,14 +55,23 @@ namespace WalkingControllers RetargetingElement m_leftHand; /**< Left hand retargeting element */ RetargetingElement m_rightHand; /**< Right hand retargeting element */ - double m_comHeightInputZero; + /** Offset of the CoM Height coming from the user. It is required given the different size + * between the human and the robot */ + double m_comHeightInputOffset; + + /** Desired value of the CoM height used during walking. The simplified model used for + * the locomotion is based on the assumption of a constant CoM height */ double m_comConstantHeight; - struct SecondOrder + + /** Factor required to scale the human CoM displacement to a desired robot CoM displacement */ + double m_comHeightScalingFactor; + + struct KinematicState { double position; double velocity; }; - RetargetingElement m_comHeight; + RetargetingElement m_comHeight; std::vector m_retargetJointsIndex; /**< Vector containing the indices of the retargeted joints. */ RetargetingElement m_jointRetargeting; /**< Joint retargeting element */ @@ -101,7 +110,7 @@ namespace WalkingControllers bool initialize(const yarp::os::Searchable &config, const std::string &name, const double &period, - const std::vector& controlledJointsName); + const std::vector& controlledJointNames); /** * Reset the client diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index 2152e84f..cb3efddc 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -27,7 +27,7 @@ void RetargetingClient::convertYarpVectorPoseIntoTransform(const yarp::sig::Vect bool RetargetingClient::initialize(const yarp::os::Searchable &config, const std::string &name, const double &period, - const std::vector& controlledJointsName) + const std::vector& controlledJointNames) { if(config.isNull()) { @@ -53,8 +53,8 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, return false; } - m_jointRetargeting.data.resize(controlledJointsName.size()); - m_jointRetargeting.yarpVector.resize(controlledJointsName.size()); + m_jointRetargeting.data.resize(controlledJointNames.size()); + m_jointRetargeting.yarpReadBuffer.resize(controlledJointNames.size()); if(!m_useHandRetargeting && !m_useVirtualizer && !m_useJointRetargeting && !m_useCoMHeightRetargeting) @@ -103,7 +103,7 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, } hand.smoothingTimeInWalking = smoothingTimeWalking; - hand.yarpVector.resize(6); + hand.yarpReadBuffer.resize(6); hand.smoother = std::make_unique(6, period, hand.smoothingTimeInApproaching); return true; @@ -134,8 +134,8 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, // find the indices for(const std::string& joint : retargetJointNames) { - int index = std::distance(controlledJointsName.begin(), std::find(controlledJointsName.begin(), - controlledJointsName.end(), + int index = std::distance(controlledJointNames.begin(), std::find(controlledJointNames.begin(), + controlledJointNames.end(), joint)); m_retargetJointsIndex.push_back(index); } @@ -161,7 +161,7 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, return false; } - m_jointRetargeting.smoother = std::make_unique(controlledJointsName.size(), period, + m_jointRetargeting.smoother = std::make_unique(controlledJointNames.size(), period, m_jointRetargeting.smoothingTimeInApproaching); } @@ -183,7 +183,7 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, { const yarp::os::Bottle& option = config.findGroup("COM_RETARGETING"); - m_comHeight.yarpVector.resize(1); + m_comHeight.yarpReadBuffer.resize(1); if(!YarpUtilities::getStringFromSearchable(option, "com_height_retargeting_port_name", portName)) { @@ -205,6 +205,13 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, } m_comHeight.smoother = std::make_unique(1, period, m_comHeight.smoothingTimeInApproaching); + + if(!YarpUtilities::getNumberFromSearchable(option, "com_height_scaling_factor", + m_comHeightScalingFactor)) + { + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + return false; + } } return true; @@ -222,13 +229,13 @@ bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, { auto resetHandSmoother = [](auto& hand) { - iDynTree::toEigen(hand.yarpVector).template segment<3>(0) = + iDynTree::toEigen(hand.yarpReadBuffer).template segment<3>(0) = iDynTree::toEigen(hand.data.getPosition()); - iDynTree::toEigen(hand.yarpVector).template segment<3>(3) = + iDynTree::toEigen(hand.yarpReadBuffer).template segment<3>(3) = iDynTree::toEigen(hand.data.getRotation().asRPY()); - hand.smoother->init(hand.yarpVector); + hand.smoother->init(hand.yarpReadBuffer); }; resetHandSmoother(m_leftHand); @@ -237,9 +244,9 @@ bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, // joint retargeting m_jointRetargeting.data = jointValues; - iDynTree::toEigen(m_jointRetargeting.yarpVector) = iDynTree::toEigen(jointValues); + iDynTree::toEigen(m_jointRetargeting.yarpReadBuffer) = iDynTree::toEigen(jointValues); if (m_useJointRetargeting) - m_jointRetargeting.smoother->init(m_jointRetargeting.yarpVector); + m_jointRetargeting.smoother->init(m_jointRetargeting.yarpReadBuffer); m_comHeight.data.position = comHeight; m_comHeight.data.velocity = 0; @@ -247,8 +254,8 @@ bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, if(m_useCoMHeightRetargeting) { - m_comHeight.yarpVector(0) = comHeight; - m_comHeight.smoother->init(m_comHeight.yarpVector); + m_comHeight.yarpReadBuffer(0) = comHeight; + m_comHeight.smoother->init(m_comHeight.yarpReadBuffer); // let's read the port to reset the comHeightInput bool okCoMHeight = false; @@ -260,7 +267,7 @@ bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, auto desiredCoMHeight = m_comHeight.port.read(false); if(desiredCoMHeight != nullptr) { - m_comHeightInputZero = (*desiredCoMHeight)(2); + m_comHeightInputOffset = (*desiredCoMHeight)(2); okCoMHeight = true; } } @@ -288,9 +295,9 @@ void RetargetingClient::getFeedback() { auto desiredHandPose = hand.port.read(false); if(desiredHandPose != nullptr) - hand.yarpVector = *desiredHandPose; + hand.yarpReadBuffer = *desiredHandPose; - hand.smoother->computeNextValues(hand.yarpVector); + hand.smoother->computeNextValues(hand.yarpReadBuffer); convertYarpVectorPoseIntoTransform(hand.smoother->getPos(), hand.data); }; @@ -304,10 +311,10 @@ void RetargetingClient::getFeedback() if(desiredJoint != nullptr) { for(int i =0; i < desiredJoint->size(); i++) - m_jointRetargeting.yarpVector(m_retargetJointsIndex[i]) = (*desiredJoint)(i); + m_jointRetargeting.yarpReadBuffer(m_retargetJointsIndex[i]) = (*desiredJoint)(i); } - m_jointRetargeting.smoother->computeNextValues(m_jointRetargeting.yarpVector); + m_jointRetargeting.smoother->computeNextValues(m_jointRetargeting.yarpReadBuffer); iDynTree::toEigen(m_jointRetargeting.data) = iDynTree::toEigen(m_jointRetargeting.smoother->getPos()); } @@ -315,17 +322,17 @@ void RetargetingClient::getFeedback() if(m_useCoMHeightRetargeting) { if(!m_isStancePhase) - m_comHeight.yarpVector(0) = m_comConstantHeight; + m_comHeight.yarpReadBuffer(0) = m_comConstantHeight; else { auto desiredCoMHeight = m_comHeight.port.read(false); if(desiredCoMHeight != nullptr) - m_comHeight.yarpVector(0) = ((*desiredCoMHeight)(2) - m_comHeightInputZero) * 0.5 - + m_comConstantHeight; + m_comHeight.yarpReadBuffer(0) = ((*desiredCoMHeight)(2) - m_comHeightInputOffset) + * m_comHeightScalingFactor + m_comConstantHeight; } - m_comHeight.smoother->computeNextValues(m_comHeight.yarpVector); + m_comHeight.smoother->computeNextValues(m_comHeight.yarpReadBuffer); m_comHeight.data.position = m_comHeight.smoother->getPos()(0); m_comHeight.data.velocity = m_comHeight.smoother->getVel()(0); } From e171fc6eccce0acb23d51ce9527316dae4d7e886 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Apr 2020 15:05:20 +0200 Subject: [PATCH 44/58] Remove useless comment in TrajectoryGenerator.cpp --- src/TrajectoryPlanner/src/TrajectoryGenerator.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp index ce1e4cef..850e70f6 100644 --- a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp +++ b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp @@ -55,8 +55,6 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) m_dT = config.check("sampling_time", yarp::os::Value(0.016)).asDouble(); m_plannerHorizon = config.check("plannerHorizon", yarp::os::Value(20.0)).asDouble(); double unicycleGain = config.check("unicycleGain", yarp::os::Value(10.0)).asDouble(); - - // TODO please remove me. Counters are evil double stancePhaseDelaySeconds = config.check("stance_phase_delay",yarp::os::Value(0.0)).asDouble(); m_stancePhaseDelay = (std::size_t) std::round(stancePhaseDelaySeconds / m_dT); From d549a26646bd8bd14556554079d424e88f6d86ef Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Apr 2020 15:09:02 +0200 Subject: [PATCH 45/58] Add the com_height_scaling_factor parameter in jointRetargeting.ini configuration file --- .../dcm_walking/joint_retargeting/jointRetargeting.ini | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini index b4129722..314b695b 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini @@ -20,4 +20,5 @@ robot_orientation_port_name /torsoYaw:o [COM_RETARGETING] com_height_retargeting_port_name /CoM:i smoothing_time_approaching 2.0 -smoothing_time_walking 1.0 \ No newline at end of file +smoothing_time_walking 1.0 +com_height_scaling_factor 0.5 From c6cb27271ec37ad0be74ffa6039612025ed4a94a Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Apr 2020 16:39:39 +0200 Subject: [PATCH 46/58] Implement getJointPos method in WalkingFK class --- .../include/WalkingControllers/KinDynWrapper/Wrapper.h | 6 ++++++ src/KinDynWrapper/src/Wrapper.cpp | 10 ++++++++++ 2 files changed, 16 insertions(+) diff --git a/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h b/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h index 5f395d30..33e34f59 100644 --- a/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h +++ b/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h @@ -258,6 +258,12 @@ namespace WalkingControllers * @return true/false in case of success/failure. */ bool getCoMJacobian(iDynTree::MatrixDynSize &jacobian); + + /** + * Get the joint position + * @return the joint position expressed in radians + */ + iDynTree::VectorDynSize getJointPos(); }; }; #endif diff --git a/src/KinDynWrapper/src/Wrapper.cpp b/src/KinDynWrapper/src/Wrapper.cpp index d769a76f..57dfdcb7 100644 --- a/src/KinDynWrapper/src/Wrapper.cpp +++ b/src/KinDynWrapper/src/Wrapper.cpp @@ -517,3 +517,13 @@ bool WalkingFK::getCoMJacobian(iDynTree::MatrixDynSize &jacobian) { return m_kinDyn.getCenterOfMassJacobian(jacobian); } + +iDynTree::VectorDynSize WalkingFK::getJointPos() +{ + iDynTree::VectorDynSize jointPos; + bool ok = m_kinDyn.getJointPos(jointPos); + + assert(ok); + + return jointPos; +} From 3a1b89f3093940864d754afc6a0f9454e9d6b4b3 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Apr 2020 16:41:16 +0200 Subject: [PATCH 47/58] Pass the KinDynWrapper to reset the RetargetingHelper --- src/RetargetingHelper/CMakeLists.txt | 1 + .../RetargetingHelper/Helper.h | 13 +++++-------- src/RetargetingHelper/src/Helper.cpp | 19 ++++++++----------- src/WalkingModule/src/Module.cpp | 8 +------- .../src/QPInverseKinematics.cpp | 1 - 5 files changed, 15 insertions(+), 27 deletions(-) diff --git a/src/RetargetingHelper/CMakeLists.txt b/src/RetargetingHelper/CMakeLists.txt index 35ae84d8..740e8f12 100644 --- a/src/RetargetingHelper/CMakeLists.txt +++ b/src/RetargetingHelper/CMakeLists.txt @@ -24,6 +24,7 @@ if(WALKING_CONTROLLERS_COMPILE_RetargetingHelper) target_link_libraries(${LIBRARY_TARGET_NAME} PUBLIC WalkingControllers::YarpUtilities + WalkingControllers::KinDynWrapper ${iDynTree_LIBRARIES} ctrlLib) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index 5e8dc74f..62c56750 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -26,6 +26,8 @@ // iCub-ctrl #include +#include + namespace WalkingControllers { @@ -114,16 +116,11 @@ namespace WalkingControllers /** * Reset the client - * @param leftHandTransform head_T_leftHand transform - * @param rightHandTransform head_T_rightHand transform - * @param jointValues joint values [rad] - * @param comHeight height of the CoM + * @param kinDynWrapper a wrapper of KinDynComputations useful to get some + * informations of the current status of the robot * @return true/false in case of success/failure */ - bool reset(const iDynTree::Transform& leftHandTransform, - const iDynTree::Transform& rightHandTransform, - const iDynTree::VectorDynSize& jointValues, - const double& comHeight); + bool reset(WalkingFK& kinDynWrapper); /** * Close the client diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index cb3efddc..167d2aaa 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -217,13 +217,10 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, return true; } -bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, - const iDynTree::Transform& rightHandTransform, - const iDynTree::VectorDynSize& jointValues, - const double& comHeight) +bool RetargetingClient::reset(WalkingFK& kinDynWrapper) { - m_leftHand.data = leftHandTransform; - m_rightHand.data = rightHandTransform; + m_leftHand.data = kinDynWrapper.getLeftHandToWorldTransform(); + m_rightHand.data = kinDynWrapper.getRightHandToWorldTransform(); if(m_useHandRetargeting) { @@ -243,18 +240,18 @@ bool RetargetingClient::reset(const iDynTree::Transform& leftHandTransform, } // joint retargeting - m_jointRetargeting.data = jointValues; - iDynTree::toEigen(m_jointRetargeting.yarpReadBuffer) = iDynTree::toEigen(jointValues); + m_jointRetargeting.data = kinDynWrapper.getJointPos(); + iDynTree::toEigen(m_jointRetargeting.yarpReadBuffer) = iDynTree::toEigen(m_jointRetargeting.data); if (m_useJointRetargeting) m_jointRetargeting.smoother->init(m_jointRetargeting.yarpReadBuffer); - m_comHeight.data.position = comHeight; + m_comHeight.data.position = kinDynWrapper.getCoMPosition()(2); m_comHeight.data.velocity = 0; - m_comConstantHeight = comHeight; + m_comConstantHeight = m_comHeight.data.position; if(m_useCoMHeightRetargeting) { - m_comHeight.yarpReadBuffer(0) = comHeight; + m_comHeight.yarpReadBuffer(0) = m_comHeight.data.position; m_comHeight.smoother->init(m_comHeight.yarpReadBuffer); // let's read the port to reset the comHeightInput diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index a869c19e..7b491ad7 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -509,12 +509,7 @@ bool WalkingModule::updateModule() return false; } - if(!m_retargetingClient->reset(m_FKSolver->getHeadToWorldTransform().inverse() - * m_FKSolver->getLeftHandToWorldTransform(), - m_FKSolver->getHeadToWorldTransform().inverse() - * m_FKSolver->getRightHandToWorldTransform(), - m_robotControlHelper->getJointPosition(), - m_comHeightTrajectory.front())) + if(!m_retargetingClient->reset(*m_FKSolver)) { yError() << "[WalkingModule::updateModule] Unable to reset the retargeting client."; return false; @@ -1358,4 +1353,3 @@ bool WalkingModule::stopWalking() m_robotState = WalkingFSM::Stopped; return true; } - diff --git a/src/WholeBodyControllers/src/QPInverseKinematics.cpp b/src/WholeBodyControllers/src/QPInverseKinematics.cpp index 2ab35ea4..1c3447e7 100644 --- a/src/WholeBodyControllers/src/QPInverseKinematics.cpp +++ b/src/WholeBodyControllers/src/QPInverseKinematics.cpp @@ -895,4 +895,3 @@ const iDynTree::VectorDynSize& WalkingQPIK::getGradient() const { return m_gradient; } - From ab9da12a431fbf7b77bd0fce5ae2d47d82556525 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Apr 2020 18:16:49 +0200 Subject: [PATCH 48/58] Pass the KinDynWrapper to setRobotState() in QPInverseKinematics class --- src/WalkingModule/src/Module.cpp | 10 +----- src/WholeBodyControllers/CMakeLists.txt | 1 + .../QPInverseKinematics.h | 18 +++-------- .../src/QPInverseKinematics.cpp | 31 ++++++------------- 4 files changed, 15 insertions(+), 45 deletions(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 7b491ad7..f0e1c9d6 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -379,15 +379,7 @@ bool WalkingModule::solveQPIK(const std::unique_ptr& solver, const { bool ok = true; solver->setPhase(m_isStancePhase.front()); - - ok &= solver->setRobotState(m_robotControlHelper->getJointPosition(), - m_FKSolver->getLeftFootToWorldTransform(), - m_FKSolver->getRightFootToWorldTransform(), - m_FKSolver->getLeftHandToWorldTransform(), - m_FKSolver->getRightHandToWorldTransform(), - m_FKSolver->getNeckOrientation(), - m_FKSolver->getCoMPosition()); - + ok &= solver->setRobotState(*m_FKSolver); solver->setDesiredNeckOrientation(desiredNeckOrientation.inverse()); solver->setDesiredFeetTransformation(m_leftTrajectory.front(), diff --git a/src/WholeBodyControllers/CMakeLists.txt b/src/WholeBodyControllers/CMakeLists.txt index 3dd5ea92..364229aa 100644 --- a/src/WholeBodyControllers/CMakeLists.txt +++ b/src/WholeBodyControllers/CMakeLists.txt @@ -38,6 +38,7 @@ if(WALKING_CONTROLLERS_COMPILE_WholeBodyControllers) target_link_libraries(${LIBRARY_TARGET_NAME} PUBLIC WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities + WalkingControllers::KinDynWrapper osqp::osqp OsqpEigen::OsqpEigen ${qpOASES_LIBRARIES} diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h index d5419b6c..ab756730 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h @@ -24,6 +24,7 @@ #include #include +#include namespace WalkingControllers { @@ -239,22 +240,11 @@ namespace WalkingControllers /** * Set the robot state. - * @param jointPosition vector of joint positions (in rad); - * @param leftFootToWorldTransform transformation between the inertial frame and the left foot; - * @param rightFootToWorldTransform transformation between the inertial frame and the right foot; - * @param leftHandToWorldTransform transformation between the inertial frame and the left foot; - * @param rightHandToWorldTransform transformation between the inertial frame and the right foot; - * @param neckOrientation rotation between the inertial frame and the neck; - * @param comPosition position of the CoM + * @param kinDynWrapper wrapper required to retrieve information related to the forward + * kinematics * @return true/false in case of success/failure. */ - bool setRobotState(const iDynTree::VectorDynSize& jointPosition, - const iDynTree::Transform& leftFootToWorldTransform, - const iDynTree::Transform& rightFootToWorldTransform, - const iDynTree::Transform& leftHandToWorldTransform, - const iDynTree::Transform& rightHandToWorldTransform, - const iDynTree::Rotation& neckOrientation, - const iDynTree::Position& comPosition); + bool setRobotState(WalkingFK& kinDynWrapper); /** * Set the Jacobian of the CoM diff --git a/src/WholeBodyControllers/src/QPInverseKinematics.cpp b/src/WholeBodyControllers/src/QPInverseKinematics.cpp index 1c3447e7..bf2ad168 100644 --- a/src/WholeBodyControllers/src/QPInverseKinematics.cpp +++ b/src/WholeBodyControllers/src/QPInverseKinematics.cpp @@ -184,7 +184,7 @@ bool WalkingQPIK::initializeMatrices(const yarp::os::Searchable& config) m_jointRegularizationGains.resize(m_actuatedDOFs); if(!YarpUtilities::getVectorFromSearchable(config, "joint_regularization_gains", - m_jointRegularizationGains)) + m_jointRegularizationGains)) { yError() << "Initialization failed while reading jointRegularizationGains vector."; return false; @@ -373,33 +373,20 @@ bool WalkingQPIK::initializeJointRetargeting(const yarp::os::Searchable& config) return true; } -bool WalkingQPIK::setRobotState(const iDynTree::VectorDynSize& jointPosition, - const iDynTree::Transform& leftFootToWorldTransform, - const iDynTree::Transform& rightFootToWorldTransform, - const iDynTree::Transform& leftHandToWorldTransform, - const iDynTree::Transform& rightHandToWorldTransform, - const iDynTree::Rotation& neckOrientation, - const iDynTree::Position& comPosition) +bool WalkingQPIK::setRobotState(WalkingFK& kinDynWrapper) { - if(jointPosition.size() != m_actuatedDOFs) - { - yError() << "[setRobotState] The size of the jointPosition vector is not coherent with the " - << "number of the actuated Joint"; - return false; - } - // avoid to copy the vector if the application is not ran in retargeting mode if(m_enableHandRetargeting) { - m_leftHandToWorldTransform = leftHandToWorldTransform; - m_rightHandToWorldTransform = rightHandToWorldTransform; + m_leftHandToWorldTransform = kinDynWrapper.getLeftHandToWorldTransform(); + m_rightHandToWorldTransform = kinDynWrapper.getRightHandToWorldTransform(); } - m_jointPosition = jointPosition; - m_leftFootToWorldTransform = leftFootToWorldTransform; - m_rightFootToWorldTransform = rightFootToWorldTransform; - m_neckOrientation = neckOrientation; - m_comPosition = comPosition; + m_jointPosition = kinDynWrapper.getJointPos(); + m_leftFootToWorldTransform = kinDynWrapper.getLeftFootToWorldTransform(); + m_rightFootToWorldTransform = kinDynWrapper.getRightFootToWorldTransform(); + m_neckOrientation = kinDynWrapper.getNeckOrientation(); + m_comPosition = kinDynWrapper.getCoMPosition(); return true; } From f7442cff90563d0294c24214332117865ad41652 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 30 Apr 2020 18:20:58 +0200 Subject: [PATCH 49/58] Improve the readibility of evaluateGradientVector() in WalkingQPIK class --- src/WholeBodyControllers/src/QPInverseKinematics.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/WholeBodyControllers/src/QPInverseKinematics.cpp b/src/WholeBodyControllers/src/QPInverseKinematics.cpp index bf2ad168..7df54f87 100644 --- a/src/WholeBodyControllers/src/QPInverseKinematics.cpp +++ b/src/WholeBodyControllers/src/QPInverseKinematics.cpp @@ -692,10 +692,12 @@ void WalkingQPIK::evaluateGradientVector() // g = Weight * K_p * (regularizationTerm - jointPosition) // Weight and K_p are two diagonal matrices so their product can be also evaluated multiplying component-wise // the elements of the vectors and then generating the diagonal matrix - gradient.tail(m_actuatedDOFs) += (-jointRegularizationGains.cwiseProduct( - iDynTree::toEigen(m_jointRegularizationWeightSmoother->getPos()))).asDiagonal() - * (regularizationTerm - jointPosition) - + (-jointRetargetingGains.cwiseProduct(iDynTree::toEigen(m_jointRetargetingWeightSmoother->getPos()))).asDiagonal() + const auto& jointRegularizationWeight = m_jointRegularizationWeightSmoother->getPos(); + gradient.tail(m_actuatedDOFs) += (-jointRegularizationGains.cwiseProduct(iDynTree::toEigen(jointRegularizationWeight))).asDiagonal() + * (regularizationTerm - jointPosition); + + const auto& jointRetargetingWeight = m_jointRetargetingWeightSmoother->getPos(); + gradient.tail(m_actuatedDOFs) += (-jointRetargetingGains.cwiseProduct(iDynTree::toEigen(jointRetargetingWeight))).asDiagonal() * (jointRetargetingValues - jointPosition); } From d86a5627f5cf62d6dc98c004db020986e5bfc564 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 May 2020 13:33:20 +0200 Subject: [PATCH 50/58] Use an enum to define the state in the retargeting client and consequently change the signature of RetargetingClient::setPhase --- .../RetargetingHelper/Helper.h | 14 +++++++--- src/RetargetingHelper/src/Helper.cpp | 27 ++++++++++++------- src/WalkingModule/src/Module.cpp | 12 +++++++-- 3 files changed, 37 insertions(+), 16 deletions(-) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index 62c56750..fbad9d6f 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -36,6 +36,14 @@ namespace WalkingControllers */ class RetargetingClient { + public: + enum class Phase + { + approacing, + stance, + walking + }; + private: template @@ -80,9 +88,7 @@ namespace WalkingControllers yarp::os::BufferedPort m_robotOrientationPort; /**< Average orientation of the robot.*/ - bool m_isStancePhase{true}; /**< True if the robot is not walking */ - - bool m_isApproachingPhase; /**< True if the robot is in the approaching phase */ + Phase m_phase{Phase::approacing}; double m_startingApproachingPhaseTime; /**< Initial time of the approaching phase (seconds) */ double m_approachPhaseDuration; /**< Duration of the approaching phase (seconds) */ @@ -165,7 +171,7 @@ namespace WalkingControllers void setRobotBaseOrientation(const iDynTree::Rotation& rotation); - void setPhase(bool isStancePhase); + void setPhase(Phase phase); /** * Start the approaching phase diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index 167d2aaa..f58ce717 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -44,9 +44,6 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, m_useVirtualizer = config.check("use_virtualizer", yarp::os::Value(false)).asBool(); m_useCoMHeightRetargeting = config.check("use_com_retargeting", yarp::os::Value(false)).asBool(); - // The approaching phase is set only if the startApproachingPhase method is called - m_isApproachingPhase = false; - if(m_useJointRetargeting && m_useHandRetargeting) { yError() << "[RetargetingClient::initialize] You cannot enable the joint retargeting along with the hand retargeting."; @@ -318,7 +315,7 @@ void RetargetingClient::getFeedback() if(m_useCoMHeightRetargeting) { - if(!m_isStancePhase) + if(m_phase == Phase::walking) m_comHeight.yarpReadBuffer(0) = m_comConstantHeight; else { @@ -335,7 +332,7 @@ void RetargetingClient::getFeedback() } // check if the approaching phase is finished - if(m_isApproachingPhase) + if(m_phase == Phase::approacing) { double now = yarp::os::Time::now(); if(now - m_startingApproachingPhaseTime > m_approachPhaseDuration) @@ -399,9 +396,20 @@ void RetargetingClient::setRobotBaseOrientation(const iDynTree::Rotation& rotati m_robotOrientationPort.write(false); } -void RetargetingClient::setPhase(bool isStancePhase) +void RetargetingClient::setPhase(Phase phase) { - m_isStancePhase = isStancePhase; + if(phase == Phase::approacing) + { + startApproachingPhase(); + } + + if(m_phase == Phase::approacing && phase == Phase::walking) + stopApproachingPhase(); + + if(m_phase == Phase::approacing && phase == Phase::stance) + stopApproachingPhase(); + + m_phase = phase; } void RetargetingClient::stopApproachingPhase() @@ -422,7 +430,7 @@ void RetargetingClient::stopApproachingPhase() m_comHeight.smoother->setT(m_comHeight.smoothingTimeInWalking); } - m_isApproachingPhase = false; + m_phase = Phase::stance; } void RetargetingClient::startApproachingPhase() @@ -449,10 +457,9 @@ void RetargetingClient::startApproachingPhase() m_comHeight.smoother->setT(m_comHeight.smoothingTimeInApproaching); } - m_isApproachingPhase = true; } bool RetargetingClient::isApproachingPhase() const { - return m_isApproachingPhase; + return m_phase == Phase::approacing; } diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index f0e1c9d6..958139c7 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -587,7 +587,13 @@ bool WalkingModule::updateModule() return false; } - m_retargetingClient->setPhase(m_isStancePhase.front()); + // if the retargeting is not in the approaching phase we can set the stance/walking phase + if(!m_retargetingClient->isApproachingPhase()) + { + auto retargetingPhase = m_isStancePhase.front() ? RetargetingClient::Phase::stance : RetargetingClient::Phase::walking; + m_retargetingClient->setPhase(retargetingPhase); + } + m_retargetingClient->getFeedback(); if(!updateFKSolver()) @@ -1247,7 +1253,9 @@ bool WalkingModule::startWalking() return false; } - m_retargetingClient->startApproachingPhase(); + // before running the controller the retargeting client goes in approaching phase this + // guarantees a smooth transition + m_retargetingClient->setPhase(RetargetingClient::Phase::approacing); m_robotState = WalkingFSM::Walking; return true; From 4650260fd5f3a502e37933377e99db654bb6deaf Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 May 2020 13:41:45 +0200 Subject: [PATCH 51/58] Add a description for the approacing phase --- .../WalkingControllers/RetargetingHelper/Helper.h | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index fbad9d6f..dc899cb6 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -37,12 +37,15 @@ namespace WalkingControllers class RetargetingClient { public: - enum class Phase - { - approacing, - stance, - walking - }; + enum class Phase + { + /** In this phase the smoothing time of the minimum jerk trajectory is + increased. This will guarantee a smoother transition between the + initial joint configuration and the desired joint configuration. */ + approacing, + stance, + walking + }; private: From 22992f9b905c784fdb8208059bce24879cbce286 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 May 2020 13:59:23 +0200 Subject: [PATCH 52/58] Improve the documentation in the qpInverseKinematics.ini files --- .../dcm_walking/hand_retargeting/qpInverseKinematics.ini | 1 + .../dcm_walking/joint_retargeting/qpInverseKinematics.ini | 2 +- .../dcm_walking/joypad_control/qpInverseKinematics.ini | 2 +- .../dcm_walking/hand_retargeting/qpInverseKinematics.ini | 1 + .../dcm_walking/joint_retargeting/qpInverseKinematics.ini | 2 +- .../dcm_walking/joypad_control/qpInverseKinematics.ini | 2 +- 6 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/qpInverseKinematics.ini index 05feedcd..d3dc2349 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/qpInverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/hand_retargeting/qpInverseKinematics.ini @@ -6,6 +6,7 @@ use_com_as_constraint 1 neck_weight 5.0 additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) +## joint regularization values in degrees joint_regularization (15, 0, 0, -7, 22, 11, 30, 0, 0, 0, -7, 22, 11, 30, 0, 0, 0, diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematics.ini index 93cd706a..e000bd7e 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematics.ini @@ -6,7 +6,7 @@ torso_weight_walking 5.0 torso_weight_stance 0.0 additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) -## Regularization +# joint regularization values in degrees joint_regularization (0,0,0, 15, 0, 0, -7, 22, 11, 30, 0, 0, 0, diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/qpInverseKinematics.ini index ce2e2bb8..0b716b7b 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/qpInverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/qpInverseKinematics.ini @@ -6,7 +6,7 @@ use_com_as_constraint 1 neck_weight 5.0 additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) -# Joint regularization term +# joint regularization values in degrees joint_regularization (15, 0, 0, -7, 22, 11, 30, -7, 22, 11, 30, diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/qpInverseKinematics.ini index 12971f11..44582b0a 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/qpInverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/qpInverseKinematics.ini @@ -6,6 +6,7 @@ use_com_as_constraint 1 neck_weight 5.0 additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) +## Regularization in degrees joint_regularization (15, 0, 0, -7, 22, 11, 30, 0, -7, 22, 11, 30, 0, diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini index 39469251..edd3fc7d 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/qpInverseKinematics.ini @@ -6,7 +6,7 @@ torso_weight_walking 5.0 torso_weight_stance 0.0 additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) -## Regularization +# joint regularization values in degrees joint_regularization (0,0,0, 15, 0, 0, -7, 22, 11, 30, 0, diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/qpInverseKinematics.ini index 0641e893..8af3b5e9 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/qpInverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/qpInverseKinematics.ini @@ -6,7 +6,7 @@ use_com_as_constraint 1 neck_weight 5.0 additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) -# Joint regularization term +# joint regularization values in degrees joint_regularization (15, 0, 0, -7, 22, 11, 30, -7, 22, 11, 30, From 787ac48394c05a60c5bf9edaafa0c51e2d43a857 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 May 2020 14:41:43 +0200 Subject: [PATCH 53/58] Use the an enum class instead of flags to enable the retargeting in QPInverseKinematics --- .../QPInverseKinematics.h | 5 +- .../src/QPInverseKinematics.cpp | 52 ++++++++++--------- 2 files changed, 31 insertions(+), 26 deletions(-) diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h index ab756730..23716757 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h @@ -42,6 +42,9 @@ namespace WalkingControllers { private: + enum class RetargetingType { handRetargeting, jointRetargeting, none }; + RetargetingType m_retargetingType; + /** * Set the joint positions and velocities bounds * @param jointVelocitiesBounds joint velocities bounds in [rad/s] @@ -149,8 +152,6 @@ namespace WalkingControllers bool m_useCoMAsConstraint; /**< True if the CoM is added as a constraint. */ bool m_useJointsLimitsConstraint; /**< True if the CoM is added as a constraint. */ - bool m_enableHandRetargeting; /**< True if the hand retargeting is used */ - bool m_enableJointRetargeting; /**< True if the joint retargeting is used */ iDynTree::MatrixDynSize m_hessianDense; /**< Hessian matrix */ iDynTree::VectorDynSize m_gradient; /**< Gradient vector */ diff --git a/src/WholeBodyControllers/src/QPInverseKinematics.cpp b/src/WholeBodyControllers/src/QPInverseKinematics.cpp index 7df54f87..73d440f4 100644 --- a/src/WholeBodyControllers/src/QPInverseKinematics.cpp +++ b/src/WholeBodyControllers/src/QPInverseKinematics.cpp @@ -47,15 +47,22 @@ bool WalkingQPIK::initialize(const yarp::os::Searchable &config, } m_useCoMAsConstraint = config.check("use_com_as_constraint", yarp::os::Value(false)).asBool(); - m_enableHandRetargeting = config.check("use_hand_retargeting", yarp::os::Value(false)).asBool(); - m_enableJointRetargeting = config.check("use_joint_retargeting", yarp::os::Value(false)).asBool(); + bool enableHandRetargeting = config.check("use_hand_retargeting", yarp::os::Value(false)).asBool(); + bool enableJointRetargeting = config.check("use_joint_retargeting", yarp::os::Value(false)).asBool(); - if(m_enableHandRetargeting && m_enableJointRetargeting) + if(enableHandRetargeting && enableJointRetargeting) { yError() << "[initialize] You cannot enable both hand and joint retargeting."; return false; } + if(enableJointRetargeting) + m_retargetingType = RetargetingType::jointRetargeting; + else if(enableHandRetargeting) + m_retargetingType = RetargetingType::handRetargeting; + else + m_retargetingType = RetargetingType::none; + m_useJointsLimitsConstraint = config.check("use_joint_limits_constraint", yarp::os::Value(false)).asBool(); // TODO in the future the number of constraints should be added inside @@ -104,14 +111,14 @@ bool WalkingQPIK::initialize(const yarp::os::Searchable &config, initializeSolverSpecificMatrices(); - if(m_enableHandRetargeting) + if(m_retargetingType == RetargetingType::handRetargeting) if(!initializeHandRetargeting(config)) { yError() << "[initialize] Unable to Initialize the hand retargeting."; return false; } - if(m_enableJointRetargeting) + if(m_retargetingType == RetargetingType::jointRetargeting) if(!initializeJointRetargeting(config)) { yError() << "[initialize] Unable to Initialize the joint retargeting."; @@ -165,7 +172,7 @@ bool WalkingQPIK::initializeMatrices(const yarp::os::Searchable& config) } // if the joint retargeting is enabled the weights of the cost function are time-variant. - if(!m_enableJointRetargeting) + if (m_retargetingType != RetargetingType::jointRetargeting) { if(!YarpUtilities::getNumberFromSearchable(config, "neck_weight", m_neckWeight)) { @@ -375,12 +382,9 @@ bool WalkingQPIK::initializeJointRetargeting(const yarp::os::Searchable& config) bool WalkingQPIK::setRobotState(WalkingFK& kinDynWrapper) { - // avoid to copy the vector if the application is not ran in retargeting mode - if(m_enableHandRetargeting) - { - m_leftHandToWorldTransform = kinDynWrapper.getLeftHandToWorldTransform(); - m_rightHandToWorldTransform = kinDynWrapper.getRightHandToWorldTransform(); - } + + m_leftHandToWorldTransform = kinDynWrapper.getLeftHandToWorldTransform(); + m_rightHandToWorldTransform = kinDynWrapper.getRightHandToWorldTransform(); m_jointPosition = kinDynWrapper.getJointPos(); m_leftFootToWorldTransform = kinDynWrapper.getLeftFootToWorldTransform(); @@ -395,10 +399,10 @@ void WalkingQPIK::setPhase(const bool& isStancePhase) { if(isStancePhase) { - if(m_enableHandRetargeting) + if(m_retargetingType == RetargetingType::handRetargeting) m_handWeightSmoother->computeNextValues(m_handWeightStanceVector); - if(m_enableJointRetargeting) + else if(m_retargetingType == RetargetingType::jointRetargeting) { m_torsoWeightSmoother->computeNextValues(yarp::sig::Vector(1, m_torsoWeightStance)); m_jointRetargetingWeightSmoother->computeNextValues(m_jointRetargetingWeightStance); @@ -407,10 +411,10 @@ void WalkingQPIK::setPhase(const bool& isStancePhase) } else { - if(m_enableHandRetargeting) + if(m_retargetingType == RetargetingType::handRetargeting) m_handWeightSmoother->computeNextValues(m_handWeightWalkingVector); - if(m_enableJointRetargeting) + else if(m_retargetingType == RetargetingType::jointRetargeting) { m_torsoWeightSmoother->computeNextValues(yarp::sig::Vector(1, m_torsoWeightWalking)); m_jointRetargetingWeightSmoother->computeNextValues(m_jointRetargetingWeightWalking); @@ -479,7 +483,7 @@ bool WalkingQPIK::setRightFootJacobian(const iDynTree::MatrixDynSize& rightFootJ bool WalkingQPIK::setLeftHandJacobian(const iDynTree::MatrixDynSize& leftHandJacobian) { - if(!m_enableHandRetargeting) + if(m_retargetingType != RetargetingType::handRetargeting) return true; if(leftHandJacobian.rows() != 6) @@ -500,7 +504,7 @@ bool WalkingQPIK::setLeftHandJacobian(const iDynTree::MatrixDynSize& leftHandJac bool WalkingQPIK::setRightHandJacobian(const iDynTree::MatrixDynSize& rightHandJacobian) { - if(!m_enableHandRetargeting) + if(m_retargetingType != RetargetingType::handRetargeting) return true; if(rightHandJacobian.rows() != 6) @@ -569,7 +573,7 @@ void WalkingQPIK::setDesiredFeetTwist(const iDynTree::Twist& leftFootTwist, void WalkingQPIK::setDesiredHandsTransformation(const iDynTree::Transform& desiredLeftHandToWorldTransform, const iDynTree::Transform& desiredRightHandToWorldTransform) { - if(!m_enableHandRetargeting) + if(m_retargetingType != RetargetingType::handRetargeting) return; m_desiredLeftHandToWorldTransform = desiredLeftHandToWorldTransform; @@ -579,7 +583,7 @@ void WalkingQPIK::setDesiredHandsTransformation(const iDynTree::Transform& desir bool WalkingQPIK::setDesiredRetargetingJoint(const iDynTree::VectorDynSize& jointPosition) { // if the joint retargeting is not used return - if(!m_enableJointRetargeting) + if(m_retargetingType != RetargetingType::jointRetargeting) return true; if(jointPosition.size() != m_actuatedDOFs) @@ -618,7 +622,7 @@ void WalkingQPIK::evaluateHessianMatrix() auto hessianDense(iDynTree::toEigen(m_hessianDense)); // if the joint retargeting is enable the weights of the cost function are time variant - if(!m_enableJointRetargeting) + if (m_retargetingType != RetargetingType::jointRetargeting) { hessianDense = iDynTree::toEigen(m_neckJacobian).transpose() * m_neckWeight * iDynTree::toEigen(m_neckJacobian); hessianDense.bottomRightCorner(m_actuatedDOFs, m_actuatedDOFs) += iDynTree::toEigen(m_jointRegularizationWeights).asDiagonal(); @@ -633,7 +637,7 @@ void WalkingQPIK::evaluateHessianMatrix() iDynTree::toEigen(m_jointRetargetingWeightSmoother->getPos())).asDiagonal(); } - if(m_enableHandRetargeting) + if(m_retargetingType == RetargetingType::handRetargeting) { // think about the possibility to project in the null space the joint regularization hessianDense += iDynTree::toEigen(m_leftHandJacobian).transpose() @@ -669,7 +673,7 @@ void WalkingQPIK::evaluateGradientVector() // Neck orientation iDynTree::Matrix3x3 errorNeckAttitude = iDynTreeUtilities::Rotation::skewSymmetric(m_neckOrientation * m_desiredNeckOrientation.inverse()); - if(!m_enableJointRetargeting) + if(m_retargetingType != RetargetingType::jointRetargeting) { auto jointRegularizationWeights(iDynTree::toEigen(m_jointRegularizationWeights)); @@ -701,7 +705,7 @@ void WalkingQPIK::evaluateGradientVector() * (jointRetargetingValues - jointPosition); } - if(m_enableHandRetargeting) + if(m_retargetingType == RetargetingType::handRetargeting) { Eigen::Map leftHandCorrectionLinearVel(iDynTree::toEigen(m_leftHandCorrection.getLinearVec3())); Eigen::Map leftHandCorrectionAngularVel(iDynTree::toEigen(m_leftHandCorrection.getAngularVec3())); From 220c4764d2cb52eb2241dd3c99d674ee4114beb2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 May 2020 15:06:38 +0200 Subject: [PATCH 54/58] Reduce the dynamic memory allocation in QPInverseKinematics class --- .../QPInverseKinematics.h | 6 +++++ .../src/QPInverseKinematics.cpp | 24 +++++++++++-------- 2 files changed, 20 insertions(+), 10 deletions(-) diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h index 23716757..a1762f1e 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h @@ -109,6 +109,12 @@ namespace WalkingControllers double m_kCom; /**< Gain related to the desired CoM position. */ iDynTree::VectorDynSize m_jointRegularizationWeights; /**< Weight related to the the regularization term */ + + iDynTree::VectorDynSize m_jointRegularizationGainsTimeWeights; /**< Product between the jointRegularizationGains + and the jointRegularizationWeights. + This quantity is used only if the joint retargeting is + disabled*/ + iDynTree::Vector3 m_comWeight; /**< CoM weight. */ double m_neckWeight; /**< Neck weight matrix. */ iDynSparseMatrix m_jointRegularizationHessian; /**< Contains a constant matrix that can be useful diff --git a/src/WholeBodyControllers/src/QPInverseKinematics.cpp b/src/WholeBodyControllers/src/QPInverseKinematics.cpp index 73d440f4..23d5cd5d 100644 --- a/src/WholeBodyControllers/src/QPInverseKinematics.cpp +++ b/src/WholeBodyControllers/src/QPInverseKinematics.cpp @@ -171,6 +171,14 @@ bool WalkingQPIK::initializeMatrices(const yarp::os::Searchable& config) } } + m_jointRegularizationGains.resize(m_actuatedDOFs); + if(!YarpUtilities::getVectorFromSearchable(config, "joint_regularization_gains", + m_jointRegularizationGains)) + { + yError() << "Initialization failed while reading jointRegularizationGains vector."; + return false; + } + // if the joint retargeting is enabled the weights of the cost function are time-variant. if (m_retargetingType != RetargetingType::jointRetargeting) { @@ -187,14 +195,11 @@ bool WalkingQPIK::initializeMatrices(const yarp::os::Searchable& config) yError() << "Initialization failed while reading jointRegularizationWeights vector."; return false; } - } - m_jointRegularizationGains.resize(m_actuatedDOFs); - if(!YarpUtilities::getVectorFromSearchable(config, "joint_regularization_gains", - m_jointRegularizationGains)) - { - yError() << "Initialization failed while reading jointRegularizationGains vector."; - return false; + // if the joint retargeting is NOT enabled both the jointRegularizationWeights and the + // jointRegularizationGains are constant. For this reason we can compute their product + m_jointRegularizationGainsTimeWeights.resize(m_actuatedDOFs); + iDynTree::toEigen(m_jointRegularizationGainsTimeWeights) = iDynTree::toEigen(m_jointRegularizationWeights).cwiseProduct(iDynTree::toEigen(m_jointRegularizationGains)); } if(!YarpUtilities::getNumberFromSearchable(config, "k_posFoot", m_kPosFoot)) @@ -675,15 +680,14 @@ void WalkingQPIK::evaluateGradientVector() iDynTree::Matrix3x3 errorNeckAttitude = iDynTreeUtilities::Rotation::skewSymmetric(m_neckOrientation * m_desiredNeckOrientation.inverse()); if(m_retargetingType != RetargetingType::jointRetargeting) { - auto jointRegularizationWeights(iDynTree::toEigen(m_jointRegularizationWeights)); + auto jointRegularizationGainsTimeWeights(iDynTree::toEigen(m_jointRegularizationGainsTimeWeights)); gradient = -neckJacobian.transpose() * m_neckWeight * (-m_kNeck * iDynTree::unskew(iDynTree::toEigen(errorNeckAttitude))); // g = Weight * K_p * (regularizationTerm - jointPosition) // Weight and K_p are two diagonal matrices so their product can be also evaluated multiplying component-wise // the elements of the vectors and then generating the diagonal matrix - gradient.tail(m_actuatedDOFs) += (-jointRegularizationWeights.cwiseProduct(jointRegularizationGains)).asDiagonal() - * (regularizationTerm - jointPosition); + gradient.tail(m_actuatedDOFs) -= jointRegularizationGainsTimeWeights.asDiagonal() * (regularizationTerm - jointPosition); } else { From fde9abd53a560d6b030c1646df3609792214a320 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 May 2020 15:14:53 +0200 Subject: [PATCH 55/58] Store the joint positions variable in the KinDynWrapper class --- .../WalkingControllers/KinDynWrapper/Wrapper.h | 4 +++- src/KinDynWrapper/src/Wrapper.cpp | 12 ++++++++---- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h b/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h index 33e34f59..975ae75c 100644 --- a/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h +++ b/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h @@ -69,6 +69,8 @@ namespace WalkingControllers bool m_firstStep; /**< True only during the first step. */ + iDynTree::VectorDynSize m_jointPositions; /**< joint positions in radians. */ + /** * Set the model of the robot. * @param model iDynTree model. @@ -263,7 +265,7 @@ namespace WalkingControllers * Get the joint position * @return the joint position expressed in radians */ - iDynTree::VectorDynSize getJointPos(); + const iDynTree::VectorDynSize& getJointPos(); }; }; #endif diff --git a/src/KinDynWrapper/src/Wrapper.cpp b/src/KinDynWrapper/src/Wrapper.cpp index 57dfdcb7..48764dad 100644 --- a/src/KinDynWrapper/src/Wrapper.cpp +++ b/src/KinDynWrapper/src/Wrapper.cpp @@ -262,6 +262,10 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config, m_useFilters = config.check("use_filters", yarp::os::Value(false)).asBool(); m_firstStep = true; + + + // resize the joint positions + m_jointPositions.resize(model.getNrOfDOFs()); return true; } @@ -518,12 +522,12 @@ bool WalkingFK::getCoMJacobian(iDynTree::MatrixDynSize &jacobian) return m_kinDyn.getCenterOfMassJacobian(jacobian); } -iDynTree::VectorDynSize WalkingFK::getJointPos() +const iDynTree::VectorDynSize& WalkingFK::getJointPos() { - iDynTree::VectorDynSize jointPos; - bool ok = m_kinDyn.getJointPos(jointPos); + + bool ok = m_kinDyn.getJointPos(m_jointPositions); assert(ok); - return jointPos; + return m_jointPositions; } From c2828ba194b295b916502f36e776a0efe488979e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 May 2020 15:16:35 +0200 Subject: [PATCH 56/58] Fix documentation in QPInverseKinematics.h --- .../WholeBodyControllers/QPInverseKinematics.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h index a1762f1e..d83682dd 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h @@ -320,7 +320,7 @@ namespace WalkingControllers const iDynTree::Twist& rightHandTwist); /** - * Set the desired twist of both feet + * Set the desired joint positions in radians * @param jointPosition contain the desired joint position used in the retargeting. * @return true/false in case of success/failure. */ From a9a2591e7e1e2a6a93e5ff5a08cfdcb51d9b3003 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 May 2020 15:18:05 +0200 Subject: [PATCH 57/58] Remove useless variable in WalkingModule::setPlannerInput() method --- src/WalkingModule/src/Module.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 958139c7..da4f385a 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -1311,8 +1311,6 @@ bool WalkingModule::setPlannerInput(double x, double y) m_newTrajectoryRequired = true; - double treshold = 0.01; - return true; } From 5d0a4fda1eb186ca06f2c33f2f3074aea9c951c2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 May 2020 15:25:10 +0200 Subject: [PATCH 58/58] Improve the documentation in jointRetargeting.ini --- .../dcm_walking/joint_retargeting/jointRetargeting.ini | 4 ++++ .../dcm_walking/joint_retargeting/jointRetargeting.ini | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini index 87884bc0..2ae4e31f 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/jointRetargeting.ini @@ -4,6 +4,10 @@ approaching_phase_duration 4.0 [JOINT_RETARGETING] +## List of the retargeting joint. This list must be the same or a subset of the +## "joints_list" in robotControl.ini. The order of the joints should be choseen +## accordingly to the order of the joints received in the +## "joint_retargeting_port_name" port retargeting_joint_list ("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini index 314b695b..58a0faab 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/jointRetargeting.ini @@ -4,6 +4,10 @@ approaching_phase_duration 4.0 [JOINT_RETARGETING] +## List of the retargeting joint. This list must be the same or a subset of the +## "joints_list" in robotControl.ini. The order of the joints should be choseen +## accordingly to the order of the joints received in the +## "joint_retargeting_port_name" port retargeting_joint_list ("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup",