diff --git a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h index ece43cd50..e9de80899 100644 --- a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h +++ b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h @@ -156,9 +156,9 @@ namespace WalkingControllers * Get all the feedback signal from the interfaces * @return true in case of success and false otherwise. */ - bool getFeedbacks(unsigned int maxAttempts = 1); + bool getFeedbacks(unsigned int maxAttempts, double attemptDelay); - bool getFeedbacksRaw(unsigned int maxAttempts = 1); + bool getFeedbacksRaw(unsigned int maxAttempts, double attemptDelay); /** * Set the desired position reference. (The position will be sent using PositionControl mode) @@ -190,7 +190,7 @@ namespace WalkingControllers * Reset filters. * @return true in case of success and false otherwise. */ - bool resetFilters(); + bool resetFilters(unsigned int maxAttempts, double attemptDelay); /** * Close the polydrives. diff --git a/src/RobotInterface/src/Helper.cpp b/src/RobotInterface/src/Helper.cpp index ce1451555..93f031b8f 100644 --- a/src/RobotInterface/src/Helper.cpp +++ b/src/RobotInterface/src/Helper.cpp @@ -47,7 +47,7 @@ bool RobotInterface::getWorstError(const iDynTree::VectorDynSize& desiredJointPo return true; } -bool RobotInterface::getFeedbacksRaw(unsigned int maxAttempts) +bool RobotInterface::getFeedbacksRaw(unsigned int maxAttempts, double attemptDelay) { if(!m_encodersInterface) { @@ -187,7 +187,7 @@ bool RobotInterface::getFeedbacksRaw(unsigned int maxAttempts) return true; } - yarp::os::Time::delay(0.001); + yarp::os::Time::delay(attemptDelay); attempt++; } while (attempt < maxAttempts); @@ -665,9 +665,9 @@ bool RobotInterface::configurePIDHandler(const yarp::os::Bottle& config) } -bool RobotInterface::resetFilters() +bool RobotInterface::resetFilters(unsigned int maxAttempts, double attemptDelay) { - if(!getFeedbacksRaw(100)) + if(!getFeedbacksRaw(maxAttempts, attemptDelay)) { yError() << "[RobotInterface::resetFilters] Unable to get the feedback from the robot"; return false; @@ -710,9 +710,9 @@ bool RobotInterface::resetFilters() return true; } -bool RobotInterface::getFeedbacks(unsigned int maxAttempts) +bool RobotInterface::getFeedbacks(unsigned int maxAttempts, double attemptDelay) { - if(!getFeedbacksRaw(maxAttempts)) + if(!getFeedbacksRaw(maxAttempts, attemptDelay)) { yError() << "[RobotInterface::getFeedbacks] Unable to get the feedback from the robot"; return false; diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_hand_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_hand_retargeting.ini index e11360fd1..c5866054f 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_hand_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_hand_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini index 573251482..068d7291e 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini @@ -41,6 +41,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_with_joypad.ini index 33fb3781b..df5315c3e 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_with_joypad.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_hand_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_hand_retargeting.ini index 71585596e..81d155d27 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_hand_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_hand_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_joint_retargeting.ini index 0d83596df..3469cfb90 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_joint_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_retargeting.ini index 1339e82f9..ecd35678a 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_with_joypad.ini index 625b02573..a1fb750de 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_with_joypad.ini @@ -41,6 +41,9 @@ goal_port_scaling (1.0, 1.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_hand_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_hand_retargeting.ini index cd34af55a..406940560 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_hand_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_hand_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_iFeel_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_iFeel_joint_retargeting.ini index 49d9c55d1..1c5782047 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_iFeel_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_iFeel_joint_retargeting.ini @@ -23,6 +23,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini index 52157eaca..0d94a9939 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_with_joypad.ini index d71c289d6..0405e8b38 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_with_joypad.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini index 8891b89dc..16bf19c1e 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini @@ -15,18 +15,18 @@ useGainScheduling 0 [DEFAULT] #NAME P I D #torso_pitch 5.0 1.0 0.5 -l_knee -30000.0 -1000.0 0.0 -r_knee 30000.0 1000.0 0.0 +l_knee -33000.0 -1000.0 0.0 +r_knee 33000.0 1000.0 0.0 l_ankle_pitch -15000.0 -1000.0 0.0 r_ankle_pitch 15000.0 1000.0 0.0 -l_ankle_roll -8000.0 -1000.0 0.0 -r_ankle_roll 8000.0 1000.0 0.0 -l_hip_pitch -10000.0 -1500.0 0.0 -r_hip_pitch 10000.0 1500.0 0.0 -l_hip_roll 25000.0 1000.0 0.0 -r_hip_roll -25000.0 -1000.0 0.0 -r_hip_yaw 10000.0 2000.0 0.0 -l_hip_yaw -7000.0 -2000.0 0.0 +l_ankle_roll -10000.0 -1000.0 0.0 +r_ankle_roll 10000.0 1000.0 0.0 +l_hip_pitch -11000.0 -1500.0 0.0 +r_hip_pitch 11000.0 1500.0 0.0 +l_hip_roll 27500.0 1000.0 0.0 +r_hip_roll -27500.0 -1000.0 0.0 +r_hip_yaw 11000.0 2000.0 0.0 +l_hip_yaw -11000.0 -2000.0 0.0 #r_shoulder_pitch -10000.0 -10000.0 0.0 #r_shoulder_roll -10000.0 -10000.0 0.0 r_shoulder_yaw -3000.0 -1500.0 0.0 @@ -34,4 +34,4 @@ r_shoulder_yaw -3000.0 -1500.0 0.0 #l_shoulder_pitch 10000.0 10000.0 0.0 #l_shoulder_roll 10000.0 10000.0 0.0 l_shoulder_yaw 3000.0 1500.0 0.0 -#l_elbow -4000.0 -4000.0 0.0 +#l_elbow -4000.0 -4000.0 0.0 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini index 3e1104fd0..d3bf9f1a9 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini @@ -10,7 +10,7 @@ referencePosition (0.1 0.0) timeWeight 2.5 positionWeight 1.0 slowWhenTurningGain 2.0 -slowWhenBackwardFactor 1.0 +slowWhenBackwardFactor 0.8 slowWhenSidewaysFactor 0.2 # Conservative factors that multiply the unicycle velocity saturations @@ -21,8 +21,8 @@ saturationFactors (0.7, 0.7) ##Bounds #Step length -maxStepLength 0.27 -minStepLength 0.05 +maxStepLength 0.25 +minStepLength 0.01 #Width minWidth 0.12 #Angle Variations in DEGREES @@ -47,8 +47,8 @@ lastStepSwitchTime 0.8 switchOverSwingRatio 0.3 #ZMP Delta -leftZMPDelta (0.01 -0.0) -rightZMPDelta (0.01 0.01) +leftZMPDelta (0.005 -0.0) +rightZMPDelta (0.005 0.0) #Feet cartesian offset on the yaw leftYawDeltaInDeg 0.0 @@ -77,4 +77,4 @@ startAlwaysSameFoot 1 # useMinimumJerkFootTrajectory 1 ##Remove this line if you want to enable the pause conditon -isPauseActive 0 +isPauseActive 1 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini index 4477491d6..1153d6fcc 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini @@ -4,7 +4,7 @@ useGainScheduling 1 smoothingTime 0.05 # if the gain scheduling is off only k*_walking parameters are used -kZMP_walking 2.0 +kZMP_walking 1.8 # kZMP_stance 1.0 kCoM_walking 5.0 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini index d6fffcaca..8611abc63 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini @@ -7,8 +7,8 @@ kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) states ("STANCE", "WALKING") -sampling_time 0.01 -settling_time 4.5 +sampling_time 0.002 +settling_time 5.0 [STANCE] name "stance" @@ -21,9 +21,17 @@ weight (0.0, 0.0, 0.0, [WALKING] name "walking" +# weight (0.0, 0.0, 0.0, +# 2.0, 2.0, 2.0, +# 2.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0, +# 2.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0, +# 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, +# 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + weight (0.0, 0.0, 0.0, - 1.0, 1.0, 1.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 2.0, 2.0, 2.0, + 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini index 903728e5b..45f04e11e 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini @@ -7,8 +7,8 @@ kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) states ("STANCE", "WALKING") -sampling_time 0.01 -settling_time 4.5 +sampling_time 0.002 +settling_time 5.0 [STANCE] name "stance" @@ -21,9 +21,17 @@ weight (2.0, 2.0, 2.0, [WALKING] name "walking" +# weight (2.0, 2.0, 2.0, +# 0.0, 0.0, 0.0, +# 0.0, 0.0, 0.0, 2.0, 2.0, 5.0, 5.0, +# 0.0, 0.0, 0.0, 2.0, 2.0, 5.0, 5.0, +# 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, +# 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + + weight (2.0, 2.0, 2.0, - 1.0, 1.0, 1.0, - 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, - 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, + 0.0, 0.0, 0.0, + 2.0, 0.0, 2.0, 2.0, 2.0, 5.0, 5.0, + 2.0, 0.0, 2.0, 2.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini index e69638dd4..01d7def3d 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini @@ -4,8 +4,8 @@ kp_angular 5.0 states ("STANCE", "WALKING") -sampling_time 0.01 -settling_time 4.5 +sampling_time 0.002 +settling_time 3.0 [STANCE] name "stance" diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini index 87c2081f9..4b75c8316 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini @@ -6,7 +6,7 @@ kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) states ("stance", "walking") -sampling_time 0.01 +sampling_time 0.002 settling_time 0.5 [stance] diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini index 8258c0af5..89f8654e2 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini @@ -4,7 +4,7 @@ kp_angular 5.0 states ("stance", "walking") -sampling_time 0.01 +sampling_time 0.002 settling_time 0.5 [stance] diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_hand_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_hand_retargeting.ini index 1228cfb99..abf20497a 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_hand_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_hand_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini index 8eedaea43..74989199d 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini @@ -20,7 +20,7 @@ max_initial_com_vel 0.15 constant_ZMP_tolerance 0.000001 # Consecutive times in which the ZMP remains constant. If this limit is reached, the module stops. Use a negative value to disable. -constant_ZMP_counter 25 +constant_ZMP_counter 125 # Minimum force to consider the ZMP valid. minimum_normal_force_ZMP 1.0 @@ -39,7 +39,10 @@ goal_port_suffix /goal:i goal_port_scaling (0.5, 1.0, 0.5) # How much in advance the planner should be called. The time is in seconds -planner_advance_time_in_s 0.08 +planner_advance_time_in_s 0.02 + +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 # general parameters [GENERAL] @@ -47,7 +50,7 @@ name walking-coordinator # height of the com com_height 0.565 # sampling time -sampling_time 0.01 +sampling_time 0.002 # enable joint retargeting use_joint_retargeting 1 # enable the virtualizer diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_joint_retargeting.ini index 42949a07d..32a4ed6e3 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_joint_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini index e13120db0..86edcbe05 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini @@ -20,7 +20,7 @@ max_initial_com_vel 0.15 constant_ZMP_tolerance 0.00001 # Consecutive times in which the ZMP remains constant. If this limit is reached, the module stops. Use a negative value to disable. -constant_ZMP_counter 25 +constant_ZMP_counter 125 # Minimum force to consider the ZMP valid. minimum_normal_force_ZMP 1.0 @@ -39,7 +39,10 @@ goal_port_suffix /goal:i goal_port_scaling (0.5, 1.0, 0.5) # How much in advance the planner should be called. The time is in seconds -planner_advance_time_in_s 0.08 +planner_advance_time_in_s 0.02 + +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 # general parameters [GENERAL] @@ -47,10 +50,11 @@ name walking-coordinator # height of the com com_height 0.565 # sampling time -sampling_time 0.01 +sampling_time 0.002 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link height_reference_frame root_link + # include robot control parameters [include ROBOT_CONTROL "./dcm_walking/joypad_control/robotControl.ini"] diff --git a/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking_with_joypad.ini index 98d01c2f6..8d6572072 100644 --- a/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking_with_joypad.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 7f472d5e1..eaabb5b46 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -148,6 +148,9 @@ namespace WalkingControllers size_t m_plannerAdvanceTimeSteps; /** How many steps in advance the planner should be called. */ + size_t m_feedbackAttempts; + double m_feedbackAttemptDelay; + // debug std::unique_ptr m_velocityIntegral{nullptr}; diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 085cbade4..ed565e933 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -12,8 +12,11 @@ #include // YARP +#include +#include #include #include +#include #include #include @@ -194,6 +197,11 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) return false; } + double maxFBDelay = rf.check("max_feedback_delay_in_s", yarp::os::Value(1.0)).asFloat64(); + m_feedbackAttemptDelay = m_dT / 10; + m_feedbackAttempts = maxFBDelay / m_feedbackAttemptDelay; + + double plannerAdvanceTimeInS = rf.check("planner_advance_time_in_s", yarp::os::Value(0.18)).asFloat64(); m_plannerAdvanceTimeSteps = std::round(plannerAdvanceTimeInS / m_dT) + 2; //The additional 2 steps are because the trajectory from the planner is requested two steps in advance wrt the merge point @@ -396,7 +404,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) { yarp::os::Bottle& loggerOptions = rf.findGroup("WALKING_LOGGER"); // open and connect the data logger port - std::string portInput, portOutput; + std::string portOutput; // open the connect the data logger port if(!YarpUtilities::getStringFromSearchable(loggerOptions, "dataLoggerOutputPort_name", @@ -405,21 +413,8 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) yError() << "[WalkingModule::configure] Unable to get the string from searchable."; return false; } - if(!YarpUtilities::getStringFromSearchable(loggerOptions, - "dataLoggerInputPort_name", - portInput)) - { - yError() << "[WalkingModule::configure] Unable to get the string from searchable."; - return false; - } m_loggerPort.open("/" + name + portOutput); - - if(!yarp::os::Network::connect("/" + name + portOutput, portInput)) - { - yError() << "Unable to connect the ports " << "/" + name + portOutput << "and" << portInput; - return false; - } } // time profiler @@ -618,7 +613,7 @@ bool WalkingModule::updateModule() if(m_robotState == WalkingFSM::Preparing) { - if(!m_robotControlHelper->getFeedbacksRaw(100)) + if(!m_robotControlHelper->getFeedbacksRaw(m_feedbackAttempts, m_feedbackAttemptDelay)) { yError() << "[updateModule] Unable to get the feedback."; return false; @@ -663,7 +658,7 @@ bool WalkingModule::updateModule() m_stableDCMModel->reset(m_DCMPositionDesired.front()); // reset the retargeting - if(!m_robotControlHelper->getFeedbacks(100)) + if(!m_robotControlHelper->getFeedbacks(m_feedbackAttempts, m_feedbackAttemptDelay)) { yError() << "[WalkingModule::updateModule] Unable to get the feedback."; return false; @@ -787,7 +782,7 @@ bool WalkingModule::updateModule() } // get feedbacks and evaluate useful quantities - if(!m_robotControlHelper->getFeedbacks(100)) + if(!m_robotControlHelper->getFeedbacks(m_feedbackAttempts, m_feedbackAttemptDelay)) { yError() << "[WalkingModule::updateModule] Unable to get the feedback."; return false; @@ -1269,7 +1264,7 @@ bool WalkingModule::prepareRobot(bool onTheFly) // get the current state of the robot // this is necessary because the trajectories for the joints, CoM height and neck orientation // depend on the current state of the robot - if(!m_robotControlHelper->getFeedbacksRaw(100)) + if(!m_robotControlHelper->getFeedbacksRaw(m_feedbackAttempts, m_feedbackAttemptDelay)) { yError() << "[WalkingModule::prepareRobot] Unable to get the feedback."; return false; @@ -1595,7 +1590,7 @@ bool WalkingModule::startWalking() // if the robot was only prepared the filters has to be reseted if(m_robotState == WalkingFSM::Prepared) { - m_robotControlHelper->resetFilters(); + m_robotControlHelper->resetFilters(m_feedbackAttempts, m_feedbackAttemptDelay); updateFKSolver();