Skip to content

Commit 7ab9d86

Browse files
gabrielenavagabrielenava
authored
Merge pull request #46 from robotology/iros2018_yogaInLoop
New Yoga options and fixed the torso coupling matrix
2 parents b3d8fa7 + 94e1a9f commit 7ab9d86

File tree

14 files changed

+274
-166
lines changed

14 files changed

+274
-166
lines changed

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -126,9 +126,9 @@
126126
1 t 0;
127127
0 -t t];
128128

129-
T_torso = [0 -0.5 0.5;
130-
0 0.5 0.5;
131-
r/R r/(2*R) r/(2*R)];
129+
T_torso = [ 0.5 -0.5 0;
130+
0.5 0.5 0;
131+
r/(2*R) r/(2*R) r/R];
132132

133133
if Config.INCLUDE_COUPLING
134134

‎torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -157,9 +157,9 @@
157157
1 t 0;
158158
0 -t t];
159159

160-
T_torso = [0 -0.5 0.5;
161-
0 0.5 0.5;
162-
r/R r/(2*R) r/(2*R)];
160+
T_torso = [ 0.5 -0.5 0;
161+
0.5 0.5 0;
162+
r/(2*R) r/(2*R) r/R];
163163

164164
if Config.INCLUDE_COUPLING
165165

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -126,9 +126,9 @@
126126
1 t 0;
127127
0 -t t];
128128

129-
T_torso = [0 -0.5 0.5;
130-
0 0.5 0.5;
131-
r/R r/(2*R) r/(2*R)];
129+
T_torso = [ 0.5 -0.5 0;
130+
0.5 0.5 0;
131+
r/(2*R) r/(2*R) r/R];
132132

133133
if Config.INCLUDE_COUPLING
134134

‎torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -157,9 +157,9 @@
157157
1 t 0;
158158
0 -t t];
159159

160-
T_torso = [0 -0.5 0.5;
161-
0 0.5 0.5;
162-
r/R r/(2*R) r/(2*R)];
160+
T_torso = [ 0.5 -0.5 0;
161+
0.5 0.5 0;
162+
r/(2*R) r/(2*R) r/R];
163163

164164
if Config.INCLUDE_COUPLING
165165

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

Lines changed: 16 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -115,10 +115,10 @@
115115
1 t 0;
116116
0 -t t];
117117

118-
T_torso = [0 -0.5 0.5;
119-
0 0.5 0.5;
120-
r/R r/(2*R) r/(2*R)];
121-
118+
T_torso = [ 0.5 -0.5 0;
119+
0.5 0.5 0;
120+
r/(2*R) r/(2*R) r/R];
121+
122122
if Config.INCLUDE_COUPLING
123123

124124
Config.T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12));
@@ -182,18 +182,18 @@
182182
Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1);
183183

184184
% configuration parameters for state machine (YOGA DEMO ONLY)
185-
Sm.tBalancing = 1;
186-
Sm.tBalancingBeforeYoga = 1;
187-
Sm.skipYoga = false;
188-
Sm.demoOnlyBalancing = false;
189-
Sm.demoStartsOnRightSupport = false;
190-
Sm.yogaAlsoOnRightFoot = false;
191-
Sm.yogaInLoop = false;
192-
Sm.repeatYogaMoveset = false;
193-
194-
% smoothing time for the second time the Yoga moveset are performed (YOGA DEMO ONLY)
195-
Sm.smoothingTimeSecondYogaLeft = 1;
196-
Sm.smoothingTimeSecondYogaRight = 1;
185+
Sm.tBalancing = 1;
186+
Sm.tBalancingBeforeYoga = 1;
187+
Sm.skipYoga = false;
188+
Sm.demoOnlyBalancing = false;
189+
Sm.demoStartsOnRightSupport = false;
190+
Sm.yogaAlsoOnRightFoot = false;
191+
Sm.twoFeetYogaInLoop = false;
192+
Sm.oneFootYogaInLoop = false;
193+
Sm.yogaCounter = 5;
194+
Sm.repeatTwiceYogaWithDifferentSpeed = false;
195+
Sm.smoothingTimeSecondYogaLeft = 1;
196+
Sm.smoothingTimeSecondYogaRight = 1;
197197

198198
%% Constraints for QP for balancing
199199

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

