remove some small bug

robotology · Jul 22, 2020 · 2f85f15 · 2f85f15
1 parent 922bc18
commit 2f85f15
Show file tree

Hide file tree

Showing 4 changed files with 26 additions and 1 deletion.
diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/robotControl.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/step_adaptation/robotControl.ini
@@ -37,3 +37,10 @@ joint_velocity_cut_frequency       10.0
 
 use_wrench_filter                  0
 wrench_cut_frequency               10.0
+
+# if true a good joint traking is considered mandatory
+good_tracking_required  (true, true, true,
+                         true, true, true, true,
+                         true, true, true, true,
+                         true, true, true, true, true, true,
+                         true, true, true, true, true, true)
diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/step_adaptation/robotControl.ini
@@ -34,3 +34,10 @@ joint_velocity_cut_frequency       10.0
 
 use_wrench_filter                  0
 wrench_cut_frequency               10.0
+
+# if true a good joint traking is considered mandatory
+good_tracking_required  (true, true, true,
+                         true, true, true, true,
+                         true, true, true, true,
+                         true, true, true, true, true, true,
+                         true, true, true, true, true, true)
diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h
@@ -133,6 +133,7 @@ namespace WalkingControllers
 
        bool m_firstStep; /**< True if this is the first step. */
        bool m_useStepAdaptation; /**< True if the step adaptation is used. */
+       bool m_useStepAdaptationConfigurationFileValue; /**< True if the step adaptation is used. */
 
         bool m_useMPC; /**< True if the MPC controller is used. */
         bool m_useQPIK; /**< True if the QP-IK is used. */

diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp
@@ -142,6 +142,8 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf)
 
         m_pushRecoveryActiveIndex=0;
         m_useStepAdaptation = rf.check("use_step_adaptation", yarp::os::Value(false)).asBool();
+        m_useStepAdaptationConfigurationFileValue = rf.check("use_step_adaptation", yarp::os::Value(false)).asBool();
+
         m_impactTimeNominal = 0;
         m_impactTimeAdjusted = 0;
 
@@ -633,7 +635,7 @@ bool WalkingModule::updateModule()
         {
             m_useStepAdaptation=false;
         }
-        else if(desiredUnicyclePosition != nullptr && m_newTrajectoryMergeCounter == 2)
+        else if(desiredUnicyclePosition != nullptr && m_newTrajectoryMergeCounter == 2 && m_useStepAdaptationConfigurationFileValue)
         {
             m_useStepAdaptation=true;
         }
@@ -1507,12 +1509,20 @@ bool WalkingModule::startWalking()
 
      }
 
+
     if (!m_robotControlHelper->loadCustomInteractionMode())
     {
         yError() << "[WalkingModule::startWalking] Unable to set the intraction mode of the joints";
         return false;
     }
 
+
+    if (!m_robotControlHelper->setImpedanceControlGain())
+    {
+        yError() << "[WalkingModule::startWalking] Unable to set the set Impedance Control gains of the joints";
+        return false;
+    }
+
     // before running the controller the retargeting client goes in approaching phase this
     // guarantees a smooth transition
     m_retargetingClient->setPhase(RetargetingClient::Phase::approacing);