From e2882e555d5e9703e0f1421d9bed03bbc86cead5 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 28 Sep 2022 17:21:51 +0000 Subject: [PATCH 1/6] Increase the frequency of the walking-controller to 500Hz --- .../dcm_walking/joypad_control/tasks/regularization.ini | 2 +- .../iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini | 2 +- .../iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini | 2 +- .../app/robots/iCubGenova09/dcm_walking_with_joypad.ini | 5 +++-- 4 files changed, 6 insertions(+), 5 deletions(-) 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_iFeel_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini index 8eedaea4..7e24d798 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 @@ -39,7 +39,7 @@ 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 # general parameters [GENERAL] 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..8e8260c2 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini @@ -39,7 +39,7 @@ 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 # general parameters [GENERAL] @@ -47,10 +47,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"] From 920b694cfc943093cc9e668ed317f6518f38f274 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 4 Oct 2022 09:37:34 +0000 Subject: [PATCH 2/6] Increased ZMP counter since the frequency is higher --- .../robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini | 2 +- .../app/robots/iCubGenova09/dcm_walking_with_joypad.ini | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 7e24d798..788dc698 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 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 8e8260c2..e627a179 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 From 8c566f72001d9cf2c0c6a6e585b06829b9e42410 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 5 Oct 2022 12:49:15 +0200 Subject: [PATCH 3/6] Avoid to force the connection to the logger when the dump_data is enabled --- src/WalkingModule/src/Module.cpp | 15 +-------------- 1 file changed, 1 insertion(+), 14 deletions(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 085cbade..ead2de7f 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -396,7 +396,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 +405,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 From 541d99739a82e5632f9f2d2fa2364034e42273ee Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 8 Oct 2022 18:06:42 +0200 Subject: [PATCH 4/6] Make the walking running at 500Hz while iFeel retargeting is enable --- .../iFeel_joint_retargeting/tasks/regularization.ini | 4 ++-- .../iFeel_joint_retargeting/tasks/retargeting.ini | 6 +++--- .../dcm_walking/iFeel_joint_retargeting/tasks/torso.ini | 4 ++-- .../iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) 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..6512f658 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 3.0 [STANCE] name "stance" 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..1c4bd716 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 3.0 [STANCE] name "stance" @@ -22,7 +22,7 @@ weight (2.0, 2.0, 2.0, [WALKING] name "walking" weight (2.0, 2.0, 2.0, - 1.0, 1.0, 1.0, + 0.0, 0.0, 0.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, 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_iFeel_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini index 788dc698..2ead6b99 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 @@ -47,7 +47,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 From 237a7778bf83b5a9bb6c1251b77a51152f66e73c Mon Sep 17 00:00:00 2001 From: Stefano Date: Sun, 9 Oct 2022 15:21:16 +0200 Subject: [PATCH 5/6] Added possibility to change the timeout behaviors to get feedback --- .../WalkingControllers/RobotInterface/Helper.h | 6 +++--- src/RobotInterface/src/Helper.cpp | 12 ++++++------ .../dcm_walking_hand_retargeting.ini | 3 +++ .../dcm_walking_joint_retargeting.ini | 3 +++ .../iCubGazeboV2_5/dcm_walking_with_joypad.ini | 3 +++ .../dcm_walking_hand_retargeting.ini | 3 +++ .../dcm_walking_joint_retargeting.ini | 3 +++ .../iCubGazeboV3/dcm_walking_retargeting.ini | 3 +++ .../iCubGazeboV3/dcm_walking_with_joypad.ini | 3 +++ .../dcm_walking_hand_retargeting.ini | 3 +++ .../dcm_walking_iFeel_joint_retargeting.ini | 3 +++ .../dcm_walking_joint_retargeting.ini | 3 +++ .../iCubGenova04/dcm_walking_with_joypad.ini | 3 +++ .../dcm_walking_hand_retargeting.ini | 3 +++ .../dcm_walking_iFeel_joint_retargeting.ini | 3 +++ .../dcm_walking_joint_retargeting.ini | 3 +++ .../iCubGenova09/dcm_walking_with_joypad.ini | 3 +++ .../icubGazeboSim/dcm_walking_with_joypad.ini | 3 +++ .../WalkingControllers/WalkingModule/Module.h | 3 +++ src/WalkingModule/src/Module.cpp | 18 +++++++++++++----- 20 files changed, 73 insertions(+), 14 deletions(-) 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..93f031b8 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 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_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 2ead6b99..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 @@ -41,6 +41,9 @@ 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.02 +# 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_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 e627a179..86edcbe0 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini @@ -41,6 +41,9 @@ 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.02 +# 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/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..eaabb5b4 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 ead2de7f..ed565e93 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 @@ -605,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; @@ -650,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; @@ -774,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; @@ -1256,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; @@ -1582,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(); From 4cddf8ece48e4b9a919397b116dd10e08d184d4a Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 13 Feb 2023 16:02:56 +0100 Subject: [PATCH 6/6] Tune the parameters and gains for the ANA avatar Xprize - low level pid - planner - com-zmp controller - ik tasks (regularization and retargeting) --- .../dcm_walking/common/pidParams.ini | 22 +++++++++---------- .../dcm_walking/common/plannerParams.ini | 12 +++++----- .../common/zmpControllerParams.ini | 2 +- .../tasks/regularization.ini | 16 ++++++++++---- .../tasks/retargeting.ini | 14 +++++++++--- 5 files changed, 41 insertions(+), 25 deletions(-) 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..16bf19c1 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 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 6512f658..8611abc6 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 @@ -8,7 +8,7 @@ kp (5.0, 5.0, 5.0, states ("STANCE", "WALKING") sampling_time 0.002 -settling_time 3.0 +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 1c4bd716..45f04e11 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 @@ -8,7 +8,7 @@ kp (5.0, 5.0, 5.0, states ("STANCE", "WALKING") sampling_time 0.002 -settling_time 3.0 +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, 0.0, 0.0, 0.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, + 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)