From 725c82208590f0be1bf3511cd5a71d1dd512d712 Mon Sep 17 00:00:00 2001 From: iCubGenova04 Date: Thu, 4 Oct 2018 16:16:45 +0200 Subject: [PATCH] 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 {