tests on iCubGenova04

torque-controllers/momentum-based-standup/stopTorqueBalancingStandup.m

@@ -45,16 +45,16 @@
         title('Yarp time vs. Simulink time')
     end
 
-    % number of times the Simulink time step was bigger than twice the
+    % number of times the real time step was bigger than twice the
     % desired time step value
-    numOfTimeStepViolations = sum(diff(sim_time) > 2*Config.Ts);
+    numOfTimeStepViolations = sum(diff(yarp_time0) > 2*Config.Ts);
 
-    if numOfTimeStepViolations > 1
+    if numOfTimeStepViolations > 1 && numOfTimeStepViolations <= 50
 
         warning(['The time step tolerance of ', num2str(Config.Ts), '[s] has been violated at least once.'])
 
-    elseif numOfTimeStepViolations > 1000
+    elseif numOfTimeStepViolations > 50
 
-        warning(['The time step tolerance of ', num2str(Config.Ts), '[s] has been violated  more than 1000 times.'])
+        warning(['The time step tolerance of ', num2str(Config.Ts), '[s] has been violated  more than 50 times.'])
     end
 end

torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m

@@ -131,15 +131,15 @@
                       0.0,  0.00,  0.0;   %% state ==  2  COM TRANSITION TO LEFT FOOT
                       0.0,  0.005, 0.0;   %% state ==  3  LEFT FOOT BALANCING 
                       0.0,  0.005, 0.0;   %% state ==  4  YOGA LEFT FOOT
-                      0.0,  0.00,  0.0;   %% state ==  5  PREPARING FOR SWITCHING
+                      0.0,  0.005, 0.0;   %% state ==  5  PREPARING FOR SWITCHING
                       0.02,-0.09,  0.0;   %% state ==  6  LOOKING FOR CONTACT 
                       0.0,  0.00,  0.0;   %% NOT USED
                       % THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE RIGHT FOOT
-                      0.0,  0.012,  0.0;   %% state ==  8  COM TRANSITION TO RIGHT FOOT
-                      0.0, -0.015,  0.0;   %% state ==  9  RIGHT FOOT BALANCING 
-                      0.0, -0.017,  0.0;   %% state == 10  YOGA RIGHT FOOT
-                      0.0,  0.00,   0.0;   %% state == 11  PREPARING FOR SWITCHING
-                      0.02, 0.025,  0.0;   %% state == 12  LOOKING FOR CONTACT 
+                      0.0,  0.00,   0.0;   %% state ==  8  COM TRANSITION TO RIGHT FOOT
+                      0.0, -0.005,  0.0;   %% state ==  9  RIGHT FOOT BALANCING 
+                      0.0, -0.005,  0.0;   %% state == 10  YOGA RIGHT FOOT
+                      0.0, -0.015,  0.0;   %% state == 11  PREPARING FOR SWITCHING
+                      0.02, 0.02,   0.0;   %% state == 12  LOOKING FOR CONTACT 
                       0.0,  0.00,   0.0];  %% NOT USED
 
 % configuration parameters for state machine (YOGA DEMO ONLY) 
@@ -166,7 +166,7 @@
 % on left foot for the number of times the user specifies in the Sm.yogaCounter,
 % and then it will repeat the yoga on the right foot for the same number of times.
 % This option is ignored if Sm.repeatTwiceYogaWithDifferentSpeed = true.
-Sm.oneFootYogaInLoop        = true;
+Sm.oneFootYogaInLoop        = false;
 Sm.yogaCounter              = 10;
 
 % the robot will repeat the yoga moveset twice. This option works as the 
@@ -492,12 +492,12 @@
 
 % gain for feedforward term in joint torques calculation. Valid range: a
 % value between 0 and 1
-Config.K_ff  = 0;
+Config.K_ff  = 1;
 
 % Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA if true, the desired joints
 % accelerations are used for computing the feedforward term in joint
 % torques calculations. Not effective if Config.K_ff = 0.
-Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA = false;
+Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA = true;
 
 %% Constraints for QP for balancing
 

torque-controllers/momentum-based-yoga/stopTorqueBalancingYoga.m

@@ -45,16 +45,16 @@
         title('Yarp time vs. Simulink time')
     end
 
-    % number of times the Simulink time step was bigger than twice the
+    % number of times the real time step was bigger than twice the
     % desired time step value
-    numOfTimeStepViolations = sum(diff(sim_time) > 2*Config.Ts);
+    numOfTimeStepViolations = sum(diff(yarp_time0) > 2*Config.Ts);
 
-    if numOfTimeStepViolations > 1
+    if numOfTimeStepViolations > 1 && numOfTimeStepViolations <= 50
 
         warning(['The time step tolerance of ', num2str(Config.Ts), '[s] has been violated at least once.'])
 
-    elseif numOfTimeStepViolations > 1000
+    elseif numOfTimeStepViolations > 50
 
-        warning(['The time step tolerance of ', num2str(Config.Ts), '[s] has been violated  more than 1000 times.'])
+        warning(['The time step tolerance of ', num2str(Config.Ts), '[s] has been violated  more than 50 times.'])
     end
 end