iros 2018 working yoga in loop

torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m

@@ -182,18 +182,18 @@
 Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1);
 
 % configuration parameters for state machine (YOGA DEMO ONLY) 
-Sm.tBalancing               = 1;
-Sm.tBalancingBeforeYoga     = 1;
-Sm.skipYoga                 = false;
-Sm.demoOnlyBalancing        = false;
-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;
+Sm.tBalancing                        = 1;
+Sm.tBalancingBeforeYoga              = 1;
+Sm.skipYoga                          = false;
+Sm.demoOnlyBalancing                 = false;
+Sm.demoStartsOnRightSupport          = false;
+Sm.yogaAlsoOnRightFoot               = false;
+Sm.twoFeetYogaInLoop                 = false;
+Sm.oneFootYogaInLoop                 = false;
+Sm.yogaCounter                       = 5;
+Sm.repeatTwiceYogaWithDifferentSpeed = false;
+Sm.smoothingTimeSecondYogaLeft       = 1;
+Sm.smoothingTimeSecondYogaRight      = 1;
 
 %% Constraints for QP for balancing
 

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

@@ -144,16 +144,36 @@
 Sm.yogaExtended                 = true;
 Sm.skipYoga                     = false;
 Sm.demoOnlyBalancing            = false;
-Sm.demoStartsOnRightSupport     = true;
-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            = false;
+Sm.demoStartsOnRightSupport     = false; % If false, the Yoga demo is performed on the left foot first
+Sm.yogaAlsoOnRightFoot          = false; % TO DO: yoga on both feet starting from right foot (not available for now)
+
+%%%% List of possible "Yoga in loop" %%%%
+
+% the robot will repeat the FULL DEMO (two feet balancing, yoga on left
+% foot, back on two feet, yoga right foot, back on two feet). The demo is
+% repeated until the user stops the Simulink model. This option is ignored
+% if Sm.demoStartsOnRightSupport = true.
+Sm.twoFeetYogaInLoop        = false;
+
+% the robot will repeat the ONE FOOT yoga for the number of times the user
+% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two
+% feet balancing in between to consecutive yoga. WARNING: if the option 
+% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga
+% 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        = false;
+Sm.yogaCounter              = 5;
+
+% the robot will repeat the yoga moveset twice. This option works as the 
+% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However,
+% it is possible to set a different yoga speed for the two yoga. 
+% Uneffective if Sm.yogaExtended = false;
+Sm.repeatTwiceYogaWithDifferentSpeed = false;
 
 % smoothing time for the second time the Yoga moveset are performed
-Sm.smoothingTimeSecondYogaLeft  = 1.2;
-Sm.smoothingTimeSecondYogaRight = 1.2;
+Sm.smoothingTimeSecondYogaLeft       = 1.2;
+Sm.smoothingTimeSecondYogaRight      = 1.2;
 
 %% Joint references (YOGA DEMO ONLY)
 Sm.joints_references = [  zeros(1,ROBOT_DOF);                                %% THIS REFERENCE IS IGNORED 
@@ -369,16 +389,17 @@
 Sm.joints_rightSecondYogaRef(1) = Sm.smoothingTimeCoM_Joints(10);
 
 % 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
+% also, Sm.repeatTwiceYogaWithDifferentSpeed 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);
-    Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10);
+    Sm.repeatTwiceYogaWithDifferentSpeed = 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);
+    Sm.joints_rightYogaRef(8,1)          = 15*Sm.smoothingTimeCoM_Joints(10);
 end
 
 % MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA					 

torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m

@@ -182,18 +182,18 @@
 Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1);
 
 % configuration parameters for state machine (YOGA DEMO ONLY) 
-Sm.tBalancing               = 1;
-Sm.tBalancingBeforeYoga     = 1;
-Sm.skipYoga                 = false;
-Sm.demoOnlyBalancing        = false;
-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;
+Sm.tBalancing                        = 1;
+Sm.tBalancingBeforeYoga              = 1;
+Sm.skipYoga                          = false;
+Sm.demoOnlyBalancing                 = false;
+Sm.demoStartsOnRightSupport          = false;
+Sm.yogaAlsoOnRightFoot               = false;
+Sm.twoFeetYogaInLoop                 = false;
+Sm.oneFootYogaInLoop                 = false;
+Sm.yogaCounter                       = 5;
+Sm.repeatTwiceYogaWithDifferentSpeed = false;
+Sm.smoothingTimeSecondYogaLeft       = 1;
+Sm.smoothingTimeSecondYogaRight      = 1;
 
 %% Constraints for QP for balancing
 

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

