diff --git a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h index ece43cd5..e9de8089 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 ce145155..670b6652 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); @@ -494,7 +494,7 @@ bool RobotInterface::configureForceTorqueSensor(const std::string& portPrefix, measuredWrench.port = std::make_unique>(); measuredWrench.port->open("/" + portPrefix + portInputName); // connect port - if(!yarp::os::Network::connect(wholeBodyDynamicsPortName, "/" + portPrefix + portInputName)) + if(!yarp::os::Network::connect(wholeBodyDynamicsPortName, "/" + portPrefix + portInputName, "fast_tcp")) { yError() << "[RobotInterface::configureForceTorqueSensors] Unable to connect to port " << wholeBodyDynamicsPortName << " to " << "/" + portPrefix + portInputName; @@ -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 e11360fd..c5866054 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 57325148..068d7291 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 33fb3781..df5315c3 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 71585596..81d155d2 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 0d83596d..3469cfb9 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 1339e82f..ecd35678 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 625b0257..a1fb750d 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 cd34af55..40694056 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 49d9c55d..1c578204 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 52157eac..0d94a993 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 d71c289d..0405e8b3 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 8891b89d..ae37d599 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 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 3e1104fd..d3bf9f1a 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 4477491d..1153d6fc 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 d6fffcac..ea2f5860 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 903728e5..d12adc66 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,19 @@ 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, - 1.0, 1.0, 1.0, 1.0, 1.0, 1.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 e69638dd..01d7def3 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 87c2081f..4b75c831 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 8258c0af..89f8654e 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 1228cfb9..abf20497 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 8eedaea4..74989199 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 42949a07..32a4ed6e 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 e13120db..86edcbe0 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 98d01c2f..8d657207 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 7f472d5e..42ff77c1 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -148,11 +148,26 @@ 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}; yarp::os::BufferedPort m_loggerPort; /**< Logger port. */ + yarp::os::BufferedPort m_leftFTArmRotatedPort; /**< Desired robot position port. */ + yarp::os::BufferedPort m_rightFTArmRotatedPort; /**< Desired robot position port. */ + + yarp::os::BufferedPort m_leftFTArmPort; /**< Desired robot position port. */ + yarp::os::BufferedPort m_rightFTArmPort; /**< Desired robot position port. */ + + yarp::os::BufferedPort m_rightHandPort; + yarp::os::BufferedPort m_leftHandPort; + + int m_sendInfoIndex{0}; + int m_sendInfoMaxCounter{0}; + /** * Get the robot model from the resource finder and set it. * @param rf is the reference to a resource finder object. diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 085cbade..60da3cb3 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,13 @@ 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; + + // we will send the data every 100ms + m_sendInfoMaxCounter = std::floor(0.01 / m_dT); + 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 @@ -247,6 +257,56 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) return false; } + ///////////////// TODO remove this code. DONOT merge + std::string leftFTArmRoatedPort = "/" + getName() + "/left_arm_inertial_ft"; + if(!m_leftFTArmRotatedPort.open(leftFTArmRoatedPort)) + { + yError() << "[WalkingModule::configure] Could not open" << leftFTArmRoatedPort << " port."; + return false; + } + + std::string rightFTArmRoatedPort = "/" + getName() + "/right_arm_inertial_ft"; + if(!m_rightFTArmRotatedPort.open(rightFTArmRoatedPort)) + { + yError() << "[WalkingModule::configure] Could not open" << rightFTArmRoatedPort << " port."; + return false; + } + + std::string leftFTArmPort = "/" + getName() + "/left_arm_filtered_ft"; + if(!m_leftFTArmPort.open(leftFTArmPort)) + { + yError() << "[WalkingModule::configure] Could not open" << leftFTArmPort << " port."; + return false; + } + + + std::string rightFTArmPort = "/" + getName() + "/right_arm_filtered_ft"; + if(!m_rightFTArmPort.open(rightFTArmPort)) + { + yError() << "[WalkingModule::configure] Could not open" << rightFTArmPort << " port."; + return false; + } + + + yarp::os::Network::connect("/wholeBodyDynamics/filteredFT/l_arm_ft_sensor", leftFTArmPort, "fast_tcp"); + yarp::os::Network::connect("/wholeBodyDynamics/filteredFT/r_arm_ft_sensor", rightFTArmPort, "fast_tcp"); + + + std::string leftHandPort = "/" + getName() + "/left_hand:o"; + if(!m_leftHandPort.open(leftHandPort)) + { + yError() << "[WalkingModule::configure] Could not open" << leftHandPort << " port."; + return false; + } + + std::string rightHandPort = "/" + getName() + "/right_hand:o"; + if(!m_rightHandPort.open(rightHandPort)) + { + yError() << "[WalkingModule::configure] Could not open" << rightHandPort << " port."; + return false; + } + ///////////////// + // initialize the trajectory planner m_trajectoryGenerator = std::make_unique(); yarp::os::Bottle& trajectoryPlannerOptions = rf.findGroup("TRAJECTORY_PLANNER"); @@ -396,7 +456,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 +465,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 +665,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 +710,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; @@ -725,6 +772,80 @@ bool WalkingModule::updateModule() m_profiler->setInitTime("Total"); + ///////////////////////// TODO this part of the code should be removed. DONOT merge this + ///feature + if (m_sendInfoIndex == 0) + { + yarp::sig::Vector* leftFTData = m_leftFTArmPort.read(false); + yarp::sig::Vector& leftArmRotated = m_leftFTArmRotatedPort.prepare(); + leftArmRotated.resize(3); + if (leftFTData == nullptr) + { + yarp::eigen::toEigen(leftArmRotated).setZero(); + } else + { + iDynTree::Vector3 temp; + temp(0) = (*leftFTData)[0]; + temp(1) = (*leftFTData)[1]; + temp(2) = (*leftFTData)[2]; + + yarp::eigen::toEigen(leftArmRotated) + = iDynTree::toEigen(m_FKSolver->getKinDyn() + ->getWorldTransform("l_arm_ft_sensor") + .getRotation()) + * iDynTree::toEigen(temp); + } + m_leftFTArmRotatedPort.write(); + + yarp::sig::Vector* rightFTData = m_rightFTArmPort.read(false); + yarp::sig::Vector& rightArmRotated = m_rightFTArmRotatedPort.prepare(); + rightArmRotated.resize(3); + if (rightFTData == nullptr) + { + yarp::eigen::toEigen(rightArmRotated).setZero(); + } else + { + iDynTree::Vector3 temp; + temp(0) = (*rightFTData)[0]; + temp(1) = (*rightFTData)[1]; + temp(2) = (*rightFTData)[2]; + + yarp::eigen::toEigen(rightArmRotated) + = iDynTree::toEigen(m_FKSolver->getKinDyn() + ->getWorldTransform("r_arm_ft_sensor") + .getRotation()) + * iDynTree::toEigen(temp); + } + m_rightFTArmRotatedPort.write(); + + const iDynTree::Transform head_T_left_hand + = m_FKSolver->getHeadToWorldTransform().inverse() + * m_FKSolver->getLeftHandToWorldTransform(); + + const iDynTree::Transform head_T_right_hand + = m_FKSolver->getHeadToWorldTransform().inverse() + * m_FKSolver->getRightHandToWorldTransform(); + + yarp::sig::Matrix& matrixLeft = m_leftHandPort.prepare(); + matrixLeft.resize(4, 4); + iDynTree::toYarp(head_T_left_hand, matrixLeft); + m_leftHandPort.write(); + + yarp::sig::Matrix& matrixRight = m_rightHandPort.prepare(); + matrixRight.resize(4, 4); + iDynTree::toYarp(head_T_right_hand, matrixRight); + m_rightHandPort.write(); + } + + // send the data every m_sendInfoMaxCounter + m_sendInfoIndex++; + if (m_sendInfoIndex > m_sendInfoMaxCounter) + { + m_sendInfoIndex = 0; + } + + ////////////////////////////////// + // check desired planner input yarp::sig::Vector* desiredUnicyclePosition = nullptr; desiredUnicyclePosition = m_desiredUnyciclePositionPort.read(false); @@ -787,7 +908,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 +1390,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 +1716,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();