Skip to content

Commit

Permalink
final version in simulaton
Browse files Browse the repository at this point in the history
  • Loading branch information
@gabrielenava
gabrielenava committed Sep 20, 2018
1 parent 609e580 commit 8933687
Show file tree
Hide file tree
Showing 6 changed files with 96 additions and 28 deletions.
2 changes: 1 addition & 1 deletion torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initStateMachineYoga.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,7 @@
Sm.yogaExtended = true;
Sm.skipYoga = false;
Sm.demoOnlyBalancing = false;
Sm.demoStartsOnRightSupport = false;
Sm.demoStartsOnRightSupport = true;
Sm.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot
Sm.yogaInLoop = false;

Expand Down
15 changes: 15 additions & 0 deletions torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,6 +145,13 @@
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 = true;

% smoothing time for the second time the Yoga moveset are performed
Sm.smoothingTimeSecondYogaLeft = 0.8;
Sm.smoothingTimeSecondYogaRight = 0.8;

%% Joint references (YOGA DEMO ONLY)
Sm.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED
[-0.0348,0.0779,0.0429, ... %% state == 2 COM TRANSITION TO LEFT
Expand Down Expand Up @@ -350,9 +357,17 @@
24*Sm.smoothingTimeCoM_Joints(10);
25*Sm.smoothingTimeCoM_Joints(10)];

% smoothing time vector for the second time the Yoga moveset are performed
Sm.joints_leftSecondYogaRef = Sm.smoothingTimeSecondYogaLeft.*(0:(length(Sm.joints_rightYogaRef(:,1))-1));
Sm.joints_rightSecondYogaRef = Sm.smoothingTimeSecondYogaRight.*(0:(length(Sm.joints_rightYogaRef(:,1))-1));

% 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
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);
Expand Down
15 changes: 15 additions & 0 deletions torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,6 +145,13 @@
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 = true;

% smoothing time for the second time the Yoga moveset are performed
Sm.smoothingTimeSecondYogaLeft = 0.8;
Sm.smoothingTimeSecondYogaRight = 0.8;

%% Joint references (YOGA DEMO ONLY)
Sm.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED
[-0.0348,0.0779,0.0429, ... %% state == 2 COM TRANSITION TO LEFT
Expand Down Expand Up @@ -350,9 +357,17 @@
24*Sm.smoothingTimeCoM_Joints(10);
25*Sm.smoothingTimeCoM_Joints(10)];

% smoothing time vector for the second time the Yoga moveset are performed
Sm.joints_leftSecondYogaRef = Sm.smoothingTimeSecondYogaLeft.*(0:(length(Sm.joints_rightYogaRef(:,1))-1));
Sm.joints_rightSecondYogaRef = Sm.smoothingTimeSecondYogaRight.*(0:(length(Sm.joints_rightYogaRef(:,1))-1));

% 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
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);
Expand Down
12 changes: 12 additions & 0 deletions torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,6 +136,14 @@
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;