@@ -146,16 +146,36 @@
 Sm.yogaExtended             = true;
 Sm.skipYoga                 = false;
 Sm.demoOnlyBalancing        = false;
-Sm.demoStartsOnRightSupport = false;
+Sm.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first
 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        = false;
+%%%% List of possible "Yoga in loop" %%%%
+
+% the robot will repeat the FULL DEMO (two feet balancing, yoga on left
+% foot, back on two feet, yoga right foot, back on two feet). The demo is
+% repeated until the user stops the Simulink model. This option is ignored
+% if Sm.demoStartsOnRightSupport = true.
+Sm.twoFeetYogaInLoop        = false;
+
+% the robot will repeat the ONE FOOT yoga for the number of times the user
+% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two
+% feet balancing in between to consecutive yoga. WARNING: if the option 
+% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga
+% 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        = false;
+Sm.yogaCounter              = 5;
+
+% the robot will repeat the yoga moveset twice. This option works as the 
+% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However,
+% it is possible to set a different yoga speed for the two yoga. 
+% Uneffective if Sm.yogaExtended = false;
+Sm.repeatTwiceYogaWithDifferentSpeed = false;
 
 % smoothing time for the second time the Yoga moveset are performed
-Sm.smoothingTimeSecondYogaLeft  = 0.6;
-Sm.smoothingTimeSecondYogaRight = 0.6;
+Sm.smoothingTimeSecondYogaLeft       = 0.6;
+Sm.smoothingTimeSecondYogaRight      = 0.6;
 
 %% Joint references (YOGA DEMO ONLY)
 Sm.joints_references = [  zeros(1,ROBOT_DOF);                                %% THIS REFERENCE IS IGNORED 
@@ -371,16 +391,17 @@
 Sm.joints_rightSecondYogaRef(1) = Sm.smoothingTimeCoM_Joints(10);
 
 % 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
+% also, Sm.repeatTwiceYogaWithDifferentSpeed 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);
-    Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10);
+    Sm.repeatTwiceYogaWithDifferentSpeed = 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);
+    Sm.joints_rightYogaRef(8,1)          = 15*Sm.smoothingTimeCoM_Joints(10);
 end
 
 % MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA					 

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

@@ -13,7 +13,7 @@
 Config.LEFT_FOOT_IN_CONTACT_AT_0 = true;
 
 % If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY)
-Config.DEMO_MOVEMENTS = false;
+Config.DEMO_MOVEMENTS = true;
 
 % If equal to true, the desired streamed values of the center of mass 
 % are smoothed internally 
@@ -182,18 +182,18 @@
 Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1);
 
 % configuration parameters for state machine (YOGA DEMO ONLY) 
-Sm.tBalancing               = 1;
-Sm.tBalancingBeforeYoga     = 1;
-Sm.skipYoga                 = false;
-Sm.demoOnlyBalancing        = false;
-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;
+Sm.tBalancing                        = 1;
+Sm.tBalancingBeforeYoga              = 1;
+Sm.skipYoga                          = false;
+Sm.demoOnlyBalancing                 = false;
+Sm.demoStartsOnRightSupport          = false;
+Sm.yogaAlsoOnRightFoot               = false;
+Sm.twoFeetYogaInLoop                 = false;
+Sm.oneFootYogaInLoop                 = false;
+Sm.yogaCounter                       = 5;
+Sm.repeatTwiceYogaWithDifferentSpeed = false;
+Sm.smoothingTimeSecondYogaLeft       = 1;
+Sm.smoothingTimeSecondYogaRight      = 1;
 
 %% Constraints for QP for balancing
 

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

@@ -144,16 +144,36 @@
 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        = false;
+Sm.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first
+Sm.yogaAlsoOnRightFoot      = false; % TO DO: yoga on both feet starting from right foot (not available for now)
+
+%%%% List of possible "Yoga in loop" %%%%
+
+% the robot will repeat the FULL DEMO (two feet balancing, yoga on left
+% foot, back on two feet, yoga right foot, back on two feet). The demo is
+% repeated until the user stops the Simulink model. This option is ignored
+% if Sm.demoStartsOnRightSupport = true.
+Sm.twoFeetYogaInLoop        = false;
+
+% the robot will repeat the ONE FOOT yoga for the number of times the user
+% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two
+% feet balancing in between to consecutive yoga. WARNING: if the option 
+% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga
+% 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.yogaCounter              = 10;
+
+% the robot will repeat the yoga moveset twice. This option works as the 
+% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However,
+% it is possible to set a different yoga speed for the two yoga. 
+% Uneffective if Sm.yogaExtended = false;
+Sm.repeatTwiceYogaWithDifferentSpeed = false;
 
 % smoothing time for the second time the Yoga moveset are performed