Lines changed: 40 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -144,16 +144,36 @@
144144
Sm.yogaExtended = true;
145145
Sm.skipYoga = false;
146146
Sm.demoOnlyBalancing = false;
147-
Sm.demoStartsOnRightSupport = true;
148-
Sm.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot
149-
Sm.yogaInLoop = false;
150-
151-
% repeat the yoga movements faster. Uneffective if Sm.yogaExtended = false;
152-
Sm.repeatYogaMoveset = false;
147+
Sm.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first
148+
Sm.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot (not available for now)
149+
150+
%%%% List of possible "Yoga in loop" %%%%
151+
152+
% the robot will repeat the FULL DEMO (two feet balancing, yoga on left
153+
% foot, back on two feet, yoga right foot, back on two feet). The demo is
154+
% repeated until the user stops the Simulink model. This option is ignored
155+
% if Sm.demoStartsOnRightSupport = true.
156+
Sm.twoFeetYogaInLoop = false;
157+
158+
% the robot will repeat the ONE FOOT yoga for the number of times the user
159+
% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two
160+
% feet balancing in between to consecutive yoga. WARNING: if the option
161+
% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga
162+
% on left foot for the number of times the user specifies in the Sm.yogaCounter,
163+
% and then it will repeat the yoga on the right foot for the same number of times.
164+
% This option is ignored if Sm.repeatTwiceYogaWithDifferentSpeed = true.
165+
Sm.oneFootYogaInLoop = false;
166+
Sm.yogaCounter = 5;
167+
168+
% the robot will repeat the yoga moveset twice. This option works as the
169+
% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However,
170+
% it is possible to set a different yoga speed for the two yoga.
171+
% Uneffective if Sm.yogaExtended = false;
172+
Sm.repeatTwiceYogaWithDifferentSpeed = false;
153173

154174
% smoothing time for the second time the Yoga moveset are performed
155-
Sm.smoothingTimeSecondYogaLeft = 1.2;
156-
Sm.smoothingTimeSecondYogaRight = 1.2;
175+
Sm.smoothingTimeSecondYogaLeft = 1.2;
176+
Sm.smoothingTimeSecondYogaRight = 1.2;
157177

158178
%% Joint references (YOGA DEMO ONLY)
159179
Sm.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED
@@ -369,16 +389,17 @@
369389
Sm.joints_rightSecondYogaRef(1) = Sm.smoothingTimeCoM_Joints(10);
370390

371391
% if the demo is not "yogaExtended", stop at the 8th move
372-
% also, Sm.repeatYogaMoveset must be set to "false". The reason is that the
373-
% first and the last Yoga moveset for the "not extended" one are very
374-
% different, and the robot may "jump" when restarting the Yoga the second time
392+
% also, Sm.repeatTwiceYogaWithDifferentSpeed must be set to "false". The
393+
% reason is that the first and the last Yoga moveset for the "not extended"
394+
% one are very different, and the robot may "jump" when restarting the Yoga
395+
% the second time
375396
if ~Sm.yogaExtended
376397

377-
Sm.repeatYogaMoveset = false;
378-
Sm.joints_leftYogaRef = Sm.joints_leftYogaRef(1:8,:);
379-
Sm.joints_rightYogaRef = Sm.joints_rightYogaRef(1:8,:);
380-
Sm.joints_leftYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(4);
381-
Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10);
398+
Sm.repeatTwiceYogaWithDifferentSpeed = false;
399+
Sm.joints_leftYogaRef = Sm.joints_leftYogaRef(1:8,:);
400+
Sm.joints_rightYogaRef = Sm.joints_rightYogaRef(1:8,:);
401+
Sm.joints_leftYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(4);
402+
Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10);
382403
end
383404

384405
% MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA
@@ -439,9 +460,9 @@
439460
1 t 0;
440461
0 -t t];
441462

442-
T_torso = [0 -0.5 0.5;
443-
0 0.5 0.5;
444-
r/R r/(2*R) r/(2*R)];
463+
T_torso = [ 0.5 -0.5 0;
464+
0.5 0.5 0;
465+
r/(2*R) r/(2*R) r/R];
445466

446467
if Config.INCLUDE_COUPLING
447468

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

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -115,9 +115,9 @@
115115
1 t 0;
116116
0 -t t];
117117

