From 5c5cf88bfcd8273be1fca5749183ac427078e531 Mon Sep 17 00:00:00 2001 From: Milad Date: Tue, 25 Feb 2020 16:23:17 +0100 Subject: [PATCH 1/3] adding the step adaptation library changing the name of QPSolver class remove small bug combining the two class of step adaptation into one class small modifications of step adaptation library modifications of step adaptation library modifications of step adaptation library small modifications of step adaptation library removing some useless methods removing some useless methods small modifications adding an enum for recognizing of swing foot in solve method Using step adaptation library inside the walking controller fixing some bugs fixing the bug related to the size of hessian matrix of step adaptation optimization push recovery by two arms in saggital and lateral direction cleaning the code some primary modifications separation of the step adaptation config files some primary modifications adding the documentation link to the simple estimator modification of the configuration file make an object of TrajectoryGeneration for step adjustment some modifications in trajectoryGenerator and module some small modifications in Module.cpp moving the arms push detection inside the stepAdjustment module modifying some comments dubeg of moving the step adjustment stuff from walkingModule to step adaptation library Keeping the dcm replanning part inside the walkingModule remove some magic numbers modify the input of runstepAdaptation as a reference small modifications step adaptation documentation and configuration files modification Bring the arm joints name for push detection inside the configuration file some modification on walkingModule and exceptions some modifications in walkingModule defining the output and input of step adaptation structure as a private member removing some small bugs! add missing trajectoryPlanner inside cmake lists remove the bug related to zmp offset copy the retargeting planner parameters to reduce the shaking tuning pull request on the robot step adaptation left arm lateral direction tuning the planner parametr for yaw movement tuning unicycle planner parameter for rotational movement tuning step adaptaion on the robot set stiffness and damping joint again inside the code tune the step adaptation parameters tuning step adaptation tuning step adjustment tune the step adaptation parameters activating push recovery every two steps lateral direction is working remove the yaw error remove some comments tuning step adaptaion parameters remove the bug related to the yaw orientation small modification removing the bug related to the push detection during rotational walking parameter tuning some small modifications solve the problem of joystick command during step adaptation solve the problem of joystick increasing speed of walking some modifications try to solve joystick problem small modification try to solve the last step impact saturation of the com error tunning parameters saturation of the com error video recorded version pull request modifications Fix some rebase issues. --- .../WalkingControllersFindDependencies.cmake | 7 +- src/CMakeLists.txt | 1 + .../RobotInterface/Helper.h | 15 +- src/RobotInterface/src/Helper.cpp | 40 + src/StepAdaptationController/CMakeLists.txt | 59 + .../DCMSimpleEstimator.hpp | 63 + .../StepAdaptationController.hpp | 382 ++++++ .../src/DCMSimpleEstimator.cpp | 57 + .../src/StepAdaptationController.cpp | 1039 +++++++++++++++++ .../TrajectoryPlanner/TrajectoryGenerator.h | 78 ++ .../src/TrajectoryGenerator.cpp | 117 ++ src/WalkingModule/CMakeLists.txt | 1 + .../joypad_control/robotControl.ini | 13 + .../step_adaptation/plannerParams.ini | 53 + .../step_adaptation/robotControl.ini | 39 + .../step_adaptation/stepAdaptation.ini | 32 + .../dcm_walking_step_adaptation.ini | 63 + .../joypad_control/robotControl.ini | 11 + .../step_adaptation/plannerParams.ini | 60 + .../step_adaptation/robotControl.ini | 36 + .../step_adaptation/stepAdaptation.ini | 32 + .../step_adaptation/zmpControllerParams.ini | 18 + .../dcm_walking_step_adaptation.ini | 59 + .../WalkingControllers/WalkingModule/Module.h | 86 +- src/WalkingModule/src/Module.cpp | 586 +++++++++- 25 files changed, 2917 insertions(+), 30 deletions(-) create mode 100644 src/StepAdaptationController/CMakeLists.txt create mode 100644 src/StepAdaptationController/include/WalkingControllers/StepAdaptationController/DCMSimpleEstimator.hpp create mode 100644 src/StepAdaptationController/include/WalkingControllers/StepAdaptationController/StepAdaptationController.hpp create mode 100644 src/StepAdaptationController/src/DCMSimpleEstimator.cpp create mode 100644 src/StepAdaptationController/src/StepAdaptationController.cpp create mode 100644 src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/plannerParams.ini create mode 100644 src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/robotControl.ini create mode 100644 src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/stepAdaptation.ini create mode 100644 src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_step_adaptation.ini create mode 100644 src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/plannerParams.ini create mode 100644 src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/robotControl.ini create mode 100644 src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/stepAdaptation.ini create mode 100644 src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/zmpControllerParams.ini create mode 100644 src/WalkingModule/app/robots/iCubGenova04/dcm_walking_step_adaptation.ini diff --git a/cmake/WalkingControllersFindDependencies.cmake b/cmake/WalkingControllersFindDependencies.cmake index a577bc498..a3cc3ab8c 100644 --- a/cmake/WalkingControllersFindDependencies.cmake +++ b/cmake/WalkingControllersFindDependencies.cmake @@ -131,7 +131,7 @@ checkandset_dependency(ICUBcontrib) find_package(iDynTree QUIET) checkandset_dependency(iDynTree) -find_package(UnicyclePlanner 0.1.102 QUIET) +find_package(UnicyclePlanner 0.2.102 QUIET) checkandset_dependency(UnicyclePlanner) find_package(osqp QUIET) @@ -162,8 +162,11 @@ walking_controllers_dependent_option(WALKING_CONTROLLERS_COMPILE_RetargetingHelp "WALKING_CONTROLLERS_HAS_iDynTree;WALKING_CONTROLLERS_COMPILE_YarpUtilities;WALKING_CONTROLLERS_HAS_ICUB" OFF) walking_controllers_dependent_option(WALKING_CONTROLLERS_COMPILE_LoggerClient "Compile LoggerClient library?" ON WALKING_CONTROLLERS_COMPILE_YarpUtilities OFF) +walking_controllers_dependent_option(WALKING_CONTROLLERS_COMPILE_StepAdaptationController "Compile StepAdaptationController library?" ON + "WALKING_CONTROLLERS_HAS_iDynTree;WALKING_CONTROLLERS_COMPILE_iDynTreeUtilities;WALKING_CONTROLLERS_COMPILE_YarpUtilities;WALKING_CONTROLLERS_COMPILE_TrajectoryPlanner;WALKING_CONTROLLERS_HAS_osqp;WALKING_CONTROLLERS_HAS_OsqpEigen" OFF) + walking_controllers_dependent_option(WALKING_CONTROLLERS_COMPILE_WalkingModule "Compile WalkingModule app?" ON - "WALKING_CONTROLLERS_COMPILE_YarpUtilities;WALKING_CONTROLLERS_COMPILE_iDynTreeUtilities;WALKING_CONTROLLERS_COMPILE_RobotInterface;WALKING_CONTROLLERS_COMPILE_KinDynWrapper;WALKING_CONTROLLERS_COMPILE_TrajectoryPlanner;WALKING_CONTROLLERS_COMPILE_SimplifiedModelControllers;WALKING_CONTROLLERS_COMPILE_WholeBodyControllers;WALKING_CONTROLLERS_COMPILE_RetargetingHelper;WALKING_CONTROLLERS_COMPILE_LoggerClient;WALKING_CONTROLLERS_HAS_ICUBcontrib" OFF) + "WALKING_CONTROLLERS_COMPILE_YarpUtilities;WALKING_CONTROLLERS_COMPILE_iDynTreeUtilities;WALKING_CONTROLLERS_COMPILE_RobotInterface;WALKING_CONTROLLERS_COMPILE_KinDynWrapper;WALKING_CONTROLLERS_COMPILE_TrajectoryPlanner;WALKING_CONTROLLERS_COMPILE_SimplifiedModelControllers;WALKING_CONTROLLERS_COMPILE_WholeBodyControllers;WALKING_CONTROLLERS_COMPILE_RetargetingHelper;WALKING_CONTROLLERS_COMPILE_LoggerClient;WALKING_CONTROLLERS_HAS_ICUBcontrib;WALKING_CONTROLLERS_COMPILE_StepAdaptationController" OFF) walking_controllers_dependent_option(WALKING_CONTROLLERS_COMPILE_JoypadModule "Compile JoypadModule app?" ON "WALKING_CONTROLLERS_COMPILE_YarpUtilities;WALKING_CONTROLLERS_HAS_ICUBcontrib" OFF) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index bea59a565..e87f53775 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -16,3 +16,4 @@ add_subdirectory(LoggerClient) add_subdirectory(WalkingModule) add_subdirectory(JoypadModule) add_subdirectory(LoggerModule) +add_subdirectory(StepAdaptationController) diff --git a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h index 57f5fd8d3..bbd1267f6 100644 --- a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h +++ b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h @@ -21,6 +21,8 @@ #include #include #include +#include + #include #include @@ -53,7 +55,7 @@ namespace WalkingControllers 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::IImpedanceControl *m_impedanceControlInterface{nullptr}; std::unique_ptr m_PIDHandler; /**< Pointer to the PID handler object. */ yarp::os::Bottle m_remoteControlBoards; /**< Contain all the name of the controlled joints. */ @@ -71,6 +73,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 */ // 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,6 +94,9 @@ namespace WalkingControllers std::unique_ptr m_rightWrenchFilter; /**< Right wrench low pass filter.*/ bool m_useWrenchFilter; /**< True if the wrench filter is used. */ + iDynTree::VectorDynSize m_stiffnessGainVector; /**< the vector of stifness gains for the joint */ + iDynTree::VectorDynSize m_dampingGainVector; /**< the vector of damping gains for the joint */ + std::vector m_jointModes; /**< True if the joint is in the stiff mode */ double m_startingPositionControlTime; bool m_positionMoveSkipped; @@ -254,6 +262,11 @@ namespace WalkingControllers */ bool isExternalRobotBaseUsed(); + /** + * Set the impedance control gains of the joints(stiffness/damping). + * @return true in case of success and false otherwise. + */ + bool setImpedanceControlGain(); }; }; #endif diff --git a/src/RobotInterface/src/Helper.cpp b/src/RobotInterface/src/Helper.cpp index caa6eb7b7..93d56ec6b 100644 --- a/src/RobotInterface/src/Helper.cpp +++ b/src/RobotInterface/src/Helper.cpp @@ -227,6 +227,8 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) m_jointInteractionMode.resize(m_actuatedDOFs); m_currentJointInteractionMode.resize(m_actuatedDOFs); std::vector isJointInStiffMode(m_actuatedDOFs); + m_stiffnessGainVector.resize(m_actuatedDOFs); + m_dampingGainVector.resize(m_actuatedDOFs); if(!YarpUtilities::getVectorOfBooleanFromSearchable(config, "joint_is_stiff_mode", isJointInStiffMode)) { @@ -234,6 +236,18 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) return false; } + if(!YarpUtilities::getVectorFromSearchable(config,"joint_stiffness_gain",m_stiffnessGainVector)) + { + yError() << "[RobotInterface::configureRobot] Unable to find joint_stiffness_gain into config file."; + return false; + } + + if(!YarpUtilities::getVectorFromSearchable(config,"joint_damping_gain",m_dampingGainVector)) + { + yError() << "[RobotInterface::configureRobot] Unable to find joint_damping_gain into config file."; + return false; + } + for (unsigned int i = 0; i < m_actuatedDOFs; i++) { if(isJointInStiffMode[i]) @@ -306,6 +320,12 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) return false; } + if(!m_robotDevice.view(m_impedanceControlInterface) || !m_impedanceControlInterface) + { + yError() << "[configureRobot] Cannot obtain ImpedanceControl interface"; + return false; + } + // resize the buffers m_positionFeedbackDeg.resize(m_actuatedDOFs, 0.0); m_velocityFeedbackDeg.resize(m_actuatedDOFs, 0.0); @@ -637,6 +657,12 @@ bool RobotInterface::setPositionReferences(const iDynTree::VectorDynSize& desire return false; } + if(m_impedanceControlInterface == nullptr) + { + yError() << "[RobotInterface::setPositionReferences] IImpedanceControlInterface interface is not ready."; + return false; + } + m_desiredJointPositionRad = desiredJointPositionsRad; std::pair worstError(0, 0.0); @@ -921,3 +947,17 @@ bool RobotInterface::loadCustomInteractionMode() { return setInteractionMode(m_jointInteractionMode); } + +bool RobotInterface::setImpedanceControlGain() +{ + for (unsigned i = 0; i < m_actuatedDOFs; i++) + { + if(!m_impedanceControlInterface->setImpedance(i,m_stiffnessGainVector(i),m_dampingGainVector(i))) + { + yError() << "[RobotInterface::setImpedanceControlGain] Error while setting the impedance control gains"; + return false; + } + } + + return true; +} diff --git a/src/StepAdaptationController/CMakeLists.txt b/src/StepAdaptationController/CMakeLists.txt new file mode 100644 index 000000000..969896cc5 --- /dev/null +++ b/src/StepAdaptationController/CMakeLists.txt @@ -0,0 +1,59 @@ +# Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT) +# All Rights Reserved. +# Authors: Milad Shafiee + +if(WALKING_CONTROLLERS_COMPILE_StepAdaptationController) + +# set target name + set(LIBRARY_TARGET_NAME StepAdaptationController) + + # set cpp files + set(${LIBRARY_TARGET_NAME}_SRC + src/StepAdaptationController.cpp + src/DCMSimpleEstimator.cpp + ) + + # set hpp files + set(${LIBRARY_TARGET_NAME}_HDR + include/WalkingControllers/StepAdaptationController/StepAdaptationController.hpp + include/WalkingControllers/StepAdaptationController/DCMSimpleEstimator.hpp + ) + + # add an executable to the project using the specified source files. + add_library(${LIBRARY_TARGET_NAME} SHARED ${${LIBRARY_TARGET_NAME}_SRC} ${${LIBRARY_TARGET_NAME}_HDR}) + set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES OUTPUT_NAME "${PROJECT_NAME}${LIBRARY_TARGET_NAME}") + + target_link_libraries(${LIBRARY_TARGET_NAME} PUBLIC + WalkingControllers::YarpUtilities + WalkingControllers::iDynTreeUtilities + WalkingControllers::TrajectoryPlanner + osqp::osqp + OsqpEigen::OsqpEigen + ${qpOASES_LIBRARIES}) + + + add_library(WalkingControllers::${LIBRARY_TARGET_NAME} ALIAS ${LIBRARY_TARGET_NAME}) + + set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES VERSION ${WalkingControllers_VERSION} + PUBLIC_HEADER "${${LIBRARY_TARGET_NAME}_HDR}") + + #Specify include directories for both compilation and installation process. + #The $ generator expression is useful to ensure to create + #relocatable configuration files, see https://cmake.org/cmake/help/latest/manual/cmake-packages.7.html#creating-relocatable-packages + target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC "$" + "$/${CMAKE_INSTALL_INCLUDEDIR}>") + + # Specify installation targets, typology and destination folders. + install(TARGETS ${LIBRARY_TARGET_NAME} + EXPORT ${PROJECT_NAME} + COMPONENT runtime + LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib + ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib + RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin + PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WalkingControllers/StepAdaptationController" COMPONENT dev) + + set_property(GLOBAL APPEND PROPERTY WalkingControllers_TARGETS ${LIBRARY_TARGET_NAME}) + + message(STATUS "Created target ${LIBRARY_TARGET_NAME} for export ${PROJECT_NAME}.") + +endif() diff --git a/src/StepAdaptationController/include/WalkingControllers/StepAdaptationController/DCMSimpleEstimator.hpp b/src/StepAdaptationController/include/WalkingControllers/StepAdaptationController/DCMSimpleEstimator.hpp new file mode 100644 index 000000000..bef851e6a --- /dev/null +++ b/src/StepAdaptationController/include/WalkingControllers/StepAdaptationController/DCMSimpleEstimator.hpp @@ -0,0 +1,63 @@ +/** + * @file DCMSimpleEstimator.hpp + * @authors Milad Shafiee + * @copyright 2020 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2020 + */ + +#ifndef WALKING_CONTROLLERS_DCM_SIMPLE_ESTIMATOR_H +#define WALKING_CONTROLLERS_DCM_SIMPLE_ESTIMATOR_H + +// YARP +#include +#include + +//iDynTree +#include +#include +#include + + +namespace WalkingControllers +{ +/** + * The theory related to this simple estimator has been discussed in the following github issue: + *https://github.com/robotology/walking-controllers/issues/61 +*/ + class DCMSimpleEstimator + { + double m_omega; /**< Inverted time constant of the 3D-LIPM. */ + double m_mass; /**< Mass of the robot. */ + iDynTree::Vector2 m_dcmEstimatedPosition; /**< Position of the estimated DCM. */ + iDynTree::Vector2 m_dcmPosition; /**< Position of the DCM. */ + iDynTree::Vector2 m_dcmVelocity; /**< Velocity of the dcm. */ + + public: + + /** + * Config the DCMEstimator. + * @param config config of the simple DCM estimator; + * @return true on success, false otherwise. + */ + bool configure(const yarp::os::Searchable& config); + + /** + * Get the position of the DCM. + * @return position of the DCM. + */ + const iDynTree::Vector2& getDCMPosition() const; + + /** + * run the pendulum estimator + * @param footOrientation the orientation of stance foot. + * @param zmp the vector of zmp position with respect to the inertial frame. + * @param com com the com position obtained as if the foot is not rotated. + * @param CoMVelocity3d the vector of com velocity that is simple time derivative of the com position. + * @return true/false in case of success/failure + */ + bool update(const iDynTree::Rotation& footOrientation,const iDynTree::Vector3& zmp,const iDynTree::Vector3& com,const iDynTree::LinVelocity& CoMVelocity3d); + }; +}; + +#endif diff --git a/src/StepAdaptationController/include/WalkingControllers/StepAdaptationController/StepAdaptationController.hpp b/src/StepAdaptationController/include/WalkingControllers/StepAdaptationController/StepAdaptationController.hpp new file mode 100644 index 000000000..f8407fa66 --- /dev/null +++ b/src/StepAdaptationController/include/WalkingControllers/StepAdaptationController/StepAdaptationController.hpp @@ -0,0 +1,382 @@ +/** + * @file StepAdaptationController.hpp + * @authors Milad Shafiee + * @copyright 2020 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2020 + */ + +#ifndef WALKING_CONTROLLERS_STEP_ADAPTATION_CONTROLLERS_H +#define WALKING_CONTROLLERS_STEP_ADAPTATION_CONTROLLERS_H + + +// std +#include + +// iDynTree +#include +#include +#include +#include +#include +#include +#include + +// yarp +#include + +#include + +#include +#include +#include +#include + +struct FootTrajectoryGenerationInput +{ + double maxFootHeight; + double discretizationTime ; + double takeOffTime; + double yawAngleAtImpact; + iDynTree::Vector2 zmpToCenterOfFootPosition; + iDynTree::Transform currentFootTransform; + iDynTree::Twist currentFootTwist; +}; + +struct StepAdapterInput +{ + double time; + double timeOffset; + double dT; + double omega; + std::deque leftInContact; + std::deque rightInContact; + std::vector> dcmSubTrajectories; + std::shared_ptr leftFootprints; + std::shared_ptr rightFootprints; + iDynTree::Vector2 dcmPositionSmoothed; + StepList rightStepList; + StepList leftStepList; +}; + +struct StepAdapterOutput +{ + iDynTree::Transform adaptedFootLeftTransform; + iDynTree::Transform adaptedFootRightTransform; + iDynTree::Twist adaptedFootRightTwist; + iDynTree::Twist adaptedFootLeftTwist; + iDynTree::SpatialAcc adaptedFootLeftAcceleration; + iDynTree::SpatialAcc adaptedFootRightAcceleration; + iDynTree::Transform currentFootLeftTransform; + iDynTree::Transform currentFootRightTransform; + iDynTree::Twist currentFootLeftTwist; + iDynTree::Twist currentFootRightTwist; + iDynTree::SpatialAcc currentFootLeftAcceleration; + iDynTree::SpatialAcc currentFootRightAcceleration; + std::deque dcmPositionAdjusted; + std::deque dcmVelocityAdjusted; + iDynTree::Vector2 zmpNominal; + iDynTree::Vector2 zmpAdjusted; + double isPushActive; + double isRollActive; + double isPitchActive; + int pushRecoveryActiveIndex; + double kDCMSmoother; + double kFootSmoother; + int indexSmoother; + int indexFootSmoother; + int timeIndexAfterPushDetection; + int FootTimeIndexAfterPushDetection; + int indexPush; + double impactTimeNominal; + double impactTimeAdjusted; +}; +/** + * StepAdaptationController class contains the controller instances. + */ +namespace WalkingControllers +{ + +/** + * Enumerator for understanding which foot is in the swing phase. + */ + enum class SwingFoot{Left,Right}; + + class StepAdaptationController + { + /** + * Pointer to the optimization solver + */ + std::unique_ptr m_QPSolver_qpOASES{nullptr}; /**< qpOASES Optimization solver. */ + + iDynSparseMatrix m_hessianMatrix;/**< hessian matrix of cost function. */ + iDynSparseMatrix m_constraintsMatrix; /**< constraints matrix. */ + iDynTree::VectorDynSize m_gradient;/**< Gradient vector. */ + iDynTree::VectorDynSize m_lowerBound; /**< Lower bound vector. */ + iDynTree::VectorDynSize m_upperBound; /**< Upper bound vector. */ + std::unique_ptr m_DCMEstimator; /**< Pointer to the simple pendulum estimator. */ + iDynTree::VectorDynSize m_solution; /**< solution vector of the optimization. */ + + int m_inputSize; /**< Size of the controlled input vector . */ + int m_numberOfConstraints; /**< Size of the constraint vector . */ + + bool m_isFirstTime;/**< boolean that indicates whether the solver has been already initilized? . */ + bool m_numberOfStepFlag; + int m_stepCounter; + int m_pushStepNumber; + + iDynTree::Vector2 m_zmpPositionNominal; /**< The next desired step position(The zmp position for next single support) .. */ + iDynTree::Vector2 m_dcmOffsetNominal; /**< The next desired dcm offset*/ + double m_sigmaNominal; /**< The exponential function of step duration multplied by the natural frequency of the LIPM.*/ + + iDynTree::Vector2 m_zmpPositionWeight; /**< The wight of next step position term in the cost function.*/ + iDynTree::Vector2 m_dcmOffsetWeight;/**< The wight of dcm offset term in the cost function.*/ + double m_sigmaWeight;/**< The wight of step timing term in the cost function.*/ + int m_pushRecoveryActivationIndex;/**< A threshold index for activation of push recovery.*/ + + std::vector m_pushDetectionListRightArmX; /**< Vector containing the name of the right arm joints that will be used for push detection in X direction. */ + std::vector m_pushDetectionListLeftArmX;/**< Vector containing the name of the left arm joints that will be used for push detection in X direction. */ + std::vector m_pushDetectionListRightArmY;/**< Vector containing the name of the right arm joints that will be used for push detection in Y direction. */ + std::vector m_pushDetectionListLeftArmY;/**< Vector containing the name of the left arm joints that will be used for push detection in Y direction. */ + + iDynTree::Vector2 m_dcmErrorThreshold; /**< The threshold for activating the push recovery based on DCM error.*/ + iDynTree::Vector2 m_rollPitchErrorThreshold; /**< The threshold for activating the pendulum estimator based on the foot orientation error.*/ + iDynTree::Vector2 m_armRollPitchErrorOffset; /**< The offset for arm joints error to generate DCM error inside the simple DCM estimator to use in step adaptation*/ + + iDynTree::Vector2 m_currentZmpPosition; /**< The current step position(The zmp position of current stance foot). */ + iDynTree::Vector2 m_currentDcmPosition; /**< The current DCM position.*/ + + iDynTree::Vector2 m_zmpToCenterOfFootPositionLeft; + iDynTree::Vector2 m_zmpToCenterOfFootPositionRight; + + double m_remainingSingleSupportDuration;/**< The remained single support duration.*/ + double m_stepTiming; /**< The remanined single support duration+(next double support duration)/2 that is used for optimization.*/ + double m_stepDurationTolerance;/**< The tolerance of step timing with respect to the nominal value.*/ + double m_stepHeight;/**< The maximum height of swing foot.*/ + + double m_omega;/**< The natural frequency of LIPM.*/ + + double m_currentTime;/**< The current time.*/ + double m_nextDoubleSupportDuration;/**< The timing of next double support.*/ + + bool m_isPitchActive=0;/**< The boolean that shows whether push is detected with pitch arm joints or no.*/ + bool m_isRollActive=0;/**< The boolean that shows whether push is detected with roll arm joints or no.*/ + + double m_armRollError;/**< The error related to the roll joints position of the arm .*/ + double m_armPitchError;/**< The error related to the pitch joints position of the arm .*/ + + /** + *The buffered vectors for the interpolation of the foot trajectory + */ + iDynTree::VectorDynSize m_xPositionsBuffer, m_yPositionsBuffer, m_zFirstPiecePositionsBuffer,m_zSecondPiecePositionsBuffer, + m_yawsBuffer, m_timesBuffer, m_zFirstPieceTimesBuffer,m_zSecondPieceTimesBuffer; + + iDynTree::ConvexHullProjectionConstraint m_convexHullComputer; /**< iDynTree convex hull helper. */ + std::vector m_feetExtendedPolygon;/**< convex hull of the allowable landing foot position. */ + iDynTree::Transform m_footTransform; /**< transform of the next foot position. */ + + bool m_isSolutionEvaluated{false}; /**< True if the solution is evaluated. */ + + /** + * Compute the hessian matrix. + * Please do not call this function to update the hessian matrix! It can be set only once. + * @return true/false in case of success/failure. + */ + bool computeHessianMatrix(); + + /** + * Compute or update the linear constraints matrix(A) related to equality and inequality constraints(C& leftInContact, const std::deque& rightInContact, std::vector jointsListVector); + + /** + * Reset the controller + */ + void reset(); + + /** + * Set the nominal next step position and yaw angle + * @param nominalZmpPosition Nominal next step position(with a constant offset) + * @param angle yaw angle of the swing foot at landing moment. + * @return true/false in case of success/failure. + */ + void setNominalNextStepPosition(const iDynTree::Vector2& nominalZmpPosition, const double& angle); + + /** + * Set the varibales related to the timing of a step + * @param omega Nominal next step position(with a constant offset). + * @param currentTime current time of walking. + * @param nextImpactTime Next impact time + * @param nextDoubleSupportDuration Double support duration of the next step. + */ + void setTimings(const double & omega, const double & currentTime, const double& nextImpactTime, + const double &nextDoubleSupportDuration); + + /** + * Set the nominal DCM offset. + * @param nominalDcmOffset Nominal DCM offset. + * @return true/false in case of success/failure. + */ + void setNominalDcmOffset(const iDynTree::Vector2& nominalDcmOffset); + + /** + * Set the current ZMP(or with a constant offset the next step) Position. + * @param currentZmpPosition Current position of the zmp. + * @return true/false in case of success/failure. + */ + void setCurrentZmpPosition(const iDynTree::Vector2& currentZmpPosition); + + /** + * Set the current Dcm Position. + * @param currentDcmPosition Current position of the DCM. + * @return true/false in case of success/failure. + */ + void setCurrentDcmPosition(const iDynTree::Vector2& currentDcmPosition); + + /** + * Get the adapted step timing. + * @return The adapted step timing. + */ + double getDesiredImpactTime(); + + /** + * Get the adapted zmp(in another words, the next step position). + * @return The adapted zmp of the next step. + */ + iDynTree::Vector2 getDesiredZmp(); + + /** + * Get the next DCM Offset + * @return The DCM offset of the next step. + */ + iDynTree::Vector2 getDesiredDCMOffset(); + + /** + * Get the roll and pitch error threshold that has been set by the configuration file. + * @return 2D vector of roll and pitch error threshold that will be used for push detection. + */ + iDynTree::Vector2 getRollPitchErrorThreshold(); + + /** + * Get the DCM error threshold that has been set by the configuration file. + * @return 2D vector of the DCM error threshold that will be used for push detection. + */ + iDynTree::Vector2 getDCMErrorThreshold(); + + /** + * Replan the swing foot trajectory. + * @param input Structure that includes data that we need as input for function. + * @param adaptatedFootTransform Adapted transform of the swing foot. + * @param adaptedFootTwist Adapted twist of the swing foot. + * @param adaptedFootAcceleration Adapted acceleration of the swing foot. + * @return true/false in case of success/failure. + */ + bool getAdaptatedFootTrajectory(const FootTrajectoryGenerationInput &input, iDynTree::Transform& adaptatedFootTransform, + iDynTree::Twist& adaptedFootTwist, iDynTree::SpatialAcc& adaptedFootAcceleration); + + /** + * Get the Value of the arm joints roll error .. + * @return Value of the arm joints roll error . + */ + const double& getArmRollError()const; + + /** + * Get the Value of the arm joints pitch error .. + * @return Value of the arm joints pitch error . + */ + const double& getArmPitchError()const; + + /** + * Get the threshold of push recovery activation index . + * @return The integer threshold of push recovery activation index . + */ + const int &getPushRecoveryActivationIndex() const; + + /** + * Get the boolean to specify that the push in forward direction has been detected .. + * @return True in case that the pitch joints of the arm detect the push . + */ + bool isArmPitchActive(); + + /** + * Get the boolean to specify that the push in lateral direction has been detected .. + * @return True in a case that the roll+yaw joints of the arm detect the push . + */ + bool isArmRollActive(); + + /** + * Get the estimated position of the DCM. + * @return ertimated position of the DCM. + */ + const iDynTree::Vector2& getEstimatedDCM() const; + + /** + * update the pendulum estimator + * @param CoM2DPosition CoM2DPosition the 2D com position obtained as if the foot is not rotated. + * @param CoMVelocity the vector of com velocity that is simple time derivative of the com position. + * @param measuredZMP the vector of measured zmp position with respect to the inertial frame. + * @param CoMHeight the CoM height. + * @return true/false in case of success/failure + */ + bool UpdateDCMEstimator(const iDynTree::Vector2 &CoM2DPosition, const iDynTree::Vector2 &CoMVelocity, const iDynTree::Vector2 &measuredZMP, const double &CoMHeight, const double &yaw); + + + /** + * Run the step adaptation. + * @param input Structure that includes data that we need as input for function. + * @param output Structure that includes data that is output of step adjustment. + * @return true/false in case of success/failure. + */ + bool runStepAdaptation(const StepAdapterInput &input, StepAdapterOutput& output); + + }; +}; + +#endif diff --git a/src/StepAdaptationController/src/DCMSimpleEstimator.cpp b/src/StepAdaptationController/src/DCMSimpleEstimator.cpp new file mode 100644 index 000000000..ce14e89e1 --- /dev/null +++ b/src/StepAdaptationController/src/DCMSimpleEstimator.cpp @@ -0,0 +1,57 @@ +/** + * @file DCMSimpleEstimator.cpp + * @authors Milad Shafiee + * @copyright 2020 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2020 + */ + +// YARP +#include +#include + +//iDynTree +#include +#include +#include + +#include +#include + +using namespace WalkingControllers; + +bool DCMSimpleEstimator::configure(const yarp::os::Searchable& config) +{ + if(config.isNull()) + { + yError() << "[DCMSimpleEstimator::configure] Empty configuration."; + return false; + } + + double comHeight; + if(!YarpUtilities::getNumberFromSearchable(config, "com_height", comHeight)) + { + yError() << "[DCMSimpleEstimator::configure] Unable to get CoM height from configuration file. "; + return false; + } + double gravityAcceleration = config.check("gravity_acceleration", yarp::os::Value(9.81)).asDouble(); + + m_omega = sqrt(gravityAcceleration / comHeight); + return true; +} + +bool DCMSimpleEstimator::update(const iDynTree::Rotation& footOrientation,const iDynTree::Vector3& zmp,const iDynTree::Vector3& com,const iDynTree::LinVelocity& CoMVelocity3d) +{ + iDynTree::Vector3 CoMPositionEstimated; + iDynTree::toEigen(CoMPositionEstimated)=iDynTree::toEigen(zmp)+iDynTree::toEigen(footOrientation)*(iDynTree::toEigen(com)-iDynTree::toEigen(zmp)); + + m_dcmEstimatedPosition(0)=CoMPositionEstimated(0)+CoMVelocity3d(0)/m_omega; + m_dcmEstimatedPosition(1)=CoMPositionEstimated(1)+CoMVelocity3d(1)/m_omega; + + return true; +} + +const iDynTree::Vector2& DCMSimpleEstimator::getDCMPosition() const +{ + return m_dcmEstimatedPosition; +} diff --git a/src/StepAdaptationController/src/StepAdaptationController.cpp b/src/StepAdaptationController/src/StepAdaptationController.cpp new file mode 100644 index 000000000..2313d1675 --- /dev/null +++ b/src/StepAdaptationController/src/StepAdaptationController.cpp @@ -0,0 +1,1039 @@ +// std +#define NOMINMAX +#include + +// yarp +#include +#include + +// iDynTree +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +// eigen +#include + +typedef Eigen::Matrix MatrixXd; +using namespace WalkingControllers; + +typedef Eigen::Matrix MatrixXd; +using namespace WalkingControllers; + +StepAdaptationController::StepAdaptationController() + : m_inputSize(5) + , m_numberOfConstraints(7) + , m_xPositionsBuffer(2) + , m_yPositionsBuffer(2) + , m_zFirstPiecePositionsBuffer(3) + , m_zSecondPiecePositionsBuffer(2) + , m_yawsBuffer(2) + , m_timesBuffer(2) + , m_zFirstPieceTimesBuffer(3) + , m_zSecondPieceTimesBuffer(2) +{ + m_constraintsMatrix.resize(m_numberOfConstraints, m_inputSize); + m_upperBound.resize(m_numberOfConstraints); + m_lowerBound.resize(m_numberOfConstraints); + m_gradient.resize(m_inputSize); + + // set the constant elements of the constraint matrix + m_constraintsMatrix(0, 0) = 1; + m_constraintsMatrix(1, 1) = 1; + m_constraintsMatrix(0, 3) = 1; + m_constraintsMatrix(1, 4) = 1; + m_constraintsMatrix(6, 2) = 1; + + m_hessianMatrix.resize(m_inputSize, m_inputSize); + m_solution.resize(m_inputSize); + + // qpoases + m_QPSolver_qpOASES = std::make_unique(m_inputSize, + m_numberOfConstraints); + + m_QPSolver_qpOASES->setPrintLevel(qpOASES::PL_LOW); + m_isFirstTime = true; +} + +bool StepAdaptationController::configure(const yarp::os::Searchable &config) +{ + if(!YarpUtilities::getNumberFromSearchable(config, "stepHeight", m_stepHeight)) + { + yError() << "[StepAdaptationController::Configure] Unable to get the number"; + return false; + } + + if(!YarpUtilities::getVectorFromSearchable(config, "next_zmp_position_weight", m_zmpPositionWeight)) + { + yError() << "[StepAdaptationController::Configure] Unable to get the vector"; + return false; + } + + if(!YarpUtilities::getVectorFromSearchable(config, "next_dcm_offset_weight", m_dcmOffsetWeight)) + { + yError() << "[StepAdaptationController::Configure] Unable to get the vector"; + return false; + } + + if(!YarpUtilities::getNumberFromSearchable(config, "sigma_weight", m_sigmaWeight)) + { + yError() << "[StepAdaptationController::Configure] Unable to get the number"; + return false; + } + + if(!YarpUtilities::getNumberFromSearchable(config, "push_recovery_activation_index", m_pushRecoveryActivationIndex)) + { + yError() << "[StepAdaptationController::Configure] Unable to get the number"; + return false; + } + + if(!YarpUtilities::getVectorFromSearchable(config, "leftZMPDelta", m_zmpToCenterOfFootPositionLeft)) + { + yError() << "[StepAdaptationController::Configure] Unable to get the vector of leftZMPDelta"; + return false; + } + + if(!YarpUtilities::getVectorFromSearchable(config, "rightZMPDelta", m_zmpToCenterOfFootPositionRight)) + { + yError() << "[StepAdaptationController::Configure] Unable to get the vector of rightZMPDelta"; + return false; + } + + if(!YarpUtilities::getVectorFromSearchable(config, "offset_roll_pitch_arm_error", m_armRollPitchErrorOffset)) + { + yError() << "[StepAdaptationController::Configure] Unable to get the vector of offset_roll_pitch_arm_error"; + return false; + } + + yarp::os::Value *jointsListForPushDetectionYarpRX; + if(!config.check("joints_list_push_detection_right_arm_x", jointsListForPushDetectionYarpRX)) + { + yError() << "[StepAdaptationController::Configure] Unable to find joints_list into config file."; + return false; + } + + if(!YarpUtilities::yarpListToStringVector(jointsListForPushDetectionYarpRX, m_pushDetectionListRightArmX)) + { + yError() << "[StepAdaptationController::Configure] Unable to convert yarp list into a vector of strings."; + return false; + } + + yarp::os::Value *jointsListForPushDetectionYarpRY; + if(!config.check("joints_list_push_detection_right_arm_y", jointsListForPushDetectionYarpRY)) + { + yError() << "[StepAdaptationController::Configure] Unable to find joints_list into config file."; + return false; + } + + if(!YarpUtilities::yarpListToStringVector(jointsListForPushDetectionYarpRY, m_pushDetectionListRightArmY)) + { + yError() << "[StepAdaptationController::Configure] Unable to convert yarp list into a vector of strings."; + return false; + } + + yarp::os::Value *jointsListForPushDetectionYarpLX; + if(!config.check("joints_list_push_detection_left_arm_x", jointsListForPushDetectionYarpLX)) + { + yError() << "[StepAdaptationController::Configure] Unable to find joints_list into config file."; + return false; + } + + if(!YarpUtilities::yarpListToStringVector(jointsListForPushDetectionYarpLX, m_pushDetectionListLeftArmX)) + { + yError() << "[StepAdaptationController::Configure] Unable to convert yarp list into a vector of strings."; + return false; + } + + yarp::os::Value *jointsListForPushDetectionYarpLY; + if(!config.check("joints_list_push_detection_left_arm_y", jointsListForPushDetectionYarpLY)) + { + yError() << "[StepAdaptationController::Configure] Unable to find joints_list into config file."; + return false; + } + + if(!YarpUtilities::yarpListToStringVector(jointsListForPushDetectionYarpLY, m_pushDetectionListLeftArmY)) + { + yError() << "[StepAdaptationController::Configure] Unable to convert yarp list into a vector of strings."; + return false; + } + + m_feetExtendedPolygon.resize(2); + iDynTree::Polygon foot; + iDynTree::Vector4 nextZmpConstraintBoundLeftFoot; + if(!YarpUtilities::getVectorFromSearchable(config, "next_zmp_constraint_bound_left_foot", nextZmpConstraintBoundLeftFoot)) + { + yError() << "[StepAdaptationController::Configure] Unable to get the vector"; + return false; + } + + if(!YarpUtilities::getVectorFromSearchable(config, "threshold_dcm_error", m_dcmErrorThreshold)) + { + yError() << "[StepAdaptationController::Configure] Unable to get the vector of DCM error threshold"; + return false; + } + + if(!YarpUtilities::getVectorFromSearchable(config, "threshold_roll_pitch_error", m_rollPitchErrorThreshold)) + { + yError() << "[StepAdaptationController::Configure] Unable to get the vector of roll pitch imu error threshold"; + return false; + } + + foot = iDynTree::Polygon::XYRectangleFromOffsets(nextZmpConstraintBoundLeftFoot(0), nextZmpConstraintBoundLeftFoot(1), + nextZmpConstraintBoundLeftFoot(2), nextZmpConstraintBoundLeftFoot(3)); + m_feetExtendedPolygon[0] = foot; + + iDynTree::Vector4 nextZmpConstraintBoundRightFoot; + if(!YarpUtilities::getVectorFromSearchable(config, "next_zmp_constraint_bound_right_foot", nextZmpConstraintBoundRightFoot)) + { + yError() << "[StepAdaptationController::Configure] Unable to get the vector"; + return false; + } + + foot = iDynTree::Polygon::XYRectangleFromOffsets(nextZmpConstraintBoundRightFoot(0), nextZmpConstraintBoundRightFoot(1), + nextZmpConstraintBoundRightFoot(2), nextZmpConstraintBoundRightFoot(3)); + + m_feetExtendedPolygon[1] = foot; + + if(!YarpUtilities::getNumberFromSearchable(config, "step_duration_tolerance", m_stepDurationTolerance)) + { + yError() << "[StepAdaptationController::Configure] Unable to get the number"; + return false; + } + + if(!computeHessianMatrix()) + { + yError() << "[StepAdaptationController::Configure] Unable set the hessian"; + return false; + } + + //initialize the DCM simple estimator + m_DCMEstimator=std::make_unique(); + if(!m_DCMEstimator->configure(config)) + { + yError() << "[StepAdaptationController::Configure] Failed to configure the DCM pendulum estimator."; + return false; + } + m_isRollActive=0; + m_isPitchActive=0; + // reset the solver + reset(); + + return true; +} + +bool StepAdaptationController::computeHessianMatrix() +{ + m_hessianMatrix(0,0) = m_zmpPositionWeight(0); + m_hessianMatrix(1,1) = m_zmpPositionWeight(1); + + m_hessianMatrix(2,2) = m_sigmaWeight; + + m_hessianMatrix(3,3) = m_dcmOffsetWeight(0); + m_hessianMatrix(4,4) = m_dcmOffsetWeight(1); + + return true; +} + +bool StepAdaptationController::computeGradientVector() +{ + iDynTree::toEigen(m_gradient).segment(0, 2) = -(iDynTree::toEigen(m_zmpPositionWeight).asDiagonal() * iDynTree::toEigen(m_zmpPositionNominal)); + m_gradient(2) = -m_sigmaWeight * m_sigmaNominal; + iDynTree::toEigen(m_gradient).segment(3, 2) = -(iDynTree::toEigen(m_dcmOffsetWeight).asDiagonal() * iDynTree::toEigen(m_dcmOffsetNominal)); + + return true; +} + +bool StepAdaptationController::computeConstraintsMatrix() +{ + if(m_convexHullComputer.A.rows() != 4 || m_convexHullComputer.A.cols() != 2) + { + yError() << "QPSolver::setConstraintsMatrix the convex hull matrix size is strange " << m_convexHullComputer.A.toString(); + return false; + } + + iDynTree::Vector2 temp; + iDynTree::toEigen(temp) = iDynTree::toEigen(m_currentZmpPosition) - iDynTree::toEigen(m_currentDcmPosition); + + m_constraintsMatrix(0, 2) = temp(0); + m_constraintsMatrix(1, 2) = temp(1); + m_constraintsMatrix(2, 0) = m_convexHullComputer.A(0, 0); + m_constraintsMatrix(2, 1) = m_convexHullComputer.A(0, 1); + m_constraintsMatrix(3, 0) = m_convexHullComputer.A(1, 0); + m_constraintsMatrix(3, 1) = m_convexHullComputer.A(1, 1); + m_constraintsMatrix(4, 0) = m_convexHullComputer.A(2, 0); + m_constraintsMatrix(4, 1) = m_convexHullComputer.A(2, 1); + m_constraintsMatrix(5, 0) = m_convexHullComputer.A(3, 0); + m_constraintsMatrix(5, 1) = m_convexHullComputer.A(3, 1); + + return true; +} + +bool StepAdaptationController::computeBoundsVectorOfConstraints() +{ + if(m_convexHullComputer.b.size() != 4) + { + yError() << "QPSolver::setConstraintsVector the convex hull vector size is strange " << m_convexHullComputer.b.toString(); + return false; + } + + iDynTree::toEigen(m_upperBound).segment(0, 2) = iDynTree::toEigen(m_currentZmpPosition); + iDynTree::toEigen(m_lowerBound).segment(0, 2) = iDynTree::toEigen(m_currentZmpPosition); + iDynTree::toEigen(m_upperBound).segment(2, 4) = iDynTree::toEigen(m_convexHullComputer.b); + + m_lowerBound(2) = -qpOASES::INFTY; + m_lowerBound(3) = -qpOASES::INFTY; + m_lowerBound(4) = -qpOASES::INFTY; + m_lowerBound(5) = -qpOASES::INFTY; + + m_upperBound(6) = std::exp((m_stepTiming + m_stepDurationTolerance) * m_omega); + m_lowerBound(6) = std::exp((m_stepTiming - std::min(m_stepDurationTolerance, m_remainingSingleSupportDuration)) * m_omega); + + return true; +} + +void StepAdaptationController::setNominalNextStepPosition(const iDynTree::Vector2& nominalZmpPosition, const double& angle) +{ + m_zmpPositionNominal = nominalZmpPosition; + m_footTransform.setPosition(iDynTree::Position(nominalZmpPosition(0), nominalZmpPosition(1), 0)); + m_footTransform.setRotation(iDynTree::Rotation::RPY(0, 0, angle)); +} + +void StepAdaptationController::setTimings(const double & omega, const double & currentTime, const double& nextImpactTime, + const double &nextDoubleSupportDuration) +{ + m_nextDoubleSupportDuration = nextDoubleSupportDuration; + m_currentTime = currentTime; + + m_stepTiming = nextImpactTime + nextDoubleSupportDuration / 2 - currentTime; + m_remainingSingleSupportDuration = nextImpactTime - currentTime; + + m_sigmaNominal = std::exp(omega * m_stepTiming); + m_omega = omega; +} + +void StepAdaptationController::setNominalDcmOffset(const iDynTree::Vector2& nominalDcmOffset) +{ + m_dcmOffsetNominal = nominalDcmOffset; +} + +void StepAdaptationController::setCurrentZmpPosition(const iDynTree::Vector2& currentZmpPosition) +{ + m_currentZmpPosition = currentZmpPosition; +} + +void StepAdaptationController::setCurrentDcmPosition(const iDynTree::Vector2& currentDcmPosition) +{ + m_currentDcmPosition = currentDcmPosition; +} + +bool StepAdaptationController::solve(SwingFoot swingFoot) +{ + + // generate the convex hull + iDynTree::Direction xAxis, yAxis; + xAxis.zero(); + xAxis(0) = 1; + yAxis.zero(); + yAxis(1) = 1; + + // initilize plane origin + iDynTree::Position planeOrigin; + planeOrigin.zero(); + + std::vector feetTransforms; + feetTransforms.push_back(m_footTransform); + + if(swingFoot==SwingFoot::Left) + { + + if(!m_convexHullComputer.buildConvexHull(xAxis, yAxis, planeOrigin, + std::vector(1, m_feetExtendedPolygon[0]), + feetTransforms)) + { + yInfo() << "unable to build the convex hull (left)"; + return false; + } + } + else + { + + if(!m_convexHullComputer.buildConvexHull(xAxis, yAxis, planeOrigin, + std::vector(1, m_feetExtendedPolygon[1]), + feetTransforms)) + { + yInfo() << "unable to build the convex hull (right)"; + return false; + } + } + + if (!computeGradientVector()) + { + yError() << "[StepAdaptationController::RunStepAdaptationController] Unable to set the Gradient Vector"; + return false; + } + + if(!computeConstraintsMatrix()) + { + yError() << "[StepAdaptationController::RunStepAdaptationController] Unable to set the constraint matrix"; + return false; + } + + if(!computeBoundsVectorOfConstraints()) + { + yError() << "[StepAdaptationController::RunStepAdaptationController] Unable to set the bounds"; + return false; + } + + m_isSolutionEvaluated = false; + + MatrixXd constraintMatrix = MatrixXd(iDynTree::toEigen(m_constraintsMatrix)); + MatrixXd hessianMatrix = MatrixXd(iDynTree::toEigen(m_hessianMatrix)); + + int nWSR = 100; + if(!m_isFirstTime) + { + if(m_QPSolver_qpOASES->hotstart(hessianMatrix.data(), m_gradient.data(), constraintMatrix.data(), + nullptr, nullptr,m_lowerBound.data(), m_upperBound.data(), nWSR, 0) + != qpOASES::SUCCESSFUL_RETURN) + { + yError() << "[solve] Unable to solve the optimization problem."; + return false; + } + } + else + { + if(m_QPSolver_qpOASES->init(hessianMatrix.data(), m_gradient.data(), constraintMatrix.data(), + nullptr, nullptr,m_lowerBound.data(), m_upperBound.data(), nWSR, 0) + != qpOASES::SUCCESSFUL_RETURN) + { + yError() << "[solve] Unable to solve the optimization problem."; + return false; + } + m_isFirstTime = false; + } + + m_QPSolver_qpOASES->getPrimalSolution(m_solution.data()); + + m_isSolutionEvaluated = true; + return true; +} + +double StepAdaptationController::getDesiredImpactTime() +{ + double optimalStepDuration = std::log(m_solution(2)) / m_omega; + return m_currentTime + optimalStepDuration - m_nextDoubleSupportDuration / 2; +} + +iDynTree::Vector2 StepAdaptationController::getDesiredZmp() +{ + iDynTree::Vector2 desiredZmp; + desiredZmp(0) = m_solution(0); + desiredZmp(1) = m_solution(1); + return desiredZmp; +} + +iDynTree::Vector2 StepAdaptationController::getDesiredDCMOffset() +{ + iDynTree::Vector2 desiredDCMOffset; + desiredDCMOffset(0) = m_solution(3); + desiredDCMOffset(1) = m_solution(4); + return desiredDCMOffset; +} + +bool StepAdaptationController::getAdaptatedFootTrajectory(const FootTrajectoryGenerationInput& input, + iDynTree::Transform& adaptatedFootTransform, iDynTree::Twist& adaptedFootTwist, + iDynTree::SpatialAcc& adaptedFootAcceleration) +{ + + iDynTree::CubicSpline xSpline, ySpline, zSpline, yawSpline; + + if(m_currentTime >= getDesiredImpactTime()) + { + adaptedFootTwist.zero(); + iDynTree::Position newPosition; + newPosition(0)=getDesiredZmp()(0) - (cos(input.yawAngleAtImpact) * input.zmpToCenterOfFootPosition(0) - sin(input.yawAngleAtImpact) * input.zmpToCenterOfFootPosition(1)); + newPosition(1)=getDesiredZmp()(1) - (cos(input.yawAngleAtImpact) * input.zmpToCenterOfFootPosition(1) + sin(input.yawAngleAtImpact) * input.zmpToCenterOfFootPosition(0)); + newPosition(2)= 0; + adaptatedFootTransform.setPosition(newPosition); + + adaptatedFootTransform.setRotation(iDynTree::Rotation::RPY(0.0, 0.0, input.yawAngleAtImpact)); + + return true; + } + + double maxFootHeightTime = (getDesiredImpactTime() - input.takeOffTime) * 0.8 + input.takeOffTime; + if (m_currentTime < maxFootHeightTime) + { + m_zFirstPieceTimesBuffer(0)= m_currentTime; + m_zFirstPieceTimesBuffer(1)= maxFootHeightTime; + m_zFirstPieceTimesBuffer(2)= getDesiredImpactTime(); + m_zFirstPiecePositionsBuffer(0)= input.currentFootTransform.getPosition()(2); + m_zFirstPiecePositionsBuffer(1)= input.maxFootHeight; + m_zFirstPiecePositionsBuffer(2)= 0; + + zSpline.setInitialConditions(input.currentFootTwist.getLinearVec3()(2), 0.0); + zSpline.setFinalConditions(0.0,0.0); + + if (!zSpline.setData(m_zFirstPieceTimesBuffer, m_zFirstPiecePositionsBuffer)) + { + std::cerr << "[StepAdaptationController::getAdaptatedFootTrajectory] Failed to initialize the z-dimension spline." << std::endl; + return false; + } + } + else + { + m_zSecondPieceTimesBuffer(0)= m_currentTime; + m_zSecondPieceTimesBuffer(1)= getDesiredImpactTime(); + + iDynTree::Position PositionsBuffer=input.currentFootTransform.getPosition(); + m_zSecondPiecePositionsBuffer(0)=PositionsBuffer(2); + m_zSecondPiecePositionsBuffer(1)= 0; + zSpline.setInitialConditions(input.currentFootTwist.getLinearVec3()(2), 0.0); + zSpline.setFinalConditions(0.0,0.0); + + if (!zSpline.setData(m_zSecondPieceTimesBuffer, m_zSecondPiecePositionsBuffer)) + { + std::cerr << "[StepAdaptationController::getAdaptatedFootTrajectory] Failed to initialize the z-dimension spline." << std::endl; + return false; + } + } + + m_xPositionsBuffer(0)= input.currentFootTransform.getPosition()(0); + m_yPositionsBuffer(0)= input.currentFootTransform.getPosition()(1); + + + iDynTree::Rotation initialRotation = iDynTree::Rotation::RotZ(input.currentFootTransform.getRotation().asRPY()(2)); + iDynTree::Rotation finalRotation = iDynTree::Rotation::RotZ(input.yawAngleAtImpact); + + m_yawsBuffer(0) = input.yawAngleAtImpact - (initialRotation.inverse()*finalRotation).asRPY()(2); + + m_xPositionsBuffer(1)= getDesiredZmp()(0) - (cos(input.yawAngleAtImpact) * input.zmpToCenterOfFootPosition(0) - sin(input.yawAngleAtImpact) * input.zmpToCenterOfFootPosition(1)); + m_yPositionsBuffer(1)= getDesiredZmp()(1) - (cos(input.yawAngleAtImpact) * input.zmpToCenterOfFootPosition(1) + sin(input.yawAngleAtImpact) * input.zmpToCenterOfFootPosition(0)); + + m_yawsBuffer(1) = input.yawAngleAtImpact; + + m_timesBuffer(0) = m_currentTime; + m_timesBuffer(1) = getDesiredImpactTime(); + + double yawAngle; + + iDynTree::AngularMotionVector3 rightTrivializedAngVelocity; + iDynTree::Vector3 rpyDerivativeCurrent; + iDynTree::Vector3 rpyDerivative; + iDynTree::toEigen(rpyDerivativeCurrent) = iDynTree::toEigen(iDynTree::Rotation::RPYRightTrivializedDerivativeInverse(0.0, 0.0, m_yawsBuffer(0))) * iDynTree::toEigen(input.currentFootTwist.getAngularVec3()); + + xSpline.setInitialConditions(input.currentFootTwist.getLinearVec3()(0), 0.0); + ySpline.setInitialConditions(input.currentFootTwist.getLinearVec3()(1), 0.0); + yawSpline.setInitialConditions(rpyDerivativeCurrent(2), 0.0); + + xSpline.setFinalConditions(0.0,0.0); + ySpline.setFinalConditions(0.0,0.0); + yawSpline.setFinalConditions(0.0, 0.0); + + if (!xSpline.setData(m_timesBuffer, m_xPositionsBuffer)){ + std::cerr << "[StepAdaptationController::getAdaptatedFootTrajectory] Failed to initialize the x-dimension spline." << std::endl; + return false; + } + if (!ySpline.setData(m_timesBuffer, m_yPositionsBuffer)){ + std::cerr << "[StepAdaptationController::getAdaptatedFootTrajectory] Failed to initialize the y-dimension spline." << std::endl; + return false; + } + + if (!yawSpline.setData(m_timesBuffer, m_yawsBuffer)){ + std::cerr << "[StepAdaptationController::getAdaptatedFootTrajectory] Failed to initialize the yaw-dimension spline." << std::endl; + return false; + } + + iDynTree::Transform newTransform; + iDynTree::Position newPosition; + iDynTree::Vector3 linearVelocity; + iDynTree::Vector3 linearAcceleration; + iDynTree::Vector3 rpySecondDerivative; + + newPosition(0) = xSpline.evaluatePoint(m_currentTime + input.discretizationTime, linearVelocity(0), linearAcceleration(0)); + newPosition(1) = ySpline.evaluatePoint(m_currentTime + input.discretizationTime, linearVelocity(1), linearAcceleration(1)); + newPosition(2) = zSpline.evaluatePoint(m_currentTime + input.discretizationTime, linearVelocity(2), linearAcceleration(2)); + + yawAngle = yawSpline.evaluatePoint(m_currentTime + input.discretizationTime, rpyDerivative(2), rpySecondDerivative(2)); + + adaptatedFootTransform.setPosition(newPosition); + adaptatedFootTransform.setRotation(iDynTree::Rotation::RPY(0.0, 0.0, yawAngle)); + + rpyDerivative(0)=0; + rpyDerivative(1)=0; + + iDynTree::toEigen(rightTrivializedAngVelocity) = iDynTree::toEigen(iDynTree::Rotation::RPYRightTrivializedDerivative(0.0, 0.0, yawAngle)) *iDynTree::toEigen(rpyDerivative); + adaptedFootTwist.setLinearVec3(linearVelocity); + adaptedFootTwist.setAngularVec3(rightTrivializedAngVelocity); + adaptedFootAcceleration.setLinearVec3(linearAcceleration); + iDynTree::Vector3 vector; + vector.zero(); + adaptedFootAcceleration.setAngularVec3(vector); + + return true; +} + +bool StepAdaptationController::triggerStepAdapterByArmCompliant(const double &numberOfActuatedDof, const iDynTree::VectorDynSize &qDesired, const iDynTree::VectorDynSize &qActual, + const std::deque& leftInContact,const std::deque& rightInContact,std::vector jointsListVector) +{ + double leftArmPitchError=0; + double rightArmPitchError=0; + double leftArmRollError=0; + double rightArmRollError=0; + m_isRollActive=0; + m_isPitchActive=0; + m_armRollError=0; + + for (int var=0;var::iterator it = std::find(jointsListVector.begin(), jointsListVector.end(), m_pushDetectionListRightArmX.at(var)); + if(it == jointsListVector.end()) + { + yError() << "[StepAdaptationController::triggerStepAdapterByArmCompliant] Unable to to find"<< m_pushDetectionListRightArmX.at(var)<<"inside the controlled joints of the robot"; + return false; + } + + rightArmPitchError=rightArmPitchError+(qDesired(std::distance(jointsListVector.begin(), it))-qActual(std::distance(jointsListVector.begin(), it))); + } + + for (int var=0;var::iterator it = std::find(jointsListVector.begin(), jointsListVector.end(), m_pushDetectionListRightArmY.at(var)); + if(it == jointsListVector.end()) + { + yError() << "[StepAdaptationController::triggerStepAdapterByArmCompliant] Unable to to find"<< m_pushDetectionListRightArmY.at(var)<<"inside the controlled joints of the robot"; + return false; + } + + rightArmRollError=rightArmRollError+abs(qDesired(std::distance(jointsListVector.begin(), it))-qActual(std::distance(jointsListVector.begin(), it))); + } + + for (int var=0;var::iterator it = std::find(jointsListVector.begin(), jointsListVector.end(), m_pushDetectionListLeftArmX.at(var)); + if(it == jointsListVector.end()) + { + yError() << "[StepAdaptationController::triggerStepAdapterByArmCompliant] Unable to to find"<< m_pushDetectionListLeftArmX.at(var)<<"inside the controlled joints of the robot"; + return false; + } + + leftArmPitchError=leftArmPitchError+(qDesired(std::distance(jointsListVector.begin(), it))-qActual(std::distance(jointsListVector.begin(), it))); + } + + for (int var=0;var::iterator it = std::find(jointsListVector.begin(), jointsListVector.end(), m_pushDetectionListLeftArmY.at(var)); + if(it == jointsListVector.end()) + { + yError() << "[StepAdaptationController::triggerStepAdapterByArmCompliant] Unable to to find"<< m_pushDetectionListLeftArmY.at(var)<<"inside the controlled joints of the robot"; + return false; + } + + leftArmRollError=leftArmRollError+abs(qDesired(std::distance(jointsListVector.begin(), it))-qActual(std::distance(jointsListVector.begin(), it))); + } + + if(leftArmPitchError>getRollPitchErrorThreshold()(1)) + { + m_armPitchError=leftArmPitchError; + m_isPitchActive=1; + } + else if(rightArmPitchError>getRollPitchErrorThreshold()(1)) + { + m_armPitchError=rightArmPitchError; + m_isPitchActive=1; + } + else + { + m_armPitchError=0; + } + + if(abs(leftArmRollError)>getRollPitchErrorThreshold()(0) && abs(leftArmRollError)>abs(rightArmRollError)) + { + if(!((m_pushStepNumber+1)==(m_stepCounter))) + { + if (leftInContact.front() ) + { + m_armRollError=1*leftArmRollError; + m_isRollActive=1; + m_pushStepNumber=m_stepCounter; + } + } + else + { + m_isRollActive=0; + m_armRollError=0; + } + } + else if(abs(rightArmRollError)>getRollPitchErrorThreshold()(0) && abs(rightArmRollError)>abs(leftArmRollError)) + { + if(!((m_pushStepNumber+1)==(m_stepCounter))) + { + if (rightInContact.front()) + { + m_armRollError=-1*rightArmRollError; + m_isRollActive=1; + m_pushStepNumber=m_stepCounter; + } + } + else + { + m_isRollActive=0; + m_armRollError=0; + } + } + else + { + m_armRollError=0; + } + + return true; +} + +const double& StepAdaptationController::getArmRollError() const +{ + return m_armRollError; +} + +const double& StepAdaptationController::getArmPitchError() const +{ + return m_armPitchError; +} + +const int& StepAdaptationController::getPushRecoveryActivationIndex() const +{ + return m_pushRecoveryActivationIndex; +} + +bool StepAdaptationController::UpdateDCMEstimator(const iDynTree::Vector2& CoM2DPosition,const iDynTree::Vector2& CoMVelocity,const iDynTree::Vector2& measuredZMP,const double& CoMHeight,const double & yaw) +{ + iDynTree::Rotation imuRotation; + iDynTree::Vector3 imuRPY; + iDynTree::Rotation relativeStanceFootOrientation; + iDynTree::Rotation stanceFootOrientation; + stanceFootOrientation=iDynTree::Rotation::Identity(); + relativeStanceFootOrientation=iDynTree::Rotation::Identity(); + int sign=0; + double Yaw=0; + if (m_isRollActive) + { + if(m_armRollError>0) + { + sign=1; + } + else if(m_armRollError<0) + { + sign=-1; + } + else + { + sign=0; + } + Yaw=yaw; + m_armRollError= (m_armRollError+sign*m_armRollPitchErrorOffset(0)); + } + else + { + m_armRollError=0; + } + if (m_isPitchActive) + { + if(m_armPitchError>0) + { + sign=1; + } + else if(m_armPitchError<0) + { + sign=-1; + } + else + { + sign=0; + } + Yaw=yaw; + m_armPitchError= m_armPitchError+sign*m_armRollPitchErrorOffset(1); + } + else + { + m_armPitchError=0; + } + + relativeStanceFootOrientation=iDynTree::Rotation::RPY (m_armRollError,m_armPitchError,0); //=m_FKSolver->getRootLinkToWorldTransform().getRotation().asRPY(); + iDynTree::toEigen(stanceFootOrientation)=iDynTree::toEigen(iDynTree::Rotation::RotZ(Yaw))*iDynTree::toEigen(relativeStanceFootOrientation); + iDynTree::Vector3 ZMP3d; + ZMP3d(0)=measuredZMP(0); + ZMP3d(1)=measuredZMP(1); + ZMP3d(2)=0; + + iDynTree::Vector3 CoM3d; + //iDynTree::Vector3 ZMP3d; + CoM3d(0)=CoM2DPosition(0); + CoM3d(1)=CoM2DPosition(1); + CoM3d(2)=CoMHeight; + + iDynTree::Vector3 CoMVelocity3d; + //iDynTree::Vector3 ZMP3d; + CoMVelocity3d(0)=CoMVelocity(0); + CoMVelocity3d(1)=CoMVelocity(1); + CoMVelocity3d(2)=0; + + if(!m_DCMEstimator->update(stanceFootOrientation,ZMP3d,CoM3d,CoMVelocity3d)) + { + yError() << "[StepAdaptationController::UpdateDCMEstimator] Unable to to recieve DCM from pendulumEstimator"; + return false; + } + + return true; +} + +bool StepAdaptationController::runStepAdaptation(const StepAdapterInput &input, StepAdapterOutput &output) +{ + if((input.leftInContact.front() && input.rightInContact.front()) && m_numberOfStepFlag) + { + m_stepCounter=m_stepCounter+1; + m_numberOfStepFlag=false; + } + if (!input.leftInContact.front() || !input.rightInContact.front()) + { m_numberOfStepFlag=true; + output.indexPush=output.indexPush+1; + + int numberOfSubTrajectories = input.dcmSubTrajectories.size(); + + if(numberOfSubTrajectories<4) + { + yError() << "[StepAdaptationController::runStepAdaptation] the number of sub-trajectories should be equal or greater than 4"; + return false; + } + + auto firstSS = input.dcmSubTrajectories[numberOfSubTrajectories-2]; + auto secondSS = input.dcmSubTrajectories[numberOfSubTrajectories-4]; + + auto secondDS = input.dcmSubTrajectories[numberOfSubTrajectories-3]; + auto firstDS = input.dcmSubTrajectories[numberOfSubTrajectories-1]; + + iDynTree::Vector2 nextZmpPosition, currentZmpPosition; + bool checkFeasibility = false; + if(!secondSS->getZMPPosition(0, nextZmpPosition, checkFeasibility)) + { + yError() << "[StepAdaptationController::runStepAdaptation] unable to get ZMP Position for second single support"; + return false; + } + + double angle = !input.leftInContact.front()? input.leftFootprints->getSteps()[1].angle : input.rightFootprints->getSteps()[1].angle; + setNominalNextStepPosition(nextZmpPosition, angle); + if(!firstSS->getZMPPosition(0, currentZmpPosition, checkFeasibility)) + { + yError() << "[StepAdaptationController::runStepAdaptation] unable to get ZMP Position for first single support"; + return false; + } + + setCurrentZmpPosition(currentZmpPosition); + + output.isPushActive=0; + + if((abs(input.dcmPositionSmoothed(0) - getEstimatedDCM()(0))) > getDCMErrorThreshold()(0) ||(abs(input.dcmPositionSmoothed(1) - getEstimatedDCM()(1)))> getDCMErrorThreshold()(1) ) + { + if (output.pushRecoveryActiveIndex==m_pushRecoveryActivationIndex ) + { + output.isPushActive=1; + iDynTree::Vector2 tempDCMError; + tempDCMError=output.dcmPositionAdjusted.front(); + output.pushRecoveryActiveIndex++; + yInfo()<<"triggering the push recovery"; + if((abs(input.dcmPositionSmoothed(0) - getEstimatedDCM()(0))) > getDCMErrorThreshold()(0)) + { + tempDCMError(0)=getEstimatedDCM()(0); + } + if((abs(input.dcmPositionSmoothed(1) - getEstimatedDCM()(1))) > getDCMErrorThreshold()(1)) + { + if (!input.leftInContact.front()) + { + tempDCMError(1)=getEstimatedDCM()(1); + } + else + { + tempDCMError(1)=getEstimatedDCM()(1); + } + } + setCurrentDcmPosition(tempDCMError); + } + else + { + output.pushRecoveryActiveIndex++; + setCurrentDcmPosition(output.dcmPositionAdjusted.front()); + } + + } + else + { + if (output.pushRecoveryActiveIndex<=m_pushRecoveryActivationIndex) + { + output.pushRecoveryActiveIndex=0; + setCurrentDcmPosition(output.dcmPositionAdjusted.front()); + } + else if(output.pushRecoveryActiveIndex==(m_pushRecoveryActivationIndex+1)) + { + setCurrentDcmPosition(output.dcmPositionAdjusted.front()); + output.pushRecoveryActiveIndex++; + } + else + { + setCurrentDcmPosition(output.dcmPositionAdjusted.front()); + } + } + + iDynTree::Vector2 dcmAtTimeAlpha; + double timeAlpha = (secondDS->getTrajectoryDomain().second + secondDS->getTrajectoryDomain().first) / 2; + + if(!input.dcmSubTrajectories[numberOfSubTrajectories-2]->getDCMPosition(timeAlpha, dcmAtTimeAlpha, checkFeasibility)) + { + yError() << "[StepAdaptationController::runStepAdaptation] unable to get DCM Position "; + return false; + } + + iDynTree::Vector2 nominalDcmOffset; + iDynTree::toEigen(nominalDcmOffset) = iDynTree::toEigen(dcmAtTimeAlpha) - iDynTree::toEigen(nextZmpPosition); + setNominalDcmOffset(nominalDcmOffset); + + //timeOffset is the time of start of this step(that will be updated in updateTrajectory function at starting point of each step ) + setTimings(input.omega, input.time - input.timeOffset, firstSS->getTrajectoryDomain().second, + secondDS->getTrajectoryDomain().second - secondDS->getTrajectoryDomain().first); + + SwingFoot swingFoot; + if (!input.leftInContact.front()) + { + swingFoot=SwingFoot::Left; + } + else + { + swingFoot=SwingFoot::Right; + } + + if(!solve(swingFoot)) + { + yError() << "[StepAdaptationController::runStepAdaptation] unable to solve the step adjustment optimization problem"; + return false; + } + + output.impactTimeNominal = firstSS->getTrajectoryDomain().second + input.timeOffset; + if(output.pushRecoveryActiveIndex==(m_pushRecoveryActivationIndex+1)) + { + double timeOfSmoothing=(secondDS->getTrajectoryDomain().second-secondDS->getTrajectoryDomain().first)/2 +getDesiredImpactTime()-(input.time - input.timeOffset); + output.indexSmoother=timeOfSmoothing/input.dT; + output.kDCMSmoother=0; + } + if(output.pushRecoveryActiveIndex==(m_pushRecoveryActivationIndex+1)) + { + double timeOfSmoothing=getDesiredImpactTime()-(input.time - input.timeOffset); + output.indexFootSmoother=timeOfSmoothing/input.dT; + output.kFootSmoother=0; + } + output.impactTimeAdjusted = getDesiredImpactTime() + input.timeOffset; + + output.zmpNominal = nextZmpPosition; + output.zmpAdjusted = getDesiredZmp(); + + iDynTree::Vector2 zmpOffset; + if (!input.leftInContact.front()) + { + zmpOffset=m_zmpToCenterOfFootPositionLeft; + } + if (!input.rightInContact.front()) + { + zmpOffset=m_zmpToCenterOfFootPositionRight; + } + + if (!input.leftInContact.front()) + { + output.currentFootLeftTransform = output.adaptedFootLeftTransform; + output.currentFootLeftTwist = output.adaptedFootLeftTwist; + output.currentFootLeftAcceleration = output.adaptedFootLeftAcceleration; + + FootTrajectoryGenerationInput inputLeftFootTrajectory; + inputLeftFootTrajectory.maxFootHeight=m_stepHeight; + inputLeftFootTrajectory.discretizationTime=input.dT; + inputLeftFootTrajectory.takeOffTime =firstSS->getTrajectoryDomain().first; + inputLeftFootTrajectory.yawAngleAtImpact=input.leftStepList.at(1).angle; + inputLeftFootTrajectory.zmpToCenterOfFootPosition=zmpOffset; + inputLeftFootTrajectory.currentFootTransform=output.currentFootLeftTransform; + inputLeftFootTrajectory.currentFootTwist=output.currentFootLeftTwist; + + if(!getAdaptatedFootTrajectory(inputLeftFootTrajectory, + output.adaptedFootLeftTransform, output.adaptedFootLeftTwist, output.adaptedFootLeftAcceleration )) + { + yError() << "[StepAdaptationController::runStepAdaptation] unable to get get adaptated left foot trajectory"; + return false; + } + } + else + { + + output.currentFootRightTransform = output.adaptedFootRightTransform; + output.currentFootRightTwist = output.adaptedFootRightTwist; + output.currentFootRightAcceleration = output.adaptedFootRightAcceleration; + + FootTrajectoryGenerationInput inputRightFootTrajectory; + inputRightFootTrajectory.maxFootHeight=m_stepHeight; + inputRightFootTrajectory.discretizationTime=input.dT; + inputRightFootTrajectory.takeOffTime =firstSS->getTrajectoryDomain().first; + inputRightFootTrajectory.yawAngleAtImpact=input.rightStepList.at(1).angle; + inputRightFootTrajectory.zmpToCenterOfFootPosition=zmpOffset; + inputRightFootTrajectory.currentFootTransform=output.currentFootRightTransform; + inputRightFootTrajectory.currentFootTwist=output.currentFootRightTwist; + + if(!getAdaptatedFootTrajectory(inputRightFootTrajectory, + output.adaptedFootRightTransform, output.adaptedFootRightTwist, output.adaptedFootRightAcceleration )) + { + yError() << "[StepAdaptationController::runStepAdaptation] unable to get the adaptated right foot trajectory"; + return false; + } + } + + + } + + else + { + output.currentFootLeftAcceleration=output.adaptedFootLeftAcceleration; + output.currentFootLeftTwist=output.adaptedFootLeftTwist; + output.currentFootLeftTransform=output.adaptedFootLeftTransform; + + output.currentFootRightAcceleration=output.adaptedFootRightAcceleration; + output.currentFootRightTwist=output.adaptedFootRightTwist; + output.currentFootRightTransform=output.adaptedFootRightTransform; + } + return true; +} + +const iDynTree::Vector2& StepAdaptationController::getEstimatedDCM() const +{ + return m_DCMEstimator->getDCMPosition(); +} + +bool StepAdaptationController::isArmRollActive() +{ + return m_isRollActive; +} + +bool StepAdaptationController::isArmPitchActive() +{ + return m_isPitchActive; +} + +iDynTree::Vector2 StepAdaptationController::getDCMErrorThreshold(){ + return m_dcmErrorThreshold; +} + +iDynTree::Vector2 StepAdaptationController::getRollPitchErrorThreshold(){ + return m_rollPitchErrorThreshold; +} + +void StepAdaptationController::reset(){ + m_isSolutionEvaluated = false; +} diff --git a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h index 1ed205190..cf80e0e88 100644 --- a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h +++ b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h @@ -51,6 +51,9 @@ namespace WalkingControllers double m_nominalWidth; /**< Nominal width between two feet. */ double m_initTime; /**< Init time of the current trajectory. */ + double m_nominalCoMHeight; /**< Nominal CoM height during walking. */ + double m_switchOverSwingRatio;/**< Double support suration devided by single support duration. */ + iDynTree::Vector2 m_referencePointDistance; /**< Vector between the center of the unicycle and the point that has to be reach the goal. */ GeneratorState m_generatorState{GeneratorState::NotConfigured}; /**< Useful to track the generator state. */ @@ -240,6 +243,81 @@ namespace WalkingControllers * Reset the planner */ void reset(); + + /** + * Get the desired 2D-ZMP position trajectory + * @param ZMPPositionTrajectory desired trajectory of the ZMP. + * @return true/false in case of success/failure. + */ + bool getZMPPositionTrajectory(std::vector& ZMPPositionTrajectory); + + /** + * Get the desired trajectories + * @param dcmSubTrajectories desired trajectories. + * @return true/false in case of success/failure. + */ + bool getDCMSubTrajectories(std::vector>& dcmSubTrajectories); + + /** + * Get the feet acceleration + * @param lFootAcceleration vector containing the left foot acceleration; + * @param rFootAcceleration vector containing the right foot acceleration. + * @return true/false in case of success/failure. + */ + bool getFeetAcceleration(std::vector& lFootAcceleration, std::vector& rFootAcceleration); + + /** + * Generate trajectories for a given footprints + * @param left the left foot footprint; + * @param right the right foot footprint; + * @param initTime + * @param initialState + * @return true/false in case of success/failure. + */ + bool generateTrajectoriesFromFootprints(std::shared_ptr left, std::shared_ptr right, const double &initTime, DCMInitialState initialState); + + /** + * Get the phases of each foot during walking from unicycle + * @param leftPhases vector containing all the phases that left foot experience. + * @param rightPhases vector containing all the phases that right foot experience. + * @return true/false in case of success/failure. + */ + bool getStepPhases(std::vector &leftPhases, std::vector &rightPhases); + + /** + * Get the left foot print + * @param leftFootPrint pointer to the left footprint + * @return true/false in case of success/failure. + */ + bool getLeftFootprint(std::shared_ptr& leftFootPrint); + + /** + * Get the right foot print + * @param rightFootPrint pointer to the right footprint + * @return true/false in case of success/failure. + */ + bool getRightFootprint(std::shared_ptr& rightFootPrint); + + /** + * Get the Nominal CoM height trajectory for omega calculation + * @param nominalCoMHeight nominal CoM height + * @return true/false in case of success/failure. + */ + bool getNominalCoMHeight(double & nominalCoMHeight); + + /** + * Get the ratio of double support to single support + * @param switchOverSwingRatio returning ratio of double support to single support + * @return true/false in case of success/failure. + */ + bool getSwitchOverSwingRatio(double &switchOverSwingRatio); + + /** + * Get the DCM boundary condition at merge point + * @param DCMBoundryConditionAtMergePoint is DCM boundary condition at merge point + * @return true/false in case of success/failure. + */ + bool getDCMBoundaryConditionAtMergePoint(DCMInitialState DCMBoundryConditionAtMergePoint); }; }; diff --git a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp index 95221c5b3..38c73609b 100644 --- a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp +++ b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp @@ -96,6 +96,7 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) double landingVelocity = config.check("stepLandingVelocity", yarp::os::Value(0.0)).asDouble(); double apexTime = config.check("footApexTime", yarp::os::Value(0.5)).asDouble(); double comHeight = config.check("com_height", yarp::os::Value(0.49)).asDouble(); + m_nominalCoMHeight = comHeight; double comHeightDelta = config.check("comHeightDelta", yarp::os::Value(0.01)).asDouble(); double nominalDuration = config.check("nominalDuration", yarp::os::Value(4.0)).asDouble(); double lastStepSwitchTime = config.check("lastStepSwitchTime", yarp::os::Value(0.5)).asDouble(); @@ -501,6 +502,25 @@ bool TrajectoryGenerator::isTrajectoryAsked() return m_generatorState == GeneratorState::Called; } +bool TrajectoryGenerator::generateTrajectoriesFromFootprints(std::shared_ptr left, std::shared_ptr right, const double &initTime,DCMInitialState initialState) +{ + if (!m_dcmGenerator->setDCMInitialState(initialState)) + { + yError() << "[TrajectoryGenerator_Thread] Failed to set the initial state."; + return false; + } + + if(!m_trajectoryGenerator.generateFromFootPrints(left, right, initTime, m_dT)) + { + yError() << "[generateTrajectoriesFromFootprints] Failed to generate trajectory from foot-prints"; + m_generatorState = GeneratorState::Configured; + return false; + } + + m_generatorState = GeneratorState::Returned; + return true; +} + bool TrajectoryGenerator::getDCMPositionTrajectory(std::vector& DCMPositionTrajectory) { if(!isTrajectoryComputed()) @@ -513,6 +533,18 @@ bool TrajectoryGenerator::getDCMPositionTrajectory(std::vector& ZMPPositionTrajectory) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getZMPPositionTrajectory] No trajectories are available"; + return false; + } + + ZMPPositionTrajectory = m_dcmGenerator->getZMPPosition(); + return true; +} + bool TrajectoryGenerator::getDCMVelocityTrajectory(std::vector& DCMVelocityTrajectory) { if(!isTrajectoryComputed()) @@ -525,6 +557,19 @@ bool TrajectoryGenerator::getDCMVelocityTrajectory(std::vector> & dcmSubTrajectories) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getDCMSubTrajectories] No trajectories are available"; + return false; + } + + dcmSubTrajectories = m_dcmGenerator->getDCMSubTrajectories(); + + return true; +} + bool TrajectoryGenerator::getFeetTrajectories(std::vector& lFootTrajectory, std::vector& rFootTrajectory) { @@ -553,6 +598,19 @@ bool TrajectoryGenerator::getFeetTwist(std::vector& lFootTwist, return true; } +bool TrajectoryGenerator::getFeetAcceleration(std::vector& lFootAcceleration, + std::vector& rFootAcceleration) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getFeetAcceleration] No trajectories are available"; + return false; + } + + m_feetGenerator->getFeetAccelerationInMixedRepresentation(lFootAcceleration, rFootAcceleration); + return true; +} + bool TrajectoryGenerator::getWhenUseLeftAsFixed(std::vector& isLeftFixedFrame) { if(!isTrajectoryComputed()) @@ -680,3 +738,62 @@ bool TrajectoryGenerator::getIsStancePhase(std::vector& isStancePhase) return true; } + +bool TrajectoryGenerator::getStepPhases(std::vector &leftPhases, std::vector &rightPhases) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getStepPhases] No trajectories are available"; + return false; + } + + m_trajectoryGenerator.getStepPhases(leftPhases,rightPhases); + return true; +} + +bool TrajectoryGenerator::getLeftFootprint(std::shared_ptr& leftFootPrint) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getLeftFootprint] No trajectories are available"; + return false; + } + + leftFootPrint = m_trajectoryGenerator.getLeftFootPrint(); + return true; +} + +bool TrajectoryGenerator::getRightFootprint(std::shared_ptr& rightFootPrint) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getRightFootprint] No trajectories are available"; + return false; + } + + rightFootPrint = m_trajectoryGenerator.getRightFootPrint(); + return true; +} + +bool TrajectoryGenerator::getNominalCoMHeight(double& nominalCoMHeight) +{ + nominalCoMHeight = m_nominalCoMHeight; + return true; +} + + +bool TrajectoryGenerator::getSwitchOverSwingRatio(double& switchOverSwingRatio) +{ + switchOverSwingRatio = m_switchOverSwingRatio; + return true; +} + +bool TrajectoryGenerator::getDCMBoundaryConditionAtMergePoint(DCMInitialState DCMBoundryConditionAtMergePoint) +{ + + DCMBoundryConditionAtMergePoint.initialPosition = m_DCMBoundaryConditionAtMergePointPosition; + DCMBoundryConditionAtMergePoint.initialVelocity = m_DCMBoundaryConditionAtMergePointVelocity; + + return true; +} + diff --git a/src/WalkingModule/CMakeLists.txt b/src/WalkingModule/CMakeLists.txt index 8c7215d08..a49ba165f 100644 --- a/src/WalkingModule/CMakeLists.txt +++ b/src/WalkingModule/CMakeLists.txt @@ -53,6 +53,7 @@ if(WALKING_CONTROLLERS_COMPILE_WalkingModule) WalkingControllers::WholeBodyControllers WalkingControllers::RetargetingHelper WalkingControllers::LoggerClient + WalkingControllers::StepAdaptationController ) install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin) 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 c448c336e..240e0e651 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 @@ -26,6 +26,19 @@ joint_is_stiff_mode (true, true, true, true, true, true, true, true, true, true, true, true, true, true, true) +joint_damping_gain (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) + +joint_stiffness_gain (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) + + # if true a good joint traking is considered mandatory good_tracking_required (true, true, true, true, true, true, true, diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/plannerParams.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/plannerParams.ini new file mode 100644 index 000000000..8dc88fd84 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/plannerParams.ini @@ -0,0 +1,53 @@ +##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.16 +minStepLength 0.01 +#Width +minWidth 0.13 +#Angle Variations in DEGREES +#maxAngleVariation 12.0 +maxAngleVariation 6.0 +minAngleVariation 4.0 +#Timings +maxStepDuration 1.4 +minStepDuration 1.2 + +##Nominal Values +#Width +nominalWidth 0.15 +#Height +stepHeight 0.025 +stepLandingVelocity 0.0 +footApexTime 0.8 +comHeightDelta 0.0 +#Timings +nominalDuration 1.3 +lastStepSwitchTime 0.8 +switchOverSwingRatio 0.8 + +#ZMP Delta +leftZMPDelta (0.03 -0.000) +rightZMPDelta (0.03 0.000) + +#MergePoint +mergePointRatio 0.5 + +# pitch delta +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/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/robotControl.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/robotControl.ini new file mode 100644 index 000000000..eca45e387 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/robotControl.ini @@ -0,0 +1,39 @@ +robot icubSim + +joints_list ("torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", + "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, + false, false, false, false, + false, false, false, false, + true, true, true, true, true, true, + true, true, true, true, true, true) + +joint_damping_gain (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) + +joint_stiffness_gain (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) + +remote_control_boards ("torso", "left_arm", "right_arm", "left_leg", "right_leg") + +# Base data robot port name of Gazebo +floating_base_port_name "/icubSim/floating_base/state:o" + +# 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/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/stepAdaptation.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/stepAdaptation.ini new file mode 100644 index 000000000..0411ac613 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/stepAdaptation.ini @@ -0,0 +1,32 @@ +#The gain of cost function components(alpha1,...) +next_zmp_position_weight (1.0, 1.0) +next_dcm_offset_weight (100.0, 100.0) +sigma_weight 0.035 + +step_duration_tolerance 0.0 + +# front (x positive), back (x negative), left (y positive) and right (y negative) +next_zmp_constraint_bound_left_foot (0.08, 0.05, 0.07, 0.01) +next_zmp_constraint_bound_right_foot (0.08, 0.05, 0.01, 0.07) + +#threshold for triggering +threshold_dcm_error (0.02, 0.02) + +#threshold for triggering imu orientation in push recovery +threshold_roll_pitch_error (0.3, 0.3) + +#an offset for arm joints error to generate DCM error inside the simple DCM estimator to use in step adaptation +offset_roll_pitch_arm_error (0.1, 0.1) + +#This index is an integer threshold that shows how many sequential control cycles of push detection should happen to active the step adaptation controller +push_recovery_activation_index 5 + +#Joints list that will be used for push detection +joints_list_push_detection_left_arm_x ("l_shoulder_pitch","l_elbow") + +joints_list_push_detection_left_arm_y ("l_shoulder_roll","l_shoulder_yaw") + +joints_list_push_detection_right_arm_x ("r_shoulder_pitch","r_elbow") + +joints_list_push_detection_right_arm_y ("r_shoulder_roll","r_shoulder_yaw") + diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_step_adaptation.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_step_adaptation.ini new file mode 100644 index 000000000..09df5a9d0 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_step_adaptation.ini @@ -0,0 +1,63 @@ +# 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 + +# Remove this line if you don't want to use the step adjustment +use_step_adaptation 1 + +# general parameters +[GENERAL] +name walking-coordinator +# height of the com +com_height 0.53 +# sampling time +sampling_time 0.01 + +# comment out the following line if the position of the base is not provided by an +# external software(here Gazebo) +# use_external_robot_base 1 + +# include robot control parameters +[include ROBOT_CONTROL "./dcm_walking/step_adaptation/robotControl.ini"] + +# include trajectory planner parameters +[include TRAJECTORY_PLANNER "./dcm_walking/step_adaptation/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/joypad_control/inverseKinematics.ini"] + +# include qp inverse kinematcs parameters +[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/joypad_control/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"] + +# include Step Adaptator parameters +[include STEP_ADAPTATOR "./dcm_walking/step_adaptation/stepAdaptation.ini"] 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 68515325d..9c83fa227 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 @@ -9,6 +9,17 @@ joints_list ("torso_pitch", "torso_roll", "torso_yaw", remote_control_boards ("torso", "left_arm", "right_arm", "left_leg", "right_leg") +joint_damping_gain (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) + +joint_stiffness_gain (0.55, 0.55, 0.55, + 0.55, 0.55, 0.55, 0.55, + 0.55, 0.55, 0.55, 0.55, + 0.55, 0.55, 0.55, 0.55, 0.55, 0.55, + 0.55, 0.55, 0.55, 0.55, 0.55, 0.55) # filters # if use_*_filter is equal to 0 the low pass filters are not used use_joint_velocity_filter 0 diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/plannerParams.ini new file mode 100644 index 000000000..ec14f3774 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/plannerParams.ini @@ -0,0 +1,60 @@ +##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.16 +minStepLength 0.01 +#Width +minWidth 0.13 +#Angle Variations in DEGREES +#maxAngleVariation 12.0 +maxAngleVariation 8.0 +minAngleVariation 6.0 +#Timings +maxStepDuration 1.2 +minStepDuration 0.8 + +##Nominal Values +#Width +nominalWidth 0.15 +#Height +stepHeight 0.025 +stepLandingVelocity 0.0 +footApexTime 0.8 +comHeightDelta 0.0 +#Timings +nominalDuration 1.0 +lastStepSwitchTime 0.8 +switchOverSwingRatio 0.5 + +#ZMP Delta +leftZMPDelta (0.03 -0.000) +rightZMPDelta (0.03 0.000) + +# 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.5 + +# pitch delta +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/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/robotControl.ini new file mode 100644 index 000000000..8dd3911be --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/robotControl.ini @@ -0,0 +1,36 @@ +robot icub + +joints_list ("torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", + "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 ("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, + false, false, false, false, + false, false, false, false, + true, true, true, true, true, true, + true, true, true, true, true, true) + +joint_damping_gain (0.1, 0.1, 0.1, + 0.085, 0.001, 0.2, 0.085, + 0.085, 0.001, 0.2, 0.085, + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1) + +joint_stiffness_gain (0.55, 0.55, 0.55, + 0.55, 0.45, 0.7, 0.55, + 0.55, 0.45, 0.7, 0.55, + 0.55, 0.55, 0.55, 0.55, 0.55, 0.55, + 0.55, 0.55, 0.55, 0.55, 0.55, 0.55) + +# 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/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/stepAdaptation.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/stepAdaptation.ini new file mode 100644 index 000000000..90821be01 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/stepAdaptation.ini @@ -0,0 +1,32 @@ +#The gain of cost function components(alpha1,...) +next_zmp_position_weight (1.0, 0.1) +next_dcm_offset_weight (100.0, 100.0) +sigma_weight 0.05 + +step_duration_tolerance 0.0 + +# front (x positive), back (x negative), left (y positive) and right (y negative) +next_zmp_constraint_bound_left_foot (0.04, 0.00001, 0.07, 0.00001) +next_zmp_constraint_bound_right_foot (0.04, 0.00001, 0.0001, 0.07) + +#threshold for triggering +threshold_dcm_error (0.02, 0.02) + +#threshold for triggering imu orientation in push recovery +threshold_roll_pitch_error (0.13, 0.15) + +#an offset for arm joints error to generate DCM error inside the simple DCM estimator to use in step adaptation +offset_roll_pitch_arm_error (0.28, 0.08) + +#This index is an integer threshold that shows how many sequential control cycles of push detection should happen to active the step adaptation controller +push_recovery_activation_index 5 + +#Joints list that will be used for push detection +joints_list_push_detection_left_arm_x ("l_shoulder_pitch","l_elbow") + +joints_list_push_detection_left_arm_y ("l_shoulder_roll") + +joints_list_push_detection_right_arm_x ("r_shoulder_pitch","r_elbow") + +joints_list_push_detection_right_arm_y ("r_shoulder_roll") + diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/zmpControllerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/zmpControllerParams.ini new file mode 100644 index 000000000..8f57df14b --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/zmpControllerParams.ini @@ -0,0 +1,18 @@ +# 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 2.7 +kCoM_walking 10.0 + +kZMP_stance 0.9 +kCoM_stance 6.0 + +# old parameters +#kZMP 2.0 low velocity reactive +#kZMP 1.9 low velocity MPC + +#kCoM 10.0 low velocity reactive +#kCoM 8.0 low velocity MPC diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_step_adaptation.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_step_adaptation.ini new file mode 100644 index 000000000..689aa45a9 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_step_adaptation.ini @@ -0,0 +1,59 @@ +# 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 + +# Remove this line if you don't want to use the step adjustment +use_step_adaptation 1 + +# general parameters +[GENERAL] +name walking-coordinator +# height of the com +com_height 0.53 +# sampling time +sampling_time 0.01 + +# include robot control parameters +[include ROBOT_CONTROL "./dcm_walking/step_adaptation/robotControl.ini"] + +# include trajectory planner parameters +[include TRAJECTORY_PLANNER "./dcm_walking/step_adaptation/plannerParams.ini"] + +# include MPC parameters +[include DCM_MPC_CONTROLLER "./dcm_walking/common/controllerParams.ini"] + +# include Reactive parameters +[include DCM_REACTIVE_CONTROLLER "./dcm_walking/common/dcmReactiveControllerParams.ini"] + +# include ZMP-CoM controller parameters +[include ZMP_CONTROLLER "./dcm_walking/step_adaptation/zmpControllerParams.ini"] + +# include inverse kinematcs parameters +[include INVERSE_KINEMATICS_SOLVER "./dcm_walking/joypad_control/inverseKinematics.ini"] + +# include qp inverse kinematcs parameters +[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/joypad_control/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"] + +# include Step Adaptator parameters +[include STEP_ADAPTATOR "./dcm_walking/step_adaptation/stepAdaptation.ini"] diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 0c1f221c2..aea78982d 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -23,6 +23,9 @@ // iDynTree #include #include +#include +#include +#include // WalkingControllers library #include @@ -39,8 +42,10 @@ #include #include -#include +#include +#include +#include #include #include @@ -66,6 +71,68 @@ namespace WalkingControllers double m_dT; /**< RFModule period. */ double m_time; /**< Current time. */ std::string m_robot; /**< Robot name. */ + double m_stepHeight; /**< maximum height of step. */ + double m_startOfWalkingTime; /**< The time that the robot starts walking. */ + + double m_timeOffset;/**< timeOffset is the time of start of this step(that will be updated in updateTrajectory function at starting point of each step)*/ + double m_impactTimeNominal;/**< Nominal absolute time of the impact */ + double m_impactTimeAdjusted;/**< Adjusted absolute time of the impact */ + iDynTree::Vector2 m_zmpNominal;/**< Nominal 2D zmp position */ + iDynTree::Vector2 m_zmpAdjusted;/**< Absolute 2D zmp position */ + int m_indexPush;/**< Number of control cycle that step adjustment is active */ + + iDynTree::Vector2 m_dcmEstimatedI; /**< The estimated DCM. */ + + std::vector> m_DCMSubTrajectories;/**< The different trajectories that are output of DCM motion planing. */ + + iDynTree::Transform m_adaptedFootLeftTransform;/**< The adapted transform of left foot. */ + iDynTree::Transform m_adaptedFootRightTransform;/**< The adapted transform of right foot. */ + iDynTree::Twist m_adaptedFootRightTwist;/**< The adapted twist of right foot. */ + iDynTree::Twist m_adaptedFootLeftTwist;/**< The adapted twist of left foot. */ + iDynTree::SpatialAcc m_adaptedFootLeftAcceleration;/**< The adapted acceleration of the left foot. */ + iDynTree::SpatialAcc m_adaptedFootRightAcceleration;/**< The adapted acceleration of the right foot. */ + + iDynTree::Transform m_currentFootLeftTransform;/**< The current adapted transform of the left foot that has been found in previous control cycle. */ + iDynTree::Transform m_currentFootRightTransform;/**< The current adapted transform of the right foot that has been found in previous control cycle. */ + iDynTree::Twist m_currentFootLeftTwist;/**< The current adapted twist of the left foot that has been found in previous control cycle. */ + iDynTree::Twist m_currentFootRightTwist;/**< The current adapted twist of the right foot that has been found in previous control cycle. */ + iDynTree::SpatialAcc m_currentFootLeftAcceleration; /**< The current adapted acceleration of the left foot that has been found in previous control cycle. */ + iDynTree::SpatialAcc m_currentFootRightAcceleration;/**< The current adapted acceleratiom of the right foot that has been found in previous control cycle. */ + + std::shared_ptr m_jleftFootprints; /**< The left foot prints */ + std::shared_ptr m_jRightFootprints;/**< The right foot prints */ + + iDynTree::Vector2 m_zmpToCenterOfFootPositionLeft; + iDynTree::Vector2 m_zmpToCenterOfFootPositionRight; + + StepList m_jLeftstepList; /**< The list of left foot steps */ + StepList m_jRightstepList;/**< The list of right foot steps */ + + StepAdapterOutput m_outputStepAdaptation;/**< The structure of outputs of step adaptation */ + StepAdapterInput m_inputStepAdaptation;/**< The structure of inputs of step adaptation */ + + double m_isPushActive;/**< Is push recovery active? */ + double m_isRollActive;/**< Is the threshold of roll angles of arm active? */ + double m_isPitchActive;/**< Is the threshold of pitch angles of arm active? */ + int m_pushRecoveryActiveIndex;/**< The number of control cycles that push recovery is active? */ + double m_kDCMSmoother;/**< The gain for smoothing of the DCM trajectories */ + double m_kFootSmoother;/**< The gain for smoothing of the feet trajectories */ + int m_indexSmoother;/**< The index for the control cycle number of the DCM trajectories smoothing */ + int m_indexFootSmoother;/**< The index for the control cycle number of the feet trajectories smoothing */ + int m_timeIndexAfterPushDetection;/**< The index for the control cycle number after detecting the push that is used for DCM smoothing*/ + int m_FootTimeIndexAfterPushDetection;/**< The index for the control cycle number after detecting the push that is used for feet trajectory smoothing*/ + + iDynTree::Transform m_smoothedFootLeftTransform;/**< The smoothed transform of left foot after adaptation. */ + iDynTree::Transform m_smoothedFootRightTransform;/**< The smoothed transform of right foot after adaptation. */ + iDynTree::Twist m_smoothedFootLeftTwist;/**< The smoothed twist of left foot after adaptation. */ + iDynTree::Twist m_smoothedFootRightTwist;/**< The smoothed twist of right foot after adaptation. */ + iDynTree::Vector2 m_DCMPositionSmoothed;/**< The smoothed position of DCM after adaptation. */ + + std::deque m_CurrentDCMPositionAdjusted; /**< Deque containing the desired DCM position. */ + std::deque m_CurrentDCMVelocityAdjusted; /**< Deque containing the desired DCM position. */ + + bool m_firstStep; /**< True if this is the first step. */ + bool m_useStepAdaptation; /**< True if the step adaptation is used. */ bool m_useMPC; /**< True if the MPC controller is used. */ bool m_useQPIK; /**< True if the QP-IK is used. */ @@ -74,6 +141,7 @@ namespace WalkingControllers std::unique_ptr m_robotControlHelper; /**< Robot control helper. */ std::unique_ptr m_trajectoryGenerator; /**< Pointer to the trajectory generator object. */ + std::unique_ptr m_trajectoryGeneratorStepAdjustment; /**< Pointer to the trajectory generator object. */ std::unique_ptr m_walkingController; /**< Pointer to the walking DCM MPC object. */ std::unique_ptr m_walkingDCMReactiveController; /**< Pointer to the walking DCM reactive controller object. */ std::unique_ptr m_walkingZMPController; /**< Pointer to the walking ZMP controller object. */ @@ -85,6 +153,7 @@ namespace WalkingControllers std::unique_ptr m_retargetingClient; /**< Pointer to the stable DCM dynamics. */ std::unique_ptr m_walkingLogger; /**< Pointer to the Walking Logger object. */ std::unique_ptr m_profiler; /**< Time profiler. */ + std::unique_ptr m_stepAdapter; /**< Pointer to the step adaptation object. */ double m_additionalRotationWeightDesired; /**< Desired additional rotational weight matrix. */ double m_desiredJointsWeight; /**< Desired joint weight matrix. */ @@ -96,6 +165,10 @@ namespace WalkingControllers std::deque m_leftTwistTrajectory; /**< Deque containing the twist trajectory of the left foot. */ std::deque m_rightTwistTrajectory; /**< Deque containing the twist trajectory of the right foot. */ + std::deque m_leftAccelerationTrajectory; /**< Deque containing the acceleration trajectory of the left foot. */ + std::deque m_rightAccelerationTrajectory; /**< Deque containing the acceleration trajectory of the right foot. */ + + std::deque m_ZMPPositionDesired; /**< Deque containing the desired ZMP position. */ std::deque m_DCMPositionDesired; /**< Deque containing the desired DCM position. */ std::deque m_DCMVelocityDesired; /**< Deque containing the desired DCM velocity. */ std::deque m_leftInContact; /**< Deque containing the left foot state. */ @@ -104,6 +177,10 @@ namespace WalkingControllers 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_DCMPositionAdjusted; /**< Deque containing the DCM adjusted position. */ + std::deque m_DCMVelocityAdjusted; /**< Deque containing the DCM adjusted velocity. */ + std::deque m_weightInLeft; /**< Deque containing the left foot weight percentage. */ + std::deque m_weightInRight; /**< Deque containing the right foot weight percentage. */ 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 @@ -228,6 +305,11 @@ namespace WalkingControllers */ void reset(); + bool dcmSmoother(const iDynTree::Vector2 adaptedDCM, const iDynTree::Vector2 desiredDCM, iDynTree::Vector2 &smoothedDCM); + + bool feetTrajectorySmoother(const iDynTree::Transform adaptedFeetTransform, const iDynTree::Transform desiredFootTrajectory, + iDynTree::Transform &smoothedFootTrajectory, const iDynTree::Twist adaptedFeetTwist, + const iDynTree::Twist desiredFootTwist, iDynTree::Twist &smoothedFootTwist); public: /** @@ -286,6 +368,8 @@ namespace WalkingControllers * @return true in case of success and false otherwise. */ virtual bool stopWalking() override; + + bool runStepAdaptation(iDynTree::Vector2 measuredZMP); }; }; #endif diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 92a0d82b6..c3d2aefbe 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -44,6 +44,9 @@ bool WalkingModule::advanceReferenceSignals() || m_leftInContact.empty() || m_rightInContact.empty() || m_DCMPositionDesired.empty() + || m_ZMPPositionDesired.empty() + || m_DCMPositionAdjusted.empty() + || m_DCMVelocityAdjusted.empty() || m_DCMVelocityDesired.empty() || m_comHeightTrajectory.empty()) { @@ -87,6 +90,18 @@ bool WalkingModule::advanceReferenceSignals() m_isStancePhase.pop_front(); m_isStancePhase.push_back(m_isStancePhase.back()); + m_ZMPPositionDesired.pop_front(); + m_ZMPPositionDesired.push_back(m_ZMPPositionDesired.back()); + + if (m_useStepAdaptation) + { + m_DCMPositionAdjusted.pop_front(); + m_DCMPositionAdjusted.push_back(m_DCMPositionAdjusted.back()); + + m_DCMVelocityAdjusted.pop_front(); + m_DCMVelocityAdjusted.push_back(m_DCMVelocityAdjusted.back()); + } + // at each sampling time the merge points are decreased by one. // If the first merge point is equal to 0 it will be dropped. // A new trajectory will be merged at the first merge point or if the deque is empty @@ -127,6 +142,35 @@ bool WalkingModule::setRobotModel(const yarp::os::Searchable& rf) bool WalkingModule::configure(yarp::os::ResourceFinder& rf) { + + m_pushRecoveryActiveIndex=0; + m_useStepAdaptation = rf.check("use_step_adaptation", yarp::os::Value(false)).asBool(); + m_impactTimeNominal = 0; + m_impactTimeAdjusted = 0; + + m_zmpNominal.zero(); + m_zmpAdjusted.zero(); + m_isPushActive=0; + + m_adaptedFootLeftTransform=iDynTree::Transform::Identity(); + m_currentFootLeftTransform=iDynTree::Transform::Identity(); + m_smoothedFootLeftTransform=iDynTree::Transform::Identity(); + + m_adaptedFootRightTransform=iDynTree::Transform::Identity(); + m_currentFootRightTransform=iDynTree::Transform::Identity(); + m_smoothedFootRightTransform=iDynTree::Transform::Identity(); + + m_DCMPositionSmoothed.zero(); + m_DCMPositionAdjusted.push_back(m_DCMPositionSmoothed); + m_DCMVelocityAdjusted.push_back(m_DCMPositionSmoothed); + m_adaptedFootLeftTwist.zero(); + m_adaptedFootRightTwist.zero(); + m_smoothedFootLeftTwist.zero(); + m_smoothedFootRightTwist.zero(); + m_currentFootLeftTwist.zero(); + m_currentFootRightTwist.zero(); + + // module name (used as prefix for opened ports) m_useMPC = rf.check("use_mpc", yarp::os::Value(false)).asBool(); m_useQPIK = rf.check("use_QP-IK", yarp::os::Value(false)).asBool(); @@ -184,6 +228,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) // initialize the trajectory planner m_trajectoryGenerator = std::make_unique(); + m_trajectoryGeneratorStepAdjustment = std::make_unique(); yarp::os::Bottle& trajectoryPlannerOptions = rf.findGroup("TRAJECTORY_PLANNER"); trajectoryPlannerOptions.append(generalOptions); if(!m_trajectoryGenerator->initialize(trajectoryPlannerOptions)) @@ -192,6 +237,41 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) return false; } + if(!m_trajectoryGeneratorStepAdjustment->initialize(trajectoryPlannerOptions)) + { + yError() << "[configure] Unable to initialize the planner."; + return false; + } + + m_stepHeight = trajectoryPlannerOptions.check("stepHeight", yarp::os::Value(0.005)).asDouble(); + + if(m_useStepAdaptation) + { + // initialize the step adaptation + m_stepAdapter = std::make_unique(); + yarp::os::Bottle& stepAdaptatorOptions = rf.findGroup("STEP_ADAPTATOR"); + stepAdaptatorOptions.append(generalOptions); + stepAdaptatorOptions.append(trajectoryPlannerOptions); + if(!m_stepAdapter->configure(stepAdaptatorOptions)) + { + yError() << "[configure] Unable to initialize the step adaptator!"; + return false; + } + + if(!YarpUtilities::getVectorFromSearchable(stepAdaptatorOptions, "leftZMPDelta", m_zmpToCenterOfFootPositionLeft)) + { + yError() << "[configure] Unable to get the vector of leftZMPDelta"; + return false; + } + + if(!YarpUtilities::getVectorFromSearchable(stepAdaptatorOptions, "rightZMPDelta", m_zmpToCenterOfFootPositionRight)) + { + yError() << "[configure] Unable to get the vector of rightZMPDelta"; + return false; + } + + } + if(m_useMPC) { // initialize the MPC controller @@ -337,6 +417,8 @@ void WalkingModule::reset() m_trajectoryGenerator->reset(); + m_trajectoryGeneratorStepAdjustment->reset(); + if(m_dumpData) m_walkingLogger->quit(); } @@ -365,12 +447,14 @@ bool WalkingModule::close() // clear all the pointer m_trajectoryGenerator.reset(nullptr); + m_trajectoryGeneratorStepAdjustment.reset(nullptr); m_walkingController.reset(nullptr); m_walkingZMPController.reset(nullptr); m_IKSolver.reset(nullptr); m_QPIKSolver.reset(nullptr); m_FKSolver.reset(nullptr); m_stableDCMModel.reset(nullptr); + m_stepAdapter.reset(nullptr); return true; } @@ -385,11 +469,22 @@ bool WalkingModule::solveQPIK(const std::unique_ptr& solver, const ok &= solver->setRobotState(*m_FKSolver); solver->setDesiredNeckOrientation(desiredNeckOrientation.inverse()); - solver->setDesiredFeetTransformation(m_leftTrajectory.front(), - m_rightTrajectory.front()); + if (m_useStepAdaptation) + { + solver->setDesiredFeetTransformation(m_smoothedFootLeftTransform, + m_smoothedFootRightTransform); + + solver->setDesiredFeetTwist(m_smoothedFootLeftTwist, + m_smoothedFootRightTwist); + } + else + { + solver->setDesiredFeetTransformation(m_leftTrajectory.front(), + m_rightTrajectory.front()); - solver->setDesiredFeetTwist(m_leftTwistTrajectory.front(), - m_rightTwistTrajectory.front()); + solver->setDesiredFeetTwist(m_leftTwistTrajectory.front(), + m_rightTwistTrajectory.front()); + } solver->setDesiredCoMVelocity(desiredCoMVelocity); solver->setDesiredCoMPosition(desiredCoMPosition); @@ -488,8 +583,16 @@ bool WalkingModule::updateModule() m_velocityIntegral = std::make_unique(m_dT, buffer, jointLimits); // reset the models - m_walkingZMPController->reset(m_DCMPositionDesired.front()); - m_stableDCMModel->reset(m_DCMPositionDesired.front()); + if (m_useStepAdaptation) + { + m_walkingZMPController->reset(m_DCMPositionAdjusted.front()); + m_stableDCMModel->reset(m_DCMPositionAdjusted.front()); + } + else + { + m_walkingZMPController->reset(m_DCMPositionDesired.front()); + m_stableDCMModel->reset(m_DCMPositionDesired.front()); + } // reset the retargeting if(!m_robotControlHelper->getFeedbacks(100)) @@ -528,27 +631,50 @@ bool WalkingModule::updateModule() // check desired planner input yarp::sig::Vector* desiredUnicyclePosition = nullptr; desiredUnicyclePosition = m_desiredUnyciclePositionPort.read(false); + + if(m_DCMSubTrajectories.size()<4 && desiredUnicyclePosition == nullptr) + { + m_useStepAdaptation=false; + } + else if(desiredUnicyclePosition != nullptr && m_newTrajectoryMergeCounter == 2) + { + m_useStepAdaptation=true; + } + if(desiredUnicyclePosition != nullptr) + { if(!setPlannerInput((*desiredUnicyclePosition)(0), (*desiredUnicyclePosition)(1))) { yError() << "[WalkingModule::updateModule] Unable to set the planner input"; return false; - } + } + } // if a new trajectory is required check if its the time to evaluate the new trajectory or // the time to attach new one if(m_newTrajectoryRequired) { // when we are near to the merge point the new trajectory is evaluated - if(m_newTrajectoryMergeCounter == 20) + if(m_newTrajectoryMergeCounter == 10) { double initTimeTrajectory; initTimeTrajectory = m_time + m_newTrajectoryMergeCounter * m_dT; - iDynTree::Transform measuredTransform = m_isLeftFixedFrame.front() ? - m_rightTrajectory[m_newTrajectoryMergeCounter] : - m_leftTrajectory[m_newTrajectoryMergeCounter]; + m_startOfWalkingTime=initTimeTrajectory; + + iDynTree::Transform TempRightFoot; + iDynTree::Transform TempLeftFoot; + iDynTree::Transform measuredTransform ; + + if(!m_useStepAdaptation){ + measuredTransform = m_isLeftFixedFrame.front() ? + m_rightTrajectory[m_newTrajectoryMergeCounter] : m_leftTrajectory[m_newTrajectoryMergeCounter]; + } + else { + measuredTransform = m_isLeftFixedFrame.front() ? + m_currentFootRightTransform : m_currentFootLeftTransform; + } // ask for a new trajectory if(!askNewTrajectories(initTimeTrajectory, !m_isLeftFixedFrame.front(), @@ -612,13 +738,56 @@ bool WalkingModule::updateModule() } // evaluate 3D-LIPM reference signal - m_stableDCMModel->setInput(m_DCMPositionDesired.front()); + if(m_useStepAdaptation) + { + if (!m_leftInContact.front() || !m_rightInContact.front()) + { + if(!m_leftInContact.front()) + { + if (!feetTrajectorySmoother(m_adaptedFootLeftTransform,m_leftTrajectory.front(),m_smoothedFootLeftTransform,m_adaptedFootLeftTwist,m_leftTwistTrajectory.front(),m_smoothedFootLeftTwist)) { + yError()<<"[WalkingModule::updateModule] The left foot trajectory smoother can not evaluate the smoothed trajectory!"; + } + } + + if(!m_rightInContact.front()) + { + if (!feetTrajectorySmoother(m_adaptedFootRightTransform,m_rightTrajectory.front(),m_smoothedFootRightTransform,m_adaptedFootRightTwist,m_rightTwistTrajectory.front(),m_smoothedFootRightTwist)) { + yError()<<"[WalkingModule::updateModule] The right foot trajectory smoother can not evaluate the smoothed trajectory!"; + } + } + } + else + { + m_smoothedFootLeftTransform=m_adaptedFootLeftTransform; + m_smoothedFootLeftTwist=m_adaptedFootLeftTwist; + + m_smoothedFootRightTransform=m_adaptedFootRightTransform; + m_smoothedFootRightTwist=m_adaptedFootRightTwist; + } + + if (!dcmSmoother(m_DCMPositionAdjusted.front(),m_DCMPositionDesired.front(),m_DCMPositionSmoothed)) + { + yError()<<"the DCM smoother can not evaluate the smoothed DCM!"; + } + m_stableDCMModel->setInput(m_DCMPositionSmoothed); + } + else + { + m_stableDCMModel->setInput(m_DCMPositionDesired.front()); + } + if(!m_stableDCMModel->integrateModel()) { yError() << "[WalkingModule::updateModule] Unable to propagate the 3D-LIPM."; return false; } + if(!runStepAdaptation(measuredZMP)) + { + yError() << "[WalkingModule::updateModule] Unable to run step adjustment"; + return false; + } + // DCM controller if(m_useMPC) { @@ -653,9 +822,26 @@ bool WalkingModule::updateModule() } else { - m_walkingDCMReactiveController->setFeedback(m_FKSolver->getDCM()); - m_walkingDCMReactiveController->setReferenceSignal(m_DCMPositionDesired.front(), - m_DCMVelocityDesired.front()); + if (!m_useStepAdaptation) + { + m_walkingDCMReactiveController->setFeedback(m_FKSolver->getDCM()); + m_walkingDCMReactiveController->setReferenceSignal(m_DCMPositionDesired.front(), + m_DCMVelocityDesired.front()); + } + else + { + iDynTree::Vector2 DCMPositionDesiredAdjusted; + DCMPositionDesiredAdjusted(0) = m_DCMPositionAdjusted.front()(0); + DCMPositionDesiredAdjusted(1) = m_DCMPositionAdjusted.front()(1); + + iDynTree::Vector2 DCMVelocityDesiredAdjusted; + DCMVelocityDesiredAdjusted(0) = m_DCMVelocityAdjusted.front()(0); + DCMVelocityDesiredAdjusted(1) = m_DCMVelocityAdjusted.front()(1); + + m_walkingDCMReactiveController->setFeedback(m_FKSolver->getDCM()); + m_walkingDCMReactiveController->setReferenceSignal(m_DCMPositionSmoothed, + DCMVelocityDesiredAdjusted); + } if(!m_walkingDCMReactiveController->evaluateControl()) { @@ -701,12 +887,15 @@ bool WalkingModule::updateModule() desiredCoMPosition(0) = outputZMPCoMControllerPosition(0); desiredCoMPosition(1) = outputZMPCoMControllerPosition(1); desiredCoMPosition(2) = m_retargetingClient->comHeight(); - - + if(abs(desiredCoMPosition(0) - m_FKSolver->getCoMPosition()(0))>0.07 || abs(desiredCoMPosition(1) - m_FKSolver->getCoMPosition()(1))>0.07) + { + yError() << "[WalkingModule::updateModule] The error of com tracking is not small"; + return false; + } iDynTree::Vector3 desiredCoMVelocity; desiredCoMVelocity(0) = outputZMPCoMControllerVelocity(0); desiredCoMVelocity(1) = outputZMPCoMControllerVelocity(1); - desiredCoMVelocity(2) = m_retargetingClient->comHeightVelocity(); + desiredCoMVelocity(2) = m_comHeightVelocity.front(); // evaluate desired neck transformation double yawLeft = m_leftTrajectory.front().getRotation().asRPY()(2); @@ -1085,13 +1274,27 @@ bool WalkingModule::askNewTrajectories(const double& initTime, const bool& isLef return false; } - if(!m_trajectoryGenerator->updateTrajectories(initTime, m_DCMPositionDesired[mergePoint], - m_DCMVelocityDesired[mergePoint], isLeftSwinging, - measuredTransform, desiredPosition)) + if (!m_useStepAdaptation) { - yError() << "[WalkingModule::askNewTrajectories] Unable to update the trajectory."; - return false; + if(!m_trajectoryGenerator->updateTrajectories(initTime, m_DCMPositionDesired[mergePoint], + m_DCMVelocityDesired[mergePoint], isLeftSwinging, + measuredTransform, desiredPosition)) + { + yError() << "[WalkingModule::askNewTrajectories] Unable to update the trajectory."; + return false; + } } + else + { + if(!m_trajectoryGenerator->updateTrajectories(initTime, m_DCMPositionAdjusted[mergePoint], + m_DCMVelocityAdjusted[mergePoint], isLeftSwinging, + measuredTransform, desiredPosition)) + { + yError() << "[WalkingModule::askNewTrajectories] Unable to update the trajectory."; + return false; + } + } + return true; } @@ -1116,10 +1319,14 @@ bool WalkingModule::updateTrajectories(const size_t& mergePoint) std::vector mergePoints; std::vector isLeftFixedFrame; std::vector isStancePhase; + std::vector ZMPPositionDesired; + + m_timeOffset = m_time + mergePoint * m_dT; // get dcm position and velocity m_trajectoryGenerator->getDCMPositionTrajectory(DCMPositionDesired); m_trajectoryGenerator->getDCMVelocityTrajectory(DCMVelocityDesired); + m_trajectoryGenerator->getZMPPositionTrajectory(ZMPPositionDesired); // get feet trajectories m_trajectoryGenerator->getFeetTrajectories(leftTrajectory, rightTrajectory); @@ -1152,13 +1359,66 @@ bool WalkingModule::updateTrajectories(const size_t& mergePoint) StdUtilities::appendVectorToDeque(comHeightTrajectory, m_comHeightTrajectory, mergePoint); StdUtilities::appendVectorToDeque(comHeightVelocity, m_comHeightVelocity, mergePoint); + StdUtilities::appendVectorToDeque(ZMPPositionDesired, m_ZMPPositionDesired, mergePoint); + + if (m_useStepAdaptation) + { + StdUtilities::appendVectorToDeque(DCMPositionDesired, m_DCMPositionAdjusted, mergePoint); + StdUtilities::appendVectorToDeque(DCMVelocityDesired, m_DCMVelocityAdjusted, mergePoint); + } StdUtilities::appendVectorToDeque(isStancePhase, m_isStancePhase, mergePoint); m_mergePoints.assign(mergePoints.begin(), mergePoints.end()); + if (m_useStepAdaptation) + { + m_DCMSubTrajectories.clear(); + m_trajectoryGenerator->getDCMSubTrajectories(m_DCMSubTrajectories); + + std::shared_ptr tempLeft; + m_trajectoryGenerator->getLeftFootprint(tempLeft); + m_jleftFootprints = std::make_shared(); + m_jleftFootprints->setFootName("left"); + for(auto step: tempLeft->getSteps()) + { + if(!m_jleftFootprints->addStep(step)) + { + yError() << "[updateTrajectories] unable to add left foot step"; + return false; + } + } + m_jLeftstepList=m_jleftFootprints->getSteps(); + + std::shared_ptr tempRight; + m_trajectoryGenerator->getRightFootprint(tempRight); + m_jRightFootprints = std::make_shared(); + m_jRightFootprints->setFootName("right"); + for(auto step: tempRight->getSteps()) + { + if(!m_jRightFootprints->addStep(step)) + { + yError() << "[updateTrajectories] unable to add right foot step"; + return false; + } + } + m_jRightstepList=m_jRightFootprints->getSteps(); + } + // the first merge point is always equal to 0 m_mergePoints.pop_front(); + if (m_useStepAdaptation) + { + m_adaptedFootLeftTwist.zero(); + m_adaptedFootRightTwist.zero(); + m_adaptedFootLeftAcceleration.zero(); + m_adaptedFootRightAcceleration.zero(); + + m_adaptedFootLeftTransform = leftTrajectory.front(); + m_smoothedFootLeftTransform=leftTrajectory.front(); + m_adaptedFootRightTransform = rightTrajectory.front(); + m_smoothedFootRightTransform=rightTrajectory.front(); + } return true; } @@ -1285,13 +1545,13 @@ bool WalkingModule::setPlannerInput(double x, double y) return true; // Since the evaluation of a new trajectory takes time the new trajectory will be merged after x cycles - m_newTrajectoryMergeCounter = 20; + m_newTrajectoryMergeCounter = 10; } // the trajectory was not finished the new trajectory will be attached at the next merge point else { - if(m_mergePoints.front() > 20) + if(m_mergePoints.front() > 10) m_newTrajectoryMergeCounter = m_mergePoints.front(); else if(m_mergePoints.size() > 1) { @@ -1305,7 +1565,7 @@ bool WalkingModule::setPlannerInput(double x, double y) if(m_newTrajectoryRequired) return true; - m_newTrajectoryMergeCounter = 20; + m_newTrajectoryMergeCounter = 10; } } @@ -1354,3 +1614,277 @@ bool WalkingModule::stopWalking() m_robotState = WalkingFSM::Stopped; return true; } + +bool WalkingModule::feetTrajectorySmoother(const iDynTree::Transform adaptedFeetTransform,const iDynTree::Transform desiredFootTransform,iDynTree::Transform& smoothedFootTransform , + const iDynTree::Twist adaptedFeetTwist,const iDynTree::Twist desiredFootTwist,iDynTree::Twist& smoothedFootTwist) +{ + + if(m_pushRecoveryActiveIndex<(m_stepAdapter->getPushRecoveryActivationIndex()+1) ) + { + m_kFootSmoother=0; + m_FootTimeIndexAfterPushDetection=0; + } + else if (m_pushRecoveryActiveIndex>=(m_stepAdapter->getPushRecoveryActivationIndex()+1) && m_FootTimeIndexAfterPushDetection<(m_indexFootSmoother)) + { + m_FootTimeIndexAfterPushDetection=m_FootTimeIndexAfterPushDetection+1; + + m_kFootSmoother=((double)(m_FootTimeIndexAfterPushDetection)/((double)(m_indexFootSmoother))); + + if(m_FootTimeIndexAfterPushDetection==m_indexFootSmoother) + { + m_indexFootSmoother=0; + } + } + else + { + m_kFootSmoother=1; + } + + smoothedFootTransform.setRotation(adaptedFeetTransform.getRotation()); + smoothedFootTwist.setAngularVec3(adaptedFeetTwist.getAngularVec3()); + + iDynTree::Position tempPosition; + iDynTree::LinVelocity tempVel; + iDynTree::toEigen(tempPosition)=iDynTree::toEigen(desiredFootTransform.getPosition())+m_kFootSmoother*(iDynTree::toEigen(adaptedFeetTransform.getPosition())-iDynTree::toEigen(desiredFootTransform.getPosition())); + iDynTree::toEigen(tempVel)=iDynTree::toEigen(desiredFootTwist.getLinearVec3())+m_kFootSmoother*(iDynTree::toEigen(adaptedFeetTwist.getLinearVec3())-iDynTree::toEigen(desiredFootTwist.getLinearVec3())); + smoothedFootTransform.setPosition(tempPosition); + smoothedFootTwist.setLinearVec3(tempVel); + return true; +} + +bool WalkingModule::dcmSmoother(const iDynTree::Vector2 adaptedDCM,const iDynTree::Vector2 desiredDCM,iDynTree::Vector2& smoothedDCM ) +{ + if(m_pushRecoveryActiveIndex<(m_stepAdapter->getPushRecoveryActivationIndex()+1) ) + { + m_kDCMSmoother=0; + m_timeIndexAfterPushDetection=0; + } + else if (m_pushRecoveryActiveIndex>=(m_stepAdapter->getPushRecoveryActivationIndex()+1) && m_timeIndexAfterPushDetection<(m_indexSmoother)) + { + m_timeIndexAfterPushDetection=m_timeIndexAfterPushDetection+1; + + m_kDCMSmoother=((double)(m_timeIndexAfterPushDetection)/((double)(m_indexSmoother))); + if(m_timeIndexAfterPushDetection==m_indexSmoother) + { + m_indexSmoother=0; + m_pushRecoveryActiveIndex=0; + } + } + else + { + m_kDCMSmoother=1; + } + + iDynTree::toEigen(smoothedDCM)=iDynTree::toEigen(desiredDCM)+m_kDCMSmoother*(iDynTree::toEigen(adaptedDCM)-iDynTree::toEigen(desiredDCM)); + return true; +} + +bool WalkingModule::runStepAdaptation(iDynTree::Vector2 measuredZMP) +{ + if (m_useStepAdaptation) + { + m_isPitchActive=0; + m_isRollActive=0; + m_stepAdapter->triggerStepAdapterByArmCompliant(m_robotControlHelper->getActuatedDoFs(),m_qDesired,m_robotControlHelper->getJointPosition(), + m_leftInContact,m_rightInContact,m_robotControlHelper->getAxesList()); + m_isRollActive=m_stepAdapter->isArmRollActive(); + m_isPitchActive=m_stepAdapter->isArmPitchActive(); + double yawAngle = m_leftInContact.front()? m_jleftFootprints->getSteps()[1].angle : m_jRightFootprints->getSteps()[1].angle; + if(!m_stepAdapter->UpdateDCMEstimator(m_stableDCMModel->getCoMPosition(),m_stableDCMModel->getCoMVelocity(),measuredZMP,m_comHeightTrajectory.front(),yawAngle)) + { + yError() << "[WalkingModule::updateModule] Unable to to recieve DCM from pendulumEstimator"; + return false; + } + m_dcmEstimatedI= m_stepAdapter->getEstimatedDCM(); + } + + if(m_useStepAdaptation) + { + //step adjustment + double comHeight; + double omega; + if(!m_trajectoryGenerator->getNominalCoMHeight(comHeight)) + { + yError() << "[updateModule] Unable to get the nominal CoM height!"; + return false; + } + omega = sqrt(9.81/comHeight); + + m_outputStepAdaptation.indexPush=m_indexPush; + m_outputStepAdaptation.zmpNominal=m_zmpNominal; + m_outputStepAdaptation.zmpAdjusted=m_zmpAdjusted; + m_outputStepAdaptation.isPushActive=m_isPushActive; + m_outputStepAdaptation.isRollActive=m_isRollActive; + m_outputStepAdaptation.kDCMSmoother=m_kDCMSmoother; + m_outputStepAdaptation.indexSmoother=m_indexSmoother; + m_outputStepAdaptation.impactTimeNominal=m_impactTimeNominal; + m_outputStepAdaptation.isPitchActive=m_isPitchActive; + m_outputStepAdaptation.kFootSmoother=m_kFootSmoother; + m_outputStepAdaptation.indexFootSmoother=m_indexFootSmoother; + m_outputStepAdaptation.impactTimeAdjusted=m_impactTimeAdjusted; + m_outputStepAdaptation.dcmPositionAdjusted=m_DCMPositionAdjusted; + m_outputStepAdaptation.dcmVelocityAdjusted=m_DCMVelocityAdjusted; + m_outputStepAdaptation.adaptedFootLeftTwist=m_adaptedFootLeftTwist; + m_outputStepAdaptation.currentFootLeftTwist=m_currentFootLeftTwist; + m_outputStepAdaptation.pushRecoveryActiveIndex=m_pushRecoveryActiveIndex; + m_outputStepAdaptation.adaptedFootRightTwist=m_adaptedFootRightTwist; + m_outputStepAdaptation.currentFootRightTwist=m_currentFootRightTwist; + m_outputStepAdaptation.adaptedFootLeftTransform=m_adaptedFootLeftTransform; + m_outputStepAdaptation.currentFootLeftTransform=m_currentFootLeftTransform; + m_outputStepAdaptation.adaptedFootRightTransform=m_adaptedFootRightTransform; + m_outputStepAdaptation.currentFootRightTransform=m_currentFootRightTransform; + m_outputStepAdaptation.adaptedFootLeftAcceleration=m_adaptedFootLeftAcceleration; + m_outputStepAdaptation.currentFootLeftAcceleration=m_currentFootLeftAcceleration; + m_outputStepAdaptation.timeIndexAfterPushDetection=m_timeIndexAfterPushDetection; + m_outputStepAdaptation.adaptedFootRightAcceleration=m_adaptedFootRightAcceleration; + m_outputStepAdaptation.currentFootRightAcceleration=m_currentFootRightAcceleration; + m_outputStepAdaptation.FootTimeIndexAfterPushDetection=m_FootTimeIndexAfterPushDetection; + + m_inputStepAdaptation.dT=m_dT; + m_inputStepAdaptation.omega=omega; + m_inputStepAdaptation.time=m_time; + m_inputStepAdaptation.timeOffset=m_timeOffset; + m_inputStepAdaptation.leftStepList=m_jLeftstepList; + m_inputStepAdaptation.leftInContact=m_leftInContact; + m_inputStepAdaptation.leftFootprints=m_jleftFootprints; + m_inputStepAdaptation.rightStepList=m_jRightstepList; + m_inputStepAdaptation.rightInContact=m_rightInContact; + m_inputStepAdaptation.rightFootprints=m_jRightFootprints; + m_inputStepAdaptation.dcmSubTrajectories=m_DCMSubTrajectories; + m_inputStepAdaptation.dcmPositionSmoothed=m_DCMPositionSmoothed; + + if(!m_stepAdapter->runStepAdaptation(m_inputStepAdaptation,m_outputStepAdaptation)) + { + yError() << "[WalkingModule::runStepAdaptation] unable to run step adaptation"; + return false; + } + + // adapted dcm trajectory + // add the offset on the zmp evaluated by the step adjustment for each footprint in the trajectory + // the same approach is used also for the impact time since the step adjustment change the impact time + if (!m_inputStepAdaptation.leftInContact.front() || !m_inputStepAdaptation.rightInContact.front()) + { + iDynTree::Vector2 adaptedZMPOffset; + iDynTree::toEigen(adaptedZMPOffset) = iDynTree::toEigen(m_stepAdapter->getDesiredZmp()) - iDynTree::toEigen(m_outputStepAdaptation.zmpNominal); + double adaptedTimeOffset; + int numberOfSubTrajectories = m_inputStepAdaptation.dcmSubTrajectories.size(); + if (numberOfSubTrajectories<2) + { + yError() << "[WalkingModule::runningStepAdaptation] the size of dcm subtrajectories should be equal or greater than 2"; + return false; + } + auto firstSS = m_inputStepAdaptation.dcmSubTrajectories[numberOfSubTrajectories-2]; + + adaptedTimeOffset = m_stepAdapter->getDesiredImpactTime() - firstSS->getTrajectoryDomain().second; + + std::shared_ptr leftTemp = std::make_unique(); + leftTemp->setFootName("left"); + leftTemp->addStep(m_inputStepAdaptation.leftFootprints->getSteps()[0]); + for(int i = 1; i < m_inputStepAdaptation.leftFootprints->getSteps().size(); i++) + { + iDynTree::Vector2 position; + iDynTree::toEigen(position) = iDynTree::toEigen(m_inputStepAdaptation.leftFootprints->getSteps()[i].position) + iDynTree::toEigen(adaptedZMPOffset); + if(!leftTemp->addStep(position, m_inputStepAdaptation.leftFootprints->getSteps()[i].angle, m_inputStepAdaptation.leftFootprints->getSteps()[i].impactTime + adaptedTimeOffset)) + { + yError() << "[WalkingModule::runStepAdaptation] unable to add left step"; + return false; + } + } + + std::shared_ptr rightTemp = std::make_unique(); + rightTemp->setFootName("right"); + rightTemp->addStep(m_inputStepAdaptation.rightFootprints->getSteps()[0]); + for(int i = 1; i < m_inputStepAdaptation.rightFootprints->getSteps().size(); i++) + { + iDynTree::Vector2 position; + iDynTree::toEigen(position) = iDynTree::toEigen(m_inputStepAdaptation.rightFootprints->getSteps()[i].position) + iDynTree::toEigen(adaptedZMPOffset); + + if(!rightTemp->addStep(position, m_inputStepAdaptation.rightFootprints->getSteps()[i].angle, m_inputStepAdaptation.rightFootprints->getSteps()[i].impactTime + adaptedTimeOffset)) + { + yError() << "[WalkingModule::runStepAdaptation] unable to add right foot step"; + return false; + } + } + + DCMInitialState tempDCMInitialState; + if(!m_trajectoryGenerator->getDCMBoundaryConditionAtMergePoint(tempDCMInitialState)) + { + yError() << "[WalkingModule::runStepAdaptation] unable to get get DCM boundary condition at merge point"; + return false; + } + + // generate the DCM trajectory + if(!m_trajectoryGeneratorStepAdjustment->generateTrajectoriesFromFootprints(leftTemp, rightTemp, m_inputStepAdaptation.timeOffset,tempDCMInitialState)) + { + yError() << "[WalkingModule::updateModule] unable to generatempDCMInitialStatete new trajectorie after step adjustment."; + return false; + } + + std::vector DCMPositionAdjusted; + std::vector DCMVelocityAdjusted; + + if(!m_trajectoryGeneratorStepAdjustment->getDCMPositionTrajectory(DCMPositionAdjusted)) + { + yError() << "[WalkingModule::runStepAdaptation] unable to get DCM Position Trajectory"; + return false; + } + + if(!m_trajectoryGeneratorStepAdjustment->getDCMVelocityTrajectory(DCMVelocityAdjusted)) + { + yError() << "[WalkingModule::runStepAdaptation] unable to get DCM Velocity Trajectory"; + return false; + } + + size_t startIndexOfDCMAdjusted = (size_t)round((m_inputStepAdaptation.time - m_inputStepAdaptation.timeOffset) /m_inputStepAdaptation.dT); + + if ((DCMPositionAdjusted.size() - startIndexOfDCMAdjusted)<1) + { + yError() << "[WalkingModule::runningStepAdaptation] the size of dcmPositionAdjusted should be greater than 0"; + return false; + } + m_outputStepAdaptation.dcmPositionAdjusted.resize(DCMPositionAdjusted.size() - startIndexOfDCMAdjusted); + for(int i = 0; i < m_outputStepAdaptation.dcmPositionAdjusted.size(); i++) + m_outputStepAdaptation.dcmPositionAdjusted[i] = DCMPositionAdjusted[i + startIndexOfDCMAdjusted]; + + if ((DCMVelocityAdjusted.size() - startIndexOfDCMAdjusted)<1) + { + yError() << "[WalkingModule::runningStepAdaptation] the size of dcmVelocityAdjusted should be greater than 0"; + return false; + } + m_outputStepAdaptation.dcmVelocityAdjusted.resize(DCMVelocityAdjusted.size() - startIndexOfDCMAdjusted); + for(int i = 0; i < m_outputStepAdaptation.dcmVelocityAdjusted.size(); i++) + m_outputStepAdaptation.dcmVelocityAdjusted[i] = DCMVelocityAdjusted[i + startIndexOfDCMAdjusted]; + } + + m_indexPush=m_outputStepAdaptation.indexPush; + m_zmpNominal=m_outputStepAdaptation.zmpNominal; + m_zmpAdjusted=m_outputStepAdaptation.zmpAdjusted; + m_isPushActive=m_outputStepAdaptation.isPushActive; + m_isRollActive=m_outputStepAdaptation.isRollActive; + m_kDCMSmoother=m_outputStepAdaptation.kDCMSmoother; + m_indexSmoother=m_outputStepAdaptation.indexSmoother; + m_impactTimeNominal=m_outputStepAdaptation.impactTimeNominal; + m_isPitchActive=m_outputStepAdaptation.isPitchActive; + m_kFootSmoother=m_outputStepAdaptation.kFootSmoother; + m_indexFootSmoother=m_outputStepAdaptation.indexFootSmoother; + m_impactTimeAdjusted=m_outputStepAdaptation.impactTimeAdjusted; + m_DCMPositionAdjusted=m_outputStepAdaptation.dcmPositionAdjusted; + m_DCMVelocityAdjusted=m_outputStepAdaptation.dcmVelocityAdjusted; + m_adaptedFootLeftTwist=m_outputStepAdaptation.adaptedFootLeftTwist; + m_currentFootLeftTwist=m_outputStepAdaptation.currentFootLeftTwist; + m_pushRecoveryActiveIndex=m_outputStepAdaptation.pushRecoveryActiveIndex; + m_adaptedFootRightTwist=m_outputStepAdaptation.adaptedFootRightTwist; + m_currentFootRightTwist=m_outputStepAdaptation.currentFootRightTwist; + m_adaptedFootLeftTransform=m_outputStepAdaptation.adaptedFootLeftTransform; + m_currentFootLeftTransform=m_outputStepAdaptation.currentFootLeftTransform; + m_adaptedFootRightTransform=m_outputStepAdaptation.adaptedFootRightTransform; + m_currentFootRightTransform=m_outputStepAdaptation.currentFootRightTransform; + m_adaptedFootLeftAcceleration=m_outputStepAdaptation.adaptedFootLeftAcceleration; + m_currentFootLeftAcceleration=m_outputStepAdaptation.currentFootLeftAcceleration; + m_timeIndexAfterPushDetection=m_outputStepAdaptation.timeIndexAfterPushDetection; + m_adaptedFootRightAcceleration=m_outputStepAdaptation.adaptedFootRightAcceleration; + m_currentFootRightAcceleration=m_outputStepAdaptation.currentFootRightAcceleration; + m_FootTimeIndexAfterPushDetection=m_outputStepAdaptation.FootTimeIndexAfterPushDetection; + } + return true; +} From 4909497f9bcfb6d4ed4469447cd652e96e641f76 Mon Sep 17 00:00:00 2001 From: MiladShafiee Date: Wed, 22 Jul 2020 18:48:35 +0200 Subject: [PATCH 2/3] remove some small bug --- .../dcm_walking/step_adaptation/robotControl.ini | 7 +++++++ .../dcm_walking/step_adaptation/robotControl.ini | 7 +++++++ .../WalkingControllers/WalkingModule/Module.h | 1 + src/WalkingModule/src/Module.cpp | 12 +++++++++++- 4 files changed, 26 insertions(+), 1 deletion(-) diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/robotControl.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/robotControl.ini index eca45e387..a924c714f 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/robotControl.ini @@ -37,3 +37,10 @@ joint_velocity_cut_frequency 10.0 use_wrench_filter 0 wrench_cut_frequency 10.0 + +# 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/step_adaptation/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/robotControl.ini index 8dd3911be..e70c4e655 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/robotControl.ini @@ -34,3 +34,10 @@ joint_velocity_cut_frequency 10.0 use_wrench_filter 0 wrench_cut_frequency 10.0 + +# 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/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index aea78982d..37bb5e5e7 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -133,6 +133,7 @@ namespace WalkingControllers bool m_firstStep; /**< True if this is the first step. */ bool m_useStepAdaptation; /**< True if the step adaptation is used. */ + bool m_useStepAdaptationConfigurationFileValue; /**< True if the step adaptation is used. */ bool m_useMPC; /**< True if the MPC controller is used. */ bool m_useQPIK; /**< True if the QP-IK is used. */ diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index c3d2aefbe..49802fa3c 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -145,6 +145,8 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) m_pushRecoveryActiveIndex=0; m_useStepAdaptation = rf.check("use_step_adaptation", yarp::os::Value(false)).asBool(); + m_useStepAdaptationConfigurationFileValue = rf.check("use_step_adaptation", yarp::os::Value(false)).asBool(); + m_impactTimeNominal = 0; m_impactTimeAdjusted = 0; @@ -636,7 +638,7 @@ bool WalkingModule::updateModule() { m_useStepAdaptation=false; } - else if(desiredUnicyclePosition != nullptr && m_newTrajectoryMergeCounter == 2) + else if(desiredUnicyclePosition != nullptr && m_newTrajectoryMergeCounter == 2 && m_useStepAdaptationConfigurationFileValue) { m_useStepAdaptation=true; } @@ -1510,12 +1512,20 @@ bool WalkingModule::startWalking() } + if (!m_robotControlHelper->loadCustomInteractionMode()) { yError() << "[WalkingModule::startWalking] Unable to set the intraction mode of the joints"; return false; } + + if (!m_robotControlHelper->setImpedanceControlGain()) + { + yError() << "[WalkingModule::startWalking] Unable to set the set Impedance Control gains of the joints"; + return false; + } + // before running the controller the retargeting client goes in approaching phase this // guarantees a smooth transition m_retargetingClient->setPhase(RetargetingClient::Phase::approacing); From d7d70991e846ec03391bd46cbf018335ed6df280 Mon Sep 17 00:00:00 2001 From: iCubGenova04 Date: Thu, 24 Sep 2020 16:30:12 +0000 Subject: [PATCH 3/3] Some tuning on the robot. --- .../dcm_walking/joypad_control/zmpControllerParams.ini | 2 +- .../dcm_walking/step_adaptation/plannerParams.ini | 8 ++++---- .../robots/iCubGenova04/dcm_walking_step_adaptation.ini | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/zmpControllerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/zmpControllerParams.ini index 1187be044..eefa2d035 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/zmpControllerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/zmpControllerParams.ini @@ -4,7 +4,7 @@ useGainScheduling 1 smoothingTime 0.1 # if the gain scheduling is off only k*_walking parameters are used -kZMP_walking 3.5 +kZMP_walking 2.5 kCoM_walking 10.0 kZMP_stance 0.9 diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/plannerParams.ini index ec14f3774..491acc4db 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/plannerParams.ini @@ -10,7 +10,7 @@ slowWhenTurningGain 5.0 ##Bounds #Step length -maxStepLength 0.16 +maxStepLength 0.18 minStepLength 0.01 #Width minWidth 0.13 @@ -19,7 +19,7 @@ minWidth 0.13 maxAngleVariation 8.0 minAngleVariation 6.0 #Timings -maxStepDuration 1.2 +maxStepDuration 1.0 minStepDuration 0.8 ##Nominal Values @@ -31,12 +31,12 @@ stepLandingVelocity 0.0 footApexTime 0.8 comHeightDelta 0.0 #Timings -nominalDuration 1.0 +nominalDuration 0.9 lastStepSwitchTime 0.8 switchOverSwingRatio 0.5 #ZMP Delta -leftZMPDelta (0.03 -0.000) +leftZMPDelta (0.03 -0.005) rightZMPDelta (0.03 0.000) # Last Step DCM Offset diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_step_adaptation.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_step_adaptation.ini index 689aa45a9..26e6c98a2 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_step_adaptation.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_step_adaptation.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 # Remove this line if you don't want to use the step adjustment use_step_adaptation 1