Ft rotated for ines

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.

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<yarp::os::BufferedPort<yarp::sig::Vector>>();
     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;

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

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

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

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

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

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

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

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

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

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

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

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

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

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
 

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)
+

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)
+
+

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"

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]

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]

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

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,15 +39,18 @@ 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]
 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