final working version on iCubGenova02

dbdxnuliba · Sep 24, 2018 · a1bfe34 · a1bfe34
1 parent 5a0168c
commit a1bfe34
Show file tree

Hide file tree

Showing 4 changed files with 352 additions and 44 deletions.
diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m
@@ -151,7 +151,10 @@
 %% State machine parameters
 
 % smoothing time for joints and CoM
-Sm.smoothingTimeCoM_Joints = 3; 
+Sm.smoothingTimeCoM_Joints  = 3; 
+
+% scale factor smoothing time (YOGA DEMO ONLY)
+Sm.scaleFactorSmoothingTime = 0.9; 
 
 % time between two yoga positions (YOGA DEMO ONLY)
 Sm.joints_pauseBetweenYogaMoves = 0;
@@ -172,9 +175,11 @@
 Sm.CoM_delta = [0; 0; 0];
 
 % joint references (YOGA DEMO ONLY)
-Sm.joints_references   = zeros(1,ROBOT_DOF);
-Sm.joints_leftYogaRef  = zeros(1,ROBOT_DOF+1);
-Sm.joints_rightYogaRef = zeros(1,ROBOT_DOF+1);
+Sm.joints_references         = zeros(1,ROBOT_DOF);
+Sm.joints_leftYogaRef        = zeros(1,ROBOT_DOF+1);
+Sm.joints_rightYogaRef       = zeros(1,ROBOT_DOF+1);
+Sm.joints_leftSecondYogaRef  = zeros(1,ROBOT_DOF+1);
+Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1);
 
 % configuration parameters for state machine (YOGA DEMO ONLY) 
 Sm.tBalancing               = 1;
@@ -184,6 +189,11 @@
 Sm.demoStartsOnRightSupport = false;
 Sm.yogaAlsoOnRightFoot      = false;
 Sm.yogaInLoop               = false;
+Sm.repeatYogaMoveset        = false;
+
+% smoothing time for the second time the Yoga moveset are performed (YOGA DEMO ONLY)
+Sm.smoothingTimeSecondYogaLeft  = 1;
+Sm.smoothingTimeSecondYogaRight = 1;
 
 %% Constraints for QP for balancing
 

diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m
@@ -74,11 +74,10 @@
 
 Gain.impedances(10,18:23) = Gain.impedances(10,18:23)*1.5; 
 Gain.impedances(10,1:3)   = Gain.impedances(10,1:3)*2;
-
 Gain.impedances(4,12:17)  = Gain.impedances(4,12:17)*1.5; 
 Gain.impedances(4,1:3)    = Gain.impedances(4,1:3)*2;
-
-Gain.impedances(7,2:3)      = Gain.impedances(7,2:3)*1.5;
+Gain.impedances(4,4:11)   = Gain.impedances(4,4:11)*1.5;
+Gain.impedances(7,2:3)    = Gain.impedances(7,2:3)*1.5;
 
 Gain.dampings = 0*2*sqrt(Gain.impedances(1,:))/40;  
 
@@ -102,6 +101,12 @@
                                     5;    %% state == 12  LOOKING FOR CONTACT 
                                     10];  %% state == 13  TRANSITION INIT POSITION
 
+% scale factor smoothing time multiplies the smoothing factor during the
+% Yoga (state 4 and 10). The purpose is to reduce the time necessary for 
+% the reference to converge to the next position, but without changing also
+% the valuse stored in Sm.joints_leftYogaRef/Sm.joints_rightYogaRef (YOGA DEMO ONLY)
+Sm.scaleFactorSmoothingTime = 0.9;
+
 % time between two yoga positions (YOGA DEMO ONLY)
 Sm.joints_pauseBetweenYogaMoves  = 5;
 
@@ -125,7 +130,7 @@
                       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.02,-0.09,  0.0;   %% state ==  6  LOOKING FOR CONTACT 
+                      0.02,-0.08,  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.00,  0.0;   %% state ==  8  COM TRANSITION TO RIGHT FOOT
@@ -146,7 +151,7 @@
 Sm.yogaInLoop               = false;
 
 % repeat the yoga movements faster. Uneffective if Sm.yogaExtended = false;
-Sm.repeatYogaMoveset            = true;
+Sm.repeatYogaMoveset        = false;
 
 % smoothing time for the second time the Yoga moveset are performed
 Sm.smoothingTimeSecondYogaLeft  = 0.6;

diff --git a/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m b/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m
@@ -414,8 +414,16 @@
     elseif secondYoga && state == 10 && t >= (Sm.joints_rightSecondYogaRef(2) + tSwitch)
 
         jointsSmoothingTime = Sm.smoothingTimeSecondYogaRight;
-    else
-        jointsSmoothingTime = Sm.smoothingTimeCoM_Joints(state);
+    else       
+
+        if state == 4 || state == 10
+
+            % during the yoga, reduce the time necessary for the reference
+            % to converge to the next position
+            jointsSmoothingTime = Sm.scaleFactorSmoothingTime*Sm.smoothingTimeCoM_Joints(state);
+        else
+            jointsSmoothingTime = Sm.smoothingTimeCoM_Joints(state);
+        end
     end
 
 end