118-
T_torso = [0 -0.5 0.5;
119-
0 0.5 0.5;
120-
r/R r/(2*R) r/(2*R)];
118+
T_torso = [ 0.5 -0.5 0;
119+
0.5 0.5 0;
120+
r/(2*R) r/(2*R) r/R];
121121

122122
if Config.INCLUDE_COUPLING
123123

@@ -182,18 +182,18 @@
182182
Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1);
183183

184184
% configuration parameters for state machine (YOGA DEMO ONLY)
185-
Sm.tBalancing = 1;
186-
Sm.tBalancingBeforeYoga = 1;
187-
Sm.skipYoga = false;
188-
Sm.demoOnlyBalancing = false;
189-
Sm.demoStartsOnRightSupport = false;
190-
Sm.yogaAlsoOnRightFoot = false;
191-
Sm.yogaInLoop = false;
192-
Sm.repeatYogaMoveset = false;
193-
194-
% smoothing time for the second time the Yoga moveset are performed (YOGA DEMO ONLY)
195-
Sm.smoothingTimeSecondYogaLeft = 1;
196-
Sm.smoothingTimeSecondYogaRight = 1;
185+
Sm.tBalancing = 1;
186+
Sm.tBalancingBeforeYoga = 1;
187+
Sm.skipYoga = false;
188+
Sm.demoOnlyBalancing = false;
189+
Sm.demoStartsOnRightSupport = false;
190+
Sm.yogaAlsoOnRightFoot = false;
191+
Sm.twoFeetYogaInLoop = false;
192+
Sm.oneFootYogaInLoop = false;
193+
Sm.yogaCounter = 5;
194+
Sm.repeatTwiceYogaWithDifferentSpeed = false;
195+
Sm.smoothingTimeSecondYogaLeft = 1;
196+
Sm.smoothingTimeSecondYogaRight = 1;
197197

198198
%% Constraints for QP for balancing
199199

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

Lines changed: 38 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -146,16 +146,36 @@
146146
Sm.yogaExtended = true;
147147
Sm.skipYoga = false;
148148
Sm.demoOnlyBalancing = false;
149-
Sm.demoStartsOnRightSupport = false;
149+
Sm.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first
150150
Sm.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot
151-
Sm.yogaInLoop = false;
152151

153-
% repeat the yoga movements faster. Uneffective if Sm.yogaExtended = false;
154-
Sm.repeatYogaMoveset = false;
152+
%%%% List of possible "Yoga in loop" %%%%
153+
154+
% the robot will repeat the FULL DEMO (two feet balancing, yoga on left
155+
% foot, back on two feet, yoga right foot, back on two feet). The demo is
156+
% repeated until the user stops the Simulink model. This option is ignored
157+
% if Sm.demoStartsOnRightSupport = true.
158+
Sm.twoFeetYogaInLoop = false;
159+
160+
% the robot will repeat the ONE FOOT yoga for the number of times the user
161+
% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two
162+
% feet balancing in between to consecutive yoga. WARNING: if the option
163+
% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga
164+
% on left foot for the number of times the user specifies in the Sm.yogaCounter,
165+
% and then it will repeat the yoga on the right foot for the same number of times.
166+
% This option is ignored if Sm.repeatTwiceYogaWithDifferentSpeed = true.
167+
Sm.oneFootYogaInLoop = false;
168+
Sm.yogaCounter = 5;
169+
170+
% the robot will repeat the yoga moveset twice. This option works as the
171+
% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However,
172+
% it is possible to set a different yoga speed for the two yoga.
173+
% Uneffective if Sm.yogaExtended = false;
174+
Sm.repeatTwiceYogaWithDifferentSpeed = false;
155175

156176
% smoothing time for the second time the Yoga moveset are performed
157-
Sm.smoothingTimeSecondYogaLeft = 0.6;
158-
Sm.smoothingTimeSecondYogaRight = 0.6;
177+
Sm.smoothingTimeSecondYogaLeft = 0.6;
178+
Sm.smoothingTimeSecondYogaRight = 0.6;
159179

160180
%% Joint references (YOGA DEMO ONLY)
161181
Sm.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED
@@ -371,16 +391,17 @@
371391
Sm.joints_rightSecondYogaRef(1) = Sm.smoothingTimeCoM_Joints(10);
372392

