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 index bc640e0..192c888 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initStateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initStateMachineYoga.m @@ -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; diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m index 544386e..8f178f4 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m @@ -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 @@ -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); diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m index aa70c6f..21643d1 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m @@ -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 @@ -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); diff --git a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m b/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m index b9e0938..06a9eb5 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m @@ -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 @@ -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) diff --git a/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m b/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m index 53817ba..a5807a3 100644 --- a/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m @@ -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) @@ -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 @@ -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 diff --git a/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl b/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl index 5ebae8f..2cac445 100644 --- a/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl +++ b/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.3280" + ComputedModelVersion "1.3281" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -656,9 +656,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Thu Sep 20 10:41:51 2018" - RTWModifiedTimeStamp 459339702 - ModelVersionFormat "1.%" + LastModifiedDate "Thu Sep 20 15:40:11 2018" + RTWModifiedTimeStamp 459357933 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -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','%','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','%','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 { @@ -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