updated smoothing times

dbdxnuliba · Sep 21, 2018 · 820269a · 820269a
1 parent 189189b
commit 820269a
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 12 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
@@ -146,8 +146,8 @@
 Sm.repeatYogaMoveset            = true;
 
 % smoothing time for the second time the Yoga moveset are performed
-Sm.smoothingTimeSecondYogaLeft  = 0.6;
-Sm.smoothingTimeSecondYogaRight = 0.6;
+Sm.smoothingTimeSecondYogaLeft  = 1.2;
+Sm.smoothingTimeSecondYogaRight = 1.2;
 
 %% Joint references (YOGA DEMO ONLY)
 Sm.joints_references = [  zeros(1,ROBOT_DOF);                                %% THIS REFERENCE IS IGNORED 
@@ -358,6 +358,10 @@
 Sm.joints_leftSecondYogaRef  = Sm.smoothingTimeSecondYogaLeft.*(0:(length(Sm.joints_rightYogaRef(:,1))-1));
 Sm.joints_rightSecondYogaRef = Sm.smoothingTimeSecondYogaRight.*(0:(length(Sm.joints_rightYogaRef(:,1))-1));
 
+% keep a high smoothing time for switching from the first to the second yoga moveset
+Sm.joints_leftSecondYogaRef(1)  = Sm.smoothingTimeCoM_Joints(4);
+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

diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m
@@ -149,8 +149,8 @@
 Sm.repeatYogaMoveset            = true;
 
 % smoothing time for the second time the Yoga moveset are performed
-Sm.smoothingTimeSecondYogaLeft  = 0.8;
-Sm.smoothingTimeSecondYogaRight = 0.8;
+Sm.smoothingTimeSecondYogaLeft  = 0.6;
+Sm.smoothingTimeSecondYogaRight = 0.6;
 
 %% Joint references (YOGA DEMO ONLY)
 Sm.joints_references = [  zeros(1,ROBOT_DOF);                                %% THIS REFERENCE IS IGNORED 
@@ -361,6 +361,10 @@
 Sm.joints_leftSecondYogaRef  = Sm.smoothingTimeSecondYogaLeft.*(0:(length(Sm.joints_rightYogaRef(:,1))-1));
 Sm.joints_rightSecondYogaRef = Sm.smoothingTimeSecondYogaRight.*(0:(length(Sm.joints_rightYogaRef(:,1))-1));
 
+% keep a high smoothing time for switching from the first to the second yoga moveset
+Sm.joints_leftSecondYogaRef(1)  = Sm.smoothingTimeCoM_Joints(4);
+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

diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m
@@ -128,9 +128,9 @@
                       0.0,  0.012,  0.0;   %% state ==  8  COM TRANSITION TO RIGHT FOOT
                       0.0, -0.015,  0.0;   %% state ==  9  RIGHT FOOT BALANCING 
                       0.0, -0.017,  0.0;   %% state == 10  YOGA RIGHT FOOT
-                      0.0,  0.00,  0.0;   %% state == 11  PREPARING FOR SWITCHING
-                      0.02,  0.025,  0.0;   %% state == 12  LOOKING FOR CONTACT 
-                      0.0,  0.00,  0.0];  %% NOT USED
+                      0.0,  0.00,   0.0;   %% state == 11  PREPARING FOR SWITCHING
+                      0.02, 0.025,  0.0;   %% state == 12  LOOKING FOR CONTACT 
+                      0.0,  0.00,   0.0];  %% NOT USED
 
 % configuration parameters for state machine (YOGA DEMO ONLY) 
 Sm.tBalancing               = 1;
@@ -146,8 +146,8 @@
 Sm.repeatYogaMoveset            = true;
 
 % smoothing time for the second time the Yoga moveset are performed
-Sm.smoothingTimeSecondYogaLeft  = 0.5;
-Sm.smoothingTimeSecondYogaRight = 0.5;
+Sm.smoothingTimeSecondYogaLeft  = 0.6;
+Sm.smoothingTimeSecondYogaRight = 0.6;
 
 %% Joint references (YOGA DEMO ONLY)
 Sm.joints_references = [  zeros(1,ROBOT_DOF);                                %% THIS REFERENCE IS IGNORED 
@@ -365,8 +365,8 @@
 Sm.joints_rightSecondYogaRef = Sm.smoothingTimeSecondYogaRight.*(0:(length(Sm.joints_rightYogaRef(:,1))-1));
 
 % keep a high smoothing time for switching from the first to the second yoga moveset
-Sm.joints_leftSecondYogaRef(1)  = 0.9;
-Sm.joints_rightSecondYogaRef(1) = 0.9;
+Sm.joints_leftSecondYogaRef(1)  = Sm.smoothingTimeCoM_Joints(4);
+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

diff --git a/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m b/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m
@@ -405,6 +405,17 @@
 
     %% Update parameters
     currentState        = state;
-    jointsSmoothingTime = Sm.smoothingTimeCoM_Joints(state);
+
+    % Update CoM and joints smoothing time for the repeated yoga demo
+    if secondYoga && state == 4 && t >= (Sm.joints_leftSecondYogaRef(2) + tSwitch)
+
+        jointsSmoothingTime = Sm.smoothingTimeSecondYogaLeft;
+
+    elseif secondYoga && state == 10 && t >= (Sm.joints_rightSecondYogaRef(2) + tSwitch)
+
+        jointsSmoothingTime = Sm.smoothingTimeSecondYogaRight;
+    else
+        jointsSmoothingTime = Sm.smoothingTimeCoM_Joints(state);
+    end
 
 end