-Sm.smoothingTimeSecondYogaLeft  = 0.6;
-Sm.smoothingTimeSecondYogaRight = 0.6;
+Sm.smoothingTimeSecondYogaLeft       = 0.7;
+Sm.smoothingTimeSecondYogaRight      = 0.7;
 
 %% Joint references (YOGA DEMO ONLY)
 Sm.joints_references = [  zeros(1,ROBOT_DOF);                                %% THIS REFERENCE IS IGNORED 
@@ -375,16 +395,17 @@
 Sm.joints_rightSecondYogaRef(1) = Sm.smoothingTimeCoM_Joints(10);
 
 % 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
+% also, Sm.repeatTwiceYogaWithDifferentSpeed 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);
-    Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10);
+    Sm.repeatTwiceYogaWithDifferentSpeed = 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);
+    Sm.joints_rightYogaRef(8,1)          = 15*Sm.smoothingTimeCoM_Joints(10);
 end
 
 % MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA					 

torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m

@@ -182,18 +182,18 @@
 Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1);
 
 % configuration parameters for state machine (YOGA DEMO ONLY) 
-Sm.tBalancing               = 1;
-Sm.tBalancingBeforeYoga     = 1;
-Sm.skipYoga                 = false;
-Sm.demoOnlyBalancing        = false;
-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;
+Sm.tBalancing                        = 1;
+Sm.tBalancingBeforeYoga              = 1;
+Sm.skipYoga                          = false;
+Sm.demoOnlyBalancing                 = false;
+Sm.demoStartsOnRightSupport          = false;
+Sm.yogaAlsoOnRightFoot               = false;
+Sm.twoFeetYogaInLoop                 = false;
+Sm.oneFootYogaInLoop                 = false;
+Sm.yogaCounter                       = 5;
+Sm.repeatTwiceYogaWithDifferentSpeed = false;
+Sm.smoothingTimeSecondYogaLeft       = 1;
+Sm.smoothingTimeSecondYogaRight      = 1;
 
 %% Constraints for QP for balancing
 

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

@@ -138,17 +138,36 @@
 Sm.tBalancingBeforeYoga     = 1;
 Sm.skipYoga                 = false;
 Sm.demoOnlyBalancing        = false;
-Sm.demoStartsOnRightSupport = false;
-Sm.yogaAlsoOnRightFoot      = false;
-Sm.yogaInLoop               = false;
-
-% leave this variable to false, it has been tuned with the Yoga Extended
-% (not available for icubGazeboSim)
-Sm.repeatYogaMoveset        = false;
-
-% smoothing time for the second time the Yoga moveset are performed (NOT USED)
-Sm.smoothingTimeSecondYogaLeft  = 0.6;
-Sm.smoothingTimeSecondYogaRight = 0.6;
+Sm.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first
+Sm.yogaAlsoOnRightFoot      = false; % TO DO: yoga on both feet starting from right foot (not available for now)
+
+%%%% List of possible "Yoga in loop" %%%%
+
+% the robot will repeat the FULL DEMO (two feet balancing, yoga on left
+% foot, back on two feet, yoga right foot, back on two feet). The demo is
+% repeated until the user stops the Simulink model. This option is ignored
+% if Sm.demoStartsOnRightSupport = true.
+Sm.twoFeetYogaInLoop        = false;
+
+% the robot will repeat the ONE FOOT yoga for the number of times the user
+% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two
+% feet balancing in between to consecutive yoga. WARNING: if the option 
+% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga
+% 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        = false;
+Sm.yogaCounter              = 5;
+
+% the robot will repeat the yoga moveset twice. This option works as the 
+% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However,
+% it is possible to set a different yoga speed for the two yoga. 
+% (NOT AVAILABLE FOR ICUBGAZEBOSIM)
+Sm.repeatTwiceYogaWithDifferentSpeed = false;
+
+% smoothing time for the second time the Yoga moveset are performed
+Sm.smoothingTimeSecondYogaLeft       = 1.2;
+Sm.smoothingTimeSecondYogaRight      = 1.2;
 
 %% Joint references (YOGA DEMO ONLY)
 Sm.joints_references = [zeros(1,ROBOT_DOF);                                %% NOT USED