Merge pull request #46 from robotology/iros2018_yogaInLoop

New Yoga options and fixed the torso coupling matrix
robotology · Oct 9, 2018 · 7ab9d86 · 7ab9d86
2 parents b3d8fa7 + 94e1a9f
commit 7ab9d86
Show file tree

Hide file tree

Showing 14 changed files with 274 additions and 166 deletions.
diff --git a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m b/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m
@@ -126,9 +126,9 @@
                 1  t  0;
                 0 -t  t];
 
-T_torso = [0   -0.5     0.5;
-           0    0.5     0.5;
-           r/R  r/(2*R) r/(2*R)];
+T_torso = [ 0.5    -0.5     0;
+            0.5     0.5     0;
+            r/(2*R) r/(2*R) r/R];
 
 if Config.INCLUDE_COUPLING
 

diff --git a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m b/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m
@@ -157,9 +157,9 @@
                 1  t  0;
                 0 -t  t];
 
-T_torso = [0   -0.5     0.5;
-           0    0.5     0.5;
-           r/R  r/(2*R) r/(2*R)];
+T_torso = [ 0.5    -0.5     0;
+            0.5     0.5     0;
+            r/(2*R) r/(2*R) r/R];
 
 if Config.INCLUDE_COUPLING
 

diff --git a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m b/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m
@@ -126,9 +126,9 @@
                 1  t  0;
                 0 -t  t];
 
-T_torso = [0   -0.5     0.5;
-           0    0.5     0.5;
-           r/R  r/(2*R) r/(2*R)];
+T_torso = [ 0.5    -0.5     0;
+            0.5     0.5     0;
+            r/(2*R) r/(2*R) r/R];
 
 if Config.INCLUDE_COUPLING
 

diff --git a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m b/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m
@@ -157,9 +157,9 @@
                 1  t  0;
                 0 -t  t];
 
-T_torso = [0   -0.5     0.5;
-           0    0.5     0.5;
-           r/R  r/(2*R) r/(2*R)];
+T_torso = [ 0.5    -0.5     0;
+            0.5     0.5     0;
+            r/(2*R) r/(2*R) r/R];
 
 if Config.INCLUDE_COUPLING
 

diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m
@@ -115,10 +115,10 @@
                 1  t  0;
                 0 -t  t];
 
-T_torso = [0   -0.5     0.5;
-           0    0.5     0.5;
-           r/R  r/(2*R) r/(2*R)];
-
+T_torso = [ 0.5    -0.5     0;
+            0.5     0.5     0;
+            r/(2*R) r/(2*R) r/R];
+        
 if Config.INCLUDE_COUPLING
 
     Config.T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12));
@@ -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
 

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
@@ -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					 
@@ -439,9 +460,9 @@
                 1  t  0;
                 0 -t  t];
 
-T_torso = [0   -0.5     0.5;
-           0    0.5     0.5;
-           r/R  r/(2*R) r/(2*R)];
+T_torso = [ 0.5    -0.5     0;
+            0.5     0.5     0;
+            r/(2*R) r/(2*R) r/R];
 
 if Config.INCLUDE_COUPLING
 

diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m
@@ -115,9 +115,9 @@
                 1  t  0;
                 0 -t  t];
 
-T_torso = [0   -0.5     0.5;
-           0    0.5     0.5;
-           r/R  r/(2*R) r/(2*R)];
+T_torso = [ 0.5    -0.5     0;
+            0.5     0.5     0;
+            r/(2*R) r/(2*R) r/R];
 
 if Config.INCLUDE_COUPLING
 
@@ -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
 

diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m b/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					 
@@ -441,9 +462,9 @@
                 1  t  0;
                 0 -t  t];
 
-T_torso = [0   -0.5     0.5;
-           0    0.5     0.5;
-           r/R  r/(2*R) r/(2*R)];
+T_torso = [ 0.5    -0.5     0;
+            0.5     0.5     0;
+            r/(2*R) r/(2*R) r/R];
 
 if Config.INCLUDE_COUPLING
 

diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m b/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 
@@ -115,9 +115,9 @@
                 1  t  0;
                 0 -t  t];
 
-T_torso = [0   -0.5     0.5;
-           0    0.5     0.5;
-           r/R  r/(2*R) r/(2*R)];
+T_torso = [ 0.5    -0.5     0;
+            0.5     0.5     0;
+            r/(2*R) r/(2*R) r/R];
 
 if Config.INCLUDE_COUPLING
 
@@ -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