diff --git a/cmake/WalkingControllersFindDependencies.cmake b/cmake/WalkingControllersFindDependencies.cmake index a577bc49..a3cc3ab8 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 bea59a56..e87f5377 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 57f5fd8d..bbd1267f 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 caa6eb7b..93d56ec6 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 00000000..969896cc --- /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 00000000..bef851e6 --- /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 00000000..f8407fa6 --- /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 00000000..ce14e89e --- /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 00000000..2313d167 --- /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 1ed20519..cf80e0e8 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 95221c5b..38c73609 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 8c7215d0..a49ba165 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 c448c336..240e0e65 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 00000000..8dc88fd8 --- /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 00000000..a924c714 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/robotControl.ini @@ -0,0 +1,46 @@ +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 + +# 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/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 00000000..0411ac61 --- /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 00000000..09df5a9d --- /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 68515325..9c83fa22 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/joypad_control/zmpControllerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/zmpControllerParams.ini index 1187be04..eefa2d03 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 new file mode 100644 index 00000000..491acc4d --- /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.18 +minStepLength 0.01 +#Width +minWidth 0.13 +#Angle Variations in DEGREES +#maxAngleVariation 12.0 +maxAngleVariation 8.0 +minAngleVariation 6.0 +#Timings +maxStepDuration 1.0 +minStepDuration 0.8 + +##Nominal Values +#Width +nominalWidth 0.15 +#Height +stepHeight 0.025 +stepLandingVelocity 0.0 +footApexTime 0.8 +comHeightDelta 0.0 +#Timings +nominalDuration 0.9 +lastStepSwitchTime 0.8 +switchOverSwingRatio 0.5 + +#ZMP Delta +leftZMPDelta (0.03 -0.005) +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 00000000..e70c4e65 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/robotControl.ini @@ -0,0 +1,43 @@ +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 + +# 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/stepAdaptation.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/stepAdaptation.ini new file mode 100644 index 00000000..90821be0 --- /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 00000000..8f57df14 --- /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 00000000..26e6c98a --- /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 0c1f221c..37bb5e5e 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,69 @@ 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_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. */ @@ -74,6 +142,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 +154,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 +166,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 +178,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 +306,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 +369,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 92a0d82b..49802fa3 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,37 @@ 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_useStepAdaptationConfigurationFileValue = 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 +230,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 +239,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 +419,8 @@ void WalkingModule::reset() m_trajectoryGenerator->reset(); + m_trajectoryGeneratorStepAdjustment->reset(); + if(m_dumpData) m_walkingLogger->quit(); } @@ -365,12 +449,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 +471,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_leftTwistTrajectory.front(), - m_rightTwistTrajectory.front()); + solver->setDesiredFeetTwist(m_smoothedFootLeftTwist, + m_smoothedFootRightTwist); + } + else + { + solver->setDesiredFeetTransformation(m_leftTrajectory.front(), + m_rightTrajectory.front()); + + solver->setDesiredFeetTwist(m_leftTwistTrajectory.front(), + m_rightTwistTrajectory.front()); + } solver->setDesiredCoMVelocity(desiredCoMVelocity); solver->setDesiredCoMPosition(desiredCoMPosition); @@ -488,8 +585,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 +633,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_useStepAdaptationConfigurationFileValue) + { + 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 +740,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 +824,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 +889,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 +1276,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 +1321,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 +1361,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; } @@ -1250,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); @@ -1285,13 +1555,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 +1575,7 @@ bool WalkingModule::setPlannerInput(double x, double y) if(m_newTrajectoryRequired) return true; - m_newTrajectoryMergeCounter = 20; + m_newTrajectoryMergeCounter = 10; } } @@ -1354,3 +1624,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; +}