updated repeated yoga in simulation

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

Hide file tree

Showing 3 changed files with 567 additions and 466 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
@@ -324,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),q2];
+                          25*Sm.smoothingTimeCoM_Joints(4),q8];
 
 Sm.joints_rightYogaRef      = Sm.joints_leftYogaRef;
 Sm.joints_rightYogaRef(:,1) = [0,                              ;

diff --git a/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m b/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m
@@ -1,4 +1,4 @@
-function  [w_H_b, CoM_des, qj_des, constraints, impedances, KPCoM, KDCoM, currentState, jointsSmoothingTime] = ...
+function  [w_H_b, CoM_des, qj_des, constraints, impedances, KPCoM, KDCoM, currentState, jointsSmoothingTime,test] = ...
             stateMachineYoga(CoM_0, qj_0, l_sole_CoM, r_sole_CoM, qj, t, wrench_rightFoot, wrench_leftFoot, l_sole_H_b, r_sole_H_b, Sm, Gain)
 
     persistent state;
@@ -110,37 +110,46 @@
         KPCoM        = Gain.KP_COM(state,:);   
         KDCoM        = Gain.KD_COM(state,:);   
 
+        % iterate over the yoga positions
         for i = 1: size(Sm.joints_leftYogaRef,1)-1
 
+            % positions for the first yoga
             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)';
 
+            % positions for the second yoga
             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
         end
-        if t > (Sm.joints_leftYogaRef(end,1) + tSwitch) 
+        if t > (Sm.joints_leftYogaRef(end,1) + tSwitch) && secondYoga == false
 
             qj_des = Sm.joints_leftYogaRef(end,2:end)';
 
-            if t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatYogaMoveset == true && secondYoga == false
+            % if Sm.repeatYogaMoveset == true, repeat the Yoga
+            if t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatYogaMoveset == true
 
                 tSwitch    = t;
                 secondYoga = true;
 
-            elseif  t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatYogaMoveset == false
+            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
+            end
+        end        
+        if t > (Sm.joints_leftSecondYogaRef(end) + tSwitch) && secondYoga == true
+
+            % exit loop condition for the second yoga
+            if  t > (Sm.joints_leftSecondYogaRef(end) + tSwitch + Sm.smoothingTimeSecondYogaLeft + Sm.joints_pauseBetweenYogaMoves)
 
                 state      = 5;
                 tSwitch    = t;
-                secondYoga = false;
+                secondYoga = false;          
             end
         end
     end
@@ -368,5 +377,6 @@
     %% Update parameters
     currentState        = state;
     jointsSmoothingTime = Sm.smoothingTimeCoM_Joints(state);
+    test = (Sm.joints_leftSecondYogaRef(end) + tSwitch + Sm.smoothingTimeSecondYogaLeft + Sm.joints_pauseBetweenYogaMoves);
 
 end