first test on icubGenova04

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

Hide file tree

Showing 3 changed files with 484 additions and 560 deletions.
diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m
@@ -72,12 +72,9 @@
                     30   30   30, 10   10    10   10, 10   10    10   10,220  550  220   200     65 300,100  350   20   200     10 100  % state == 12  LOOKING FOR CONTACT
                     30   40   30, 10   10    10   10, 10   10    10   10,220  550  220   200     65 300,100  350   20   200     10 100];% state == 13  TRANSITION TO INITIAL POSITION
 
-%Gain.impedances(4,12:17) = Gain.impedances(4,12:17)*2; 
-Gain.impedances(4,1:3)   = Gain.impedances(4,1:3)*3;
-
-Gain.impedances(6,18:23) = Gain.impedances(6,18:23)*2; 
+Gain.impedances(4,1:3)    = Gain.impedances(4,1:3)*3;
+Gain.impedances(6,18:23)  = Gain.impedances(6,18:23)*2; 
 Gain.impedances(10,1:3)   = Gain.impedances(10,1:3)*3;
-
 Gain.impedances(13,1:3)   = Gain.impedances(13,1:3)*3;
 
 Gain.dampings = 0*sqrt(Gain.impedances(1,:));  
@@ -149,8 +146,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.5;
+Sm.smoothingTimeSecondYogaRight = 0.5;
 
 %% Joint references (YOGA DEMO ONLY)
 Sm.joints_references = [  zeros(1,ROBOT_DOF);                                %% THIS REFERENCE IS IGNORED 
@@ -301,6 +298,12 @@
              -0.4181, 1.6800,0.7373, 0.3031, ...
               0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
              -0.3514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614];
+
+q18 =        [-0.0852,-0.4273,0.0821, ...
+              0.1391, 1.4585,0.2464, 0.3042, ...
+             -0.4181, 1.6800,0.7373, 0.3031, ...
+              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
+              0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614];
 
 Sm.joints_leftYogaRef  = [ 0,                              q1;
                            1*Sm.smoothingTimeCoM_Joints(4),q2;
@@ -327,7 +330,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),q18];
 
 Sm.joints_rightYogaRef      = Sm.joints_leftYogaRef;
 Sm.joints_rightYogaRef(:,1) = [0,                              ;
@@ -361,6 +364,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)  = 0.9;
+Sm.joints_rightSecondYogaRef(1) = 0.9;
+
 % 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/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,test] = ...
+function  [w_H_b, CoM_des, qj_des, constraints, impedances, KPCoM, KDCoM, currentState, jointsSmoothingTime] = ...
             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;
@@ -406,6 +406,5 @@
     %% Update parameters
     currentState        = state;
     jointsSmoothingTime = Sm.smoothingTimeCoM_Joints(state);
-    test = (Sm.joints_leftSecondYogaRef(end) + tSwitch);
 
 end