first repeated yoga on iCubGazeboV2_5

dbdxnuliba · Sep 20, 2018 · f72460f · f72460f
1 parent ac77b99
commit f72460f
Show file tree

Hide file tree

Showing 3 changed files with 1,336 additions and 551 deletions.
diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initStateMachineYoga.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initStateMachineYoga.m
@@ -133,14 +133,21 @@
                       0.0,  0.00,  0.0];  %% NOT USED
 
 % configuration parameters for state machine (YOGA DEMO ONLY) 
-Sm.tBalancing               = 1;
-Sm.tBalancingBeforeYoga     = 1;
-Sm.yogaExtended             = true;
-Sm.skipYoga                 = false;
-Sm.demoOnlyBalancing        = false;
-Sm.demoStartsOnRightSupport = false;
-Sm.yogaAlsoOnRightFoot      = true; % TO DO: yoga on both feet starting from right foot
-Sm.yogaInLoop               = false;
+Sm.tBalancing                   = 1;
+Sm.tBalancingBeforeYoga         = 1;
+Sm.yogaExtended                 = true;
+Sm.skipYoga                     = false;
+Sm.demoOnlyBalancing            = false;
+Sm.demoStartsOnRightSupport     = false;
+Sm.yogaAlsoOnRightFoot          = false; % TO DO: yoga on both feet starting from right foot
+Sm.yogaInLoop                   = false;
+
+% repeat the yoga movements faster. Uneffective if Sm.yogaExtended = false;
+Sm.repeatYogaMoveset            = true;
+
+% smoothing time for the second time the Yoga moveset are performed
+Sm.smoothingTimeSecondYogaLeft  = 0.6;
+Sm.smoothingTimeSecondYogaRight = 0.6;
 
 %% Joint references (YOGA DEMO ONLY)
 Sm.joints_references = [  zeros(1,ROBOT_DOF);                                %% THIS REFERENCE IS IGNORED 
@@ -194,13 +201,13 @@
              -1.1621,0.6663, 0.4919, 0.9947, ... 
              -1.0717,1.2904,-0.2447, 1.0948, ...
               0.2092,0.2060, 0.0006,-0.1741,-0.1044, 0.0700, ...
-              0.3484,0.4008,-0.0004,-0.3672,-0.0530,-0.0875];
+              0.3484,0.4008,-0.0004,-0.3672,-0.1060,-0.0875];
 
 q2 =        [-0.0790,0.2279, 0.4519, ...
              -1.1621,0.6663, 0.4965, 0.9947, ...
              -1.0717,1.2904,-0.2493, 1.0948, ...
               0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... 
-              0.3714,0.9599, 1.3253,-1.6594, 0.6374,-0.0614];
+              0.3714,0.9599, 1.3253,-1.6594,-0.1060,-0.0614];
 
 q3 =        [-0.0852,-0.4273,0.0821,...
               0.1391, 1.4585,0.2464, 0.3042, ...
@@ -317,7 +324,7 @@
                           22*Sm.smoothingTimeCoM_Joints(4),q15;
                           23*Sm.smoothingTimeCoM_Joints(4),q16;
                           24*Sm.smoothingTimeCoM_Joints(4),q17;
-                          25*Sm.smoothingTimeCoM_Joints(4),q8];
+                          25*Sm.smoothingTimeCoM_Joints(4),q2];
 
 Sm.joints_rightYogaRef      = Sm.joints_leftYogaRef;
 Sm.joints_rightYogaRef(:,1) = [0,                              ;
@@ -347,9 +354,17 @@
                               24*Sm.smoothingTimeCoM_Joints(10);
                               25*Sm.smoothingTimeCoM_Joints(10)];
 
+% smoothing time vector for the second time the Yoga moveset are performed
+Sm.joints_leftSecondYogaRef  = Sm.smoothingTimeSecondYogaLeft.*(0:(length(Sm.joints_rightYogaRef(:,1))-1));
+Sm.joints_rightSecondYogaRef = Sm.smoothingTimeSecondYogaRight.*(0:(length(Sm.joints_rightYogaRef(:,1))-1));
+
 % if the demo is not "yogaExtended", stop at the 8th move
+% also, Sm.repeatYogaMoveset must be set to "false". The reason is that the
+% first and the last Yoga moveset for the "not extended" one are very
+% different, and the robot may "jump" when restarting the Yoga the second time
 if ~Sm.yogaExtended
 
+    Sm.repeatYogaMoveset        = false;
     Sm.joints_leftYogaRef       = Sm.joints_leftYogaRef(1:8,:);
     Sm.joints_rightYogaRef      = Sm.joints_rightYogaRef(1:8,:);
     Sm.joints_leftYogaRef(8,1)  = 15*Sm.smoothingTimeCoM_Joints(4);

diff --git a/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m b/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m
@@ -4,11 +4,13 @@
     persistent state;
     persistent tSwitch;
     persistent w_H_fixedLink;
+    persistent secondYoga;
 
-    if isempty(state) || isempty(tSwitch) || isempty(w_H_fixedLink) 
+    if isempty(state) || isempty(tSwitch) || isempty(w_H_fixedLink) || isempty(secondYoga) 
         state         = Sm.stateAt0;
         tSwitch       = 0;
         w_H_fixedLink = eye(4);
+        secondYoga    = false;
     end
 
     CoM_des      = CoM_0;
@@ -110,7 +112,11 @@
 
         for i = 1: size(Sm.joints_leftYogaRef,1)-1
 
-            if t > (Sm.joints_leftYogaRef(i,1) + tSwitch) && t <= (Sm.joints_leftYogaRef(i+1,1)+ tSwitch)
+            if t > (Sm.joints_leftYogaRef(i,1) + tSwitch) && t <= (Sm.joints_leftYogaRef(i+1,1)+ tSwitch) && secondYoga == false
+
+                qj_des = Sm.joints_leftYogaRef(i,2:end)';
+
+            elseif t > (Sm.joints_leftSecondYogaRef(i) + tSwitch) && t <= (Sm.joints_leftSecondYogaRef(i+1)+ tSwitch) && secondYoga == true
 
                 qj_des = Sm.joints_leftYogaRef(i,2:end)';
             end
@@ -119,9 +125,22 @@
 
             qj_des = Sm.joints_leftYogaRef(end,2:end)';
 
-            if t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves)
-                state   = 5;
-                tSwitch = t;
+            if t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatYogaMoveset == true && secondYoga == false
+
+                tSwitch    = t;
+                secondYoga = true;
+
+            elseif  t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatYogaMoveset == false
+
+                state      = 5;
+                tSwitch    = t;
+                secondYoga = false;
+
+            elseif  t > (Sm.joints_leftSecondYogaRef(end) + tSwitch + Sm.smoothingTimeSecondYogaLeft + Sm.joints_pauseBetweenYogaMoves) && secondYoga == true
+
+                state      = 5;
+                tSwitch    = t;
+                secondYoga = false;
             end
         end
     end