%% Joint references (YOGA DEMO ONLY)
Sm.joints_references = [zeros(1,ROBOT_DOF); %% NOT USED
[-0.0348,0.0779,0.0429, ... %% state == 2 COM TRANSITION TO LEFT
Expand Down Expand Up @@ -250,6 +258,10 @@
5*Sm.smoothingTimeCoM_Joints(10);
6*Sm.smoothingTimeCoM_Joints(10);
7*Sm.smoothingTimeCoM_Joints(10)];

% smoothing time vector for the second time the Yoga moveset are performed (NOT USED)
Sm.joints_leftSecondYogaRef = Sm.smoothingTimeSecondYogaLeft.*(0:(length(Sm.joints_rightYogaRef(:,1))-1));
Sm.joints_rightSecondYogaRef = Sm.smoothingTimeSecondYogaRight.*(0:(length(Sm.joints_rightYogaRef(:,1))-1));

% MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA
for i = 1:size(Sm.joints_rightYogaRef,1)
Expand Down
41 changes: 35 additions & 6 deletions torque-controllers/momentum-based-yoga/src/stateMachineYoga.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,6 +144,8 @@
end
if t > (Sm.joints_leftSecondYogaRef(end) + tSwitch) && secondYoga == true

qj_des = Sm.joints_leftYogaRef(end,2:end)';

% exit loop condition for the second yoga
if t > (Sm.joints_leftSecondYogaRef(end) + tSwitch + Sm.smoothingTimeSecondYogaLeft + Sm.joints_pauseBetweenYogaMoves)

Expand Down Expand Up @@ -291,21 +293,48 @@
KPCoM = Gain.KP_COM(state,:);
KDCoM = Gain.KD_COM(state,:);

% iterate over the yoga positions
for i = 1: size(Sm.joints_rightYogaRef,1)-1

if t > (Sm.joints_rightYogaRef(i,1) + tSwitch) && t <= (Sm.joints_rightYogaRef(i+1,1)+ tSwitch)
% positions for the first yoga
if t > (Sm.joints_rightYogaRef(i,1) + tSwitch) && t <= (Sm.joints_rightYogaRef(i+1,1)+ tSwitch) && secondYoga == false

qj_des = Sm.joints_rightYogaRef(i,2:end)';

% positions for the second yoga
elseif t > (Sm.joints_rightSecondYogaRef(i) + tSwitch) && t <= (Sm.joints_rightSecondYogaRef(i+1)+ tSwitch) && secondYoga == true

qj_des = Sm.joints_rightYogaRef(i,2:end)';

end
end
if t > Sm.joints_rightYogaRef(end,1) + tSwitch
if t > (Sm.joints_rightYogaRef(end,1) + tSwitch) && secondYoga == false

qj_des = Sm.joints_rightYogaRef(end,2:end)';

if t > (Sm.joints_rightYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves)
% if Sm.repeatYogaMoveset == true, repeat the Yoga
if t > (Sm.joints_rightYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatYogaMoveset == true

tSwitch = t;
secondYoga = true;

elseif t > (Sm.joints_rightYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatYogaMoveset == false

state = 11;
tSwitch = t;
state = 11;
tSwitch = t;
secondYoga = false;
end
end
if t > (Sm.joints_rightSecondYogaRef(end) + tSwitch) && secondYoga == true

qj_des = Sm.joints_rightYogaRef(end,2:end)';

% exit loop condition for the second yoga
if t > (Sm.joints_rightSecondYogaRef(end) + tSwitch + Sm.smoothingTimeSecondYogaRight + Sm.joints_pauseBetweenYogaMoves)

state = 11;
tSwitch = t;
secondYoga = false;
end
end
end
Expand Down Expand Up @@ -377,6 +406,6 @@
%% Update parameters
currentState = state;
jointsSmoothingTime = Sm.smoothingTimeCoM_Joints(state);
test = (Sm.joints_leftSecondYogaRef(end) + tSwitch + Sm.smoothingTimeSecondYogaLeft + Sm.joints_pauseBetweenYogaMoves);
test = (Sm.joints_leftSecondYogaRef(end) + tSwitch);

end
39 changes: 18 additions & 21 deletions torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ Model {
NumRootInports 0
NumRootOutports 0
ParameterArgumentNames ""
ComputedModelVersion "1.3280"
ComputedModelVersion "1.3281"
NumModelReferences 0
NumTestPointedSignals 0
NumProvidedFunctions 0
Expand Down Expand Up @@ -656,9 +656,9 @@ Model {
ModifiedByFormat "%<Auto>"
LastModifiedBy "gnava"
ModifiedDateFormat "%<Auto>"
LastModifiedDate "Thu Sep 20 10:41:51 2018"
RTWModifiedTimeStamp 459339702
ModelVersionFormat "1.%<AutoIncrement:3280>"
LastModifiedDate "Thu Sep 20 15:40:11 2018"
RTWModifiedTimeStamp 459357933
ModelVersionFormat "1.%<AutoIncrement:3281>"
SampleTimeColors off
SampleTimeAnnotations off
LibraryLinkDisplay "disabled"
Expand Down Expand Up @@ -11740,20 +11740,20 @@ Model {
ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(ext"
"mgr.Configuration('Core','General UI',true),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration("
"'Sources','WiredSimulink',true,'DataLoggingVariableName','ScopeData1'),extmgr.Configuration('Visuals','Time Dom"
"ain',true,'SerializedDisplays',{struct('MinYLimReal','33.07','MaxYLimReal','233.07','YLabelReal','','MinYLimMag"
"','33.07','MaxYLimMag','233.07','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesCo"
"lor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.066666"
"6666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.8313"
"72549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509803921568"
"6 0.650980392156863],'Title','%<SignalLabel>','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLin"
"es',1,'LineNames',{{'stateMachineYogaFCN/test'}},'ShowContent',true,'Placement',1)},'DisplayPropertyDefaults',s"
"truct('MinYLimReal','33.07','MaxYLimReal','233.07','YLabelReal','','MinYLimMag','33.07','MaxYLimMag','233.07','"
"LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6"
"86274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[0.0745098039215686 0.623529411764706 1;1 0.41"
"1764705882353 0.16078431372549;1 1 0.0666666666666667;0.717647058823529 0.274509803921569 1;0.392156862745098 0"
".831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863])),extmgr.Con"
"figuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('To"
"ols','Measurements',true,'Version','2017b')),'Version','2017b','Position',[1058 433 560 420])"
"ain',true,'SerializedDisplays',{struct('MinYLimReal','4.6475','MaxYLimReal','108.1725','YLabelReal','','MinYLim"
"Mag','4.6475','MaxYLimMag','108.1725','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'"
"AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0."
"0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 "
"0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509803"
"9215686 0.650980392156863],'Title','%<SignalLabel>','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'"
"NumLines',1,'LineNames',{{'stateMachineYogaFCN/test'}},'ShowContent',true,'Placement',1)},'DisplayPropertyDefau"
"lts',struct('MinYLimReal','4.6475','MaxYLimReal','108.1725','YLabelReal','','MinYLimMag','4.6475','MaxYLimMag',"
"'108.1725','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTic"
"kColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[0.0745098039215686 0.623529411764"
"706 1;1 0.411764705882353 0.16078431372549;1 1 0.0666666666666667;0.717647058823529 0.274509803921569 1;0.39215"
"6862745098 0.831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863])"
"),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Confi"
"guration('Tools','Measurements',true,'Version','2017b')),'Version','2017b','Position',[636 307 560 420])"
NumInputPorts "1"
}
Block {
Expand Down Expand Up @@ -23514,9 +23514,6 @@ Stateflow {
screen [1 1 1280 1024 1.000677131425054]
treeNode [0 133 0 0]
viewObj 132
visible 1
subviewS {
}
ssIdHighWaterMark 84
decomposition CLUSTER_CHART
type EML_CHART
Expand Down

0 comments on commit 8933687

Please sign in to comment.