373393
% if the demo is not "yogaExtended", stop at the 8th move
374-
% also, Sm.repeatYogaMoveset must be set to "false". The reason is that the
375-
% first and the last Yoga moveset for the "not extended" one are very
376-
% different, and the robot may "jump" when restarting the Yoga the second time
394+
% also, Sm.repeatTwiceYogaWithDifferentSpeed must be set to "false". The
395+
% reason is that the first and the last Yoga moveset for the "not extended"
396+
% one are very different, and the robot may "jump" when restarting the Yoga
397+
% the second time
377398
if ~Sm.yogaExtended
378399

379-
Sm.repeatYogaMoveset = false;
380-
Sm.joints_leftYogaRef = Sm.joints_leftYogaRef(1:8,:);
381-
Sm.joints_rightYogaRef = Sm.joints_rightYogaRef(1:8,:);
382-
Sm.joints_leftYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(4);
383-
Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10);
400+
Sm.repeatTwiceYogaWithDifferentSpeed = false;
401+
Sm.joints_leftYogaRef = Sm.joints_leftYogaRef(1:8,:);
402+
Sm.joints_rightYogaRef = Sm.joints_rightYogaRef(1:8,:);
403+
Sm.joints_leftYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(4);
404+
Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10);
384405
end
385406

386407
% MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA
@@ -441,9 +462,9 @@
441462
1 t 0;
442463
0 -t t];
443464

444-
T_torso = [0 -0.5 0.5;
445-
0 0.5 0.5;
446-
r/R r/(2*R) r/(2*R)];
465+
T_torso = [ 0.5 -0.5 0;
466+
0.5 0.5 0;
467+
r/(2*R) r/(2*R) r/R];
447468

448469
if Config.INCLUDE_COUPLING
449470

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

Lines changed: 16 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313
Config.LEFT_FOOT_IN_CONTACT_AT_0 = true;
1414

1515
% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY)
16-
Config.DEMO_MOVEMENTS = false;
16+
Config.DEMO_MOVEMENTS = true;
1717

1818
% If equal to true, the desired streamed values of the center of mass
1919
% are smoothed internally
@@ -115,9 +115,9 @@
115115
1 t 0;
116116
0 -t t];
117117

118-
T_torso = [0 -0.5 0.5;
119-
0 0.5 0.5;
120-
r/R r/(2*R) r/(2*R)];
118+
T_torso = [ 0.5 -0.5 0;
119+
0.5 0.5 0;
120+
r/(2*R) r/(2*R) r/R];
121121

122122
if Config.INCLUDE_COUPLING
123123

@@ -182,18 +182,18 @@
182182
Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1);
183183

184184
% configuration parameters for state machine (YOGA DEMO ONLY)
185-
Sm.tBalancing = 1;
186-
Sm.tBalancingBeforeYoga = 1;
187-
Sm.skipYoga = false;
188-
Sm.demoOnlyBalancing = false;
189-
Sm.demoStartsOnRightSupport = false;
190-
Sm.yogaAlsoOnRightFoot = false;
191-
Sm.yogaInLoop = false;
192-
Sm.repeatYogaMoveset = false;
193-
194-
% smoothing time for the second time the Yoga moveset are performed (YOGA DEMO ONLY)
195-
Sm.smoothingTimeSecondYogaLeft = 1;
196-
Sm.smoothingTimeSecondYogaRight = 1;
185+
Sm.tBalancing = 1;
186+
Sm.tBalancingBeforeYoga = 1;
187+
Sm.skipYoga = false;
188+
Sm.demoOnlyBalancing = false;
189+
Sm.demoStartsOnRightSupport = false;
190+
Sm.yogaAlsoOnRightFoot = false;
191+
Sm.twoFeetYogaInLoop = false;
192+
Sm.oneFootYogaInLoop = false;
193+
Sm.yogaCounter = 5;
194+
Sm.repeatTwiceYogaWithDifferentSpeed = false;
195+
Sm.smoothingTimeSecondYogaLeft = 1;
196+
Sm.smoothingTimeSecondYogaRight = 1;
197197

198198
%% Constraints for QP for balancing
199199

0 commit comments

Comments
 (0)