From 725c82208590f0be1bf3511cd5a71d1dd512d712 Mon Sep 17 00:00:00 2001 From: iCubGenova04 Date: Thu, 4 Oct 2018 16:16:45 +0200 Subject: [PATCH 1/5] iros 2018 working yoga in loop --- .../robots/iCubGazeboV2_5/initCoordinator.m | 24 +- .../iCubGazeboV2_5/initStateMachineYoga.m | 53 +- .../app/robots/iCubGenova02/initCoordinator.m | 24 +- .../iCubGenova02/initStateMachineYoga.m | 49 +- .../app/robots/iCubGenova04/initCoordinator.m | 26 +- .../iCubGenova04/initStateMachineYoga.m | 53 +- .../robots/icubGazeboSim/initCoordinator.m | 24 +- .../icubGazeboSim/initStateMachineYoga.m | 41 +- .../initTorqueBalancingYoga.m | 20 +- .../src/stateMachineYoga.m | 52 +- .../torqueBalancingYoga.mdl | 2010 +++++++++++------ 11 files changed, 1584 insertions(+), 792 deletions(-) diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m index b30bab0..d60c09e 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m @@ -182,18 +182,18 @@ Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1); % configuration parameters for state machine (YOGA DEMO ONLY) -Sm.tBalancing = 1; -Sm.tBalancingBeforeYoga = 1; -Sm.skipYoga = false; -Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; -Sm.yogaAlsoOnRightFoot = false; -Sm.yogaInLoop = false; -Sm.repeatYogaMoveset = false; - -% smoothing time for the second time the Yoga moveset are performed (YOGA DEMO ONLY) -Sm.smoothingTimeSecondYogaLeft = 1; -Sm.smoothingTimeSecondYogaRight = 1; +Sm.tBalancing = 1; +Sm.tBalancingBeforeYoga = 1; +Sm.skipYoga = false; +Sm.demoOnlyBalancing = false; +Sm.demoStartsOnRightSupport = false; +Sm.yogaAlsoOnRightFoot = false; +Sm.twoFeetYogaInLoop = false; +Sm.oneFootYogaInLoop = false; +Sm.yogaCounter = 5; +Sm.repeatTwiceYogaWithDifferentSpeed = false; +Sm.smoothingTimeSecondYogaLeft = 1; +Sm.smoothingTimeSecondYogaRight = 1; %% Constraints for QP for balancing 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 0cbadb5..4b6a150 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 @@ -144,16 +144,36 @@ Sm.yogaExtended = true; Sm.skipYoga = false; Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = true; -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 = false; +Sm.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first +Sm.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot (not available for now) + +%%%% List of possible "Yoga in loop" %%%% + +% the robot will repeat the FULL DEMO (two feet balancing, yoga on left +% foot, back on two feet, yoga right foot, back on two feet). The demo is +% repeated until the user stops the Simulink model. This option is ignored +% if Sm.demoStartsOnRightSupport = true. +Sm.twoFeetYogaInLoop = false; + +% the robot will repeat the ONE FOOT yoga for the number of times the user +% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two +% feet balancing in between to consecutive yoga. WARNING: if the option +% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga +% on left foot for the number of times the user specifies in the Sm.yogaCounter, +% and then it will repeat the yoga on the right foot for the same number of times. +% This option is ignored if Sm.repeatTwiceYogaWithDifferentSpeed = true. +Sm.oneFootYogaInLoop = false; +Sm.yogaCounter = 5; + +% the robot will repeat the yoga moveset twice. This option works as the +% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However, +% it is possible to set a different yoga speed for the two yoga. +% Uneffective if Sm.yogaExtended = false; +Sm.repeatTwiceYogaWithDifferentSpeed = false; % smoothing time for the second time the Yoga moveset are performed -Sm.smoothingTimeSecondYogaLeft = 1.2; -Sm.smoothingTimeSecondYogaRight = 1.2; +Sm.smoothingTimeSecondYogaLeft = 1.2; +Sm.smoothingTimeSecondYogaRight = 1.2; %% Joint references (YOGA DEMO ONLY) Sm.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED @@ -369,16 +389,17 @@ 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 -% different, and the robot may "jump" when restarting the Yoga the second time +% also, Sm.repeatTwiceYogaWithDifferentSpeed 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); - Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10); + Sm.repeatTwiceYogaWithDifferentSpeed = 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); + Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10); end % MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m index b9f688a..8535b59 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m @@ -182,18 +182,18 @@ Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1); % configuration parameters for state machine (YOGA DEMO ONLY) -Sm.tBalancing = 1; -Sm.tBalancingBeforeYoga = 1; -Sm.skipYoga = false; -Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; -Sm.yogaAlsoOnRightFoot = false; -Sm.yogaInLoop = false; -Sm.repeatYogaMoveset = false; - -% smoothing time for the second time the Yoga moveset are performed (YOGA DEMO ONLY) -Sm.smoothingTimeSecondYogaLeft = 1; -Sm.smoothingTimeSecondYogaRight = 1; +Sm.tBalancing = 1; +Sm.tBalancingBeforeYoga = 1; +Sm.skipYoga = false; +Sm.demoOnlyBalancing = false; +Sm.demoStartsOnRightSupport = false; +Sm.yogaAlsoOnRightFoot = false; +Sm.twoFeetYogaInLoop = false; +Sm.oneFootYogaInLoop = false; +Sm.yogaCounter = 5; +Sm.repeatTwiceYogaWithDifferentSpeed = false; +Sm.smoothingTimeSecondYogaLeft = 1; +Sm.smoothingTimeSecondYogaRight = 1; %% Constraints for QP for balancing 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 d38b11b..1acacda 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m @@ -146,16 +146,36 @@ Sm.yogaExtended = true; Sm.skipYoga = false; Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; +Sm.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first 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 = false; +%%%% List of possible "Yoga in loop" %%%% + +% the robot will repeat the FULL DEMO (two feet balancing, yoga on left +% foot, back on two feet, yoga right foot, back on two feet). The demo is +% repeated until the user stops the Simulink model. This option is ignored +% if Sm.demoStartsOnRightSupport = true. +Sm.twoFeetYogaInLoop = false; + +% the robot will repeat the ONE FOOT yoga for the number of times the user +% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two +% feet balancing in between to consecutive yoga. WARNING: if the option +% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga +% on left foot for the number of times the user specifies in the Sm.yogaCounter, +% and then it will repeat the yoga on the right foot for the same number of times. +% This option is ignored if Sm.repeatTwiceYogaWithDifferentSpeed = true. +Sm.oneFootYogaInLoop = false; +Sm.yogaCounter = 5; + +% the robot will repeat the yoga moveset twice. This option works as the +% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However, +% it is possible to set a different yoga speed for the two yoga. +% Uneffective if Sm.yogaExtended = false; +Sm.repeatTwiceYogaWithDifferentSpeed = false; % smoothing time for the second time the Yoga moveset are performed -Sm.smoothingTimeSecondYogaLeft = 0.6; -Sm.smoothingTimeSecondYogaRight = 0.6; +Sm.smoothingTimeSecondYogaLeft = 0.6; +Sm.smoothingTimeSecondYogaRight = 0.6; %% Joint references (YOGA DEMO ONLY) Sm.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED @@ -371,16 +391,17 @@ 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 -% different, and the robot may "jump" when restarting the Yoga the second time +% also, Sm.repeatTwiceYogaWithDifferentSpeed 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); - Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10); + Sm.repeatTwiceYogaWithDifferentSpeed = 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); + Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10); end % MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m index b30bab0..741c0df 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m @@ -13,7 +13,7 @@ Config.LEFT_FOOT_IN_CONTACT_AT_0 = true; % If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) -Config.DEMO_MOVEMENTS = false; +Config.DEMO_MOVEMENTS = true; % If equal to true, the desired streamed values of the center of mass % are smoothed internally @@ -182,18 +182,18 @@ Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1); % configuration parameters for state machine (YOGA DEMO ONLY) -Sm.tBalancing = 1; -Sm.tBalancingBeforeYoga = 1; -Sm.skipYoga = false; -Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; -Sm.yogaAlsoOnRightFoot = false; -Sm.yogaInLoop = false; -Sm.repeatYogaMoveset = false; - -% smoothing time for the second time the Yoga moveset are performed (YOGA DEMO ONLY) -Sm.smoothingTimeSecondYogaLeft = 1; -Sm.smoothingTimeSecondYogaRight = 1; +Sm.tBalancing = 1; +Sm.tBalancingBeforeYoga = 1; +Sm.skipYoga = false; +Sm.demoOnlyBalancing = false; +Sm.demoStartsOnRightSupport = false; +Sm.yogaAlsoOnRightFoot = false; +Sm.twoFeetYogaInLoop = false; +Sm.oneFootYogaInLoop = false; +Sm.yogaCounter = 5; +Sm.repeatTwiceYogaWithDifferentSpeed = false; +Sm.smoothingTimeSecondYogaLeft = 1; +Sm.smoothingTimeSecondYogaRight = 1; %% Constraints for QP for balancing 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 3b69419..ab6f0e1 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m @@ -144,16 +144,36 @@ Sm.yogaExtended = true; Sm.skipYoga = false; Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; -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 = false; +Sm.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first +Sm.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot (not available for now) + +%%%% List of possible "Yoga in loop" %%%% + +% the robot will repeat the FULL DEMO (two feet balancing, yoga on left +% foot, back on two feet, yoga right foot, back on two feet). The demo is +% repeated until the user stops the Simulink model. This option is ignored +% if Sm.demoStartsOnRightSupport = true. +Sm.twoFeetYogaInLoop = false; + +% the robot will repeat the ONE FOOT yoga for the number of times the user +% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two +% feet balancing in between to consecutive yoga. WARNING: if the option +% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga +% on left foot for the number of times the user specifies in the Sm.yogaCounter, +% and then it will repeat the yoga on the right foot for the same number of times. +% This option is ignored if Sm.repeatTwiceYogaWithDifferentSpeed = true. +Sm.oneFootYogaInLoop = true; +Sm.yogaCounter = 10; + +% the robot will repeat the yoga moveset twice. This option works as the +% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However, +% it is possible to set a different yoga speed for the two yoga. +% Uneffective if Sm.yogaExtended = false; +Sm.repeatTwiceYogaWithDifferentSpeed = false; % smoothing time for the second time the Yoga moveset are performed -Sm.smoothingTimeSecondYogaLeft = 0.6; -Sm.smoothingTimeSecondYogaRight = 0.6; +Sm.smoothingTimeSecondYogaLeft = 0.7; +Sm.smoothingTimeSecondYogaRight = 0.7; %% Joint references (YOGA DEMO ONLY) Sm.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED @@ -375,16 +395,17 @@ 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 -% different, and the robot may "jump" when restarting the Yoga the second time +% also, Sm.repeatTwiceYogaWithDifferentSpeed 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); - Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10); + Sm.repeatTwiceYogaWithDifferentSpeed = 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); + Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10); end % MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA diff --git a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m index 1481c9a..e83e88b 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m +++ b/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m @@ -182,18 +182,18 @@ Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1); % configuration parameters for state machine (YOGA DEMO ONLY) -Sm.tBalancing = 1; -Sm.tBalancingBeforeYoga = 1; -Sm.skipYoga = false; -Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; -Sm.yogaAlsoOnRightFoot = false; -Sm.yogaInLoop = false; -Sm.repeatYogaMoveset = false; - -% smoothing time for the second time the Yoga moveset are performed (YOGA DEMO ONLY) -Sm.smoothingTimeSecondYogaLeft = 1; -Sm.smoothingTimeSecondYogaRight = 1; +Sm.tBalancing = 1; +Sm.tBalancingBeforeYoga = 1; +Sm.skipYoga = false; +Sm.demoOnlyBalancing = false; +Sm.demoStartsOnRightSupport = false; +Sm.yogaAlsoOnRightFoot = false; +Sm.twoFeetYogaInLoop = false; +Sm.oneFootYogaInLoop = false; +Sm.yogaCounter = 5; +Sm.repeatTwiceYogaWithDifferentSpeed = false; +Sm.smoothingTimeSecondYogaLeft = 1; +Sm.smoothingTimeSecondYogaRight = 1; %% Constraints for QP for balancing 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 fb2a9a5..befe7ec 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m @@ -138,17 +138,36 @@ Sm.tBalancingBeforeYoga = 1; Sm.skipYoga = false; Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; -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; +Sm.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first +Sm.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot (not available for now) + +%%%% List of possible "Yoga in loop" %%%% + +% the robot will repeat the FULL DEMO (two feet balancing, yoga on left +% foot, back on two feet, yoga right foot, back on two feet). The demo is +% repeated until the user stops the Simulink model. This option is ignored +% if Sm.demoStartsOnRightSupport = true. +Sm.twoFeetYogaInLoop = false; + +% the robot will repeat the ONE FOOT yoga for the number of times the user +% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two +% feet balancing in between to consecutive yoga. WARNING: if the option +% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga +% on left foot for the number of times the user specifies in the Sm.yogaCounter, +% and then it will repeat the yoga on the right foot for the same number of times. +% This option is ignored if Sm.repeatTwiceYogaWithDifferentSpeed = true. +Sm.oneFootYogaInLoop = false; +Sm.yogaCounter = 5; + +% the robot will repeat the yoga moveset twice. This option works as the +% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However, +% it is possible to set a different yoga speed for the two yoga. +% (NOT AVAILABLE FOR ICUBGAZEBOSIM) +Sm.repeatTwiceYogaWithDifferentSpeed = false; + +% smoothing time for the second time the Yoga moveset are performed +Sm.smoothingTimeSecondYogaLeft = 1.2; +Sm.smoothingTimeSecondYogaRight = 1.2; %% Joint references (YOGA DEMO ONLY) Sm.joints_references = [zeros(1,ROBOT_DOF); %% NOT USED diff --git a/torque-controllers/momentum-based-yoga/initTorqueBalancingYoga.m b/torque-controllers/momentum-based-yoga/initTorqueBalancingYoga.m index 8e91d02..a3b13aa 100644 --- a/torque-controllers/momentum-based-yoga/initTorqueBalancingYoga.m +++ b/torque-controllers/momentum-based-yoga/initTorqueBalancingYoga.m @@ -43,27 +43,27 @@ % % app/robots/YARP_ROBOT_NAME/initRefGen.m % -SM_TYPE = 'YOGA'; +SM_TYPE = 'YOGA'; % Config.SCOPES: if set to true, all visualizers for debugging are active -Config.SCOPES_ALL = true; +Config.SCOPES_ALL = true; % You can also activate only some specific debugging scopes -Config.SCOPES_EXT_WRENCHES = false; -Config.SCOPES_GAIN_SCHE_INFO = false; -Config.SCOPES_MAIN = false; -Config.SCOPES_QP = false; -Config.SCOPES_INERTIA = false; +Config.SCOPES_EXT_WRENCHES = false; +Config.SCOPES_GAIN_SCHE_INFO = false; +Config.SCOPES_MAIN = false; +Config.SCOPES_QP = false; +Config.SCOPES_INERTIA = true; % Config.CHECK_LIMITS: if set to true, the controller will stop as soon as % any of the joint limit is touched. -Config.CHECK_LIMITS = false; +Config.CHECK_LIMITS = false; % DATA PROCESSING % % If Config.SAVE_WORKSPACE = True, every time the simulink model is run the % workspace is saved after stopping the simulation -Config.SAVE_WORKSPACE = false; +Config.SAVE_WORKSPACE = false; % If CHECK_INTEGRATION_TIME = True, after stopping the simulation the % Simulink time is compared with the Yarp time to check if the desired @@ -75,7 +75,7 @@ %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Controller period [s] -Config.Ts = 0.01; +Config.Ts = 0.01; addpath('./src/') addpath(genpath('../../library')); diff --git a/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m b/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m index be4607a..b2a90fe 100644 --- a/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m @@ -5,12 +5,14 @@ persistent tSwitch; persistent w_H_fixedLink; persistent secondYoga; - - if isempty(state) || isempty(tSwitch) || isempty(w_H_fixedLink) || isempty(secondYoga) - state = Sm.stateAt0; - tSwitch = 0; - w_H_fixedLink = eye(4); - secondYoga = false; + persistent yogaMovesetCounter; + + if isempty(state) || isempty(tSwitch) || isempty(w_H_fixedLink) || isempty(secondYoga) || isempty(yogaMovesetCounter) + state = Sm.stateAt0; + tSwitch = 0; + w_H_fixedLink = eye(4); + secondYoga = false; + yogaMovesetCounter = 1; end CoM_des = CoM_0; @@ -129,13 +131,25 @@ qj_des = Sm.joints_leftYogaRef(end,2:end)'; - % if Sm.repeatYogaMoveset == true, repeat the Yoga - if t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatYogaMoveset == true + % if Sm.repeatTwiceYogaWithDifferentSpeed == true, repeat the Yoga + if t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatTwiceYogaWithDifferentSpeed == true tSwitch = t; secondYoga = true; - elseif t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatYogaMoveset == false + elseif t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatTwiceYogaWithDifferentSpeed == false && Sm.oneFootYogaInLoop + + tSwitch = t; + yogaMovesetCounter = yogaMovesetCounter +1; + + % if the robot repeated the Yoga moveset for the number of + % times required by the user, then exit the loop + if yogaMovesetCounter > Sm.yogaCounter + + state = 5; + end + + elseif t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatTwiceYogaWithDifferentSpeed == false && Sm.oneFootYogaInLoop == false state = 5; tSwitch = t; @@ -312,13 +326,25 @@ qj_des = Sm.joints_rightYogaRef(end,2:end)'; - % 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 + % if Sm.repeatTwiceYogaWithDifferentSpeed == true, repeat the Yoga + if t > (Sm.joints_rightYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatTwiceYogaWithDifferentSpeed == true tSwitch = t; secondYoga = true; + + elseif t > (Sm.joints_rightYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatTwiceYogaWithDifferentSpeed == false && Sm.oneFootYogaInLoop + + tSwitch = t; + yogaMovesetCounter = yogaMovesetCounter +1; - elseif t > (Sm.joints_rightYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatYogaMoveset == false + % if the robot repeated the Yoga moveset for the number of + % times required by the user, then exit the loop + if yogaMovesetCounter > Sm.yogaCounter + + state = 11; + end + + elseif t > (Sm.joints_rightYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatTwiceYogaWithDifferentSpeed == false && Sm.oneFootYogaInLoop == false state = 11; tSwitch = t; @@ -390,7 +416,7 @@ if t - tSwitch > Sm.tBalancing - if Sm.yogaInLoop + if Sm.twoFeetYogaInLoop state = 2; w_H_fixedLink = w_H_fixedLink*r_sole_H_b/l_sole_H_b; diff --git a/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl b/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl index 2bdff0a..4b7a526 100644 --- a/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl +++ b/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl @@ -1,12 +1,12 @@ Model { Name "torqueBalancingYoga" - Version 8.8 + Version 9.0 SavedCharacterEncoding "UTF-8" GraphicalInterface { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.3290" + ComputedModelVersion "1.3302" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -15,12 +15,566 @@ Model { HasInitializeEvent 0 HasTerminateEvent 0 IsExportFunctionModel 0 + NumParameterArguments 0 + NumExternalFileReferences 85 + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Configuration" + Path "torqueBalancingYoga/Configuration" + SID "4537" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" + Path "torqueBalancingYoga/Dump and visualize/Visualizer/Get Measurement" + SID "4542" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" + Path "torqueBalancingYoga/Dump and visualize/Visualizer/Get Measurement1" + SID "4543" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" + Path "torqueBalancingYoga/Dynamics and Kinematics/Dynamics/Centroidal Momentum" + SID "2345" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Get Bias Forces" + Path "torqueBalancingYoga/Dynamics and Kinematics/Dynamics/Get Bias Forces" + SID "2348" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Mass Matrix" + Path "torqueBalancingYoga/Dynamics and Kinematics/Dynamics/Mass Matrix" + SID "2349" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" + Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/DotJ Nu l_sole " + SID "2375" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" + Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/DotJ Nu r_sole " + SID "2376" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" + Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/Jacobian com" + SID "2378" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" + Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/Jacobian l_sole" + SID "2379" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" + Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/Jacobian r_sole" + SID "2380" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/l_sole" + SID "2405" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/r_sole" + SID "2406" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/xCom/CoM" + SID "4363" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "simulink/Logic and Bit\nOperations/Compare\nTo Zero" + Path "torqueBalancingYoga/Robot State and References/Compare To Zero" + SID "2916" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "simulink/Logic and Bit\nOperations/Compare\nTo Zero" + Path "torqueBalancingYoga/Robot State and References/Compare To Zero1" + SID "2917" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "simulink/Logic and Bit\nOperations/Bitwise\nOperator" + Path "torqueBalancingYoga/Robot State and References/Coordinator" + SID "2908" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" + Path "torqueBalancingYoga/Robot State and References/Get Measurement" + SID "4538" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to bas" + "e link transform /Fixed base to imu transform" + SID "4052" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to bas" + "e link transform /Fixed base to root link transform" + SID "4053" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Read" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to bas" + "e link transform /Neck Position" + SID "4056" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to bas" + "e link transform /holder 1" + SID "4060" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to bas" + "e link transform /holder 2" + SID "4061" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to wor" + "ld transform (fixed base)" + SID "4071" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/RFoot to wor" + "ld transform (fixed base)" + SID "4084" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Compute State Velocity/Feet Jac" + "obians/Jacobian LFoot" + SID "4094" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Compute State Velocity/Feet Jac" + "obians/Jacobian RFoot" + SID "4095" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Compute State Velocity/Get Meas" + "urement" + SID "4540" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Read" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/IMU measurements" + SID "3222" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/holder " + SID "2109" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/holder 1" + SID "2110" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/xCom/CoM" + SID "2141" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" + Path "torqueBalancingYoga/Robot State and References/Select State and References/Minimum Jerk Trajectory " + "Generator1" + SID "2152" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" + Path "torqueBalancingYoga/Robot State and References/Select State and References/Minimum Jerk Trajectory " + "Generator2" + SID "2153" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute State Velocity/Feet Jacob" + "ians/Jacobian LFoot" + SID "4219" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute State Velocity/Feet Jacob" + "ians/Jacobian RFoot" + SID "4220" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute State Velocity/Get Measur" + "ement" + SID "4541" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" + "orm/LFoot to base link transform /Fixed base to imu transform" + SID "4250" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" + "orm/LFoot to base link transform /Fixed base to root link transform" + SID "4251" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Read" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" + "orm/LFoot to base link transform /Neck Position" + SID "4254" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" + "orm/LFoot to base link transform /holder 1" + SID "4258" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" + "orm/LFoot to base link transform /holder 2" + SID "4259" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" + "orm/LFoot to world transform (fixed base)" + SID "4269" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" + "orm/RFoot to base link transform/Fixed base to imu transform" + SID "4275" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" + "orm/RFoot to base link transform/Fixed base to root link transform" + SID "4276" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Read" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" + "orm/RFoot to base link transform/Neck Position" + SID "4279" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" + "orm/RFoot to base link transform/holder 1" + SID "4283" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" + "orm/RFoot to base link transform/holder 2" + SID "4284" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" + "orm/RFoot to world transform (fixed base)" + SID "4306" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Minimum Jerk Trajectory Generator" + SID "2176" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/holder 1" + SID "2187" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/holder 2" + SID "2188" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Read" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/inertial" + SID "2630" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Read" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/left_foot_wrench" + SID "2206" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Read" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/right_foot_wrench" + SID "2218" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/xCom/CoM" + SID "4229" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/xCom1/CoM" + SID "4303" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "simulink/Logic and Bit\nOperations/Bitwise\nOperator" + Path "torqueBalancingYoga/Robot State and References/Yoga" + SID "2909" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyActuators/Set References" + Path "torqueBalancingYoga/Set References" + SID "2354" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Centroidal Momentum" + SID "3718" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" + "sform/LFoot to base link transform /Fixed base to imu transform" + SID "4431" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" + "sform/LFoot to base link transform /Fixed base to root link transform" + SID "4432" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Read" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" + "sform/LFoot to base link transform /Neck Position" + SID "4435" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" + "sform/LFoot to base link transform /holder 1" + SID "4439" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" + "sform/LFoot to base link transform /holder 2" + SID "4440" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" + "sform/LFoot to world transform (fixed base)" + SID "4450" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" + "sform/RFoot to base link transform/Fixed base to imu transform" + SID "4456" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" + "sform/RFoot to base link transform/Fixed base to root link transform" + SID "4457" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Read" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" + "sform/RFoot to base link transform/Neck Position" + SID "4460" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" + "sform/RFoot to base link transform/holder 1" + SID "4464" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" + "sform/RFoot to base link transform/holder 2" + SID "4465" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" + "sform/RFoot to world transform (fixed base)" + SID "4475" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Jacobian" + SID "3719" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Jacobian1" + SID "3720" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Read" + Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/inertial" + SID "3724" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Match Signal Sizes" + Path "torqueBalancingYoga/controller_QP/Compute joint torques/QPSolver/One Foot/Match Signal Sizes1" + SID "4601" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/QP" + Path "torqueBalancingYoga/controller_QP/Compute joint torques/QPSolver/One Foot/QP Two Feet" + SID "4603" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Match Signal Sizes" + Path "torqueBalancingYoga/controller_QP/Compute joint torques/QPSolver/Two Feet/Match Signal Sizes" + SID "4615" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/QP" + Path "torqueBalancingYoga/controller_QP/Compute joint torques/QPSolver/Two Feet/QP Two Feet" + SID "4617" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" + Path "torqueBalancingYoga/controller_QP/Compute joint torques (motor reflected inertia)/Get Measurement" + SID "4539" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/wholeBodyStates/Get Limits" + Path "torqueBalancingYoga/emergency stop: joint limits/Get Limits" + SID "2432" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Simulator Synchronizer" + Path "torqueBalancingYoga/synchronizer/GAZEBO_SYNCHRONIZER/Simulator Synchronizer" + SID "2426" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Real Time Synchronizer" + Path "torqueBalancingYoga/synchronizer/REAL_TIME_SYNC/Real Time Synchronizer" + SID "2431" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Clock" + Path "torqueBalancingYoga/synchronizer/Yarp Clock" + SID "4629" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueBalancingYoga/tauDot Saturation/holder " + SID "4559" + Type "LIBRARY_BLOCK" + } + OrderedModelArguments 1 } LogicAnalyzerGraphicalSettings "" LogicAnalyzerPlugin "on" LogicAnalyzerSignalOrdering "" DiagnosticSuppressor "on" SuppressorTable "22 serialization::archive 11 0 6 0 0 0 11 0" + CustomCodeFunctionData "" + SLCCPlugin "on" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on DisableAllScopes off @@ -42,7 +596,7 @@ Model { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [65.0, -4.0, 2495.0, 1444.0] + Location [65.0, -4.0, 1855.0, 1084.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -64,11 +618,11 @@ Model { $ObjectID 5 $ClassName "Simulink.EditorInfo" IsActive [1] - ViewObjType "SimulinkSubsys" - LoadSaveID "4519" - Extents [2457.0, 1267.0] - ZoomFactor [2.5618562647286631] - Offset [-431.2538499639769, -296.28163274495677] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [1817.0, 907.0] + ZoomFactor [1.25] + Offset [559.76461888509721, 174.25984072810013] } Object { $PropName "DockComponentsInfo" @@ -88,28 +642,30 @@ Model { "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABiAP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAALAD///8AAAm/AAAFMQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "QBjAHQAbwByAAAAAAD/////AAAALAD///8AAAc/AAADyQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" "////wEAAACA/////wAAAAAAAAAA/////wEAAADo/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAFo/////wAAAAAAAAAA/" "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAANR/////wAAAAAAAAAA/" "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" } } + HideAutomaticNames on Created "Tue Feb 18 10:13:36 2014" Creator "daniele" UpdateHistory "UpdateHistoryNever" ModifiedByFormat "%" - LastModifiedBy "gnava" + LastModifiedBy "icub" ModifiedDateFormat "%" - LastModifiedDate "Tue Sep 25 14:19:35 2018" - RTWModifiedTimeStamp 459785975 - ModelVersionFormat "1.%" + LastModifiedDate "Thu Oct 04 16:14:04 2018" + RTWModifiedTimeStamp 460564663 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" WideLines off ShowLineDimensions on ShowPortDataTypes off + PortDataTypeDisplayFormat "AliasTypeOnly" ShowEditTimeErrors on ShowEditTimeWarnings on ShowEditTimeAdvisorChecks off @@ -130,12 +686,14 @@ Model { BlockNameDataTip off BlockParametersDataTip off BlockDescriptionStringDataTip off + BlockVariantConditionDataTip off ToolBar on StatusBar on BrowserShowLibraryLinks on FunctionConnectors off BrowserLookUnderMasks on SimulationMode "normal" + VisualizeLoggedSignalsWhenLoggingToFile off PauseTimes "5" NumberOfSteps 1 SnapshotBufferSize 10 @@ -191,12 +749,13 @@ Model { ExtModeAutoUpdateStatusClock on ShowModelReferenceBlockVersion off ShowModelReferenceBlockIO off + OrderedModelArguments on Array { Type "Handle" Dimension 1 Simulink.ConfigSet { $ObjectID 8 - Version "1.16.5" + Version "1.17.1" DisabledProps [] Description "" Array { @@ -204,7 +763,7 @@ Model { Dimension 9 Simulink.SolverCC { $ObjectID 9 - Version "1.16.5" + Version "1.17.1" DisabledProps [] Description "" StartTime "0.0" @@ -223,7 +782,6 @@ Model { MaxConsecutiveMinStep "1" RelTol "1e-3" EnableMultiTasking on - EnableConcurrentExecution off ConcurrentTasks off Solver "FixedStepDiscrete" SolverName "FixedStepDiscrete" @@ -240,10 +798,11 @@ Model { SampleTimeConstraint "Unconstrained" InsertRTBMode "Whenever possible" SampleTimeProperty [] + DecoupledContinuousIntegration off } Simulink.DataIOCC { $ObjectID 10 - Version "1.16.5" + Version "1.17.1" DisabledProps [] Description "" Decimation "1" @@ -278,12 +837,13 @@ Model { ReturnWorkspaceOutputsName "out" Refine "1" LoggingToFile off + DatasetSignalFormat "timeseries" LoggingFileName "out.mat" LoggingIntervals "[-inf, inf]" } Simulink.OptimizationCC { $ObjectID 11 - Version "1.16.5" + Version "1.17.1" Array { Type "Cell" Dimension 8 @@ -338,10 +898,14 @@ Model { BufferReusableBoundary on SimCompilerOptimization "off" AccelVerboseBuild off + OptimizeBlockOrder "off" + OptimizeDataStoreBuffers on + BusAssignmentInplaceUpdate on + DifferentSizesBufferReuse off } Simulink.DebuggingCC { $ObjectID 12 - Version "1.16.5" + Version "1.17.1" Array { Type "Cell" Dimension 1 @@ -380,6 +944,7 @@ Model { MultiTaskRateTransMsg "error" SingleTaskRateTransMsg "none" TasksWithSamePriorityMsg "warning" + ExportedTasksRateTransMsg "none" SigSpecEnsureSampleTimeMsg "warning" CheckMatrixSingularityMsg "none" IntegerOverflowMsg "warning" @@ -421,6 +986,7 @@ Model { StateNameClashWarn "warning" SimStateInterfaceChecksumMismatchMsg "warning" SimStateOlderReleaseMsg "error" + ChecksumConsistencyForSSReuse "none" InitInArrayFormatMsg "warning" StrictBusMsg "ErrorLevel1" BusNameAdapt "WarnAndRepair" @@ -428,6 +994,7 @@ Model { SymbolicDimMinMaxWarning "warning" LossOfSymbolicDimsSimulationWarning "warning" LossOfSymbolicDimsCodeGenerationWarning "error" + SymbolicDimsDataTypeCodeGenerationDiagnostic "error" BlockIODiagnostic "none" SFUnusedDataAndEventsDiag "warning" SFUnexpectedBacktrackingDiag "warning" @@ -446,10 +1013,14 @@ Model { AllowedUnitSystems "all" UnitsInconsistencyMsg "warning" AllowAutomaticUnitConversions on + RCSCRenamedMsg "warning" + RCSCObservableMsg "warning" + ForceCombineOutputUpdateInSim off + UnitDatabase "" } Simulink.HardwareCC { $ObjectID 13 - Version "1.16.5" + Version "1.17.1" DisabledProps [] Description "" ProdBitPerChar 8 @@ -497,7 +1068,7 @@ Model { } Simulink.ModelReferenceCC { $ObjectID 14 - Version "1.16.5" + Version "1.17.1" DisabledProps [] Description "" UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" @@ -516,7 +1087,7 @@ Model { } Simulink.SFSimCC { $ObjectID 15 - Version "1.16.5" + Version "1.17.1" DisabledProps [] Description "" SimCustomSourceCode "" @@ -540,11 +1111,13 @@ Model { ModelFunctionsGlobalVisibility "on" CompileTimeRecursionLimit 50 EnableRuntimeRecursion on + MATLABDynamicMemAlloc on + MATLABDynamicMemAllocThreshold 65536 } Simulink.RTWCC { $BackupClass "Simulink.RTWCC" $ObjectID 16 - Version "1.16.5" + Version "1.17.1" Array { Type "Cell" Dimension 16 @@ -595,6 +1168,7 @@ Model { CustomLibrary "" CustomDefine "" CustomLAPACKCallback "" + CustomFFTCallback "" CustomInitializer "" CustomTerminator "" Toolchain "Automatically locate an installed toolchain" @@ -630,7 +1204,7 @@ Model { Dimension 2 Simulink.CodeAppCC { $ObjectID 17 - Version "1.16.5" + Version "1.17.1" Array { Type "Cell" Dimension 27 @@ -679,6 +1253,7 @@ Model { SFDataObjDesc off MATLABFcnDesc off MangleLength 1 + SharedChecksumLength 8 CustomSymbolStrGlobalVar "$R$N$M" CustomSymbolStrType "$N$R$M_T" CustomSymbolStrField "$N$M" @@ -689,6 +1264,8 @@ Model { CustomSymbolStrTmpVar "$N$M" CustomSymbolStrMacro "$R$N$M" CustomSymbolStrUtil "$N$C" + CustomSymbolStrEmxType "emxArray_$M$N" + CustomSymbolStrEmxFcn "emx$M$N" CustomUserTokenString "" CustomCommentsFcn "" DefineNamingRule "None" @@ -700,6 +1277,7 @@ Model { InsertBlockDesc off InsertPolySpaceComments off SimulinkBlockComments on + StateflowObjectComments on MATLABSourceComments off EnableCustomComments off InternalIdentifierFile "" @@ -712,7 +1290,7 @@ Model { Simulink.GRTTargetCC { $BackupClass "Simulink.TargetCC" $ObjectID 18 - Version "1.16.5" + Version "1.17.1" Array { Type "Cell" Dimension 17 @@ -743,7 +1321,7 @@ Model { TargetLangStandard "C89/C90 (ANSI)" CodeReplacementLibrary "None" UtilityFuncGeneration "Auto" - ERTMultiwordTypeDef "System defined" + MultiwordTypeDef "System defined" MultiwordLength 2048 GenerateFullHeader on InferredTypesCompatibility off @@ -786,6 +1364,11 @@ Model { UseToolchainInfoCompliant on GenerateSharedConstants on CoderGroups [] + AccessMethods [] + LookupTableObjectStructAxisOrder "1,2,3,4,..." + LUTObjectStructOrderExplicitValues "Size,Breakpoints,Table" + LUTObjectStructOrderEvenSpacing "Size,Breakpoints,Table" + ArrayLayout "Column-major" UseMalloc off ExtMode off ExtModeStaticAlloc off @@ -807,7 +1390,7 @@ Model { } SlCovCC.ConfigComp { $ObjectID 19 - Version "1.16.5" + Version "1.17.1" DisabledProps [] Description "Simulink Coverage Configuration Component" Name "Simulink Coverage" @@ -843,6 +1426,7 @@ Model { CovUseTimeInterval off CovStartTime 0 CovStopTime 0 + CovMcdcMode "Masking" } PropName "Components" } @@ -878,6 +1462,7 @@ Model { FontWeight "normal" FontAngle "normal" ShowName on + HideAutomaticName on BlockRotation 0 BlockMirror off } @@ -891,7 +1476,12 @@ Model { FontSize 10 FontWeight "normal" FontAngle "normal" + MarkupType "model" UseDisplayTextAsClickCallback off + AnnotationType "note_annotation" + FixedHeight off + FixedWidth off + Interpreter "off" } LineDefaults { FontName "Helvetica" @@ -903,7 +1493,7 @@ Model { SelfModifiable "off" IconFrame "on" IconOpaque "opaque" - RunInitForIconRedraw "off" + RunInitForIconRedraw "analyze" IconRotate "none" PortRotate "default" IconUnits "autoscale" @@ -1067,6 +1657,8 @@ Model { InitialOutput "[]" MustResolveToSignalObject off OutputWhenUnConnected off + OutputWhenUnconnectedValue "0" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Product @@ -1148,10 +1740,12 @@ Model { MaskHideContents off SFBlockType "NONE" GeneratePreprocessorConditionals off + AllowZeroVariantControls off PropagateVariantConditions off TreatAsGroupedWhenPropagatingVariantConditions on ContentPreviewEnabled off IsWebBlock off + IsObserver off } Block { BlockType Sum @@ -1201,8 +1795,9 @@ Model { } System { Name "torqueBalancingYoga" - Location [65, 24, 2560, 1440] - Open off + Location [65, -4, 1920, 1080] + Open on + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1213,7 +1808,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "195" + ZoomFactor "125" ReportName "simulink-default.rpt" SIDHighWatermark "4649" Block { @@ -1226,15 +1821,10 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Configuration" SourceType "" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off ConfigSource "Workspace" ConfigObject "'WBTConfigRobot'" - RobotName "'icubSim'" - UrdfFile "'model.urdf'" - ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" - ControlBoardsNames "{'left_arm','right_arm','torso'}" - LocalName "'WBT'" - GravityVector "[0,0,-9.81]" } Block { BlockType SubSystem @@ -1256,8 +1846,9 @@ Model { } System { Name "Dump and visualize" - Location [65, 24, 2560, 1440] + Location [65, 24, 1920, 1080] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1268,7 +1859,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "287" + ZoomFactor "205" Block { BlockType From Name "From" @@ -1391,8 +1982,9 @@ Model { Variant off System { Name "Visualizer" - Location [65, 24, 2560, 1440] + Location [65, 24, 1920, 1080] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1403,7 +1995,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "129" + ZoomFactor "91" Block { BlockType Inport Name "tau_des" @@ -1956,6 +2548,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" SourceType "Get Measurement" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off measuredType "Joints Position" Port { @@ -1974,6 +2567,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" SourceType "Get Measurement" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off measuredType "Joints Torque" } @@ -3857,8 +4451,9 @@ Model { Variant off System { Name "Dynamics and Kinematics" - Location [65, -4, 2560, 1440] + Location [65, 24, 1918, 1060] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3869,7 +4464,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "216" + ZoomFactor "100" Block { BlockType Inport Name "w_H_b" @@ -3909,6 +4504,7 @@ Model { Name "Dynamics" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3959,6 +4555,7 @@ Model { Name "Add motors reflected inertias" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3995,6 +4592,7 @@ Model { Name "Add motor reflected inertias" Location [223, 338, 826, 833] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4006,7 +4604,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "52" + SIDHighWatermark "55" Block { BlockType Inport Name "M" @@ -4018,25 +4616,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4518::51" + SID "4518::54" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 41 + ZOrder 44 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4518::50" + SID "4518::53" Tag "Stateflow S-Function torqueBalancingYoga 6" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 40 + ZOrder 43 FunctionName "sf_sfun" Parameters "Config" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -4046,9 +4644,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4518::52" + SID "4518::55" Position [460, 241, 480, 259] - ZOrder 42 + ZOrder 45 } Block { BlockType Outport @@ -4059,7 +4657,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 54 + ZOrder 58 SrcBlock "M" SrcPort 1 DstBlock " SFunction " @@ -4067,7 +4665,7 @@ Model { } Line { Name "M_with_inertia" - ZOrder 55 + ZOrder 59 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -4075,14 +4673,14 @@ Model { DstPort 1 } Line { - ZOrder 56 + ZOrder 60 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 57 + ZOrder 61 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -4124,6 +4722,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" SourceType "Centroidal Momentum" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -4162,6 +4761,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Get Bias Forces" SourceType "Get Generalized Bias Forces" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -4174,6 +4774,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Mass Matrix" SourceType "Mass Matrix" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -4348,6 +4949,7 @@ Model { Name "Kinematics" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4422,6 +5024,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" SourceType "DotJ Nu" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -4435,6 +5038,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" SourceType "DotJ Nu" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -4449,6 +5053,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.COM" } @@ -4463,6 +5068,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -4477,6 +5083,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -4491,6 +5098,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -4505,6 +5113,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -4522,6 +5131,7 @@ Model { Name "xCom" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4561,6 +5171,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.COM" } @@ -5197,8 +5808,9 @@ Model { Variant off System { Name "Robot State and References" - Location [326, 394, 953, 1032] + Location [65, 24, 1918, 1060] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5209,7 +5821,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "39" + ZoomFactor "125" Block { BlockType Reference Name "Compare\nTo Zero" @@ -5221,6 +5833,8 @@ Model { LibraryVersion "1.401" SourceBlock "simulink/Logic and Bit\nOperations/Compare\nTo Zero" SourceType "Compare To Zero" + SourceProductName "Simulink" + SourceProductBaseCode "SL" ContentPreviewEnabled off relop "~=" OutDataTypeStr "boolean" @@ -5237,6 +5851,8 @@ Model { LibraryVersion "1.401" SourceBlock "simulink/Logic and Bit\nOperations/Compare\nTo Zero" SourceType "Compare To Zero" + SourceProductName "Simulink" + SourceProductBaseCode "SL" ContentPreviewEnabled off relop "~=" OutDataTypeStr "boolean" @@ -5275,6 +5891,8 @@ Model { LibraryVersion "1.401" SourceBlock "simulink/Logic and Bit\nOperations/Bitwise\nOperator" SourceType "Bitwise Operator" + SourceProductName "Simulink" + SourceProductBaseCode "SL" logicop "AND" UseBitMask on NumInputPorts "1" @@ -5292,6 +5910,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" SourceType "Get Measurement" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off measuredType "Joints Position" } @@ -5344,6 +5963,7 @@ Model { Name "Internal Coordinator" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5384,6 +6004,7 @@ Model { Name "Base to fixed_link" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5443,6 +6064,7 @@ Model { Name "LFoot to base link transform " Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5499,6 +6121,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.IMU" } @@ -5512,6 +6135,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.BASE" } @@ -5562,6 +6186,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + SourceProductName "Whole Body Toolbox" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -5609,6 +6234,7 @@ Model { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5620,7 +6246,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3767" + SIDHighWatermark "3770" SIDPrevWatermark "9" Block { BlockType Inport @@ -5678,25 +6304,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4059::3766" + SID "4059::3769" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 166 + ZOrder 169 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4059::3765" + SID "4059::3768" Tag "Stateflow S-Function torqueBalancingYoga 2" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 165 + ZOrder 168 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -5706,9 +6332,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4059::3767" + SID "4059::3770" Position [460, 241, 480, 259] - ZOrder 167 + ZOrder 170 } Block { BlockType Outport @@ -5719,42 +6345,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 127 + ZOrder 136 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 128 + ZOrder 137 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 129 + ZOrder 138 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 130 + ZOrder 139 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 131 + ZOrder 140 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 132 + ZOrder 141 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -5762,7 +6388,7 @@ Model { } Line { Name "w_H_b" - ZOrder 133 + ZOrder 142 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -5770,14 +6396,14 @@ Model { DstPort 1 } Line { - ZOrder 134 + ZOrder 143 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 135 + ZOrder 144 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -5797,6 +6423,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -5811,6 +6438,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -5826,6 +6454,7 @@ Model { Name "neck transform" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -6150,6 +6779,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -6163,6 +6793,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -6298,6 +6929,7 @@ Model { Name "Compute State Velocity" Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -6354,6 +6986,7 @@ Model { Name "Compute Base Velocity" Location [19, 45, 910, 1950] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -6365,7 +6998,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3776" + SIDHighWatermark "3779" Block { BlockType Inport Name "J_LeftFoot" @@ -6404,25 +7037,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4090::3775" + SID "4090::3778" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 144 + ZOrder 147 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4090::3774" + SID "4090::3777" Tag "Stateflow S-Function torqueBalancingYoga 5" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 143 + ZOrder 146 FunctionName "sf_sfun" Parameters "Reg" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -6432,9 +7065,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4090::3776" + SID "4090::3779" Position [460, 241, 480, 259] - ZOrder 145 + ZOrder 148 } Block { BlockType Outport @@ -6445,28 +7078,28 @@ Model { IconDisplay "Port number" } Line { - ZOrder 99 + ZOrder 106 SrcBlock "J_LeftFoot" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 100 + ZOrder 107 SrcBlock "J_RightFoot" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 101 + ZOrder 108 SrcBlock "feetInContact" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 102 + ZOrder 109 SrcBlock "qjDot" SrcPort 1 DstBlock " SFunction " @@ -6474,7 +7107,7 @@ Model { } Line { Name "nu_b" - ZOrder 103 + ZOrder 110 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -6482,14 +7115,14 @@ Model { DstPort 1 } Line { - ZOrder 104 + ZOrder 111 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 105 + ZOrder 112 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -6510,6 +7143,7 @@ Model { Name "Feet Jacobians" Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -6548,6 +7182,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -6561,6 +7196,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -6642,6 +7278,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" SourceType "Get Measurement" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off measuredType "Joints Velocity" } @@ -6805,6 +7442,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + SourceProductName "Whole Body Toolbox" portName "Ports.IMU" signalSize "12" timeout "0.5" @@ -6843,6 +7481,7 @@ Model { Name "Reference Generator CoM" Location [53, 45, 896, 1715] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -6854,7 +7493,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3752" + SIDHighWatermark "3755" Block { BlockType Inport Name "pos_CoM_0" @@ -6875,25 +7514,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "2134::3751" + SID "2134::3754" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 154 + ZOrder 157 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2134::3750" + SID "2134::3753" Tag "Stateflow S-Function torqueBalancingYoga 10" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 153 + ZOrder 156 FunctionName "sf_sfun" Parameters "Config" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -6903,9 +7542,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2134::3752" + SID "2134::3755" Position [460, 241, 480, 259] - ZOrder 155 + ZOrder 158 } Block { BlockType Outport @@ -6916,7 +7555,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 71 + ZOrder 76 SrcBlock "pos_CoM_0" SrcPort 1 Points [120, 0] @@ -6924,7 +7563,7 @@ Model { DstPort 1 } Line { - ZOrder 72 + ZOrder 77 SrcBlock "t" SrcPort 1 DstBlock " SFunction " @@ -6932,7 +7571,7 @@ Model { } Line { Name "references_CoM" - ZOrder 73 + ZOrder 78 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -6940,14 +7579,14 @@ Model { DstPort 1 } Line { - ZOrder 74 + ZOrder 79 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 75 + ZOrder 80 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -6988,6 +7627,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -7002,6 +7642,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -7027,6 +7668,7 @@ Model { Name "xCom" Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -7066,6 +7708,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.COM" } @@ -7479,6 +8122,7 @@ Model { Name "Select State and References" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -7556,6 +8200,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" SourceType "Minimum Jerk Trajectory Generator" + SourceProductName "Whole Body Toolbox" externalSettlingTime on settlingTime "0.01" sampleTime "Config.Ts" @@ -7575,6 +8220,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" SourceType "Minimum Jerk Trajectory Generator" + SourceProductName "Whole Body Toolbox" externalSettlingTime on settlingTime "0.01" sampleTime "Config.Ts" @@ -7700,6 +8346,7 @@ Model { Name "Visualize Gain Tuning " Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -8850,6 +9497,7 @@ Model { Name "State Machine Yoga" Location [65, -4, 1920, 1080] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -8899,6 +9547,7 @@ Model { Name "Compute State Velocity" Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -8955,6 +9604,7 @@ Model { Name "Compute Base Velocity" Location [19, 45, 910, 1950] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -8966,7 +9616,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3776" + SIDHighWatermark "3779" Block { BlockType Inport Name "J_LeftFoot" @@ -9005,25 +9655,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4215::3775" + SID "4215::3778" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 144 + ZOrder 147 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4215::3774" + SID "4215::3777" Tag "Stateflow S-Function torqueBalancingYoga 7" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 143 + ZOrder 146 FunctionName "sf_sfun" Parameters "Reg" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -9033,9 +9683,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4215::3776" + SID "4215::3779" Position [460, 241, 480, 259] - ZOrder 145 + ZOrder 148 } Block { BlockType Outport @@ -9046,28 +9696,28 @@ Model { IconDisplay "Port number" } Line { - ZOrder 99 + ZOrder 106 SrcBlock "J_LeftFoot" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 100 + ZOrder 107 SrcBlock "J_RightFoot" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 101 + ZOrder 108 SrcBlock "feetInContact" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 102 + ZOrder 109 SrcBlock "qjDot" SrcPort 1 DstBlock " SFunction " @@ -9075,7 +9725,7 @@ Model { } Line { Name "nu_b" - ZOrder 103 + ZOrder 110 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -9083,14 +9733,14 @@ Model { DstPort 1 } Line { - ZOrder 104 + ZOrder 111 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 105 + ZOrder 112 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -9111,6 +9761,7 @@ Model { Name "Feet Jacobians" Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -9149,6 +9800,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -9162,6 +9814,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -9243,6 +9896,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" SourceType "Get Measurement" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off measuredType "Joints Velocity" } @@ -9346,6 +10000,7 @@ Model { Name "Compute base to fixed link transform" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -9396,6 +10051,7 @@ Model { Name "LFoot to base link transform " Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -9452,6 +10108,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.IMU" } @@ -9465,6 +10122,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.BASE" } @@ -9515,6 +10173,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + SourceProductName "Whole Body Toolbox" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -9562,6 +10221,7 @@ Model { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -9573,7 +10233,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3767" + SIDHighWatermark "3770" SIDPrevWatermark "9" Block { BlockType Inport @@ -9631,25 +10291,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4257::3766" + SID "4257::3769" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 166 + ZOrder 169 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4257::3765" + SID "4257::3768" Tag "Stateflow S-Function torqueBalancingYoga 8" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 165 + ZOrder 168 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -9659,9 +10319,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4257::3767" + SID "4257::3770" Position [460, 241, 480, 259] - ZOrder 167 + ZOrder 170 } Block { BlockType Outport @@ -9672,42 +10332,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 127 + ZOrder 136 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 128 + ZOrder 137 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 129 + ZOrder 138 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 130 + ZOrder 139 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 131 + ZOrder 140 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 132 + ZOrder 141 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -9715,7 +10375,7 @@ Model { } Line { Name "w_H_b" - ZOrder 133 + ZOrder 142 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -9723,14 +10383,14 @@ Model { DstPort 1 } Line { - ZOrder 134 + ZOrder 143 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 135 + ZOrder 144 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -9750,6 +10410,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -9764,6 +10425,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -9779,6 +10441,7 @@ Model { Name "neck transform" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -10102,6 +10765,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -10118,6 +10782,7 @@ Model { Name "RFoot to base link transform" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -10174,6 +10839,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.IMU" } @@ -10187,6 +10853,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.BASE" } @@ -10237,6 +10904,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + SourceProductName "Whole Body Toolbox" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -10284,6 +10952,7 @@ Model { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -10295,7 +10964,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3767" + SIDHighWatermark "3770" SIDPrevWatermark "9" Block { BlockType Inport @@ -10353,25 +11022,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4282::3766" + SID "4282::3769" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 166 + ZOrder 169 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4282::3765" + SID "4282::3768" Tag "Stateflow S-Function torqueBalancingYoga 3" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 165 + ZOrder 168 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -10381,9 +11050,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4282::3767" + SID "4282::3770" Position [460, 241, 480, 259] - ZOrder 167 + ZOrder 170 } Block { BlockType Outport @@ -10394,42 +11063,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 127 + ZOrder 136 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 128 + ZOrder 137 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 129 + ZOrder 138 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 130 + ZOrder 139 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 131 + ZOrder 140 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 132 + ZOrder 141 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -10437,7 +11106,7 @@ Model { } Line { Name "w_H_b" - ZOrder 133 + ZOrder 142 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -10445,14 +11114,14 @@ Model { DstPort 1 } Line { - ZOrder 134 + ZOrder 143 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 135 + ZOrder 144 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -10472,6 +11141,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -10486,6 +11156,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -10501,6 +11172,7 @@ Model { Name "neck transform" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -10824,6 +11496,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -11001,6 +11674,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" SourceType "Minimum Jerk Trajectory Generator" + SourceProductName "Whole Body Toolbox" externalSettlingTime off settlingTime "Gain.SmoothingTimeGainScheduling" sampleTime "Config.Ts" @@ -11072,6 +11746,7 @@ Model { Name "Visualise extrnal wrenches " Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -11331,6 +12006,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -11345,6 +12021,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -11358,6 +12035,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + SourceProductName "Whole Body Toolbox" portName "Ports.IMU" signalSize "12" timeout "0.5" @@ -11378,6 +12056,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + SourceProductName "Whole Body Toolbox" portName "Ports.WRENCH_LEFT_FOOT" signalSize "6" timeout "0.5" @@ -11402,6 +12081,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + SourceProductName "Whole Body Toolbox" portName "Ports.WRENCH_RIGHT_FOOT" signalSize "6" timeout "0.5" @@ -11438,6 +12118,7 @@ Model { Name "stateMachineYogaFCN" Location [219, 337, 814, 775] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -11449,7 +12130,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3753" + SIDHighWatermark "3756" SIDPrevWatermark "9" Block { BlockType Inport @@ -11543,25 +12224,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "2220::3752" + SID "2220::3755" Ports [1, 1] Position [270, 460, 320, 500] - ZOrder 150 + ZOrder 153 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2220::3751" + SID "2220::3754" Tag "Stateflow S-Function torqueBalancingYoga 13" Ports [10, 10] Position [180, 100, 230, 320] - ZOrder 149 + ZOrder 152 FunctionName "sf_sfun" Parameters "Gain,Sm" PortCounts "[10 10]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -11603,9 +12284,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2220::3753" + SID "2220::3756" Position [460, 471, 480, 489] - ZOrder 151 + ZOrder 154 } Block { BlockType Outport @@ -11688,70 +12369,70 @@ Model { IconDisplay "Port number" } Line { - ZOrder 443 + ZOrder 464 SrcBlock "wrench_rightFoot" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 444 + ZOrder 465 SrcBlock "wrench_leftFoot" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 445 + ZOrder 466 SrcBlock "CoM_0" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 446 + ZOrder 467 SrcBlock "q0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 447 + ZOrder 468 SrcBlock "l_sole_CoM" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 448 + ZOrder 469 SrcBlock "r_sole_CoM" SrcPort 1 DstBlock " SFunction " DstPort 6 } Line { - ZOrder 449 + ZOrder 470 SrcBlock "qj" SrcPort 1 DstBlock " SFunction " DstPort 7 } Line { - ZOrder 450 + ZOrder 471 SrcBlock "t" SrcPort 1 DstBlock " SFunction " DstPort 8 } Line { - ZOrder 451 + ZOrder 472 SrcBlock "l_sole_H_b" SrcPort 1 DstBlock " SFunction " DstPort 9 } Line { - ZOrder 452 + ZOrder 473 SrcBlock "r_sole_H_b" SrcPort 1 DstBlock " SFunction " @@ -11759,7 +12440,7 @@ Model { } Line { Name "w_H_b" - ZOrder 453 + ZOrder 474 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -11768,7 +12449,7 @@ Model { } Line { Name "CoM_des" - ZOrder 454 + ZOrder 475 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -11777,7 +12458,7 @@ Model { } Line { Name "qj_des" - ZOrder 455 + ZOrder 476 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -11786,7 +12467,7 @@ Model { } Line { Name "constraints" - ZOrder 456 + ZOrder 477 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -11795,7 +12476,7 @@ Model { } Line { Name "impedances" - ZOrder 457 + ZOrder 478 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -11804,7 +12485,7 @@ Model { } Line { Name "KPCoM" - ZOrder 458 + ZOrder 479 Labels [0, 0] SrcBlock " SFunction " SrcPort 7 @@ -11813,7 +12494,7 @@ Model { } Line { Name "KDCoM" - ZOrder 459 + ZOrder 480 Labels [0, 0] SrcBlock " SFunction " SrcPort 8 @@ -11822,7 +12503,7 @@ Model { } Line { Name "currentState" - ZOrder 460 + ZOrder 481 Labels [0, 0] SrcBlock " SFunction " SrcPort 9 @@ -11831,7 +12512,7 @@ Model { } Line { Name "jointsSmoothingTime" - ZOrder 461 + ZOrder 482 Labels [0, 0] SrcBlock " SFunction " SrcPort 10 @@ -11839,14 +12520,14 @@ Model { DstPort 1 } Line { - ZOrder 462 + ZOrder 483 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 463 + ZOrder 484 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -11869,6 +12550,7 @@ Model { Name "xCom" Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -11908,6 +12590,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.COM" } @@ -11977,6 +12660,7 @@ Model { Name "xCom1" Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -12016,6 +12700,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.COM" } @@ -12535,6 +13220,8 @@ Model { LibraryVersion "1.401" SourceBlock "simulink/Logic and Bit\nOperations/Bitwise\nOperator" SourceType "Bitwise Operator" + SourceProductName "Simulink" + SourceProductBaseCode "SL" logicop "AND" UseBitMask on NumInputPorts "1" @@ -13021,6 +13708,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyActuators/Set References" SourceType "Set References" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off controlType "Torque" refSpeed "50" @@ -13038,8 +13726,9 @@ Model { Variant off System { Name "controller_QP" - Location [65, 24, 2560, 1440] + Location [65, 24, 1920, 1080] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -13050,7 +13739,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "108" + ZoomFactor "77" Block { BlockType Inport Name "M" @@ -13267,6 +13956,7 @@ Model { Name "Balancing Controller\n" Location [219, 337, 814, 775] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -13278,7 +13968,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3752" + SIDHighWatermark "3755" Block { BlockType Inport Name "constraints" @@ -13488,25 +14178,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "2407::3751" + SID "2407::3754" Ports [1, 1] Position [270, 640, 320, 680] - ZOrder 154 + ZOrder 157 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2407::3750" + SID "2407::3753" Tag "Stateflow S-Function torqueBalancingYoga 17" Ports [23, 15] Position [180, 65, 230, 545] - ZOrder 153 + ZOrder 156 FunctionName "sf_sfun" Parameters "Gain,Reg" PortCounts "[23 15]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -13568,9 +14258,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2407::3752" + SID "2407::3755" Position [460, 651, 480, 669] - ZOrder 155 + ZOrder 158 } Block { BlockType Outport @@ -13698,161 +14388,161 @@ Model { IconDisplay "Port number" } Line { - ZOrder 879 + ZOrder 918 SrcBlock "constraints" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 880 + ZOrder 919 SrcBlock "ROBOT_DOF_FOR_SIMULINK" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 881 + ZOrder 920 SrcBlock "ConstraintsMatrix" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 882 + ZOrder 921 SrcBlock "bVectorConstraints" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 883 + ZOrder 922 SrcBlock "qj" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 884 + ZOrder 923 SrcBlock "qjDes" SrcPort 1 DstBlock " SFunction " DstPort 6 } Line { - ZOrder 885 + ZOrder 924 SrcBlock "nu" SrcPort 1 DstBlock " SFunction " DstPort 7 } Line { - ZOrder 886 + ZOrder 925 SrcBlock "M" SrcPort 1 DstBlock " SFunction " DstPort 8 } Line { - ZOrder 887 + ZOrder 926 SrcBlock "h" SrcPort 1 DstBlock " SFunction " DstPort 9 } Line { - ZOrder 888 + ZOrder 927 SrcBlock "L" SrcPort 1 DstBlock " SFunction " DstPort 10 } Line { - ZOrder 889 + ZOrder 928 SrcBlock "intLw" SrcPort 1 DstBlock " SFunction " DstPort 11 } Line { - ZOrder 890 + ZOrder 929 SrcBlock "w_H_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 12 } Line { - ZOrder 891 + ZOrder 930 SrcBlock "w_H_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 13 } Line { - ZOrder 892 + ZOrder 931 SrcBlock "JL" SrcPort 1 DstBlock " SFunction " DstPort 14 } Line { - ZOrder 893 + ZOrder 932 SrcBlock "JR" SrcPort 1 DstBlock " SFunction " DstPort 15 } Line { - ZOrder 894 + ZOrder 933 SrcBlock "dJLv" SrcPort 1 DstBlock " SFunction " DstPort 16 } Line { - ZOrder 895 + ZOrder 934 SrcBlock "dJRv" SrcPort 1 DstBlock " SFunction " DstPort 17 } Line { - ZOrder 896 + ZOrder 935 SrcBlock "xCoM" SrcPort 1 DstBlock " SFunction " DstPort 18 } Line { - ZOrder 897 + ZOrder 936 SrcBlock "J_CoM" SrcPort 1 DstBlock " SFunction " DstPort 19 } Line { - ZOrder 898 + ZOrder 937 SrcBlock "desired_x_dx_ddx_CoM" SrcPort 1 DstBlock " SFunction " DstPort 20 } Line { - ZOrder 899 + ZOrder 938 SrcBlock "gainsPCOM" SrcPort 1 DstBlock " SFunction " DstPort 21 } Line { - ZOrder 900 + ZOrder 939 SrcBlock "gainsDCOM" SrcPort 1 DstBlock " SFunction " DstPort 22 } Line { - ZOrder 901 + ZOrder 940 SrcBlock "impedances" SrcPort 1 DstBlock " SFunction " @@ -13860,7 +14550,7 @@ Model { } Line { Name "tauModel" - ZOrder 902 + ZOrder 941 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -13869,7 +14559,7 @@ Model { } Line { Name "Sigma" - ZOrder 903 + ZOrder 942 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -13878,7 +14568,7 @@ Model { } Line { Name "NA" - ZOrder 904 + ZOrder 943 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -13887,7 +14577,7 @@ Model { } Line { Name "fLdotDesC1C2" - ZOrder 905 + ZOrder 944 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -13896,7 +14586,7 @@ Model { } Line { Name "HessianMatrixQP1Foot" - ZOrder 906 + ZOrder 945 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -13905,7 +14595,7 @@ Model { } Line { Name "gradientQP1Foot" - ZOrder 907 + ZOrder 946 Labels [0, 0] SrcBlock " SFunction " SrcPort 7 @@ -13914,7 +14604,7 @@ Model { } Line { Name "ConstraintsMatrixQP1Foot" - ZOrder 908 + ZOrder 947 Labels [0, 0] SrcBlock " SFunction " SrcPort 8 @@ -13923,7 +14613,7 @@ Model { } Line { Name "bVectorConstraintsQp1Foot" - ZOrder 909 + ZOrder 948 Labels [0, 0] SrcBlock " SFunction " SrcPort 9 @@ -13932,7 +14622,7 @@ Model { } Line { Name "HessianMatrixQP2Feet" - ZOrder 910 + ZOrder 949 Labels [0, 0] SrcBlock " SFunction " SrcPort 10 @@ -13941,7 +14631,7 @@ Model { } Line { Name "gradientQP2Feet" - ZOrder 911 + ZOrder 950 Labels [0, 0] SrcBlock " SFunction " SrcPort 11 @@ -13950,7 +14640,7 @@ Model { } Line { Name "ConstraintsMatrixQP2Feet" - ZOrder 912 + ZOrder 951 Labels [0, 0] SrcBlock " SFunction " SrcPort 12 @@ -13959,7 +14649,7 @@ Model { } Line { Name "bVectorConstraintsQp2Feet" - ZOrder 913 + ZOrder 952 Labels [0, 0] SrcBlock " SFunction " SrcPort 13 @@ -13968,7 +14658,7 @@ Model { } Line { Name "errorCoM" - ZOrder 914 + ZOrder 953 Labels [0, 0] SrcBlock " SFunction " SrcPort 14 @@ -13977,7 +14667,7 @@ Model { } Line { Name "f_noQP" - ZOrder 915 + ZOrder 954 Labels [0, 0] SrcBlock " SFunction " SrcPort 15 @@ -13985,14 +14675,14 @@ Model { DstPort 1 } Line { - ZOrder 916 + ZOrder 955 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 917 + ZOrder 956 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -14013,6 +14703,7 @@ Model { Name "Compute angular momentum integral" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -14069,6 +14760,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" SourceType "Centroidal Momentum" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -14085,6 +14777,7 @@ Model { Name "Compute base to fixed link transform" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -14135,6 +14828,7 @@ Model { Name "LFoot to base link transform " Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -14191,6 +14885,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.IMU" } @@ -14204,6 +14899,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.BASE" } @@ -14254,6 +14950,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + SourceProductName "Whole Body Toolbox" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -14301,6 +14998,7 @@ Model { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -14312,7 +15010,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3767" + SIDHighWatermark "3770" SIDPrevWatermark "9" Block { BlockType Inport @@ -14370,25 +15068,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4438::3766" + SID "4438::3769" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 166 + ZOrder 169 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4438::3765" + SID "4438::3768" Tag "Stateflow S-Function torqueBalancingYoga 1" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 165 + ZOrder 168 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -14398,9 +15096,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4438::3767" + SID "4438::3770" Position [460, 241, 480, 259] - ZOrder 167 + ZOrder 170 } Block { BlockType Outport @@ -14411,42 +15109,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 127 + ZOrder 136 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 128 + ZOrder 137 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 129 + ZOrder 138 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 130 + ZOrder 139 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 131 + ZOrder 140 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 132 + ZOrder 141 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -14454,7 +15152,7 @@ Model { } Line { Name "w_H_b" - ZOrder 133 + ZOrder 142 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -14462,14 +15160,14 @@ Model { DstPort 1 } Line { - ZOrder 134 + ZOrder 143 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 135 + ZOrder 144 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -14489,6 +15187,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -14503,6 +15202,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -14518,6 +15218,7 @@ Model { Name "neck transform" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -14841,6 +15542,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -14857,6 +15559,7 @@ Model { Name "RFoot to base link transform" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -14913,6 +15616,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.IMU" } @@ -14926,6 +15630,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.BASE" } @@ -14976,6 +15681,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + SourceProductName "Whole Body Toolbox" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -15023,6 +15729,7 @@ Model { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -15034,7 +15741,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3767" + SIDHighWatermark "3770" SIDPrevWatermark "9" Block { BlockType Inport @@ -15092,25 +15799,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4463::3766" + SID "4463::3769" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 166 + ZOrder 169 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4463::3765" + SID "4463::3768" Tag "Stateflow S-Function torqueBalancingYoga 4" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 165 + ZOrder 168 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -15120,9 +15827,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4463::3767" + SID "4463::3770" Position [460, 241, 480, 259] - ZOrder 167 + ZOrder 170 } Block { BlockType Outport @@ -15133,42 +15840,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 127 + ZOrder 136 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 128 + ZOrder 137 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 129 + ZOrder 138 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 130 + ZOrder 139 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 131 + ZOrder 140 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 132 + ZOrder 141 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -15176,7 +15883,7 @@ Model { } Line { Name "w_H_b" - ZOrder 133 + ZOrder 142 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -15184,14 +15891,14 @@ Model { DstPort 1 } Line { - ZOrder 134 + ZOrder 143 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 135 + ZOrder 144 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -15211,6 +15918,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -15225,6 +15933,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -15240,6 +15949,7 @@ Model { Name "neck transform" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -15563,6 +16273,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -15707,6 +16418,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -15720,6 +16432,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -15741,6 +16454,7 @@ Model { Name "References for L" Location [223, 338, 826, 833] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -15752,7 +16466,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3753" + SIDHighWatermark "3756" Block { BlockType Inport Name "qjErr" @@ -15791,25 +16505,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "3721::3752" + SID "3721::3755" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 77 + ZOrder 80 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "3721::3751" + SID "3721::3754" Tag "Stateflow S-Function torqueBalancingYoga 20" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 76 + ZOrder 79 FunctionName "sf_sfun" Parameters "Reg" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -15819,9 +16533,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "3721::3753" + SID "3721::3756" Position [460, 241, 480, 259] - ZOrder 78 + ZOrder 81 } Block { BlockType Outport @@ -15832,28 +16546,28 @@ Model { IconDisplay "Port number" } Line { - ZOrder 99 + ZOrder 106 SrcBlock "qjErr" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 100 + ZOrder 107 SrcBlock "JL" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 101 + ZOrder 108 SrcBlock "JR" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 102 + ZOrder 109 SrcBlock "activeFeetConstraints" SrcPort 1 DstBlock " SFunction " @@ -15861,7 +16575,7 @@ Model { } Line { Name "nu_b_equivalent" - ZOrder 103 + ZOrder 110 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -15869,14 +16583,14 @@ Model { DstPort 1 } Line { - ZOrder 104 + ZOrder 111 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 105 + ZOrder 112 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -15940,6 +16654,7 @@ Model { Name "choose base to world transform" Location [223, 338, 826, 833] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -15951,7 +16666,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "66" + SIDHighWatermark "69" Block { BlockType Inport Name "state" @@ -15963,24 +16678,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4027::65" + SID "4027::68" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 56 + ZOrder 59 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4027::64" + SID "4027::67" Tag "Stateflow S-Function torqueBalancingYoga 19" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 55 + ZOrder 58 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -15990,9 +16705,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4027::66" + SID "4027::69" Position [460, 241, 480, 259] - ZOrder 57 + ZOrder 60 } Block { BlockType Outport @@ -16003,7 +16718,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 57 + ZOrder 61 SrcBlock "state" SrcPort 1 DstBlock " SFunction " @@ -16011,7 +16726,7 @@ Model { } Line { Name "booleanState" - ZOrder 58 + ZOrder 62 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -16019,14 +16734,14 @@ Model { DstPort 1 } Line { - ZOrder 59 + ZOrder 63 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 60 + ZOrder 64 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -16045,6 +16760,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + SourceProductName "Whole Body Toolbox" portName "Ports.IMU" signalSize "12" timeout "0.5" @@ -16249,8 +16965,9 @@ Model { Variant off System { Name "Compute joint torques" - Location [65, 24, 2560, 1440] + Location [65, -4, 1920, 1080] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -16261,7 +16978,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "187" + ZoomFactor "137" Block { BlockType Inport Name "LEFT_RIGHT_FOOT_IN_CONTACT" @@ -16450,8 +17167,9 @@ Model { Variant off System { Name "QPSolver" - Location [65, 24, 2560, 1440] + Location [65, -4, 1920, 1080] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -16462,7 +17180,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "209" + ZoomFactor "154" Block { BlockType Inport Name "LEFT_RIGHT_FOOT_IN_CONTACT" @@ -16569,6 +17287,7 @@ Model { Name "ContactsTransition" Location [223, 338, 826, 833] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -16580,7 +17299,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "36" + SIDHighWatermark "39" Block { BlockType Inport Name "LR_FootInContact" @@ -16592,24 +17311,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4590::35" + SID "4590::38" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 26 + ZOrder 29 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4590::34" + SID "4590::37" Tag "Stateflow S-Function torqueBalancingYoga 15" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 25 + ZOrder 28 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -16619,9 +17338,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4590::36" + SID "4590::39" Position [460, 241, 480, 259] - ZOrder 27 + ZOrder 30 } Block { BlockType Outport @@ -16632,7 +17351,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 13 + ZOrder 17 SrcBlock "LR_FootInContact" SrcPort 1 DstBlock " SFunction " @@ -16640,7 +17359,7 @@ Model { } Line { Name "onOneFoot" - ZOrder 14 + ZOrder 18 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -16648,14 +17367,14 @@ Model { DstPort 1 } Line { - ZOrder 15 + ZOrder 19 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 16 + ZOrder 20 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -16707,6 +17426,7 @@ Model { Name "One Foot" Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -16778,6 +17498,7 @@ Model { Name "Analytical Solution One Foot (unconstrained)" Location [223, 338, 826, 833] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -16789,7 +17510,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "37" + SIDHighWatermark "40" Block { BlockType Inport Name "H" @@ -16810,24 +17531,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4600::36" + SID "4600::39" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 27 + ZOrder 30 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4600::35" + SID "4600::38" Tag "Stateflow S-Function torqueBalancingYoga 22" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 26 + ZOrder 29 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -16837,9 +17558,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4600::37" + SID "4600::40" Position [460, 241, 480, 259] - ZOrder 28 + ZOrder 31 } Block { BlockType Outport @@ -16850,7 +17571,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 16 + ZOrder 21 SrcBlock "H" SrcPort 1 Points [120, 0] @@ -16858,7 +17579,7 @@ Model { DstPort 1 } Line { - ZOrder 17 + ZOrder 22 SrcBlock "g" SrcPort 1 DstBlock " SFunction " @@ -16866,7 +17587,7 @@ Model { } Line { Name "analyticalSolution" - ZOrder 18 + ZOrder 23 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -16874,14 +17595,14 @@ Model { DstPort 1 } Line { - ZOrder 19 + ZOrder 24 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 20 + ZOrder 25 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -16899,6 +17620,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Match Signal Sizes" SourceType "Match Signal Sizes" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -16918,6 +17640,7 @@ Model { Name "Process QP output" Location [223, 338, 826, 833] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -16929,7 +17652,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "44" + SIDHighWatermark "47" Block { BlockType Inport Name "analyticalSolution" @@ -16959,25 +17682,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4602::43" + SID "4602::46" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 28 + ZOrder 31 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4602::42" + SID "4602::45" Tag "Stateflow S-Function torqueBalancingYoga 12" Ports [3, 2] Position [180, 100, 230, 180] - ZOrder 27 + ZOrder 30 FunctionName "sf_sfun" Parameters "Config" PortCounts "[3 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -16987,9 +17710,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4602::44" + SID "4602::47" Position [460, 241, 480, 259] - ZOrder 29 + ZOrder 32 } Block { BlockType Outport @@ -17000,21 +17723,21 @@ Model { IconDisplay "Port number" } Line { - ZOrder 19 + ZOrder 25 SrcBlock "analyticalSolution" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 20 + ZOrder 26 SrcBlock "primalSolution" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 21 + ZOrder 27 SrcBlock "QPStatus" SrcPort 1 DstBlock " SFunction " @@ -17022,7 +17745,7 @@ Model { } Line { Name "f0" - ZOrder 22 + ZOrder 28 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17030,14 +17753,14 @@ Model { DstPort 1 } Line { - ZOrder 23 + ZOrder 29 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 24 + ZOrder 30 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -17056,6 +17779,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/QP" SourceType "QP" + SourceProductName "Whole Body Toolbox" lbA off ubA on lb off @@ -17201,6 +17925,7 @@ Model { Name "Process One Foot Output" Location [223, 338, 826, 833] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -17212,7 +17937,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "37" + SIDHighWatermark "40" Block { BlockType Inport Name "primalSolution" @@ -17233,24 +17958,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4606::36" + SID "4606::39" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 27 + ZOrder 30 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4606::35" + SID "4606::38" Tag "Stateflow S-Function torqueBalancingYoga 16" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 26 + ZOrder 29 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -17260,9 +17985,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4606::37" + SID "4606::40" Position [460, 241, 480, 259] - ZOrder 28 + ZOrder 31 } Block { BlockType Outport @@ -17273,7 +17998,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 16 + ZOrder 21 SrcBlock "primalSolution" SrcPort 1 Points [120, 0] @@ -17281,7 +18006,7 @@ Model { DstPort 1 } Line { - ZOrder 17 + ZOrder 22 SrcBlock "LR_FootInContact" SrcPort 1 DstBlock " SFunction " @@ -17289,7 +18014,7 @@ Model { } Line { Name "f0_oneFoot" - ZOrder 18 + ZOrder 23 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17297,14 +18022,14 @@ Model { DstPort 1 } Line { - ZOrder 19 + ZOrder 24 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 20 + ZOrder 25 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -17335,8 +18060,9 @@ Model { Variant off System { Name "Two Feet" - Location [65, 24, 1920, 1080] + Location [65, -4, 1920, 1080] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -17408,6 +18134,7 @@ Model { Name "Analytical Solution Two Feet (unconstrained)" Location [223, 338, 826, 833] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -17419,7 +18146,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "37" + SIDHighWatermark "40" Block { BlockType Inport Name "H" @@ -17440,24 +18167,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4614::36" + SID "4614::39" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 27 + ZOrder 30 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4614::35" + SID "4614::38" Tag "Stateflow S-Function torqueBalancingYoga 21" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 26 + ZOrder 29 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -17467,9 +18194,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4614::37" + SID "4614::40" Position [460, 241, 480, 259] - ZOrder 28 + ZOrder 31 } Block { BlockType Outport @@ -17480,7 +18207,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 16 + ZOrder 21 SrcBlock "H" SrcPort 1 Points [120, 0] @@ -17488,7 +18215,7 @@ Model { DstPort 1 } Line { - ZOrder 17 + ZOrder 22 SrcBlock "g" SrcPort 1 DstBlock " SFunction " @@ -17496,7 +18223,7 @@ Model { } Line { Name "analyticalSolution" - ZOrder 18 + ZOrder 23 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17504,14 +18231,14 @@ Model { DstPort 1 } Line { - ZOrder 19 + ZOrder 24 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 20 + ZOrder 25 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -17529,6 +18256,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Match Signal Sizes" SourceType "Match Signal Sizes" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -17548,6 +18276,7 @@ Model { Name "Process QP output" Location [223, 338, 826, 833] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -17559,7 +18288,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "44" + SIDHighWatermark "47" Block { BlockType Inport Name "analyticalSolution" @@ -17589,25 +18318,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4616::43" + SID "4616::46" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 28 + ZOrder 31 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4616::42" + SID "4616::45" Tag "Stateflow S-Function torqueBalancingYoga 23" Ports [3, 2] Position [180, 100, 230, 180] - ZOrder 27 + ZOrder 30 FunctionName "sf_sfun" Parameters "Config" PortCounts "[3 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -17617,9 +18346,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4616::44" + SID "4616::47" Position [460, 241, 480, 259] - ZOrder 29 + ZOrder 32 } Block { BlockType Outport @@ -17630,21 +18359,21 @@ Model { IconDisplay "Port number" } Line { - ZOrder 19 + ZOrder 25 SrcBlock "analyticalSolution" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 20 + ZOrder 26 SrcBlock "primalSolution" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 21 + ZOrder 27 SrcBlock "QPStatus" SrcPort 1 DstBlock " SFunction " @@ -17652,7 +18381,7 @@ Model { } Line { Name "f0" - ZOrder 22 + ZOrder 28 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17660,14 +18389,14 @@ Model { DstPort 1 } Line { - ZOrder 23 + ZOrder 29 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 24 + ZOrder 30 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -17686,6 +18415,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/QP" SourceType "QP" + SourceProductName "Whole Body Toolbox" lbA off ubA on lb off @@ -17827,6 +18557,7 @@ Model { Name "Visualize eventual QP failures" Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -18343,8 +19074,9 @@ Model { Variant off System { Name "Compute joint torques (motor reflected inertia)" - Location [65, -4, 2560, 1440] + Location [65, 24, 1920, 1080] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -18355,7 +19087,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "256" + ZoomFactor "189" Block { BlockType Inport Name "u" @@ -18389,6 +19121,7 @@ Model { Name "(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" Location [223, 338, 826, 833] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -18400,36 +19133,36 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "62" + SIDHighWatermark "66" Block { BlockType Demux Name " Demux " - SID "4551::60" + SID "4551::64" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 51 + ZOrder 55 Outputs "1" } Block { BlockType Ground Name " Ground " - SID "4551::62" + SID "4551::66" Position [20, 121, 40, 139] - ZOrder 53 + ZOrder 57 } Block { BlockType S-Function Name " SFunction " - SID "4551::59" + SID "4551::63" Tag "Stateflow S-Function torqueBalancingYoga 9" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 50 + ZOrder 54 FunctionName "sf_sfun" Parameters "Config" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -18439,9 +19172,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4551::61" + SID "4551::65" Position [460, 241, 480, 259] - ZOrder 52 + ZOrder 56 } Block { BlockType Outport @@ -18453,7 +19186,7 @@ Model { } Line { Name "reflectedInertia" - ZOrder 41 + ZOrder 45 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -18461,21 +19194,21 @@ Model { DstPort 1 } Line { - ZOrder 42 + ZOrder 46 SrcBlock " Ground " SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 43 + ZOrder 47 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 44 + ZOrder 48 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -18506,8 +19239,9 @@ Model { Variant off System { Name "Check the contribution of feedforward" - Location [65, -4, 2560, 1440] + Location [65, 24, 1920, 1080] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -18518,7 +19252,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "300" + ZoomFactor "214" Block { BlockType Inport Name "feedForward" @@ -18666,56 +19400,63 @@ Model { ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" "eName','ScopeData3','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-23.63249'" - ",'MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.95301','LegendVisibility','On','" - "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" - "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" - "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " - "1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{" - "struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5" - ",'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'L" - "ineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabel" - "Real','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false" - ",'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0" - "666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83" - "1372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" - " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" - "t',2),struct('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" - "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" - "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" - "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" - "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," - "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" - "uct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNam" - "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-66.44849','MaxYL" - "imReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" - ",true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803" - "922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" - "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411764" - "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1" - " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" - "',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','" - "LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'S" - "howContent',true,'Placement',4),struct('MinYLimReal','-66.44849','MaxYLimReal','41.64929','YLabelReal','','MinYLim" - "Mag','0','MaxYLimMag','10','LegendVisibility','on','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.0" - "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[" - "]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" + "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-4.00947'," + "'MaxYLimReal','3.98016','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','4.00947','LegendVisibility','On','XGr" + "id',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980392" + "2 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058823" + "53 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0" + ".0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{str" + "uct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineSt" + "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'M" + "arker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'" + "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',3,'Line" + "Names',{{'torso:1','torso:2','torso:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-5.60547','MaxYLi" + "mReal','5.28394','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',t" + "rue,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980392" + "2],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843137254" + "9;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 " + "1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0" + "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidt" + "h',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" + "ible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," + "[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','Li" + "neWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'left_arm" + ":1','left_arm:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-7.60671','Ma" + "xYLimReal','9.00886','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGri" + "d',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745098" + "03922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784313" + "72549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117" + "647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1" + " 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Line" + "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'," + "'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" + "or',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-'" + ",'LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'righ" + "t_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-4.62" + "843','MaxYLimReal','5.18344','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',tr" + "ue,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68" + "6274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1" + "6078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882" + "35294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('C" + "olor',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','" + "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker'" + ",'none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" + "uct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineSt" + "yle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',6,'LineNames'" + ",{{'left_leg:1','left_leg:2','left_leg:3','left_leg:4','left_leg:5','left_leg:6'}},'ShowContent',true,'Placement'," + "4),struct('MinYLimReal','-6.10919','MaxYLimReal','4.2908','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Lege" + "ndVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509" + "803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764" + "706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882352" + "9 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Lin" + "ePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" + "'Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle'" + ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marke" + "r','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" + "truct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames'," + "{{}},'NumLines',6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right_leg:6" + "'}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803" "9215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392" "15686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title'" @@ -18725,37 +19466,9 @@ Model { "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Use" "rDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','9" - "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',{struct('Title','%','" - "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" - "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" - "s',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" - " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('T" - "itle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" - ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" - "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2017b','Location',[385 262 2230 1233])" + "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','" + "Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',tru" + "e,'Version','2017b')),'Version','2017b','Location',[66 78 1921 1079])" NumInputPorts "5" } Block { @@ -18769,8 +19482,8 @@ Model { ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" "eName','ScopeData2','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-23.63249'" - ",'MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.95301','LegendVisibility','On','" + "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-27.03156'" + ",'MaxYLimReal','23.45711','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.03156','LegendVisibility','On','" "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " @@ -18779,86 +19492,65 @@ Model { "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5" ",'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'L" - "ineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabel" - "Real','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false" - ",'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0" - "666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83" - "1372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" - " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" - "t',2),struct('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" - "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" - "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" - "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" - "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," - "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" - "uct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNam" - "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-66.44849','MaxYL" - "imReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" - ",true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803" - "922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" - "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411764" - "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1" - " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" - "',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','" - "LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'S" - "howContent',true,'Placement',4),struct('MinYLimReal','-66.44849','MaxYLimReal','41.64929','YLabelReal','','MinYLim" - "Mag','0','MaxYLimMag','10','LegendVisibility','on','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.0" - "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[" - "]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" - "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803" - "9215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392" - "15686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title'" - ",'%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Use" - "rDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','9" - "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',{struct('Title','%','" - "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" - "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" - "s',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" - " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('T" - "itle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" - ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" - "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2017b','Location',[385 262 2230 1233])" + "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',3,'L" + "ineNames',{{'torso:1','torso:2','torso:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-9.20326','Max" + "YLimReal','6.73322','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid" + "',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980" + "3922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843137" + "2549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352941176" + "47 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 " + "1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineW" + "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'left_" + "arm:1','left_arm:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-8.52552'," + "'MaxYLimReal','9.07858','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'Y" + "Grid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745" + "09803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784" + "31372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294" + "117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color'" + ",[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','L" + "ineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" + "Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle'," + "'-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'r" + "ight_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-2" + "1.72462','MaxYLimReal','37.25581','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGri" + "d',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.41176470588235" + "3 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0." + "0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{stru" + "ct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Ma" + "rker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" + "),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'L" + "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',6,'LineN" + "ames',{{'left_leg:1','left_leg:2','left_leg:3','left_leg:4','left_leg:5','left_leg:6'}},'ShowContent',true,'Placem" + "ent',4),struct('MinYLimReal','-51.45904','MaxYLimReal','51.5335','YLabelReal','','MinYLimMag','0','MaxYLimMag','10" + "','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68" + "6274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352" + "9411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470" + "58823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')," + "struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'Lin" + "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5" + ",'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," + "'on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannel" + "Names',{{}},'NumLines',6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','righ" + "t_leg:6'}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0]" + ",'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07" + "45098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745" + "098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]," + "'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','" + "none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" + "t('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" + "}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamp" + "les','90','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',[]),extmgr.Configuration('T" + "ools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measuremen" + "ts',true,'Version','2017b')),'Version','2017b','Location',[71 103 1926 1104])" NumInputPorts "5" } Block { @@ -18872,8 +19564,8 @@ Model { ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" "eName','ScopeData1','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-23.63249'" - ",'MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.95301','LegendVisibility','On','" + "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-29.67835'" + ",'MaxYLimReal','25.62961','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','29.67835','LegendVisibility','On','" "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " @@ -18882,86 +19574,65 @@ Model { "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5" ",'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'L" - "ineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabel" - "Real','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false" - ",'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0" - "666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83" - "1372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" - " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" - "t',2),struct('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" - "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" - "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" - "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" - "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," - "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" - "uct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNam" - "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-66.44849','MaxYL" - "imReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" - ",true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803" - "922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" - "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411764" - "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1" - " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" - "',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','" - "LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'S" - "howContent',true,'Placement',4),struct('MinYLimReal','-66.44849','MaxYLimReal','41.64929','YLabelReal','','MinYLim" - "Mag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0" + "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',3,'L" + "ineNames',{{'torso:1','torso:2','torso:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-10.1735','Max" + "YLimReal','8.43835','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid" + "',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980" + "3922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843137" + "2549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352941176" + "47 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 " + "1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineW" + "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'left_" + "arm:1','left_arm:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-10.59277'" + ",'MaxYLimReal','10.20572','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true," + "'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627" + "4509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607" + "8431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352" + "94117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Colo" + "r',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct" + "('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle" + "','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{" + "'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3),struct('MinYLimReal','" + "-21.85608','MaxYLimReal','37.79901','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XG" + "rid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098039" + "22 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882" + "353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;" + "0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{st" + "ruct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineS" + "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'" + "Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1]," + "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',6,'Lin" + "eNames',{{'left_leg:1','left_leg:2','left_leg:3','left_leg:4','left_leg:5','left_leg:6'}},'ShowContent',true,'Plac" + "ement',4),struct('MinYLimReal','-52.14583','MaxYLimReal','52.78096','YLabelReal','','MinYLimMag','0','MaxYLimMag'," + "'10','LegendVisibility','on','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.62" + "3529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176" + "47058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'" + "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth'," + "0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" + "e','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChan" + "nelNames',{{}},'NumLines',6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','r" + "ight_leg:6'}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','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.0" "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[" - "]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" - "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803" - "9215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392" - "15686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title'" - ",'%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Use" - "rDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','9" - "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',{struct('Title','%','" - "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" - "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" - "s',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" - " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('T" - "itle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" - ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" - "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2017b','Location',[385 262 2230 1233])" + "3],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker" + "','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" + "ruct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineS" + "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'" + "Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeS" + "amples','90','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',[]),extmgr.Configuration" + "('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measure" + "ments',true,'Version','2017b')),'Version','2017b','Location',[66 78 1921 1079])" NumInputPorts "5" } Line { @@ -19175,6 +19846,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" SourceType "Get Measurement" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off measuredType "Joints Acceleration" } @@ -19731,6 +20403,7 @@ Model { Name "emergency stop: joint limits" Location [65, -4, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -19777,6 +20450,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Limits" SourceType "Get Limits" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off limitsSource "ControlBoard" limitsType "Position" @@ -19800,6 +20474,7 @@ Model { Name "MATLAB Function" Location [121, 45, 868, 1245] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -19811,7 +20486,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3752" + SIDHighWatermark "3755" Block { BlockType Inport Name "umin" @@ -19850,24 +20525,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "2421::3751" + SID "2421::3754" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 101 + ZOrder 104 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2421::3750" + SID "2421::3753" Tag "Stateflow S-Function torqueBalancingYoga 18" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 100 + ZOrder 103 FunctionName "sf_sfun" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -19877,9 +20552,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2421::3752" + SID "2421::3755" Position [460, 241, 480, 259] - ZOrder 102 + ZOrder 105 } Block { BlockType Outport @@ -19890,28 +20565,28 @@ Model { IconDisplay "Port number" } Line { - ZOrder 127 + ZOrder 134 SrcBlock "umin" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 128 + ZOrder 135 SrcBlock "umax" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 129 + ZOrder 136 SrcBlock "u" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 130 + ZOrder 137 SrcBlock "tol" SrcPort 1 DstBlock " SFunction " @@ -19919,7 +20594,7 @@ Model { } Line { Name "inRange" - ZOrder 131 + ZOrder 138 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -19927,14 +20602,14 @@ Model { DstPort 1 } Line { - ZOrder 132 + ZOrder 139 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 133 + ZOrder 140 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -20011,6 +20686,7 @@ Model { Name "synchronizer" Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -20035,6 +20711,7 @@ Model { Name "GAZEBO_SYNCHRONIZER" Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -20066,6 +20743,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" SourceType "Simulator Synchronizer" + SourceProductName "Whole Body Toolbox" period "Config.Ts" serverPortName "'/clock/rpc'" clientPortName "'/WBT_synchronizer/rpc:o'" @@ -20110,6 +20788,7 @@ Model { Name "REAL_TIME_SYNC" Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -20142,6 +20821,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" + SourceProductName "Whole Body Toolbox" period "Config.Ts" } } @@ -20171,6 +20851,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Clock" SourceType "YARP Clock" + SourceProductName "Whole Body Toolbox" } Line { ZOrder 1 @@ -20219,6 +20900,7 @@ Model { Name "tauDot Saturation" Location [65, 24, 2560, 1440] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -20265,6 +20947,7 @@ Model { Name "Saturate the Torque Derivative" Location [223, 338, 826, 833] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -20276,7 +20959,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "44" + SIDHighWatermark "47" Block { BlockType Inport Name "u" @@ -20297,25 +20980,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4556::43" + SID "4556::46" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 33 + ZOrder 36 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4556::42" + SID "4556::45" Tag "Stateflow S-Function torqueBalancingYoga 11" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 32 + ZOrder 35 FunctionName "sf_sfun" Parameters "Config" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -20325,9 +21008,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4556::44" + SID "4556::47" Position [460, 241, 480, 259] - ZOrder 34 + ZOrder 37 } Block { BlockType Outport @@ -20338,7 +21021,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 55 + ZOrder 60 SrcBlock "u" SrcPort 1 Points [120, 0] @@ -20346,7 +21029,7 @@ Model { DstPort 1 } Line { - ZOrder 56 + ZOrder 61 SrcBlock "u_0" SrcPort 1 DstBlock " SFunction " @@ -20354,7 +21037,7 @@ Model { } Line { Name "uSat" - ZOrder 57 + ZOrder 62 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -20362,14 +21045,14 @@ Model { DstPort 1 } Line { - ZOrder 58 + ZOrder 63 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 59 + ZOrder 64 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -20398,6 +21081,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" + SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -20692,7 +21376,7 @@ Stateflow { name "torqueBalancingYoga" created "18-Feb-2014 10:14:40" isLibrary 0 - sfVersion 80000010 + sfVersion 80000012 firstTarget 271 } chart { From de27f80473ba7bb76d93570a99aa1f49de7f17a2 Mon Sep 17 00:00:00 2001 From: Gabriele Nava Date: Tue, 9 Oct 2018 17:10:01 +0200 Subject: [PATCH 2/5] updated model --- .../torqueBalancingYoga.mdl | 2124 ++++++----------- 1 file changed, 737 insertions(+), 1387 deletions(-) diff --git a/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl b/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl index 4b7a526..999e9db 100644 --- a/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl +++ b/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl @@ -1,12 +1,12 @@ Model { - Name "torqueBalancingYoga" - Version 9.0 + Name "torqueBalancingYoga2016" + Version 8.8 SavedCharacterEncoding "UTF-8" GraphicalInterface { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.3302" + ComputedModelVersion "1.3294" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -15,566 +15,12 @@ Model { HasInitializeEvent 0 HasTerminateEvent 0 IsExportFunctionModel 0 - NumParameterArguments 0 - NumExternalFileReferences 85 - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Configuration" - Path "torqueBalancingYoga/Configuration" - SID "4537" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingYoga/Dump and visualize/Visualizer/Get Measurement" - SID "4542" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingYoga/Dump and visualize/Visualizer/Get Measurement1" - SID "4543" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" - Path "torqueBalancingYoga/Dynamics and Kinematics/Dynamics/Centroidal Momentum" - SID "2345" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Get Bias Forces" - Path "torqueBalancingYoga/Dynamics and Kinematics/Dynamics/Get Bias Forces" - SID "2348" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Mass Matrix" - Path "torqueBalancingYoga/Dynamics and Kinematics/Dynamics/Mass Matrix" - SID "2349" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/DotJ Nu l_sole " - SID "2375" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/DotJ Nu r_sole " - SID "2376" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/Jacobian com" - SID "2378" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/Jacobian l_sole" - SID "2379" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/Jacobian r_sole" - SID "2380" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/l_sole" - SID "2405" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/r_sole" - SID "2406" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/xCom/CoM" - SID "4363" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "simulink/Logic and Bit\nOperations/Compare\nTo Zero" - Path "torqueBalancingYoga/Robot State and References/Compare To Zero" - SID "2916" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "simulink/Logic and Bit\nOperations/Compare\nTo Zero" - Path "torqueBalancingYoga/Robot State and References/Compare To Zero1" - SID "2917" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "simulink/Logic and Bit\nOperations/Bitwise\nOperator" - Path "torqueBalancingYoga/Robot State and References/Coordinator" - SID "2908" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingYoga/Robot State and References/Get Measurement" - SID "4538" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to bas" - "e link transform /Fixed base to imu transform" - SID "4052" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to bas" - "e link transform /Fixed base to root link transform" - SID "4053" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to bas" - "e link transform /Neck Position" - SID "4056" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to bas" - "e link transform /holder 1" - SID "4060" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to bas" - "e link transform /holder 2" - SID "4061" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to wor" - "ld transform (fixed base)" - SID "4071" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/RFoot to wor" - "ld transform (fixed base)" - SID "4084" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Compute State Velocity/Feet Jac" - "obians/Jacobian LFoot" - SID "4094" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Compute State Velocity/Feet Jac" - "obians/Jacobian RFoot" - SID "4095" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Compute State Velocity/Get Meas" - "urement" - SID "4540" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/IMU measurements" - SID "3222" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/holder " - SID "2109" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/holder 1" - SID "2110" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/xCom/CoM" - SID "2141" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - Path "torqueBalancingYoga/Robot State and References/Select State and References/Minimum Jerk Trajectory " - "Generator1" - SID "2152" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - Path "torqueBalancingYoga/Robot State and References/Select State and References/Minimum Jerk Trajectory " - "Generator2" - SID "2153" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute State Velocity/Feet Jacob" - "ians/Jacobian LFoot" - SID "4219" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute State Velocity/Feet Jacob" - "ians/Jacobian RFoot" - SID "4220" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute State Velocity/Get Measur" - "ement" - SID "4541" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/LFoot to base link transform /Fixed base to imu transform" - SID "4250" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/LFoot to base link transform /Fixed base to root link transform" - SID "4251" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/LFoot to base link transform /Neck Position" - SID "4254" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/LFoot to base link transform /holder 1" - SID "4258" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/LFoot to base link transform /holder 2" - SID "4259" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/LFoot to world transform (fixed base)" - SID "4269" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/RFoot to base link transform/Fixed base to imu transform" - SID "4275" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/RFoot to base link transform/Fixed base to root link transform" - SID "4276" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/RFoot to base link transform/Neck Position" - SID "4279" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/RFoot to base link transform/holder 1" - SID "4283" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/RFoot to base link transform/holder 2" - SID "4284" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/RFoot to world transform (fixed base)" - SID "4306" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Minimum Jerk Trajectory Generator" - SID "2176" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/holder 1" - SID "2187" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/holder 2" - SID "2188" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/inertial" - SID "2630" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/left_foot_wrench" - SID "2206" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/right_foot_wrench" - SID "2218" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/xCom/CoM" - SID "4229" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/xCom1/CoM" - SID "4303" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "simulink/Logic and Bit\nOperations/Bitwise\nOperator" - Path "torqueBalancingYoga/Robot State and References/Yoga" - SID "2909" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyActuators/Set References" - Path "torqueBalancingYoga/Set References" - SID "2354" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Centroidal Momentum" - SID "3718" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/LFoot to base link transform /Fixed base to imu transform" - SID "4431" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/LFoot to base link transform /Fixed base to root link transform" - SID "4432" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/LFoot to base link transform /Neck Position" - SID "4435" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/LFoot to base link transform /holder 1" - SID "4439" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/LFoot to base link transform /holder 2" - SID "4440" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/LFoot to world transform (fixed base)" - SID "4450" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/RFoot to base link transform/Fixed base to imu transform" - SID "4456" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/RFoot to base link transform/Fixed base to root link transform" - SID "4457" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/RFoot to base link transform/Neck Position" - SID "4460" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/RFoot to base link transform/holder 1" - SID "4464" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/RFoot to base link transform/holder 2" - SID "4465" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/RFoot to world transform (fixed base)" - SID "4475" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Jacobian" - SID "3719" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Jacobian1" - SID "3720" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/inertial" - SID "3724" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Match Signal Sizes" - Path "torqueBalancingYoga/controller_QP/Compute joint torques/QPSolver/One Foot/Match Signal Sizes1" - SID "4601" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/QP" - Path "torqueBalancingYoga/controller_QP/Compute joint torques/QPSolver/One Foot/QP Two Feet" - SID "4603" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Match Signal Sizes" - Path "torqueBalancingYoga/controller_QP/Compute joint torques/QPSolver/Two Feet/Match Signal Sizes" - SID "4615" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/QP" - Path "torqueBalancingYoga/controller_QP/Compute joint torques/QPSolver/Two Feet/QP Two Feet" - SID "4617" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingYoga/controller_QP/Compute joint torques (motor reflected inertia)/Get Measurement" - SID "4539" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Limits" - Path "torqueBalancingYoga/emergency stop: joint limits/Get Limits" - SID "2432" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Simulator Synchronizer" - Path "torqueBalancingYoga/synchronizer/GAZEBO_SYNCHRONIZER/Simulator Synchronizer" - SID "2426" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Real Time Synchronizer" - Path "torqueBalancingYoga/synchronizer/REAL_TIME_SYNC/Real Time Synchronizer" - SID "2431" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Clock" - Path "torqueBalancingYoga/synchronizer/Yarp Clock" - SID "4629" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/tauDot Saturation/holder " - SID "4559" - Type "LIBRARY_BLOCK" - } - OrderedModelArguments 1 } LogicAnalyzerGraphicalSettings "" LogicAnalyzerPlugin "on" LogicAnalyzerSignalOrdering "" DiagnosticSuppressor "on" SuppressorTable "22 serialization::archive 11 0 6 0 0 0 11 0" - CustomCodeFunctionData "" - SLCCPlugin "on" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on DisableAllScopes off @@ -596,7 +42,7 @@ Model { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [65.0, -4.0, 1855.0, 1084.0] + Location [65.0, 24.0, 1855.0, 1056.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -621,8 +67,8 @@ Model { ViewObjType "SimulinkTopLevel" LoadSaveID "0" Extents [1817.0, 907.0] - ZoomFactor [1.25] - Offset [559.76461888509721, 174.25984072810013] + ZoomFactor [1.3908227848101267] + Offset [559.2895335608647, 203.93401592719] } Object { $PropName "DockComponentsInfo" @@ -649,23 +95,21 @@ Model { "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" } } - HideAutomaticNames on Created "Tue Feb 18 10:13:36 2014" Creator "daniele" UpdateHistory "UpdateHistoryNever" ModifiedByFormat "%" - LastModifiedBy "icub" + LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Thu Oct 04 16:14:04 2018" - RTWModifiedTimeStamp 460564663 - ModelVersionFormat "1.%" + LastModifiedDate "Tue Oct 09 17:08:29 2018" + RTWModifiedTimeStamp 461005709 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" WideLines off ShowLineDimensions on ShowPortDataTypes off - PortDataTypeDisplayFormat "AliasTypeOnly" ShowEditTimeErrors on ShowEditTimeWarnings on ShowEditTimeAdvisorChecks off @@ -686,14 +130,12 @@ Model { BlockNameDataTip off BlockParametersDataTip off BlockDescriptionStringDataTip off - BlockVariantConditionDataTip off ToolBar on StatusBar on BrowserShowLibraryLinks on FunctionConnectors off BrowserLookUnderMasks on SimulationMode "normal" - VisualizeLoggedSignalsWhenLoggingToFile off PauseTimes "5" NumberOfSteps 1 SnapshotBufferSize 10 @@ -710,13 +152,13 @@ Model { $PropName "DataLoggingOverride" $ObjectID 7 $ClassName "Simulink.SimulationData.ModelLoggingInfo" - model_ "torqueBalancingYoga" + model_ "torqueBalancingYoga2016" signals_ [] overrideMode_ [0.0] Array { Type "Cell" Dimension 1 - Cell "torqueBalancingYoga" + Cell "torqueBalancingYoga2016" PropName "logAsSpecifiedByModels_" } Array { @@ -749,13 +191,12 @@ Model { ExtModeAutoUpdateStatusClock on ShowModelReferenceBlockVersion off ShowModelReferenceBlockIO off - OrderedModelArguments on Array { Type "Handle" Dimension 1 Simulink.ConfigSet { $ObjectID 8 - Version "1.17.1" + Version "1.16.5" DisabledProps [] Description "" Array { @@ -763,7 +204,7 @@ Model { Dimension 9 Simulink.SolverCC { $ObjectID 9 - Version "1.17.1" + Version "1.16.5" DisabledProps [] Description "" StartTime "0.0" @@ -782,6 +223,7 @@ Model { MaxConsecutiveMinStep "1" RelTol "1e-3" EnableMultiTasking on + EnableConcurrentExecution off ConcurrentTasks off Solver "FixedStepDiscrete" SolverName "FixedStepDiscrete" @@ -798,11 +240,10 @@ Model { SampleTimeConstraint "Unconstrained" InsertRTBMode "Whenever possible" SampleTimeProperty [] - DecoupledContinuousIntegration off } Simulink.DataIOCC { $ObjectID 10 - Version "1.17.1" + Version "1.16.5" DisabledProps [] Description "" Decimation "1" @@ -837,13 +278,12 @@ Model { ReturnWorkspaceOutputsName "out" Refine "1" LoggingToFile off - DatasetSignalFormat "timeseries" LoggingFileName "out.mat" LoggingIntervals "[-inf, inf]" } Simulink.OptimizationCC { $ObjectID 11 - Version "1.17.1" + Version "1.16.5" Array { Type "Cell" Dimension 8 @@ -898,14 +338,10 @@ Model { BufferReusableBoundary on SimCompilerOptimization "off" AccelVerboseBuild off - OptimizeBlockOrder "off" - OptimizeDataStoreBuffers on - BusAssignmentInplaceUpdate on - DifferentSizesBufferReuse off } Simulink.DebuggingCC { $ObjectID 12 - Version "1.17.1" + Version "1.16.5" Array { Type "Cell" Dimension 1 @@ -944,7 +380,6 @@ Model { MultiTaskRateTransMsg "error" SingleTaskRateTransMsg "none" TasksWithSamePriorityMsg "warning" - ExportedTasksRateTransMsg "none" SigSpecEnsureSampleTimeMsg "warning" CheckMatrixSingularityMsg "none" IntegerOverflowMsg "warning" @@ -986,7 +421,6 @@ Model { StateNameClashWarn "warning" SimStateInterfaceChecksumMismatchMsg "warning" SimStateOlderReleaseMsg "error" - ChecksumConsistencyForSSReuse "none" InitInArrayFormatMsg "warning" StrictBusMsg "ErrorLevel1" BusNameAdapt "WarnAndRepair" @@ -994,7 +428,6 @@ Model { SymbolicDimMinMaxWarning "warning" LossOfSymbolicDimsSimulationWarning "warning" LossOfSymbolicDimsCodeGenerationWarning "error" - SymbolicDimsDataTypeCodeGenerationDiagnostic "error" BlockIODiagnostic "none" SFUnusedDataAndEventsDiag "warning" SFUnexpectedBacktrackingDiag "warning" @@ -1013,14 +446,10 @@ Model { AllowedUnitSystems "all" UnitsInconsistencyMsg "warning" AllowAutomaticUnitConversions on - RCSCRenamedMsg "warning" - RCSCObservableMsg "warning" - ForceCombineOutputUpdateInSim off - UnitDatabase "" } Simulink.HardwareCC { $ObjectID 13 - Version "1.17.1" + Version "1.16.5" DisabledProps [] Description "" ProdBitPerChar 8 @@ -1068,7 +497,7 @@ Model { } Simulink.ModelReferenceCC { $ObjectID 14 - Version "1.17.1" + Version "1.16.5" DisabledProps [] Description "" UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" @@ -1087,7 +516,7 @@ Model { } Simulink.SFSimCC { $ObjectID 15 - Version "1.17.1" + Version "1.16.5" DisabledProps [] Description "" SimCustomSourceCode "" @@ -1111,13 +540,11 @@ Model { ModelFunctionsGlobalVisibility "on" CompileTimeRecursionLimit 50 EnableRuntimeRecursion on - MATLABDynamicMemAlloc on - MATLABDynamicMemAllocThreshold 65536 } Simulink.RTWCC { $BackupClass "Simulink.RTWCC" $ObjectID 16 - Version "1.17.1" + Version "1.16.5" Array { Type "Cell" Dimension 16 @@ -1142,7 +569,7 @@ Model { SystemTargetFile "grt.tlc" HardwareBoard "None" TLCOptions "" - GenCodeOnly off + GenCodeOnly on MakeCommand "make_rtw" GenerateMakefile on PackageGeneratedCodeAndArtifacts off @@ -1168,7 +595,6 @@ Model { CustomLibrary "" CustomDefine "" CustomLAPACKCallback "" - CustomFFTCallback "" CustomInitializer "" CustomTerminator "" Toolchain "Automatically locate an installed toolchain" @@ -1183,7 +609,7 @@ Model { CodeProfilingSaveOptions "SummaryOnly" CodeProfilingInstrumentation off SILDebugging off - TargetLang "C" + TargetLang "C++" IncludeBusHierarchyInRTWFileBlockHierarchyMap off GenerateTraceInfo off GenerateTraceReport off @@ -1204,10 +630,10 @@ Model { Dimension 2 Simulink.CodeAppCC { $ObjectID 17 - Version "1.17.1" + Version "1.16.5" Array { Type "Cell" - Dimension 27 + Dimension 28 Cell "IgnoreCustomStorageClasses" Cell "IgnoreTestpoints" Cell "InsertBlockDesc" @@ -1235,6 +661,7 @@ Model { Cell "CustomUserTokenString" Cell "CustomSymbolStrEmxType" Cell "CustomSymbolStrEmxFcn" + Cell "BlockCommentType" PropName "DisabledProps" } Description "" @@ -1253,7 +680,6 @@ Model { SFDataObjDesc off MATLABFcnDesc off MangleLength 1 - SharedChecksumLength 8 CustomSymbolStrGlobalVar "$R$N$M" CustomSymbolStrType "$N$R$M_T" CustomSymbolStrField "$N$M" @@ -1264,8 +690,6 @@ Model { CustomSymbolStrTmpVar "$N$M" CustomSymbolStrMacro "$R$N$M" CustomSymbolStrUtil "$N$C" - CustomSymbolStrEmxType "emxArray_$M$N" - CustomSymbolStrEmxFcn "emx$M$N" CustomUserTokenString "" CustomCommentsFcn "" DefineNamingRule "None" @@ -1277,7 +701,6 @@ Model { InsertBlockDesc off InsertPolySpaceComments off SimulinkBlockComments on - StateflowObjectComments on MATLABSourceComments off EnableCustomComments off InternalIdentifierFile "" @@ -1290,7 +713,7 @@ Model { Simulink.GRTTargetCC { $BackupClass "Simulink.TargetCC" $ObjectID 18 - Version "1.17.1" + Version "1.16.5" Array { Type "Cell" Dimension 17 @@ -1314,14 +737,47 @@ Model { PropName "DisabledProps" } Description "" + Array { + Type "Handle" + Dimension 1 + Simulink.CPPComponent { + $ObjectID 19 + Version "1.17.1" + Array { + Type "Cell" + Dimension 10 + Cell "Description" + Cell "Components" + Cell "Name" + Cell "GenerateDestructor" + Cell "GenerateExternalIOAccessMethods" + Cell "ParameterMemberVisibility" + Cell "InternalMemberVisibility" + Cell "GenerateParameterAccessMethods" + Cell "GenerateInternalMemberAccessMethods" + Cell "UseOperatorNewForModelRefRegistration" + PropName "DisabledProps" + } + Description "" + Name "CPPClassGenComp" + GenerateDestructor on + GenerateExternalIOAccessMethods "None" + ParameterMemberVisibility "private" + InternalMemberVisibility "private" + GenerateParameterAccessMethods "None" + GenerateInternalMemberAccessMethods "None" + UseOperatorNewForModelRefRegistration off + } + PropName "Components" + } TargetFcnLib "ansi_tfl_table_tmw.mat" TargetLibSuffix "" TargetPreCompLibLocation "" GenFloatMathFcnCalls "NOT IN USE" - TargetLangStandard "C89/C90 (ANSI)" + TargetLangStandard "C++03 (ISO)" CodeReplacementLibrary "None" UtilityFuncGeneration "Auto" - MultiwordTypeDef "System defined" + ERTMultiwordTypeDef "System defined" MultiwordLength 2048 GenerateFullHeader on InferredTypesCompatibility off @@ -1334,17 +790,17 @@ Model { ConcurrentExecutionCompliant on IncludeMdlTerminateFcn on GeneratePreprocessorConditionals "Disable all" - CombineOutputUpdateFcns off + CombineOutputUpdateFcns on CombineSignalStateStructs off SuppressErrorStatus off IncludeFileDelimiter "Auto" ERTCustomFileBanners off SupportAbsoluteTime on LogVarNameModifier "rt_" - MatFileLogging on - MultiInstanceERTCode off - CodeInterfacePackaging "Nonreusable function" - SupportNonFinite on + MatFileLogging off + MultiInstanceERTCode on + CodeInterfacePackaging "C++ class" + SupportNonFinite off SupportComplex on PurelyIntegerCode off SupportContinuousTime on @@ -1359,16 +815,11 @@ Model { CPPClassGenCompliant on AutosarCompliant off MDXCompliant off - GRTInterface on + GRTInterface off GenerateAllocFcn off UseToolchainInfoCompliant on GenerateSharedConstants on CoderGroups [] - AccessMethods [] - LookupTableObjectStructAxisOrder "1,2,3,4,..." - LUTObjectStructOrderExplicitValues "Size,Breakpoints,Table" - LUTObjectStructOrderEvenSpacing "Size,Breakpoints,Table" - ArrayLayout "Column-major" UseMalloc off ExtMode off ExtModeStaticAlloc off @@ -1389,8 +840,8 @@ Model { } } SlCovCC.ConfigComp { - $ObjectID 19 - Version "1.17.1" + $ObjectID 20 + Version "1.16.5" DisabledProps [] Description "Simulink Coverage Configuration Component" Name "Simulink Coverage" @@ -1426,14 +877,13 @@ Model { CovUseTimeInterval off CovStartTime 0 CovStopTime 0 - CovMcdcMode "Masking" } PropName "Components" } Name "Configuration" - ExtraOptions "" - CurrentDlgPage "Solver" - ConfigPrmDlgPosition [ 75, 71, 1155, 671 ] + ExtraOptions "-aBlockCommentType=\"BlockPathComment\" -aIgnoreTestpoints=0 " + CurrentDlgPage "Code Generation/Interface" + ConfigPrmDlgPosition [ 730, 510, 2252, 1380 ] } PropName "ConfigurationSets" } @@ -1443,7 +893,7 @@ Model { } Object { $PropName "DataTransfer" - $ObjectID 20 + $ObjectID 21 $ClassName "Simulink.GlobalDataTransfer" DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" @@ -1462,7 +912,6 @@ Model { FontWeight "normal" FontAngle "normal" ShowName on - HideAutomaticName on BlockRotation 0 BlockMirror off } @@ -1476,12 +925,7 @@ Model { FontSize 10 FontWeight "normal" FontAngle "normal" - MarkupType "model" UseDisplayTextAsClickCallback off - AnnotationType "note_annotation" - FixedHeight off - FixedWidth off - Interpreter "off" } LineDefaults { FontName "Helvetica" @@ -1493,7 +937,7 @@ Model { SelfModifiable "off" IconFrame "on" IconOpaque "opaque" - RunInitForIconRedraw "analyze" + RunInitForIconRedraw "off" IconRotate "none" PortRotate "default" IconUnits "autoscale" @@ -1657,8 +1101,6 @@ Model { InitialOutput "[]" MustResolveToSignalObject off OutputWhenUnConnected off - OutputWhenUnconnectedValue "0" - VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Product @@ -1740,12 +1182,10 @@ Model { MaskHideContents off SFBlockType "NONE" GeneratePreprocessorConditionals off - AllowZeroVariantControls off PropagateVariantConditions off TreatAsGroupedWhenPropagatingVariantConditions on ContentPreviewEnabled off IsWebBlock off - IsObserver off } Block { BlockType Sum @@ -1794,10 +1234,9 @@ Model { } } System { - Name "torqueBalancingYoga" - Location [65, -4, 1920, 1080] - Open on - PortBlocksUseCompactNotation off + Name "torqueBalancingYoga2016" + Location [65, 24, 1920, 1080] + Open off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1808,7 +1247,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "125" + ZoomFactor "139" ReportName "simulink-default.rpt" SIDHighWatermark "4649" Block { @@ -1821,10 +1260,15 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Configuration" SourceType "" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off ConfigSource "Workspace" ConfigObject "'WBTConfigRobot'" + RobotName "'icubSim'" + UrdfFile "'model.urdf'" + ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" + ControlBoardsNames "{'left_arm','right_arm','torso'}" + LocalName "'WBT'" + GravityVector "[0,0,-9.81]" } Block { BlockType SubSystem @@ -1840,15 +1284,14 @@ Model { Variant off Object { $PropName "MaskObject" - $ObjectID 21 + $ObjectID 22 $ClassName "Simulink.Mask" Display "disp('VISUALIZER')" } System { Name "Dump and visualize" - Location [65, 24, 1920, 1080] + Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1859,7 +1302,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "205" + ZoomFactor "287" Block { BlockType From Name "From" @@ -1982,9 +1425,8 @@ Model { Variant off System { Name "Visualizer" - Location [65, 24, 1920, 1080] + Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1995,7 +1437,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "91" + ZoomFactor "129" Block { BlockType Inport Name "tau_des" @@ -2548,7 +1990,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off measuredType "Joints Position" Port { @@ -2567,7 +2008,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off measuredType "Joints Torque" } @@ -4451,9 +3891,8 @@ Model { Variant off System { Name "Dynamics and Kinematics" - Location [65, 24, 1918, 1060] + Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4464,7 +3903,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "100" + ZoomFactor "216" Block { BlockType Inport Name "w_H_b" @@ -4504,7 +3943,6 @@ Model { Name "Dynamics" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4555,7 +3993,6 @@ Model { Name "Add motors reflected inertias" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4592,7 +4029,6 @@ Model { Name "Add motor reflected inertias" Location [223, 338, 826, 833] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4604,7 +4040,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "55" + SIDHighWatermark "58" Block { BlockType Inport Name "M" @@ -4616,25 +4052,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4518::54" + SID "4518::57" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 44 + ZOrder 47 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4518::53" - Tag "Stateflow S-Function torqueBalancingYoga 6" + SID "4518::56" + Tag "Stateflow S-Function torqueBalancingYoga2016 6" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 43 + ZOrder 46 FunctionName "sf_sfun" Parameters "Config" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -4644,9 +4080,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4518::55" + SID "4518::58" Position [460, 241, 480, 259] - ZOrder 45 + ZOrder 48 } Block { BlockType Outport @@ -4657,7 +4093,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 58 + ZOrder 62 SrcBlock "M" SrcPort 1 DstBlock " SFunction " @@ -4665,7 +4101,7 @@ Model { } Line { Name "M_with_inertia" - ZOrder 59 + ZOrder 63 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -4673,14 +4109,14 @@ Model { DstPort 1 } Line { - ZOrder 60 + ZOrder 64 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 61 + ZOrder 65 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -4722,7 +4158,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" SourceType "Centroidal Momentum" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -4761,7 +4196,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Get Bias Forces" SourceType "Get Generalized Bias Forces" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -4774,7 +4208,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Mass Matrix" SourceType "Mass Matrix" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -4949,7 +4382,6 @@ Model { Name "Kinematics" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5024,7 +4456,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" SourceType "DotJ Nu" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -5038,7 +4469,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" SourceType "DotJ Nu" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -5053,7 +4483,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.COM" } @@ -5068,7 +4497,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -5083,7 +4511,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -5098,7 +4525,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -5113,7 +4539,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -5131,7 +4556,6 @@ Model { Name "xCom" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5171,7 +4595,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.COM" } @@ -5808,9 +5231,8 @@ Model { Variant off System { Name "Robot State and References" - Location [65, 24, 1918, 1060] + Location [326, 394, 953, 1032] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5821,7 +5243,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "125" + ZoomFactor "39" Block { BlockType Reference Name "Compare\nTo Zero" @@ -5833,8 +5255,6 @@ Model { LibraryVersion "1.401" SourceBlock "simulink/Logic and Bit\nOperations/Compare\nTo Zero" SourceType "Compare To Zero" - SourceProductName "Simulink" - SourceProductBaseCode "SL" ContentPreviewEnabled off relop "~=" OutDataTypeStr "boolean" @@ -5851,8 +5271,6 @@ Model { LibraryVersion "1.401" SourceBlock "simulink/Logic and Bit\nOperations/Compare\nTo Zero" SourceType "Compare To Zero" - SourceProductName "Simulink" - SourceProductBaseCode "SL" ContentPreviewEnabled off relop "~=" OutDataTypeStr "boolean" @@ -5891,8 +5309,6 @@ Model { LibraryVersion "1.401" SourceBlock "simulink/Logic and Bit\nOperations/Bitwise\nOperator" SourceType "Bitwise Operator" - SourceProductName "Simulink" - SourceProductBaseCode "SL" logicop "AND" UseBitMask on NumInputPorts "1" @@ -5910,7 +5326,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off measuredType "Joints Position" } @@ -5963,7 +5378,6 @@ Model { Name "Internal Coordinator" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -6004,7 +5418,6 @@ Model { Name "Base to fixed_link" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -6064,7 +5477,6 @@ Model { Name "LFoot to base link transform " Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -6121,7 +5533,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.IMU" } @@ -6135,7 +5546,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.BASE" } @@ -6186,7 +5596,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -6234,7 +5643,6 @@ Model { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -6246,7 +5654,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3770" + SIDHighWatermark "3773" SIDPrevWatermark "9" Block { BlockType Inport @@ -6304,25 +5712,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4059::3769" + SID "4059::3772" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 169 + ZOrder 172 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4059::3768" - Tag "Stateflow S-Function torqueBalancingYoga 2" + SID "4059::3771" + Tag "Stateflow S-Function torqueBalancingYoga2016 2" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 168 + ZOrder 171 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -6332,9 +5740,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4059::3770" + SID "4059::3773" Position [460, 241, 480, 259] - ZOrder 170 + ZOrder 173 } Block { BlockType Outport @@ -6345,42 +5753,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 136 + ZOrder 145 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 137 + ZOrder 146 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 138 + ZOrder 147 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 139 + ZOrder 148 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 140 + ZOrder 149 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 141 + ZOrder 150 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -6388,7 +5796,7 @@ Model { } Line { Name "w_H_b" - ZOrder 142 + ZOrder 151 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -6396,14 +5804,14 @@ Model { DstPort 1 } Line { - ZOrder 143 + ZOrder 152 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 144 + ZOrder 153 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -6423,7 +5831,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -6438,7 +5845,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -6454,7 +5860,6 @@ Model { Name "neck transform" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -6779,7 +6184,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -6793,7 +6197,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -6929,7 +6332,6 @@ Model { Name "Compute State Velocity" Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -6986,7 +6388,6 @@ Model { Name "Compute Base Velocity" Location [19, 45, 910, 1950] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -6998,7 +6399,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3779" + SIDHighWatermark "3782" Block { BlockType Inport Name "J_LeftFoot" @@ -7037,25 +6438,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4090::3778" + SID "4090::3781" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 147 + ZOrder 150 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4090::3777" - Tag "Stateflow S-Function torqueBalancingYoga 5" + SID "4090::3780" + Tag "Stateflow S-Function torqueBalancingYoga2016 5" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 146 + ZOrder 149 FunctionName "sf_sfun" Parameters "Reg" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -7065,9 +6466,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4090::3779" + SID "4090::3782" Position [460, 241, 480, 259] - ZOrder 148 + ZOrder 151 } Block { BlockType Outport @@ -7078,28 +6479,28 @@ Model { IconDisplay "Port number" } Line { - ZOrder 106 + ZOrder 113 SrcBlock "J_LeftFoot" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 107 + ZOrder 114 SrcBlock "J_RightFoot" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 108 + ZOrder 115 SrcBlock "feetInContact" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 109 + ZOrder 116 SrcBlock "qjDot" SrcPort 1 DstBlock " SFunction " @@ -7107,7 +6508,7 @@ Model { } Line { Name "nu_b" - ZOrder 110 + ZOrder 117 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -7115,14 +6516,14 @@ Model { DstPort 1 } Line { - ZOrder 111 + ZOrder 118 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 112 + ZOrder 119 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -7143,7 +6544,6 @@ Model { Name "Feet Jacobians" Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -7182,7 +6582,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -7196,7 +6595,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -7278,7 +6676,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off measuredType "Joints Velocity" } @@ -7442,7 +6839,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" portName "Ports.IMU" signalSize "12" timeout "0.5" @@ -7481,7 +6877,6 @@ Model { Name "Reference Generator CoM" Location [53, 45, 896, 1715] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -7493,7 +6888,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3755" + SIDHighWatermark "3758" Block { BlockType Inport Name "pos_CoM_0" @@ -7514,25 +6909,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "2134::3754" + SID "2134::3757" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 157 + ZOrder 160 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2134::3753" - Tag "Stateflow S-Function torqueBalancingYoga 10" + SID "2134::3756" + Tag "Stateflow S-Function torqueBalancingYoga2016 10" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 156 + ZOrder 159 FunctionName "sf_sfun" Parameters "Config" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -7542,9 +6937,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2134::3755" + SID "2134::3758" Position [460, 241, 480, 259] - ZOrder 158 + ZOrder 161 } Block { BlockType Outport @@ -7555,7 +6950,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 76 + ZOrder 81 SrcBlock "pos_CoM_0" SrcPort 1 Points [120, 0] @@ -7563,7 +6958,7 @@ Model { DstPort 1 } Line { - ZOrder 77 + ZOrder 82 SrcBlock "t" SrcPort 1 DstBlock " SFunction " @@ -7571,7 +6966,7 @@ Model { } Line { Name "references_CoM" - ZOrder 78 + ZOrder 83 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -7579,14 +6974,14 @@ Model { DstPort 1 } Line { - ZOrder 79 + ZOrder 84 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 80 + ZOrder 85 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -7627,7 +7022,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -7642,7 +7036,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -7668,7 +7061,6 @@ Model { Name "xCom" Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -7708,7 +7100,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.COM" } @@ -8122,7 +7513,6 @@ Model { Name "Select State and References" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -8200,7 +7590,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" SourceType "Minimum Jerk Trajectory Generator" - SourceProductName "Whole Body Toolbox" externalSettlingTime on settlingTime "0.01" sampleTime "Config.Ts" @@ -8220,7 +7609,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" SourceType "Minimum Jerk Trajectory Generator" - SourceProductName "Whole Body Toolbox" externalSettlingTime on settlingTime "0.01" sampleTime "Config.Ts" @@ -8346,7 +7734,6 @@ Model { Name "Visualize Gain Tuning " Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -9497,7 +8884,6 @@ Model { Name "State Machine Yoga" Location [65, -4, 1920, 1080] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -9547,7 +8933,6 @@ Model { Name "Compute State Velocity" Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -9604,7 +8989,6 @@ Model { Name "Compute Base Velocity" Location [19, 45, 910, 1950] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -9616,7 +9000,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3779" + SIDHighWatermark "3782" Block { BlockType Inport Name "J_LeftFoot" @@ -9655,25 +9039,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4215::3778" + SID "4215::3781" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 147 + ZOrder 150 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4215::3777" - Tag "Stateflow S-Function torqueBalancingYoga 7" + SID "4215::3780" + Tag "Stateflow S-Function torqueBalancingYoga2016 7" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 146 + ZOrder 149 FunctionName "sf_sfun" Parameters "Reg" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -9683,9 +9067,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4215::3779" + SID "4215::3782" Position [460, 241, 480, 259] - ZOrder 148 + ZOrder 151 } Block { BlockType Outport @@ -9696,28 +9080,28 @@ Model { IconDisplay "Port number" } Line { - ZOrder 106 + ZOrder 113 SrcBlock "J_LeftFoot" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 107 + ZOrder 114 SrcBlock "J_RightFoot" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 108 + ZOrder 115 SrcBlock "feetInContact" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 109 + ZOrder 116 SrcBlock "qjDot" SrcPort 1 DstBlock " SFunction " @@ -9725,7 +9109,7 @@ Model { } Line { Name "nu_b" - ZOrder 110 + ZOrder 117 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -9733,14 +9117,14 @@ Model { DstPort 1 } Line { - ZOrder 111 + ZOrder 118 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 112 + ZOrder 119 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -9761,7 +9145,6 @@ Model { Name "Feet Jacobians" Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -9800,7 +9183,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -9814,7 +9196,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -9896,7 +9277,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off measuredType "Joints Velocity" } @@ -10000,7 +9380,6 @@ Model { Name "Compute base to fixed link transform" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -10051,7 +9430,6 @@ Model { Name "LFoot to base link transform " Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -10108,7 +9486,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.IMU" } @@ -10122,7 +9499,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.BASE" } @@ -10173,7 +9549,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -10221,7 +9596,6 @@ Model { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -10233,7 +9607,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3770" + SIDHighWatermark "3773" SIDPrevWatermark "9" Block { BlockType Inport @@ -10291,25 +9665,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4257::3769" + SID "4257::3772" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 169 + ZOrder 172 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4257::3768" - Tag "Stateflow S-Function torqueBalancingYoga 8" + SID "4257::3771" + Tag "Stateflow S-Function torqueBalancingYoga2016 8" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 168 + ZOrder 171 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -10319,9 +9693,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4257::3770" + SID "4257::3773" Position [460, 241, 480, 259] - ZOrder 170 + ZOrder 173 } Block { BlockType Outport @@ -10332,42 +9706,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 136 + ZOrder 145 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 137 + ZOrder 146 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 138 + ZOrder 147 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 139 + ZOrder 148 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 140 + ZOrder 149 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 141 + ZOrder 150 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -10375,7 +9749,7 @@ Model { } Line { Name "w_H_b" - ZOrder 142 + ZOrder 151 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -10383,14 +9757,14 @@ Model { DstPort 1 } Line { - ZOrder 143 + ZOrder 152 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 144 + ZOrder 153 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -10410,7 +9784,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -10425,7 +9798,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -10441,7 +9813,6 @@ Model { Name "neck transform" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -10765,7 +10136,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -10782,7 +10152,6 @@ Model { Name "RFoot to base link transform" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -10839,7 +10208,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.IMU" } @@ -10853,7 +10221,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.BASE" } @@ -10904,7 +10271,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -10952,7 +10318,6 @@ Model { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -10964,7 +10329,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3770" + SIDHighWatermark "3773" SIDPrevWatermark "9" Block { BlockType Inport @@ -11022,25 +10387,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4282::3769" + SID "4282::3772" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 169 + ZOrder 172 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4282::3768" - Tag "Stateflow S-Function torqueBalancingYoga 3" + SID "4282::3771" + Tag "Stateflow S-Function torqueBalancingYoga2016 3" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 168 + ZOrder 171 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -11050,9 +10415,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4282::3770" + SID "4282::3773" Position [460, 241, 480, 259] - ZOrder 170 + ZOrder 173 } Block { BlockType Outport @@ -11063,42 +10428,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 136 + ZOrder 145 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 137 + ZOrder 146 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 138 + ZOrder 147 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 139 + ZOrder 148 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 140 + ZOrder 149 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 141 + ZOrder 150 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -11106,7 +10471,7 @@ Model { } Line { Name "w_H_b" - ZOrder 142 + ZOrder 151 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -11114,14 +10479,14 @@ Model { DstPort 1 } Line { - ZOrder 143 + ZOrder 152 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 144 + ZOrder 153 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -11141,7 +10506,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -11156,7 +10520,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -11172,7 +10535,6 @@ Model { Name "neck transform" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -11496,7 +10858,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -11674,7 +11035,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" SourceType "Minimum Jerk Trajectory Generator" - SourceProductName "Whole Body Toolbox" externalSettlingTime off settlingTime "Gain.SmoothingTimeGainScheduling" sampleTime "Config.Ts" @@ -11746,7 +11106,6 @@ Model { Name "Visualise extrnal wrenches " Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -12006,7 +11365,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -12021,7 +11379,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -12035,7 +11392,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" portName "Ports.IMU" signalSize "12" timeout "0.5" @@ -12056,7 +11412,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" portName "Ports.WRENCH_LEFT_FOOT" signalSize "6" timeout "0.5" @@ -12081,7 +11436,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" portName "Ports.WRENCH_RIGHT_FOOT" signalSize "6" timeout "0.5" @@ -12118,7 +11472,6 @@ Model { Name "stateMachineYogaFCN" Location [219, 337, 814, 775] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -12130,7 +11483,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3756" + SIDHighWatermark "3759" SIDPrevWatermark "9" Block { BlockType Inport @@ -12224,25 +11577,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "2220::3755" + SID "2220::3758" Ports [1, 1] Position [270, 460, 320, 500] - ZOrder 153 + ZOrder 156 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2220::3754" - Tag "Stateflow S-Function torqueBalancingYoga 13" + SID "2220::3757" + Tag "Stateflow S-Function torqueBalancingYoga2016 13" Ports [10, 10] Position [180, 100, 230, 320] - ZOrder 152 + ZOrder 155 FunctionName "sf_sfun" Parameters "Gain,Sm" PortCounts "[10 10]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -12284,9 +11637,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2220::3756" + SID "2220::3759" Position [460, 471, 480, 489] - ZOrder 154 + ZOrder 157 } Block { BlockType Outport @@ -12369,70 +11722,70 @@ Model { IconDisplay "Port number" } Line { - ZOrder 464 + ZOrder 485 SrcBlock "wrench_rightFoot" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 465 + ZOrder 486 SrcBlock "wrench_leftFoot" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 466 + ZOrder 487 SrcBlock "CoM_0" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 467 + ZOrder 488 SrcBlock "q0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 468 + ZOrder 489 SrcBlock "l_sole_CoM" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 469 + ZOrder 490 SrcBlock "r_sole_CoM" SrcPort 1 DstBlock " SFunction " DstPort 6 } Line { - ZOrder 470 + ZOrder 491 SrcBlock "qj" SrcPort 1 DstBlock " SFunction " DstPort 7 } Line { - ZOrder 471 + ZOrder 492 SrcBlock "t" SrcPort 1 DstBlock " SFunction " DstPort 8 } Line { - ZOrder 472 + ZOrder 493 SrcBlock "l_sole_H_b" SrcPort 1 DstBlock " SFunction " DstPort 9 } Line { - ZOrder 473 + ZOrder 494 SrcBlock "r_sole_H_b" SrcPort 1 DstBlock " SFunction " @@ -12440,7 +11793,7 @@ Model { } Line { Name "w_H_b" - ZOrder 474 + ZOrder 495 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -12449,7 +11802,7 @@ Model { } Line { Name "CoM_des" - ZOrder 475 + ZOrder 496 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -12458,7 +11811,7 @@ Model { } Line { Name "qj_des" - ZOrder 476 + ZOrder 497 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -12467,7 +11820,7 @@ Model { } Line { Name "constraints" - ZOrder 477 + ZOrder 498 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -12476,7 +11829,7 @@ Model { } Line { Name "impedances" - ZOrder 478 + ZOrder 499 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -12485,7 +11838,7 @@ Model { } Line { Name "KPCoM" - ZOrder 479 + ZOrder 500 Labels [0, 0] SrcBlock " SFunction " SrcPort 7 @@ -12494,7 +11847,7 @@ Model { } Line { Name "KDCoM" - ZOrder 480 + ZOrder 501 Labels [0, 0] SrcBlock " SFunction " SrcPort 8 @@ -12503,7 +11856,7 @@ Model { } Line { Name "currentState" - ZOrder 481 + ZOrder 502 Labels [0, 0] SrcBlock " SFunction " SrcPort 9 @@ -12512,7 +11865,7 @@ Model { } Line { Name "jointsSmoothingTime" - ZOrder 482 + ZOrder 503 Labels [0, 0] SrcBlock " SFunction " SrcPort 10 @@ -12520,14 +11873,14 @@ Model { DstPort 1 } Line { - ZOrder 483 + ZOrder 504 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 484 + ZOrder 505 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -12550,7 +11903,6 @@ Model { Name "xCom" Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -12590,7 +11942,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.COM" } @@ -12660,7 +12011,6 @@ Model { Name "xCom1" Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -12700,7 +12050,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.COM" } @@ -13220,8 +12569,6 @@ Model { LibraryVersion "1.401" SourceBlock "simulink/Logic and Bit\nOperations/Bitwise\nOperator" SourceType "Bitwise Operator" - SourceProductName "Simulink" - SourceProductBaseCode "SL" logicop "AND" UseBitMask on NumInputPorts "1" @@ -13708,7 +13055,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyActuators/Set References" SourceType "Set References" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off controlType "Torque" refSpeed "50" @@ -13726,9 +13072,8 @@ Model { Variant off System { Name "controller_QP" - Location [65, 24, 1920, 1080] + Location [8, 35, 2503, 1479] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -13739,7 +13084,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "77" + ZoomFactor "108" Block { BlockType Inport Name "M" @@ -13956,7 +13301,6 @@ Model { Name "Balancing Controller\n" Location [219, 337, 814, 775] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -13968,7 +13312,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3755" + SIDHighWatermark "3758" Block { BlockType Inport Name "constraints" @@ -14178,25 +13522,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "2407::3754" + SID "2407::3757" Ports [1, 1] Position [270, 640, 320, 680] - ZOrder 157 + ZOrder 160 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2407::3753" - Tag "Stateflow S-Function torqueBalancingYoga 17" + SID "2407::3756" + Tag "Stateflow S-Function torqueBalancingYoga2016 17" Ports [23, 15] Position [180, 65, 230, 545] - ZOrder 156 + ZOrder 159 FunctionName "sf_sfun" Parameters "Gain,Reg" PortCounts "[23 15]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -14258,9 +13602,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2407::3755" + SID "2407::3758" Position [460, 651, 480, 669] - ZOrder 158 + ZOrder 161 } Block { BlockType Outport @@ -14388,161 +13732,161 @@ Model { IconDisplay "Port number" } Line { - ZOrder 918 + ZOrder 957 SrcBlock "constraints" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 919 + ZOrder 958 SrcBlock "ROBOT_DOF_FOR_SIMULINK" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 920 + ZOrder 959 SrcBlock "ConstraintsMatrix" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 921 + ZOrder 960 SrcBlock "bVectorConstraints" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 922 + ZOrder 961 SrcBlock "qj" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 923 + ZOrder 962 SrcBlock "qjDes" SrcPort 1 DstBlock " SFunction " DstPort 6 } Line { - ZOrder 924 + ZOrder 963 SrcBlock "nu" SrcPort 1 DstBlock " SFunction " DstPort 7 } Line { - ZOrder 925 + ZOrder 964 SrcBlock "M" SrcPort 1 DstBlock " SFunction " DstPort 8 } Line { - ZOrder 926 + ZOrder 965 SrcBlock "h" SrcPort 1 DstBlock " SFunction " DstPort 9 } Line { - ZOrder 927 + ZOrder 966 SrcBlock "L" SrcPort 1 DstBlock " SFunction " DstPort 10 } Line { - ZOrder 928 + ZOrder 967 SrcBlock "intLw" SrcPort 1 DstBlock " SFunction " DstPort 11 } Line { - ZOrder 929 + ZOrder 968 SrcBlock "w_H_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 12 } Line { - ZOrder 930 + ZOrder 969 SrcBlock "w_H_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 13 } Line { - ZOrder 931 + ZOrder 970 SrcBlock "JL" SrcPort 1 DstBlock " SFunction " DstPort 14 } Line { - ZOrder 932 + ZOrder 971 SrcBlock "JR" SrcPort 1 DstBlock " SFunction " DstPort 15 } Line { - ZOrder 933 + ZOrder 972 SrcBlock "dJLv" SrcPort 1 DstBlock " SFunction " DstPort 16 } Line { - ZOrder 934 + ZOrder 973 SrcBlock "dJRv" SrcPort 1 DstBlock " SFunction " DstPort 17 } Line { - ZOrder 935 + ZOrder 974 SrcBlock "xCoM" SrcPort 1 DstBlock " SFunction " DstPort 18 } Line { - ZOrder 936 + ZOrder 975 SrcBlock "J_CoM" SrcPort 1 DstBlock " SFunction " DstPort 19 } Line { - ZOrder 937 + ZOrder 976 SrcBlock "desired_x_dx_ddx_CoM" SrcPort 1 DstBlock " SFunction " DstPort 20 } Line { - ZOrder 938 + ZOrder 977 SrcBlock "gainsPCOM" SrcPort 1 DstBlock " SFunction " DstPort 21 } Line { - ZOrder 939 + ZOrder 978 SrcBlock "gainsDCOM" SrcPort 1 DstBlock " SFunction " DstPort 22 } Line { - ZOrder 940 + ZOrder 979 SrcBlock "impedances" SrcPort 1 DstBlock " SFunction " @@ -14550,7 +13894,7 @@ Model { } Line { Name "tauModel" - ZOrder 941 + ZOrder 980 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -14559,7 +13903,7 @@ Model { } Line { Name "Sigma" - ZOrder 942 + ZOrder 981 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -14568,7 +13912,7 @@ Model { } Line { Name "NA" - ZOrder 943 + ZOrder 982 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -14577,7 +13921,7 @@ Model { } Line { Name "fLdotDesC1C2" - ZOrder 944 + ZOrder 983 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -14586,7 +13930,7 @@ Model { } Line { Name "HessianMatrixQP1Foot" - ZOrder 945 + ZOrder 984 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -14595,7 +13939,7 @@ Model { } Line { Name "gradientQP1Foot" - ZOrder 946 + ZOrder 985 Labels [0, 0] SrcBlock " SFunction " SrcPort 7 @@ -14604,7 +13948,7 @@ Model { } Line { Name "ConstraintsMatrixQP1Foot" - ZOrder 947 + ZOrder 986 Labels [0, 0] SrcBlock " SFunction " SrcPort 8 @@ -14613,7 +13957,7 @@ Model { } Line { Name "bVectorConstraintsQp1Foot" - ZOrder 948 + ZOrder 987 Labels [0, 0] SrcBlock " SFunction " SrcPort 9 @@ -14622,7 +13966,7 @@ Model { } Line { Name "HessianMatrixQP2Feet" - ZOrder 949 + ZOrder 988 Labels [0, 0] SrcBlock " SFunction " SrcPort 10 @@ -14631,7 +13975,7 @@ Model { } Line { Name "gradientQP2Feet" - ZOrder 950 + ZOrder 989 Labels [0, 0] SrcBlock " SFunction " SrcPort 11 @@ -14640,7 +13984,7 @@ Model { } Line { Name "ConstraintsMatrixQP2Feet" - ZOrder 951 + ZOrder 990 Labels [0, 0] SrcBlock " SFunction " SrcPort 12 @@ -14649,7 +13993,7 @@ Model { } Line { Name "bVectorConstraintsQp2Feet" - ZOrder 952 + ZOrder 991 Labels [0, 0] SrcBlock " SFunction " SrcPort 13 @@ -14658,7 +14002,7 @@ Model { } Line { Name "errorCoM" - ZOrder 953 + ZOrder 992 Labels [0, 0] SrcBlock " SFunction " SrcPort 14 @@ -14667,7 +14011,7 @@ Model { } Line { Name "f_noQP" - ZOrder 954 + ZOrder 993 Labels [0, 0] SrcBlock " SFunction " SrcPort 15 @@ -14675,14 +14019,14 @@ Model { DstPort 1 } Line { - ZOrder 955 + ZOrder 994 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 956 + ZOrder 995 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -14703,7 +14047,6 @@ Model { Name "Compute angular momentum integral" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -14760,7 +14103,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" SourceType "Centroidal Momentum" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -14777,7 +14119,6 @@ Model { Name "Compute base to fixed link transform" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -14828,7 +14169,6 @@ Model { Name "LFoot to base link transform " Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -14885,7 +14225,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.IMU" } @@ -14899,7 +14238,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.BASE" } @@ -14950,7 +14288,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -14998,7 +14335,6 @@ Model { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -15010,7 +14346,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3770" + SIDHighWatermark "3773" SIDPrevWatermark "9" Block { BlockType Inport @@ -15068,25 +14404,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4438::3769" + SID "4438::3772" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 169 + ZOrder 172 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4438::3768" - Tag "Stateflow S-Function torqueBalancingYoga 1" + SID "4438::3771" + Tag "Stateflow S-Function torqueBalancingYoga2016 1" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 168 + ZOrder 171 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -15096,9 +14432,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4438::3770" + SID "4438::3773" Position [460, 241, 480, 259] - ZOrder 170 + ZOrder 173 } Block { BlockType Outport @@ -15109,42 +14445,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 136 + ZOrder 145 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 137 + ZOrder 146 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 138 + ZOrder 147 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 139 + ZOrder 148 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 140 + ZOrder 149 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 141 + ZOrder 150 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -15152,7 +14488,7 @@ Model { } Line { Name "w_H_b" - ZOrder 142 + ZOrder 151 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -15160,14 +14496,14 @@ Model { DstPort 1 } Line { - ZOrder 143 + ZOrder 152 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 144 + ZOrder 153 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -15187,7 +14523,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -15202,7 +14537,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -15218,7 +14552,6 @@ Model { Name "neck transform" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -15542,7 +14875,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -15559,7 +14891,6 @@ Model { Name "RFoot to base link transform" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -15616,7 +14947,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.IMU" } @@ -15630,7 +14960,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.BASE" } @@ -15681,7 +15010,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -15729,7 +15057,6 @@ Model { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -15741,7 +15068,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3770" + SIDHighWatermark "3773" SIDPrevWatermark "9" Block { BlockType Inport @@ -15799,25 +15126,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4463::3769" + SID "4463::3772" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 169 + ZOrder 172 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4463::3768" - Tag "Stateflow S-Function torqueBalancingYoga 4" + SID "4463::3771" + Tag "Stateflow S-Function torqueBalancingYoga2016 4" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 168 + ZOrder 171 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -15827,9 +15154,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4463::3770" + SID "4463::3773" Position [460, 241, 480, 259] - ZOrder 170 + ZOrder 173 } Block { BlockType Outport @@ -15840,42 +15167,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 136 + ZOrder 145 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 137 + ZOrder 146 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 138 + ZOrder 147 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 139 + ZOrder 148 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 140 + ZOrder 149 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 141 + ZOrder 150 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -15883,7 +15210,7 @@ Model { } Line { Name "w_H_b" - ZOrder 142 + ZOrder 151 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -15891,14 +15218,14 @@ Model { DstPort 1 } Line { - ZOrder 143 + ZOrder 152 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 144 + ZOrder 153 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -15918,7 +15245,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -15933,7 +15259,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -15949,7 +15274,6 @@ Model { Name "neck transform" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -16273,7 +15597,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -16418,7 +15741,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.LEFT_FOOT" } @@ -16432,7 +15754,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off frameName "Frames.RIGHT_FOOT" } @@ -16454,7 +15775,6 @@ Model { Name "References for L" Location [223, 338, 826, 833] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -16466,7 +15786,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3756" + SIDHighWatermark "3759" Block { BlockType Inport Name "qjErr" @@ -16505,25 +15825,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "3721::3755" + SID "3721::3758" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 80 + ZOrder 83 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "3721::3754" - Tag "Stateflow S-Function torqueBalancingYoga 20" + SID "3721::3757" + Tag "Stateflow S-Function torqueBalancingYoga2016 20" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 79 + ZOrder 82 FunctionName "sf_sfun" Parameters "Reg" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -16533,9 +15853,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "3721::3756" + SID "3721::3759" Position [460, 241, 480, 259] - ZOrder 81 + ZOrder 84 } Block { BlockType Outport @@ -16546,28 +15866,28 @@ Model { IconDisplay "Port number" } Line { - ZOrder 106 + ZOrder 113 SrcBlock "qjErr" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 107 + ZOrder 114 SrcBlock "JL" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 108 + ZOrder 115 SrcBlock "JR" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 109 + ZOrder 116 SrcBlock "activeFeetConstraints" SrcPort 1 DstBlock " SFunction " @@ -16575,7 +15895,7 @@ Model { } Line { Name "nu_b_equivalent" - ZOrder 110 + ZOrder 117 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -16583,14 +15903,14 @@ Model { DstPort 1 } Line { - ZOrder 111 + ZOrder 118 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 112 + ZOrder 119 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -16654,7 +15974,6 @@ Model { Name "choose base to world transform" Location [223, 338, 826, 833] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -16666,7 +15985,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "69" + SIDHighWatermark "72" Block { BlockType Inport Name "state" @@ -16678,24 +15997,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4027::68" + SID "4027::71" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 59 + ZOrder 62 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4027::67" - Tag "Stateflow S-Function torqueBalancingYoga 19" + SID "4027::70" + Tag "Stateflow S-Function torqueBalancingYoga2016 19" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 58 + ZOrder 61 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -16705,9 +16024,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4027::69" + SID "4027::72" Position [460, 241, 480, 259] - ZOrder 60 + ZOrder 63 } Block { BlockType Outport @@ -16718,7 +16037,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 61 + ZOrder 65 SrcBlock "state" SrcPort 1 DstBlock " SFunction " @@ -16726,7 +16045,7 @@ Model { } Line { Name "booleanState" - ZOrder 62 + ZOrder 66 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -16734,14 +16053,14 @@ Model { DstPort 1 } Line { - ZOrder 63 + ZOrder 67 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 64 + ZOrder 68 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -16760,7 +16079,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" portName "Ports.IMU" signalSize "12" timeout "0.5" @@ -16965,9 +16283,8 @@ Model { Variant off System { Name "Compute joint torques" - Location [65, -4, 1920, 1080] + Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -16978,7 +16295,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "137" + ZoomFactor "187" Block { BlockType Inport Name "LEFT_RIGHT_FOOT_IN_CONTACT" @@ -17167,9 +16484,8 @@ Model { Variant off System { Name "QPSolver" - Location [65, -4, 1920, 1080] + Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -17180,7 +16496,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "154" + ZoomFactor "209" Block { BlockType Inport Name "LEFT_RIGHT_FOOT_IN_CONTACT" @@ -17287,7 +16603,6 @@ Model { Name "ContactsTransition" Location [223, 338, 826, 833] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -17299,7 +16614,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "39" + SIDHighWatermark "42" Block { BlockType Inport Name "LR_FootInContact" @@ -17311,24 +16626,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4590::38" + SID "4590::41" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 29 + ZOrder 32 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4590::37" - Tag "Stateflow S-Function torqueBalancingYoga 15" + SID "4590::40" + Tag "Stateflow S-Function torqueBalancingYoga2016 15" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 28 + ZOrder 31 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -17338,9 +16653,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4590::39" + SID "4590::42" Position [460, 241, 480, 259] - ZOrder 30 + ZOrder 33 } Block { BlockType Outport @@ -17351,7 +16666,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 17 + ZOrder 21 SrcBlock "LR_FootInContact" SrcPort 1 DstBlock " SFunction " @@ -17359,7 +16674,7 @@ Model { } Line { Name "onOneFoot" - ZOrder 18 + ZOrder 22 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17367,14 +16682,14 @@ Model { DstPort 1 } Line { - ZOrder 19 + ZOrder 23 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 20 + ZOrder 24 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -17426,7 +16741,6 @@ Model { Name "One Foot" Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -17498,7 +16812,6 @@ Model { Name "Analytical Solution One Foot (unconstrained)" Location [223, 338, 826, 833] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -17510,7 +16823,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "40" + SIDHighWatermark "43" Block { BlockType Inport Name "H" @@ -17531,24 +16844,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4600::39" + SID "4600::42" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 30 + ZOrder 33 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4600::38" - Tag "Stateflow S-Function torqueBalancingYoga 22" + SID "4600::41" + Tag "Stateflow S-Function torqueBalancingYoga2016 22" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 29 + ZOrder 32 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -17558,9 +16871,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4600::40" + SID "4600::43" Position [460, 241, 480, 259] - ZOrder 31 + ZOrder 34 } Block { BlockType Outport @@ -17571,7 +16884,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 21 + ZOrder 26 SrcBlock "H" SrcPort 1 Points [120, 0] @@ -17579,7 +16892,7 @@ Model { DstPort 1 } Line { - ZOrder 22 + ZOrder 27 SrcBlock "g" SrcPort 1 DstBlock " SFunction " @@ -17587,7 +16900,7 @@ Model { } Line { Name "analyticalSolution" - ZOrder 23 + ZOrder 28 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17595,14 +16908,14 @@ Model { DstPort 1 } Line { - ZOrder 24 + ZOrder 29 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 25 + ZOrder 30 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -17620,7 +16933,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Match Signal Sizes" SourceType "Match Signal Sizes" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -17640,7 +16952,6 @@ Model { Name "Process QP output" Location [223, 338, 826, 833] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -17652,7 +16963,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "47" + SIDHighWatermark "50" Block { BlockType Inport Name "analyticalSolution" @@ -17682,25 +16993,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4602::46" + SID "4602::49" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 31 + ZOrder 34 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4602::45" - Tag "Stateflow S-Function torqueBalancingYoga 12" + SID "4602::48" + Tag "Stateflow S-Function torqueBalancingYoga2016 12" Ports [3, 2] Position [180, 100, 230, 180] - ZOrder 30 + ZOrder 33 FunctionName "sf_sfun" Parameters "Config" PortCounts "[3 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -17710,9 +17021,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4602::47" + SID "4602::50" Position [460, 241, 480, 259] - ZOrder 32 + ZOrder 35 } Block { BlockType Outport @@ -17723,21 +17034,21 @@ Model { IconDisplay "Port number" } Line { - ZOrder 25 + ZOrder 31 SrcBlock "analyticalSolution" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 26 + ZOrder 32 SrcBlock "primalSolution" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 27 + ZOrder 33 SrcBlock "QPStatus" SrcPort 1 DstBlock " SFunction " @@ -17745,7 +17056,7 @@ Model { } Line { Name "f0" - ZOrder 28 + ZOrder 34 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17753,14 +17064,14 @@ Model { DstPort 1 } Line { - ZOrder 29 + ZOrder 35 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 30 + ZOrder 36 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -17779,7 +17090,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/QP" SourceType "QP" - SourceProductName "Whole Body Toolbox" lbA off ubA on lb off @@ -17925,7 +17235,6 @@ Model { Name "Process One Foot Output" Location [223, 338, 826, 833] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -17937,7 +17246,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "40" + SIDHighWatermark "43" Block { BlockType Inport Name "primalSolution" @@ -17958,24 +17267,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4606::39" + SID "4606::42" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 30 + ZOrder 33 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4606::38" - Tag "Stateflow S-Function torqueBalancingYoga 16" + SID "4606::41" + Tag "Stateflow S-Function torqueBalancingYoga2016 16" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 29 + ZOrder 32 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -17985,9 +17294,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4606::40" + SID "4606::43" Position [460, 241, 480, 259] - ZOrder 31 + ZOrder 34 } Block { BlockType Outport @@ -17998,7 +17307,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 21 + ZOrder 26 SrcBlock "primalSolution" SrcPort 1 Points [120, 0] @@ -18006,7 +17315,7 @@ Model { DstPort 1 } Line { - ZOrder 22 + ZOrder 27 SrcBlock "LR_FootInContact" SrcPort 1 DstBlock " SFunction " @@ -18014,7 +17323,7 @@ Model { } Line { Name "f0_oneFoot" - ZOrder 23 + ZOrder 28 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -18022,14 +17331,14 @@ Model { DstPort 1 } Line { - ZOrder 24 + ZOrder 29 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 25 + ZOrder 30 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -18060,9 +17369,8 @@ Model { Variant off System { Name "Two Feet" - Location [65, -4, 1920, 1080] + Location [65, 24, 1920, 1080] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -18134,7 +17442,6 @@ Model { Name "Analytical Solution Two Feet (unconstrained)" Location [223, 338, 826, 833] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -18146,7 +17453,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "40" + SIDHighWatermark "43" Block { BlockType Inport Name "H" @@ -18167,24 +17474,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4614::39" + SID "4614::42" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 30 + ZOrder 33 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4614::38" - Tag "Stateflow S-Function torqueBalancingYoga 21" + SID "4614::41" + Tag "Stateflow S-Function torqueBalancingYoga2016 21" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 29 + ZOrder 32 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -18194,9 +17501,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4614::40" + SID "4614::43" Position [460, 241, 480, 259] - ZOrder 31 + ZOrder 34 } Block { BlockType Outport @@ -18207,7 +17514,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 21 + ZOrder 26 SrcBlock "H" SrcPort 1 Points [120, 0] @@ -18215,7 +17522,7 @@ Model { DstPort 1 } Line { - ZOrder 22 + ZOrder 27 SrcBlock "g" SrcPort 1 DstBlock " SFunction " @@ -18223,7 +17530,7 @@ Model { } Line { Name "analyticalSolution" - ZOrder 23 + ZOrder 28 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -18231,14 +17538,14 @@ Model { DstPort 1 } Line { - ZOrder 24 + ZOrder 29 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 25 + ZOrder 30 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -18256,7 +17563,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Match Signal Sizes" SourceType "Match Signal Sizes" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -18276,7 +17582,6 @@ Model { Name "Process QP output" Location [223, 338, 826, 833] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -18288,7 +17593,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "47" + SIDHighWatermark "50" Block { BlockType Inport Name "analyticalSolution" @@ -18318,25 +17623,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4616::46" + SID "4616::49" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 31 + ZOrder 34 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4616::45" - Tag "Stateflow S-Function torqueBalancingYoga 23" + SID "4616::48" + Tag "Stateflow S-Function torqueBalancingYoga2016 23" Ports [3, 2] Position [180, 100, 230, 180] - ZOrder 30 + ZOrder 33 FunctionName "sf_sfun" Parameters "Config" PortCounts "[3 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -18346,9 +17651,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4616::47" + SID "4616::50" Position [460, 241, 480, 259] - ZOrder 32 + ZOrder 35 } Block { BlockType Outport @@ -18359,21 +17664,21 @@ Model { IconDisplay "Port number" } Line { - ZOrder 25 + ZOrder 31 SrcBlock "analyticalSolution" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 26 + ZOrder 32 SrcBlock "primalSolution" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 27 + ZOrder 33 SrcBlock "QPStatus" SrcPort 1 DstBlock " SFunction " @@ -18381,7 +17686,7 @@ Model { } Line { Name "f0" - ZOrder 28 + ZOrder 34 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -18389,14 +17694,14 @@ Model { DstPort 1 } Line { - ZOrder 29 + ZOrder 35 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 30 + ZOrder 36 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -18415,7 +17720,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/QP" SourceType "QP" - SourceProductName "Whole Body Toolbox" lbA off ubA on lb off @@ -18557,7 +17861,6 @@ Model { Name "Visualize eventual QP failures" Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -19074,9 +18377,8 @@ Model { Variant off System { Name "Compute joint torques (motor reflected inertia)" - Location [65, 24, 1920, 1080] + Location [8, 35, 2503, 1479] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -19087,7 +18389,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "189" + ZoomFactor "256" Block { BlockType Inport Name "u" @@ -19121,7 +18423,6 @@ Model { Name "(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" Location [223, 338, 826, 833] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -19133,36 +18434,36 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "66" + SIDHighWatermark "70" Block { BlockType Demux Name " Demux " - SID "4551::64" + SID "4551::68" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 55 + ZOrder 59 Outputs "1" } Block { BlockType Ground Name " Ground " - SID "4551::66" + SID "4551::70" Position [20, 121, 40, 139] - ZOrder 57 + ZOrder 61 } Block { BlockType S-Function Name " SFunction " - SID "4551::63" - Tag "Stateflow S-Function torqueBalancingYoga 9" + SID "4551::67" + Tag "Stateflow S-Function torqueBalancingYoga2016 9" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 54 + ZOrder 58 FunctionName "sf_sfun" Parameters "Config" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -19172,9 +18473,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4551::65" + SID "4551::69" Position [460, 241, 480, 259] - ZOrder 56 + ZOrder 60 } Block { BlockType Outport @@ -19186,7 +18487,7 @@ Model { } Line { Name "reflectedInertia" - ZOrder 45 + ZOrder 49 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -19194,21 +18495,21 @@ Model { DstPort 1 } Line { - ZOrder 46 + ZOrder 50 SrcBlock " Ground " SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 47 + ZOrder 51 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 48 + ZOrder 52 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -19239,9 +18540,8 @@ Model { Variant off System { Name "Check the contribution of feedforward" - Location [65, 24, 1920, 1080] + Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -19252,7 +18552,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "214" + ZoomFactor "300" Block { BlockType Inport Name "feedForward" @@ -19400,63 +18700,56 @@ Model { ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" "eName','ScopeData3','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-4.00947'," - "'MaxYLimReal','3.98016','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','4.00947','LegendVisibility','On','XGr" - "id',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980392" - "2 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058823" - "53 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0" - ".0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{str" - "uct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'" - "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',3,'Line" - "Names',{{'torso:1','torso:2','torso:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-5.60547','MaxYLi" - "mReal','5.28394','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',t" - "rue,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980392" - "2],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843137254" - "9;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 " - "1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidt" - "h',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" - "ible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," - "[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','Li" - "neWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'left_arm" - ":1','left_arm:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-7.60671','Ma" - "xYLimReal','9.00886','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGri" - "d',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745098" - "03922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784313" - "72549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117" - "647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1" - " 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Line" - "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'," - "'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" - "or',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-'" - ",'LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'righ" - "t_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-4.62" - "843','MaxYLimReal','5.18344','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',tr" - "ue,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68" - "6274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1" - "6078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882" - "35294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('C" - "olor',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','" - "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker'" - ",'none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" - "uct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',6,'LineNames'" - ",{{'left_leg:1','left_leg:2','left_leg:3','left_leg:4','left_leg:5','left_leg:6'}},'ShowContent',true,'Placement'," - "4),struct('MinYLimReal','-6.10919','MaxYLimReal','4.2908','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Lege" - "ndVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509" - "803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764" - "706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882352" - "9 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Lin" - "ePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" - "truct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames'," - "{{}},'NumLines',6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right_leg:6" - "'}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" + "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-23.63249'" + ",'MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.95301','LegendVisibility','On','" + "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" + "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" + "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " + "1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{" + "struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'Lin" + "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5" + ",'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," + "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1" + "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'L" + "ineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabel" + "Real','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false" + ",'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0" + "666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83" + "1372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" + " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" + "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" + "t',2),struct('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" + "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" + "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" + "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" + "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," + "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" + "uct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" + "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'M" + "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNam" + "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-66.44849','MaxYL" + "imReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" + ",true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803" + "922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" + "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411764" + "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1" + " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" + "',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','" + "LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'S" + "howContent',true,'Placement',4),struct('MinYLimReal','-66.44849','MaxYLimReal','41.64929','YLabelReal','','MinYLim" + "Mag','0','MaxYLimMag','10','LegendVisibility','on','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.0" + "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" + "3],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[" + "]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803" "9215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392" "15686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title'" @@ -19466,9 +18759,37 @@ Model { "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Use" "rDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','9" - "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','" - "Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',tru" - "e,'Version','2017b')),'Version','2017b','Location',[66 78 1921 1079])" + "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',{struct('Title','%','" + "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" + "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" + "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" + "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" + "s',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" + "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," + "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" + "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" + ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" + "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" + "','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" + " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" + "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[" + "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" + "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" + "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('T" + "itle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" + "ne','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" + "'Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'" + ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marke" + "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" + ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" + "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" + "on','2017b','Location',[385 262 2230 1233])" NumInputPorts "5" } Block { @@ -19482,8 +18803,8 @@ Model { ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" "eName','ScopeData2','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-27.03156'" - ",'MaxYLimReal','23.45711','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.03156','LegendVisibility','On','" + "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-23.63249'" + ",'MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.95301','LegendVisibility','On','" "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " @@ -19492,65 +18813,86 @@ Model { "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5" ",'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',3,'L" - "ineNames',{{'torso:1','torso:2','torso:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-9.20326','Max" - "YLimReal','6.73322','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid" - "',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980" - "3922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843137" - "2549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352941176" - "47 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 " - "1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'left_" - "arm:1','left_arm:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-8.52552'," - "'MaxYLimReal','9.07858','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'Y" - "Grid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745" - "09803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784" - "31372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294" - "117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color'" - ",[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','L" - "ineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" - "Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle'," - "'-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'r" - "ight_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-2" - "1.72462','MaxYLimReal','37.25581','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGri" - "d',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.41176470588235" - "3 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0." - "0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{stru" - "ct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'L" - "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',6,'LineN" - "ames',{{'left_leg:1','left_leg:2','left_leg:3','left_leg:4','left_leg:5','left_leg:6'}},'ShowContent',true,'Placem" - "ent',4),struct('MinYLimReal','-51.45904','MaxYLimReal','51.5335','YLabelReal','','MinYLimMag','0','MaxYLimMag','10" - "','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68" - "6274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352" - "9411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470" - "58823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')," - "struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5" - ",'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannel" - "Names',{{}},'NumLines',6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','righ" - "t_leg:6'}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0]" - ",'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07" - "45098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745" - "098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]," - "'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','" - "none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyl" - "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" - "}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamp" - "les','90','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',[]),extmgr.Configuration('T" - "ools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measuremen" - "ts',true,'Version','2017b')),'Version','2017b','Location',[71 103 1926 1104])" + "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'L" + "ineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabel" + "Real','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false" + ",'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0" + "666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83" + "1372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" + " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" + "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" + "t',2),struct('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" + "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" + "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" + "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" + "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," + "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" + "uct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" + "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'M" + "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNam" + "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-66.44849','MaxYL" + "imReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" + ",true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803" + "922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" + "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411764" + "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1" + " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" + "',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','" + "LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'S" + "howContent',true,'Placement',4),struct('MinYLimReal','-66.44849','MaxYLimReal','41.64929','YLabelReal','','MinYLim" + "Mag','0','MaxYLimMag','10','LegendVisibility','on','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.0" + "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" + "3],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[" + "]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" + "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803" + "9215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392" + "15686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title'" + ",'%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Use" + "rDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','9" + "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',{struct('Title','%','" + "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" + "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" + "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" + "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" + "s',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" + "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," + "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" + "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" + ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" + "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" + "','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" + " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" + "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[" + "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" + "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" + "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('T" + "itle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" + "ne','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" + "'Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'" + ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marke" + "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" + ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" + "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" + "on','2017b','Location',[385 262 2230 1233])" NumInputPorts "5" } Block { @@ -19564,8 +18906,8 @@ Model { ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" "eName','ScopeData1','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-29.67835'" - ",'MaxYLimReal','25.62961','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','29.67835','LegendVisibility','On','" + "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-23.63249'" + ",'MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.95301','LegendVisibility','On','" "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " @@ -19574,65 +18916,86 @@ Model { "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5" ",'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',3,'L" - "ineNames',{{'torso:1','torso:2','torso:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-10.1735','Max" - "YLimReal','8.43835','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid" - "',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980" - "3922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843137" - "2549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352941176" - "47 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 " - "1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'left_" - "arm:1','left_arm:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-10.59277'" - ",'MaxYLimReal','10.20572','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true," - "'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627" - "4509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607" - "8431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352" - "94117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Colo" - "r',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct" - "('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle" - "','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{" - "'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3),struct('MinYLimReal','" - "-21.85608','MaxYLimReal','37.79901','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XG" - "rid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098039" - "22 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882" - "353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;" - "0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{st" - "ruct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'" - "Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1]," - "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',6,'Lin" - "eNames',{{'left_leg:1','left_leg:2','left_leg:3','left_leg:4','left_leg:5','left_leg:6'}},'ShowContent',true,'Plac" - "ement',4),struct('MinYLimReal','-52.14583','MaxYLimReal','52.78096','YLabelReal','','MinYLimMag','0','MaxYLimMag'," - "'10','LegendVisibility','on','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.62" - "3529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176" - "47058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'" - "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth'," - "0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" - "e','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChan" - "nelNames',{{}},'NumLines',6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','r" - "ight_leg:6'}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0" + "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'L" + "ineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabel" + "Real','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false" + ",'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0" + "666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83" + "1372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" + " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" + "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" + "t',2),struct('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" + "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" + "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" + "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" + "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," + "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" + "uct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" + "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'M" + "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNam" + "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-66.44849','MaxYL" + "imReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" + ",true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803" + "922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" + "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411764" + "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1" + " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" + "',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','" + "LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'S" + "howContent',true,'Placement',4),struct('MinYLimReal','-66.44849','MaxYLimReal','41.64929','YLabelReal','','MinYLim" + "Mag','0','MaxYLimMag','10','LegendVisibility','on','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.0" "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker" - "','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" - "ruct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'" - "Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeS" - "amples','90','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',[]),extmgr.Configuration" - "('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measure" - "ments',true,'Version','2017b')),'Version','2017b','Location',[66 78 1921 1079])" + "3],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[" + "]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" + "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803" + "9215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392" + "15686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title'" + ",'%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Use" + "rDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','9" + "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',{struct('Title','%','" + "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" + "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" + "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" + "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" + "s',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" + "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," + "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" + "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" + ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" + "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" + "','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" + " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" + "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[" + "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" + "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" + "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('T" + "itle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" + "ne','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" + "'Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'" + ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marke" + "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" + ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" + "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" + "on','2017b','Location',[385 262 2230 1233])" NumInputPorts "5" } Line { @@ -19846,7 +19209,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off measuredType "Joints Acceleration" } @@ -20403,7 +19765,6 @@ Model { Name "emergency stop: joint limits" Location [65, -4, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -20450,7 +19811,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Limits" SourceType "Get Limits" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off limitsSource "ControlBoard" limitsType "Position" @@ -20474,7 +19834,6 @@ Model { Name "MATLAB Function" Location [121, 45, 868, 1245] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -20486,7 +19845,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3755" + SIDHighWatermark "3758" Block { BlockType Inport Name "umin" @@ -20525,24 +19884,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "2421::3754" + SID "2421::3757" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 104 + ZOrder 107 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2421::3753" - Tag "Stateflow S-Function torqueBalancingYoga 18" + SID "2421::3756" + Tag "Stateflow S-Function torqueBalancingYoga2016 18" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 103 + ZOrder 106 FunctionName "sf_sfun" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -20552,9 +19911,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2421::3755" + SID "2421::3758" Position [460, 241, 480, 259] - ZOrder 105 + ZOrder 108 } Block { BlockType Outport @@ -20565,28 +19924,28 @@ Model { IconDisplay "Port number" } Line { - ZOrder 134 + ZOrder 141 SrcBlock "umin" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 135 + ZOrder 142 SrcBlock "umax" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 136 + ZOrder 143 SrcBlock "u" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 137 + ZOrder 144 SrcBlock "tol" SrcPort 1 DstBlock " SFunction " @@ -20594,7 +19953,7 @@ Model { } Line { Name "inRange" - ZOrder 138 + ZOrder 145 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -20602,14 +19961,14 @@ Model { DstPort 1 } Line { - ZOrder 139 + ZOrder 146 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 140 + ZOrder 147 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -20678,7 +20037,7 @@ Model { Variant off Object { $PropName "MaskObject" - $ObjectID 22 + $ObjectID 23 $ClassName "Simulink.Mask" Display "disp('SYNCHRONIZER')" } @@ -20686,7 +20045,6 @@ Model { Name "synchronizer" Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -20711,7 +20069,6 @@ Model { Name "GAZEBO_SYNCHRONIZER" Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -20743,7 +20100,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" SourceType "Simulator Synchronizer" - SourceProductName "Whole Body Toolbox" period "Config.Ts" serverPortName "'/clock/rpc'" clientPortName "'/WBT_synchronizer/rpc:o'" @@ -20788,7 +20144,6 @@ Model { Name "REAL_TIME_SYNC" Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -20821,7 +20176,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" - SourceProductName "Whole Body Toolbox" period "Config.Ts" } } @@ -20851,7 +20205,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Clock" SourceType "YARP Clock" - SourceProductName "Whole Body Toolbox" } Line { ZOrder 1 @@ -20900,7 +20253,6 @@ Model { Name "tauDot Saturation" Location [65, 24, 2560, 1440] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -20947,7 +20299,6 @@ Model { Name "Saturate the Torque Derivative" Location [223, 338, 826, 833] Open off - PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -20959,7 +20310,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "47" + SIDHighWatermark "50" Block { BlockType Inport Name "u" @@ -20980,25 +20331,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4556::46" + SID "4556::49" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 36 + ZOrder 39 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4556::45" - Tag "Stateflow S-Function torqueBalancingYoga 11" + SID "4556::48" + Tag "Stateflow S-Function torqueBalancingYoga2016 11" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 35 + ZOrder 38 FunctionName "sf_sfun" Parameters "Config" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -21008,9 +20359,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4556::47" + SID "4556::50" Position [460, 241, 480, 259] - ZOrder 37 + ZOrder 40 } Block { BlockType Outport @@ -21021,7 +20372,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 60 + ZOrder 65 SrcBlock "u" SrcPort 1 Points [120, 0] @@ -21029,7 +20380,7 @@ Model { DstPort 1 } Line { - ZOrder 61 + ZOrder 66 SrcBlock "u_0" SrcPort 1 DstBlock " SFunction " @@ -21037,7 +20388,7 @@ Model { } Line { Name "uSat" - ZOrder 62 + ZOrder 67 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -21045,14 +20396,14 @@ Model { DstPort 1 } Line { - ZOrder 63 + ZOrder 68 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 64 + ZOrder 69 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -21081,7 +20432,6 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" ContentPreviewEnabled off } Block { @@ -21373,10 +20723,10 @@ Model { Stateflow { machine { id 1 - name "torqueBalancingYoga" + name "torqueBalancingYoga2016" created "18-Feb-2014 10:14:40" isLibrary 0 - sfVersion 80000012 + sfVersion 80000010 firstTarget 271 } chart { From aa9ffe50665cec1ee60b2154e055c1e0b4462ce8 Mon Sep 17 00:00:00 2001 From: Gabriele Nava Date: Tue, 9 Oct 2018 17:25:49 +0200 Subject: [PATCH 3/5] resolve conflitcs with master --- .../torqueBalancingYoga.mdl | 987 +++++++++--------- 1 file changed, 480 insertions(+), 507 deletions(-) diff --git a/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl b/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl index 999e9db..9c4299f 100644 --- a/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl +++ b/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl @@ -1,12 +1,12 @@ Model { - Name "torqueBalancingYoga2016" + Name "torqueBalancingYoga" Version 8.8 SavedCharacterEncoding "UTF-8" GraphicalInterface { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.3294" + ComputedModelVersion "1.3293" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -16,11 +16,11 @@ Model { HasTerminateEvent 0 IsExportFunctionModel 0 } + DiagnosticSuppressor "on" + SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" LogicAnalyzerGraphicalSettings "" LogicAnalyzerPlugin "on" LogicAnalyzerSignalOrdering "" - DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 6 0 0 0 11 0" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on DisableAllScopes off @@ -42,7 +42,7 @@ Model { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [65.0, 24.0, 1855.0, 1056.0] + Location [8.0, 35.0, 2495.0, 1444.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -66,9 +66,9 @@ Model { IsActive [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [1817.0, 907.0] - ZoomFactor [1.3908227848101267] - Offset [559.2895335608647, 203.93401592719] + Extents [2441.0, 1214.0] + ZoomFactor [1.8945686900958467] + Offset [568.29005059021927, 209.61045531197306] } Object { $PropName "DockComponentsInfo" @@ -86,24 +86,24 @@ Model { } WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABiAP///wAAAAEAAAAAAAAAAPwCA" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAB4AP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAALAD///8AAAc/AAADyQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "QBjAHQAbwByAAAAAAD/////AAAAMQD///8AAAmvAAAFBAAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACA/////wAAAAAAAAAA/////wEAAADo/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAFo/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAANR/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" + "////wEAAACG/////wAAAAAAAAAA/////wEAAADu/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAF0/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAOq/////wAAAAAAAAAA/" + "////wEAAAPf/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" } } Created "Tue Feb 18 10:13:36 2014" Creator "daniele" UpdateHistory "UpdateHistoryNever" ModifiedByFormat "%" - LastModifiedBy "gnava" + LastModifiedBy "dferigo" ModifiedDateFormat "%" - LastModifiedDate "Tue Oct 09 17:08:29 2018" - RTWModifiedTimeStamp 461005709 - ModelVersionFormat "1.%" + LastModifiedDate "Fri Sep 28 09:51:57 2018" + RTWModifiedTimeStamp 460029116 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -152,13 +152,13 @@ Model { $PropName "DataLoggingOverride" $ObjectID 7 $ClassName "Simulink.SimulationData.ModelLoggingInfo" - model_ "torqueBalancingYoga2016" + model_ "torqueBalancingYoga" signals_ [] overrideMode_ [0.0] Array { Type "Cell" Dimension 1 - Cell "torqueBalancingYoga2016" + Cell "torqueBalancingYoga" PropName "logAsSpecifiedByModels_" } Array { @@ -742,7 +742,7 @@ Model { Dimension 1 Simulink.CPPComponent { $ObjectID 19 - Version "1.17.1" + Version "1.18.0" Array { Type "Cell" Dimension 10 @@ -1146,7 +1146,6 @@ Model { Block { BlockType Scope DefaultConfigurationName "Simulink.scopes.TimeScopeBlockCfg" - Floating off } Block { BlockType Selector @@ -1181,6 +1180,7 @@ Model { Opaque off MaskHideContents off SFBlockType "NONE" + Variant off GeneratePreprocessorConditionals off PropagateVariantConditions off TreatAsGroupedWhenPropagatingVariantConditions on @@ -1234,8 +1234,8 @@ Model { } } System { - Name "torqueBalancingYoga2016" - Location [65, 24, 1920, 1080] + Name "torqueBalancingYoga" + Location [8, 35, 2503, 1479] Open off ModelBrowserVisibility off ModelBrowserWidth 200 @@ -1247,7 +1247,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "139" + ZoomFactor "189" ReportName "simulink-default.rpt" SIDHighWatermark "4649" Block { @@ -1281,7 +1281,6 @@ Model { BackgroundColor "[0.333333, 1.000000, 1.000000]" ShowName off RequestExecContextInheritance off - Variant off Object { $PropName "MaskObject" $ObjectID 22 @@ -1422,7 +1421,6 @@ Model { Position [715, 151, 855, 509] ZOrder 707 RequestExecContextInheritance off - Variant off System { Name "Visualizer" Location [65, 24, 2560, 1440] @@ -1598,9 +1596,10 @@ Model { "lor',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'" "NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','400','TimeRangeFrames','40" "0','DisplayLayoutDimensions',[4 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'Pre" - "viousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2017b','L" + "viousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','L" "ocation',[66 78 2561 1439])" NumInputPorts "4" + Floating off } Block { BlockType Selector @@ -2158,8 +2157,9 @@ Model { "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[" "]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','400','TimeRangeFrames','400','DisplayLayoutDimensions" "',[2 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tools','" - "Measurements',true,'Version','2017b')),'Version','2017b','Location',[640 424 1920 1072])" + "Measurements',true,'Version','2017b')),'Version','2018a','Location',[640 424 1920 1072])" NumInputPorts "2" + Floating off } Block { BlockType Scope @@ -2265,8 +2265,9 @@ Model { "e','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'Ti" "meRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[7 1]),extmgr.Configuration('Tools','Plot" " Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true" - ",'Version','2017b')),'Version','2017b','Location',[411 452 1701 1130])" + ",'Version','2017b')),'Version','2018a','Location',[411 452 1701 1130])" NumInputPorts "7" + Floating off } Block { BlockType Scope @@ -2359,8 +2360,9 @@ Model { "le','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'T" "imeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plo" "t Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',tru" - "e,'Version','2017b')),'Version','2017b','Location',[66 106 1377 795])" + "e,'Version','2017b')),'Version','2018a','Location',[66 106 1377 795])" NumInputPorts "6" + Floating off } Block { BlockType Scope @@ -2455,8 +2457,9 @@ Model { "DefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples'," "'40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',tru" "e,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017" - "b')),'Version','2017b','Location',[76 179 1784 1070])" + "b')),'Version','2018a','Location',[76 179 1784 1070])" NumInputPorts "6" + Floating off } Block { BlockType Scope @@ -2547,8 +2550,9 @@ Model { "Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',t" "rue,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr.Confi" "guration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tool" - "s','Measurements',true,'Version','2017b')),'Version','2017b','Location',[76 146 1996 1074])" + "s','Measurements',true,'Version','2017b')),'Version','2018a','Location',[76 146 1996 1074])" NumInputPorts "6" + Floating off } Block { BlockType Scope @@ -2639,8 +2643,9 @@ Model { "','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'P" "lacement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configurati" "on('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Me" - "asurements',true,'Version','2017b')),'Version','2017b','Location',[816 261 1744 1234])" + "asurements',true,'Version','2017b')),'Version','2018a','Location',[816 261 1744 1234])" NumInputPorts "6" + Floating off } Block { BlockType Scope @@ -2734,9 +2739,10 @@ Model { "-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{" "[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions'" ",[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr" - ".Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2017b','Location',[706 340 2551 1268]" + ".Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[706 340 2551 1268]" ")" NumInputPorts "6" + Floating off } Block { BlockType Scope @@ -2830,8 +2836,9 @@ Model { "idth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'Sho" "wContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),e" "xtmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configur" - "ation('Tools','Measurements',true,'Version','2017b')),'Version','2017b','Location',[712 340 2557 1268])" + "ation('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[712 340 2557 1268])" NumInputPorts "6" + Floating off } Block { BlockType Scope @@ -2925,8 +2932,9 @@ Model { "lNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','90','TimeRan" "geFrames','90','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop" "',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version" - "','2017b','Location',[66 78 1921 1079])" + "','2018a','Location',[66 78 1921 1079])" NumInputPorts "6" + Floating off } Block { BlockType Scope @@ -3020,8 +3028,9 @@ Model { "nelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeR" "angeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtSt" "op',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2017b','Location',[1271 533 2561 1263])" + "on','2018a','Location',[1271 533 2561 1263])" NumInputPorts "6" + Floating off } Line { Name "CoM_Error" @@ -3888,7 +3897,6 @@ Model { BackgroundColor "[0.000000, 1.000000, 0.498039]" NamePlacement "alternate" RequestExecContextInheritance off - Variant off System { Name "Dynamics and Kinematics" Location [65, -4, 2560, 1440] @@ -3938,7 +3946,6 @@ Model { Position [-655, -196, -550, 16] ZOrder 838 RequestExecContextInheritance off - Variant off System { Name "Dynamics" Location [65, -4, 2560, 1440] @@ -3988,7 +3995,6 @@ Model { Position [580, 34, 705, 76] ZOrder -19 RequestExecContextInheritance off - Variant off System { Name "Add motors reflected inertias" Location [65, -4, 2560, 1440] @@ -4024,7 +4030,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "Add motor reflected inertias" Location [223, 338, 826, 833] @@ -4040,7 +4045,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "58" + SIDHighWatermark "55" Block { BlockType Inport Name "M" @@ -4052,25 +4057,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4518::57" + SID "4518::54" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 47 + ZOrder 44 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4518::56" - Tag "Stateflow S-Function torqueBalancingYoga2016 6" + SID "4518::53" + Tag "Stateflow S-Function torqueBalancingYoga 6" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 46 + ZOrder 43 FunctionName "sf_sfun" Parameters "Config" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -4080,9 +4085,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4518::58" + SID "4518::55" Position [460, 241, 480, 259] - ZOrder 48 + ZOrder 45 } Block { BlockType Outport @@ -4093,7 +4098,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 62 + ZOrder 58 SrcBlock "M" SrcPort 1 DstBlock " SFunction " @@ -4101,7 +4106,7 @@ Model { } Line { Name "M_with_inertia" - ZOrder 63 + ZOrder 59 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -4109,14 +4114,14 @@ Model { DstPort 1 } Line { - ZOrder 64 + ZOrder 60 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 65 + ZOrder 61 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -4377,7 +4382,6 @@ Model { Position [-400, 21, -200, 359] ZOrder 837 RequestExecContextInheritance off - Variant off System { Name "Kinematics" Location [65, -4, 2560, 1440] @@ -4551,7 +4555,6 @@ Model { ZOrder 872 ShowName off RequestExecContextInheritance off - Variant off System { Name "xCom" Location [65, -4, 2560, 1440] @@ -5228,7 +5231,6 @@ Model { BackgroundColor "orange" Priority "-10" RequestExecContextInheritance off - Variant off System { Name "Robot State and References" Location [326, 394, 953, 1032] @@ -5252,7 +5254,7 @@ Model { Position [-280, 2850, -250, 2880] ZOrder 613 ShowName off - LibraryVersion "1.401" + LibraryVersion "1.441" SourceBlock "simulink/Logic and Bit\nOperations/Compare\nTo Zero" SourceType "Compare To Zero" ContentPreviewEnabled off @@ -5268,7 +5270,7 @@ Model { Position [-280, 2580, -250, 2610] ZOrder 614 ShowName off - LibraryVersion "1.401" + LibraryVersion "1.441" SourceBlock "simulink/Logic and Bit\nOperations/Compare\nTo Zero" SourceType "Compare To Zero" ContentPreviewEnabled off @@ -5306,9 +5308,10 @@ Model { Position [-440, 2581, -380, 2609] ZOrder 605 ShowName off - LibraryVersion "1.401" + LibraryVersion "1.441" SourceBlock "simulink/Logic and Bit\nOperations/Bitwise\nOperator" SourceType "Bitwise Operator" + MultiThreadCoSim "auto" logicop "AND" UseBitMask on NumInputPorts "1" @@ -5373,7 +5376,6 @@ Model { Position [-210, 2617, -30, 2823] ZOrder 505 RequestExecContextInheritance off - Variant off System { Name "Internal Coordinator" Location [65, -4, 2560, 1440] @@ -5413,7 +5415,6 @@ Model { Position [-480, 281, -350, 319] ZOrder 886 RequestExecContextInheritance off - Variant off System { Name "Base to fixed_link" Location [65, -4, 2560, 1440] @@ -5472,7 +5473,6 @@ Model { Position [1000, -287, 1195, 117] ZOrder 718 RequestExecContextInheritance off - Variant off System { Name "LFoot to base link transform " Location [65, -4, 2560, 1440] @@ -5596,6 +5596,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + MultiThreadCoSim "auto" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -5638,7 +5639,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] @@ -5654,7 +5654,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3773" + SIDHighWatermark "3770" SIDPrevWatermark "9" Block { BlockType Inport @@ -5712,25 +5712,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4059::3772" + SID "4059::3769" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 172 + ZOrder 169 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4059::3771" - Tag "Stateflow S-Function torqueBalancingYoga2016 2" + SID "4059::3768" + Tag "Stateflow S-Function torqueBalancingYoga 2" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 171 + ZOrder 168 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -5740,9 +5740,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4059::3773" + SID "4059::3770" Position [460, 241, 480, 259] - ZOrder 173 + ZOrder 170 } Block { BlockType Outport @@ -5753,42 +5753,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 145 + ZOrder 136 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 146 + ZOrder 137 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 147 + ZOrder 138 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 148 + ZOrder 139 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 149 + ZOrder 140 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 150 + ZOrder 141 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -5796,7 +5796,7 @@ Model { } Line { Name "w_H_b" - ZOrder 151 + ZOrder 142 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -5804,14 +5804,14 @@ Model { DstPort 1 } Line { - ZOrder 152 + ZOrder 143 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 153 + ZOrder 144 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -5855,7 +5855,6 @@ Model { Position [1135, 305, 1260, 335] ZOrder 743 RequestExecContextInheritance off - Variant off System { Name "neck transform" Location [65, -4, 2560, 1440] @@ -6327,7 +6326,6 @@ Model { Position [165, 239, 315, 311] ZOrder 887 RequestExecContextInheritance off - Variant off System { Name "Compute State Velocity" Location [65, 24, 2560, 1440] @@ -6383,7 +6381,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "Compute Base Velocity" Location [19, 45, 910, 1950] @@ -6399,7 +6396,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3782" + SIDHighWatermark "3779" Block { BlockType Inport Name "J_LeftFoot" @@ -6438,25 +6435,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4090::3781" + SID "4090::3778" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 150 + ZOrder 147 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4090::3780" - Tag "Stateflow S-Function torqueBalancingYoga2016 5" + SID "4090::3777" + Tag "Stateflow S-Function torqueBalancingYoga 5" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 149 + ZOrder 146 FunctionName "sf_sfun" Parameters "Reg" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -6466,9 +6463,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4090::3782" + SID "4090::3779" Position [460, 241, 480, 259] - ZOrder 151 + ZOrder 148 } Block { BlockType Outport @@ -6479,28 +6476,28 @@ Model { IconDisplay "Port number" } Line { - ZOrder 113 + ZOrder 106 SrcBlock "J_LeftFoot" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 114 + ZOrder 107 SrcBlock "J_RightFoot" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 115 + ZOrder 108 SrcBlock "feetInContact" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 116 + ZOrder 109 SrcBlock "qjDot" SrcPort 1 DstBlock " SFunction " @@ -6508,7 +6505,7 @@ Model { } Line { Name "nu_b" - ZOrder 117 + ZOrder 110 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -6516,14 +6513,14 @@ Model { DstPort 1 } Line { - ZOrder 118 + ZOrder 111 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 119 + ZOrder 112 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -6539,7 +6536,6 @@ Model { Position [55, -191, 205, -149] ZOrder -21 RequestExecContextInheritance off - Variant off System { Name "Feet Jacobians" Location [65, 24, 2560, 1440] @@ -6839,6 +6835,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + MultiThreadCoSim "auto" portName "Ports.IMU" signalSize "12" timeout "0.5" @@ -6872,7 +6869,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "Reference Generator CoM" Location [53, 45, 896, 1715] @@ -6888,7 +6884,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3758" + SIDHighWatermark "3755" Block { BlockType Inport Name "pos_CoM_0" @@ -6909,25 +6905,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "2134::3757" + SID "2134::3754" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 160 + ZOrder 157 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2134::3756" - Tag "Stateflow S-Function torqueBalancingYoga2016 10" + SID "2134::3753" + Tag "Stateflow S-Function torqueBalancingYoga 10" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 159 + ZOrder 156 FunctionName "sf_sfun" Parameters "Config" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -6937,9 +6933,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2134::3758" + SID "2134::3755" Position [460, 241, 480, 259] - ZOrder 161 + ZOrder 158 } Block { BlockType Outport @@ -6950,7 +6946,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 81 + ZOrder 76 SrcBlock "pos_CoM_0" SrcPort 1 Points [120, 0] @@ -6958,7 +6954,7 @@ Model { DstPort 1 } Line { - ZOrder 82 + ZOrder 77 SrcBlock "t" SrcPort 1 DstBlock " SFunction " @@ -6966,7 +6962,7 @@ Model { } Line { Name "references_CoM" - ZOrder 83 + ZOrder 78 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -6974,14 +6970,14 @@ Model { DstPort 1 } Line { - ZOrder 84 + ZOrder 79 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 85 + ZOrder 80 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -7056,7 +7052,6 @@ Model { ZOrder -30 ShowName off RequestExecContextInheritance off - Variant off System { Name "xCom" Location [65, 24, 2560, 1440] @@ -7508,7 +7503,6 @@ Model { Position [320, 2689, 515, 2971] ZOrder 702 RequestExecContextInheritance off - Variant off System { Name "Select State and References" Location [65, -4, 2560, 1440] @@ -7590,6 +7584,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" SourceType "Minimum Jerk Trajectory Generator" + MultiThreadCoSim "auto" externalSettlingTime on settlingTime "0.01" sampleTime "Config.Ts" @@ -7609,6 +7604,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" SourceType "Minimum Jerk Trajectory Generator" + MultiThreadCoSim "auto" externalSettlingTime on settlingTime "0.01" sampleTime "Config.Ts" @@ -7729,7 +7725,6 @@ Model { Position [380, -87, 690, -3] ZOrder 720 RequestExecContextInheritance off - Variant off System { Name "Visualize Gain Tuning " Location [65, -4, 2560, 1440] @@ -8002,9 +7997,10 @@ Model { "r',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-'," "'LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'" "ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',t" - "rue,'OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2017b','Lo" + "rue,'OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Lo" "cation',[76 116 1387 805])" NumInputPorts "6" + Floating off } Block { BlockType Scope @@ -8040,8 +8036,9 @@ Model { "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'M" "arker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true," "'Placement',1)),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tool" - "s','Measurements',true,'Version','2017b')),'Version','2017b','Location',[1000 538 1560 958])" + "s','Measurements',true,'Version','2017b')),'Version','2018a','Location',[1000 538 1560 958])" NumInputPorts "1" + Floating off } Block { BlockType Selector @@ -8114,8 +8111,9 @@ Model { "le','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedCha" "nnelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[2 1])" ",extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tools','Measurement" - "s',true,'Version','2017b')),'Version','2017b','Location',[179 459 1300 1439])" + "s',true,'Version','2017b')),'Version','2018a','Location',[179 459 1300 1439])" NumInputPorts "2" + Floating off } Block { BlockType Scope @@ -8173,8 +8171,9 @@ Model { "0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0" ",'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[3 1]),extmgr.Configuration('Tools" "','Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2017b'))," - "'Version','2017b','Location',[76 459 1996 1439])" + "'Version','2018a','Location',[76 459 1996 1439])" NumInputPorts "3" + Floating off } Block { BlockType Scope @@ -8264,8 +8263,9 @@ Model { "','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRa" "ngeSamples','60','TimeRangeFrames','60','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Naviga" "tion',true,'OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','201" - "7b','Location',[384 319 2304 1299])" + "8a','Location',[384 319 2304 1299])" NumInputPorts "6" + Floating off } Line { Name "state" @@ -8879,7 +8879,6 @@ Model { ZOrder 506 Priority "-100" RequestExecContextInheritance off - Variant off System { Name "State Machine Yoga" Location [65, -4, 1920, 1080] @@ -8928,7 +8927,6 @@ Model { Position [590, -76, 740, -4] ZOrder 889 RequestExecContextInheritance off - Variant off System { Name "Compute State Velocity" Location [65, 24, 2560, 1440] @@ -8984,7 +8982,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "Compute Base Velocity" Location [19, 45, 910, 1950] @@ -9000,7 +8997,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3782" + SIDHighWatermark "3779" Block { BlockType Inport Name "J_LeftFoot" @@ -9039,25 +9036,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4215::3781" + SID "4215::3778" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 150 + ZOrder 147 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4215::3780" - Tag "Stateflow S-Function torqueBalancingYoga2016 7" + SID "4215::3777" + Tag "Stateflow S-Function torqueBalancingYoga 7" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 149 + ZOrder 146 FunctionName "sf_sfun" Parameters "Reg" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -9067,9 +9064,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4215::3782" + SID "4215::3779" Position [460, 241, 480, 259] - ZOrder 151 + ZOrder 148 } Block { BlockType Outport @@ -9080,28 +9077,28 @@ Model { IconDisplay "Port number" } Line { - ZOrder 113 + ZOrder 106 SrcBlock "J_LeftFoot" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 114 + ZOrder 107 SrcBlock "J_RightFoot" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 115 + ZOrder 108 SrcBlock "feetInContact" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 116 + ZOrder 109 SrcBlock "qjDot" SrcPort 1 DstBlock " SFunction " @@ -9109,7 +9106,7 @@ Model { } Line { Name "nu_b" - ZOrder 117 + ZOrder 110 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -9117,14 +9114,14 @@ Model { DstPort 1 } Line { - ZOrder 118 + ZOrder 111 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 119 + ZOrder 112 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -9140,7 +9137,6 @@ Model { Position [55, -191, 205, -149] ZOrder -21 RequestExecContextInheritance off - Variant off System { Name "Feet Jacobians" Location [65, 24, 2560, 1440] @@ -9375,7 +9371,6 @@ Model { Position [-375, 387, -265, 458] ZOrder 900 RequestExecContextInheritance off - Variant off System { Name "Compute base to fixed link transform" Location [65, -4, 2560, 1440] @@ -9425,7 +9420,6 @@ Model { Position [435, 15, 580, 55] ZOrder 893 RequestExecContextInheritance off - Variant off System { Name "LFoot to base link transform " Location [65, -4, 2560, 1440] @@ -9549,6 +9543,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + MultiThreadCoSim "auto" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -9591,7 +9586,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] @@ -9607,7 +9601,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3773" + SIDHighWatermark "3770" SIDPrevWatermark "9" Block { BlockType Inport @@ -9665,25 +9659,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4257::3772" + SID "4257::3769" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 172 + ZOrder 169 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4257::3771" - Tag "Stateflow S-Function torqueBalancingYoga2016 8" + SID "4257::3768" + Tag "Stateflow S-Function torqueBalancingYoga 8" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 171 + ZOrder 168 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -9693,9 +9687,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4257::3773" + SID "4257::3770" Position [460, 241, 480, 259] - ZOrder 173 + ZOrder 170 } Block { BlockType Outport @@ -9706,42 +9700,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 145 + ZOrder 136 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 146 + ZOrder 137 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 147 + ZOrder 138 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 148 + ZOrder 139 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 149 + ZOrder 140 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 150 + ZOrder 141 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -9749,7 +9743,7 @@ Model { } Line { Name "w_H_b" - ZOrder 151 + ZOrder 142 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -9757,14 +9751,14 @@ Model { DstPort 1 } Line { - ZOrder 152 + ZOrder 143 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 153 + ZOrder 144 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -9808,7 +9802,6 @@ Model { Position [1135, 305, 1260, 335] ZOrder 743 RequestExecContextInheritance off - Variant off System { Name "neck transform" Location [65, -4, 2560, 1440] @@ -10147,7 +10140,6 @@ Model { Position [435, 100, 580, 140] ZOrder 896 RequestExecContextInheritance off - Variant off System { Name "RFoot to base link transform" Location [65, -4, 2560, 1440] @@ -10271,6 +10263,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + MultiThreadCoSim "auto" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -10313,7 +10306,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] @@ -10329,7 +10321,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3773" + SIDHighWatermark "3770" SIDPrevWatermark "9" Block { BlockType Inport @@ -10387,25 +10379,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4282::3772" + SID "4282::3769" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 172 + ZOrder 169 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4282::3771" - Tag "Stateflow S-Function torqueBalancingYoga2016 3" + SID "4282::3768" + Tag "Stateflow S-Function torqueBalancingYoga 3" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 171 + ZOrder 168 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -10415,9 +10407,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4282::3773" + SID "4282::3770" Position [460, 241, 480, 259] - ZOrder 173 + ZOrder 170 } Block { BlockType Outport @@ -10428,42 +10420,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 145 + ZOrder 136 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 146 + ZOrder 137 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 147 + ZOrder 138 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 148 + ZOrder 139 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 149 + ZOrder 140 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 150 + ZOrder 141 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -10471,7 +10463,7 @@ Model { } Line { Name "w_H_b" - ZOrder 151 + ZOrder 142 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -10479,14 +10471,14 @@ Model { DstPort 1 } Line { - ZOrder 152 + ZOrder 143 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 153 + ZOrder 144 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -10530,7 +10522,6 @@ Model { Position [1135, 305, 1260, 335] ZOrder 743 RequestExecContextInheritance off - Variant off System { Name "neck transform" Location [65, -4, 2560, 1440] @@ -11035,6 +11026,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" SourceType "Minimum Jerk Trajectory Generator" + MultiThreadCoSim "auto" externalSettlingTime off settlingTime "Gain.SmoothingTimeGainScheduling" sampleTime "Config.Ts" @@ -11101,7 +11093,6 @@ Model { Position [-240, 11, -40, 84] ZOrder 681 RequestExecContextInheritance off - Variant off System { Name "Visualise extrnal wrenches " Location [65, 24, 2560, 1440] @@ -11233,8 +11224,9 @@ Model { "sible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefined" "ChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[3 " "1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Config" - "uration('Tools','Measurements',true,'Version','2017b')),'Version','2017b','Location',[248 273 1228 835])" + "uration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[248 273 1228 835])" NumInputPorts "3" + Floating off } Block { BlockType Scope @@ -11292,8 +11284,9 @@ Model { "arker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true," "'Placement',1),'TimeRangeSamples','42.68','TimeRangeFrames','42.68','DisplayLayoutDimensions',[3 1]),extmgr.Config" "uration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','" - "Measurements',true,'Version','2017b')),'Version','2017b','Location',[76 151 1355 851])" + "Measurements',true,'Version','2017b')),'Version','2018a','Location',[76 151 1355 851])" NumInputPorts "3" + Floating off } Line { Name "thresholdContactOn" @@ -11392,6 +11385,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + MultiThreadCoSim "auto" portName "Ports.IMU" signalSize "12" timeout "0.5" @@ -11412,6 +11406,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + MultiThreadCoSim "auto" portName "Ports.WRENCH_LEFT_FOOT" signalSize "6" timeout "0.5" @@ -11436,6 +11431,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + MultiThreadCoSim "auto" portName "Ports.WRENCH_RIGHT_FOOT" signalSize "6" timeout "0.5" @@ -11462,7 +11458,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off Port { PortNumber 4 Name "constraintsSmooth" @@ -11483,7 +11478,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3759" + SIDHighWatermark "3756" SIDPrevWatermark "9" Block { BlockType Inport @@ -11577,25 +11572,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "2220::3758" + SID "2220::3755" Ports [1, 1] Position [270, 460, 320, 500] - ZOrder 156 + ZOrder 153 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2220::3757" - Tag "Stateflow S-Function torqueBalancingYoga2016 13" + SID "2220::3754" + Tag "Stateflow S-Function torqueBalancingYoga 13" Ports [10, 10] Position [180, 100, 230, 320] - ZOrder 155 + ZOrder 152 FunctionName "sf_sfun" Parameters "Gain,Sm" PortCounts "[10 10]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -11637,9 +11632,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2220::3759" + SID "2220::3756" Position [460, 471, 480, 489] - ZOrder 157 + ZOrder 154 } Block { BlockType Outport @@ -11722,70 +11717,70 @@ Model { IconDisplay "Port number" } Line { - ZOrder 485 + ZOrder 464 SrcBlock "wrench_rightFoot" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 486 + ZOrder 465 SrcBlock "wrench_leftFoot" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 487 + ZOrder 466 SrcBlock "CoM_0" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 488 + ZOrder 467 SrcBlock "q0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 489 + ZOrder 468 SrcBlock "l_sole_CoM" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 490 + ZOrder 469 SrcBlock "r_sole_CoM" SrcPort 1 DstBlock " SFunction " DstPort 6 } Line { - ZOrder 491 + ZOrder 470 SrcBlock "qj" SrcPort 1 DstBlock " SFunction " DstPort 7 } Line { - ZOrder 492 + ZOrder 471 SrcBlock "t" SrcPort 1 DstBlock " SFunction " DstPort 8 } Line { - ZOrder 493 + ZOrder 472 SrcBlock "l_sole_H_b" SrcPort 1 DstBlock " SFunction " DstPort 9 } Line { - ZOrder 494 + ZOrder 473 SrcBlock "r_sole_H_b" SrcPort 1 DstBlock " SFunction " @@ -11793,7 +11788,7 @@ Model { } Line { Name "w_H_b" - ZOrder 495 + ZOrder 474 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -11802,7 +11797,7 @@ Model { } Line { Name "CoM_des" - ZOrder 496 + ZOrder 475 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -11811,7 +11806,7 @@ Model { } Line { Name "qj_des" - ZOrder 497 + ZOrder 476 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -11820,7 +11815,7 @@ Model { } Line { Name "constraints" - ZOrder 498 + ZOrder 477 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -11829,7 +11824,7 @@ Model { } Line { Name "impedances" - ZOrder 499 + ZOrder 478 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -11838,7 +11833,7 @@ Model { } Line { Name "KPCoM" - ZOrder 500 + ZOrder 479 Labels [0, 0] SrcBlock " SFunction " SrcPort 7 @@ -11847,7 +11842,7 @@ Model { } Line { Name "KDCoM" - ZOrder 501 + ZOrder 480 Labels [0, 0] SrcBlock " SFunction " SrcPort 8 @@ -11856,7 +11851,7 @@ Model { } Line { Name "currentState" - ZOrder 502 + ZOrder 481 Labels [0, 0] SrcBlock " SFunction " SrcPort 9 @@ -11865,7 +11860,7 @@ Model { } Line { Name "jointsSmoothingTime" - ZOrder 503 + ZOrder 482 Labels [0, 0] SrcBlock " SFunction " SrcPort 10 @@ -11873,14 +11868,14 @@ Model { DstPort 1 } Line { - ZOrder 504 + ZOrder 483 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 505 + ZOrder 484 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -11898,7 +11893,6 @@ Model { ZOrder 890 ShowName off RequestExecContextInheritance off - Variant off System { Name "xCom" Location [65, 24, 2560, 1440] @@ -12006,7 +12000,6 @@ Model { ZOrder 898 ShowName off RequestExecContextInheritance off - Variant off System { Name "xCom1" Location [65, 24, 2560, 1440] @@ -12566,9 +12559,10 @@ Model { Position [-445, 2851, -385, 2879] ZOrder 606 ShowName off - LibraryVersion "1.401" + LibraryVersion "1.441" SourceBlock "simulink/Logic and Bit\nOperations/Bitwise\nOperator" SourceType "Bitwise Operator" + MultiThreadCoSim "auto" logicop "AND" UseBitMask on NumInputPorts "1" @@ -13069,7 +13063,6 @@ Model { ZOrder 542 BackgroundColor "lightBlue" RequestExecContextInheritance off - Variant off System { Name "controller_QP" Location [8, 35, 2503, 1479] @@ -13286,7 +13279,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off Port { PortNumber 13 Name "CoM_Error" @@ -13312,7 +13304,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3758" + SIDHighWatermark "3755" Block { BlockType Inport Name "constraints" @@ -13522,25 +13514,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "2407::3757" + SID "2407::3754" Ports [1, 1] Position [270, 640, 320, 680] - ZOrder 160 + ZOrder 157 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2407::3756" - Tag "Stateflow S-Function torqueBalancingYoga2016 17" + SID "2407::3753" + Tag "Stateflow S-Function torqueBalancingYoga 17" Ports [23, 15] Position [180, 65, 230, 545] - ZOrder 159 + ZOrder 156 FunctionName "sf_sfun" Parameters "Gain,Reg" PortCounts "[23 15]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -13602,9 +13594,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2407::3758" + SID "2407::3755" Position [460, 651, 480, 669] - ZOrder 161 + ZOrder 158 } Block { BlockType Outport @@ -13732,161 +13724,161 @@ Model { IconDisplay "Port number" } Line { - ZOrder 957 + ZOrder 918 SrcBlock "constraints" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 958 + ZOrder 919 SrcBlock "ROBOT_DOF_FOR_SIMULINK" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 959 + ZOrder 920 SrcBlock "ConstraintsMatrix" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 960 + ZOrder 921 SrcBlock "bVectorConstraints" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 961 + ZOrder 922 SrcBlock "qj" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 962 + ZOrder 923 SrcBlock "qjDes" SrcPort 1 DstBlock " SFunction " DstPort 6 } Line { - ZOrder 963 + ZOrder 924 SrcBlock "nu" SrcPort 1 DstBlock " SFunction " DstPort 7 } Line { - ZOrder 964 + ZOrder 925 SrcBlock "M" SrcPort 1 DstBlock " SFunction " DstPort 8 } Line { - ZOrder 965 + ZOrder 926 SrcBlock "h" SrcPort 1 DstBlock " SFunction " DstPort 9 } Line { - ZOrder 966 + ZOrder 927 SrcBlock "L" SrcPort 1 DstBlock " SFunction " DstPort 10 } Line { - ZOrder 967 + ZOrder 928 SrcBlock "intLw" SrcPort 1 DstBlock " SFunction " DstPort 11 } Line { - ZOrder 968 + ZOrder 929 SrcBlock "w_H_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 12 } Line { - ZOrder 969 + ZOrder 930 SrcBlock "w_H_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 13 } Line { - ZOrder 970 + ZOrder 931 SrcBlock "JL" SrcPort 1 DstBlock " SFunction " DstPort 14 } Line { - ZOrder 971 + ZOrder 932 SrcBlock "JR" SrcPort 1 DstBlock " SFunction " DstPort 15 } Line { - ZOrder 972 + ZOrder 933 SrcBlock "dJLv" SrcPort 1 DstBlock " SFunction " DstPort 16 } Line { - ZOrder 973 + ZOrder 934 SrcBlock "dJRv" SrcPort 1 DstBlock " SFunction " DstPort 17 } Line { - ZOrder 974 + ZOrder 935 SrcBlock "xCoM" SrcPort 1 DstBlock " SFunction " DstPort 18 } Line { - ZOrder 975 + ZOrder 936 SrcBlock "J_CoM" SrcPort 1 DstBlock " SFunction " DstPort 19 } Line { - ZOrder 976 + ZOrder 937 SrcBlock "desired_x_dx_ddx_CoM" SrcPort 1 DstBlock " SFunction " DstPort 20 } Line { - ZOrder 977 + ZOrder 938 SrcBlock "gainsPCOM" SrcPort 1 DstBlock " SFunction " DstPort 21 } Line { - ZOrder 978 + ZOrder 939 SrcBlock "gainsDCOM" SrcPort 1 DstBlock " SFunction " DstPort 22 } Line { - ZOrder 979 + ZOrder 940 SrcBlock "impedances" SrcPort 1 DstBlock " SFunction " @@ -13894,7 +13886,7 @@ Model { } Line { Name "tauModel" - ZOrder 980 + ZOrder 941 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -13903,7 +13895,7 @@ Model { } Line { Name "Sigma" - ZOrder 981 + ZOrder 942 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -13912,7 +13904,7 @@ Model { } Line { Name "NA" - ZOrder 982 + ZOrder 943 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -13921,7 +13913,7 @@ Model { } Line { Name "fLdotDesC1C2" - ZOrder 983 + ZOrder 944 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -13930,7 +13922,7 @@ Model { } Line { Name "HessianMatrixQP1Foot" - ZOrder 984 + ZOrder 945 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -13939,7 +13931,7 @@ Model { } Line { Name "gradientQP1Foot" - ZOrder 985 + ZOrder 946 Labels [0, 0] SrcBlock " SFunction " SrcPort 7 @@ -13948,7 +13940,7 @@ Model { } Line { Name "ConstraintsMatrixQP1Foot" - ZOrder 986 + ZOrder 947 Labels [0, 0] SrcBlock " SFunction " SrcPort 8 @@ -13957,7 +13949,7 @@ Model { } Line { Name "bVectorConstraintsQp1Foot" - ZOrder 987 + ZOrder 948 Labels [0, 0] SrcBlock " SFunction " SrcPort 9 @@ -13966,7 +13958,7 @@ Model { } Line { Name "HessianMatrixQP2Feet" - ZOrder 988 + ZOrder 949 Labels [0, 0] SrcBlock " SFunction " SrcPort 10 @@ -13975,7 +13967,7 @@ Model { } Line { Name "gradientQP2Feet" - ZOrder 989 + ZOrder 950 Labels [0, 0] SrcBlock " SFunction " SrcPort 11 @@ -13984,7 +13976,7 @@ Model { } Line { Name "ConstraintsMatrixQP2Feet" - ZOrder 990 + ZOrder 951 Labels [0, 0] SrcBlock " SFunction " SrcPort 12 @@ -13993,7 +13985,7 @@ Model { } Line { Name "bVectorConstraintsQp2Feet" - ZOrder 991 + ZOrder 952 Labels [0, 0] SrcBlock " SFunction " SrcPort 13 @@ -14002,7 +13994,7 @@ Model { } Line { Name "errorCoM" - ZOrder 992 + ZOrder 953 Labels [0, 0] SrcBlock " SFunction " SrcPort 14 @@ -14011,7 +14003,7 @@ Model { } Line { Name "f_noQP" - ZOrder 993 + ZOrder 954 Labels [0, 0] SrcBlock " SFunction " SrcPort 15 @@ -14019,14 +14011,14 @@ Model { DstPort 1 } Line { - ZOrder 994 + ZOrder 955 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 995 + ZOrder 956 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -14042,7 +14034,6 @@ Model { Position [1195, 552, 1305, 588] ZOrder 835 RequestExecContextInheritance off - Variant off System { Name "Compute angular momentum integral" Location [65, -4, 2560, 1440] @@ -14114,7 +14105,6 @@ Model { ZOrder 901 NamePlacement "alternate" RequestExecContextInheritance off - Variant off System { Name "Compute base to fixed link transform" Location [65, -4, 2560, 1440] @@ -14164,7 +14154,6 @@ Model { Position [435, 15, 580, 55] ZOrder 893 RequestExecContextInheritance off - Variant off System { Name "LFoot to base link transform " Location [65, -4, 2560, 1440] @@ -14288,6 +14277,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + MultiThreadCoSim "auto" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -14330,7 +14320,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] @@ -14346,7 +14335,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3773" + SIDHighWatermark "3770" SIDPrevWatermark "9" Block { BlockType Inport @@ -14404,25 +14393,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4438::3772" + SID "4438::3769" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 172 + ZOrder 169 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4438::3771" - Tag "Stateflow S-Function torqueBalancingYoga2016 1" + SID "4438::3768" + Tag "Stateflow S-Function torqueBalancingYoga 1" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 171 + ZOrder 168 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -14432,9 +14421,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4438::3773" + SID "4438::3770" Position [460, 241, 480, 259] - ZOrder 173 + ZOrder 170 } Block { BlockType Outport @@ -14445,42 +14434,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 145 + ZOrder 136 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 146 + ZOrder 137 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 147 + ZOrder 138 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 148 + ZOrder 139 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 149 + ZOrder 140 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 150 + ZOrder 141 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -14488,7 +14477,7 @@ Model { } Line { Name "w_H_b" - ZOrder 151 + ZOrder 142 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -14496,14 +14485,14 @@ Model { DstPort 1 } Line { - ZOrder 152 + ZOrder 143 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 153 + ZOrder 144 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -14547,7 +14536,6 @@ Model { Position [1135, 305, 1260, 335] ZOrder 743 RequestExecContextInheritance off - Variant off System { Name "neck transform" Location [65, -4, 2560, 1440] @@ -14886,7 +14874,6 @@ Model { Position [435, 100, 580, 140] ZOrder 896 RequestExecContextInheritance off - Variant off System { Name "RFoot to base link transform" Location [65, -4, 2560, 1440] @@ -15010,6 +14997,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + MultiThreadCoSim "auto" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -15052,7 +15040,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "fromImuToHomogeousTransformFCN" Location [219, 337, 814, 775] @@ -15068,7 +15055,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3773" + SIDHighWatermark "3770" SIDPrevWatermark "9" Block { BlockType Inport @@ -15126,25 +15113,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4463::3772" + SID "4463::3769" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 172 + ZOrder 169 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4463::3771" - Tag "Stateflow S-Function torqueBalancingYoga2016 4" + SID "4463::3768" + Tag "Stateflow S-Function torqueBalancingYoga 4" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 171 + ZOrder 168 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -15154,9 +15141,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4463::3773" + SID "4463::3770" Position [460, 241, 480, 259] - ZOrder 173 + ZOrder 170 } Block { BlockType Outport @@ -15167,42 +15154,42 @@ Model { IconDisplay "Port number" } Line { - ZOrder 145 + ZOrder 136 SrcBlock "imu_H_link" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 146 + ZOrder 137 SrcBlock "imu_H_link_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 147 + ZOrder 138 SrcBlock "link_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 148 + ZOrder 139 SrcBlock "inertial_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 149 + ZOrder 140 SrcBlock "inertial" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 150 + ZOrder 141 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -15210,7 +15197,7 @@ Model { } Line { Name "w_H_b" - ZOrder 151 + ZOrder 142 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -15218,14 +15205,14 @@ Model { DstPort 1 } Line { - ZOrder 152 + ZOrder 143 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 153 + ZOrder 144 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -15269,7 +15256,6 @@ Model { Position [1135, 305, 1260, 335] ZOrder 743 RequestExecContextInheritance off - Variant off System { Name "neck transform" Location [65, -4, 2560, 1440] @@ -15770,7 +15756,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "References for L" Location [223, 338, 826, 833] @@ -15786,7 +15771,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3759" + SIDHighWatermark "3756" Block { BlockType Inport Name "qjErr" @@ -15825,25 +15810,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "3721::3758" + SID "3721::3755" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 83 + ZOrder 80 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "3721::3757" - Tag "Stateflow S-Function torqueBalancingYoga2016 20" + SID "3721::3754" + Tag "Stateflow S-Function torqueBalancingYoga 20" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 82 + ZOrder 79 FunctionName "sf_sfun" Parameters "Reg" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -15853,9 +15838,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "3721::3759" + SID "3721::3756" Position [460, 241, 480, 259] - ZOrder 84 + ZOrder 81 } Block { BlockType Outport @@ -15866,28 +15851,28 @@ Model { IconDisplay "Port number" } Line { - ZOrder 113 + ZOrder 106 SrcBlock "qjErr" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 114 + ZOrder 107 SrcBlock "JL" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 115 + ZOrder 108 SrcBlock "JR" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 116 + ZOrder 109 SrcBlock "activeFeetConstraints" SrcPort 1 DstBlock " SFunction " @@ -15895,7 +15880,7 @@ Model { } Line { Name "nu_b_equivalent" - ZOrder 117 + ZOrder 110 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -15903,14 +15888,14 @@ Model { DstPort 1 } Line { - ZOrder 118 + ZOrder 111 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 119 + ZOrder 112 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -15969,7 +15954,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "choose base to world transform" Location [223, 338, 826, 833] @@ -15985,7 +15969,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "72" + SIDHighWatermark "69" Block { BlockType Inport Name "state" @@ -15997,24 +15981,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4027::71" + SID "4027::68" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 62 + ZOrder 59 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4027::70" - Tag "Stateflow S-Function torqueBalancingYoga2016 19" + SID "4027::67" + Tag "Stateflow S-Function torqueBalancingYoga 19" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 61 + ZOrder 58 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -16024,9 +16008,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4027::72" + SID "4027::69" Position [460, 241, 480, 259] - ZOrder 63 + ZOrder 60 } Block { BlockType Outport @@ -16037,7 +16021,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 65 + ZOrder 61 SrcBlock "state" SrcPort 1 DstBlock " SFunction " @@ -16045,7 +16029,7 @@ Model { } Line { Name "booleanState" - ZOrder 66 + ZOrder 62 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -16053,14 +16037,14 @@ Model { DstPort 1 } Line { - ZOrder 67 + ZOrder 63 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 68 + ZOrder 64 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -16079,6 +16063,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" SourceType "YARP Read" + MultiThreadCoSim "auto" portName "Ports.IMU" signalSize "12" timeout "0.5" @@ -16280,7 +16265,6 @@ Model { Position [1840, 29, 2050, 1056] ZOrder 846 RequestExecContextInheritance off - Variant off System { Name "Compute joint torques" Location [65, 24, 2560, 1440] @@ -16481,7 +16465,6 @@ Model { Position [580, 478, 815, 847] ZOrder 652 RequestExecContextInheritance off - Variant off System { Name "QPSolver" Location [65, 24, 2560, 1440] @@ -16598,7 +16581,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "ContactsTransition" Location [223, 338, 826, 833] @@ -16614,7 +16596,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "42" + SIDHighWatermark "39" Block { BlockType Inport Name "LR_FootInContact" @@ -16626,24 +16608,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4590::41" + SID "4590::38" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 32 + ZOrder 29 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4590::40" - Tag "Stateflow S-Function torqueBalancingYoga2016 15" + SID "4590::37" + Tag "Stateflow S-Function torqueBalancingYoga 15" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 31 + ZOrder 28 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -16653,9 +16635,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4590::42" + SID "4590::39" Position [460, 241, 480, 259] - ZOrder 33 + ZOrder 30 } Block { BlockType Outport @@ -16666,7 +16648,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 21 + ZOrder 17 SrcBlock "LR_FootInContact" SrcPort 1 DstBlock " SFunction " @@ -16674,7 +16656,7 @@ Model { } Line { Name "onOneFoot" - ZOrder 22 + ZOrder 18 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -16682,14 +16664,14 @@ Model { DstPort 1 } Line { - ZOrder 23 + ZOrder 19 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 24 + ZOrder 20 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -16736,7 +16718,6 @@ Model { ZOrder 722 TreatAsAtomicUnit on RequestExecContextInheritance off - Variant off System { Name "One Foot" Location [65, 24, 2560, 1440] @@ -16807,7 +16788,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "Analytical Solution One Foot (unconstrained)" Location [223, 338, 826, 833] @@ -16823,7 +16803,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "43" + SIDHighWatermark "40" Block { BlockType Inport Name "H" @@ -16844,24 +16824,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4600::42" + SID "4600::39" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 33 + ZOrder 30 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4600::41" - Tag "Stateflow S-Function torqueBalancingYoga2016 22" + SID "4600::38" + Tag "Stateflow S-Function torqueBalancingYoga 22" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 32 + ZOrder 29 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -16871,9 +16851,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4600::43" + SID "4600::40" Position [460, 241, 480, 259] - ZOrder 34 + ZOrder 31 } Block { BlockType Outport @@ -16884,7 +16864,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 26 + ZOrder 21 SrcBlock "H" SrcPort 1 Points [120, 0] @@ -16892,7 +16872,7 @@ Model { DstPort 1 } Line { - ZOrder 27 + ZOrder 22 SrcBlock "g" SrcPort 1 DstBlock " SFunction " @@ -16900,7 +16880,7 @@ Model { } Line { Name "analyticalSolution" - ZOrder 28 + ZOrder 23 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -16908,14 +16888,14 @@ Model { DstPort 1 } Line { - ZOrder 29 + ZOrder 24 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 30 + ZOrder 25 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -16947,7 +16927,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "Process QP output" Location [223, 338, 826, 833] @@ -16963,7 +16942,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "50" + SIDHighWatermark "47" Block { BlockType Inport Name "analyticalSolution" @@ -16993,25 +16972,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4602::49" + SID "4602::46" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 34 + ZOrder 31 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4602::48" - Tag "Stateflow S-Function torqueBalancingYoga2016 12" + SID "4602::45" + Tag "Stateflow S-Function torqueBalancingYoga 12" Ports [3, 2] Position [180, 100, 230, 180] - ZOrder 33 + ZOrder 30 FunctionName "sf_sfun" Parameters "Config" PortCounts "[3 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -17021,9 +17000,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4602::50" + SID "4602::47" Position [460, 241, 480, 259] - ZOrder 35 + ZOrder 32 } Block { BlockType Outport @@ -17034,21 +17013,21 @@ Model { IconDisplay "Port number" } Line { - ZOrder 31 + ZOrder 25 SrcBlock "analyticalSolution" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 32 + ZOrder 26 SrcBlock "primalSolution" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 33 + ZOrder 27 SrcBlock "QPStatus" SrcPort 1 DstBlock " SFunction " @@ -17056,7 +17035,7 @@ Model { } Line { Name "f0" - ZOrder 34 + ZOrder 28 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17064,14 +17043,14 @@ Model { DstPort 1 } Line { - ZOrder 35 + ZOrder 29 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 36 + ZOrder 30 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -17090,6 +17069,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/QP" SourceType "QP" + MultiThreadCoSim "auto" lbA off ubA on lb off @@ -17230,7 +17210,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "Process One Foot Output" Location [223, 338, 826, 833] @@ -17246,7 +17225,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "43" + SIDHighWatermark "40" Block { BlockType Inport Name "primalSolution" @@ -17267,24 +17246,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4606::42" + SID "4606::39" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 33 + ZOrder 30 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4606::41" - Tag "Stateflow S-Function torqueBalancingYoga2016 16" + SID "4606::38" + Tag "Stateflow S-Function torqueBalancingYoga 16" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 32 + ZOrder 29 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -17294,9 +17273,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4606::43" + SID "4606::40" Position [460, 241, 480, 259] - ZOrder 34 + ZOrder 31 } Block { BlockType Outport @@ -17307,7 +17286,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 26 + ZOrder 21 SrcBlock "primalSolution" SrcPort 1 Points [120, 0] @@ -17315,7 +17294,7 @@ Model { DstPort 1 } Line { - ZOrder 27 + ZOrder 22 SrcBlock "LR_FootInContact" SrcPort 1 DstBlock " SFunction " @@ -17323,7 +17302,7 @@ Model { } Line { Name "f0_oneFoot" - ZOrder 28 + ZOrder 23 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17331,14 +17310,14 @@ Model { DstPort 1 } Line { - ZOrder 29 + ZOrder 24 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 30 + ZOrder 25 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -17366,7 +17345,6 @@ Model { NamePlacement "alternate" TreatAsAtomicUnit on RequestExecContextInheritance off - Variant off System { Name "Two Feet" Location [65, 24, 1920, 1080] @@ -17437,7 +17415,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "Analytical Solution Two Feet (unconstrained)" Location [223, 338, 826, 833] @@ -17453,7 +17430,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "43" + SIDHighWatermark "40" Block { BlockType Inport Name "H" @@ -17474,24 +17451,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4614::42" + SID "4614::39" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 33 + ZOrder 30 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4614::41" - Tag "Stateflow S-Function torqueBalancingYoga2016 21" + SID "4614::38" + Tag "Stateflow S-Function torqueBalancingYoga 21" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 32 + ZOrder 29 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -17501,9 +17478,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4614::43" + SID "4614::40" Position [460, 241, 480, 259] - ZOrder 34 + ZOrder 31 } Block { BlockType Outport @@ -17514,7 +17491,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 26 + ZOrder 21 SrcBlock "H" SrcPort 1 Points [120, 0] @@ -17522,7 +17499,7 @@ Model { DstPort 1 } Line { - ZOrder 27 + ZOrder 22 SrcBlock "g" SrcPort 1 DstBlock " SFunction " @@ -17530,7 +17507,7 @@ Model { } Line { Name "analyticalSolution" - ZOrder 28 + ZOrder 23 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17538,14 +17515,14 @@ Model { DstPort 1 } Line { - ZOrder 29 + ZOrder 24 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 30 + ZOrder 25 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -17577,7 +17554,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "Process QP output" Location [223, 338, 826, 833] @@ -17593,7 +17569,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "50" + SIDHighWatermark "47" Block { BlockType Inport Name "analyticalSolution" @@ -17623,25 +17599,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4616::49" + SID "4616::46" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 34 + ZOrder 31 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4616::48" - Tag "Stateflow S-Function torqueBalancingYoga2016 23" + SID "4616::45" + Tag "Stateflow S-Function torqueBalancingYoga 23" Ports [3, 2] Position [180, 100, 230, 180] - ZOrder 33 + ZOrder 30 FunctionName "sf_sfun" Parameters "Config" PortCounts "[3 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -17651,9 +17627,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4616::50" + SID "4616::47" Position [460, 241, 480, 259] - ZOrder 35 + ZOrder 32 } Block { BlockType Outport @@ -17664,21 +17640,21 @@ Model { IconDisplay "Port number" } Line { - ZOrder 31 + ZOrder 25 SrcBlock "analyticalSolution" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 32 + ZOrder 26 SrcBlock "primalSolution" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 33 + ZOrder 27 SrcBlock "QPStatus" SrcPort 1 DstBlock " SFunction " @@ -17686,7 +17662,7 @@ Model { } Line { Name "f0" - ZOrder 34 + ZOrder 28 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17694,14 +17670,14 @@ Model { DstPort 1 } Line { - ZOrder 35 + ZOrder 29 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 36 + ZOrder 30 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -17720,6 +17696,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/QP" SourceType "QP" + MultiThreadCoSim "auto" lbA off ubA on lb off @@ -17856,7 +17833,6 @@ Model { Position [500, -21, 650, 66] ZOrder -17 RequestExecContextInheritance off - Variant off System { Name "Visualize eventual QP failures" Location [65, 24, 2560, 1440] @@ -17946,8 +17922,9 @@ Model { "ible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions'," "[2 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.C" - "onfiguration('Tools','Measurements',true,'Version','2017b')),'Version','2017b','Location',[318 489 1511 1216])" + "onfiguration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[318 489 1511 1216])" NumInputPorts "2" + Floating off } Line { Name "exitFlagQP" @@ -18374,7 +18351,6 @@ Model { Position [2100, 256, 2255, 314] ZOrder 862 RequestExecContextInheritance off - Variant off System { Name "Compute joint torques (motor reflected inertia)" Location [8, 35, 2503, 1479] @@ -18418,7 +18394,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" Location [223, 338, 826, 833] @@ -18434,36 +18409,36 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "70" + SIDHighWatermark "66" Block { BlockType Demux Name " Demux " - SID "4551::68" + SID "4551::64" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 59 + ZOrder 55 Outputs "1" } Block { BlockType Ground Name " Ground " - SID "4551::70" + SID "4551::66" Position [20, 121, 40, 139] - ZOrder 61 + ZOrder 57 } Block { BlockType S-Function Name " SFunction " - SID "4551::67" - Tag "Stateflow S-Function torqueBalancingYoga2016 9" + SID "4551::63" + Tag "Stateflow S-Function torqueBalancingYoga 9" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 58 + ZOrder 54 FunctionName "sf_sfun" Parameters "Config" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -18473,9 +18448,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4551::69" + SID "4551::65" Position [460, 241, 480, 259] - ZOrder 60 + ZOrder 56 } Block { BlockType Outport @@ -18487,7 +18462,7 @@ Model { } Line { Name "reflectedInertia" - ZOrder 49 + ZOrder 45 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -18495,21 +18470,21 @@ Model { DstPort 1 } Line { - ZOrder 50 + ZOrder 46 SrcBlock " Ground " SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 51 + ZOrder 47 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 52 + ZOrder 48 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -18537,7 +18512,6 @@ Model { Position [30, 66, 140, 119] ZOrder 894 RequestExecContextInheritance off - Variant off System { Name "Check the contribution of feedforward" Location [65, -4, 2560, 1440] @@ -18789,8 +18763,9 @@ Model { "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2017b','Location',[385 262 2230 1233])" + "on','2018a','Location',[385 262 2230 1233])" NumInputPorts "5" + Floating off } Block { BlockType Scope @@ -18892,8 +18867,9 @@ Model { "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2017b','Location',[385 262 2230 1233])" + "on','2018a','Location',[385 262 2230 1233])" NumInputPorts "5" + Floating off } Block { BlockType Scope @@ -18995,8 +18971,9 @@ Model { "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2017b','Location',[385 262 2230 1233])" + "on','2018a','Location',[385 262 2230 1233])" NumInputPorts "5" + Floating off } Line { Name "right_leg" @@ -19760,7 +19737,6 @@ Model { BackgroundColor "red" ShowName off RequestExecContextInheritance off - Variant off System { Name "emergency stop: joint limits" Location [65, -4, 2560, 1440] @@ -19829,7 +19805,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "MATLAB Function" Location [121, 45, 868, 1245] @@ -19845,7 +19820,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3758" + SIDHighWatermark "3755" Block { BlockType Inport Name "umin" @@ -19884,24 +19859,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "2421::3757" + SID "2421::3754" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 107 + ZOrder 104 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2421::3756" - Tag "Stateflow S-Function torqueBalancingYoga2016 18" + SID "2421::3753" + Tag "Stateflow S-Function torqueBalancingYoga 18" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 106 + ZOrder 103 FunctionName "sf_sfun" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -19911,9 +19886,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2421::3758" + SID "2421::3755" Position [460, 241, 480, 259] - ZOrder 108 + ZOrder 105 } Block { BlockType Outport @@ -19924,28 +19899,28 @@ Model { IconDisplay "Port number" } Line { - ZOrder 141 + ZOrder 134 SrcBlock "umin" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 142 + ZOrder 135 SrcBlock "umax" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 143 + ZOrder 136 SrcBlock "u" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 144 + ZOrder 137 SrcBlock "tol" SrcPort 1 DstBlock " SFunction " @@ -19953,7 +19928,7 @@ Model { } Line { Name "inRange" - ZOrder 145 + ZOrder 138 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -19961,14 +19936,14 @@ Model { DstPort 1 } Line { - ZOrder 146 + ZOrder 139 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 147 + ZOrder 140 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -20034,7 +20009,6 @@ Model { BackgroundColor "green" ShowName off RequestExecContextInheritance off - Variant off Object { $PropName "MaskObject" $ObjectID 23 @@ -20064,7 +20038,6 @@ Model { Position [5, 15, 130, 75] ZOrder -7 RequestExecContextInheritance off - Variant off System { Name "GAZEBO_SYNCHRONIZER" Location [65, 24, 2560, 1440] @@ -20100,6 +20073,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" SourceType "Simulator Synchronizer" + MultiThreadCoSim "auto" period "Config.Ts" serverPortName "'/clock/rpc'" clientPortName "'/WBT_synchronizer/rpc:o'" @@ -20139,7 +20113,6 @@ Model { Position [-170, 12, -45, 72] ZOrder -9 RequestExecContextInheritance off - Variant off System { Name "REAL_TIME_SYNC" Location [65, 24, 2560, 1440] @@ -20176,6 +20149,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" + MultiThreadCoSim "auto" period "Config.Ts" } } @@ -20205,6 +20179,7 @@ Model { LibraryVersion "1.649" SourceBlock "WBToolboxLibrary/Utilities/Yarp Clock" SourceType "YARP Clock" + MultiThreadCoSim "auto" } Line { ZOrder 1 @@ -20248,7 +20223,6 @@ Model { ZOrder 555 BackgroundColor "orange" RequestExecContextInheritance off - Variant off System { Name "tauDot Saturation" Location [65, 24, 2560, 1440] @@ -20294,7 +20268,6 @@ Model { TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" - Variant off System { Name "Saturate the Torque Derivative" Location [223, 338, 826, 833] @@ -20310,7 +20283,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "50" + SIDHighWatermark "47" Block { BlockType Inport Name "u" @@ -20331,25 +20304,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4556::49" + SID "4556::46" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 39 + ZOrder 36 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4556::48" - Tag "Stateflow S-Function torqueBalancingYoga2016 11" + SID "4556::45" + Tag "Stateflow S-Function torqueBalancingYoga 11" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 38 + ZOrder 35 FunctionName "sf_sfun" Parameters "Config" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -20359,9 +20332,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4556::50" + SID "4556::47" Position [460, 241, 480, 259] - ZOrder 40 + ZOrder 37 } Block { BlockType Outport @@ -20372,7 +20345,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 65 + ZOrder 60 SrcBlock "u" SrcPort 1 Points [120, 0] @@ -20380,7 +20353,7 @@ Model { DstPort 1 } Line { - ZOrder 66 + ZOrder 61 SrcBlock "u_0" SrcPort 1 DstBlock " SFunction " @@ -20388,7 +20361,7 @@ Model { } Line { Name "uSat" - ZOrder 67 + ZOrder 62 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -20396,14 +20369,14 @@ Model { DstPort 1 } Line { - ZOrder 68 + ZOrder 63 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 69 + ZOrder 64 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -20717,13 +20690,13 @@ Model { } #Finite State Machines # -# Stateflow 80000012 +# Stateflow 80000014 # # Stateflow { machine { id 1 - name "torqueBalancingYoga2016" + name "torqueBalancingYoga" created "18-Feb-2014 10:14:40" isLibrary 0 sfVersion 80000010 From 744677118d0f30f040f90ff36cdabf7306aded86 Mon Sep 17 00:00:00 2001 From: Gabriele Nava Date: Tue, 9 Oct 2018 17:33:02 +0200 Subject: [PATCH 4/5] fixed a bug in torso coupling matrix --- .../app/robots/iCubGenova02/initCoordinator.m | 6 +++--- .../app/robots/iCubGenova02/initStateMachineStandup.m | 6 +++--- .../app/robots/icubGazeboSim/initCoordinator.m | 6 +++--- .../app/robots/icubGazeboSim/initStateMachineStandup.m | 6 +++--- .../app/robots/iCubGazeboV2_5/initCoordinator.m | 6 +++--- .../app/robots/iCubGazeboV2_5/initStateMachineYoga.m | 6 +++--- .../app/robots/iCubGenova02/initCoordinator.m | 6 +++--- .../app/robots/iCubGenova02/initStateMachineYoga.m | 6 +++--- .../app/robots/iCubGenova04/initCoordinator.m | 6 +++--- .../app/robots/iCubGenova04/initStateMachineYoga.m | 6 +++--- .../app/robots/icubGazeboSim/initCoordinator.m | 6 +++--- .../app/robots/icubGazeboSim/initStateMachineYoga.m | 6 +++--- 12 files changed, 36 insertions(+), 36 deletions(-) diff --git a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m b/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m index 8f9a47a..0d49332 100644 --- a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m +++ b/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m @@ -126,9 +126,9 @@ 1 t 0; 0 -t t]; -T_torso = [0 -0.5 0.5; - 0 0.5 0.5; - r/R r/(2*R) r/(2*R)]; +T_torso = [-0.5 0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; if Config.INCLUDE_COUPLING diff --git a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m b/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m index 83afd8b..ff666d7 100644 --- a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m +++ b/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m @@ -157,9 +157,9 @@ 1 t 0; 0 -t t]; -T_torso = [0 -0.5 0.5; - 0 0.5 0.5; - r/R r/(2*R) r/(2*R)]; +T_torso = [-0.5 0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; if Config.INCLUDE_COUPLING diff --git a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m b/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m index 4434124..9716cf5 100644 --- a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m +++ b/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m @@ -126,9 +126,9 @@ 1 t 0; 0 -t t]; -T_torso = [0 -0.5 0.5; - 0 0.5 0.5; - r/R r/(2*R) r/(2*R)]; +T_torso = [-0.5 0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; if Config.INCLUDE_COUPLING diff --git a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m b/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m index f5b8b6f..61d3b43 100644 --- a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m +++ b/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m @@ -157,9 +157,9 @@ 1 t 0; 0 -t t]; -T_torso = [0 -0.5 0.5; - 0 0.5 0.5; - r/R r/(2*R) r/(2*R)]; +T_torso = [-0.5 0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; if Config.INCLUDE_COUPLING diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m index d60c09e..c95d95b 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m @@ -115,9 +115,9 @@ 1 t 0; 0 -t t]; -T_torso = [0 -0.5 0.5; - 0 0.5 0.5; - r/R r/(2*R) r/(2*R)]; +T_torso = [-0.5 0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; if Config.INCLUDE_COUPLING 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 4b6a150..40dc339 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 @@ -460,9 +460,9 @@ 1 t 0; 0 -t t]; -T_torso = [0 -0.5 0.5; - 0 0.5 0.5; - r/R r/(2*R) r/(2*R)]; +T_torso = [-0.5 0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; if Config.INCLUDE_COUPLING diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m index 8535b59..6bc4e84 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m @@ -115,9 +115,9 @@ 1 t 0; 0 -t t]; -T_torso = [0 -0.5 0.5; - 0 0.5 0.5; - r/R r/(2*R) r/(2*R)]; +T_torso = [-0.5 0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; if Config.INCLUDE_COUPLING 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 1acacda..b804a12 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m @@ -462,9 +462,9 @@ 1 t 0; 0 -t t]; -T_torso = [0 -0.5 0.5; - 0 0.5 0.5; - r/R r/(2*R) r/(2*R)]; +T_torso = [-0.5 0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; if Config.INCLUDE_COUPLING diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m index 741c0df..81e2d98 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m @@ -115,9 +115,9 @@ 1 t 0; 0 -t t]; -T_torso = [0 -0.5 0.5; - 0 0.5 0.5; - r/R r/(2*R) r/(2*R)]; +T_torso = [-0.5 0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; if Config.INCLUDE_COUPLING 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 ab6f0e1..d64f910 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m @@ -465,9 +465,9 @@ 1 t 0; 0 -t t]; -T_torso = [0 -0.5 0.5; - 0 0.5 0.5; - r/R r/(2*R) r/(2*R)]; +T_torso = [-0.5 0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; if Config.INCLUDE_COUPLING diff --git a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m index e83e88b..3f78aa4 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m +++ b/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m @@ -115,9 +115,9 @@ 1 t 0; 0 -t t]; -T_torso = [0 -0.5 0.5; - 0 0.5 0.5; - r/R r/(2*R) r/(2*R)]; +T_torso = [-0.5 0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; if Config.INCLUDE_COUPLING 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 befe7ec..32719a9 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m @@ -346,9 +346,9 @@ 1 t 0; 0 -t t]; -T_torso = [0 -0.5 0.5; - 0 0.5 0.5; - r/R r/(2*R) r/(2*R)]; +T_torso = [-0.5 0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; if Config.INCLUDE_COUPLING From 94e1a9fa7ba68500e28000c0c7b0274207298ae2 Mon Sep 17 00:00:00 2001 From: Gabriele Nava Date: Tue, 9 Oct 2018 18:27:25 +0200 Subject: [PATCH 5/5] fixed again torso coupling matrix --- .../app/robots/iCubGenova02/initCoordinator.m | 2 +- .../app/robots/iCubGenova02/initStateMachineStandup.m | 2 +- .../app/robots/icubGazeboSim/initCoordinator.m | 2 +- .../app/robots/icubGazeboSim/initStateMachineStandup.m | 2 +- .../app/robots/iCubGazeboV2_5/initCoordinator.m | 4 ++-- .../app/robots/iCubGazeboV2_5/initStateMachineYoga.m | 2 +- .../app/robots/iCubGenova02/initCoordinator.m | 2 +- .../app/robots/iCubGenova02/initStateMachineYoga.m | 2 +- .../app/robots/iCubGenova04/initCoordinator.m | 2 +- .../app/robots/iCubGenova04/initStateMachineYoga.m | 2 +- .../app/robots/icubGazeboSim/initCoordinator.m | 2 +- .../app/robots/icubGazeboSim/initStateMachineYoga.m | 2 +- 12 files changed, 13 insertions(+), 13 deletions(-) diff --git a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m b/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m index 0d49332..0fe4e8b 100644 --- a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m +++ b/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m @@ -126,7 +126,7 @@ 1 t 0; 0 -t t]; -T_torso = [-0.5 0.5 0; +T_torso = [ 0.5 -0.5 0; 0.5 0.5 0; r/(2*R) r/(2*R) r/R]; diff --git a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m b/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m index ff666d7..e3b7657 100644 --- a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m +++ b/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m @@ -157,7 +157,7 @@ 1 t 0; 0 -t t]; -T_torso = [-0.5 0.5 0; +T_torso = [ 0.5 -0.5 0; 0.5 0.5 0; r/(2*R) r/(2*R) r/R]; diff --git a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m b/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m index 9716cf5..cffdc27 100644 --- a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m +++ b/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m @@ -126,7 +126,7 @@ 1 t 0; 0 -t t]; -T_torso = [-0.5 0.5 0; +T_torso = [ 0.5 -0.5 0; 0.5 0.5 0; r/(2*R) r/(2*R) r/R]; diff --git a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m b/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m index 61d3b43..f178334 100644 --- a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m +++ b/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m @@ -157,7 +157,7 @@ 1 t 0; 0 -t t]; -T_torso = [-0.5 0.5 0; +T_torso = [ 0.5 -0.5 0; 0.5 0.5 0; r/(2*R) r/(2*R) r/R]; diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m index c95d95b..66d347f 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m @@ -115,10 +115,10 @@ 1 t 0; 0 -t t]; -T_torso = [-0.5 0.5 0; +T_torso = [ 0.5 -0.5 0; 0.5 0.5 0; r/(2*R) r/(2*R) r/R]; - + if Config.INCLUDE_COUPLING Config.T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12)); 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 40dc339..6c5a347 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 @@ -460,7 +460,7 @@ 1 t 0; 0 -t t]; -T_torso = [-0.5 0.5 0; +T_torso = [ 0.5 -0.5 0; 0.5 0.5 0; r/(2*R) r/(2*R) r/R]; diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m index 6bc4e84..4abb537 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m @@ -115,7 +115,7 @@ 1 t 0; 0 -t t]; -T_torso = [-0.5 0.5 0; +T_torso = [ 0.5 -0.5 0; 0.5 0.5 0; r/(2*R) r/(2*R) r/R]; 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 b804a12..1817fa0 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m @@ -462,7 +462,7 @@ 1 t 0; 0 -t t]; -T_torso = [-0.5 0.5 0; +T_torso = [ 0.5 -0.5 0; 0.5 0.5 0; r/(2*R) r/(2*R) r/R]; diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m index 81e2d98..3424452 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m @@ -115,7 +115,7 @@ 1 t 0; 0 -t t]; -T_torso = [-0.5 0.5 0; +T_torso = [ 0.5 -0.5 0; 0.5 0.5 0; r/(2*R) r/(2*R) r/R]; 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 d64f910..ead5af3 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m @@ -465,7 +465,7 @@ 1 t 0; 0 -t t]; -T_torso = [-0.5 0.5 0; +T_torso = [ 0.5 -0.5 0; 0.5 0.5 0; r/(2*R) r/(2*R) r/R]; diff --git a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m index 3f78aa4..563f243 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m +++ b/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m @@ -115,7 +115,7 @@ 1 t 0; 0 -t t]; -T_torso = [-0.5 0.5 0; +T_torso = [ 0.5 -0.5 0; 0.5 0.5 0; r/(2*R) r/(2*R) r/R]; 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 32719a9..5916e6a 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m +++ b/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m @@ -346,7 +346,7 @@ 1 t 0; 0 -t t]; -T_torso = [-0.5 0.5 0; +T_torso = [ 0.5 -0.5 0; 0.5 0.5 0; r/(2*R) r/(2*R) r/R];