diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/CMakeLists.txt b/controllers/floating-base-balancing-torque-control-with-simulator/CMakeLists.txt new file mode 100644 index 00000000..2d0a60ef --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/CMakeLists.txt @@ -0,0 +1 @@ +register_mdl(MODELNAME "torqueControlBalancingWithSimu") \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/README.md b/controllers/floating-base-balancing-torque-control-with-simulator/README.md new file mode 100644 index 00000000..b5214cf1 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/README.md @@ -0,0 +1,29 @@ +## Module description + +This module implements a torque control balancing strategy. It computes the interaction forces at the feet in order to stabilise a desired `centroidal momentum` dynamics, which implies the tracking of a desired center-of-mass trajectory. A cost function penalizing high joint torques - that generate the feet forces - is added to the control framework. + + + +For details see also: [iCub whole-body control through force regulation on rigid non-coplanar contacts](http://journal.frontiersin.org/article/10.3389/frobt.2015.00006/abstract) and [Stability Analysis and Design of Momentum-Based Controllers for Humanoid Robots](https://ieeexplore.ieee.org/document/7759126). + +### Compatibility + +The folder contains the Simulink model `torqueControlBalancing.mdl`, which is generated by using Matlab R2017b. + +### Supported robots + +Currently, supported robots are: `iCubGenova04`, `iCubGenova02`, `icubGazeboSim`, `iCubGazeboV2_5`. + +## Module details + +### How to run the demo + +For information on how to use the controllers both in **simulation** and with the **real robot**, please refer to the **wiki** of the repo. + +### Configuration file + +At start, the module calls the initialization file initTorqueControlBalancing.m. Once opened, this file contains some configuration variables. Please follow the instruction inside the script to properly configure your simulation. + +### Robot and demo specific configurations + +The gains and references for a specific robot (specified by the variable YARP_ROBOT_NAME) or a specific demo can be found in the folder `app/robots/YARP_ROBOT_NAME`. diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobot.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobot.m new file mode 100644 index 00000000..55304cd7 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobot.m @@ -0,0 +1,103 @@ +% CONFIGROBOT initializes parameters specific of a particular robot +% (e.g., icubGazeboSim) + +%% --- Initialization --- + +Config.ON_GAZEBO = true; +ROBOT_DOF = 23; +Config.GRAV_ACC = 9.81; + +% Robot configuration for WBToolbox +WBTConfigRobot = WBToolbox.Configuration; +WBTConfigRobot.RobotName = 'icubSim'; +WBTConfigRobot.UrdfFile = 'model.urdf'; +WBTConfigRobot.LocalName = 'WBT'; + +% Controlboards and joints list. Each joint is associated to the corresponding controlboard +WBTConfigRobot.ControlBoardsNames = {'torso','left_arm','right_arm','left_leg','right_leg'}; +WBTConfigRobot.ControlledJoints = []; +Config.numOfJointsForEachControlboard = []; + +ControlBoards = struct(); +ControlBoards.(WBTConfigRobot.ControlBoardsNames{1}) = {'torso_pitch','torso_roll','torso_yaw'}; +ControlBoards.(WBTConfigRobot.ControlBoardsNames{2}) = {'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow'}; +ControlBoards.(WBTConfigRobot.ControlBoardsNames{3}) = {'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow'}; +ControlBoards.(WBTConfigRobot.ControlBoardsNames{4}) = {'l_hip_pitch','l_hip_roll','l_hip_yaw','l_knee','l_ankle_pitch','l_ankle_roll'}; +ControlBoards.(WBTConfigRobot.ControlBoardsNames{5}) = {'r_hip_pitch','r_hip_roll','r_hip_yaw','r_knee','r_ankle_pitch','r_ankle_roll'}; + +for n = 1:length(WBTConfigRobot.ControlBoardsNames) + + WBTConfigRobot.ControlledJoints = [WBTConfigRobot.ControlledJoints, ... + ControlBoards.(WBTConfigRobot.ControlBoardsNames{n})]; + Config.numOfJointsForEachControlboard = [Config.numOfJointsForEachControlboard; length(ControlBoards.(WBTConfigRobot.ControlBoardsNames{n}))]; +end + +% Total degrees of freedom +Config.N_DOF = numel(WBTConfigRobot.ControlledJoints); + +% Frames list +Frames.BASE = 'root_link'; +Frames.IMU = 'imu_frame'; +Frames.LEFT_FOOT = 'l_sole'; +Frames.RIGHT_FOOT = 'r_sole'; +Frames.COM = 'com'; + +% Config.SATURATE_TORQUE_DERIVATIVE: if true, the derivative of the control +% input is saturated. In this way, it is possible to reduce high frequency +% oscillations and discontinuities in the control input. +Config.SATURATE_TORQUE_DERIVATIVE = true; + +% if TRUE, the controller will STOP if the joints hit the joints limits +% and/or if the (unsigned) difference between two consecutive joints +% encoders measurements is greater than a given threshold. +Config.EMERGENCY_STOP_WITH_JOINTS_LIMITS = false; +Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES = false; + +% Config.USE_MOTOR_REFLECTED_INERTIA: if set to true, motors reflected +% inertias are included in the system mass matrix. If +% Config.INCLUDE_COUPLING is true, then the coupling effect (some joints +% motion is the result of more than one motor motion) is taken into account. +% Config.INCLUDE_HARMONIC_DRIVE_INERTIA is true, then the harmonic drive +% reflected inertia is also considered +Config.USE_MOTOR_REFLECTED_INERTIA = false; +Config.INCLUDE_COUPLING = false; +Config.INCLUDE_HARMONIC_DRIVE_INERTIA = false; + +% Config.USE_IMU4EST_BASE: if set to false, the base frame is estimated by +% assuming that either the left or the right foot stay stuck on the ground. +% Which foot the controller uses depends on the contact forces acting on it. +% If set to true, the base orientation is estimated by using the IMU, while +% the base position by assuming that the origin of either the right or the +% left foot do not move. +Config.USE_IMU4EST_BASE = false; + +% Config.YAW_IMU_FILTER when the flag Config.USE_IMU4EST_BASE = true, then +% the orientation of the floating base is estimated as explained above. However, +% the foot is usually perpendicular to gravity when the robot stands on flat +% surfaces, and rotation about the gravity axis may be affected by the IMU drift +% in estimating this angle. Hence, when either of the flags Config.YAW_IMU_FILTER +% is set to true, then the yaw angle of the contact foot is ignored and kept +% equal to the initial value. +Config.FILTER_IMU_YAW = true; + +% Config.CORRECT_NECK_IMU: when set equal to true, the kinematics from the +% IMU and the contact foot is corrected by using the neck angles. If it set +% equal to false, recall that the neck is assumed to be in (0,0,0). Valid +% ONLY while using the ICUB HEAD IMU! +Config.CORRECT_NECK_IMU = false; + +% Config.USE_QP_SOLVER: if set to true, a QP solver is used to account for +% inequality constraints of contact wrenches. +Config.USE_QP_SOLVER = true; + +% Ports name list +Ports.WRENCH_LEFT_FOOT = '/wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o'; +Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o'; +Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; +Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; + +% Ports dimensions +Ports.NECK_POS_PORT_SIZE = 3; +Ports.IMU_PORT_SIZE = 12; +Ports.IMU_PORT_ORIENTATION_INDEX = [1,2,3]; +Ports.WRENCH_PORT_SIZE = 6; \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobotSim.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobotSim.m new file mode 100644 index 00000000..44b1c405 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobotSim.m @@ -0,0 +1,189 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% % +% COMMON ROBOT CONFIGURATION PARAMETERS % +% % +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%% Init simulator core physics paramaters +physics_config.GRAVITY_ACC = [0,0,-9.81]; +physics_config.TIME_STEP = Config.tStepSim; + +% Robot configuration for WBToolbox +WBTConfigRobotSim = WBToolbox.Configuration; +WBTConfigRobotSim.RobotName = 'icubSim'; +WBTConfigRobotSim.UrdfFile = 'model.urdf'; +WBTConfigRobotSim.LocalName = 'WBT'; +WBTConfigRobotSim.GravityVector = physics_config.GRAVITY_ACC; + +% Controlboards and joints list. Each joint is associated to the corresponding controlboard +WBTConfigRobotSim.ControlBoardsNames = {'torso','left_arm','right_arm','left_leg','right_leg'}; %,'head'}; +WBTConfigRobotSim.ControlledJoints = []; +numOfJointsForEachControlboard = []; + +ControlBoards = struct(); +ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{1}) = {'torso_pitch','torso_roll','torso_yaw'}; +ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{2}) = {'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow'}; +ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{3}) = {'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow'}; +ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{4}) = {'l_hip_pitch','l_hip_roll','l_hip_yaw','l_knee','l_ankle_pitch','l_ankle_roll'}; +ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{5}) = {'r_hip_pitch','r_hip_roll','r_hip_yaw','r_knee','r_ankle_pitch','r_ankle_roll'}; +% ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{6}) = {'neck_pitch','neck_roll','neck_yaw'}; + +for n = 1:length(WBTConfigRobotSim.ControlBoardsNames) + WBTConfigRobotSim.ControlledJoints = [WBTConfigRobotSim.ControlledJoints, ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{n})]; + numOfJointsForEachControlboard = [numOfJointsForEachControlboard; length(ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{n}))]; +end + +% structure used to configure the Robot class +% +robot_config.jointOrder = WBTConfigRobotSim.ControlledJoints; +robot_config.numOfJointsForEachControlboard = numOfJointsForEachControlboard; + +% Note: Since iDynTree 3.0.0, if meshFilePrefix='', the standard iDynTree workflow of locating the +% mesh via the ExternalMesh.getFileLocationOnLocalFileSystem method is used. The iCub model meshes +% file tree is compatible with this workflow. +robot_config.meshFilePrefix = ''; +robot_config.fileName = WBTConfigRobotSim.UrdfFile; +robot_config.N_DOF = numel(WBTConfigRobotSim.ControlledJoints); +robot_config.N_DOF_MATRIX = eye(robot_config.N_DOF); + +% Initial condition of iCub and for the integrators. +initialConditions.base_position = [0; 0; 0.619]; +initialConditions.orientation = diag([-1, -1, 1]); +initialConditions.w_H_b = mwbs.State.Rp2H(initialConditions.orientation, initialConditions.base_position); + +% joint (inital) position +initialConditions.s = [ + 0; 0; 0; ... + -35.97; 29.97; 0.06; 50.00; ... + -35.97; 29.97; 0.06; 50.00; ... + 10; 0; 0; -20; -10; 0; ... + 10; 0; 0; -20; -10; 0]*pi/180; + +% velocty initial conditions +initialConditions.base_linear_velocity = [0; 0; 0]; +initialConditions.base_angular_velocity = [0; 0; 0]; +initialConditions.base_pose_dot = [initialConditions.base_linear_velocity; initialConditions.base_angular_velocity]; +initialConditions.s_dot = zeros(robot_config.N_DOF, 1); + +robot_config.initialConditions = initialConditions; + +% Reflected inertia +robot_config.SIMULATE_MOTOR_REFLECTED_INERTIA = true; +INCLUDE_COUPLING = true; + +% Robot frames list +FramesSim.BASE = 'root_link'; +FramesSim.IMU = 'imu_frame'; +FramesSim.LEFT_FOOT = 'l_sole'; +FramesSim.RIGHT_FOOT = 'r_sole'; +FramesSim.COM = 'com'; + +robot_config.robotFrames = FramesSim; + +% structure used to configure the Contacts class +% + +% foot print of the feet (iCub) +vertex = zeros(3, 4); +vertex(:, 1) = [-0.06; 0.04; 0]; +vertex(:, 2) = [0.11; 0.04; 0]; +vertex(:, 3) = [0.11; -0.035; 0]; +vertex(:, 4) = [-0.06; -0.035; 0]; + +contact_config.foot_print = vertex; +contact_config.total_num_vertices = size(vertex,2)*2; + +% friction coefficient for the feet +contact_config.friction_coefficient = 0.1; + +%% Motors reflected inertia + +% transmission ratio (1/N) +Gamma = 0.01*eye(ROBOT_DOF); + +% modify the value of the transmission ratio for the hip pitch. +% TODO: avoid to hard-code the joint numbering +Gamma(end-5, end-5) = 0.0067; +Gamma(end-11,end-11) = 0.0067; + +% motors inertia (Kg*m^2) +legsMotors_I_m = 0.0827*1e-4; +torsoPitchRollMotors_I_m = 0.0827*1e-4; +torsoYawMotors_I_m = 0.0585*1e-4; +armsMotors_I_m = 0.0585*1e-4; + +% add harmonic drives reflected inertia +legsMotors_I_m = legsMotors_I_m + 0.054*1e-4; +torsoPitchRollMotors_I_m = torsoPitchRollMotors_I_m + 0.054*1e-4; +torsoYawMotors_I_m = torsoYawMotors_I_m + 0.021*1e-4; +armsMotors_I_m = armsMotors_I_m + 0.021*1e-4; + +I_m = diag([torsoPitchRollMotors_I_m*ones(2,1); + torsoYawMotors_I_m; + armsMotors_I_m*ones(8,1); + legsMotors_I_m*ones(12,1)]); + +% parameters for coupling matrices. Updated according to the wiki: +% +% http://wiki.icub.org/wiki/ICub_coupled_joints +% +% and corrected according to https://github.com/robotology/robots-configuration/issues/39 +t = 0.615; +r = 0.022; +R = 0.04; + +% coupling matrices +T_LShoulder = [-1 0 0; + -1 -t 0; + 0 t -t]; + +T_RShoulder = [ 1 0 0; + 1 t 0; + 0 -t t]; + +T_torso = [ 0.5 -0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; + +if INCLUDE_COUPLING + T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12)); +else + T = eye(robot_config.N_DOF); +end + +motorsReflectedInertia = wbc.computeMotorsReflectedInertia(Gamma,T,I_m); + +% Joint friction + +% === Mapping === +jointDefaultOrder = {... + 'torso_pitch','torso_roll','torso_yaw', ... + 'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow', ... + 'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow', ... + 'l_hip_pitch','l_hip_roll','l_hip_yaw','l_knee','l_ankle_pitch','l_ankle_roll', ... + 'r_hip_pitch','r_hip_roll','r_hip_yaw','r_knee','r_ankle_pitch','r_ankle_roll'}; + +KvmechMappingTorso = 0.2*ones(3,1); +KvmechMappingLeftArm = 0.2*ones(4,1); +KvmechMappingRightArm = 0.2*ones(4,1); +KvmechMappingLeftLeg = [0.2 0.2 0.2 0.2 0.6 0.6]'; +KvmechMappingRightLeg = [0.2 0.2 0.2 0.2 0.6 0.6]'; + +KvmechMapping = containers.Map(... + jointDefaultOrder, ... + [ + KvmechMappingTorso + KvmechMappingLeftArm + KvmechMappingRightArm + KvmechMappingLeftLeg + KvmechMappingRightLeg + ]); + +KvmechMat = diag(cell2mat(KvmechMapping.values(robot_config.jointOrder))); + +jointFrictionMat = wbc.computeMotorsReflectedInertia(eye(robot_config.N_DOF),T,KvmechMat); + +%% size of the square you see around the robot +visualizerAroundRobot = 1; % mt + +clear ControlBoards numOfJointsForEachControlboard FramesSim initialConditions vertex diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m new file mode 100644 index 00000000..0c4cc531 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m @@ -0,0 +1,160 @@ +% CONFIGSTATEMACHINE configures the state machine (type of demo, velocity +% of the demo, repeat movements, and so on). + +%% --- Initialization --- + +% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) +Config.LEFT_RIGHT_MOVEMENTS = false; + +% If equal to one, the desired values of the center of mass are smoothed internally +Config.SMOOTH_COM_DES = true; + +% If equal to one, the desired values of the postural tasks are smoothed internally +Config.SMOOTH_JOINT_DES = true; + +% Joint torques saturation [Nm] +Sat.torque = 60; + +% Joint torques rate of change saturation +Sat.uDotMax = 300; + +% max unsigned difference between two consecutive (measured) joint positions, +% i.e. delta_qj = abs(qj(k) - qj(k-1)) +Sat.maxJointsPositionDelta = 15*pi/180; % [rad] + +%% Regularization parameters +Reg.pinvDamp_baseVel = 1e-7; % For the base velocity estmation. +Reg.pinvDamp = 1; % Used in the null space calculation. The higher the value and the more coupled the postural task is with the momentum control. +Reg.pinvTol = 1e-5; % Related to the pseudo-inverse computation for the momentum control before the QP. +Reg.KP_postural = 0.1; % Not the actual postural gains. +Reg.KD_postural = 0; % Not the actual postural gains. +Reg.HessianQP = 1e-7; + +%% State Machine configuration + +% time between two yoga positions +StateMachine.joints_pauseBetweenYogaMoves = 5; + +% contact forces threshold +StateMachine.wrench_thresholdContactOn = 50; +StateMachine.wrench_thresholdContactOff = 100; + +% threshold on CoM and joints error +StateMachine.CoM_threshold = 0.01; +StateMachine.joints_thresholdNotInContact = 5; +StateMachine.joints_thresholdInContact = 50; + +% initial state for state machine +StateMachine.initialState = 1; + +% other configuration parameters for state machine +StateMachine.tBalancing = 1; +StateMachine.tBalancingBeforeYoga = 1; +StateMachine.yogaExtended = true; +StateMachine.skipYoga = false; +StateMachine.demoOnlyBalancing = false; +StateMachine.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first +StateMachine.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. +StateMachine.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. +StateMachine.oneFootYogaInLoop = false; +StateMachine.yogaCounter = 5; + +%% Parameters for motors reflected inertia + +% transmission ratio (1/N) +Config.Gamma = 0.01*eye(ROBOT_DOF); + +% modify the value of the transmission ratio for the hip pitch. +% TODO: avoid to hard-code the joint numbering +Config.Gamma(end-5, end-5) = 0.0067; +Config.Gamma(end-11,end-11) = 0.0067; + +% motors inertia (Kg*m^2) +legsMotors_I_m = 0.0827*1e-4; +torsoPitchRollMotors_I_m = 0.0827*1e-4; +torsoYawMotors_I_m = 0.0585*1e-4; +armsMotors_I_m = 0.0585*1e-4; + +% add harmonic drives reflected inertia +if Config.INCLUDE_HARMONIC_DRIVE_INERTIA + + legsMotors_I_m = legsMotors_I_m + 0.054*1e-4; + torsoPitchRollMotors_I_m = torsoPitchRollMotors_I_m + 0.054*1e-4; + torsoYawMotors_I_m = torsoYawMotors_I_m + 0.021*1e-4; + armsMotors_I_m = armsMotors_I_m + 0.021*1e-4; +end + +Config.I_m = diag([torsoPitchRollMotors_I_m*ones(2,1); + torsoYawMotors_I_m; + armsMotors_I_m*ones(8,1); + legsMotors_I_m*ones(12,1)]); + +% parameters for coupling matrices. Updated according to the wiki: +% +% http://wiki.icub.org/wiki/ICub_coupled_joints +% +% and corrected according to https://github.com/robotology/robots-configuration/issues/39 +t = 0.615; +r = 0.022; +R = 0.04; + +% coupling matrices +T_LShoulder = [-1 0 0; + -1 -t 0; + 0 t -t]; + +T_RShoulder = [ 1 0 0; + 1 t 0; + 0 -t t]; + +T_torso = [ 0.5 -0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; + +if Config.INCLUDE_COUPLING + + Config.T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12)); +else + Config.T = eye(ROBOT_DOF); +end + +% gain for feedforward term in joint torques calculation. Valid range: a +% value between 0 and 1 +Config.K_ff = 0; + +% Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA if true, the desired joints +% accelerations are used for computing the feedforward term in joint +% torques calculations. Not effective if Config.K_ff = 0. +Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA = false; + +%% Constraints for QP for balancing + +% The friction cone is approximated by using linear interpolation of the circle. +% So, numberOfPoints defines the number of points used to interpolate the circle +% in each cicle's quadrant +numberOfPoints = 4; +forceFrictionCoefficient = 1/3; +torsionalFrictionCoefficient = 1/75; +fZmin = 10; + +% physical size of the foot +feet_size = [-0.07 0.12 ; % xMin, xMax + -0.045 0.05 ]; % yMin, yMax + +% Compute contact constraints (friction cone, unilateral constraints) +[ConstraintsMatrix, bVectorConstraints] = wbc.computeRigidContactConstraints ... + (forceFrictionCoefficient, numberOfPoints, torsionalFrictionCoefficient, feet_size, fZmin); \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m new file mode 100644 index 00000000..47555122 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m @@ -0,0 +1,344 @@ +% GAINSANDREFERENCES compute gains matrices, references and other control +% related quantities for each state of the state machine. + +%% --- Initialization --- + +% CoM gains +Gain.KP_CoM = [50 50 10 % state == 1 TWO FEET BALANCING + 50 50 10 % state == 2 COM TRANSITION TO LEFT + 50 50 10 % state == 3 LEFT FOOT BALANCING + 50 50 10 % state == 4 YOGA LEFT FOOT + 50 50 10 % state == 5 PREPARING FOR SWITCHING + 50 50 10 % state == 6 LOOKING FOR CONTACT + 50 50 10 % state == 7 TRANSITION TO INITIAL POSITION + 50 50 10 % state == 8 COM TRANSITION TO RIGHT FOOT + 50 50 10 % state == 9 RIGHT FOOT BALANCING + 50 50 10 % state == 10 YOGA RIGHT FOOT + 50 50 10 % state == 11 PREPARING FOR SWITCHING + 50 50 10 % state == 12 LOOKING FOR CONTACT + 50 50 10]; % state == 13 TRANSITION TO INITIAL POSITION + +% The KD gain is usually lowered w.r.t. the critical damping value for compensating the velocity +% measurement noise on the real robot and on gazebo. +Gain.KD_CoM = 2*sqrt(Gain.KP_CoM)/2; + +% Angular momentum gains +Gain.KI_AngularMomentum = 0.25 ; +Gain.KP_AngularMomentum = 2*sqrt(Gain.KI_AngularMomentum); + +% Postural task gains +% % TORSO %% LEFT ARM %% RIGHT ARM %% LEFT LEG %% RIGHT LEG %% +Gain.KP_postural = [10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 1 TWO FEET BALANCING + 10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 2 COM TRANSITION TO LEFT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 100 100, 30 30 20 20 100 100 % state == 3 LEFT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10, 100 200 100 400 100 100, 100 50 30 100 100 100 % state == 4 YOGA LEFT FOOT + 30 30 30, 5 5 10 10, 10 10 20 10, 200 250 20 20 10 50, 220 350 120 200 65 100 % state == 5 PREPARING FOR SWITCHING + 30 30 30, 10 10 20 10, 10 10 20 10, 100 350 20 200 10 100, 220 350 120 200 65 100 % state == 6 LOOKING FOR CONTACT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 5 5, 30 30 30 20 5 5 % state == 7 TRANSITION TO INITIAL POSITION + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 100 100, 30 30 30 20 100 100 % state == 8 COM TRANSITION TO RIGHT FOOT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 9 RIGHT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10, 100 200 100 400 100 100, 100 50 30 100 100 100 % state == 10 YOGA RIGHT FOOT + 30 30 30, 5 5 10 10, 10 10 20 10, 200 250 20 20 10 50, 220 350 120 200 65 100 % state == 11 PREPARING FOR SWITCHING + 30 30 30, 10 10 20 10, 10 10 20 10, 100 350 20 200 10 100, 220 350 120 200 65 100 % state == 12 LOOKING FOR CONTACT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100]; % state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_postural = 2*sqrt(Gain.KP_postural(1,:))/2; + +% symmetric gains +Gain.KP_postural(8:12,:) = Gain.KP_postural(2:6,:); +Gain.KP_postural(8:12,12:17) = Gain.KP_postural(2:6,18:23); +Gain.KP_postural(8:12,18:23) = Gain.KP_postural(2:6,12:17); + +%% Smoothing times + +% Smoothing time gain scheduling +Config.SmoothingTimeGainScheduling = 2; + +% Smoothing time CoM references +StateMachine.CoMSmoothingTime = [1; %% state == 1 TWO FEET BALANCING + 1; %% state == 2 COM TRANSITION TO LEFT FOOT + 1; %% state == 3 LEFT FOOT BALANCING + 2; %% state == 4 YOGA LEFT FOOT + 2; %% state == 5 PREPARING FOR SWITCHING + 2; %% state == 6 LOOKING FOR CONTACT + 1; %% state == 7 TRANSITION INIT POSITION + 1; %% state == 8 COM TRANSITION TO RIGHT FOOT + 1; %% state == 9 RIGHT FOOT BALANCING + 2; %% state == 10 YOGA RIGHT FOOT + 2; %% state == 11 PREPARING FOR SWITCHING + 2; %% state == 12 LOOKING FOR CONTACT + 10]; %% state == 13 TRANSITION INIT POSITION + +% Smoothing time for joints references +StateMachine.jointsSmoothingTime = [1; %% state == 1 TWO FEET BALANCING + 1; %% state == 2 COM TRANSITION TO LEFT FOOT + 1; %% state == 3 LEFT FOOT BALANCING + 2; %% state == 4 YOGA LEFT FOOT + 2; %% state == 5 PREPARING FOR SWITCHING + 2; %% state == 6 LOOKING FOR CONTACT + 1; %% state == 7 TRANSITION INIT POSITION + 1; %% state == 8 COM TRANSITION TO RIGHT FOOT + 1; %% state == 9 RIGHT FOOT BALANCING + 2; %% state == 10 YOGA RIGHT FOOT + 2; %% state == 11 PREPARING FOR SWITCHING + 2; %% state == 12 LOOKING FOR CONTACT + 10]; %% state == 13 TRANSITION INIT POSITION + +% scale factor smoothing time multiplies the smoothing factor during the +% Yoga (state 4 and 10). The purpose is to reduce the time necessary for +% the reference to converge to the next position, but without changing also +% the valuse stored in Sm.joints_leftYogaRef/Sm.joints_rightYogaRef +StateMachine.scaleFactorSmoothingTime = 0.9; + +%% CoM delta + +% To be summed to the reference CoM position +StateMachine.CoM_delta = [% THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE LEFT FOOT + 0.0, 0.00, 0.0; %% NOT USED + 0.0, 0.00, -0.0; %% state == 2 COM TRANSITION TO LEFT FOOT + 0.0, 0.00, -0.0; %% state == 3 LEFT FOOT BALANCING + 0.0, 0.005,-0.0; %% state == 4 YOGA LEFT FOOT + 0.0, 0.00, -0.0; %% state == 5 PREPARING FOR SWITCHING + 0.0, -0.09, -0.0; %% state == 6 LOOKING FOR CONTACT + 0.0, 0.00, -0.0; %% state == 7 TWO FEET BALANCING + % THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE RIGHT FOOT + 0.0, 0.00, -0.0; %% state == 8 COM TRANSITION TO RIGHT FOOT + 0.0, 0.00, -0.0; %% state == 9 RIGHT FOOT BALANCING + 0.0, -0.005,-0.0; %% state == 10 YOGA RIGHT FOOT + 0.0, 0.00, -0.0; %% state == 11 PREPARING FOR SWITCHING + 0.0, 0.09, -0.0; %% state == 12 LOOKING FOR CONTACT + 0.0, 0.00, 0.0]; %% NOT USED + +%% Joint references +StateMachine.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 2 COM TRANSITION TO LEFT + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % + [ 0.0864,0.0258,0.0152, ... %% state == 3 LEFT FOOT BALANCING + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 5 PREPARING FOR SWITCHING + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % + [ 0.0864,0.0258,0.0152, ... %% state == 6 LOOKING FOR CONTACT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [ 0.0864,0.0258,0.0152, ... %% state == 8 COM TRANSITION TO RIGHT FOOT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % + [ 0.0864,0.0258,0.0152, ... %% state == 9 RIGHT FOOT BALANCING + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 11 PREPARING FOR SWITCHING + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % + [ 0.0864,0.0258,0.0152, ... %% state == 12 LOOKING FOR CONTACT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369]; % + zeros(1,ROBOT_DOF)]; %% THIS REFERENCE IS IGNORED + +% YOGA MOVESET (joint references during state 4 and 10) +q1 = [-0.0790,0.2279, 0.4519, ... + -1.1621,0.6663, 0.4919, 0.9947, ... + -1.0717,1.2904,-0.2447, 1.0948, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3484,0.4008,-0.0004,-0.3672,-0.1060,-0.0875]; + +q2 = [-0.0790,0.2279, 0.4519, ... + -1.1621,0.6663, 0.4965, 0.9947, ... + -1.0717,1.2904,-0.2493, 1.0948, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... + 0.3714,0.9599, 1.3253,-1.6594,-0.1060,-0.0614]; + +q3 = [-0.0852,-0.4273,0.0821,... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... + 0.3714,0.9599, 1.3253,-1.6594, 0.5000,-0.0614]; + +q4 = [-0.0852,-0.4273,0.0821,... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.3473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q5 = [-0.0790,-0.2273, 0.4519, ... + -1.1621,0.6663, 0.4965, 0.9947, ... + -1.0717,1.2904,-0.2493, 1.0948, ... + 0.2092, 0.4473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q6 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q7 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253, -1.6217, 0.5000,-0.0614]; + +q8 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q9 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614]; + +q10 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q11 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q12 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q13 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q14 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614]; + +q15 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 1.5514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q16 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.2514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614]; + +q17 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + -0.3514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +StateMachine.joints_leftYogaRef = [ 0, q1; + 1*StateMachine.jointsSmoothingTime(4),q2; + 2*StateMachine.jointsSmoothingTime(4),q3; + 3*StateMachine.jointsSmoothingTime(4),q4; + 4*StateMachine.jointsSmoothingTime(4),q5; + 5*StateMachine.jointsSmoothingTime(4),q6; + 6*StateMachine.jointsSmoothingTime(4),q7; + 7*StateMachine.jointsSmoothingTime(4),q8; + 8*StateMachine.jointsSmoothingTime(4),q9; + 9*StateMachine.jointsSmoothingTime(4),q10; + 10*StateMachine.jointsSmoothingTime(4),q11; + 11*StateMachine.jointsSmoothingTime(4),q12; + 12*StateMachine.jointsSmoothingTime(4),q13; + 13*StateMachine.jointsSmoothingTime(4),q14; + 14*StateMachine.jointsSmoothingTime(4),q15; + 15*StateMachine.jointsSmoothingTime(4),q16; + 16*StateMachine.jointsSmoothingTime(4),q17; + 17*StateMachine.jointsSmoothingTime(4),q10; + 18*StateMachine.jointsSmoothingTime(4),q11; + 19*StateMachine.jointsSmoothingTime(4),q12; + 20*StateMachine.jointsSmoothingTime(4),q13; + 21*StateMachine.jointsSmoothingTime(4),q14; + 22*StateMachine.jointsSmoothingTime(4),q15; + 23*StateMachine.jointsSmoothingTime(4),q16; + 24*StateMachine.jointsSmoothingTime(4),q17; + 25*StateMachine.jointsSmoothingTime(4),q8]; + +StateMachine.joints_rightYogaRef = StateMachine.joints_leftYogaRef; +StateMachine.joints_rightYogaRef(:,1) = [0; + 1*StateMachine.jointsSmoothingTime(10); + 2*StateMachine.jointsSmoothingTime(10); + 3*StateMachine.jointsSmoothingTime(10); + 4*StateMachine.jointsSmoothingTime(10); + 5*StateMachine.jointsSmoothingTime(10); + 6*StateMachine.jointsSmoothingTime(10); + 7*StateMachine.jointsSmoothingTime(10); + 8*StateMachine.jointsSmoothingTime(10); + 9*StateMachine.jointsSmoothingTime(10); + 10*StateMachine.jointsSmoothingTime(10); + 11*StateMachine.jointsSmoothingTime(10); + 12*StateMachine.jointsSmoothingTime(10); + 13*StateMachine.jointsSmoothingTime(10); + 14*StateMachine.jointsSmoothingTime(10); + 15*StateMachine.jointsSmoothingTime(10); + 16*StateMachine.jointsSmoothingTime(10); + 17*StateMachine.jointsSmoothingTime(10); + 18*StateMachine.jointsSmoothingTime(10); + 19*StateMachine.jointsSmoothingTime(10); + 20*StateMachine.jointsSmoothingTime(10); + 21*StateMachine.jointsSmoothingTime(10); + 22*StateMachine.jointsSmoothingTime(10); + 23*StateMachine.jointsSmoothingTime(10); + 24*StateMachine.jointsSmoothingTime(10); + 25*StateMachine.jointsSmoothingTime(10)]; + +% if the demo is not "yogaExtended", stop at the 8th move +if ~StateMachine.yogaExtended + + StateMachine.joints_leftYogaRef = StateMachine.joints_leftYogaRef(1:8,:); + StateMachine.joints_rightYogaRef = StateMachine.joints_rightYogaRef(1:8,:); + StateMachine.joints_leftYogaRef(8,1) = 15*StateMachine.jointsSmoothingTime(4); + StateMachine.joints_rightYogaRef(8,1) = 15*StateMachine.jointsSmoothingTime(10); +end + +% MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA +for i = 1:size(StateMachine.joints_rightYogaRef,1) + + StateMachine.joints_rightYogaRef(i,2:4) = [StateMachine.joints_rightYogaRef(i,2) -StateMachine.joints_rightYogaRef(i,3) -StateMachine.joints_rightYogaRef(i,4)]; + rightArm = StateMachine.joints_rightYogaRef(i,end-15:end-12); + StateMachine.joints_rightYogaRef(i,end-15:end-12) = StateMachine.joints_rightYogaRef(i,end-19:end-16); + StateMachine.joints_rightYogaRef(i,end-19:end-16) = rightArm; + rightLeg = StateMachine.joints_rightYogaRef(i,end-5:end); + StateMachine.joints_rightYogaRef(i,end-5:end) = StateMachine.joints_rightYogaRef(i,end-11:end-6); + StateMachine.joints_rightYogaRef(i,end-11:end-6) = rightLeg; +end + +%% References for CoM trajectory (COORDINATOR DEMO ONLY) + +% that the robot waits before starting the left-and-right +Config.noOscillationTime = 0; +Config.directionOfOscillation = [0;1;0]; +Config.amplitudeOfOscillation = 0.02; % [m] +Config.frequencyOfOscillation = 0.2; % [Hz] \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/images/initNeckPosBlock.jpeg b/controllers/floating-base-balancing-torque-control-with-simulator/images/initNeckPosBlock.jpeg new file mode 100644 index 00000000..2d1b18eb Binary files /dev/null and b/controllers/floating-base-balancing-torque-control-with-simulator/images/initNeckPosBlock.jpeg differ diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancingWithSimu.m b/controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancingWithSimu.m new file mode 100644 index 00000000..9ef27f7a --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancingWithSimu.m @@ -0,0 +1,92 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% /** +% * Copyright (C) 2016 CoDyCo +% * @author: Daniele Pucci, Gabriele Nava +% * Permission is granted to copy, distribute, and/or modify this program +% * under the terms of the GNU General Public License, version 2 or any +% * later version published by the Free Software Foundation. +% * +% * A copy of the license can be found at +% * http://www.robotcub.org/icub/license/gpl.txt +% * +% * This program is distributed in the hope that it will be useful, but +% * WITHOUT ANY WARRANTY; without even the implied warranty of +% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General +% * Public License for more details +% */ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% NOTE: THIS SCRIPT IS RUN AUTOMATICALLY WHEN THE USER STARTS THE ASSOCIATED +% SIMULINK MODEL. NO NEED TO RUN THIS SCRIPT EVERY TIME. +clearvars -except sl_synch_handles simulinkStaticGUI +clc + +% Add path to local source code +addpath('./src/') + +%% GENERAL SIMULATION INFO +% +% If you are simulating the robot with Gazebo, remember that it is required +% to launch Gazebo as follows: +% +% gazebo -slibgazebo_yarp_clock.so +% +% and properly set the environmental variable YARP_ROBOT_NAME in the .bashrc. + +% Simulation time in seconds. For long simulations, put an high number +% (not inf) for allowing automatic code generation +Config.SIMULATION_TIME = 600000; + +% Controller period [s] +Config.tStep = 0.01; % 10 ms +Config.tStepSim = 0.001; % 1 ms + +%% PRELIMINARY CONFIGURATION +% +% DEMO_TYPE: defines the kind of demo that will be performed. +% +% 'YOGA': the robot will perform the YOGA++ demo (highly dynamic movements +% while balancing on one foot and two feet) +% +% 'COORDINATOR': the robot can either balance on two feet or move from left +% to right follwing a desired center-of-mass trajectory. +% +DEMO_TYPE = 'YOGA'; + +% Config.SCOPES: debugging scopes activation +Config.SCOPES_WRENCHES = true; +Config.SCOPES_GAIN_SCHE_INFO = true; +Config.SCOPES_MAIN = true; +Config.SCOPES_QP = true; + +% DATA PROCESSING +% +% Save the Matlab workspace after stopping the simulation +Config.SAVE_WORKSPACE = false; + +% Verify that the integration time has been respected during the simulation +Config.CHECK_INTEGRATION_TIME = true; +Config.PLOT_INTEGRATION_TIME = false; + +% Run robot-specific configuration parameters +supportedMmodels = {'iCubGazeboV2_5'}; +switch(getenv('YARP_ROBOT_NAME')) + case 'iCubGazeboV2_5' + otherwise + error(['Unsupported robot model. Supported models are listed below:',repmat('\n- %s',[1 numel(supportedMmodels)])],supportedMmodels{:}); +end +run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configRobot.m')); +run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configStateMachine.m')); +run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/gainsAndReferences.m')); +run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configRobotSim.m')); +run('initVisualizer'); + +% Deactivate/activate the internal coordinator +if strcmpi(DEMO_TYPE, 'COORDINATOR') + + Config.COORDINATOR_DEMO = true; + +elseif strcmpi(DEMO_TYPE, 'YOGA') + + Config.COORDINATOR_DEMO = false; +end \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src-static-gui/closeModel.m b/controllers/floating-base-balancing-torque-control-with-simulator/src-static-gui/closeModel.m new file mode 100644 index 00000000..496418a0 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/src-static-gui/closeModel.m @@ -0,0 +1,20 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Save and close the Simulink model through Matlab command line. +% It also closes the associate static GUI + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('[closeModel]: closing the Simulink model...') + +% save and close the Simulink model +save_system('torqueControlBalancing.mdl'); +close_system('torqueControlBalancing.mdl'); + +% close all figures +close all + +% remove paths (optional) +rmpath('../../library/matlab-gui'); +rmpath('./src-static-gui'); + +disp('[closeModel]: done.') \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src-static-gui/compileModel.m b/controllers/floating-base-balancing-torque-control-with-simulator/src-static-gui/compileModel.m new file mode 100644 index 00000000..e6e31a1b --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/src-static-gui/compileModel.m @@ -0,0 +1,24 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Compile the Simulink model through Matlab command line. + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Compiling the Simulink model...') + +pause(1.5) + +try + torqueControlBalancing([], [], [], 'compile') + torqueControlBalancing([], [], [], 'term') + +catch ME + + errorMessages = ME; +end + +clc + +disp('Compilation done.') + +% warning about Simulink timing +warning('The model will anyways start with FEW SECONDS OF DELAY after pressing the ''Start Model'' button.') \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m b/controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m new file mode 100644 index 00000000..8d7d8ae9 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m @@ -0,0 +1,25 @@ +% configuration for the matlab iDyntree visualizer + +%% Specific parameters + +% Do you want to enable the Visualizer? +confVisualizer.visualizeRobot = true; + +% size of the square you see around the robot +confVisualizer.aroundRobot = 1; % [m] + +% refresh rate of the picure +confVisualizer.tStep = 0.010; % here equal to the time step used in the simulink model + + +%% Parameters copied from robot_config + +% Robot description +confVisualizer.fileName = robot_config.fileName; +confVisualizer.meshFilePrefix = robot_config.meshFilePrefix; +confVisualizer.jointOrder = robot_config.jointOrder; +confVisualizer.robotFrames = robot_config.robotFrames; + +% initial joints configuration specified in configRobot +confVisualizer.joints_positions = robot_config.initialConditions.s; +confVisualizer.world_H_base = robot_config.initialConditions.w_H_b; diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src/momentumBasedController.m b/controllers/floating-base-balancing-torque-control-with-simulator/src/momentumBasedController.m new file mode 100644 index 00000000..22f8f628 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/src/momentumBasedController.m @@ -0,0 +1,328 @@ +function [HessianMatrixOneFoot, gradientOneFoot, ConstraintsMatrixOneFoot, bVectorConstraintsOneFoot, ... + HessianMatrixTwoFeet, gradientTwoFeet, ConstraintsMatrixTwoFeet, bVectorConstraintsTwoFeet, ... + tauModel, Sigma, Na, f_LDot] = ... + momentumBasedController(feetContactStatus, ConstraintsMatrix, bVectorConstraints, jointPos, jointPos_des, nu, M, h, L, intL_angMomError, w_H_l_sole, w_H_r_sole, ... + J_l_sole, J_r_sole, JDot_l_sole_nu, JDot_r_sole_nu, pos_CoM, J_CoM, desired_pos_vel_acc_CoM, KP_CoM, KD_CoM, KP_postural, Config, Reg, Gain) + + % MOMENTUMBASEDCONTROLLER implements a momentum-based whole body + % balancing controller for humanoid robots. + % + % REFERENCES: G. Nava and F. Romano and F. Nori and D. Pucci, + % "Stability Analysis and Design of Momentum Based Controllers for Humanoid Robots", + % Available at: https://ieeexplore.ieee.org/document/7759126/ + + %% --- Initialization --- + + % Compute the momentum rate of change. The momentum rate of change + % equals the summation of the external forces and moments, i.e.: + % + % LDot = A*f + f_grav (1) + % + % where A is the matrix mapping the forces and moments into the + % momentum equations, f_grav is the gravity force, f is a vector stacking + % all the external forces and moments acting on the robot as follows: + % + % f = [f_left; f_right] + % + % where f_left are the forces and moments acting on the left foot and + % f_right are the forces and moments acting on the right foot. + + % Compute the gravity force + m = M(1,1); + gravAcc = Config.GRAV_ACC; + f_grav = [zeros(2,1); + -m*gravAcc; + zeros(3,1)]; + + % Compute matrix A in Eq. (1) + pos_leftFoot = w_H_l_sole(1:3,4); + pos_rightFoot = w_H_r_sole(1:3,4); + + % Distance between the application points of the contact forces w.r.t. CoM + r_left = pos_leftFoot - pos_CoM; + r_right = pos_rightFoot - pos_CoM; + + % Partition matrix A into the part that multiplies the left foot + % wrenches and the right foot wrenches, i.e. A = [A_left, A_right] + A_left = [eye(3), zeros(3); + wbc.skew(r_left), eye(3)]; + A_right = [eye(3), zeros(3); + wbc.skew(r_right), eye(3)]; + + A = [A_left, A_right]; + + %% MOMENTUM CONTROL + % + % We would like to achieve a desired momentum's dynamics: + % + % LDot_star = LDot_des - KP_momentum*(L-LDes) - KI_momentum*(intL-intLDes) + % + % where intL is the integral of the momentum. Assume the contact forces + % and moments can be considered as control inputs of Eq. (1). Then, the + % problem is to find f such that: + % + % LDot_star = A*f + f_grav (2) + % + % We must now distinguish two different cases: + % + % CASE 1: the robot is balancing on one foot. In this case, the solution + % to Eq. (2) is: + % + % f = A^(-1)*(LDot_star - f_grav) (3) + % + % CASE 2: the robot is balancing on two feet. In this case, there is + % redundancy as there are more control inputs (12) than variables + % to control (6). Therefore one can write: + % + % f = pinvA*(LDot_star - f_grav) + Na*f_0 (4) + % + % where pinvA is the pseudoinverse of matrix A and Na is its null space + % projector. f_0 is a free variable that does not affect the momentum + % dynamics Eq (1). + + % Gains mapping. + % + % KP_momentum = blkdiag(KD_CoM, KP_angMom) + % KD_momentum = blkdiag(KP_CoM, KI_angMom) + % + KP_angMom = Gain.KP_AngularMomentum*eye(3); + KI_angMom = Gain.KI_AngularMomentum*eye(3); + + % Desired CoM dynamics (conseguently, linear momentum) + vel_CoM = J_CoM(1:3,:) * nu; + acc_CoM_star = desired_pos_vel_acc_CoM(:,3) - KP_CoM*(pos_CoM - desired_pos_vel_acc_CoM(:,1)) - KD_CoM*(vel_CoM - desired_pos_vel_acc_CoM(:,2)); + + % Desired momentum dynamics + LDot_star = [m * acc_CoM_star; + (-KP_angMom * L(4:end) -KI_angMom * intL_angMomError)]; + + %% CASE 1: one foot balancing + % + % In this case, we make use of a QP solver. In particular, Eq. (3) is rewritten as: + % + % f^T*A^T*A*f - f^T*A^T*(LDot_star - f_grav) = 0 (5) + % + % that is the quadratic problem associated with Eq. (3). Now rewrite + % Eq. (5) as: + % + % f^T*Hessian*f + f^T*gradient = 0 + % + % where Hessian = A^T*A and gradient = - A^T*(LDot_star - f_grav). Now + % it is possible to solve the folowing QP problem: + % + % f_star = argmin_f |f^T*Hessian*f + f^T*gradient|^2 + % + % s.t. C*f < b + % + % where the inequality constraints represent the unilateral, friction + % cone and local CoP constraints at the foot. + + % Hessian matrix and gradient QP one foot + A_oneFoot = A_left*feetContactStatus(1)*(1 - feetContactStatus(2)) + A_right*feetContactStatus(2)*(1 - feetContactStatus(1)); + HessianMatrixOneFoot = A_oneFoot'*A_oneFoot + eye(size(A_oneFoot,2))*Reg.HessianQP; + gradientOneFoot = -A_oneFoot'*(LDot_star - f_grav); + + % Update constraint matrices. The contact wrench associated with the + % left foot (resp. right foot) is subject to the following constraint: + % + % ConstraintMatrixLeftFoot * l_sole_f_left < bVectorConstraints + % + % In this case, however, f_left is expressed w.r.t. the frame l_sole, + % which is solidal to the left foot. The contact forces f used in the + % controller however are expressed w.r.t. the frame l_sole[w], that is + % a frame with the origin at the contact location but the orientation + % of the inertial frame. For this reason, the mapping between the two + % frames is given by: + % + % l_sole_f_left = blkdiag(l_sole_R_w, l_sole_R_w) * l_sole[w]_f_left + % + % therefore we rewrite the contact constraints as: + % + % ConstraintMatrixLeftFoot * blkdiag(l_sole_R_w, l_sole_R_w) * l_sole[w]_f_left < bVectorConstraints + % + % and this in the end results in updating the constraint matrix as follows: + % + % ConstraintMatrixLeftFoot = ConstraintMatrixLeftFoot * blkdiag(l_sole_R_w, l_sole_R_w) + % + % The same holds for the right foot. + % + w_R_r_sole = w_H_r_sole(1:3,1:3); + w_R_l_sole = w_H_l_sole(1:3,1:3); + ConstraintMatrixLeftFoot = ConstraintsMatrix * blkdiag(w_R_l_sole', w_R_l_sole'); + ConstraintMatrixRightFoot = ConstraintsMatrix * blkdiag(w_R_r_sole', w_R_r_sole'); + + % One foot constraints + ConstraintsMatrixOneFoot = feetContactStatus(1) * (1 - feetContactStatus(2)) * ConstraintMatrixLeftFoot + ... + feetContactStatus(2) * (1 - feetContactStatus(1)) * ConstraintMatrixRightFoot; + bVectorConstraintsOneFoot = bVectorConstraints; + + %% CASE 2: two feet balancing + % + % In this case, we solve Eq (4) by means of the matrix pseudoinverse and + % NOT through the QP. The QP is instead used to calculate the vector + % projected in the null space (f_0). In particular, we choose f_0 in + % order to minimize the joint torques magnitude. To do so, it is necessary + % to write down the relationship between the joint torques and the + % contact forces: + % + % tau = pinvLambda*(Jc*invM*(h - Jc^T*f) -JcDot_nu) + NLambda*tau_0 (6) + % + % where tau_0 is given by the following equation: + % + % tau_0 = hs - Msb*invMb*hb - (Js^T - Msb*invMb*Jb^T)*f + u_0 + % + % where we have: + % + % M = [Mb, Mbs; h = [hb; Jc = [Jb, Js] + % Msb, Ms]; hs]; + % + % obtained by partitioning the dynamics in order to split the first + % six rows and the remaining NDOF rows. + % + % u_0 instead are the feedback terms associated with the postural task, + % and therefore are given by the following expression (for more info, + % look at the reference paper): + % + % u_0 = -KP_postural*NLambda*Mbar*jointPosTilde -KD_postural*NLambda*Mbar*jointVel + % + % where Mbar = Ms-Msb/Mb*Mbs. + % + % Now, let us rewrite Eq. (6) in order to isolate the terms which + % depend on the contact forces: + % + % tau = Sigma*f + tauModel (7) + % + % where Sigma = -(pinvLambda*Jc*invM*Jc^T + NLambda*(Js^T - Msb*invMb*Jb^T)) + % + % tauModel = pinvLambda*(Jc*invM*h -JcDot_nu) + ... + % NLambda*(hs - Msb*invMb*hb + u_0) + % + % Finally, we substitute Eq. (4) into Eq. (7) which gives: + % + % tau = Sigma*pinvA*(LDot_star - f_grav) + Sigma*Na*f_0 + tauModel (8) + % + % minimizing the torques implies we would like to have tau = 0 in Eq. + % (8) (note that it is not possible to achieve tau = 0 by choosing f_0) + % + % It is possible to write down Eq. (8) as a QP problem, as we + % did for Eq. (5): + % + % f_0^T*Hessian*f_0 + f_0^T*gradient = 0 (9) + % + % where Hessian = transpose(Sigma*Na)*Sigma*Na + % gradient = transpose(Sigma*Na)*(Sigma*pinvA*(LDot_star - f_grav) + tauModel) + % + % The associated QP formulation is now: + % + % f_0_star = argmin_f_0 |f_0^T*Hessian*f_0 + f_0^T*gradient|^2 + % + % s.t. C*f_0 < b + % + % Note that in this way we are assuming that the part of the contact + % forces dedicated to stabilize the momentum dynamics, i.e. the term + % + % f_LDot = pinvA*(LDot_star - f_grav) + % + % is does not violate the constraints. + + % Compute f_LDot + pinvA = pinv(A, Reg.pinvTol); + f_LDot = pinvA*(LDot_star - f_grav); + + % Null space of the matrix A + Na = (eye(12,12) - pinvA*A).*feetContactStatus(1).*feetContactStatus(2); + + %% Compute Sigma and tauModel + % + % NOTE that the formula Eq (7) will be used for computing the torques + % also in case the robot is balancing on ONE foot. In fact, in that + % case, f will be a vector of the form (left foot balancing): + % + % f = [f_left (from QP); zeros(6,1)]; + % + % same holds for the right foot balancing. The additional zeros are + % required in order to match the dimension of Sigma (NDOF x 12). + + % Contact jacobians + NDOF = size(J_l_sole(:,7:end),2); + Jc = [J_l_sole.*feetContactStatus(1); + J_r_sole.*feetContactStatus(2)]; + + % Jacobian derivative dot(Jc)*nu + JcDot_nu = [JDot_l_sole_nu.*feetContactStatus(1); + JDot_r_sole_nu.*feetContactStatus(2)]; + + % Selector of actuated DoFs + B = [zeros(6,NDOF); + eye(NDOF,NDOF)]; + + % The mass matrix is partitioned as: + % + % M = [ Mb, Mbs + % Mbs', Ms ]; + % + % where: Mb \in R^{6 x 6} + % Mbs \in R^{6 x 6+NDOF} + % Ms \in R^{NDOF x NDOF} + % + Mb = M(1:6,1:6); + Mbs = M(1:6,7:end); + Ms = M(7:end,7:end); + + % Get matrix Sigma + Jc_invM = Jc/M; + Lambda = Jc_invM*B; + pinvLambda = wbc.pinvDamped(Lambda, Reg.pinvDamp); + NullLambda = eye(NDOF) - pinvLambda*Lambda; + Sigma = -(pinvLambda*Jc_invM*Jc' + NullLambda*(transpose(Jc(:,7:end)) -Mbs'/Mb*transpose(Jc(:,1:6)))); + + % Mbar is the mass matrix associated with the joint dynamics, i.e. + Mbar = Ms-Mbs'/Mb*Mbs; + NullLambda_Mbar = NullLambda*Mbar; + + % Adaptation of the control gains for back compatibility with the older + % versions of the controller + KP_postural = KP_postural*pinv(NullLambda_Mbar, Reg.pinvTol) + Reg.KP_postural*eye(NDOF); + KD_postural = diag(Gain.KD_postural)*pinv(NullLambda_Mbar,Reg.pinvTol) + Reg.KD_postural*eye(NDOF); + + % Joints velocity and joints position error + jointVel = nu(7:end); + jointPosTilde = jointPos - jointPos_des; + + % Get the vector tauModel + u_0 = -KP_postural*NullLambda_Mbar*jointPosTilde -KD_postural*NullLambda_Mbar*jointVel; + tauModel = pinvLambda*(Jc_invM*h - JcDot_nu) + NullLambda*(h(7:end) - Mbs'/Mb*h(1:6) + u_0); + + %% QP parameters for two feet standing + % + % In the case the robot stands on two feet, the control objective is + % the minimization of the joint torques through the redundancy of the + % contact forces. See Previous comments. + + % Get the inequality constraints matrices + ConstraintsMatrixBothFeet = blkdiag(ConstraintMatrixLeftFoot,ConstraintMatrixRightFoot); + bVectorConstraintsBothFeet = [bVectorConstraints;bVectorConstraints]; + + % The optimization problem Eq. (9) seeks for the redundancy of the external + % wrench that minimize joint torques. Recall that the contact wrench can + % be written as: + % + % f = f_LDot + Na*f_0 + % + % Then, the constraints on the contact wrench is of the form + % + % ConstraintsMatrixBothFeet*f < bVectorConstraints + % + % which in terms of f0 is: + % + % ConstraintsMatrixBothFeet*Na*f0 < bVectorConstraints - ConstraintsMatrixBothFeet*f_LDot + % + ConstraintsMatrixTwoFeet = ConstraintsMatrixBothFeet*Na; + bVectorConstraintsTwoFeet = bVectorConstraintsBothFeet - ConstraintsMatrixBothFeet*f_LDot; + + % Evaluation of Hessian matrix and gradient vector for solving the + % optimization problem Eq. (9) + Sigma_Na = Sigma*Na; + HessianMatrixTwoFeet = Sigma_Na'*Sigma_Na + eye(size(Sigma_Na,2))*Reg.HessianQP; + gradientTwoFeet = Sigma_Na'*(tauModel + Sigma*f_LDot); +end diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src/processOutputQP_oneFoot.m b/controllers/floating-base-balancing-torque-control-with-simulator/src/processOutputQP_oneFoot.m new file mode 100644 index 00000000..27a00f81 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/src/processOutputQP_oneFoot.m @@ -0,0 +1,57 @@ +function f_star = processOutputQP_oneFoot(analyticalSolution,primalSolution,QPStatus,feetContactStatus,Config) + + % PROCESSOUTPUTQP_ONEFOOT evaluates the output of the WBToolbox QP block. + % In case the QP block exited with an error, a + % "default", user defined solution to the QP problem + % is provided instead of the one coming from the QP block. + % + % FORMAT: f_star = processOutputQP_oneFoot(analyticalSolution,primalSolution,QPStatus,feetContactStatus,Config) + % + % INPUT: - analyticalSolution = the alternative user defined solution to the QP + % problem to be used when the QP block fails; + % - primalSolution = the solution to the QP problem provided by the + % WBToolbox QP block; + % - QPStatus = the status check returned by the QP block. + % + % - feetContactStatus = [2 x 1] a vector describing the feet contact + % status. Format: [leftFoot; rightFoot]; + % - Config = a structure containing the robot-related configuration + % parameters; + % + % OUTPUT: - f_star = the final solution to the QP problem that will be used in + % the controller. + % + % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava + % + % all authors are with the Italian Istitute of Technology (IIT) + % email: name.surname@iit.it + % + % Genoa, Dec 2017 + % + + %% --- Initialization --- + + CONTACT_THRESHOLD = 0.1; + QP_STATUS_THRESHOLD = 0.01; + + % if the robot is balancing on one foot, extend the QP solution to + % have the correct dimension of f_star for matrix multiplication + if feetContactStatus(1) > (1 - CONTACT_THRESHOLD) + + % left foot balancing + updated_primalSolution = [primalSolution; zeros(6,1)]; + updated_analyticalSolution = [analyticalSolution; zeros(6,1)]; + + else + % right foot balancing + updated_primalSolution = [zeros(6,1); primalSolution]; + updated_analyticalSolution = [zeros(6,1); analyticalSolution]; + end + + if Config.USE_QP_SOLVER && abs(QPStatus)< QP_STATUS_THRESHOLD + + f_star = updated_primalSolution; + else + f_star = updated_analyticalSolution; + end +end \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src/processOutputQP_twoFeet.m b/controllers/floating-base-balancing-torque-control-with-simulator/src/processOutputQP_twoFeet.m new file mode 100644 index 00000000..7b328705 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/src/processOutputQP_twoFeet.m @@ -0,0 +1,41 @@ +function f_star = processOutputQP_twoFeet(analyticalSolution,primalSolution,QPStatus,Config) + + % PROCESSOUTPUTQP_TWOFEET evaluates the output of the WBToolbox QP block. + % In case the QP block exited with an error, a + % "default", user defined solution to the QP problem + % is provided instead of the one coming from the QP block. + % + % FORMAT: f_star = processOutputQP_twoFeet(analyticalSolution,primalSolution,QPStatus,feetContactStatus,Config) + % + % INPUT: - analyticalSolution = the alternative user defined solution to the QP + % problem to be used when the QP block fails; + % - primalSolution = the solution to the QP problem provided by the + % WBToolbox QP block; + % - QPStatus = the status check returned by the QP block. + % + % - feetContactStatus = [2 x 1] a vector describing the feet contact + % status. Format: [leftFoot; rightFoot]; + % - Config = a structure containing the robot-related configuration + % parameters; + % + % OUTPUT: - f_star = the final solution to the QP problem that will be used in + % the controller. + % + % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava + % + % all authors are with the Italian Istitute of Technology (IIT) + % email: name.surname@iit.it + % + % Genoa, Dec 2017 + % + + %% --- Initialization --- + QP_STATUS_THRESHOLD = 0.01; + + if Config.USE_QP_SOLVER && abs(QPStatus)< QP_STATUS_THRESHOLD + + f_star = primalSolution; + else + f_star = analyticalSolution; + end +end \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src/stateMachineMomentumControl.m b/controllers/floating-base-balancing-torque-control-with-simulator/src/stateMachineMomentumControl.m new file mode 100644 index 00000000..7e3078df --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/src/stateMachineMomentumControl.m @@ -0,0 +1,393 @@ +function [w_H_b, pos_CoM_des, jointPos_des, feetContactStatus, KP_postural_diag, KP_CoM_diag, KD_CoM_diag, state, smoothingTimeJoints, smoothingTimeCoM] = ... + stateMachineMomentumControl(pos_CoM_0, jointPos_0, pos_CoM_fixed_l_sole, pos_CoM_fixed_r_sole, jointPos, ... + time, wrench_rightFoot, wrench_leftFoot, l_sole_H_b, r_sole_H_b, StateMachine, Gain, Config) + + % STATEMACHINEMOMENTUMCONTROL generates the references for performing + % two demos: + % + % YOGA: (highly dynamic movements) on one + % foot and two feet; + % + % COORDINATOR: balancing on two feet while + % performing left-right oscillations. + + %% --- Initialization --- + + persistent currentState; + persistent t_switch; + persistent w_H_fixedLink; + persistent yogaMovesetCounter; + + if isempty(currentState) + + currentState = StateMachine.initialState; + end + if isempty(t_switch) + + t_switch = 0; + end + if isempty(w_H_fixedLink) + + w_H_fixedLink = eye(4); + end + if isempty(yogaMovesetCounter) + + yogaMovesetCounter = 1; + end + + % initialize outputs + pos_CoM_des = pos_CoM_0; + feetContactStatus = [1; 1]; + jointPos_des = jointPos_0; + w_H_b = eye(4); + + %% STATE 1: TWO FEET BALANCING + if currentState == 1 + + w_H_b = w_H_fixedLink * l_sole_H_b; + + % if the demo is COORDINATOR, keep balancing or start to perform + % left-right movements with the CoM + if Config.COORDINATOR_DEMO + + if Config.LEFT_RIGHT_MOVEMENTS + + if time > Config.noOscillationTime + + Amplitude = Config.amplitudeOfOscillation; + else + Amplitude = 0; + end + + frequency = Config.frequencyOfOscillation; + pos_CoM_des = pos_CoM_0 + Amplitude*sin(2*pi*frequency*time)*Config.directionOfOscillation; + end + else + % after tBalancing time start moving the weight to the left + if time > StateMachine.tBalancing + + currentState = 2; + + if StateMachine.demoStartsOnRightSupport + + w_H_fixedLink = w_H_fixedLink*l_sole_H_b/r_sole_H_b; + currentState = 8; + end + end + end + end + + %% STATE 2: TRANSITION TO THE LEFT FOOT + if currentState == 2 + + w_H_b = w_H_fixedLink * l_sole_H_b; + + % Set the center of mass projection onto the x-y plane to be + % coincident to the origin of the left foot (l_sole) plus a + % configurable delta + pos_CoM_des = [w_H_fixedLink(1:2,4); pos_CoM_0(3)] + StateMachine.CoM_delta(currentState,:)'; + + fixed_link_CoMDes = w_H_fixedLink\[pos_CoM_des; 1]; + pos_CoM_error = fixed_link_CoMDes(1:3) - pos_CoM_fixed_l_sole(1:3); + jointPos_des = StateMachine.joints_references(currentState,:)'; + + if norm(pos_CoM_error(2)) < StateMachine.CoM_threshold && wrench_rightFoot(3) < StateMachine.wrench_thresholdContactOff + + currentState = 3; + t_switch = time; + end + end + + %% STATE 3: LEFT FOOT BALANCING + if currentState == 3 + + w_H_b = w_H_fixedLink * l_sole_H_b; + + % Set the center of mass projection onto the x-y plane to be + % coincident to the origin of the left foot (l_sole) plus a + % configurable delta + pos_CoM_des = [w_H_fixedLink(1:2,4); pos_CoM_0(3)] + StateMachine.CoM_delta(currentState,:)'; + + % right foot is no longer in contact + feetContactStatus = [1; 0]; + + jointPos_des = StateMachine.joints_references(currentState,:)'; + + if time > (t_switch + StateMachine.tBalancingBeforeYoga) + + currentState = 4; + t_switch = time; + + if StateMachine.skipYoga + + currentState = 5; + end + end + end + + %% STATE 4: YOGA LEFT FOOT + if currentState == 4 + + w_H_b = w_H_fixedLink * l_sole_H_b; + + % Set the center of mass projection onto the x-y plane to be + % coincident to the origin of the left foot (l_sole) plus a + % configurable delta + pos_CoM_des = [w_H_fixedLink(1:2,4); pos_CoM_0(3)] + StateMachine.CoM_delta(currentState,:)'; + feetContactStatus = [1; 0]; + jointPos_des = StateMachine.joints_references(currentState,:)'; + + % iterate over the yoga positions + for i = 1: size(StateMachine.joints_leftYogaRef,1)-1 + + % positions for the yoga movements + if time > (StateMachine.joints_leftYogaRef(i,1) + t_switch) && time <= (StateMachine.joints_leftYogaRef(i+1,1)+ t_switch) + + jointPos_des = StateMachine.joints_leftYogaRef(i,2:end)'; + end + end + if time > (StateMachine.joints_leftYogaRef(end,1) + t_switch) + + jointPos_des = StateMachine.joints_leftYogaRef(end,2:end)'; + + % if StateMachine.yogaCounter > 1, yoga in the loop. Repeat the Yoga movements N times + if time > (StateMachine.joints_leftYogaRef(end,1) + t_switch + StateMachine.jointsSmoothingTime(currentState) + StateMachine.joints_pauseBetweenYogaMoves) + + t_switch = time; + 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 > StateMachine.yogaCounter || ~StateMachine.oneFootYogaInLoop + + currentState = 5; + end + end + end + end + + %% STATE 5: PREPARING FOR SWITCHING + if currentState == 5 + + w_H_b = w_H_fixedLink * l_sole_H_b; + + % Set the center of mass projection onto the x-y plane to be + % coincident to the origin of the left foot (l_sole) plus a + % configurable delta + pos_CoM_des = [w_H_fixedLink(1:2,4); pos_CoM_0(3)] + StateMachine.CoM_delta(currentState,:)'; + feetContactStatus = [1; 0]; + jointPos_des = StateMachine.joints_references(currentState,:)'; + + jointPos_errorRLeg = jointPos(end-5:end) - jointPos_des(end-5:end); + jointPos_errorLLeg = jointPos(end-11:end-6) - jointPos_des(end-11:end-6); + + if norm(jointPos_errorRLeg)*180/pi < StateMachine.joints_thresholdNotInContact && norm(jointPos_errorLLeg)*180/pi < StateMachine.joints_thresholdInContact + + currentState = 6; + t_switch = time; + end + end + + %% STATE 6: LOOKING FOR A CONTACT + if currentState == 6 + + w_H_b = w_H_fixedLink * l_sole_H_b; + + % Set the center of mass projection onto the x-y plane to be + % coincident to the origin of the left foot (l_sole) plus a + % configurable delta + pos_CoM_des = [w_H_fixedLink(1:2,4); pos_CoM_0(3)] + StateMachine.CoM_delta(currentState,:)'; + feetContactStatus = [1; 0]; + jointPos_des = StateMachine.joints_references(currentState,:)'; + + if wrench_rightFoot(3) > StateMachine.wrench_thresholdContactOn + + currentState = 7; + t_switch = time; + end + end + + %% STATE 7: TRANSITION TO INITIAL POSITION + if currentState == 7 + + w_H_b = w_H_fixedLink * l_sole_H_b; + + pos_CoM_des = pos_CoM_0 + StateMachine.CoM_delta(currentState,:)'; + + % right foot is in contact + feetContactStatus = [1; 1]; + + if norm(pos_CoM_fixed_l_sole(1:2) -pos_CoM_des(1:2)) < 10*StateMachine.CoM_threshold && StateMachine.yogaAlsoOnRightFoot && time > t_switch + StateMachine.tBalancing + + w_H_fixedLink = w_H_fixedLink*l_sole_H_b/r_sole_H_b; + currentState = 8; + t_switch = time; + end + end + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% +%% YOGA RIGHT FOOT %% +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% + + %% STATE 8: TRANSITION TO THE RIGHT FOOT + if currentState == 8 + + w_H_b = w_H_fixedLink * r_sole_H_b; + + % Set the center of mass projection onto the x-y plane to be + % coincident to the origin of the right foot (r_sole) plus a + % configurable delta + pos_CoM_des = [w_H_fixedLink(1:2,4); pos_CoM_0(3)] + StateMachine.CoM_delta(currentState,:)'; + + feetContactStatus = [1; 1]; + fixed_link_CoMDes = w_H_fixedLink\[pos_CoM_des;1]; + pos_CoM_error = fixed_link_CoMDes(1:3) - pos_CoM_fixed_r_sole(1:3); + jointPos_des = StateMachine.joints_references(currentState,:)'; + + if norm(pos_CoM_error(2)) < StateMachine.CoM_threshold && wrench_leftFoot(3) < StateMachine.wrench_thresholdContactOff + + currentState = 9; + t_switch = time; + end + end + + %% STATE 9: RIGHT FOOT BALANCING + if currentState == 9 + + w_H_b = w_H_fixedLink * r_sole_H_b; + + % left foot is no longer in contact + feetContactStatus = [0; 1]; + + pos_CoM_des = [w_H_fixedLink(1:2,4); pos_CoM_0(3)] + StateMachine.CoM_delta(currentState,:)'; + jointPos_des = StateMachine.joints_references(currentState,:)'; + + if time > (t_switch + StateMachine.tBalancingBeforeYoga) + + currentState = 10; + t_switch = time; + + if StateMachine.skipYoga + + currentState = 11; + end + end + end + + %% STATE 10: YOGA RIGHT FOOT + if currentState == 10 + + w_H_b = w_H_fixedLink*r_sole_H_b; + + feetContactStatus = [0; 1]; + pos_CoM_des = [w_H_fixedLink(1:2,4); pos_CoM_0(3)] + StateMachine.CoM_delta(currentState,:)'; + jointPos_des = StateMachine.joints_references(currentState,:)'; + + % iterate over the yoga positions + for i = 1: size(StateMachine.joints_rightYogaRef,1)-1 + + % positions for the yoga movements + if time > (StateMachine.joints_rightYogaRef(i,1) + t_switch) && time <= (StateMachine.joints_rightYogaRef(i+1,1)+ t_switch) + + jointPos_des = StateMachine.joints_rightYogaRef(i,2:end)'; + end + end + if time > (StateMachine.joints_rightYogaRef(end,1) + t_switch) + + jointPos_des = StateMachine.joints_rightYogaRef(end,2:end)'; + + % if StateMachine.yogaCounter > 1, yoga in the loop. Repeat the Yoga movements N times + if time > (StateMachine.joints_rightYogaRef(end,1) + t_switch + StateMachine.jointsSmoothingTime(currentState) + StateMachine.joints_pauseBetweenYogaMoves) + + t_switch = time; + 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 > StateMachine.yogaCounter || ~StateMachine.oneFootYogaInLoop + + currentState = 11; + end + end + end + end + + %% STATE 11: PREPARING FOR SWITCHING + if currentState == 11 + + w_H_b = w_H_fixedLink * r_sole_H_b; + + feetContactStatus = [0; 1]; + pos_CoM_des = [w_H_fixedLink(1:2,4); pos_CoM_0(3)] + StateMachine.CoM_delta(currentState,:)'; + jointPos_des = StateMachine.joints_references(currentState,:)'; + + jointPos_errorRLeg = jointPos(end-5:end) -jointPos_des(end-5:end); + jointPos_errorLLeg = jointPos(end-11:end-6) -jointPos_des(end-11:end-6); + + if norm(jointPos_errorRLeg)*180/pi < StateMachine.joints_thresholdInContact && norm(jointPos_errorLLeg)*180/pi < StateMachine.joints_thresholdNotInContact + + currentState = 12; + t_switch = time; + end + end + + %% STATE 12: LOOKING FOR A CONTACT + if currentState == 12 + + w_H_b = w_H_fixedLink * r_sole_H_b; + + feetContactStatus = [0; 1]; + pos_CoM_des = [w_H_fixedLink(1:2,4); pos_CoM_0(3)] + StateMachine.CoM_delta(currentState,:)'; + jointPos_des = StateMachine.joints_references(currentState,:)'; + + if wrench_leftFoot(3) > StateMachine.wrench_thresholdContactOn + + currentState = 13; + t_switch = time; + end + end + + %% STATE 13: TRANSITION TO INITIAL POSITION + if currentState == 13 + + w_H_b = w_H_fixedLink * r_sole_H_b; + + % left foot is in contact + feetContactStatus = [1; 1]; + + if (time -t_switch) > StateMachine.tBalancing + + if StateMachine.twoFeetYogaInLoop + + currentState = 2; + w_H_fixedLink = w_H_fixedLink*r_sole_H_b/l_sole_H_b; + + if StateMachine.demoStartsOnRightSupport + + currentState = 8; + w_H_fixedLink = w_H_fixedLink*l_sole_H_b/r_sole_H_b; + end + end + end + end + + % Update joints and CoM smoothing time + if currentState == 4 || currentState == 10 + + % during the yoga, reduce the time necessary for the joints + % reference to converge to the next position + smoothingTimeJoints = StateMachine.scaleFactorSmoothingTime*StateMachine.jointsSmoothingTime(currentState); + else + smoothingTimeJoints = StateMachine.jointsSmoothingTime(currentState); + end + + smoothingTimeCoM = StateMachine.CoMSmoothingTime(currentState); + + % update gain matrices + KP_postural_diag = Gain.KP_postural(currentState,:); + KP_CoM_diag = Gain.KP_CoM(currentState,:); + KD_CoM_diag = Gain.KD_CoM(currentState,:); + + % update current state + state = currentState; +end \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/startModelWithStaticGui.m b/controllers/floating-base-balancing-torque-control-with-simulator/startModelWithStaticGui.m new file mode 100644 index 00000000..89496c2d --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/startModelWithStaticGui.m @@ -0,0 +1,29 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% RUN THIS SCRIPT TO USE SIMULINK WITH THE STATIC SIMULINK GUI + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +clear variables +clc + +% add the path to the static gui and to some utility functions +addpath('../../library/matlab-gui'); +addpath('./src-static-gui'); + +disp('[startModel]: loading the model...') + +% open the model +open_system('torqueControlBalancing.mdl','loadonly'); + +% add message to tell the user that the model has been opened correctly +disp('[startModel]: model loaded correctly') +disp('[startModel]: the "Start Model" button is enabled only after compiling the model.') + +% add warning to warn the user NOT to close the GUI +warning('DO NOT CLOSE the GUI. The model won''t be closed! Use "Exit Model" button instead.') + +% check if the GUI is correctly opened +if ~exist('sl_synch_handles', 'var') + + error('The GUI did not load correctly, or it is already opened. Close the GUI, run "closeModel.m" or restart Matlab') +end \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/stopTorqueControlBalancingWithSimu.m b/controllers/floating-base-balancing-torque-control-with-simulator/stopTorqueControlBalancingWithSimu.m new file mode 100644 index 00000000..17d8c535 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/stopTorqueControlBalancingWithSimu.m @@ -0,0 +1,93 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% RUN THIS SCRIPT TO REMOVE LOCAL PATHS ADDED WHEN RUNNING THE +% CONTROLLER. +% +% In the Simulink model, this script is run every time the user presses +% the 'terminate' button. + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +rmpath('./src/') + +% Create a folder for collecting data +if Config.SAVE_WORKSPACE + + if (~exist(['experiments',date],'dir')) + + mkdir(['experiments',date]); + end + + matFileList = dir(['./experiments',date,'/*.mat']); + c = clock; + + save(['./experiments',date,'/exp_',num2str(c(4)),'-',num2str(c(5)),'.mat']) +end + +% Compare the Yarp time with the Simulink time to catch if the required +% integration time step is respected during the simulation +if Config.CHECK_INTEGRATION_TIME && exist('yarp_time','var') + + sim_time = yarp_time.time; + + % normalize the yarp time over the first value (at t_sim = 0) + yarp_time0 = yarp_time.signals.values - yarp_time.signals.values(1); + + % fast check of yarp time vs. Simulink time + if Config.PLOT_INTEGRATION_TIME + + figure + hold on + plot(yarp_time0,0:length(yarp_time0)-1,'o') + plot(sim_time,0:length(sim_time)-1,'o-') + xlabel('Time [s]') + ylabel('Iterations') + grid on + legend('Yarp Time','Simulink Time') + title('Yarp time vs. Simulink time') + end + + % number of times the real time step was bigger than twice the + % desired time step value + numOfTimeStepViolations = sum(diff(yarp_time0) > 2*Config.tStep); + + if numOfTimeStepViolations > 1 && numOfTimeStepViolations <= 50 + + warning(['The time step tolerance of ', num2str(Config.tStep), '[s] has been violated at least once.']) + + elseif numOfTimeStepViolations > 50 + + warning(['The time step tolerance of ', num2str(Config.tStep), '[s] has been violated more than 50 times.']) + end +end + +% If a joint hits the limits or an encoder spike is detected, print a +% warning message displaying the name of the joint +if Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES && exist('res_check_spikes','var') + + if ~isempty(res_check_spikes.signals.values) + + checkEachJoint = res_check_spikes.signals.values(end,:); + + for k = 1:length(checkEachJoint) + + if checkEachJoint(k) + + warning(['A spike occurred on joint ', WBTConfigRobot.ControlledJoints{k}]) + end + end + end +end +if Config.EMERGENCY_STOP_WITH_JOINTS_LIMITS && exist('res_check_range','var') + + if ~isempty(res_check_range.signals.values) + + checkEachJoint = res_check_range.signals.values(end,:); + + for k = 1:length(checkEachJoint) + + if checkEachJoint(k) + + warning(['Joint ', WBTConfigRobot.ControlledJoints{k}, ' exited the range']) + end + end + end +end diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl new file mode 100644 index 00000000..1b451127 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl @@ -0,0 +1,24968 @@ +Model { + Name "torqueControlBalancingWithSimu" + Version 10.2 + SavedCharacterEncoding "UTF-8" + ModelUUID "79a42ce4-6d16-4906-af88-8f5d6a197ba1" + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "3.123" + NumModelReferences 0 + NumTestPointedSignals 0 + NumProvidedFunctions 0 + NumRequiredFunctions 0 + NumResetEvents 0 + HasInitializeEvent 0 + HasTerminateEvent 0 + PreCompExecutionDomainType "Unset" + IsExportFunctionModel 0 + IsArchitectureModel 0 + IsAUTOSARArchitectureModel 0 + NumParameterArguments 0 + NumExternalFileReferences 46 + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Configuration" + Path "torqueControlBalancingWithSimu/Configuration" + SID "4537" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MatchSignalSizes" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desire" + "d Torques/QPSolver/QP One Foot/MatchSignalSizes" + SID "4691" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/QP" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desire" + "d Torques/QPSolver/QP One Foot/QP One Foot" + SID "4693" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MatchSignalSizes" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desire" + "d Torques/QPSolver/QP Two Feet/MatchSignalSizes" + SID "4679" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/QP" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desire" + "d Torques/QPSolver/QP Two Feet/QP Two Feet" + SID "4681" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angula" + "r momentum integral error/CentroidalMomentum" + SID "3718" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angula" + "r momentum integral error/Compute base to fixed link transform/l_sole to root_link relative transform" + SID "4450" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angula" + "r momentum integral error/Compute base to fixed link transform/r_sole to root_link relative transform" + SID "4475" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angula" + "r momentum integral error/Jacobian" + SID "3719" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angula" + "r momentum integral error/Jacobian1" + SID "3720" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/Centr" + "oidalMomentum" + SID "2345" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/GetBiasForces" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/GetBi" + "asForces" + SID "2348" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/MassMatrix" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/MassM" + "atrix" + SID "2349" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/CoM" + SID "4363" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/DotJNu" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Dot" + "JNu l_sole" + SID "2375" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/DotJNu" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Dot" + "JNu r_sole" + SID "2376" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Jac" + "obian com" + SID "2378" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Jac" + "obian l_sole" + SID "2379" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Jac" + "obian r_sole" + SID "2380" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/l_s" + "ole" + SID "2405" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/r_s" + "ole" + SID "2406" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "utilityBlocks_lib/Holder" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/holder 7" + SID "4996" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute Sta" + "te Velocity/Feet Jacobians/Jacobian l_sole" + SID "4219" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute Sta" + "te Velocity/Feet Jacobians/Jacobian r_sole" + SID "4220" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/LFoot to base link transform /Fixe d base to root link transform" + SID "4251" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/LFoot to base link transform /Fixed base to imu transform" + SID "4250" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "utilityBlocks_lib/Holder" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/LFoot to base link transform /holder 1" + SID "4982" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "utilityBlocks_lib/Holder" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/LFoot to base link transform /holder 2" + SID "4983" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/RFoot to base link transform/Fixed base to imu transform" + SID "4855" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/RFoot to base link transform/Fixed base to root link transform" + SID "4856" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "utilityBlocks_lib/Holder" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/RFoot to base link transform/holder 3" + SID "4984" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "utilityBlocks_lib/Holder" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/RFoot to base link transform/holder 4" + SID "4985" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/Relative transform l_sole_H_base" + SID "4269" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/Relative transform r_sole_H_base" + SID "4306" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machi" + "ne/MinimumJerkTrajectoryGenerator" + SID "2176" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "utilityBlocks_lib/Holder" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machi" + "ne/holder 5" + SID "4993" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "utilityBlocks_lib/Holder" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machi" + "ne/holder 6" + SID "4994" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machi" + "ne/xCom/CoM" + SID "4229" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machi" + "ne/xCom2/CoM" + SID "4735" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gain" + "s and References/Smooth reference CoM/MinimumJerkTrajectoryGenerator2" + SID "2153" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gain" + "s and References/Smooth reference joint position/MinimumJerkTrajectoryGenerator1" + SID "2152" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetLimits" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF J" + "OINTS HIT THE LIMITS/GetLimits" + SID "4921" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "mwbs_visualizers_lib/RobotVisualizer" + Path "torqueControlBalancingWithSimu/Robot Visualizer/RobotVisualizer" + SID "4939" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Configuration" + Path "torqueControlBalancingWithSimu/Subsystem/Configuration" + SID "4962" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "mwbs_robotSensors_lib/IMUsensor" + Path "torqueControlBalancingWithSimu/Subsystem/IMUsensor" + SID "4938" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "mwbs_robotDynamicsWithContacts_lib/RobotDynWithContacts" + Path "torqueControlBalancingWithSimu/Subsystem/RobotDynWithContacts" + SID "4937" + Type "LIBRARY_BLOCK" + } + OrderedModelArguments 1 + } + SLCCPlugin "on" + slcheck_filter_plugin "on" + DiagnosticSuppressor "on" + AnimationPlugin "on" + WebScopes_FoundationPlugin "on" + NotesPlugin "on" + LogicAnalyzerPlugin "on" + PostLoadFcn "%% Try to open the static GUI and try to adjust it \ntry\n\n staticGUI = simulinkStaticGUI;\n\nend" + EnableAccessToBaseWorkspace on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + FPTRunName "Run 1" + MaxMDLFileLineLength 120 + CloseFcn "%% Try to close the GUI (the user might have already closed it)\ntry\n\n close(simulinkStaticGUI)\n\nend" + InitFcn "%% initialize the simulation\ncd(fileparts(which(bdroot)));\ninitTorqueControlBalancingWithSimu;\n\n%% T" + "ry to edit the GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n set(sl_synch_handle" + "s.startButton,'Backgroundcolor',[0.0,1.0,0.0]);\n set(sl_synch_handles.startButton,'Enable','on');\n\nend" + StartFcn "%% Try to edit the GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n se" + "t(sl_synch_handles.startButton,'Backgroundcolor',[0.8,0.8,0.8]);\n set(sl_synch_handles.startButton,'Enable','off" + "');\n\n %% Update Compile Button\n set(sl_synch_handles.compileButton,'Backgroundcolor',[0.8,0.8,0.8]);\n s" + "et(sl_synch_handles.compileButton,'Enable','off');\n\n %% Update Exit Button\n set(sl_synch_handles.exitButton" + ",'Backgroundcolor',[0.8,0.8,0.8]);\n set(sl_synch_handles.exitButton,'Enable','off');\n\nend\n\n" + StopFcn "%% stop the simulation\ncd(fileparts(which(bdroot)));\nstopTorqueControlBalancingWithSimu;\n\n%% Try to " + "edit the GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n if get(sl_synch_handles.c" + "heckbox_recompile, 'Value') \n set(sl_synch_handles.startButton,'Backgroundcolor',[0.0,1.0,0.0]);\n s" + "et(sl_synch_handles.startButton,'Enable','on');\n end\n\n %% Update Compile Button\n set(sl_synch_handles.c" + "ompileButton,'Backgroundcolor',[1.0,0.6,0.0]);\n set(sl_synch_handles.compileButton,'Enable','on');\n\n %% Upd" + "ate Exit Button\n set(sl_synch_handles.exitButton,'Backgroundcolor',[0.0,1.0,1.0]);\n set(sl_synch_handles.exi" + "tButton,'Enable','on');\n\nend\n\n\n" + LastSavedArchitecture "maci64" + Object { + $PropName "BdWindowsInfo" + $ObjectID 1 + $ClassName "Simulink.BDWindowsInfo" + Object { + $PropName "WindowsInfo" + $ObjectID 2 + $ClassName "Simulink.WindowInfo" + IsActive [1] + Location [-75.0, -1417.0, 2560.0, 1417.0] + Object { + $PropName "ModelBrowserInfo" + $ObjectID 3 + $ClassName "Simulink.ModelBrowserInfo" + Visible [1] + DockPosition "Left" + Width [50] + Height [50] + Filter [59] + Minimized "Off" + } + Object { + $PropName "ExplorerBarInfo" + $ObjectID 4 + $ClassName "Simulink.ExplorerBarInfo" + Visible [1] + } + Array { + Type "Simulink.EditorInfo" + Dimension 83 + Object { + $ObjectID 5 + IsActive [1] + IsTabbed [1] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [2171.0, 1188.0] + ZoomFactor [2.0627701612843352] + Offset [713.17598406081265, 157.57969597446328] + SceneRectInView [713.17598406081265, 157.57969597446328, 1052.4682006493044, 575.92456120284373] + } + Object { + $ObjectID 6 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4955" + Extents [2171.0, 1188.0] + ZoomFactor [2.0773052223433903] + Offset [-119.91137569995328, 98.052613159122757] + SceneRectInView [-119.91137569995328, 98.052613159122757, 1045.1040013999066, 571.89477368175449] + } + Object { + $ObjectID 7 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4850" + Extents [2171.0, 1188.0] + ZoomFactor [1.3964802319507956] + Offset [400.19641548607717, -203.35510808500248] + SceneRectInView [400.19641548607717, -203.35510808500248, 1554.6227940278457, 850.710216170005] + } + Object { + $ObjectID 8 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4226" + Extents [2171.0, 1188.0] + ZoomFactor [4.6350722637938047] + Offset [12.87372526868586, 135.34665821934539] + SceneRectInView [12.87372526868586, 135.34665821934539, 468.38536196262828, 256.30668356130923] + } + Object { + $ObjectID 9 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4993" + Extents [2171.0, 1188.0] + ZoomFactor [3.8] + Offset [-158.02631578947376, -70.1842105263158] + SceneRectInView [-158.02631578947376, -70.1842105263158, 571.31578947368428, 312.63157894736844] + } + Object { + $ObjectID 10 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4994" + Extents [2171.0, 1188.0] + ZoomFactor [3.8] + Offset [-158.02631578947376, -70.1842105263158] + SceneRectInView [-158.02631578947376, -70.1842105263158, 571.31578947368428, 312.63157894736844] + } + Object { + $ObjectID 11 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4913" + Extents [2171.0, 1188.0] + ZoomFactor [3.2951942101426641] + Offset [80.154339026122614, 154.55463376836479] + SceneRectInView [80.154339026122614, 154.55463376836479, 658.838253999605, 360.52503258937384] + } + Object { + $ObjectID 12 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4216" + Extents [2171.0, 1188.0] + ZoomFactor [5.2673944072532821] + Offset [569.47947453590621, 441.73077216658527] + SceneRectInView [569.47947453590621, 441.73077216658527, 412.15823842818759, 225.53845566682949] + } + Object { + $ObjectID 13 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4269" + Extents [2171.0, 1188.0] + ZoomFactor [5.95] + Offset [-33.73965992647058, -45.831932773109244] + SceneRectInView [-33.73965992647058, -45.831932773109244, 364.8739495798319, 199.66386554621849] + } + Object { + $ObjectID 14 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4620" + Extents [2171.0, 1188.0] + ZoomFactor [10.0] + Offset [82.52421875, 140.1] + SceneRectInView [82.52421875, 140.1, 217.1, 118.8] + } + Object { + $ObjectID 15 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2163" + Extents [2171.0, 1188.0] + ZoomFactor [1.4201130022789792] + Offset [-608.35758121641425, -114.33481638525888] + SceneRectInView [-608.35758121641425, -114.33481638525888, 1528.7515828078519, 836.55314618872774] + } + Object { + $ObjectID 16 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "3241" + Extents [2171.0, 1188.0] + ZoomFactor [1.5612382234185733] + Offset [-827.23464439655174, -71.967241379310337] + SceneRectInView [-827.23464439655174, -71.967241379310337, 1390.5629310344827, 760.93448275862067] + } + Object { + $ObjectID 17 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4030" + Extents [2171.0, 1188.0] + ZoomFactor [3.2861189801699715] + Offset [256.79612068965514, -277.26034482758621] + SceneRectInView [256.79612068965514, -277.26034482758621, 660.65775862068972, 361.52068965517242] + } + Object { + $ObjectID 18 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4806" + Extents [2171.0, 1188.0] + ZoomFactor [2.0483743061062651] + Offset [-793.97389720558533, -290.08334454895737] + SceneRectInView [-793.97389720558533, -290.08334454895737, 1059.8648857917149, 579.97212543554] + } + Object { + $ObjectID 19 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4245" + Extents [2171.0, 1188.0] + ZoomFactor [1.0] + Offset [92.0078125, -350.98802908437432] + SceneRectInView [92.0078125, -350.98802908437432, 2171.0, 1188.0] + } + Object { + $ObjectID 20 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4211" + Extents [2171.0, 1188.0] + ZoomFactor [2.4071047957371228] + Offset [-112.83168536009441, -373.76948051948051] + SceneRectInView [-112.83168536009441, -373.76948051948051, 901.91337072018882, 493.538961038961] + } + Object { + $ObjectID 21 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4307" + Extents [2171.0, 1188.0] + ZoomFactor [2.56669704897075] + Offset [-48.06941549217828, -144.92583198051949] + SceneRectInView [-48.06941549217828, -144.92583198051949, 845.83414348435656, 462.851663961039] + } + Object { + $ObjectID 22 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4519" + Extents [2171.0, 1188.0] + ZoomFactor [1.5723743845222296] + Offset [-706.68143309105676, -478.27262581168833] + SceneRectInView [-706.68143309105676, -478.27262581168833, 1380.7144286821135, 755.54525162337666] + } + Object { + $ObjectID 23 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4353" + Extents [2171.0, 1188.0] + ZoomFactor [3.0839924670433145] + Offset [96.146189545676577, -4.1074743527112787] + SceneRectInView [96.146189545676577, -4.1074743527112787, 703.95762090864685, 385.21494870542256] + } + Object { + $ObjectID 24 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4321" + Extents [2171.0, 1188.0] + ZoomFactor [3.5884297520661157] + Offset [-211.69612560424537, 252.15114629325387] + SceneRectInView [-211.69612560424537, 252.15114629325387, 605.0, 331.06402579456471] + } + Object { + $ObjectID 25 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4504" + Extents [2171.0, 1188.0] + ZoomFactor [1.0] + Offset [-410.51953125, -620.5] + SceneRectInView [-410.51953125, -620.5, 2171.0, 1188.0] + } + Object { + $ObjectID 26 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4104" + Extents [2171.0, 1188.0] + ZoomFactor [3.0933333333333333] + Offset [-128.63423482247225, -154.52586206896552] + SceneRectInView [-128.63423482247225, -154.52586206896552, 701.83189655172418, 384.05172413793105] + } + Object { + $ObjectID 27 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2275" + Extents [2171.0, 1188.0] + ZoomFactor [1.8249725000639532] + Offset [-194.803483319316, -141.48435660218672] + SceneRectInView [-194.803483319316, -141.48435660218672, 1189.606966638632, 650.96871320437344] + } + Object { + $ObjectID 28 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2341" + Extents [2171.0, 1188.0] + ZoomFactor [1.5849611820686202] + Offset [-1178.7732548019947, -284.77258542366184] + SceneRectInView [-1178.7732548019947, -284.77258542366184, 1369.7496346039895, 749.54517084732367] + } + Object { + $ObjectID 29 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4553" + Extents [2171.0, 1188.0] + ZoomFactor [2.441659625391233] + Offset [-116.92703117366869, -144.85219724177151] + SceneRectInView [-116.92703117366869, -144.85219724177151, 889.14932180693916, 486.55430414861524] + } + Object { + $ObjectID 30 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2087" + Extents [2171.0, 1188.0] + ZoomFactor [1.7499999999999998] + Offset [-736.65534376844744, 2332.9648668093268] + SceneRectInView [-736.65534376844744, 2332.9648668093268, 1240.5714285714287, 678.85714285714289] + } + Object { + $ObjectID 31 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "3720" + Extents [2171.0, 1188.0] + ZoomFactor [5.95] + Offset [-33.739659926470551, -42.831932773109244] + SceneRectInView [-33.739659926470551, -42.831932773109244, 364.8739495798319, 199.66386554621849] + } + Object { + $ObjectID 32 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "3719" + Extents [2171.0, 1188.0] + ZoomFactor [5.95] + Offset [-33.739659926470551, -42.831932773109244] + SceneRectInView [-33.739659926470551, -42.831932773109244, 364.8739495798319, 199.66386554621849] + } + Object { + $ObjectID 33 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4422" + Extents [2171.0, 1188.0] + ZoomFactor [4.2554142103630159] + Offset [42.960073495100346, -92.586881707886135] + SceneRectInView [42.960073495100346, -92.586881707886135, 510.17360300979931, 279.17376341577227] + } + Object { + $ObjectID 34 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "3718" + Extents [2171.0, 1188.0] + ZoomFactor [4.8] + Offset [-39.932291666666657, -42.75] + SceneRectInView [-39.932291666666657, -42.75, 452.29166666666669, 247.5] + } + Object { + $ObjectID 35 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "3714" + Extents [2171.0, 1188.0] + ZoomFactor [1.2232444268048501] + Offset [-256.01528596010269, -463.59387395006996] + SceneRectInView [-256.01528596010269, -463.59387395006996, 1774.7883844202054, 971.18774790013993] + } + Object { + $ObjectID 36 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4684" + Extents [2171.0, 1188.0] + ZoomFactor [2.7884615384615383] + Offset [1438.5531788793103, 818.97931034482758] + SceneRectInView [1438.5531788793103, 818.97931034482758, 778.56551724137933, 426.04137931034484] + } + Object { + $ObjectID 37 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4696" + Extents [2171.0, 1188.0] + ZoomFactor [2.2264875239923225] + Offset [917.234213362069, -427.28793103448277] + SceneRectInView [917.234213362069, -427.28793103448277, 975.078448275862, 533.57586206896553] + } + Object { + $ObjectID 38 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4579" + Extents [2171.0, 1188.0] + ZoomFactor [2.2524271844660193] + Offset [-544.74878771551721, -352.2155172413793] + SceneRectInView [-544.74878771551721, -352.2155172413793, 963.84913793103453, 527.43103448275861] + } + Object { + $ObjectID 39 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4393" + Extents [2171.0, 1188.0] + ZoomFactor [1.6735854351990824] + Offset [374.06441393490434, 302.07339302379842] + SceneRectInView [374.06441393490434, 302.07339302379842, 1297.2149221301913, 709.85321395240317] + } + Object { + $ObjectID 40 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2355" + Extents [2171.0, 1188.0] + ZoomFactor [1.0730804810360777] + Offset [732.82082435344819, -198.046551724138] + SceneRectInView [732.82082435344819, -198.046551724138, 2023.1474137931036, 1107.093103448276] + } + Object { + $ObjectID 41 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4836" + Extents [2171.0, 1188.0] + ZoomFactor [1.7499999999999998] + Offset [89.479636723777276, -28.60714285714289] + SceneRectInView [89.479636723777276, -28.60714285714289, 1240.5714285714287, 678.85714285714289] + } + Object { + $ObjectID 42 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937:74" + Extents [2171.0, 1188.0] + ZoomFactor [3.0] + Offset [-132.82012310290415, -16.387940803946407] + SceneRectInView [-132.82012310290415, -16.387940803946407, 723.66666666666663, 396.0] + } + Object { + $ObjectID 43 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937:67" + Extents [2171.0, 1188.0] + ZoomFactor [2.87] + Offset [-165.54878048780483, -115.96864111498257] + SceneRectInView [-165.54878048780483, -115.96864111498257, 756.44599303135885, 413.93728222996515] + } + Object { + $ObjectID 44 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937:60" + Extents [2171.0, 1188.0] + ZoomFactor [2.5] + Offset [-221.49999999999994, -146.6] + SceneRectInView [-221.49999999999994, -146.6, 868.4, 475.2] + } + Object { + $ObjectID 45 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937:52" + Extents [2171.0, 1188.0] + ZoomFactor [2.85] + Offset [-173.20175438596488, -108.42105263157893] + SceneRectInView [-173.20175438596488, -108.42105263157893, 761.75438596491222, 416.84210526315786] + } + Object { + $ObjectID 46 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937:43" + Extents [2171.0, 1188.0] + ZoomFactor [2.85] + Offset [-173.20175438596488, -108.42105263157893] + SceneRectInView [-173.20175438596488, -108.42105263157893, 761.75438596491222, 416.84210526315786] + } + Object { + $ObjectID 47 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937:34" + Extents [2171.0, 1188.0] + ZoomFactor [2.92] + Offset [-164.07534246575341, -91.924657534246592] + SceneRectInView [-164.07534246575341, -91.924657534246592, 743.49315068493149, 406.84931506849318] + } + Object { + $ObjectID 48 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937:27" + Extents [2171.0, 1188.0] + ZoomFactor [2.85] + Offset [-173.20175438596488, -115.42105263157893] + SceneRectInView [-173.20175438596488, -115.42105263157893, 761.75438596491222, 416.84210526315786] + } + Object { + $ObjectID 49 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937:21" + Extents [2171.0, 1188.0] + ZoomFactor [2.85] + Offset [-165.70175438596488, -113.92105263157893] + SceneRectInView [-165.70175438596488, -113.92105263157893, 761.75438596491222, 416.84210526315786] + } + Object { + $ObjectID 50 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937:14" + Extents [2171.0, 1188.0] + ZoomFactor [2.85] + Offset [-165.70175438596488, -113.92105263157893] + SceneRectInView [-165.70175438596488, -113.92105263157893, 761.75438596491222, 416.84210526315786] + } + Object { + $ObjectID 51 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937" + Extents [2171.0, 1188.0] + ZoomFactor [1.0] + Offset [-327.9765625, -403.5] + SceneRectInView [-327.9765625, -403.5, 2171.0, 1188.0] + } + Object { + $ObjectID 52 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4938:7" + Extents [2171.0, 1188.0] + ZoomFactor [5.95] + Offset [-33.739659926470551, -42.831932773109244] + SceneRectInView [-33.739659926470551, -42.831932773109244, 364.8739495798319, 199.66386554621849] + } + Object { + $ObjectID 53 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4938:6" + Extents [2171.0, 1188.0] + ZoomFactor [5.95] + Offset [-33.739659926470551, -42.831932773109244] + SceneRectInView [-33.739659926470551, -42.831932773109244, 364.8739495798319, 199.66386554621849] + } + Object { + $ObjectID 54 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4938:13" + Extents [2171.0, 1188.0] + ZoomFactor [5.95] + Offset [-43.454503676470551, -32.331932773109244] + SceneRectInView [-43.454503676470551, -32.331932773109244, 364.8739495798319, 199.66386554621849] + } + Object { + $ObjectID 55 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4938" + Extents [2171.0, 1188.0] + ZoomFactor [2.9013369436452869] + Offset [-326.36442414255714, -208.23320111992533] + SceneRectInView [-326.36442414255714, -208.23320111992533, 748.27572328511428, 409.46640223985065] + } + Object { + $ObjectID 56 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4962" + Extents [2171.0, 1188.0] + ZoomFactor [6.0] + Offset [-116.23828124999997, -57.0] + SceneRectInView [-116.23828124999997, -57.0, 361.83333333333331, 198.0] + } + Object { + $ObjectID 57 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "5014" + Extents [2171.0, 1188.0] + ZoomFactor [2.03] + Offset [-310.81869612068976, -180.61083743842369] + SceneRectInView [-310.81869612068976, -180.61083743842369, 1069.4581280788179, 585.22167487684737] + } + Object { + $ObjectID 58 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4537" + Extents [2171.0, 1188.0] + ZoomFactor [5.1816867469879524] + Offset [-108.23828125, -66.072916666666657] + SceneRectInView [-108.23828125, -66.072916666666657, 418.97553943452374, 229.26897321428569] + } + Object { + $ObjectID 59 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4220" + Extents [2075.0, 1188.0] + ZoomFactor [5.95] + Offset [-25.672433035714278, -45.831932773109244] + SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] + } + Object { + $ObjectID 60 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2345" + Extents [2075.0, 1188.0] + ZoomFactor [4.8] + Offset [-29.932291666666686, -42.75] + SceneRectInView [-29.932291666666686, -42.75, 432.29166666666669, 247.5] + } + Object { + $ObjectID 61 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4996" + Extents [2075.0, 1188.0] + ZoomFactor [8.0] + Offset [10.776074743527147, -15.071488336727981] + SceneRectInView [10.776074743527147, -15.071488336727981, 259.375, 148.5] + } + Object { + $ObjectID 62 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4306" + Extents [2075.0, 1188.0] + ZoomFactor [5.95] + Offset [-25.756466649159648, -45.831932773109244] + SceneRectInView [-25.756466649159648, -45.831932773109244, 348.7394957983193, 199.66386554621849] + } + Object { + $ObjectID 63 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4983" + Extents [2075.0, 1188.0] + ZoomFactor [3.0] + Offset [-191.40632054176072, -117.29872084273889] + SceneRectInView [-191.40632054176072, -117.29872084273889, 691.66666666666663, 396.0] + } + Object { + $ObjectID 64 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4982" + Extents [2075.0, 1188.0] + ZoomFactor [4.43] + Offset [-92.7656132430399, -55.8837471783296] + SceneRectInView [-92.7656132430399, -55.8837471783296, 468.39729119638827, 268.17155756207677] + } + Object { + $ObjectID 65 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4229" + Extents [2075.0, 1188.0] + ZoomFactor [5.95] + Offset [-25.672433035714278, -45.831932773109244] + SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] + } + Object { + $ObjectID 66 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4985" + Extents [2075.0, 1188.0] + ZoomFactor [4.43] + Offset [-99.198645598194133, -48.972911963882623] + SceneRectInView [-99.198645598194133, -48.972911963882623, 468.39729119638827, 268.17155756207677] + } + Object { + $ObjectID 67 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4984" + Extents [2075.0, 1188.0] + ZoomFactor [4.43] + Offset [-80.667161130818016, -31.015048908954071] + SceneRectInView [-80.667161130818016, -31.015048908954071, 468.39729119638827, 268.17155756207677] + } + Object { + $ObjectID 68 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4867" + Extents [2075.0, 1188.0] + ZoomFactor [4.1304480341359344] + Offset [13.879095336481726, -85.310064935064929] + SceneRectInView [13.879095336481726, -85.310064935064929, 502.36680932703655, 287.62012987012986] + } + Object { + $ObjectID 69 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4987" + Extents [2075.0, 1188.0] + ZoomFactor [4.13] + Offset [13.972911622276058, -85.325665859564168] + SceneRectInView [13.972911622276058, -85.325665859564168, 502.42130750605327, 287.65133171912834] + } + Object { + $ObjectID 70 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4856" + Extents [2075.0, 1188.0] + ZoomFactor [5.95] + Offset [-25.672433035714278, -45.831932773109244] + SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] + } + Object { + $ObjectID 71 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4855" + Extents [2075.0, 1188.0] + ZoomFactor [5.95] + Offset [-25.672433035714278, -45.831932773109244] + SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] + } + Object { + $ObjectID 72 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4559" + Extents [2075.0, 1188.0] + ZoomFactor [4.0] + Offset [-64.168329397874857, -71.912514757969319] + SceneRectInView [-64.168329397874857, -71.912514757969319, 518.75, 297.0] + } + Object { + $ObjectID 73 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4250" + Extents [2075.0, 1188.0] + ZoomFactor [5.95] + Offset [-25.672433035714278, -45.831932773109244] + SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] + } + Object { + $ObjectID 74 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4251" + Extents [2075.0, 1188.0] + ZoomFactor [5.95] + Offset [-25.672433035714278, -45.831932773109244] + SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] + } + Object { + $ObjectID 75 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4774" + Extents [2522.0, 1188.0] + ZoomFactor [5.3226623982926506] + Offset [-8.0130751303127283, -114.03393582177664] + SceneRectInView [-8.0130751303127283, -114.03393582177664, 473.82302526062546, 223.19657177225341] + } + Object { + $ObjectID 76 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4767" + Extents [2522.0, 1188.0] + ZoomFactor [2.4463721913987064] + Offset [16.019427876904615, -398.80851543704892] + SceneRectInView [16.019427876904615, -398.80851543704892, 1030.9142692461908, 485.61703087409785] + } + Object { + $ObjectID 77 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4761" + Extents [2522.0, 1188.0] + ZoomFactor [2.9169324110708055] + Offset [-105.54956270048115, -242.68495036561927] + SceneRectInView [-105.54956270048115, -242.68495036561927, 864.60693790096229, 407.2771777265437] + } + Object { + $ObjectID 78 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4865" + Extents [1722.0, 1188.0] + ZoomFactor [1.5] + Offset [-396.83333333333337, -304.36666666666662] + SceneRectInView [-396.83333333333337, -304.36666666666662, 1148.0, 792.0] + } + Object { + $ObjectID 79 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4260" + Extents [1722.0, 1188.0] + ZoomFactor [3.9244900544786518] + Offset [34.9092200413223, -92.857244318181813] + SceneRectInView [34.9092200413223, -92.857244318181813, 438.7831224173554, 302.71448863636363] + } + Object { + $ObjectID 80 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4972" + Extents [2169.0, 1188.0] + ZoomFactor [2.5] + Offset [-183.3666666666665, -165.10000000000005] + SceneRectInView [-183.3666666666665, -165.10000000000005, 867.6, 475.2] + } + Object { + $ObjectID 81 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4917" + Extents [2169.0, 1188.0] + ZoomFactor [4.3275706438106187] + Offset [10.41989407940855, -23.235071395084702] + SceneRectInView [10.41989407940855, -23.235071395084702, 501.20498970990775, 274.51891552575859] + } + Object { + $ObjectID 82 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4925" + Extents [1815.0, 821.0] + ZoomFactor [3.8000000393627378] + Offset [-68.684210488152473, -33.394738158739756] + SceneRectInView [-68.684210488152473, -33.394738158739756, 477.63157399976672, 216.05262934094131] + } + Object { + $ObjectID 83 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4886" + Extents [1815.0, 821.0] + ZoomFactor [4.0050505050505052] + Offset [94.661097099621685, 186.50441361916774] + SceneRectInView [0.0, 0.0, 0.0, 0.0] + } + Object { + $ObjectID 84 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4890" + Extents [1815.0, 821.0] + ZoomFactor [4.3274215552523874] + Offset [5.415825977301381, -23.235182849936947] + SceneRectInView [0.0, 0.0, 0.0, 0.0] + } + Object { + $ObjectID 85 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4664" + Extents [1815.0, 821.0] + ZoomFactor [2.5] + Offset [-221.43409915356727, -143.9621523579201] + SceneRectInView [0.0, 0.0, 0.0, 0.0] + } + Object { + $ObjectID 86 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2416" + Extents [1815.0, 821.0] + ZoomFactor [2.5] + Offset [-41.475000000000023, 125.00000000000006] + SceneRectInView [0.0, 0.0, 0.0, 0.0] + } + Object { + $ObjectID 87 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4661" + Extents [1815.0, 821.0] + ZoomFactor [4.0] + Offset [-19.375, -38.5] + SceneRectInView [0.0, 0.0, 0.0, 0.0] + } + PropName "EditorsInfo" + } + Array { + Type "Simulink.DockComponentInfo" + Dimension 3 + Object { + $ObjectID 88 + Type "GLUE2:PropertyInspector" + ID "Property Inspector" + Visible [0] + CreateCallback "" + UserData "" + Floating [0] + DockPosition "Right" + Width [640] + Height [480] + Minimized "Unset" + } + Object { + $ObjectID 89 + Type "GLUE2:SpreadSheet" + ID "ModelData" + Visible [0] + CreateCallback "DataView.createSpreadSheetComponent" + UserData "" + Floating [0] + DockPosition "Bottom" + Width [640] + Height [480] + Minimized "Unset" + } + Object { + $ObjectID 90 + Type "Simulink:Editor:ReferencedFiles" + ID "Referenced Files" + Visible [0] + CreateCallback "" + UserData "{\"filterShowRefModels\":\"true\",\"filterShowRefSubs\":\"true\",\"filterShowOnlyDirtyFiles\":\"false\"}\n" + Floating [0] + DockPosition "Left" + Width [640] + Height [480] + Minimized "Unset" + } + PropName "DockComponentsInfo" + } + WindowState "AAAA/wAAAAD9AAAAAwAAAAAAAAFeAAAE4fwCAAAABPsAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQBAAAAeQAABOEAAABrAP////sAAABgAFMAaQBtAHUAb" + "ABpAG4AawA6AEUAZABpAHQAbwByADoAUgBlAGYAZQByAGUAbgBjAGUAZABGAGkAbABlAHMALwBSAGUAZgBlAHIAZQBuAGMAZQBkACAARgBpAGwAZ" + "QBzAAAAAAD/////AAAAjwD///8AAAABAAABYAAABOH8AgAAAAH8AAAAeQAABOEAAAAAAP////r/////AgAAAAH7AAAAVABHAEwAVQBFADIAOgBQA" + "HIAbwBwAGUAcgB0AHkASQBuAHMAcABlAGMAdABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA/" + "///AAAAAwAACKEAAADq/AEAAAAB+wAAADYARwBMAFUARQAyADoAUwBwAHIAZQBhAGQAUwBoAGUAZQB0AC8ATQBvAGQAZQBsAEQAYQB0AGEAAAABX" + "wAACKEAAAE6AP///wAACKEAAAThAAAAAQAAAAIAAAABAAAAAvwAAAABAAAAAgAAAAA=" + Array { + Type "Cell" + Dimension 0 + PropName "PersistedApps" + } + WindowUuid "155c2206-98f5-4f41-9ed3-18fd6f3f753f" + } + BDUuid "8d887252-6855-49e7-b7fc-16933ad93043" + } + HideAutomaticNames on + SequenceViewerTimePrecision 3 + SequenceViewerHistory 1000 + Created "Tue Feb 18 10:13:36 2014" + Creator "daniele" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "nunoguedelha" + ModifiedDateFormat "%" + LastModifiedDate "Fri May 07 17:27:52 2021" + RTWModifiedTimeStamp 542309261 + ModelVersionFormat "%" + SampleTimeColors on + SampleTimeAnnotations on + LibraryLinkDisplay "disabled" + WideLines off + ShowLineDimensions on + ShowPortDataTypes off + ShowAllPropagatedSignalLabels off + PortDataTypeDisplayFormat "AliasTypeOnly" + ShowEditTimeErrors on + ShowEditTimeWarnings on + ShowEditTimeAdvisorChecks off + ShowPortUnits off + ShowDesignRanges off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + VariantCondition off + ShowLinearizationAnnotations on + ShowVisualizeInsertedRTB off + ShowMarkup on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + BlockVariantConditionDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks on + FunctionConnectors off + BrowserLookUnderMasks on + MultiThreadCoSim "on" + SimulationMode "normal" + SILPILModeSetting "automated" + SILPILSystemUnderTest "topmodel" + SILPILSimulationModeTopModel "normal" + SILPILSimulationModeModelRef "normal" + SimTabSimulationMode "normal" + CodeVerificationMode "software-in-the-loop (sil)" + PauseTimes "5" + NumberOfSteps 1 + SnapshotBufferSize 10 + SnapshotInterval 10 + NumberOfLastSnapshots 0 + EnablePacing off + PacingRate 1 + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + Object { + $PropName "DataLoggingOverride" + $ObjectID 91 + $ClassName "Simulink.SimulationData.ModelLoggingInfo" + model_ "torqueControlBalancingWithSimu" + signals_ [] + overrideMode_ [0.0] + Array { + Type "Cell" + Dimension 1 + Cell "torqueControlBalancingWithSimu" + PropName "logAsSpecifiedByModels_" + } + Array { + Type "Cell" + Dimension 1 + Cell [] + PropName "logAsSpecifiedByModelsSSIDs_" + } + } + Object { + $PropName "InstrumentedSignals" + $ObjectID 92 + $ClassName "Simulink.HMI.InstrumentedSignals" + Persistence [] + } + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigSignalOutputPortIndex 0 + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + VariantFading on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + OrderedModelArguments on + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 93 + Version "20.1.0" + DisabledProps [] + Description "" + Array { + Type "Handle" + Dimension 9 + Simulink.SolverCC { + $ObjectID 94 + Version "20.1.0" + DisabledProps [] + Description "" + Components [] + StartTime "0.0" + StopTime "Config.SIMULATION_TIME " + AbsTol "auto" + AutoScaleAbsTol on + FixedStep "Config.tStepSim" + InitialStep "auto" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + EnableMultiTasking off + ConcurrentTasks off + SolverName "FixedStepDiscrete" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverInfoToggleStatus on + IsAutoAppliedInSIP off + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + SampleTimeProperty [] + DecoupledContinuousIntegration off + MinimalZcImpactIntegration off + ODENIntegrationMethod "ode3" + } + Simulink.DataIOCC { + $ObjectID 95 + Version "20.1.0" + DisabledProps [] + Description "" + Components [] + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveOperatingPoint off + SaveFormat "Array" + SignalLoggingSaveFormat "Dataset" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + VisualizeSimOutput on + StreamToWorkspace off + StreamVariableName "streamout" + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + LoggingToFile off + DatasetSignalFormat "timeseries" + LoggingFileName "out.mat" + LoggingIntervals "[-inf, inf]" + } + Simulink.OptimizationCC { + $ObjectID 96 + Version "20.1.0" + Array { + Type "Cell" + Dimension 9 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + Cell "UseSpecifiedMinMax" + Cell "EfficientTunableParamExpr" + PropName "DisabledProps" + } + Description "" + Components [] + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + DefaultParameterBehavior "Tunable" + UseDivisionForNetSlopeComputation "off" + GainParamInheritBuiltInType off + UseFloatMulNetSlope off + DefaultUnderspecifiedDataType "double" + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + CachingGlobalReferences off + GlobalBufferReuse on + StrengthReduction off + AdvancedOptControl "" + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + BitwiseOrLogicalOp "Same as modeled" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + PassReuseOutputArgsThreshold 12 + ExpressionDepthLimit 2147483647 + LocalBlockOutputs on + RollThreshold 5 + StateBitsets off + DataBitsets off + ActiveStateOutputEnumStorageType "Native Integer" + ZeroExternalMemoryAtStartup off + ZeroInternalMemoryAtStartup off + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "off" + AccelVerboseBuild off + OptimizeBlockOrder "off" + OptimizeDataStoreBuffers on + BusAssignmentInplaceUpdate on + DifferentSizesBufferReuse off + UseRowMajorAlgorithm off + OptimizationLevel "level2" + OptimizationPriority "Balanced" + OptimizationCustomize on + LabelGuidedReuse off + MultiThreadedLoops off + DenormalBehavior "GradualUnderflow" + EfficientTunableParamExpr off + } + Simulink.DebuggingCC { + $ObjectID 97 + Version "20.1.0" + Array { + Type "Cell" + Dimension 1 + Cell "UseOnlyExistingSharedCode" + PropName "DisabledProps" + } + Description "" + Components [] + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + StringTruncationChecking "error" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + ExportedTasksRateTransMsg "none" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + FcnCallInpInsideContextMsg "warning" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + UseOnlyExistingSharedCode "error" + SFcnCompatibilityMsg "none" + FrameProcessingCompatibilityMsg "error" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + AllowSymbolicDim off + RowMajorDimensionSupport off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceNoExplicitFinalValueMsg "none" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + OperatingPointInterfaceChecksumMismatchMsg "warning" + NonCurrentReleaseOperatingPointMsg "error" + ChecksumConsistencyForSSReuse "none" + PregeneratedLibrarySubsystemCodeDiagnostic "warning" + MatchCodeGenerationContextForUpdateDiagram "none" + InitInArrayFormatMsg "warning" + StrictBusMsg "ErrorLevel1" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + SymbolicDimMinMaxWarning "warning" + LossOfSymbolicDimsSimulationWarning "warning" + LossOfSymbolicDimsCodeGenerationWarning "error" + SymbolicDimsDataTypeCodeGenerationDiagnostic "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + SFUnreachableExecutionPathDiag "warning" + SFUndirectedBroadcastEventsDiag "warning" + SFTransitionActionBeforeConditionDiag "warning" + SFOutputUsedAsStateInMooreChartDiag "error" + SFTemporalDelaySmallerThanSampleTimeDiag "warning" + SFSelfTransitionDiag "warning" + SFExecutionAtInitializationDiag "none" + SFMachineParentedDataDiag "warning" + IntegerSaturationMsg "warning" + AllowedUnitSystems "all" + UnitsInconsistencyMsg "warning" + AllowAutomaticUnitConversions on + RCSCRenamedMsg "warning" + RCSCObservableMsg "warning" + ForceCombineOutputUpdateInSim off + UnitDatabase "" + UnderSpecifiedDimensionMsg "none" + DebugExecutionForFMUViaOutOfProcess off + ArithmeticOperatorsInVariantConditions "warning" + VariantConditionMismatch "none" + } + Simulink.HardwareCC { + $ObjectID 98 + Version "20.1.0" + DisabledProps [] + Description "" + Components [] + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerLongLong 64 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdBitPerSizeT 32 + ProdBitPerPtrDiffT 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdLongLongMode off + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerLongLong 64 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetBitPerSizeT 32 + TargetBitPerPtrDiffT 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetLongLongMode off + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + UseEmbeddedCoderFeatures on + UseSimulinkCoderFeatures on + HardwareBoardFeatureSet "EmbeddedCoderHSP" + } + Simulink.ModelReferenceCC { + $ObjectID 99 + Version "20.1.0" + DisabledProps [] + Description "" + Components [] + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + EnableRefExpFcnMdlSchedulingChecks on + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelDependencies "" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 100 + Version "20.1.0" + DisabledProps [] + Description "" + Components [] + SimCustomSourceCode "" + SimCustomHeaderCode "" + SimCustomInitializer "" + SimCustomTerminator "" + SimReservedNameArray [] + SimUserSources "" + SimUserIncludeDirs "" + SimUserLibraries "" + SimUserDefines "" + SimCustomCompilerFlags "" + SimCustomLinkerFlags "" + SFSimEcho on + SimCtrlC on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode off + SimAnalyzeCustomCode off + SimGenImportedTypeDefs off + ModelFunctionsGlobalVisibility "on" + CompileTimeRecursionLimit 50 + EnableRuntimeRecursion on + MATLABDynamicMemAlloc on + MATLABDynamicMemAllocThreshold 65536 + CustomCodeFunctionArrayLayout [] + DefaultCustomCodeFunctionArrayLayout "NotSpecified" + CustomCodeUndefinedFunction "UseInterfaceOnly" + CustomCodeGlobalsAsFunctionIO off + SimTargetLang "C++" + GPUAcceleration off + SimGPUMallocThreshold 200 + SimGPUStackLimitPerThread 1024 + SimGPUErrorChecks off + SimGPUCustomComputeCapability "" + SimGPUCompilerFlags "" + SimDLTargetLibrary "mkl-dnn" + SimDLAutoTuning on + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 101 + Version "20.1.0" + Array { + Type "Cell" + Dimension 16 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + Cell "PortableWordSizes" + Cell "GenerateWebview" + Cell "GenerateCodeMetricsReport" + Cell "GenerateCodeReplacementReport" + Cell "GenerateErtSFunction" + Cell "CreateSILPILBlock" + Cell "CodeExecutionProfiling" + Cell "CodeProfilingSaveOptions" + Cell "CodeProfilingInstrumentation" + Cell "GenerateMissedCodeReplacementReport" + PropName "DisabledProps" + } + Description "" + SystemTargetFile "grt.tlc" + HardwareBoard "None" + ShowCustomHardwareApp off + ShowEmbeddedHardwareApp off + TLCOptions "" + GenCodeOnly on + MakeCommand "make_rtw" + GenerateMakefile on + PackageGeneratedCodeAndArtifacts off + PackageName "" + TemplateMakefile "grt_default_tmf" + PostCodeGenCommand "" + GenerateReport off + RTWVerbose on + RetainRTWFile off + RTWBuildHooks [] + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + CustomSourceCode "" + CustomHeaderCode "" + CustomInclude "" + CustomSource "" + CustomLibrary "" + CustomDefine "" + CustomBLASCallback "" + CustomLAPACKCallback "" + CustomFFTCallback "" + CustomInitializer "" + CustomTerminator "" + Toolchain "Automatically locate an installed toolchain" + BuildConfiguration "Faster Builds" + CustomToolchainOptions [] + IncludeHyperlinkInReport off + LaunchReport off + PortableWordSizes off + CreateSILPILBlock "None" + CodeExecutionProfiling off + CodeExecutionProfileVariable "executionProfile" + CodeProfilingSaveOptions "SummaryOnly" + CodeProfilingInstrumentation "off" + SILDebugging off + TargetLang "C++" + GenerateGPUCode "None" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateWebview off + GenerateCodeMetricsReport off + GenerateCodeReplacementReport off + GenerateMissedCodeReplacementReport off + RTWCompilerOptimization "off" + ObjectivePriorities [] + RTWCustomCompilerOptimizations "" + CheckMdlBeforeBuild "Off" + SharedConstantsCachingThreshold 1024 + GPUKernelNamePrefix "" + GPUDeviceID -1 + GPUMallocMode "discrete" + GPUMallocThreshold 200 + GPUStackLimitPerThread 1024 + GPUcuBLAS on + GPUcuSOLVER on + GPUcuFFT on + GPUErrorChecks off + GPUComputeCapability "3.5" + GPUCustomComputeCapability "" + GPUCompilerFlags "" + DLTargetLibrary "none" + DLAutoTuning on + DLArmComputeVersion "19.05" + DLArmComputeArch "unspecified" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 102 + Version "20.1.0" + Array { + Type "Cell" + Dimension 28 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "InsertPolySpaceComments" + Cell "SFDataObjDesc" + Cell "MATLABFcnDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InternalIdentifier" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrFcnArg" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + Cell "ReqsInCode" + Cell "CustomSymbolStrModelFcn" + Cell "CustomSymbolStrUtil" + Cell "CustomUserTokenString" + Cell "CustomSymbolStrEmxType" + Cell "CustomSymbolStrEmxFcn" + Cell "BlockCommentType" + PropName "DisabledProps" + } + Description "" + Components [] + Comment "" + ForceParamTrailComments off + GenerateComments on + CommentStyle "Auto" + IgnoreCustomStorageClasses on + IgnoreTestpoints off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + OperatorAnnotations off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + MangleLength 1 + SharedChecksumLength 8 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M_T" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrModelFcn "$R$N" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + CustomSymbolStrUtil "$N$C" + CustomSymbolStrEmxType "emxArray_$M$N" + CustomSymbolStrEmxFcn "emx$M$N" + CustomUserTokenString "" + CustomCommentsFcn "" + DefineNamingRule "None" + DefineNamingFcn "" + ParamNamingRule "None" + ParamNamingFcn "" + SignalNamingRule "None" + SignalNamingFcn "" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + BlockCommentType "BlockPathComment" + StateflowObjectComments on + MATLABSourceComments off + EnableCustomComments off + InternalIdentifierFile "" + InternalIdentifier "Shortened" + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + ReservedNameArray [] + EnumMemberNameClash "error" + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 103 + Version "20.1.0" + Array { + Type "Cell" + Dimension 18 + Cell "GeneratePreprocessorConditionals" + Cell "IncludeMdlTerminateFcn" + Cell "GenerateAllocFcn" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + Cell "PurelyIntegerCode" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "CPPClassGenCompliant" + Cell "RemoveResetFunc" + Cell "ExistingSharedCode" + Cell "RemoveDisableFunc" + Cell "PreserveStateflowLocalDataDimensions" + PropName "DisabledProps" + } + Description "" + Array { + Type "Handle" + Dimension 1 + Simulink.CPPComponent { + $ObjectID 104 + Version "20.1.0" + Array { + Type "Cell" + Dimension 10 + Cell "Description" + Cell "Components" + Cell "Name" + Cell "GenerateDestructor" + Cell "GenerateExternalIOAccessMethods" + Cell "ParameterMemberVisibility" + Cell "InternalMemberVisibility" + Cell "GenerateParameterAccessMethods" + Cell "GenerateInternalMemberAccessMethods" + Cell "UseOperatorNewForModelRefRegistration" + PropName "DisabledProps" + } + Description "" + Components [] + Name "CPPClassGenComp" + GenerateDestructor on + ExternalIOMemberVisibility "private" + GenerateExternalIOAccessMethods "None" + ParameterMemberVisibility "private" + InternalMemberVisibility "private" + GenerateParameterAccessMethods "None" + GenerateInternalMemberAccessMethods "None" + UseOperatorNewForModelRefRegistration off + IncludeModelTypesInModelClass on + } + PropName "Components" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + GenFloatMathFcnCalls "NOT IN USE" + TargetLangStandard "C++03 (ISO)" + CodeReplacementLibrary "None" + UtilityFuncGeneration "Auto" + MultiwordTypeDef "System defined" + MultiwordLength 2048 + DynamicStringBufferSize 256 + GenerateFullHeader on + InferredTypesCompatibility off + ExistingSharedCode "" + GenerateSampleERTMain off + GenerateTestInterfaces off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + ConcurrentExecutionCompliant on + IncludeMdlTerminateFcn on + CombineOutputUpdateFcns on + CombineSignalStateStructs off + GroupInternalDataByFunction off + SuppressErrorStatus off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging off + CodeInterfacePackaging "C++ class" + PurelyIntegerCode off + SupportNonFinite off + SupportComplex on + SupportContinuousTime on + SupportNonInlinedSFcns on + RemoveDisableFunc off + RemoveResetFunc off + SupportVariableSizeSignals off + ParenthesesLevel "Nominal" + CastingMode "Nominal" + MATLABClassNameForMDSCustomization "Simulink.SoftwareTarget.GRTCustomization" + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant on + AutosarCompliant off + MDXCompliant off + GRTInterface off + GenerateAllocFcn off + UseToolchainInfoCompliant on + GenerateSharedConstants on + CoderGroups [] + AccessMethods [] + LookupTableObjectStructAxisOrder "1,2,3,4,..." + LUTObjectStructOrderExplicitValues "Size,Breakpoints,Table" + LUTObjectStructOrderEvenSpacing "Size,Breakpoints,Table" + ArrayLayout "Column-major" + UnsupportedSFcnMsg "error" + ERTHeaderFileRootName "$R$E" + ERTSourceFileRootName "$R$E" + ERTDataFileRootName "$R_data" + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeMexArgs "" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + MultiInstanceErrorCode "Error" + } + PropName "Components" + } + } + SlCovCC.ConfigComp { + $ObjectID 105 + Version "20.1.0" + DisabledProps [] + Description "Simulink Coverage Configuration Component" + Components [] + Name "Simulink Coverage" + CovEnable off + CovScope "EntireSystem" + CovIncludeTopModel on + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovCompData "" + CovMetricSettings "dw" + CovFilter "" + CovHTMLOptions "" + CovNameIncrementing off + CovForceBlockReductionOff on + CovEnableCumulative on + CovSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovSaveOutputData on + CovOutputDir "slcov_output/$ModelName$" + CovDataFileName "$ModelName$_cvdata" + CovReportOnPause on + CovModelRefEnable "off" + CovModelRefExcluded "" + CovExternalEMLEnable off + CovSFcnEnable on + CovBoundaryAbsTol 1e-05 + CovBoundaryRelTol 0.01 + CovUseTimeInterval off + CovStartTime 0 + CovStopTime 0 + CovMcdcMode "Masking" + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Data Import//Export" + ConfigPrmDlgPosition [ 200, 140, 1722, 1010 ] + ExtraOptions "-aBlockCommentType=\"BlockPathComment\" -aIgnoreTestpoints=0 " + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 93 + } + Object { + $PropName "DataTransfer" + $ObjectID 106 + $ClassName "Simulink.GlobalDataTransfer" + DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" + DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" + DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" + DefaultExtrapolationMethodBetweenContTasks "None" + } + ExplicitPartitioning off + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NameLocation "bottom" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + HideAutomaticName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + MarkupType "model" + UseDisplayTextAsClickCallback off + AnnotationType "note_annotation" + FixedHeight off + FixedWidth off + Interpreter "off" + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + MaskDefaults { + SelfModifiable "off" + IconFrame "on" + IconOpaque "opaque" + RunInitForIconRedraw "analyze" + IconRotate "none" + PortRotate "default" + IconUnits "autoscale" + } + MaskParameterDefaults { + Evaluate "on" + Tunable "on" + NeverSave "off" + Internal "off" + ReadOnly "off" + Enabled "on" + Visible "on" + ToolTip "on" + } + BlockParameterDefaults { + Block { + BlockType Assertion + Enabled on + StopWhenAssertionFail on + SampleTime "-1" + } + Block { + BlockType BusCreator + DisplayOption "none" + OutDataTypeStr "Inherit: auto" + NonVirtualBus off + } + Block { + BlockType BusSelector + OutputAsBus off + } + Block { + BlockType Clock + DisplayTime off + Decimation "10" + } + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Demux + Outputs "4" + DisplayOption "bar" + BusSelectionMode off + } + Block { + BlockType EnablePort + StatesWhenEnabling "held" + PropagateVarSize "Only when enabling" + ShowOutputPort off + ZeroCross on + DisallowConstTsAndPrmTs off + PortDimensions "1" + SampleTime "-1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "double" + Interpolate on + } + Block { + BlockType From + GotoTag "A" + IconDisplay "Tag" + TagVisibility "local" + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit via internal rule" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow off + SampleTime "-1" + } + Block { + BlockType Goto + GotoTag "A" + IconDisplay "Tag" + TagVisibility "local" + } + Block { + BlockType Ground + } + Block { + BlockType Inport + Port "1" + IconDisplay "Port number" + OutputFunctionCall off + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + Unit "inherit" + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + LatchByDelayingOutsideSignal off + LatchInputForFeedbackSignals off + Interpolate on + } + Block { + BlockType Logic + Operator "AND" + Inputs "2" + IconShape "rectangular" + AllPortsSameDT on + OutDataTypeStr "Inherit: Logical (see Configuration Parameters: Optimization)" + SampleTime "-1" + } + Block { + BlockType Memory + InitialCondition "0" + InheritSampleTime off + LinearizeMemory off + LinearizeAsDelay off + StateMustResolveToSignalObject off + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Outport + Port "1" + IconDisplay "Port number" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + Unit "inherit" + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + EnsureOutportIsVirtual off + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + MustResolveToSignalObject off + OutputWhenUnConnected off + OutputWhenUnconnectedValue "0" + VectorParamsAs1DForOutWhenUnconnected on + } + Block { + BlockType Product + Inputs "2" + Multiplication "Element-wise(.*)" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT off + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit via internal rule" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow off + SampleTime "-1" + } + Block { + BlockType Reshape + OutputDimensionality "1-D array" + OutputDimensions "[1,1]" + } + Block { + BlockType S-Function + FunctionName "system" + SFunctionModules "''" + PortCounts "[]" + MultiThreadCoSim "auto" + } + Block { + BlockType Saturate + UpperLimit "0.5" + LowerLimit "-0.5" + LinearizeAsGain on + ZeroCross on + SampleTime "-1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + } + Block { + BlockType Scope + DefaultConfigurationName "Simulink.scopes.TimeScopeBlockCfg" + } + Block { + BlockType Selector + NumberOfDimensions "1" + IndexMode "One-based" + InputPortWidth "-1" + SampleTime "-1" + } + Block { + BlockType SubSystem + ShowPortLabels "FromPortIcon" + Permissions "ReadWrite" + PermitHierarchicalResolution "All" + TreatAsAtomicUnit off + MinAlgLoopOccurrences off + ScheduleAs "Sample time" + SystemSampleTime "-1" + RTWSystemCode "Auto" + RTWFcnNameOpts "Auto" + RTWFileNameOpts "Auto" + FunctionInterfaceSpec "void_void" + FunctionWithSeparateData off + MatchGraphicalInterface off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + SimViewingDevice off + ActiveForDiff off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + Opaque off + MaskHideContents off + SFBlockType "NONE" + Variant off + VariantControlMode "expression" + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + TreatAsGroupedWhenPropagatingVariantConditions on + ContentPreviewEnabled off + IsWebBlock off + IsInjectorSS off + Latency "0" + AutoFrameSizeCalculation off + IsWebBlockPanel off + } + Block { + BlockType Sum + IconShape "round" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT off + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit via internal rule" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow off + SampleTime "-1" + } + Block { + BlockType Switch + Criteria "u2 >= Threshold" + Threshold "0" + InputSameDT on + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit via internal rule" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + ZeroCross on + SampleTime "-1" + AllowDiffInputSizes off + } + Block { + BlockType Terminator + } + Block { + BlockType ToWorkspace + VariableName "simulink_output" + MaxDataPoints "1000" + Decimation "1" + SaveFormat "Array" + Save2DSignal "3-D array (concatenate along third dimension)" + FixptAsFi off + NumInputs "1" + SampleTime "0" + } + Block { + BlockType UnitDelay + InitialCondition "0" + InputProcessing "Elements as channels (sample based)" + SampleTime "1" + StateMustResolveToSignalObject off + HasFrameUpgradeWarning off + } + } + System { + Name "torqueControlBalancingWithSimu" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open on + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "206" + ReportName "simulink-default.rpt" + SIDHighWatermark "5020" + SimulinkSubDomain "Simulink" + Block { + BlockType BusCreator + Name "Bus\nCreator" + SID "4944" + Ports [6, 1] + Position [1730, 400, 1735, 570] + ZOrder 1276 + Inputs "6" + DisplayOption "bar" + InheritFromInputs on + } + Block { + BlockType BusSelector + Name "Bus\nSelector1" + SID "5013" + Ports [1, 2] + Position [1465, 396, 1470, 454] + ZOrder 1290 + OutputSignals "joints_position,joints_velocity" + Port { + PortNumber 1 + Name "" + } + Port { + PortNumber 2 + Name "" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector2" + SID "4945" + Ports [1, 6] + Position [760, 404, 765, 596] + ZOrder 1277 + OutputSignals "joints_position,joints_velocity,nuDot,signal6,wrench_RFoot,wrench_LFoot" + Port { + PortNumber 1 + Name "" + } + Port { + PortNumber 2 + Name "" + } + Port { + PortNumber 3 + Name "" + } + Port { + PortNumber 4 + Name "" + } + Port { + PortNumber 5 + Name "" + } + Port { + PortNumber 6 + Name "" + } + } + Block { + BlockType Clock + Name "Clock1" + SID "2166" + Position [875, 595, 895, 615] + ZOrder 1285 + ShowName off + } + Block { + BlockType Reference + Name "Configuration" + SID "4537" + Ports [] + Position [955, 325, 1020, 345] + ZOrder 553 + ForegroundColor "magenta" + HideAutomaticName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Utilities/Configuration" + SourceType "" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + ConfigSource "Workspace" + ConfigObject "'WBTConfigRobot'" + } + Block { + BlockType Constant + Name "Constant" + SID "5003" + Position [815, 380, 880, 410] + ZOrder 1287 + Value "zeros(3,1)" + Object { + $PropName "MaskObject" + $ObjectID 107 + $ClassName "Simulink.Mask" + Type "Fixed neck position" + Description "we assume that the neck position is fixed (\"lumped\" head and torso) in the [0 0 0] position." + Display "image('images/initNeckPosBlock.jpeg'); % Use \"Edit Image\" on toolstrip to change." + RunInitForIconRedraw "off" + Object { + $PropName "DialogControls" + $ObjectID 108 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 109 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + } + } + Block { + BlockType SubSystem + Name "MOMENTUM BASED TORQUE CONTROL" + SID "4836" + Ports [8, 1] + Position [940, 388, 1090, 612] + ZOrder 961 + ForegroundColor "red" + BackgroundColor "yellow" + TreatAsAtomicUnit on + SystemSampleTime "Config.tStep" + RequestExecContextInheritance off + System { + Name "MOMENTUM BASED TORQUE CONTROL" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "175" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "neckPos" + SID "4977" + Position [160, 353, 190, 367] + ZOrder 971 + } + Block { + BlockType Inport + Name "jointPos" + SID "4837" + Position [160, 258, 190, 272] + ZOrder 556 + Port "2" + } + Block { + BlockType Inport + Name "jointVel" + SID "4838" + Position [160, 453, 190, 467] + ZOrder 557 + Port "3" + } + Block { + BlockType Inport + Name "jointAcc" + SID "4966" + Position [160, 148, 190, 162] + ZOrder 969 + Port "4" + } + Block { + BlockType Inport + Name "IMU_meas" + SID "4840" + Position [160, 503, 190, 517] + ZOrder 559 + Port "5" + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "4880" + Position [160, 553, 190, 567] + ZOrder 963 + Port "6" + } + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "4881" + Position [160, 603, 190, 617] + ZOrder 964 + Port "7" + } + Block { + BlockType Inport + Name "time" + SID "5000" + Position [160, 68, 190, 82] + ZOrder 973 + Port "8" + } + Block { + BlockType SubSystem + Name "Balancing Controller QP" + SID "2355" + Ports [21, 1] + Position [800, 3, 955, 607] + ZOrder 542 + ForegroundColor "red" + BackgroundColor "yellow" + RequestExecContextInheritance off + System { + Name "Balancing Controller QP" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "107" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "M" + SID "2360" + Position [1065, 168, 1095, 182] + ZOrder 326 + } + Block { + BlockType Inport + Name "h" + SID "2361" + Position [1065, 213, 1095, 227] + ZOrder 327 + Port "2" + } + Block { + BlockType Inport + Name "L " + SID "3219" + Position [1065, 258, 1095, 272] + ZOrder 639 + Port "3" + } + Block { + BlockType Inport + Name "w_H_l_sole" + SID "4384" + Position [1065, 348, 1095, 362] + ZOrder 837 + Port "4" + } + Block { + BlockType Inport + Name "w_H_r_sole" + SID "4391" + Position [1065, 393, 1095, 407] + ZOrder 844 + Port "5" + } + Block { + BlockType Inport + Name "J_l_sole" + SID "2357" + Position [1065, 438, 1095, 452] + ZOrder 550 + Port "6" + } + Block { + BlockType Inport + Name "J_r_sole" + SID "4392" + Position [1065, 483, 1095, 497] + ZOrder 845 + Port "7" + } + Block { + BlockType Inport + Name "J_CoM" + SID "4390" + Position [1065, 663, 1095, 677] + ZOrder 843 + Port "8" + } + Block { + BlockType Inport + Name "pos_CoM" + SID "4383" + Position [1065, 618, 1095, 632] + ZOrder 836 + Port "9" + } + Block { + BlockType Inport + Name "JDot_l_sole_nu" + SID "4386" + Position [1065, 528, 1095, 542] + ZOrder 839 + Port "10" + } + Block { + BlockType Inport + Name "JDot_r_sole_nu" + SID "4388" + Position [1065, 573, 1095, 587] + ZOrder 841 + Port "11" + } + Block { + BlockType Inport + Name "jointPos" + SID "2358" + Position [1065, 33, 1095, 47] + ZOrder 323 + Port "12" + } + Block { + BlockType Inport + Name "jointAcc" + SID "4969" + Position [2050, 103, 2080, 117] + ZOrder 863 + Port "13" + } + Block { + BlockType Inport + Name "nu" + SID "2359" + Position [1065, 123, 1095, 137] + ZOrder 325 + Port "14" + } + Block { + BlockType Inport + Name "state" + SID "4387" + Position [1065, 318, 1095, 332] + ZOrder 840 + Port "15" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "2363" + Position [1065, -102, 1095, -88] + ZOrder 386 + Port "16" + } + Block { + BlockType Inport + Name "KP_postural" + SID "2356" + Position [1065, 843, 1095, 857] + ZOrder 336 + Port "17" + } + Block { + BlockType Inport + Name "KP_CoM" + SID "3328" + Position [1065, 753, 1095, 767] + ZOrder 644 + Port "18" + } + Block { + BlockType Inport + Name "KD_CoM" + SID "3329" + Position [1065, 798, 1095, 812] + ZOrder 645 + Port "19" + } + Block { + BlockType Inport + Name "desired_pos_vel_acc_CoM" + SID "2365" + Position [1065, 708, 1095, 722] + ZOrder 335 + Port "20" + } + Block { + BlockType Inport + Name "jointPos_des" + SID "2364" + Position [1065, 78, 1095, 92] + ZOrder 324 + Port "21" + } + Block { + BlockType SubSystem + Name "Compute Desired Torques" + SID "4393" + Ports [13, 2] + Position [1825, -185, 1990, 865] + ZOrder 846 + ForegroundColor "red" + BackgroundColor "yellow" + RequestExecContextInheritance off + System { + Name "Compute Desired Torques" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "167" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "feetContactStatus" + SID "4410" + Position [430, 464, 460, 476] + ZOrder 640 + } + Block { + BlockType Inport + Name "HessianMatrixOneFoot" + SID "4411" + Position [430, 508, 460, 522] + ZOrder 641 + Port "2" + } + Block { + BlockType Inport + Name "gradientOneFoot" + SID "4412" + Position [430, 553, 460, 567] + ZOrder 642 + Port "3" + } + Block { + BlockType Inport + Name "ConstraintsMatrixOneFoot" + SID "4413" + Position [430, 598, 460, 612] + ZOrder 647 + Port "4" + } + Block { + BlockType Inport + Name "bVectorConstraintsOneFoot" + SID "4414" + Position [430, 643, 460, 657] + ZOrder 648 + Port "5" + } + Block { + BlockType Inport + Name "HessianMatrixTwoFeet" + SID "4415" + Position [430, 688, 460, 702] + ZOrder 643 + Port "6" + } + Block { + BlockType Inport + Name "gradientTwoFeet" + SID "4416" + Position [430, 733, 460, 747] + ZOrder 644 + Port "7" + } + Block { + BlockType Inport + Name "ConstraintsMatrixTwoFeet" + SID "4417" + Position [430, 778, 460, 792] + ZOrder 645 + Port "8" + } + Block { + BlockType Inport + Name "bVectorConstraintsTwoFeet" + SID "4418" + Position [430, 823, 460, 837] + ZOrder 646 + Port "9" + } + Block { + BlockType Inport + Name "tauModel" + SID "4395" + Position [1235, 553, 1265, 567] + ZOrder 412 + Port "10" + } + Block { + BlockType Inport + Name "Sigma" + SID "4396" + Position [1235, 643, 1265, 657] + ZOrder 413 + Port "11" + } + Block { + BlockType Inport + Name "Na" + SID "4397" + Position [835, 743, 865, 757] + ZOrder 414 + Port "12" + } + Block { + BlockType Inport + Name "f_LDot" + SID "4420" + Position [835, 653, 865, 667] + ZOrder 650 + Port "13" + } + Block { + BlockType Sum + Name "Add" + SID "2367" + Ports [2, 1] + Position [1525, 506, 1580, 719] + ZOrder 400 + ShowName off + IconShape "rectangular" + } + Block { + BlockType Sum + Name "Add1" + SID "2368" + Ports [2, 1] + Position [1055, 500, 1090, 855] + ZOrder 409 + ShowName off + IconShape "rectangular" + } + Block { + BlockType Product + Name "Product1" + SID "2381" + Ports [2, 1] + Position [1420, 634, 1485, 696] + ZOrder 397 + ShowName off + Multiplication "Matrix(*)" + } + Block { + BlockType Product + Name "Product2" + SID "2382" + Ports [2, 1] + Position [915, 734, 975, 796] + ZOrder 410 + ShowName off + Multiplication "Matrix(*)" + } + Block { + BlockType SubSystem + Name "QPSolver" + SID "4579" + Ports [9, 3] + Position [565, 452, 760, 848] + ZOrder 652 + ForegroundColor "red" + BackgroundColor "yellow" + RequestExecContextInheritance off + System { + Name "QPSolver" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "225" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "feetContactStatus" + SID "4580" + Position [-480, -106, -450, -94] + ZOrder 394 + } + Block { + BlockType Inport + Name "HessianMatrixOneFoot" + SID "4581" + Position [-230, -322, -200, -308] + ZOrder 395 + Port "2" + } + Block { + BlockType Inport + Name "gradientOneFoot" + SID "4582" + Position [-230, -292, -200, -278] + ZOrder 396 + Port "3" + } + Block { + BlockType Inport + Name "ConstraintsMatrixOneFoot" + SID "4583" + Position [-230, -262, -200, -248] + ZOrder 403 + Port "4" + } + Block { + BlockType Inport + Name "bVectorConstraintsOneFoot" + SID "4584" + Position [-230, -232, -200, -218] + ZOrder 404 + Port "5" + } + Block { + BlockType Inport + Name "HessianMatrixTwoFeet" + SID "4585" + Position [-230, 8, -200, 22] + ZOrder 397 + Port "6" + } + Block { + BlockType Inport + Name "gradientTwoFeet" + SID "4586" + Position [-230, 48, -200, 62] + ZOrder 398 + Port "7" + } + Block { + BlockType Inport + Name "ConstraintsMatrixTwoFeet" + SID "4587" + Position [-230, 88, -200, 102] + ZOrder 399 + Port "8" + } + Block { + BlockType Inport + Name "bVectorConstraintsTwoFeet" + SID "4588" + Position [-230, 128, -200, 142] + ZOrder 400 + Port "9" + } + Block { + BlockType Goto + Name "Goto" + SID "4710" + Position [320, -107, 380, -93] + ZOrder 737 + ShowName off + HideAutomaticName off + GotoTag "QPStatus" + TagVisibility "global" + } + Block { + BlockType Logic + Name "NOT" + SID "4625" + Ports [1, 1] + Position [57, -62, 83, -27] + ZOrder 723 + BlockRotation 270 + BlockMirror on + NameLocation "right" + HideAutomaticName off + Operator "NOT" + IconShape "distinctive" + AllPortsSameDT off + OutDataTypeStr "boolean" + } + Block { + BlockType SubSystem + Name "One Foot Two Feet QP Selector" + SID "4590" + Ports [1, 1] + Position [-345, -127, -85, -73] + ZOrder 721 + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "One Foot Two Feet QP Selector" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "108" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "feetContactStatus" + SID "4590::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Demux + Name " Demux " + SID "4590::107" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 98 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4590::106" + Tag "Stateflow S-Function 15" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 97 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "onOneFoot" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4590::108" + Position [460, 241, 480, 259] + ZOrder 99 + } + Block { + BlockType Outport + Name "onOneFoot" + SID "4590::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 109 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "onOneFoot" + ZOrder 110 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "onOneFoot" + DstPort 1 + } + Line { + ZOrder 111 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 112 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "QP One Foot" + SID "4696" + Ports [5, 2, 1] + Position [25, -332, 120, -178] + ZOrder 736 + ForegroundColor "red" + BackgroundColor "yellow" + NameLocation "top" + TreatAsAtomicUnit on + RequestExecContextInheritance off + System { + Name "QP One Foot" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "223" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "H" + SID "4685" + Position [970, -242, 1000, -228] + ZOrder 750 + } + Block { + BlockType Inport + Name "g" + SID "4686" + Position [970, -212, 1000, -198] + ZOrder 752 + Port "2" + } + Block { + BlockType Inport + Name "A" + SID "4687" + Position [970, -182, 1000, -168] + ZOrder 753 + Port "3" + } + Block { + BlockType Inport + Name "ubA" + SID "4688" + Position [970, -122, 1000, -108] + ZOrder 754 + Port "4" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "4708" + Position [1360, 13, 1390, 27] + ZOrder 762 + Port "5" + } + Block { + BlockType EnablePort + Name "Enable" + SID "4689" + Ports [] + Position [1185, -290, 1205, -270] + ZOrder 756 + ShowName off + HideAutomaticName off + } + Block { + BlockType SubSystem + Name "Analytical Solution QP One Foot (unconstrained)" + SID "4690" + Ports [2, 1] + Position [1285, -405, 1465, -305] + ZOrder 758 + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Analytical Solution QP One Foot (unconstrained)" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "109" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "H" + SID "4690::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Inport + Name "g" + SID "4690::22" + Position [20, 136, 40, 154] + ZOrder 13 + Port "2" + } + Block { + BlockType Demux + Name " Demux " + SID "4690::108" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 99 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4690::107" + Tag "Stateflow S-Function 26" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 98 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "analyticalSolution" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4690::109" + Position [460, 241, 480, 259] + ZOrder 100 + } + Block { + BlockType Outport + Name "analyticalSolution" + SID "4690::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 86 + SrcBlock "H" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 87 + SrcBlock "g" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "analyticalSolution" + ZOrder 88 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "analyticalSolution" + DstPort 1 + } + Line { + ZOrder 89 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 90 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Constant + Name "Constant" + SID "5006" + Position [970, -153, 1035, -137] + ZOrder 765 + Value "-Inf(19,1)" + } + Block { + BlockType Reference + Name "MatchSignalSizes" + SID "4691" + Ports [2, 1] + Position [1340, -277, 1415, -188] + ZOrder 757 + ShowName off + HideAutomaticName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Utilities/MatchSignalSizes" + SourceType "MatchSignalSizes" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + } + Block { + BlockType SubSystem + Name "Process QP output" + SID "4692" + Ports [4, 1] + Position [1550, -421, 1710, 86] + ZOrder 759 + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Process QP output" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "117" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "analyticalSolution" + SID "4692::29" + Position [20, 101, 40, 119] + ZOrder 20 + } + Block { + BlockType Inport + Name "primalSolution" + SID "4692::1" + Position [20, 136, 40, 154] + ZOrder -1 + Port "2" + } + Block { + BlockType Inport + Name "QPStatus" + SID "4692::22" + Position [20, 171, 40, 189] + ZOrder 13 + Port "3" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "4692::66" + Position [20, 206, 40, 224] + ZOrder 51 + Port "4" + } + Block { + BlockType Demux + Name " Demux " + SID "4692::116" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 101 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4692::115" + Tag "Stateflow S-Function 27" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 100 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[4 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "f_star" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4692::117" + Position [460, 241, 480, 259] + ZOrder 102 + } + Block { + BlockType Outport + Name "f_star" + SID "4692::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 126 + SrcBlock "analyticalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 127 + SrcBlock "primalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 128 + SrcBlock "QPStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 129 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "f_star" + ZOrder 130 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "f_star" + DstPort 1 + } + Line { + ZOrder 131 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 132 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "QP One Foot" + SID "4693" + Ports [5, 2] + Position [1140, -251, 1240, -99] + ZOrder 751 + ForegroundColor "red" + BackgroundColor "yellow" + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Utilities/QP" + SourceType "QP" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + lbA on + ubA on + lb off + ub off + computeObjVal off + stopIfFails off + } + Block { + BlockType Outport + Name "f_star_oneFoot" + SID "4695" + Position [1790, -172, 1820, -158] + ZOrder 760 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "QPStatus" + SID "4694" + Position [1360, -162, 1390, -148] + ZOrder 755 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "MatchSignalSizes" + SrcPort 1 + DstBlock "Process QP output" + DstPort 2 + } + Line { + ZOrder 3 + SrcBlock "A" + SrcPort 1 + DstBlock "QP One Foot" + DstPort 3 + } + Line { + ZOrder 4 + SrcBlock "H" + SrcPort 1 + Points [31, 0] + Branch { + ZOrder 34 + Points [0, -145] + DstBlock "Analytical Solution QP One Foot (unconstrained)" + DstPort 1 + } + Branch { + ZOrder 6 + DstBlock "QP One Foot" + DstPort 1 + } + } + Line { + ZOrder 54 + SrcBlock "ubA" + SrcPort 1 + DstBlock "QP One Foot" + DstPort 5 + } + Line { + ZOrder 8 + SrcBlock "g" + SrcPort 1 + Points [51, 0] + Branch { + ZOrder 33 + Points [0, -50] + Branch { + ZOrder 40 + Points [0, -75] + DstBlock "Analytical Solution QP One Foot (unconstrained)" + DstPort 2 + } + Branch { + ZOrder 11 + DstBlock "MatchSignalSizes" + DstPort 1 + } + } + Branch { + ZOrder 12 + DstBlock "QP One Foot" + DstPort 2 + } + } + Line { + ZOrder 13 + SrcBlock "Analytical Solution QP One Foot (unconstrained)" + SrcPort 1 + DstBlock "Process QP output" + DstPort 1 + } + Line { + ZOrder 14 + SrcBlock "QP One Foot" + SrcPort 2 + Points [44, 0] + Branch { + ZOrder 59 + Points [0, 30] + DstBlock "Process QP output" + DstPort 3 + } + Branch { + ZOrder 58 + Points [0, -20] + DstBlock "QPStatus" + DstPort 1 + } + } + Line { + ZOrder 17 + SrcBlock "QP One Foot" + SrcPort 1 + DstBlock "MatchSignalSizes" + DstPort 2 + } + Line { + ZOrder 42 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock "Process QP output" + DstPort 4 + } + Line { + ZOrder 43 + SrcBlock "Process QP output" + SrcPort 1 + DstBlock "f_star_oneFoot" + DstPort 1 + } + Line { + ZOrder 55 + SrcBlock "Constant" + SrcPort 1 + DstBlock "QP One Foot" + DstPort 4 + } + } + } + Block { + BlockType SubSystem + Name "QP Two Feet" + SID "4684" + Ports [4, 2, 1] + Position [20, -5, 120, 155] + ZOrder 734 + TreatAsAtomicUnit on + RequestExecContextInheritance off + System { + Name "QP Two Feet" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "279" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "H" + SID "4673" + Position [1460, 1023, 1490, 1037] + ZOrder 751 + } + Block { + BlockType Inport + Name "g" + SID "4674" + Position [1460, 1053, 1490, 1067] + ZOrder 753 + Port "2" + } + Block { + BlockType Inport + Name "A" + SID "4675" + Position [1460, 1083, 1490, 1097] + ZOrder 754 + Port "3" + } + Block { + BlockType Inport + Name "ubA" + SID "4676" + Position [1460, 1143, 1490, 1157] + ZOrder 755 + Port "4" + } + Block { + BlockType EnablePort + Name "Enable" + SID "4677" + Ports [] + Position [1615, 940, 1635, 960] + ZOrder 750 + ShowName off + HideAutomaticName off + } + Block { + BlockType SubSystem + Name "Analytical Solution Two Feet (unconstrained)" + SID "4678" + Ports [2, 1] + Position [1690, 850, 1875, 925] + ZOrder 758 + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Analytical Solution Two Feet (unconstrained)" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "109" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "H" + SID "4678::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Inport + Name "g" + SID "4678::22" + Position [20, 136, 40, 154] + ZOrder 13 + Port "2" + } + Block { + BlockType Demux + Name " Demux " + SID "4678::108" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 99 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4678::107" + Tag "Stateflow S-Function 24" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 98 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "analyticalSolution" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4678::109" + Position [460, 241, 480, 259] + ZOrder 100 + } + Block { + BlockType Outport + Name "analyticalSolution" + SID "4678::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 86 + SrcBlock "H" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 87 + SrcBlock "g" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "analyticalSolution" + ZOrder 88 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "analyticalSolution" + DstPort 1 + } + Line { + ZOrder 89 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 90 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Constant + Name "Constant" + SID "5007" + Position [1460, 1112, 1525, 1128] + ZOrder 766 + Value "-Inf(38,1)" + } + Block { + BlockType Reference + Name "MatchSignalSizes" + SID "4679" + Ports [2, 1] + Position [1735, 976, 1825, 1069] + ZOrder 757 + ShowName off + HideAutomaticName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Utilities/MatchSignalSizes" + SourceType "MatchSignalSizes" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + } + Block { + BlockType SubSystem + Name "Process QP output" + SID "4680" + Ports [3, 1] + Position [1920, 824, 2100, 1226] + ZOrder 759 + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Process QP output" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "117" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "analyticalSolution" + SID "4680::29" + Position [20, 101, 40, 119] + ZOrder 20 + } + Block { + BlockType Inport + Name "primalSolution" + SID "4680::1" + Position [20, 136, 40, 154] + ZOrder -1 + Port "2" + } + Block { + BlockType Inport + Name "QPStatus" + SID "4680::22" + Position [20, 171, 40, 189] + ZOrder 13 + Port "3" + } + Block { + BlockType Demux + Name " Demux " + SID "4680::116" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 100 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4680::115" + Tag "Stateflow S-Function 25" + Ports [3, 2] + Position [180, 100, 230, 180] + ZOrder 99 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[3 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "f_star" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4680::117" + Position [460, 241, 480, 259] + ZOrder 101 + } + Block { + BlockType Outport + Name "f_star" + SID "4680::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 122 + SrcBlock "analyticalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 123 + SrcBlock "primalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 124 + SrcBlock "QPStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + Name "f_star" + ZOrder 125 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "f_star" + DstPort 1 + } + Line { + ZOrder 126 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 127 + SrcBlock " SFunction " + SrcPort 1 + Points [20, 0] + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "QP Two Feet" + SID "4681" + Ports [5, 2] + Position [1575, 1014, 1685, 1166] + ZOrder 752 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Utilities/QP" + SourceType "QP" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + lbA on + ubA on + lb off + ub off + computeObjVal off + stopIfFails off + } + Block { + BlockType Outport + Name "QPStatus" + SID "4683" + Position [1765, 1098, 1795, 1112] + ZOrder 756 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "f0_twoFeet" + SID "4682" + Position [2155, 1018, 2185, 1032] + ZOrder 760 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 37 + SrcBlock "QP Two Feet" + SrcPort 2 + Points [0, 0; 23, 0] + Branch { + ZOrder 91 + Points [0, -25] + DstBlock "QPStatus" + DstPort 1 + } + Branch { + ZOrder 62 + Points [0, 30] + DstBlock "Process QP output" + DstPort 3 + } + } + Line { + ZOrder 38 + SrcBlock "A" + SrcPort 1 + DstBlock "QP Two Feet" + DstPort 3 + } + Line { + ZOrder 39 + SrcBlock "Analytical Solution Two Feet (unconstrained)" + SrcPort 1 + DstBlock "Process QP output" + DstPort 1 + } + Line { + ZOrder 40 + SrcBlock "Process QP output" + SrcPort 1 + DstBlock "f0_twoFeet" + DstPort 1 + } + Line { + ZOrder 41 + SrcBlock "MatchSignalSizes" + SrcPort 1 + DstBlock "Process QP output" + DstPort 2 + } + Line { + ZOrder 42 + SrcBlock "QP Two Feet" + SrcPort 1 + Points [20, 0; 0, -10] + DstBlock "MatchSignalSizes" + DstPort 2 + } + Line { + ZOrder 45 + SrcBlock "H" + SrcPort 1 + Points [22, 0] + Branch { + ZOrder 90 + Points [0, -160] + DstBlock "Analytical Solution Two Feet (unconstrained)" + DstPort 1 + } + Branch { + ZOrder 79 + DstBlock "QP Two Feet" + DstPort 1 + } + } + Line { + ZOrder 50 + SrcBlock "g" + SrcPort 1 + Points [47, 0] + Branch { + ZOrder 89 + Points [0, -60] + Branch { + ZOrder 92 + Points [0, -95] + DstBlock "Analytical Solution Two Feet (unconstrained)" + DstPort 2 + } + Branch { + ZOrder 47 + DstBlock "MatchSignalSizes" + DstPort 1 + } + } + Branch { + ZOrder 46 + DstBlock "QP Two Feet" + DstPort 2 + } + } + Line { + ZOrder 93 + SrcBlock "ubA" + SrcPort 1 + DstBlock "QP Two Feet" + DstPort 5 + } + Line { + ZOrder 94 + SrcBlock "Constant" + SrcPort 1 + DstBlock "QP Two Feet" + DstPort 4 + } + } + } + Block { + BlockType Switch + Name "Switch" + SID "4607" + Position [225, -133, 290, -67] + ZOrder 732 + ShowName off + HideAutomaticName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "f_star_oneFoot" + SID "4626" + Position [245, -297, 275, -283] + ZOrder 401 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "onOneFoot" + SID "4717" + Position [-10, -137, 20, -123] + ZOrder 738 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "f_0_twoFeet" + SID "4627" + Position [240, 108, 270, 122] + ZOrder 728 + Port "3" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 3 + SrcBlock "feetContactStatus" + SrcPort 1 + Points [61, 0] + Branch { + ZOrder 65 + Points [0, -95] + DstBlock "QP One Foot" + DstPort 5 + } + Branch { + ZOrder 64 + DstBlock "One Foot Two Feet QP Selector" + DstPort 1 + } + } + Line { + ZOrder 41 + SrcBlock "HessianMatrixOneFoot" + SrcPort 1 + DstBlock "QP One Foot" + DstPort 1 + } + Line { + ZOrder 45 + SrcBlock "bVectorConstraintsTwoFeet" + SrcPort 1 + DstBlock "QP Two Feet" + DstPort 4 + } + Line { + ZOrder 40 + SrcBlock "gradientOneFoot" + SrcPort 1 + DstBlock "QP One Foot" + DstPort 2 + } + Line { + ZOrder 37 + SrcBlock "ConstraintsMatrixOneFoot" + SrcPort 1 + DstBlock "QP One Foot" + DstPort 3 + } + Line { + ZOrder 44 + SrcBlock "gradientTwoFeet" + SrcPort 1 + DstBlock "QP Two Feet" + DstPort 2 + } + Line { + ZOrder 43 + SrcBlock "ConstraintsMatrixTwoFeet" + SrcPort 1 + DstBlock "QP Two Feet" + DstPort 3 + } + Line { + ZOrder 36 + SrcBlock "bVectorConstraintsOneFoot" + SrcPort 1 + DstBlock "QP One Foot" + DstPort 4 + } + Line { + ZOrder 42 + SrcBlock "HessianMatrixTwoFeet" + SrcPort 1 + DstBlock "QP Two Feet" + DstPort 1 + } + Line { + ZOrder 39 + SrcBlock "QP One Foot" + SrcPort 2 + Points [41, 0; 0, 95] + DstBlock "Switch" + DstPort 1 + } + Line { + ZOrder 62 + SrcBlock "QP Two Feet" + SrcPort 1 + Points [40, 0; 0, -115] + DstBlock "Switch" + DstPort 3 + } + Line { + ZOrder 63 + SrcBlock "QP Two Feet" + SrcPort 2 + DstBlock "f_0_twoFeet" + DstPort 1 + } + Line { + ZOrder 50 + SrcBlock "NOT" + SrcPort 1 + DstBlock "QP Two Feet" + DstPort enable + } + Line { + ZOrder 53 + SrcBlock "One Foot Two Feet QP Selector" + SrcPort 1 + Points [29, 0] + Branch { + ZOrder 79 + Points [0, -30] + DstBlock "onOneFoot" + DstPort 1 + } + Branch { + ZOrder 78 + Points [121, 0] + Branch { + ZOrder 77 + DstBlock "NOT" + DstPort 1 + } + Branch { + ZOrder 73 + DstBlock "Switch" + DstPort 2 + } + Branch { + ZOrder 55 + DstBlock "QP One Foot" + DstPort enable + } + } + } + Line { + ZOrder 74 + SrcBlock "QP One Foot" + SrcPort 1 + DstBlock "f_star_oneFoot" + DstPort 1 + } + Line { + ZOrder 75 + SrcBlock "Switch" + SrcPort 1 + DstBlock "Goto" + DstPort 1 + } + } + } + Block { + BlockType Switch + Name "f_LDot is f_star while on One Foot" + SID "4712" + Position [910, 482, 980, 698] + ZOrder 653 + NameLocation "top" + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "tau_QP" + SID "4408" + Position [1630, 608, 1660, 622] + ZOrder 425 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "f_star" + SID "4409" + Position [1440, 733, 1470, 747] + ZOrder 639 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 293 + SrcBlock "Na" + SrcPort 1 + DstBlock "Product2" + DstPort 1 + } + Line { + ZOrder 54 + SrcBlock "Product1" + SrcPort 1 + DstBlock "Add" + DstPort 2 + } + Line { + ZOrder 291 + SrcBlock "tauModel" + SrcPort 1 + DstBlock "Add" + DstPort 1 + } + Line { + ZOrder 304 + SrcBlock "Add" + SrcPort 1 + DstBlock "tau_QP" + DstPort 1 + } + Line { + ZOrder 57 + SrcBlock "Add1" + SrcPort 1 + Points [253, 0] + Branch { + ZOrder 407 + Points [0, 60] + DstBlock "f_star" + DstPort 1 + } + Branch { + ZOrder 383 + DstBlock "Product1" + DstPort 2 + } + } + Line { + ZOrder 406 + SrcBlock "f_LDot is f_star while on One Foot" + SrcPort 1 + DstBlock "Add1" + DstPort 1 + } + Line { + ZOrder 66 + SrcBlock "Product2" + SrcPort 1 + DstBlock "Add1" + DstPort 2 + } + Line { + ZOrder 292 + SrcBlock "Sigma" + SrcPort 1 + DstBlock "Product1" + DstPort 1 + } + Line { + ZOrder 355 + SrcBlock "QPSolver" + SrcPort 3 + DstBlock "Product2" + DstPort 2 + } + Line { + ZOrder 346 + SrcBlock "gradientOneFoot" + SrcPort 1 + DstBlock "QPSolver" + DstPort 3 + } + Line { + ZOrder 351 + SrcBlock "ConstraintsMatrixTwoFeet" + SrcPort 1 + DstBlock "QPSolver" + DstPort 8 + } + Line { + ZOrder 344 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock "QPSolver" + DstPort 1 + } + Line { + ZOrder 348 + SrcBlock "bVectorConstraintsOneFoot" + SrcPort 1 + DstBlock "QPSolver" + DstPort 5 + } + Line { + ZOrder 345 + SrcBlock "HessianMatrixOneFoot" + SrcPort 1 + DstBlock "QPSolver" + DstPort 2 + } + Line { + ZOrder 347 + SrcBlock "ConstraintsMatrixOneFoot" + SrcPort 1 + DstBlock "QPSolver" + DstPort 4 + } + Line { + ZOrder 350 + SrcBlock "gradientTwoFeet" + SrcPort 1 + DstBlock "QPSolver" + DstPort 7 + } + Line { + ZOrder 349 + SrcBlock "HessianMatrixTwoFeet" + SrcPort 1 + DstBlock "QPSolver" + DstPort 6 + } + Line { + ZOrder 352 + SrcBlock "bVectorConstraintsTwoFeet" + SrcPort 1 + DstBlock "QPSolver" + DstPort 9 + } + Line { + ZOrder 376 + SrcBlock "QPSolver" + SrcPort 2 + Points [27, 0; 0, -60] + DstBlock "f_LDot is f_star while on One Foot" + DstPort 2 + } + Line { + ZOrder 378 + SrcBlock "f_LDot" + SrcPort 1 + DstBlock "f_LDot is f_star while on One Foot" + DstPort 3 + } + Line { + ZOrder 405 + SrcBlock "QPSolver" + SrcPort 1 + DstBlock "f_LDot is f_star while on One Foot" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Compute angular momentum integral error" + SID "3714" + Ports [4, 1] + Position [1250, 289, 1420, 331] + ZOrder 835 + RequestExecContextInheritance off + System { + Name "Compute angular momentum integral error" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "122" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos_des" + SID "3715" + Position [-230, -147, -200, -133] + ZOrder -1 + } + Block { + BlockType Inport + Name "jointPos" + SID "3716" + Position [-230, 143, -200, 157] + ZOrder 1 + Port "2" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "3717" + Position [620, 98, 650, 112] + ZOrder 80 + Port "3" + } + Block { + BlockType Inport + Name "state" + SID "4028" + Position [50, -147, 80, -133] + ZOrder 732 + Port "4" + } + Block { + BlockType Sum + Name "Add" + SID "4724" + Ports [2, 1] + Position [-40, 114, -5, 161] + ZOrder 902 + ShowName off + HideAutomaticName off + IconShape "rectangular" + Inputs "-+" + } + Block { + BlockType Reference + Name "CentroidalMomentum" + SID "3718" + Ports [4, 1] + Position [1175, -194, 1325, 259] + ZOrder 73 + HideAutomaticName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + } + Block { + BlockType SubSystem + Name "Compute base to fixed link transform" + SID "4422" + Ports [1, 2] + Position [-120, -229, 5, -51] + ZOrder 901 + RequestExecContextInheritance off + System { + Name "Compute base to fixed link transform" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "426" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos" + SID "4423" + Position [50, 108, 80, 122] + ZOrder 902 + } + Block { + BlockType Constant + Name "Constant7" + SID "4425" + Position [50, -43, 80, -27] + ZOrder 895 + ShowName off + Value "eye(4)" + } + Block { + BlockType Product + Name "inv " + SID "4434" + Ports [1, 1] + Position [385, -34, 415, 4] + ZOrder 913 + ShowName off + HideAutomaticName off + Inputs "/" + Multiplication "Matrix(*)" + } + Block { + BlockType Product + Name "inv 1" + SID "4723" + Ports [1, 1] + Position [385, 81, 415, 119] + ZOrder 914 + ShowName off + HideAutomaticName off + Inputs "/" + Multiplication "Matrix(*)" + } + Block { + BlockType Reference + Name "l_sole to root_link relative transform" + SID "4450" + Ports [2, 1] + Position [155, -51, 320, 16] + ZOrder 894 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "r_sole to root_link relative transform" + SID "4475" + Ports [2, 1] + Position [155, 64, 325, 131] + ZOrder 899 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.RIGHT_FOOT" + } + Block { + BlockType Outport + Name "fixed_l_sole_H_b" + SID "4476" + Position [495, -22, 525, -8] + ZOrder 910 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "fixed_r_sole_H_b" + SID "4477" + Position [495, 93, 525, 107] + ZOrder 912 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "Constant7" + SrcPort 1 + Points [33, 0] + Branch { + ZOrder 26 + DstBlock "l_sole to root_link relative transform" + DstPort 1 + } + Branch { + ZOrder 23 + Points [0, 115] + DstBlock "r_sole to root_link relative transform" + DstPort 1 + } + } + Line { + ZOrder 28 + SrcBlock "inv 1" + SrcPort 1 + DstBlock "fixed_r_sole_H_b" + DstPort 1 + } + Line { + ZOrder 22 + SrcBlock "inv " + SrcPort 1 + DstBlock "fixed_l_sole_H_b" + DstPort 1 + } + Line { + ZOrder 11 + SrcBlock "jointPos" + SrcPort 1 + Points [18, 0] + Branch { + ZOrder 25 + DstBlock "r_sole to root_link relative transform" + DstPort 2 + } + Branch { + ZOrder 24 + Points [0, -115] + DstBlock "l_sole to root_link relative transform" + DstPort 2 + } + } + Line { + ZOrder 27 + SrcBlock "l_sole to root_link relative transform" + SrcPort 1 + DstBlock "inv " + DstPort 1 + } + Line { + ZOrder 29 + SrcBlock "r_sole to root_link relative transform" + SrcPort 1 + DstBlock "inv 1" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Get Equivalent Base Velocity" + SID "3721" + Ports [4, 1] + Position [810, 21, 1110, 154] + ZOrder 75 + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Get Equivalent Base Velocity" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "3825" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "J_l_sole" + SID "3721::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Inport + Name "J_r_sole" + SID "3721::23" + Position [20, 136, 40, 154] + ZOrder 9 + Port "2" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "3721::3701" + Position [20, 171, 40, 189] + ZOrder 26 + Port "3" + } + Block { + BlockType Inport + Name "jointPos_err" + SID "3721::3702" + Position [20, 206, 40, 224] + ZOrder 27 + Port "4" + } + Block { + BlockType Demux + Name " Demux " + SID "3721::3824" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 149 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "3721::3823" + Tag "Stateflow S-Function 20" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 148 + FunctionName "sf_sfun" + Parameters "Reg" + PortCounts "[4 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "baseVel_equivalent" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "3721::3825" + Position [460, 241, 480, 259] + ZOrder 150 + } + Block { + BlockType Outport + Name "baseVel_equivalent" + SID "3721::24" + Position [460, 101, 480, 119] + ZOrder 10 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 267 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 268 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 269 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 270 + SrcBlock "jointPos_err" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "baseVel_equivalent" + ZOrder 271 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "baseVel_equivalent" + DstPort 1 + } + Line { + ZOrder 272 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 273 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "Jacobian" + SID "3719" + Ports [2, 1] + Position [550, 24, 715, 46] + ZOrder 40 + ShowName off + HideAutomaticName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" + SourceType "Jacobian" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "Jacobian1" + SID "3720" + Ports [2, 1] + Position [550, 59, 715, 81] + ZOrder 79 + ShowName off + HideAutomaticName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" + SourceType "Jacobian" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.RIGHT_FOOT" + } + Block { + BlockType SubSystem + Name "Select base to world transform" + SID "4027" + Ports [1, 1] + Position [130, -159, 345, -121] + ZOrder 731 + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Select base to world transform" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "138" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "state" + SID "4027::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Demux + Name " Demux " + SID "4027::137" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 128 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4027::136" + Tag "Stateflow S-Function 19" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 127 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "booleanState" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4027::138" + Position [460, 241, 480, 259] + ZOrder 129 + } + Block { + BlockType Outport + Name "booleanState" + SID "4027::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 153 + SrcBlock "state" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "booleanState" + ZOrder 154 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "booleanState" + DstPort 1 + } + Line { + ZOrder 155 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 156 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Selector + Name "Selector" + SID "3722" + Ports [1, 1] + Position [1365, 16, 1405, 54] + ZOrder 78 + ShowName off + InputPortWidth "6" + IndexOptions "Index vector (dialog)" + Indices "[4 5 6]" + OutputSizes "1" + } + Block { + BlockType Switch + Name "Switch" + SID "4029" + Position [405, -205, 465, -75] + ZOrder 733 + ShowName off + HideAutomaticName off + Criteria "u2 > Threshold" + Threshold "0.1" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "intL_angMomError" + SID "3753" + Position [1450, 28, 1480, 42] + ZOrder -2 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "jointPos_des" + SrcPort 1 + Points [37, 0] + Branch { + ZOrder 121 + Points [0, 115] + Branch { + ZOrder 136 + Points [0, 65] + Branch { + ZOrder 131 + Points [0, 35] + Branch { + ZOrder 133 + Points [0, 50] + DstBlock "Add" + DstPort 1 + } + Branch { + ZOrder 126 + DstBlock "Jacobian1" + DstPort 2 + } + } + Branch { + ZOrder 124 + DstBlock "Jacobian" + DstPort 2 + } + } + Branch { + ZOrder 123 + DstBlock "CentroidalMomentum" + DstPort 2 + } + } + Branch { + ZOrder 90 + DstBlock "Compute base to fixed link transform" + DstPort 1 + } + } + Line { + ZOrder 127 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "Add" + DstPort 2 + } + Line { + ZOrder 129 + SrcBlock "Add" + SrcPort 1 + Points [757, 0] + Branch { + ZOrder 142 + DstBlock "Get Equivalent Base Velocity" + DstPort 4 + } + Branch { + ZOrder 141 + Points [0, 65] + DstBlock "CentroidalMomentum" + DstPort 4 + } + } + Line { + ZOrder 16 + SrcBlock "Get Equivalent Base Velocity" + SrcPort 1 + DstBlock "CentroidalMomentum" + DstPort 3 + } + Line { + ZOrder 18 + SrcBlock "CentroidalMomentum" + SrcPort 1 + DstBlock "Selector" + DstPort 1 + } + Line { + ZOrder 19 + SrcBlock "Selector" + SrcPort 1 + DstBlock "intL_angMomError" + DstPort 1 + } + Line { + ZOrder 25 + SrcBlock "state" + SrcPort 1 + DstBlock "Select base to world transform" + DstPort 1 + } + Line { + ZOrder 27 + SrcBlock "Select base to world transform" + SrcPort 1 + DstBlock "Switch" + DstPort 2 + } + Line { + ZOrder 28 + SrcBlock "Switch" + SrcPort 1 + Points [30, 0] + Branch { + ZOrder 138 + DstBlock "CentroidalMomentum" + DstPort 1 + } + Branch { + ZOrder 135 + Points [0, 170] + Branch { + ZOrder 139 + DstBlock "Jacobian" + DstPort 1 + } + Branch { + ZOrder 130 + Points [0, 35] + DstBlock "Jacobian1" + DstPort 1 + } + } + } + Line { + ZOrder 82 + SrcBlock "Compute base to fixed link transform" + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + Line { + ZOrder 83 + SrcBlock "Compute base to fixed link transform" + SrcPort 2 + DstBlock "Switch" + DstPort 3 + } + Line { + ZOrder 132 + SrcBlock "Jacobian" + SrcPort 1 + DstBlock "Get Equivalent Base Velocity" + DstPort 1 + } + Line { + ZOrder 134 + SrcBlock "Jacobian1" + SrcPort 1 + DstBlock "Get Equivalent Base Velocity" + DstPort 2 + } + Line { + ZOrder 140 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock "Get Equivalent Base Velocity" + DstPort 3 + } + } + } + Block { + BlockType Constant + Name "Constant" + SID "2371" + Position [1020, -63, 1140, -37] + ZOrder 383 + ShowName off + Value "ConstraintsMatrix" + } + Block { + BlockType Constant + Name "Constant1" + SID "2372" + Position [1020, -18, 1140, 8] + ZOrder 384 + ShowName off + Value "bVectorConstraints" + } + Block { + BlockType SubSystem + Name "From tau_QP to Joint Torques (motor reflected inertia)" + SID "4519" + Ports [2, 1] + Position [2145, 66, 2300, 124] + ZOrder 862 + RequestExecContextInheritance off + System { + Name "From tau_QP to Joint Torques (motor reflected inertia)" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "157" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "tau_QP" + SID "4520" + Position [-30, 3, 0, 17] + ZOrder 591 + } + Block { + BlockType Inport + Name "jointAcc" + SID "4998" + Position [-415, -107, -385, -93] + ZOrder 904 + Port "2" + } + Block { + BlockType Constant + Name " " + SID "4536" + Position [130, -56, 345, -44] + ZOrder 607 + Value "Config.USE_MOTOR_REFLECTED_INERTIA" + } + Block { + BlockType Constant + Name " 1" + SID "4651" + Position [-555, -134, -290, -116] + ZOrder 897 + ShowName off + HideAutomaticName off + Value "Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA" + } + Block { + BlockType SubSystem + Name "(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" + SID "4551" + Ports [0, 1] + Position [-500, -219, -340, -181] + ZOrder 871 + NameLocation "top" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + SystemSampleTime "Config.tStep" + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "158" + SimulinkSubDomain "Simulink" + Block { + BlockType Demux + Name " Demux " + SID "4551::156" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 147 + Outputs "1" + } + Block { + BlockType Ground + Name " Ground " + SID "4551::158" + Position [20, 121, 40, 139] + ZOrder 149 + } + Block { + BlockType S-Function + Name " SFunction " + SID "4551::155" + Tag "Stateflow S-Function 9" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 146 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "reflectedInertia" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4551::157" + Position [460, 241, 480, 259] + ZOrder 148 + } + Block { + BlockType Outport + Name "reflectedInertia" + SID "4551::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + Name "reflectedInertia" + ZOrder 137 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "reflectedInertia" + DstPort 1 + } + Line { + ZOrder 138 + SrcBlock " Ground " + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 139 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 140 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Sum + Name "Add" + SID "4531" + Ports [2, 1] + Position [65, -209, 110, -16] + ZOrder 602 + ShowName off + HideAutomaticName off + IconShape "rectangular" + Inputs "-+" + } + Block { + BlockType From + Name "From" + SID "4654" + Position [-460, -158, -375, -142] + ZOrder 902 + ShowName off + HideAutomaticName off + GotoTag "jointAcc_des" + TagVisibility "global" + } + Block { + BlockType Gain + Name "Gain" + SID "4525" + Position [-60, -175, 30, -145] + ZOrder 596 + ShowName off + HideAutomaticName off + Gain "Config.K_ff" + } + Block { + BlockType Product + Name "Product" + SID "4526" + Ports [2, 1] + Position [-170, -239, -135, -86] + ZOrder 597 + ShowName off + HideAutomaticName off + Multiplication "Matrix(*)" + } + Block { + BlockType Switch + Name "Switch" + SID "4535" + Position [390, -138, 440, 38] + ZOrder 606 + ShowName off + HideAutomaticName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Switch + Name "Switch1" + SID "4650" + Position [-260, -166, -205, -84] + ZOrder 896 + ShowName off + HideAutomaticName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "tau_joints" + SID "4522" + Position [485, -57, 515, -43] + ZOrder 592 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 6 + SrcBlock "Product" + SrcPort 1 + DstBlock "Gain" + DstPort 1 + } + Line { + ZOrder 16 + SrcBlock "Gain" + SrcPort 1 + DstBlock "Add" + DstPort 1 + } + Line { + ZOrder 23 + SrcBlock "Add" + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + Line { + ZOrder 24 + SrcBlock " " + SrcPort 1 + DstBlock "Switch" + DstPort 2 + } + Line { + ZOrder 41 + SrcBlock "Switch" + SrcPort 1 + DstBlock "tau_joints" + DstPort 1 + } + Line { + ZOrder 20 + SrcBlock "tau_QP" + SrcPort 1 + Points [24, 0] + Branch { + ZOrder 62 + Points [0, -75] + DstBlock "Add" + DstPort 2 + } + Branch { + ZOrder 93 + DstBlock "Switch" + DstPort 3 + } + } + Line { + ZOrder 52 + SrcBlock "(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" + SrcPort 1 + DstBlock "Product" + DstPort 1 + } + Line { + ZOrder 108 + SrcBlock " 1" + SrcPort 1 + DstBlock "Switch1" + DstPort 2 + } + Line { + ZOrder 109 + SrcBlock "Switch1" + SrcPort 1 + DstBlock "Product" + DstPort 2 + } + Line { + ZOrder 114 + SrcBlock "From" + SrcPort 1 + DstBlock "Switch1" + DstPort 1 + } + Line { + ZOrder 116 + SrcBlock "jointAcc" + SrcPort 1 + DstBlock "Switch1" + DstPort 3 + } + } + } + Block { + BlockType Goto + Name "Goto1" + SID "4495" + Position [2400, 31, 2470, 49] + ZOrder 856 + ShowName off + HideAutomaticName off + GotoTag "tau_star" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto5" + SID "4499" + Position [2035, 596, 2095, 614] + ZOrder 860 + ShowName off + HideAutomaticName off + GotoTag "f_star" + TagVisibility "global" + } + Block { + BlockType SubSystem + Name "Momentum Based Balancing Controller\n" + SID "2407" + Ports [22, 12] + Position [1465, -117, 1740, 872] + ZOrder 278 + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Momentum Based Balancing Controller\n" + Location [219, 337, 814, 775] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "3824" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "feetContactStatus" + SID "2407::28" + Position [20, 101, 40, 119] + ZOrder 14 + } + Block { + BlockType Inport + Name "ConstraintsMatrix" + SID "2407::808" + Position [20, 136, 40, 154] + ZOrder 56 + Port "2" + } + Block { + BlockType Inport + Name "bVectorConstraints" + SID "2407::809" + Position [20, 171, 40, 189] + ZOrder 57 + Port "3" + } + Block { + BlockType Inport + Name "jointPos" + SID "2407::29" + Position [20, 206, 40, 224] + ZOrder 15 + Port "4" + } + Block { + BlockType Inport + Name "jointPos_des" + SID "2407::1" + Position [20, 246, 40, 264] + ZOrder -1 + Port "5" + } + Block { + BlockType Inport + Name "nu" + SID "2407::22" + Position [20, 281, 40, 299] + ZOrder 8 + Port "6" + } + Block { + BlockType Inport + Name "M" + SID "2407::23" + Position [20, 316, 40, 334] + ZOrder 9 + Port "7" + } + Block { + BlockType Inport + Name "h" + SID "2407::24" + Position [20, 351, 40, 369] + ZOrder 10 + Port "8" + } + Block { + BlockType Inport + Name "L" + SID "2407::26" + Position [20, 386, 40, 404] + ZOrder 12 + Port "9" + } + Block { + BlockType Inport + Name "intL_angMomError" + SID "2407::21" + Position [20, 426, 40, 444] + ZOrder 7 + Port "10" + } + Block { + BlockType Inport + Name "w_H_l_sole" + SID "2407::800" + Position [20, 461, 40, 479] + ZOrder 50 + Port "11" + } + Block { + BlockType Inport + Name "w_H_r_sole" + SID "2407::27" + Position [20, 496, 40, 514] + ZOrder 13 + Port "12" + } + Block { + BlockType Inport + Name "J_l_sole" + SID "2407::841" + Position [20, 531, 40, 549] + ZOrder 84 + Port "13" + } + Block { + BlockType Inport + Name "J_r_sole" + SID "2407::790" + Position [20, 566, 40, 584] + ZOrder 41 + Port "14" + } + Block { + BlockType Inport + Name "JDot_l_sole_nu" + SID "2407::784" + Position [20, 606, 40, 624] + ZOrder 36 + Port "15" + } + Block { + BlockType Inport + Name "JDot_r_sole_nu" + SID "2407::785" + Position [20, 641, 40, 659] + ZOrder 37 + Port "16" + } + Block { + BlockType Inport + Name "pos_CoM" + SID "2407::798" + Position [20, 676, 40, 694] + ZOrder 48 + Port "17" + } + Block { + BlockType Inport + Name "J_CoM" + SID "2407::31" + Position [20, 711, 40, 729] + ZOrder 17 + Port "18" + } + Block { + BlockType Inport + Name "desired_pos_vel_acc_CoM" + SID "2407::30" + Position [20, 746, 40, 764] + ZOrder 16 + Port "19" + } + Block { + BlockType Inport + Name "KP_CoM" + SID "2407::32" + Position [20, 786, 40, 804] + ZOrder 18 + Port "20" + } + Block { + BlockType Inport + Name "KD_CoM" + SID "2407::845" + Position [20, 821, 40, 839] + ZOrder 88 + Port "21" + } + Block { + BlockType Inport + Name "KP_postural" + SID "2407::846" + Position [20, 856, 40, 874] + ZOrder 89 + Port "22" + } + Block { + BlockType Demux + Name " Demux " + SID "2407::3823" + Ports [1, 1] + Position [270, 570, 320, 610] + ZOrder 226 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "2407::3822" + Tag "Stateflow S-Function 17" + Ports [22, 13] + Position [180, 70, 230, 530] + ZOrder 225 + FunctionName "sf_sfun" + Parameters "Config,Gain,Reg" + PortCounts "[22 13]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "HessianMatrixOneFoot" + } + Port { + PortNumber 3 + Name "gradientOneFoot" + } + Port { + PortNumber 4 + Name "ConstraintsMatrixOneFoot" + } + Port { + PortNumber 5 + Name "bVectorConstraintsOneFoot" + } + Port { + PortNumber 6 + Name "HessianMatrixTwoFeet" + } + Port { + PortNumber 7 + Name "gradientTwoFeet" + } + Port { + PortNumber 8 + Name "ConstraintsMatrixTwoFeet" + } + Port { + PortNumber 9 + Name "bVectorConstraintsTwoFeet" + } + Port { + PortNumber 10 + Name "tauModel" + } + Port { + PortNumber 11 + Name "Sigma" + } + Port { + PortNumber 12 + Name "Na" + } + Port { + PortNumber 13 + Name "f_LDot" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "2407::3824" + Position [460, 581, 480, 599] + ZOrder 227 + } + Block { + BlockType Outport + Name "HessianMatrixOneFoot" + SID "2407::824" + Position [460, 101, 480, 119] + ZOrder 72 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "gradientOneFoot" + SID "2407::822" + Position [460, 136, 480, 154] + ZOrder 70 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "ConstraintsMatrixOneFoot" + SID "2407::5" + Position [460, 171, 480, 189] + ZOrder -5 + Port "3" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "bVectorConstraintsOneFoot" + SID "2407::810" + Position [460, 206, 480, 224] + ZOrder 58 + Port "4" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "HessianMatrixTwoFeet" + SID "2407::827" + Position [460, 246, 480, 264] + ZOrder 75 + Port "5" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "gradientTwoFeet" + SID "2407::828" + Position [460, 281, 480, 299] + ZOrder 76 + Port "6" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "ConstraintsMatrixTwoFeet" + SID "2407::811" + Position [460, 316, 480, 334] + ZOrder 59 + Port "7" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "bVectorConstraintsTwoFeet" + SID "2407::812" + Position [460, 351, 480, 369] + ZOrder 60 + Port "8" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "tauModel" + SID "2407::815" + Position [460, 386, 480, 404] + ZOrder 63 + Port "9" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "Sigma" + SID "2407::816" + Position [460, 426, 480, 444] + ZOrder 64 + Port "10" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "Na" + SID "2407::820" + Position [460, 461, 480, 479] + ZOrder 68 + Port "11" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "f_LDot" + SID "2407::821" + Position [460, 496, 480, 514] + ZOrder 69 + Port "12" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 2582 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 2583 + SrcBlock "ConstraintsMatrix" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 2584 + SrcBlock "bVectorConstraints" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 2585 + SrcBlock "jointPos" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 2586 + SrcBlock "jointPos_des" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 2587 + SrcBlock "nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + ZOrder 2588 + SrcBlock "M" + SrcPort 1 + DstBlock " SFunction " + DstPort 7 + } + Line { + ZOrder 2589 + SrcBlock "h" + SrcPort 1 + DstBlock " SFunction " + DstPort 8 + } + Line { + ZOrder 2590 + SrcBlock "L" + SrcPort 1 + DstBlock " SFunction " + DstPort 9 + } + Line { + ZOrder 2591 + SrcBlock "intL_angMomError" + SrcPort 1 + DstBlock " SFunction " + DstPort 10 + } + Line { + ZOrder 2592 + SrcBlock "w_H_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 11 + } + Line { + ZOrder 2593 + SrcBlock "w_H_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 12 + } + Line { + ZOrder 2594 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 13 + } + Line { + ZOrder 2595 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 14 + } + Line { + ZOrder 2596 + SrcBlock "JDot_l_sole_nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 15 + } + Line { + ZOrder 2597 + SrcBlock "JDot_r_sole_nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 16 + } + Line { + ZOrder 2598 + SrcBlock "pos_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 17 + } + Line { + ZOrder 2599 + SrcBlock "J_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 18 + } + Line { + ZOrder 2600 + SrcBlock "desired_pos_vel_acc_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 19 + } + Line { + ZOrder 2601 + SrcBlock "KP_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 20 + } + Line { + ZOrder 2602 + SrcBlock "KD_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 21 + } + Line { + ZOrder 2603 + SrcBlock "KP_postural" + SrcPort 1 + DstBlock " SFunction " + DstPort 22 + } + Line { + Name "HessianMatrixOneFoot" + ZOrder 2604 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "HessianMatrixOneFoot" + DstPort 1 + } + Line { + Name "gradientOneFoot" + ZOrder 2605 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "gradientOneFoot" + DstPort 1 + } + Line { + Name "ConstraintsMatrixOneFoot" + ZOrder 2606 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "ConstraintsMatrixOneFoot" + DstPort 1 + } + Line { + Name "bVectorConstraintsOneFoot" + ZOrder 2607 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "bVectorConstraintsOneFoot" + DstPort 1 + } + Line { + Name "HessianMatrixTwoFeet" + ZOrder 2608 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "HessianMatrixTwoFeet" + DstPort 1 + } + Line { + Name "gradientTwoFeet" + ZOrder 2609 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 7 + DstBlock "gradientTwoFeet" + DstPort 1 + } + Line { + Name "ConstraintsMatrixTwoFeet" + ZOrder 2610 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 8 + DstBlock "ConstraintsMatrixTwoFeet" + DstPort 1 + } + Line { + Name "bVectorConstraintsTwoFeet" + ZOrder 2611 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 9 + DstBlock "bVectorConstraintsTwoFeet" + DstPort 1 + } + Line { + Name "tauModel" + ZOrder 2612 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 10 + DstBlock "tauModel" + DstPort 1 + } + Line { + Name "Sigma" + ZOrder 2613 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 11 + DstBlock "Sigma" + DstPort 1 + } + Line { + Name "Na" + ZOrder 2614 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 12 + DstBlock "Na" + DstPort 1 + } + Line { + Name "f_LDot" + ZOrder 2615 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 13 + DstBlock "f_LDot" + DstPort 1 + } + Line { + ZOrder 2616 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 2617 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "tau_star " + SID "2414" + Position [2420, 88, 2450, 102] + ZOrder 279 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 303 + SrcBlock "Compute Desired Torques" + SrcPort 1 + DstBlock "From tau_QP to Joint Torques (motor reflected inertia)" + DstPort 1 + } + Line { + ZOrder 324 + SrcBlock "Compute Desired Torques" + SrcPort 2 + DstBlock "Goto5" + DstPort 1 + } + Line { + ZOrder 372 + SrcBlock "From tau_QP to Joint Torques (motor reflected inertia)" + SrcPort 1 + Points [56, 0] + Branch { + ZOrder 447 + Points [0, -55] + DstBlock "Goto1" + DstPort 1 + } + Branch { + ZOrder 389 + DstBlock "tau_star " + DstPort 1 + } + } + Line { + ZOrder 411 + SrcBlock "feetContactStatus" + SrcPort 1 + Points [63, 0] + Branch { + ZOrder 451 + Points [0, 410] + DstBlock "Compute angular momentum integral error" + DstPort 3 + } + Branch { + ZOrder 450 + Points [247, 0] + Branch { + ZOrder 493 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 1 + } + Branch { + ZOrder 446 + Points [0, -45] + DstBlock "Compute Desired Torques" + DstPort 1 + } + } + } + Line { + ZOrder 412 + SrcBlock "Constant1" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 3 + } + Line { + ZOrder 413 + SrcBlock "Constant" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 2 + } + Line { + ZOrder 414 + SrcBlock "jointPos" + SrcPort 1 + Points [81, 0] + Branch { + ZOrder 453 + Points [0, 265] + DstBlock "Compute angular momentum integral error" + DstPort 2 + } + Branch { + ZOrder 452 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 4 + } + } + Line { + ZOrder 415 + SrcBlock "jointPos_des" + SrcPort 1 + Points [101, 0] + Branch { + ZOrder 455 + Points [0, 210] + DstBlock "Compute angular momentum integral error" + DstPort 1 + } + Branch { + ZOrder 454 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 5 + } + } + Line { + ZOrder 416 + SrcBlock "nu" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 6 + } + Line { + ZOrder 417 + SrcBlock "M" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 7 + } + Line { + ZOrder 418 + SrcBlock "h" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 8 + } + Line { + ZOrder 419 + SrcBlock "L " + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 9 + } + Line { + ZOrder 420 + SrcBlock "Compute angular momentum integral error" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 10 + } + Line { + ZOrder 421 + SrcBlock "w_H_l_sole" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 11 + } + Line { + ZOrder 422 + SrcBlock "w_H_r_sole" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 12 + } + Line { + ZOrder 423 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 13 + } + Line { + ZOrder 424 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 14 + } + Line { + ZOrder 425 + SrcBlock "JDot_l_sole_nu" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 15 + } + Line { + ZOrder 426 + SrcBlock "JDot_r_sole_nu" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 16 + } + Line { + ZOrder 427 + SrcBlock "pos_CoM" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 17 + } + Line { + ZOrder 428 + SrcBlock "J_CoM" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 18 + } + Line { + ZOrder 429 + SrcBlock "desired_pos_vel_acc_CoM" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 19 + } + Line { + ZOrder 430 + SrcBlock "KP_CoM" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 20 + } + Line { + ZOrder 431 + SrcBlock "KD_CoM" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 21 + } + Line { + ZOrder 432 + SrcBlock "KP_postural" + SrcPort 1 + DstBlock "Momentum Based Balancing Controller\n" + DstPort 22 + } + Line { + ZOrder 433 + SrcBlock "Momentum Based Balancing Controller\n" + SrcPort 1 + DstBlock "Compute Desired Torques" + DstPort 2 + } + Line { + ZOrder 434 + SrcBlock "Momentum Based Balancing Controller\n" + SrcPort 2 + DstBlock "Compute Desired Torques" + DstPort 3 + } + Line { + ZOrder 435 + SrcBlock "Momentum Based Balancing Controller\n" + SrcPort 3 + DstBlock "Compute Desired Torques" + DstPort 4 + } + Line { + ZOrder 436 + SrcBlock "Momentum Based Balancing Controller\n" + SrcPort 4 + DstBlock "Compute Desired Torques" + DstPort 5 + } + Line { + ZOrder 437 + SrcBlock "Momentum Based Balancing Controller\n" + SrcPort 5 + DstBlock "Compute Desired Torques" + DstPort 6 + } + Line { + ZOrder 438 + SrcBlock "Momentum Based Balancing Controller\n" + SrcPort 6 + DstBlock "Compute Desired Torques" + DstPort 7 + } + Line { + ZOrder 439 + SrcBlock "Momentum Based Balancing Controller\n" + SrcPort 7 + DstBlock "Compute Desired Torques" + DstPort 8 + } + Line { + ZOrder 440 + SrcBlock "Momentum Based Balancing Controller\n" + SrcPort 8 + DstBlock "Compute Desired Torques" + DstPort 9 + } + Line { + ZOrder 441 + SrcBlock "Momentum Based Balancing Controller\n" + SrcPort 9 + DstBlock "Compute Desired Torques" + DstPort 10 + } + Line { + ZOrder 442 + SrcBlock "Momentum Based Balancing Controller\n" + SrcPort 10 + DstBlock "Compute Desired Torques" + DstPort 11 + } + Line { + ZOrder 443 + SrcBlock "Momentum Based Balancing Controller\n" + SrcPort 11 + DstBlock "Compute Desired Torques" + DstPort 12 + } + Line { + ZOrder 444 + SrcBlock "Momentum Based Balancing Controller\n" + SrcPort 12 + DstBlock "Compute Desired Torques" + DstPort 13 + } + Line { + ZOrder 449 + SrcBlock "state" + SrcPort 1 + DstBlock "Compute angular momentum integral error" + DstPort 4 + } + Line { + ZOrder 533 + SrcBlock "jointAcc" + SrcPort 1 + DstBlock "From tau_QP to Joint Torques (motor reflected inertia)" + DstPort 2 + } + } + } + Block { + BlockType SubSystem + Name "Dump and visualize" + SID "2275" + Ports [] + Position [305, -4, 425, 24] + ZOrder 972 + ForegroundColor "red" + BackgroundColor "yellow" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 110 + $ClassName "Simulink.Mask" + Display "disp('VISUALIZER')" + } + System { + Name "Dump and visualize" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "182" + SimulinkSubDomain "Simulink" + Block { + BlockType SubSystem + Name "Desired and Measured Forces" + SID "3241" + Ports [4, 0, 1] + Position [260, 313, 435, 387] + ZOrder 899 + ForegroundColor "red" + BackgroundColor "yellow" + RequestExecContextInheritance off + System { + Name "Desired and Measured Forces" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "156" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "3245" + Position [-580, 237, -550, 253] + ZOrder 648 + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "3251" + Position [-580, 337, -550, 353] + ZOrder 682 + Port "2" + } + Block { + BlockType Inport + Name "f_star" + SID "4510" + Position [-580, 438, -550, 452] + ZOrder 688 + Port "3" + } + Block { + BlockType Inport + Name "state" + SID "4785" + Position [-345, 68, -315, 82] + ZOrder 690 + Port "4" + Port { + PortNumber 1 + Name "state" + } + } + Block { + BlockType EnablePort + Name "Enable" + SID "3246" + Ports [] + Position [-338, -30, -318, -10] + ZOrder 649 + ShowName off + HideAutomaticName off + } + Block { + BlockType Constant + Name "Constant" + SID "2169" + Position [-425, 42, -235, 58] + ZOrder 679 + ShowName off + Value "StateMachine.wrench_thresholdContactOn" + Port { + PortNumber 1 + Name "thresholdContactOn" + } + } + Block { + BlockType Constant + Name "Constant2" + SID "3167" + Position [-425, 17, -235, 33] + ZOrder 681 + ShowName off + Value "StateMachine.wrench_thresholdContactOff" + Port { + PortNumber 1 + Name "thresholdContactOff" + } + } + Block { + BlockType SubSystem + Name "Demux Forces nd Moments" + SID "4806" + Ports [3, 8] + Position [-440, 191, -220, 494] + ZOrder 697 + RequestExecContextInheritance off + Port { + PortNumber 1 + Name "measured forces l_sole" + } + Port { + PortNumber 2 + Name "desired forces l_sole" + } + Port { + PortNumber 3 + Name "measured forces r_sole" + } + Port { + PortNumber 4 + Name "desired forces r_sole" + } + Port { + PortNumber 5 + Name "measured moments l_sole" + } + Port { + PortNumber 6 + Name "desired moments l_sole" + } + Port { + PortNumber 7 + Name "measured moments r_sole" + } + Port { + PortNumber 8 + Name "desired moments r_sole" + } + System { + Name "Demux Forces nd Moments" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "205" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "4807" + Position [-620, -98, -590, -82] + ZOrder 697 + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "4808" + Position [-620, 67, -590, 83] + ZOrder 698 + Port "2" + } + Block { + BlockType Inport + Name "f_star" + SID "4809" + Position [-200, -22, -170, -8] + ZOrder 699 + Port "3" + } + Block { + BlockType Demux + Name "Demux1" + SID "4801" + Ports [1, 2] + Position [-500, -159, -495, -26] + ZOrder 692 + ShowName off + Outputs "[3;3]" + } + Block { + BlockType Demux + Name "Demux2" + SID "4802" + Ports [1, 2] + Position [-90, -144, -85, 114] + ZOrder 693 + ShowName off + Outputs "[6;6]" + } + Block { + BlockType Demux + Name "Demux5" + SID "4803" + Ports [1, 2] + Position [-10, -127, -5, -38] + ZOrder 694 + ShowName off + Outputs "[3;3]" + } + Block { + BlockType Demux + Name "Demux6" + SID "4804" + Ports [1, 2] + Position [-10, 3, -5, 92] + ZOrder 695 + ShowName off + Outputs "[3;3]" + } + Block { + BlockType Demux + Name "Demux7" + SID "4805" + Ports [1, 2] + Position [-500, 6, -495, 139] + ZOrder 696 + ShowName off + Outputs "[3;3]" + } + Block { + BlockType Outport + Name "measured forces l_sole" + SID "4810" + Position [-365, -132, -335, -118] + ZOrder 700 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "des forces l_sole" + SID "4812" + Position [75, -112, 105, -98] + ZOrder 702 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "measured forces r_sole" + SID "4813" + Position [-365, 33, -335, 47] + ZOrder 703 + Port "3" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "des forces r_sole" + SID "4816" + Position [75, 18, 105, 32] + ZOrder 706 + Port "4" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "measured moments l_sole" + SID "4811" + Position [-365, -67, -335, -53] + ZOrder 701 + Port "5" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "des moments l_sole" + SID "4814" + Position [75, -67, 105, -53] + ZOrder 704 + Port "6" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "measured moments r_sole" + SID "4815" + Position [-365, 98, -335, 112] + ZOrder 705 + Port "7" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "des moments r_sole" + SID "4817" + Position [75, 63, 105, 77] + ZOrder 707 + Port "8" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "Demux6" + SrcPort 2 + DstBlock "des moments r_sole" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "Demux5" + SrcPort 2 + DstBlock "des moments l_sole" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "Demux1" + SrcPort 1 + DstBlock "measured forces l_sole" + DstPort 1 + } + Line { + ZOrder 4 + SrcBlock "Demux7" + SrcPort 1 + DstBlock "measured forces r_sole" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "Demux7" + SrcPort 2 + DstBlock "measured moments r_sole" + DstPort 1 + } + Line { + ZOrder 6 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock "Demux1" + DstPort 1 + } + Line { + ZOrder 7 + SrcBlock "Demux2" + SrcPort 1 + DstBlock "Demux5" + DstPort 1 + } + Line { + ZOrder 8 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock "Demux7" + DstPort 1 + } + Line { + ZOrder 9 + SrcBlock "Demux6" + SrcPort 1 + DstBlock "des forces r_sole" + DstPort 1 + } + Line { + ZOrder 10 + SrcBlock "Demux1" + SrcPort 2 + DstBlock "measured moments l_sole" + DstPort 1 + } + Line { + ZOrder 11 + SrcBlock "f_star" + SrcPort 1 + DstBlock "Demux2" + DstPort 1 + } + Line { + ZOrder 12 + SrcBlock "Demux2" + SrcPort 2 + DstBlock "Demux6" + DstPort 1 + } + Line { + ZOrder 13 + SrcBlock "Demux5" + SrcPort 1 + DstBlock "des forces l_sole" + DstPort 1 + } + } + } + Block { + BlockType Sum + Name "Sum" + SID "4818" + Ports [2, 1] + Position [-40, -10, -20, 10] + ZOrder 698 + ShowName off + Inputs "-+|" + Port { + PortNumber 1 + Name "error forces l_sole" + } + } + Block { + BlockType Sum + Name "Sum1" + SID "4821" + Ports [2, 1] + Position [-40, 175, -20, 195] + ZOrder 701 + ShowName off + Inputs "-+|" + Port { + PortNumber 1 + Name "error forces r_sole" + } + } + Block { + BlockType Sum + Name "Sum2" + SID "4824" + Ports [2, 1] + Position [-40, 420, -20, 440] + ZOrder 704 + ForegroundColor "red" + BackgroundColor "yellow" + ShowName off + HideAutomaticName off + Inputs "-+|" + Port { + PortNumber 1 + Name "error moments l_sole" + } + } + Block { + BlockType Sum + Name "Sum3" + SID "4825" + Ports [2, 1] + Position [-40, 600, -20, 620] + ZOrder 705 + ShowName off + Inputs "-+|" + Port { + PortNumber 1 + Name "error moments r_sole" + } + } + Block { + BlockType Scope + Name "left Foot forces" + SID "4800" + Ports [6] + Position [195, -61, 340, 86] + ZOrder 691 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".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','forces_l_sole_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDe" + "cimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struc" + "t('MinYLimReal','-176.20127','MaxYLimReal','526.26568','YLabelReal','','MinYLimMag',' 0.00000','MaxYLimMag','526.2" + "6568','LegendVisibility','Off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor'," + "[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0." + "623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71" + "7647058823529 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','Visi" + "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedCh" + "annelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-193.2491" + "5','MaxYLimReal','450.91468','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',t" + "rue,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6" + "86274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0." + "16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588" + "235294117647 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'),st" + "ruct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineS" + "tyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames" + "',{{[]}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-264.65056','MaxYLimReal','550.97623','YLabelReal" + "','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'A" + "xesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666" + "666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137" + "2549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0." + "650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidt" + "h',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" + "ible','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','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none" + "','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'," + "3),struct('MinYLimReal','90.00000','MaxYLimReal','110.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','L" + "egendVisibility','off','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',4),struct('MinYLimReal','40.00000','MaxYLi" + "mReal','60.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','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',5),struct('MinYLimReal','0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0" + "','MaxYLimMag','10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'A" + "xesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450" + "98039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098" + "039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Ti" + "tle','%','LinePropertiesCache',{{struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" + ",6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686" + "274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.41" + "1764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450980" + "3921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesC" + "ache',{{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','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','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',{{}},'NumLin" + "es',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','Display" + "LayoutDimensions',[6 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop'" + ",false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2" + "020a','Location',[135 169 3841 2159])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Scope + Name "left Foot moments" + SID "4823" + Ports [4] + Position [195, 339, 340, 486] + ZOrder 703 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".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','moments_l_sole_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingD" + "ecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{stru" + "ct('MinYLimReal','-18.14675','MaxYLimReal','18.61074','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','18.6107" + "4','LegendVisibility','Off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0." + "686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623" + "529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764" + "7058823529 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],'L" + "ineStyle','-','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')}},'UserDefinedChann" + "elNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-55.51989','" + "MaxYLimReal','35.88107','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',true,'" + "YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274" + "509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078" + "431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529" + "4117647 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','no" + "ne','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,'LineNames',{{[" + "]}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-58.09813','MaxYLimReal','48.08789','YLabelReal','','M" + "inYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColo" + "r',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.06666666666" + "66667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019" + "608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6509803" + "92156863],'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','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" + "ble','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),stru" + "ct('MinYLimReal','0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibilit" + "y','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6" + "86274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0." + "411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509" + "803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertie" + "sCache',{{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','-','Line" + "Width',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('Col" + "or',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumL" + "ines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4)},'DisplayPropertyDefaults',struct('YLabelReal','','Ax" + "esColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.06666" + "66666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372" + "549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6" + "50980392156863],'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','Visi" + "ble','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','-','Lin" + "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1" + "),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[4 1],'DisplayContentCache',[]),extmgr." + "Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('To" + "ols','Measurements',true,'Version','2018a')),'Version','2020a','Location',[135 169 3841 2159])" + NumInputPorts "4" + Floating off + } + Block { + BlockType Scope + Name "right Foot forces" + SID "4819" + Ports [6] + Position [195, 124, 340, 271] + ZOrder 699 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".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','forces_r_sole_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDe" + "cimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struc" + "t('MinYLimReal','-36.45598','MaxYLimReal','219.57258','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','219.572" + "58','LegendVisibility','Off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0" + ".686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.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',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-33.68004'," + "'MaxYLimReal','221.73551','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',true" + ",'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862" + "74509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160" + "78431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235" + "294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Col" + "or',[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'),struc" + "t('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{" + "{[]}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-113.33241','MaxYLimReal','33.5863','YLabelReal',''," + "'MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesCo" + "lor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.066666666" + "6666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.8313725490" + "19608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098" + "0392156863],'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','-','LineWid" + "th',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vi" + "sible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),st" + "ruct('MinYLimReal','90.00000','MaxYLimReal','110.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legend" + "Visibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098" + "03922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294117647" + "06 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529" + " 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Line" + "PropertiesCache',{{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','Visible','on'),st" + "ruct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{" + "{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','40.00000','MaxYLimReal" + "','60.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',true,'YGrid',true" + ",'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922]," + "'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0" + ".392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1" + ";1 0.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','Visibl" + "e','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','-','LineW" + "idth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowCo" + "ntent',true,'Placement',5),struct('MinYLimReal','0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','Ma" + "xYLimMag','10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTi" + "ckColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039" + "215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.074509803921" + "5686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title'," + "'%','LinePropertiesCache',{{struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)}," + "'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450" + "9803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647" + "05882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039215" + "69 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','Visibl" + "e','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',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayou" + "tDimensions',[6 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',fals" + "e,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2020a'" + ",'Location',[135 179 3841 2127])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Scope + Name "right Foot moments" + SID "4822" + Ports [4] + Position [195, 519, 340, 666] + ZOrder 702 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".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','moments_r_sole_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingD" + "ecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{stru" + "ct('MinYLimReal','-89.58974','MaxYLimReal','373.15892','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','373.15" + "892','LegendVisibility','Off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[" + "0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6" + "23529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717" + "647058823529 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','o" + "n'),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','Visib" + "le','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedCha" + "nnelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-90.08017'" + ",'MaxYLimReal','373.36109','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',tru" + "e,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686" + "274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16" + "078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823" + "5294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Co" + "lor',[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'),stru" + "ct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames'," + "{{[]}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-23.18842','MaxYLimReal','45.48642','YLabelReal',''" + ",'MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesC" + "olor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.06666666" + "66666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549" + "019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6509" + "80392156863],'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','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),s" + "truct('MinYLimReal','-39.55039','MaxYLimReal','20.45784','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legen" + "dVisibility','off','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',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4)},'DisplayPropertyDefaults',struct('YLabelRe" + "al','','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.39215686274509" + "8 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039" + "215686 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','n" + "one','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,'Mark" + "er','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Pl" + "acement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[4 1],'DisplayContentCache',{" + "struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Ma" + "rker','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],'L" + "ineStyle','-','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','%','LinePropertiesCach" + "e',{{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','Visi" + "ble','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',{{}},'PlotMagPh" + "ase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','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','no" + "ne','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePr" + "opertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" + "lor',[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'),stru" + "ct('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,'Marke" + "r','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" + "truct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'Line" + "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5," + "'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Too" + "ls','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements" + "',true,'Version','2018a')),'Version','2020a','Location',[401 474 1711 1060])" + NumInputPorts "4" + Floating off + } + Line { + ZOrder 1 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock "Demux Forces nd Moments" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock "Demux Forces nd Moments" + DstPort 2 + } + Line { + ZOrder 3 + SrcBlock "f_star" + SrcPort 1 + DstBlock "Demux Forces nd Moments" + DstPort 3 + } + Line { + Name "measured forces l_sole" + ZOrder 4 + SrcBlock "Demux Forces nd Moments" + SrcPort 1 + Points [43, 0; 0, -270; 142, 0] + Branch { + ZOrder 5 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 1 + } + Branch { + ZOrder 6 + DstBlock "Sum" + DstPort 1 + } + } + Line { + Name "desired forces l_sole" + ZOrder 7 + SrcBlock "Demux Forces nd Moments" + SrcPort 2 + Points [66, 0; 0, -255] + Branch { + ZOrder 8 + DstBlock "Sum" + DstPort 2 + } + Branch { + ZOrder 9 + Labels [-1, 0] + Points [0, -25] + DstBlock "left Foot forces" + DstPort 2 + } + } + Line { + Name "error forces l_sole" + ZOrder 10 + Labels [-1, 0] + SrcBlock "Sum" + SrcPort 1 + DstBlock "left Foot forces" + DstPort 3 + } + Line { + Name "thresholdContactOn" + ZOrder 11 + SrcBlock "Constant" + SrcPort 1 + Points [259, 0] + Branch { + ZOrder 12 + Labels [-1, 0] + Points [0, 185] + DstBlock "right Foot forces" + DstPort 5 + } + Branch { + ZOrder 13 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 5 + } + } + Line { + Name "desired forces r_sole" + ZOrder 14 + SrcBlock "Demux Forces nd Moments" + SrcPort 4 + Points [116, 0; 0, -140] + Branch { + ZOrder 15 + DstBlock "Sum1" + DstPort 2 + } + Branch { + ZOrder 16 + Labels [-1, 0] + Points [0, -25] + DstBlock "right Foot forces" + DstPort 2 + } + } + Line { + Name "measured forces r_sole" + ZOrder 17 + SrcBlock "Demux Forces nd Moments" + SrcPort 3 + Points [90, 0; 0, -155; 95, 0] + Branch { + ZOrder 18 + Labels [-1, 0] + DstBlock "right Foot forces" + DstPort 1 + } + Branch { + ZOrder 19 + DstBlock "Sum1" + DstPort 1 + } + } + Line { + Name "error forces r_sole" + ZOrder 20 + Labels [-1, 0] + SrcBlock "Sum1" + SrcPort 1 + DstBlock "right Foot forces" + DstPort 3 + } + Line { + Name "thresholdContactOff" + ZOrder 21 + SrcBlock "Constant2" + SrcPort 1 + Points [280, 0] + Branch { + ZOrder 22 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 4 + } + Branch { + ZOrder 23 + Labels [-1, 0] + Points [0, 185] + DstBlock "right Foot forces" + DstPort 4 + } + } + Line { + Name "measured moments r_sole" + ZOrder 24 + SrcBlock "Demux Forces nd Moments" + SrcPort 7 + Points [68, 0; 0, 110; 117, 0] + Branch { + ZOrder 25 + DstBlock "Sum3" + DstPort 1 + } + Branch { + ZOrder 26 + Labels [1, 0] + DstBlock "right Foot moments" + DstPort 1 + } + } + Line { + Name "state" + ZOrder 27 + SrcBlock "state" + SrcPort 1 + Points [321, 0] + Branch { + ZOrder 28 + Points [0, 185] + Branch { + ZOrder 29 + Points [0, 205; 1, 0] + Branch { + ZOrder 30 + Labels [-1, 0] + Points [0, 180] + DstBlock "right Foot moments" + DstPort 4 + } + Branch { + ZOrder 31 + DstBlock "left Foot moments" + DstPort 4 + } + } + Branch { + ZOrder 32 + Labels [-1, 0] + DstBlock "right Foot forces" + DstPort 6 + } + } + Branch { + ZOrder 33 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 6 + } + } + Line { + Name "desired moments r_sole" + ZOrder 34 + SrcBlock "Demux Forces nd Moments" + SrcPort 8 + Points [40, 0; 0, 110] + Branch { + ZOrder 35 + Labels [-1, 0] + DstBlock "right Foot moments" + DstPort 2 + } + Branch { + ZOrder 36 + Points [0, 35] + DstBlock "Sum3" + DstPort 2 + } + } + Line { + Name "error moments r_sole" + ZOrder 37 + Labels [-1, 0] + SrcBlock "Sum3" + SrcPort 1 + DstBlock "right Foot moments" + DstPort 3 + } + Line { + Name "error moments l_sole" + ZOrder 38 + Labels [-1, 0] + SrcBlock "Sum2" + SrcPort 1 + DstBlock "left Foot moments" + DstPort 3 + } + Line { + Name "desired moments l_sole" + ZOrder 39 + SrcBlock "Demux Forces nd Moments" + SrcPort 6 + Points [121, 0] + Branch { + ZOrder 40 + Points [0, 35] + DstBlock "Sum2" + DstPort 2 + } + Branch { + ZOrder 41 + Labels [-1, 0] + DstBlock "left Foot moments" + DstPort 2 + } + } + Line { + Name "measured moments l_sole" + ZOrder 42 + SrcBlock "Demux Forces nd Moments" + SrcPort 5 + Points [185, 0] + Branch { + ZOrder 43 + DstBlock "Sum2" + DstPort 1 + } + Branch { + ZOrder 44 + Labels [-1, 0] + DstBlock "left Foot moments" + DstPort 1 + } + } + } + } + Block { + BlockType SubSystem + Name "Feet Status and Gains" + SID "4104" + Ports [3, 0, 1] + Position [255, 10, 440, 90] + ZOrder 903 + RequestExecContextInheritance off + System { + Name "Feet Status and Gains" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "309" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "feetContactStatus" + SID "4106" + Position [5, -71, 35, -59] + ZOrder 646 + } + Block { + BlockType Inport + Name "KP_postural_diag" + SID "4107" + Position [5, 127, 35, 143] + ZOrder 647 + Port "2" + } + Block { + BlockType Inport + Name "state" + SID "4108" + Position [5, -6, 35, 6] + ZOrder 648 + Port "3" + } + Block { + BlockType EnablePort + Name "Enable" + SID "4110" + Ports [] + Position [392, -150, 412, -130] + ZOrder 649 + ShowName off + HideAutomaticName off + } + Block { + BlockType Demux + Name "Demux1" + SID "4569" + Ports [1, 5] + Position [195, 86, 200, 184] + ZOrder 661 + ShowName off + Outputs "Config.numOfJointsForEachControlboard" + Port { + PortNumber 1 + Name "torso" + } + Port { + PortNumber 2 + Name "left_arm" + } + Port { + PortNumber 3 + Name "right_arm" + } + Port { + PortNumber 4 + Name "left_leg" + } + Port { + PortNumber 5 + Name "right_leg" + } + } + Block { + BlockType Scope + Name "Feet Contact Activation" + SID "4117" + Ports [2] + Position [345, -97, 465, 32] + ZOrder 644 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".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','feetContactStatus_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggi" + "ngDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{s" + "truct('MinYLimReal','0.9','MaxYLimReal','1.1','YLabelReal','','MinYLimMag','0.9','MaxYLimMag','1.1','LegendVisibil" + "ity','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.0745098039215686;0.717647058823529 0.27450" + "9803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperti" + "esCache',{{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','-','Lin" + "eWidth',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('Co" + "lor',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'Num" + "Lines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','0.9','MaxYLimReal','1.1','YLab" + "elReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',fal" + "se,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0" + ".0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0." + "831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07450980392156" + "86 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[0 1 1],'LineStyle','-','Lin" + "eWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'Show" + "Content',true,'Placement',2)},'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.0745098039215686;0.7" + "17647058823529 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','-','LineWidt" + "h',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" + "ible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedC" + "hannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[2 1" + "],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutosc" + "ale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2020a','Location',[995 " + "541 1565 915])" + NumInputPorts "2" + Floating off + } + Block { + BlockType Scope + Name "Gain Scheduling Postural" + SID "4116" + Ports [6] + Position [345, 79, 465, 211] + ZOrder 639 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".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','KP_postural_diag_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggin" + "gDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{st" + "ruct('MinYLimReal','8.75','MaxYLimReal','21.25','YLabelReal','','MinYLimMag','8.75','MaxYLimMag','21.25','LegendVi" + "sibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098039" + "22 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 " + "1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0." + "274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePro" + "pertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" + "or',[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'),struc" + "t('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}}" + ",'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','7.75','MaxYLimReal','10.2" + "5','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPh" + "ase',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.3921568627" + "45098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509" + "8039215686 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'),st" + "ruct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineS" + "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'" + "Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true" + ",'Placement',2),struct('MinYLimReal','7.75','MaxYLimReal','10.25','YLabelReal','','MinYLimMag','0','MaxYLimMag','1" + "0','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6" + "86274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235" + "29411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647" + "058823529 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],'Li" + "neStyle','-','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')}},'UserDefinedChanne" + "lNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','3.75','MaxYLi" + "mReal','66.25','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',tru" + "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922]" + ",'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;" + "0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 " + "1;1 0.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','Visib" + "le','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','-','Line" + "Width',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowC" + "ontent',true,'Placement',4),struct('MinYLimReal','-1.00000','MaxYLimReal','1.00000','YLabelReal','','MinYLimMag','" + "0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'A" + "xesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450" + "98039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098" + "039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Ti" + "tle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','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',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimRe" + "al','1.00000','MaxYLimReal','3.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','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',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[" + "0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.066666666666666" + "7;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 " + "0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215" + "6863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','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],'Li" + "neStyle','-','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',{{[]}},'ShowContent',true,'Placement',1),'Display" + "LayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),extmgr.Configurat" + "ion('Tools','Measurements',true,'Version','2018a')),'Version','2020a','Location',[898 828 2209 1517])" + NumInputPorts "6" + Floating off + } + Line { + ZOrder 1 + SrcBlock "state" + SrcPort 1 + Points [85, 0] + Branch { + ZOrder 2 + DstBlock "Feet Contact Activation" + DstPort 2 + } + Branch { + ZOrder 3 + Points [0, 195] + DstBlock "Gain Scheduling Postural" + DstPort 6 + } + } + Line { + ZOrder 4 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock "Feet Contact Activation" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "KP_postural_diag" + SrcPort 1 + DstBlock "Demux1" + DstPort 1 + } + Line { + Name "left_leg" + ZOrder 6 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 4 + DstBlock "Gain Scheduling Postural" + DstPort 4 + } + Line { + Name "right_leg" + ZOrder 7 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 5 + DstBlock "Gain Scheduling Postural" + DstPort 5 + } + Line { + Name "right_arm" + ZOrder 8 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 3 + DstBlock "Gain Scheduling Postural" + DstPort 3 + } + Line { + Name "torso" + ZOrder 9 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 1 + DstBlock "Gain Scheduling Postural" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 10 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 2 + DstBlock "Gain Scheduling Postural" + DstPort 2 + } + } + } + Block { + BlockType From + Name "From" + SID "4725" + Position [490, 275, 585, 295] + ZOrder 898 + ShowName off + HideAutomaticName off + GotoTag "tau_star_afterSat" + TagVisibility "global" + } + Block { + BlockType From + Name "From1" + SID "4479" + Position [80, 198, 155, 212] + ZOrder 709 + ShowName off + HideAutomaticName off + GotoTag "state" + TagVisibility "global" + } + Block { + BlockType From + Name "From10" + SID "4793" + Position [45, 331, 140, 349] + ZOrder 945 + ShowName off + GotoTag "wrench_rightFoot" + TagVisibility "global" + } + Block { + BlockType From + Name "From11" + SID "4791" + Position [45, 311, 140, 329] + ZOrder 943 + ShowName off + GotoTag "wrench_leftFoot" + TagVisibility "global" + } + Block { + BlockType From + Name "From12" + SID "4795" + Position [500, 196, 580, 214] + ZOrder 947 + ShowName off + GotoTag "pos_CoM_des" + TagVisibility "global" + } + Block { + BlockType From + Name "From13" + SID "4798" + Position [515, 317, 565, 333] + ZOrder 950 + ShowName off + HideAutomaticName off + GotoTag "nu" + TagVisibility "global" + } + Block { + BlockType From + Name "From14" + SID "4828" + Position [510, 357, 565, 373] + ZOrder 953 + ShowName off + HideAutomaticName off + GotoTag "jointPos" + TagVisibility "global" + } + Block { + BlockType From + Name "From2" + SID "4720" + Position [60, 353, 120, 367] + ZOrder 864 + ShowName off + GotoTag "f_star" + TagVisibility "global" + } + Block { + BlockType From + Name "From3" + SID "4796" + Position [500, 156, 580, 174] + ZOrder 948 + ShowName off + GotoTag "pos_CoM" + TagVisibility "global" + } + Block { + BlockType From + Name "From4" + SID "4786" + Position [65, 16, 165, 34] + ZOrder 938 + ShowName off + HideAutomaticName off + GotoTag "feetContactStatus" + TagVisibility "global" + } + Block { + BlockType From + Name "From5" + SID "4721" + Position [505, 36, 580, 54] + ZOrder 865 + ShowName off + GotoTag "tau_star" + TagVisibility "global" + } + Block { + BlockType From + Name "From6" + SID "4781" + Position [500, 115, 580, 135] + ZOrder 937 + ShowName off + GotoTag "jointPos_des" + TagVisibility "global" + } + Block { + BlockType From + Name "From7" + SID "4485" + Position [505, 76, 580, 94] + ZOrder 715 + ShowName off + HideAutomaticName off + GotoTag "w_H_b" + TagVisibility "global" + } + Block { + BlockType From + Name "From8" + SID "4787" + Position [65, 41, 170, 59] + ZOrder 939 + ShowName off + HideAutomaticName off + GotoTag "KP_postural_diag" + TagVisibility "global" + } + Block { + BlockType From + Name "From9" + SID "4709" + Position [80, 166, 155, 184] + ZOrder 721 + ShowName off + HideAutomaticName off + GotoTag "QPStatus" + TagVisibility "global" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n1" + SID "3269" + Position [30, 279, 180, 291] + ZOrder 902 + ShowName off + Value "Config.SCOPES_WRENCHES" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n2" + SID "4593" + Position [60, 124, 175, 136] + ZOrder 720 + ShowName off + Value "Config.SCOPES_QP" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n4" + SID "3275" + Position [485, -26, 600, -14] + ZOrder 706 + ShowName off + Value "Config.SCOPES_MAIN" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n6" + SID "4103" + Position [25, -26, 205, -14] + ZOrder 906 + ShowName off + Value "Config.SCOPES_GAIN_SCHE_INFO" + } + Block { + BlockType SubSystem + Name "Visualize eventual QP failures" + SID "4620" + Ports [2, 0, 1] + Position [290, 161, 405, 219] + ZOrder 717 + RequestExecContextInheritance off + System { + Name "Visualize eventual QP failures" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "1000" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "QPStatus" + SID "4621" + Position [100, 164, 125, 176] + ZOrder 409 + } + Block { + BlockType Inport + Name "state" + SID "4622" + Position [100, 198, 125, 212] + ZOrder 554 + Port "2" + } + Block { + BlockType EnablePort + Name "Enable" + SID "4623" + Ports [] + Position [272, 175, 292, 195] + ZOrder 540 + ShowName off + HideAutomaticName off + } + Block { + BlockType Scope + Name "QP status\n(0: no failure)" + SID "4624" + Ports [2] + Position [195, 153, 240, 222] + ZOrder 408 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".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','QPStatus_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimat" + "eData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('Mi" + "nYLimReal','-2.25','MaxYLimReal','0.25','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','2.25','LegendVisibili" + "ty','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6" + "86274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0." + "411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509" + "803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertie" + "sCache',{{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','-','Line" + "Width',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('Col" + "or',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumL" + "ines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','0.25','MaxYLimReal','7.75','YLa" + "belReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',fa" + "lse,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 " + "0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0" + ".831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215" + "686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','Li" + "neWidth',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('C" + "olor',[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',{{[]}},'ShowContent',true,'Place" + "ment',2)},'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.0745098039215686;0.717647058823529 0.274" + "509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProper" + "tiesCache',{{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','-','L" + "ineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','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',{{}},'N" + "umLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[2 1]),extmgr.Configurat" + "ion('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Meas" + "urements',true,'Version','2018a')),'Version','2020a','Location',[313 476 1506 1203])" + NumInputPorts "2" + Floating off + } + Line { + ZOrder 1 + SrcBlock "QPStatus" + SrcPort 1 + DstBlock "QP status\n(0: no failure)" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "state" + SrcPort 1 + DstBlock "QP status\n(0: no failure)" + DstPort 2 + } + } + } + Block { + BlockType SubSystem + Name "Visualizer" + SID "4504" + Ports [9, 0, 1] + Position [635, 25, 775, 385] + ZOrder 707 + RequestExecContextInheritance off + System { + Name "Visualizer" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "tau_star" + SID "4505" + Position [65, -192, 95, -178] + ZOrder 591 + } + Block { + BlockType Inport + Name "w_H_b" + SID "4506" + Position [740, -457, 770, -443] + ZOrder 592 + Port "2" + } + Block { + BlockType Inport + Name "jointPos_des" + SID "4508" + Position [740, -157, 770, -143] + ZOrder 594 + Port "3" + } + Block { + BlockType Inport + Name "pos_CoM" + SID "4509" + Position [65, 253, 95, 267] + ZOrder 595 + Port "4" + Port { + PortNumber 1 + Name "CoM_Measured" + } + } + Block { + BlockType Inport + Name "pos_CoM_des" + SID "4511" + Position [65, 288, 95, 302] + ZOrder 597 + Port "5" + Port { + PortNumber 1 + Name "CoM_Desired" + } + } + Block { + BlockType Inport + Name "state" + SID "4507" + Position [65, -277, 95, -263] + ZOrder 593 + Port "6" + Port { + PortNumber 1 + Name "state" + PropagatedSignals "state" + } + } + Block { + BlockType Inport + Name "tau_star_afterSat" + SID "4562" + Position [65, -337, 95, -323] + ZOrder 873 + Port "7" + } + Block { + BlockType Inport + Name "nu" + SID "4109" + Position [740, 192, 770, 208] + ZOrder 887 + Port "8" + } + Block { + BlockType Inport + Name "jointPos" + SID "4826" + Position [740, -307, 770, -293] + ZOrder 895 + Port "9" + } + Block { + BlockType EnablePort + Name "Enable" + SID "4513" + Ports [] + Position [72, 40, 92, 60] + ZOrder 599 + ShowName off + HideAutomaticName off + } + Block { + BlockType Scope + Name "Base Pose" + SID "3704" + Ports [3] + Position [1190, -508, 1280, -392] + ZOrder 584 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" + "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," + "'DataLoggingVariableName','base_pose_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1'" + ",'DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('M" + "inYLimReal','-0.16773','MaxYLimReal','0.72406','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','0.72406','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',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-1.25','MaxYLimReal'," + "'1.25','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotM" + "agPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorO" + "rder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156" + "862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07" + "45098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Ma" + "rker','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],'L" + "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0" + ".5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent'," + "true,'Placement',2),struct('MinYLimReal','0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMa" + "g','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.0745098039215686;0.7" + "17647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',t" + "rue,'Placement',3)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274" + "509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411" + "764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882" + "3529 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'),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',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','400','TimeRangeFrame" + "s','400','DisplayLayoutDimensions',[3 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigation'," + "true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2018" + "a')),'Version','2020a','Location',[630 446 1920 1048])" + NumInputPorts "3" + Floating off + } + Block { + BlockType Scope + Name "Base linear and angular velocity" + SID "4121" + Ports [3] + Position [1195, 319, 1280, 441] + ZOrder 891 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" + "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," + "'DataLoggingVariableName','baseVel_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','" + "DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('Min" + "YLimReal','-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility'" + ",'On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862" + "74509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411" + "764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803" + "921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCa" + "che',{{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','-','LineWid" + "th',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vi" + "sible','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',{{}},'NumLine" + "s',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.04416','MaxYLimReal','0.05054'," + "'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.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3921568627450" + "98 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509803" + "9215686 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'),struc" + "t('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'P" + "lacement',2),struct('MinYLimReal','-0.04416','MaxYLimReal','0.05054','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.6" + "23529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717" + "647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','LinePr" + "opertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" + "lor',[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'),stru" + "ct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}" + "},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3)},'DisplayPropertyDefaults',struct('MinYLimReal" + "','-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','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),'DisplayLayoutDimensi" + "ons',[3 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tools','" + "Measurements',true,'Version','2018a')),'Version','2020a','Location',[135 478 2065 1412])" + NumInputPorts "3" + Floating off + } + Block { + BlockType Scope + Name "CoM Position" + SID "2286" + Ports [4] + Position [540, 245, 645, 380] + ZOrder 255 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" + "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," + "'DataLoggingVariableName','pos_CoM_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','" + "DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('Min" + "YLimReal','-0.15575','MaxYLimReal','0.70056','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','0.70056','Legend" + "Visibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980" + "3922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941176470" + "6 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 " + "0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineP" + "ropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" + "olor',[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'),str" + "uct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{" + "}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.14783','MaxYLimReal'" + ",'0.63035','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'P" + "lotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Co" + "lorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.39" + "2156862745098 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],'Lin" + "eStyle','-','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','-','LineWidt" + "h',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowConte" + "nt',true,'Placement',2),struct('MinYLimReal','-0.08636','MaxYLimReal','0.08883','YLabelReal','','MinYLimMag','0','" + "MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'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',3),struct('MinYLimReal'," + "'0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',t" + "rue,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6" + "86274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0." + "16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588" + "235294117647 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'),st" + "ruct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineS" + "tyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames" + "',{{[]}},'ShowContent',true,'Placement',4)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'" + "AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745" + "098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.074509" + "8039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'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',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSample" + "s','400','TimeRangeFrames','400','DisplayLayoutDimensions',[4 1]),extmgr.Configuration('Tools','Plot Navigation',t" + "rue,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2018a" + "')),'Version','2020a','Location',[135 239 2650 1508])" + NumInputPorts "4" + Floating off + } + Block { + BlockType Selector + Name "CoM6D -> \nCoMXYZ1" + SID "3703" + Ports [1, 1] + Position [915, -462, 955, -438] + ZOrder 583 + ShowName off + NumberOfDimensions "2" + InputPortWidth "7" + IndexOptions "Index vector (dialog),Index vector (dialog)" + Indices "[1 2 3],[1 2 3]" + OutputSizes "1,1" + Port { + PortNumber 1 + Name "baseOrientation" + } + } + Block { + BlockType Selector + Name "CoM6D -> \nCoMXYZ2" + SID "3699" + Ports [1, 1] + Position [915, -502, 955, -478] + ZOrder 581 + ShowName off + NumberOfDimensions "2" + InputPortWidth "7" + IndexOptions "Index vector (dialog),Index vector (dialog)" + Indices "[1 2 3],[4]" + OutputSizes "1,1" + Port { + PortNumber 1 + Name "basePosition" + } + } + Block { + BlockType Demux + Name "Demux" + SID "4111" + Ports [1, 2] + Position [945, 320, 950, 400] + ZOrder 890 + ShowName off + Outputs "2" + Port { + PortNumber 1 + Name "base linear velocity" + } + } + Block { + BlockType Demux + Name "Demux1" + SID "4571" + Ports [1, 5] + Position [1040, -349, 1045, -251] + ZOrder 878 + ShowName off + Outputs "Config.numOfJointsForEachControlboard" + Port { + PortNumber 1 + Name "torso" + } + Port { + PortNumber 2 + Name "left_arm" + } + Port { + PortNumber 3 + Name "right_arm" + } + Port { + PortNumber 4 + Name "left_leg" + } + Port { + PortNumber 5 + Name "right_leg" + } + } + Block { + BlockType Demux + Name "Demux10" + SID "4578" + Ports [1, 5] + Position [445, -84, 450, 14] + ZOrder 885 + ShowName off + Outputs "Config.numOfJointsForEachControlboard" + Port { + PortNumber 1 + Name "torso" + } + Port { + PortNumber 2 + Name "left_arm" + } + Port { + PortNumber 3 + Name "right_arm" + } + Port { + PortNumber 4 + Name "left_leg" + } + Port { + PortNumber 5 + Name "right_leg" + } + } + Block { + BlockType Demux + Name "Demux2" + SID "4572" + Ports [1, 5] + Position [1040, -199, 1045, -101] + ZOrder 879 + ShowName off + Outputs "Config.numOfJointsForEachControlboard" + Port { + PortNumber 1 + Name "torso" + } + Port { + PortNumber 2 + Name "left_arm" + } + Port { + PortNumber 3 + Name "right_arm" + } + Port { + PortNumber 4 + Name "left_leg" + } + Port { + PortNumber 5 + Name "right_leg" + } + } + Block { + BlockType Demux + Name "Demux3" + SID "4570" + Ports [1, 5] + Position [1040, 151, 1045, 249] + ZOrder 894 + ShowName off + Outputs "Config.numOfJointsForEachControlboard" + Port { + PortNumber 1 + Name "torso" + } + Port { + PortNumber 2 + Name "left_arm" + } + Port { + PortNumber 3 + Name "right_arm" + } + Port { + PortNumber 4 + Name "left_leg" + } + Port { + PortNumber 5 + Name "right_leg" + } + } + Block { + BlockType Demux + Name "Demux5" + SID "4573" + Ports [1, 5] + Position [1040, -39, 1045, 59] + ZOrder 880 + ShowName off + Outputs "Config.numOfJointsForEachControlboard" + Port { + PortNumber 1 + Name "torso" + } + Port { + PortNumber 2 + Name "left_arm" + } + Port { + PortNumber 3 + Name "right_arm" + } + Port { + PortNumber 4 + Name "left_leg" + } + Port { + PortNumber 5 + Name "right_leg" + } + } + Block { + BlockType Demux + Name "Demux7" + SID "4575" + Ports [1, 5] + Position [445, -379, 450, -281] + ZOrder 882 + ShowName off + Outputs "Config.numOfJointsForEachControlboard" + Port { + PortNumber 1 + Name "torso" + } + Port { + PortNumber 2 + Name "left_arm" + } + Port { + PortNumber 3 + Name "right_arm" + } + Port { + PortNumber 4 + Name "left_leg" + } + Port { + PortNumber 5 + Name "right_leg" + } + } + Block { + BlockType Demux + Name "Demux8" + SID "4576" + Ports [1, 5] + Position [445, -234, 450, -136] + ZOrder 883 + ShowName off + Outputs "Config.numOfJointsForEachControlboard" + Port { + PortNumber 1 + Name "torso" + } + Port { + PortNumber 2 + Name "left_arm" + } + Port { + PortNumber 3 + Name "right_arm" + } + Port { + PortNumber 4 + Name "left_leg" + } + Port { + PortNumber 5 + Name "right_leg" + } + } + Block { + BlockType Demux + Name "Demux9" + SID "4577" + Ports [1, 5] + Position [445, 71, 450, 169] + ZOrder 884 + ShowName off + Outputs "Config.numOfJointsForEachControlboard" + Port { + PortNumber 1 + Name "torso" + } + Port { + PortNumber 2 + Name "left_arm" + } + Port { + PortNumber 3 + Name "right_arm" + } + Port { + PortNumber 4 + Name "left_leg" + } + Port { + PortNumber 5 + Name "right_leg" + } + } + Block { + BlockType Scope + Name "Desired Torques" + SID "2336" + Ports [6] + Position [540, -230, 645, -120] + ZOrder 491 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" + "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," + "'DataLoggingVariableName','tauDesired_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1" + "','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('" + "MinYLimReal','-19.59759','MaxYLimReal','27.79759','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.79759','" + "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',1),struct('MinYLimReal','-7.66225','MaxYLi" + "mReal','8.03648','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',0,'LineNames',{{[]}},'Sho" + "wContent',true,'Placement',2),struct('MinYLimReal','-11.21869','MaxYLimReal','8.29753','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.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',3),struct('MinYLi" + "mReal','-51.65238','MaxYLimReal','50.72666','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility'," + "'on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627" + "4509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117" + "64705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039" + "21569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCac" + "he',{{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','-','LineWidt" + "h',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" + "ible','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',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-5.64312','MaxYLimReal','14.99269'," + "'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.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3921568627450" + "98 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509803" + "9215686 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'),struc" + "t('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'P" + "lacement',5),struct('MinYLimReal','-0.5','MaxYLimReal','14.5','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','','LinePropertie" + "sCache',{{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','-','Line" + "Width',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('Col" + "or',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumL" + "ines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','Ax" + "esColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.06666" + "66666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372" + "549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6" + "50980392156863],'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','Visi" + "ble','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','-','Lin" + "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1" + "),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Pl" + "ot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true," + "'Version','2018a')),'Version','2020a','Location',[811 296 1749 1223])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Scope + Name "Desired Torques After Saturation" + SID "4561" + Ports [6] + Position [540, -375, 645, -265] + ZOrder 871 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" + "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," + "'DataLoggingVariableName','tau_star_afterSat_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimat" + "ion','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{s" + "truct('MinYLimReal','-14.69019','MaxYLimReal','22.24602','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','22.2" + "4602','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.6" + "23529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717" + "647058823529 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','o" + "n'),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','Visib" + "le','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedCha" + "nnelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-10.12731'" + ",'MaxYLimReal','6.52875','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'" + "YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274" + "509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078" + "431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529" + "4117647 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','no" + "ne','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,'LineNames',{{[" + "]}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-14.66185','MaxYLimReal','8.30827','YLabelReal','','Mi" + "nYLimMag','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.0666666666666" + "667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137254901960" + "8 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392" + "156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'M" + "arker','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','Visibl" + "e','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct" + "('MinYLimReal','-23.11356','MaxYLimReal','33.28118','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisi" + "bility','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.0745098039215686;0.717647058823529 0.27" + "4509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePrope" + "rtiesCache',{{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','no" + "ne','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',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-30.29324','MaxYLimReal','2" + "7.04418','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'Plo" + "tMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Colo" + "rOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3921" + "56862745098 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],'LineS" + "tyle','-','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','o" + "n'),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',{{[]}},'ShowContent" + "',true,'Placement',5),struct('MinYLimReal','0.25','MaxYLimReal','7.75','YLabelReal','','MinYLimMag','0','MaxYLimMa" + "g','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.0745098039215686;0.7" + "17647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','Line" + "PropertiesCache',{{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','Visible','on'),st" + "ruct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{" + "{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelRea" + "l','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 " + "1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098" + " 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07450980392" + "15686 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','no" + "ne','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,'Marke" + "r','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Pla" + "cement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('T" + "ools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measuremen" + "ts',true,'Version','2018a')),'Version','2020a','Location',[701 359 2556 1241])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Gain + Name "Gain" + SID "2289" + Position [910, -318, 960, -282] + ZOrder 309 + ShowName off + Gain "180/pi" + } + Block { + BlockType Gain + Name "Gain1" + SID "4114" + Position [945, 187, 995, 213] + ZOrder 892 + ShowName off + Gain "180/pi" + } + Block { + BlockType Gain + Name "Gain2" + SID "4115" + Position [985, 366, 1030, 394] + ZOrder 893 + ShowName off + Gain "180/pi" + Port { + PortNumber 1 + Name "base angular velocity" + } + } + Block { + BlockType Gain + Name "Gain3" + SID "2290" + Position [910, -168, 960, -132] + ZOrder 500 + ShowName off + Gain "180/pi" + } + Block { + BlockType Gain + Name "Gain4" + SID "2291" + Position [910, -8, 960, 28] + ZOrder 503 + ShowName off + Gain "180/pi" + } + Block { + BlockType Scope + Name "Joint Position Desired" + SID "2334" + Ports [6] + Position [1195, -198, 1280, -82] + ZOrder 501 + HideAutomaticName off + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" + "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," + "'DataLoggingVariableName','jointPos_Desired_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimati" + "on','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{st" + "ruct('MinYLimReal','-8.36237','MaxYLimReal','29.68131','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','29.681" + "31','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.623" + "529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764" + "7058823529 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],'L" + "ineStyle','-','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')}},'UserDefinedChann" + "elNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-81.97965','" + "MaxYLimReal','72.42484','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',0,'LineNames',{{[]" + "}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-78.54318','MaxYLimReal','93.23402','YLabelReal','','Mi" + "nYLimMag','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.0666666666666" + "667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137254901960" + "8 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392" + "156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'M" + "arker','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','Visibl" + "e','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct" + "('MinYLimReal','-12.71731','MaxYLimReal','14.72789','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisi" + "bility','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.0745098039215686;0.717647058823529 0.27" + "4509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePrope" + "rtiesCache',{{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','no" + "ne','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',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-113.68313','MaxYLimReal','" + "94.48842','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'Pl" + "otMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Col" + "orOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392" + "156862745098 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],'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',{{}},'NumLines',0,'LineNames',{{[]}},'ShowConten" + "t',true,'Placement',5),struct('MinYLimReal','0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLi" + "mMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickCol" + "or',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803921568" + "6 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;" + "0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','L" + "inePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" + "t('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','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',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabel" + "Real','','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.392156862745" + "098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07450980" + "39215686 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'),stru" + "ct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Ma" + "rker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'" + "Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr.Configuration" + "('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measure" + "ments',true,'Version','2018a')),'Version','2020a','Location',[135 204 1853 1049])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Scope + Name "Joint Position Measured" + SID "2333" + Ports [6] + Position [1195, -348, 1280, -232] + ZOrder 312 + HideAutomaticName off + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" + "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," + "'DataLoggingVariableName','jointPos_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1'," + "'DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('Mi" + "nYLimReal','-2.41385','MaxYLimReal','5.58471','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','5.58471','Legen" + "dVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098" + "03922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294117647" + "06 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529" + " 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Line" + "PropertiesCache',{{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','Visible','on'),st" + "ruct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{" + "{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-39.61854','MaxYLimRea" + "l','58.9684','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.623529411764706 1;1 0.411764705882353 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],'L" + "ineStyle','-','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','-','LineWi" + "dth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowCon" + "tent',true,'Placement',2),struct('MinYLimReal','-39.58369','MaxYLimReal','58.65801','YLabelReal','','MinYLimMag','" + "0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'A" + "xesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450" + "98039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098" + "039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Ti" + "tle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','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',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimRe" + "al','-8.16303','MaxYLimReal','11.36009','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on'" + ",'XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509" + "803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.41176470" + "5882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450980392156" + "9 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],'L" + "ineStyle','-','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," + "'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-11.45092','MaxYLimReal','7.90757','YLa" + "belReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',fa" + "lse,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 " + "0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0" + ".831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215" + "686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','Li" + "neWidth',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('C" + "olor',[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',{{[]}},'ShowContent',true,'Place" + "ment',5),struct('MinYLimReal','0.75','MaxYLimReal','3.25','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',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelRe" + "al','','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.39215686274509" + "8 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039" + "215686 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','n" + "one','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,'Mark" + "er','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Pl" + "acement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('" + "Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measureme" + "nts',true,'Version','2018a')),'Version','2020a','Location',[135 184 3841 2132])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Scope + Name "Joint Positon Error" + SID "2335" + Ports [6] + Position [1195, -40, 1280, 80] + ZOrder 504 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" + "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," + "'DataLoggingVariableName','jointPos_Error_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation" + "','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{stru" + "ct('MinYLimReal','-10.85715','MaxYLimReal','13.2441','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','13.2441'" + ",'LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686" + "274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529" + "411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705" + "8823529 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'),s" + "truct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'Line" + "Style','-','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')}},'UserDefinedChannelN" + "ames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-24.65677','Max" + "YLimReal','13.37484','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',0,'LineNames',{{[]}}," + "'ShowContent',true,'Placement',2),struct('MinYLimReal','-37.11648','MaxYLimReal','33.25109','YLabelReal','','MinYL" + "imMag','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" + ".0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156" + "863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Mark" + "er','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],'Lin" + "eStyle','-','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',{{[]}},'ShowContent',true,'Placement',3),struct('M" + "inYLimReal','-28.71162','MaxYLimReal','40.05399','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibil" + "ity','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.0745098039215686;0.717647058823529 0.27450" + "9803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperti" + "esCache',{{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','-','Lin" + "eWidth',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('Co" + "lor',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'Num" + "Lines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-16.66332','MaxYLimReal','13.3" + "9274','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMa" + "gPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOr" + "der',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3921568" + "62745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074" + "5098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','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],'Li" + "neStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0." + "5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',t" + "rue,'Placement',5),struct('MinYLimReal','-0.375','MaxYLimReal','13.375','YLabelReal','','MinYLimMag','0','MaxYLimM" + "ag','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.0745098039215686;0." + "717647058823529 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',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelRe" + "al','','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.39215686274509" + "8 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039" + "215686 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','n" + "one','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,'Mark" + "er','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Pl" + "acement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('" + "Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measureme" + "nts',true,'Version','2018a')),'Version','2020a','Location',[135 169 2055 1097])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Scope + Name "Joint Velocity" + SID "4122" + Ports [6] + Position [1195, 148, 1280, 272] + ZOrder 886 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" + "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," + "'DataLoggingVariableName','jointVel_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1'," + "'DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('Mi" + "nYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibili" + "ty','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6" + "86274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0." + "411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509" + "803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertie" + "sCache',{{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','-','Line" + "Width',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('Col" + "or',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumL" + "ines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-20.50153','MaxYLimReal','20.29" + "706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMag" + "Phase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrd" + "er',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.39215686" + "2745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745" + "098039215686 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,'Mark" + "er','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],'Lin" + "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5" + ",'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',tr" + "ue,'Placement',2),struct('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxY" + "LimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickC" + "olor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215" + "686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.074509803921568" + "6;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%<" + "SignalLabel>','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" + "ble','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','-','Lin" + "eWidth',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')}},'UserDef" + "inedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-20" + ".50153','MaxYLimReal','20.29706','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.623529411764706 1;1 0.411764705882353" + " 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0" + "588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struc" + "t('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','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],'Li" + "neStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNa" + "mes',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelRea" + "l','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'A" + "xesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666" + "666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137" + "2549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0." + "650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidt" + "h',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" + "ible','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','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none" + "','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'," + "5),struct('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','L" + "egendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274" + "509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411" + "764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882" + "3529 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'),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',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('MinYL" + "imReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','AxesColor',[0 0 0" + "],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0" + "745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.074" + "5098039215686;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'),stru" + "ct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Ma" + "rker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" + ")}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSam" + "ples','60','TimeRangeFrames','60','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation'," + "true,'OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2020a','L" + "ocation',[379 354 2309 1288])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Scope + Name "Measured Torques" + SID "2338" + Ports [6] + Position [540, 73, 645, 187] + ZOrder 495 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" + "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," + "'DataLoggingVariableName','tauMeasured_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','" + "1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct(" + "'MinYLimReal','-13.53306','MaxYLimReal','13.58663','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','13.58663'," + "'LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862" + "74509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294" + "11764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058" + "823529 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'),st" + "ruct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineS" + "tyle','-','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','o" + "n'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNa" + "mes',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-4.19072','MaxYL" + "imReal','4.87188','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'," + "true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745098039" + "22],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784313725" + "49;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','-','LineWid" + "th',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vi" + "sible','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','-','L" + "ineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'Sh" + "owContent',true,'Placement',2),struct('MinYLimReal','-4.21765','MaxYLimReal','4.8762','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.074" + "5098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450" + "98039215686;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','n" + "one','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,'Mark" + "er','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}" + "},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLim" + "Real','-45.10013','MaxYLimReal','35.90119','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','" + "on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274" + "509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.41176" + "4705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450980392" + "1569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCach" + "e',{{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','Visi" + "ble','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',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-44.89002','MaxYLimReal','34.01019'," + "'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.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3921568627450" + "98 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509803" + "9215686 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'),struc" + "t('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'P" + "lacement',5),struct('MinYLimReal','-0.5','MaxYLimReal','14.5','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','','LinePropertie" + "sCache',{{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','-','Line" + "Width',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('Col" + "or',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumL" + "ines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','Ax" + "esColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.06666" + "66666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372" + "549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6" + "50980392156863],'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','Visi" + "ble','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','-','Lin" + "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1" + "),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Pl" + "ot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true," + "'Version','2018a')),'Version','2020a','Location',[1266 568 2566 1252])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Selector + Name "Selector1" + SID "4118" + Ports [1, 1] + Position [845, 188, 905, 212] + ZOrder 889 + ShowName off + InputPortWidth "6+ROBOT_DOF" + IndexOptions "Index vector (dialog)" + Indices "[7:6+ROBOT_DOF]" + OutputSizes "1" + } + Block { + BlockType Selector + Name "Selector2" + SID "4119" + Ports [1, 1] + Position [845, 348, 905, 372] + ZOrder 888 + ShowName off + InputPortWidth "6+ROBOT_DOF" + IndexOptions "Index vector (dialog)" + Indices "[1:6]" + OutputSizes "1" + } + Block { + BlockType Sum + Name "Sum" + SID "2295" + Ports [2, 1] + Position [210, -45, 230, -25] + ZOrder 493 + ShowName off + Inputs "|+-" + } + Block { + BlockType Sum + Name "Sum1" + SID "4827" + Ports [2, 1] + Position [210, 320, 230, 340] + ZOrder 896 + ShowName off + Inputs "+-|" + Port { + PortNumber 1 + Name "CoM_Error" + } + } + Block { + BlockType Sum + Name "Sum3" + SID "2298" + Ports [2, 1] + Position [845, 0, 865, 20] + ZOrder 506 + ShowName off + Inputs "-+|" + } + Block { + BlockType Scope + Name "Torques Error" + SID "2337" + Ports [6] + Position [540, -82, 645, 32] + ZOrder 494 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" + ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" + "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," + "'DataLoggingVariableName','tauError_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1'," + "'DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('Mi" + "nYLimReal','-23.63249','MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.95301','Le" + "gendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745" + "09803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294117" + "64706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823" + "529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','L" + "inePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" + "t('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','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',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimR" + "eal','5.80954','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',tru" + "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922]" + ",'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;" + "0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 " + "1;1 0.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','Visib" + "le','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','-','Line" + "Width',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowC" + "ontent',true,'Placement',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],'A" + "xesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450" + "98039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098" + "039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Ti" + "tle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','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',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimRe" + "al','-23.22756','MaxYLimReal','25.22297','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on" + "','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450" + "9803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647" + "05882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039215" + "69 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','Visibl" + "e','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',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-66.44849','MaxYLimReal','41.64929','Y" + "LabelReal','','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.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098" + " 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07450980392" + "15686 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','no" + "ne','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,'Marke" + "r','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Pla" + "cement',5),struct('MinYLimReal','0.25','MaxYLimReal','7.75','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Le" + "gendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745" + "09803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294117" + "64706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823" + "529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','LinePropertiesC" + "ache',{{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','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','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',{{}},'NumLin" + "es',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','Axes" + "Color',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666" + "666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137254" + "9019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650" + "980392156863],'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','Visibl" + "e','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','-','LineW" + "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1)," + "'TimeRangeSamples','90','TimeRangeFrames','90','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot" + " Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'V" + "ersion','2018a')),'Version','2020a','Location',[135 169 1990 1170])" + NumInputPorts "6" + Floating off + } + Line { + Name "CoM_Measured" + ZOrder 1 + SrcBlock "pos_CoM" + SrcPort 1 + Points [120, 0] + Branch { + ZOrder 2 + DstBlock "Sum1" + DstPort 1 + } + Branch { + ZOrder 3 + Labels [-1, 0] + DstBlock "CoM Position" + DstPort 1 + } + } + Line { + ZOrder 4 + SrcBlock "Gain" + SrcPort 1 + DstBlock "Demux1" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "jointPos" + SrcPort 1 + Points [80, 0] + Branch { + ZOrder 6 + DstBlock "Gain" + DstPort 1 + } + Branch { + ZOrder 7 + DstBlock "Sum3" + DstPort 1 + } + } + Line { + Name "right_leg" + ZOrder 8 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 5 + DstBlock "Joint Position Measured" + DstPort 5 + } + Line { + Name "torso" + ZOrder 9 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 1 + DstBlock "Joint Position Measured" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 10 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 2 + DstBlock "Joint Position Measured" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 11 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 4 + DstBlock "Joint Position Measured" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 12 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 3 + DstBlock "Joint Position Measured" + DstPort 3 + } + Line { + ZOrder 62 + SrcBlock "tau_star" + SrcPort 1 + Points [41, 0] + Branch { + ZOrder 63 + Points [0, 150] + Branch { + ZOrder 102 + Points [0, 155; 79, 0] + Branch { + ZOrder 14 + DstBlock "Sum" + DstPort 2 + } + Branch { + ZOrder 15 + DstBlock "Demux9" + DstPort 1 + } + } + Branch { + ZOrder 101 + DstBlock "Sum" + DstPort 1 + } + } + Branch { + ZOrder 64 + DstBlock "Demux8" + DstPort 1 + } + } + Line { + Name "right_leg" + ZOrder 16 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 5 + DstBlock "Desired Torques" + DstPort 5 + } + Line { + Name "torso" + ZOrder 17 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 1 + DstBlock "Desired Torques" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 18 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 2 + DstBlock "Desired Torques" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 19 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 4 + DstBlock "Desired Torques" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 20 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 3 + DstBlock "Desired Torques" + DstPort 3 + } + Line { + Name "right_leg" + ZOrder 21 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 5 + DstBlock "Torques Error" + DstPort 5 + } + Line { + Name "torso" + ZOrder 22 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 1 + DstBlock "Torques Error" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 23 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 2 + DstBlock "Torques Error" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 24 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 4 + DstBlock "Torques Error" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 25 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 3 + DstBlock "Torques Error" + DstPort 3 + } + Line { + ZOrder 26 + SrcBlock "Sum" + SrcPort 1 + DstBlock "Demux10" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 27 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 5 + DstBlock "Measured Torques" + DstPort 5 + } + Line { + Name "torso" + ZOrder 28 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 1 + DstBlock "Measured Torques" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 29 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 2 + DstBlock "Measured Torques" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 30 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 4 + DstBlock "Measured Torques" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 31 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 3 + DstBlock "Measured Torques" + DstPort 3 + } + Line { + ZOrder 32 + SrcBlock "jointPos_des" + SrcPort 1 + Points [32, 0] + Branch { + ZOrder 33 + Points [0, 160] + DstBlock "Sum3" + DstPort 2 + } + Branch { + ZOrder 34 + DstBlock "Gain3" + DstPort 1 + } + } + Line { + ZOrder 35 + SrcBlock "Gain3" + SrcPort 1 + DstBlock "Demux2" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 36 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 5 + DstBlock "Joint Position Desired" + DstPort 5 + } + Line { + Name "torso" + ZOrder 37 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 1 + DstBlock "Joint Position Desired" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 38 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 2 + DstBlock "Joint Position Desired" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 39 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 4 + DstBlock "Joint Position Desired" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 40 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 3 + DstBlock "Joint Position Desired" + DstPort 3 + } + Line { + ZOrder 41 + SrcBlock "Gain4" + SrcPort 1 + DstBlock "Demux5" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 42 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 5 + DstBlock "Joint Positon Error" + DstPort 5 + } + Line { + Name "torso" + ZOrder 43 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 1 + DstBlock "Joint Positon Error" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 44 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 2 + DstBlock "Joint Positon Error" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 45 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 4 + DstBlock "Joint Positon Error" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 46 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 3 + DstBlock "Joint Positon Error" + DstPort 3 + } + Line { + ZOrder 47 + SrcBlock "Sum3" + SrcPort 1 + DstBlock "Gain4" + DstPort 1 + } + Line { + Name "CoM_Desired" + ZOrder 48 + SrcBlock "pos_CoM_des" + SrcPort 1 + Points [67, 0] + Branch { + ZOrder 49 + Points [0, 35] + DstBlock "Sum1" + DstPort 2 + } + Branch { + ZOrder 50 + Labels [-1, 0] + DstBlock "CoM Position" + DstPort 2 + } + } + Line { + ZOrder 51 + SrcBlock "w_H_b" + SrcPort 1 + Points [77, 0] + Branch { + ZOrder 52 + Points [0, -40] + DstBlock "CoM6D -> \nCoMXYZ2" + DstPort 1 + } + Branch { + ZOrder 53 + DstBlock "CoM6D -> \nCoMXYZ1" + DstPort 1 + } + } + Line { + Name "basePosition" + ZOrder 54 + Labels [-1, 0] + SrcBlock "CoM6D -> \nCoMXYZ2" + SrcPort 1 + DstBlock "Base Pose" + DstPort 1 + } + Line { + Name "baseOrientation" + ZOrder 55 + Labels [-1, 0] + SrcBlock "CoM6D -> \nCoMXYZ1" + SrcPort 1 + DstBlock "Base Pose" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 56 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 4 + DstBlock "Desired Torques After Saturation" + DstPort 4 + } + Line { + Name "right_leg" + ZOrder 57 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 5 + DstBlock "Desired Torques After Saturation" + DstPort 5 + } + Line { + Name "left_arm" + ZOrder 58 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 2 + DstBlock "Desired Torques After Saturation" + DstPort 2 + } + Line { + Name "torso" + ZOrder 59 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 1 + DstBlock "Desired Torques After Saturation" + DstPort 1 + } + Line { + Name "right_arm" + ZOrder 60 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 3 + DstBlock "Desired Torques After Saturation" + DstPort 3 + } + Line { + ZOrder 61 + SrcBlock "tau_star_afterSat" + SrcPort 1 + DstBlock "Demux7" + DstPort 1 + } + Line { + ZOrder 65 + SrcBlock "nu" + SrcPort 1 + Points [26, 0] + Branch { + ZOrder 66 + DstBlock "Selector1" + DstPort 1 + } + Branch { + ZOrder 67 + Points [0, 160] + DstBlock "Selector2" + DstPort 1 + } + } + Line { + ZOrder 68 + SrcBlock "Selector2" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + ZOrder 69 + SrcBlock "Gain1" + SrcPort 1 + DstBlock "Demux3" + DstPort 1 + } + Line { + Name "base linear velocity" + ZOrder 70 + Labels [-1, 0] + SrcBlock "Demux" + SrcPort 1 + DstBlock "Base linear and angular velocity" + DstPort 1 + } + Line { + Name "base angular velocity" + ZOrder 71 + Labels [-1, 0] + SrcBlock "Gain2" + SrcPort 1 + DstBlock "Base linear and angular velocity" + DstPort 2 + } + Line { + Name "right_arm" + ZOrder 72 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 3 + DstBlock "Joint Velocity" + DstPort 3 + } + Line { + Name "right_leg" + ZOrder 73 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 5 + DstBlock "Joint Velocity" + DstPort 5 + } + Line { + ZOrder 74 + SrcBlock "Selector1" + SrcPort 1 + DstBlock "Gain1" + DstPort 1 + } + Line { + Name "torso" + ZOrder 75 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 1 + DstBlock "Joint Velocity" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 76 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 2 + DstBlock "Joint Velocity" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 77 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 4 + DstBlock "Joint Velocity" + DstPort 4 + } + Line { + ZOrder 78 + SrcBlock "Demux" + SrcPort 2 + DstBlock "Gain2" + DstPort 1 + } + Line { + Name "CoM_Error" + ZOrder 79 + Labels [-1, 0] + SrcBlock "Sum1" + SrcPort 1 + DstBlock "CoM Position" + DstPort 3 + } + Line { + Name "state" + ZOrder 80 + SrcBlock "state" + SrcPort 1 + Points [234, 0] + Branch { + ZOrder 81 + Points [0, 145] + Branch { + ZOrder 82 + Points [0, 150] + Branch { + ZOrder 83 + Points [0, 155] + Branch { + ZOrder 84 + Points [0, 185] + Branch { + ZOrder 85 + Points [0, 55; 742, 0] + Branch { + ZOrder 86 + Labels [-1, 0] + DstBlock "Base linear and angular velocity" + DstPort 3 + } + Branch { + ZOrder 87 + Points [0, -160] + Branch { + ZOrder 88 + Labels [-1, 0] + DstBlock "Joint Velocity" + DstPort 6 + } + Branch { + ZOrder 89 + Points [0, -190] + Branch { + ZOrder 90 + Labels [-1, 0] + DstBlock "Joint Positon Error" + DstPort 6 + } + Branch { + ZOrder 91 + Points [0, -160] + Branch { + ZOrder 92 + Labels [-1, 0] + DstBlock "Joint Position Desired" + DstPort 6 + } + Branch { + ZOrder 93 + Points [0, -150; 1, 0] + Branch { + ZOrder 94 + Labels [-1, 0] + Points [0, -170] + DstBlock "Base Pose" + DstPort 3 + } + Branch { + ZOrder 95 + DstBlock "Joint Position Measured" + DstPort 6 + } + } + } + } + } + } + Branch { + ZOrder 96 + Labels [-1, 0] + DstBlock "CoM Position" + DstPort 4 + } + } + Branch { + ZOrder 97 + Labels [-1, 0] + DstBlock "Measured Torques" + DstPort 6 + } + } + Branch { + ZOrder 98 + Labels [-1, 0] + DstBlock "Torques Error" + DstPort 6 + } + } + Branch { + ZOrder 99 + Labels [-1, 0] + DstBlock "Desired Torques" + DstPort 6 + } + } + Branch { + ZOrder 100 + Labels [-1, 0] + DstBlock "Desired Torques After Saturation" + DstPort 6 + } + } + } + } + Line { + ZOrder 1 + SrcBlock "ON_GAZEBO\n4" + SrcPort 1 + Points [100, 0] + DstBlock "Visualizer" + DstPort enable + } + Line { + ZOrder 2 + SrcBlock "From1" + SrcPort 1 + Points [52, 0] + Branch { + ZOrder 3 + DstBlock "Visualize eventual QP failures" + DstPort 2 + } + Branch { + ZOrder 4 + Points [0, 40] + Branch { + ZOrder 5 + Points [0, 135] + DstBlock "Desired and Measured Forces" + DstPort 4 + } + Branch { + ZOrder 6 + DstBlock "Visualizer" + DstPort 6 + } + } + Branch { + ZOrder 7 + Points [0, -130] + DstBlock "Feet Status and Gains" + DstPort 3 + } + } + Line { + ZOrder 8 + SrcBlock "From7" + SrcPort 1 + DstBlock "Visualizer" + DstPort 2 + } + Line { + ZOrder 9 + SrcBlock "From5" + SrcPort 1 + DstBlock "Visualizer" + DstPort 1 + } + Line { + ZOrder 10 + SrcBlock "From12" + SrcPort 1 + DstBlock "Visualizer" + DstPort 5 + } + Line { + ZOrder 11 + SrcBlock "ON_GAZEBO\n2" + SrcPort 1 + Points [165, 0] + DstBlock "Visualize eventual QP failures" + DstPort enable + } + Line { + ZOrder 12 + SrcBlock "From9" + SrcPort 1 + DstBlock "Visualize eventual QP failures" + DstPort 1 + } + Line { + ZOrder 13 + SrcBlock "ON_GAZEBO\n1" + SrcPort 1 + Points [160, 0] + DstBlock "Desired and Measured Forces" + DstPort enable + } + Line { + ZOrder 14 + SrcBlock "ON_GAZEBO\n6" + SrcPort 1 + Points [135, 0] + DstBlock "Feet Status and Gains" + DstPort enable + } + Line { + ZOrder 15 + SrcBlock "From4" + SrcPort 1 + DstBlock "Feet Status and Gains" + DstPort 1 + } + Line { + ZOrder 16 + SrcBlock "From8" + SrcPort 1 + DstBlock "Feet Status and Gains" + DstPort 2 + } + Line { + ZOrder 17 + SrcBlock "From2" + SrcPort 1 + DstBlock "Desired and Measured Forces" + DstPort 3 + } + Line { + ZOrder 18 + SrcBlock "From11" + SrcPort 1 + DstBlock "Desired and Measured Forces" + DstPort 1 + } + Line { + ZOrder 19 + SrcBlock "From10" + SrcPort 1 + DstBlock "Desired and Measured Forces" + DstPort 2 + } + Line { + ZOrder 20 + SrcBlock "From6" + SrcPort 1 + DstBlock "Visualizer" + DstPort 3 + } + Line { + ZOrder 21 + SrcBlock "From3" + SrcPort 1 + DstBlock "Visualizer" + DstPort 4 + } + Line { + ZOrder 22 + SrcBlock "From" + SrcPort 1 + DstBlock "Visualizer" + DstPort 7 + } + Line { + ZOrder 23 + SrcBlock "From13" + SrcPort 1 + DstBlock "Visualizer" + DstPort 8 + } + Line { + ZOrder 24 + SrcBlock "From14" + SrcPort 1 + DstBlock "Visualizer" + DstPort 9 + } + } + } + Block { + BlockType SubSystem + Name "Dynamics and Kinematics" + SID "2341" + Ports [3, 11] + Position [605, -7, 735, 317] + ZOrder 541 + BackgroundColor "[0.000000, 1.000000, 0.498039]" + NameLocation "top" + RequestExecContextInheritance off + System { + Name "Dynamics and Kinematics" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "158" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "w_H_b" + SID "2342" + Position [-850, -167, -820, -153] + ZOrder 209 + } + Block { + BlockType Inport + Name "nu" + SID "2344" + Position [-850, -27, -820, -13] + ZOrder 582 + Port "2" + } + Block { + BlockType Inport + Name "jointPos" + SID "2343" + Position [-850, -97, -820, -83] + ZOrder 224 + Port "3" + } + Block { + BlockType SubSystem + Name "Dynamics" + SID "4353" + Ports [3, 3] + Position [-645, -196, -540, 16] + ZOrder 838 + RequestExecContextInheritance off + System { + Name "Dynamics" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "308" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "w_H_b" + SID "4354" + Position [120, 28, 150, 42] + ZOrder 584 + } + Block { + BlockType Inport + Name "jointPos" + SID "4355" + Position [120, 63, 150, 77] + ZOrder 585 + Port "2" + } + Block { + BlockType Inport + Name "nu" + SID "4356" + Position [120, 188, 150, 202] + ZOrder 586 + Port "3" + } + Block { + BlockType SubSystem + Name "Add motors reflected inertia" + SID "4515" + Ports [1, 1] + Position [580, 34, 705, 76] + ZOrder -19 + RequestExecContextInheritance off + System { + Name "Add motors reflected inertia" + Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "634" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "M" + SID "4516" + Position [-55, 88, -25, 102] + ZOrder 591 + } + Block { + BlockType SubSystem + Name "Add motor reflected inertias" + SID "4518" + Ports [1, 1] + Position [70, 77, 235, 113] + ZOrder 593 + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Add motor reflected inertias" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "124" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "M" + SID "4518::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Demux + Name " Demux " + SID "4518::123" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 113 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4518::122" + Tag "Stateflow S-Function 6" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 112 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "M_with_inertia" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4518::124" + Position [460, 241, 480, 259] + ZOrder 114 + } + Block { + BlockType Outport + Name "M_with_inertia" + SID "4518::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 150 + SrcBlock "M" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "M_with_inertia" + ZOrder 151 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "M_with_inertia" + DstPort 1 + } + Line { + ZOrder 152 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 153 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Constant + Name "Constant" + SID "4846" + Position [50, 142, 255, 158] + ZOrder 725 + ShowName off + HideAutomaticName off + Value "Config.USE_MOTOR_REFLECTED_INERTIA" + } + Block { + BlockType Switch + Name "Switch" + SID "4844" + Position [310, 69, 380, 231] + ZOrder 594 + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "M_with_motor_inertia" + SID "4517" + Position [455, 143, 485, 157] + ZOrder 592 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 109 + SrcBlock "M" + SrcPort 1 + Points [36, 0] + Branch { + ZOrder 114 + Points [0, 110] + DstBlock "Switch" + DstPort 3 + } + Branch { + ZOrder 113 + DstBlock "Add motor reflected inertias" + DstPort 1 + } + } + Line { + ZOrder 111 + SrcBlock "Constant" + SrcPort 1 + DstBlock "Switch" + DstPort 2 + } + Line { + ZOrder 112 + SrcBlock "Add motor reflected inertias" + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + Line { + ZOrder 115 + SrcBlock "Switch" + SrcPort 1 + DstBlock "M_with_motor_inertia" + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "CentroidalMomentum" + SID "2345" + Ports [4, 1] + Position [395, 281, 575, 344] + ZOrder 223 + HideAutomaticName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + } + Block { + BlockType Selector + Name "CoM6D -> \nCoMXYZ1" + SID "2346" + Ports [1, 1] + Position [215, 211, 255, 229] + ZOrder 583 + ShowName off + InputPortWidth "6+ROBOT_DOF" + IndexOptions "Index vector (dialog)" + Indices "[7:6+ROBOT_DOF]" + OutputSizes "1" + } + Block { + BlockType Selector + Name "CoM6D -> \nCoMXYZ2" + SID "2347" + Ports [1, 1] + Position [215, 186, 255, 204] + ZOrder 581 + ShowName off + InputPortWidth "6+ROBOT_DOF" + IndexOptions "Index vector (dialog)" + Indices "[1 2 3 4 5 6]" + OutputSizes "1" + } + Block { + BlockType Reference + Name "GetBiasForces" + SID "2348" + Ports [4, 1] + Position [395, 132, 535, 233] + ZOrder 222 + HideAutomaticName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Dynamics/GetBiasForces" + SourceType "Get Generalized Bias Forces" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "MassMatrix" + SID "2349" + Ports [2, 1] + Position [395, 19, 535, 86] + ZOrder 221 + HideAutomaticName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Dynamics/MassMatrix" + SourceType "MassMatrix" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + } + Block { + BlockType Outport + Name "M" + SID "4357" + Position [750, 48, 780, 62] + ZOrder 587 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "h" + SID "4358" + Position [625, 178, 655, 192] + ZOrder 588 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "L" + SID "4359" + Position [625, 308, 655, 322] + ZOrder 589 + Port "3" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 83 + SrcBlock "CentroidalMomentum" + SrcPort 1 + DstBlock "L" + DstPort 1 + } + Line { + ZOrder 82 + SrcBlock "GetBiasForces" + SrcPort 1 + DstBlock "h" + DstPort 1 + } + Line { + ZOrder 78 + SrcBlock "w_H_b" + SrcPort 1 + Points [168, 0] + Branch { + ZOrder 5 + Points [0, 110] + Branch { + ZOrder 6 + Points [0, 145] + DstBlock "CentroidalMomentum" + DstPort 1 + } + Branch { + ZOrder 7 + DstBlock "GetBiasForces" + DstPort 1 + } + } + Branch { + ZOrder 8 + DstBlock "MassMatrix" + DstPort 1 + } + } + Line { + ZOrder 79 + SrcBlock "jointPos" + SrcPort 1 + Points [157, 0] + Branch { + ZOrder 10 + Points [0, 100] + Branch { + ZOrder 11 + Points [0, 135] + DstBlock "CentroidalMomentum" + DstPort 2 + } + Branch { + ZOrder 12 + DstBlock "GetBiasForces" + DstPort 2 + } + } + Branch { + ZOrder 13 + DstBlock "MassMatrix" + DstPort 2 + } + } + Line { + ZOrder 14 + SrcBlock "CoM6D -> \nCoMXYZ2" + SrcPort 1 + Points [41, 0] + Branch { + ZOrder 75 + Points [0, 125] + DstBlock "CentroidalMomentum" + DstPort 3 + } + Branch { + ZOrder 73 + DstBlock "GetBiasForces" + DstPort 3 + } + } + Line { + ZOrder 17 + SrcBlock "CoM6D -> \nCoMXYZ1" + SrcPort 1 + Points [20, 0] + Branch { + ZOrder 77 + Points [0, 115] + DstBlock "CentroidalMomentum" + DstPort 4 + } + Branch { + ZOrder 76 + DstBlock "GetBiasForces" + DstPort 4 + } + } + Line { + ZOrder 80 + SrcBlock "nu" + SrcPort 1 + Points [17, 0] + Branch { + ZOrder 74 + Points [0, 25] + DstBlock "CoM6D -> \nCoMXYZ1" + DstPort 1 + } + Branch { + ZOrder 22 + DstBlock "CoM6D -> \nCoMXYZ2" + DstPort 1 + } + } + Line { + ZOrder 106 + SrcBlock "MassMatrix" + SrcPort 1 + DstBlock "Add motors reflected inertia" + DstPort 1 + } + Line { + ZOrder 107 + SrcBlock "Add motors reflected inertia" + SrcPort 1 + DstBlock "M" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Kinematics" + SID "4321" + Ports [3, 8] + Position [-415, 21, -265, 359] + ZOrder 837 + RequestExecContextInheritance off + System { + Name "Kinematics" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "359" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "nu" + SID "4342" + Position [-250, 493, -220, 507] + ZOrder 857 + } + Block { + BlockType Inport + Name "jointPos" + SID "4339" + Position [-250, 58, -220, 72] + ZOrder 868 + Port "2" + } + Block { + BlockType Inport + Name "w_H_b" + SID "4336" + Position [-250, 163, -220, 177] + ZOrder 870 + Port "3" + } + Block { + BlockType Reference + Name "CoM" + SID "4363" + Ports [2, 1] + Position [60, 321, 225, 359] + ZOrder 882 + ShowName off + HideAutomaticName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.COM" + } + Block { + BlockType Selector + Name "CoM6D -> \nCoMXYZ2" + SID "4364" + Ports [1, 1] + Position [265, 331, 295, 349] + ZOrder 883 + ShowName off + NumberOfDimensions "2" + InputPortWidth "7" + IndexOptions "Index vector (dialog),Index vector (dialog)" + Indices "[1 2 3],[4]" + OutputSizes "1,1" + } + Block { + BlockType Selector + Name "CoM6D -> \nCoMXYZ3" + SID "2369" + Ports [1, 1] + Position [-140, 510, -100, 520] + ZOrder 833 + ShowName off + InputPortWidth "6+ROBOT_DOF" + IndexOptions "Index vector (dialog)" + Indices "[7:6+ROBOT_DOF]" + OutputSizes "1" + } + Block { + BlockType Selector + Name "CoM6D -> \nCoMXYZ4" + SID "2370" + Ports [1, 1] + Position [-140, 495, -100, 505] + ZOrder 832 + ShowName off + InputPortWidth "6+ROBOT_DOF" + IndexOptions "Index vector (dialog)" + Indices "[1 2 3 4 5 6]" + OutputSizes "1" + } + Block { + BlockType Reference + Name "DotJNu l_sole" + SID "2375" + Ports [4, 1] + Position [60, 382, 235, 443] + ZOrder 829 + ShowName off + HideAutomaticName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Jacobians/DotJNu" + SourceType "DotJNu" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "DotJNu r_sole" + SID "2376" + Ports [4, 1] + Position [65, 464, 235, 521] + ZOrder 831 + ShowName off + HideAutomaticName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Jacobians/DotJNu" + SourceType "DotJNu" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.RIGHT_FOOT" + } + Block { + BlockType Goto + Name "Goto2" + SID "4797" + Position [360, 371, 430, 389] + ZOrder 950 + ShowName off + GotoTag "pos_CoM" + TagVisibility "global" + } + Block { + BlockType Reference + Name "Jacobian com" + SID "2378" + Ports [2, 1] + Position [60, 260, 225, 300] + ZOrder 835 + ShowName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" + SourceType "Jacobian" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.COM" + } + Block { + BlockType Reference + Name "Jacobian l_sole" + SID "2379" + Ports [2, 1] + Position [60, 160, 225, 195] + ZOrder 828 + ShowName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" + SourceType "Jacobian" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "Jacobian r_sole" + SID "2380" + Ports [2, 1] + Position [60, 210, 225, 245] + ZOrder 830 + ShowName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" + SourceType "Jacobian" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.RIGHT_FOOT" + } + Block { + BlockType Reference + Name "l_sole" + SID "2405" + Ports [2, 1] + Position [60, 35, 220, 75] + ZOrder 826 + ShowName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "r_sole" + SID "2406" + Ports [2, 1] + Position [60, 95, 220, 135] + ZOrder 827 + ShowName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.RIGHT_FOOT" + } + Block { + BlockType Outport + Name "w_H_l_sole" + SID "4350" + Position [380, 48, 410, 62] + ZOrder 865 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "w_H_r_sole" + SID "4367" + Position [380, 108, 410, 122] + ZOrder 874 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_l_sole" + SID "4368" + Position [380, 173, 410, 187] + ZOrder 875 + Port "3" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_r_sole" + SID "4369" + Position [380, 223, 410, 237] + ZOrder 876 + Port "4" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_CoM" + SID "4370" + Position [380, 273, 410, 287] + ZOrder 877 + Port "5" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "pos_CoM" + SID "4371" + Position [380, 333, 410, 347] + ZOrder 878 + Port "6" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "JDot_l_sole_nu" + SID "4372" + Position [380, 408, 410, 422] + ZOrder 879 + Port "7" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "JDot_r_sole_nu" + SID "4374" + Position [380, 488, 410, 502] + ZOrder 881 + Port "8" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 37 + SrcBlock "CoM6D -> \nCoMXYZ3" + SrcPort 1 + Points [57, 0] + Branch { + ZOrder 203 + DstBlock "DotJNu r_sole" + DstPort 4 + } + Branch { + ZOrder 193 + Points [0, -80] + DstBlock "DotJNu l_sole" + DstPort 4 + } + } + Line { + ZOrder 62 + SrcBlock "CoM6D -> \nCoMXYZ4" + SrcPort 1 + Points [80, 0] + Branch { + ZOrder 206 + DstBlock "DotJNu r_sole" + DstPort 3 + } + Branch { + ZOrder 204 + Points [0, -80] + DstBlock "DotJNu l_sole" + DstPort 3 + } + } + Line { + ZOrder 129 + SrcBlock "nu" + SrcPort 1 + Points [27, 0] + Branch { + ZOrder 202 + Points [0, 15] + DstBlock "CoM6D -> \nCoMXYZ3" + DstPort 1 + } + Branch { + ZOrder 188 + DstBlock "CoM6D -> \nCoMXYZ4" + DstPort 1 + } + } + Line { + ZOrder 171 + SrcBlock "l_sole" + SrcPort 1 + DstBlock "w_H_l_sole" + DstPort 1 + } + Line { + ZOrder 172 + SrcBlock "r_sole" + SrcPort 1 + DstBlock "w_H_r_sole" + DstPort 1 + } + Line { + ZOrder 173 + SrcBlock "Jacobian l_sole" + SrcPort 1 + DstBlock "J_l_sole" + DstPort 1 + } + Line { + ZOrder 174 + SrcBlock "Jacobian r_sole" + SrcPort 1 + DstBlock "J_r_sole" + DstPort 1 + } + Line { + ZOrder 183 + SrcBlock "Jacobian com" + SrcPort 1 + DstBlock "J_CoM" + DstPort 1 + } + Line { + ZOrder 256 + SrcBlock "CoM" + SrcPort 1 + DstBlock "CoM6D -> \nCoMXYZ2" + DstPort 1 + } + Line { + ZOrder 217 + SrcBlock "DotJNu l_sole" + SrcPort 1 + DstBlock "JDot_l_sole_nu" + DstPort 1 + } + Line { + ZOrder 218 + SrcBlock "DotJNu r_sole" + SrcPort 1 + DstBlock "JDot_r_sole_nu" + DstPort 1 + } + Line { + ZOrder 144 + SrcBlock "jointPos" + SrcPort 1 + Points [235, 0] + Branch { + ZOrder 276 + Points [0, 60] + Branch { + ZOrder 160 + Points [0, 60] + Branch { + ZOrder 165 + Points [0, 50] + Branch { + ZOrder 170 + Points [0, 55] + Branch { + ZOrder 178 + DstBlock "Jacobian com" + DstPort 2 + } + Branch { + ZOrder 177 + Points [0, 60] + Branch { + ZOrder 270 + Points [0, 55] + Branch { + ZOrder 274 + Points [0, 80] + DstBlock "DotJNu r_sole" + DstPort 2 + } + Branch { + ZOrder 214 + DstBlock "DotJNu l_sole" + DstPort 2 + } + } + Branch { + ZOrder 258 + DstBlock "CoM" + DstPort 2 + } + } + } + Branch { + ZOrder 169 + DstBlock "Jacobian r_sole" + DstPort 2 + } + } + Branch { + ZOrder 164 + DstBlock "Jacobian l_sole" + DstPort 2 + } + } + Branch { + ZOrder 159 + DstBlock "r_sole" + DstPort 2 + } + } + Branch { + ZOrder 149 + DstBlock "l_sole" + DstPort 2 + } + } + Line { + ZOrder 145 + SrcBlock "w_H_b" + SrcPort 1 + Points [116, 0] + Branch { + ZOrder 166 + Points [0, 50] + Branch { + ZOrder 168 + Points [0, 50] + Branch { + ZOrder 176 + DstBlock "Jacobian com" + DstPort 1 + } + Branch { + ZOrder 175 + Points [0, 60] + Branch { + ZOrder 269 + Points [0, 60] + Branch { + ZOrder 273 + Points [0, 80] + DstBlock "DotJNu r_sole" + DstPort 1 + } + Branch { + ZOrder 220 + DstBlock "DotJNu l_sole" + DstPort 1 + } + } + Branch { + ZOrder 257 + DstBlock "CoM" + DstPort 1 + } + } + } + Branch { + ZOrder 167 + DstBlock "Jacobian r_sole" + DstPort 1 + } + } + Branch { + ZOrder 158 + DstBlock "Jacobian l_sole" + DstPort 1 + } + Branch { + ZOrder 157 + Points [0, -65] + Branch { + ZOrder 148 + DstBlock "r_sole" + DstPort 1 + } + Branch { + ZOrder 147 + Points [0, -60] + DstBlock "l_sole" + DstPort 1 + } + } + } + Line { + ZOrder 255 + SrcBlock "CoM6D -> \nCoMXYZ2" + SrcPort 1 + Points [27, 0] + Branch { + ZOrder 306 + Points [0, 40] + DstBlock "Goto2" + DstPort 1 + } + Branch { + ZOrder 305 + DstBlock "pos_CoM" + DstPort 1 + } + } + } + } + Block { + BlockType Outport + Name "M" + SID "2350" + Position [-475, -167, -445, -153] + ZOrder -2 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "h" + SID "2351" + Position [-475, -97, -445, -83] + ZOrder 217 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "L" + SID "2352" + Position [-475, -27, -445, -13] + ZOrder 216 + Port "3" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "w_H_l_sole" + SID "4375" + Position [-185, 43, -155, 57] + ZOrder 882 + Port "4" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "w_H_r_sole" + SID "4376" + Position [-185, 83, -155, 97] + ZOrder 883 + Port "5" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_l_sole" + SID "4377" + Position [-185, 123, -155, 137] + ZOrder 884 + Port "6" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_r_sole" + SID "4378" + Position [-185, 163, -155, 177] + ZOrder 885 + Port "7" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_CoM" + SID "4379" + Position [-185, 203, -155, 217] + ZOrder 886 + Port "8" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "pos_CoM" + SID "4380" + Position [-185, 243, -155, 257] + ZOrder 887 + Port "9" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "JDot_l_sole_nu" + SID "4381" + Position [-185, 283, -155, 297] + ZOrder 888 + Port "10" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "JDot_r_sole_nu" + SID "4382" + Position [-185, 323, -155, 337] + ZOrder 889 + Port "11" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 109 + SrcBlock "w_H_b" + SrcPort 1 + Points [25, 0] + Branch { + ZOrder 137 + Points [0, 460] + DstBlock "Kinematics" + DstPort 3 + } + Branch { + ZOrder 135 + DstBlock "Dynamics" + DstPort 1 + } + } + Line { + ZOrder 110 + SrcBlock "jointPos" + SrcPort 1 + Points [62, 0] + Branch { + ZOrder 158 + DstBlock "Dynamics" + DstPort 2 + } + Branch { + ZOrder 138 + Points [0, 280] + DstBlock "Kinematics" + DstPort 2 + } + } + Line { + ZOrder 111 + SrcBlock "nu" + SrcPort 1 + Points [106, 0] + Branch { + ZOrder 159 + DstBlock "Dynamics" + DstPort 3 + } + Branch { + ZOrder 139 + Points [0, 100] + DstBlock "Kinematics" + DstPort 1 + } + } + Line { + ZOrder 112 + SrcBlock "Dynamics" + SrcPort 1 + DstBlock "M" + DstPort 1 + } + Line { + ZOrder 113 + SrcBlock "Dynamics" + SrcPort 2 + DstBlock "h" + DstPort 1 + } + Line { + ZOrder 114 + SrcBlock "Dynamics" + SrcPort 3 + DstBlock "L" + DstPort 1 + } + Line { + ZOrder 129 + SrcBlock "Kinematics" + SrcPort 7 + DstBlock "JDot_l_sole_nu" + DstPort 1 + } + Line { + ZOrder 130 + SrcBlock "Kinematics" + SrcPort 8 + DstBlock "JDot_r_sole_nu" + DstPort 1 + } + Line { + ZOrder 126 + SrcBlock "Kinematics" + SrcPort 4 + DstBlock "J_r_sole" + DstPort 1 + } + Line { + ZOrder 123 + SrcBlock "Kinematics" + SrcPort 1 + DstBlock "w_H_l_sole" + DstPort 1 + } + Line { + ZOrder 125 + SrcBlock "Kinematics" + SrcPort 3 + DstBlock "J_l_sole" + DstPort 1 + } + Line { + ZOrder 127 + SrcBlock "Kinematics" + SrcPort 5 + DstBlock "J_CoM" + DstPort 1 + } + Line { + ZOrder 128 + SrcBlock "Kinematics" + SrcPort 6 + DstBlock "pos_CoM" + DstPort 1 + } + Line { + ZOrder 124 + SrcBlock "Kinematics" + SrcPort 2 + DstBlock "w_H_r_sole" + DstPort 1 + } + } + } + Block { + BlockType Goto + Name "Goto" + SID "5001" + Position [230, 65, 270, 85] + ZOrder 975 + GotoTag "time" + TagVisibility "global" + } + Block { + BlockType SubSystem + Name "Joint Torque Saturation" + SID "4553" + Ports [1, 1] + Position [1000, 286, 1110, 324] + ZOrder 555 + BackgroundColor "orange" + RequestExecContextInheritance off + System { + Name "Joint Torque Saturation" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "244" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "tau_star" + SID "4554" + Position [-115, 153, -85, 167] + ZOrder -1 + } + Block { + BlockType Constant + Name "Constant" + SID "4558" + Position [65, 87, 265, 103] + ZOrder 3 + ShowName off + Value "Config.SATURATE_TORQUE_DERIVATIVE" + } + Block { + BlockType Constant + Name "Constant1" + SID "4842" + Position [-35, 34, 25, 46] + ZOrder 723 + ShowName off + HideAutomaticName off + Value "Config.tStep" + } + Block { + BlockType Constant + Name "Constant2" + SID "4843" + Position [-35, 54, 25, 66] + ZOrder 724 + ShowName off + HideAutomaticName off + Value "Sat.uDotMax" + } + Block { + BlockType Goto + Name "Goto" + SID "4728" + Position [510, 40, 605, 60] + ZOrder 722 + ShowName off + HideAutomaticName off + GotoTag "tau_star_afterSat" + TagVisibility "global" + } + Block { + BlockType SubSystem + Name "Saturate Torque Derivative" + SID "4556" + Ports [4, 1] + Position [60, -12, 265, 72] + ZOrder 1 + NameLocation "top" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Saturate Torque Derivative" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "119" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "u" + SID "4556::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Inport + Name "u_0" + SID "4556::22" + Position [20, 136, 40, 154] + ZOrder 13 + Port "2" + } + Block { + BlockType Inport + Name "tStep" + SID "4556::67" + Position [20, 171, 40, 189] + ZOrder 56 + Port "3" + } + Block { + BlockType Inport + Name "uDotMax" + SID "4556::68" + Position [20, 206, 40, 224] + ZOrder 57 + Port "4" + } + Block { + BlockType Demux + Name " Demux " + SID "4556::118" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 107 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4556::117" + Tag "Stateflow S-Function 11" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 106 + FunctionName "sf_sfun" + PortCounts "[4 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "uSat" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4556::119" + Position [460, 241, 480, 259] + ZOrder 108 + } + Block { + BlockType Outport + Name "uSat" + SID "4556::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 231 + SrcBlock "u" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 232 + SrcBlock "u_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 233 + SrcBlock "tStep" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 234 + SrcBlock "uDotMax" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "uSat" + ZOrder 235 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "uSat" + DstPort 1 + } + Line { + ZOrder 236 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 237 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Saturate + Name "Saturation" + SID "2353" + Position [405, 77, 445, 113] + ZOrder 546 + ShowName off + UpperLimit "Sat.torque" + LowerLimit "-Sat.torque" + } + Block { + BlockType Switch + Name "Switch" + SID "4557" + Position [310, -5, 365, 195] + ZOrder 2 + ShowName off + HideAutomaticName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Reference + Name "holder 7" + SID "4996" + Ports [1, 1] + Position [-25, 12, 25, 28] + ZOrder 725 + ShowName off + HideAutomaticName off + LibraryVersion "3.11" + SourceBlock "utilityBlocks_lib/Holder" + SourceType "SubSystem" + SourceProductName "WholeBodyControllers" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off + } + Block { + BlockType Outport + Name "tau_star_sat" + SID "4555" + Position [545, 88, 575, 102] + ZOrder -2 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 2 + SrcBlock "Switch" + SrcPort 1 + DstBlock "Saturation" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "Constant" + SrcPort 1 + DstBlock "Switch" + DstPort 2 + } + Line { + ZOrder 4 + SrcBlock "tau_star" + SrcPort 1 + Points [26, 0] + Branch { + ZOrder 15 + DstBlock "Switch" + DstPort 3 + } + Branch { + ZOrder 7 + Points [0, -140] + Branch { + ZOrder 36 + DstBlock "holder 7" + DstPort 1 + } + Branch { + ZOrder 26 + Points [0, -20] + DstBlock "Saturate Torque Derivative" + DstPort 1 + } + } + } + Line { + ZOrder 5 + SrcBlock "Saturate Torque Derivative" + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + Line { + ZOrder 35 + SrcBlock "holder 7" + SrcPort 1 + DstBlock "Saturate Torque Derivative" + DstPort 2 + } + Line { + ZOrder 21 + SrcBlock "Saturation" + SrcPort 1 + Points [29, 0] + Branch { + ZOrder 23 + Points [0, -45] + DstBlock "Goto" + DstPort 1 + } + Branch { + ZOrder 22 + DstBlock "tau_star_sat" + DstPort 1 + } + } + Line { + ZOrder 29 + SrcBlock "Constant1" + SrcPort 1 + DstBlock "Saturate Torque Derivative" + DstPort 3 + } + Line { + ZOrder 30 + SrcBlock "Constant2" + SrcPort 1 + DstBlock "Saturate Torque Derivative" + DstPort 4 + } + } + } + Block { + BlockType SubSystem + Name "Robot State and References" + SID "2087" + Ports [6, 9] + Position [285, 338, 445, 632] + ZOrder 548 + ForegroundColor "blue" + BackgroundColor "cyan" + Priority "-10" + RequestExecContextInheritance off + System { + Name "Robot State and References" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "175" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "neckPos" + SID "4979" + Position [-580, 2663, -550, 2677] + ZOrder 963 + } + Block { + BlockType Inport + Name "jointPos" + SID "4834" + Position [-580, 2703, -550, 2717] + ZOrder 959 + Port "2" + } + Block { + BlockType Inport + Name "jointVel" + SID "4833" + Position [80, 2523, 110, 2537] + ZOrder 958 + Port "3" + } + Block { + BlockType Inport + Name "IMU_meas" + SID "4835" + Position [-580, 2743, -550, 2757] + ZOrder 960 + Port "4" + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "4878" + Position [-580, 2788, -550, 2802] + ZOrder 961 + Port "5" + } + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "4879" + Position [-580, 2843, -550, 2857] + ZOrder 962 + Port "6" + } + Block { + BlockType SubSystem + Name "Compute State Velocity" + SID "4211" + Ports [4, 1] + Position [155, 2481, 270, 2609] + ZOrder 904 + RequestExecContextInheritance off + System { + Name "Compute State Velocity" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "241" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos" + SID "4212" + Position [-10, -187, 20, -173] + ZOrder 562 + NameLocation "top" + } + Block { + BlockType Inport + Name "jointVel" + SID "4832" + Position [200, -92, 230, -78] + ZOrder 870 + Port "2" + } + Block { + BlockType Inport + Name "w_H_b" + SID "4213" + Position [-10, -167, 20, -153] + ZOrder 561 + Port "3" + } + Block { + BlockType Inport + Name "feetContactStatus\n" + SID "4214" + Position [200, -147, 230, -133] + ZOrder 244 + Port "4" + } + Block { + BlockType SubSystem + Name "Compute Base Velocity" + SID "4215" + Ports [4, 1] + Position [310, -192, 595, -108] + ZOrder 239 + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Compute Base Velocity" + Location [19, 45, 910, 1950] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "3848" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "J_l_sole" + SID "4215::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Inport + Name "J_r_sole" + SID "4215::1472" + Position [20, 136, 40, 154] + ZOrder 42 + Port "2" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "4215::1473" + Position [20, 171, 40, 189] + ZOrder 43 + Port "3" + } + Block { + BlockType Inport + Name "jointVel" + SID "4215::23" + Position [20, 206, 40, 224] + ZOrder 9 + Port "4" + } + Block { + BlockType Demux + Name " Demux " + SID "4215::3847" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 216 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4215::3846" + Tag "Stateflow S-Function 7" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 215 + FunctionName "sf_sfun" + Parameters "Reg" + PortCounts "[4 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "nu_b" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4215::3848" + Position [460, 241, 480, 259] + ZOrder 217 + } + Block { + BlockType Outport + Name "nu_b" + SID "4215::5" + Position [460, 101, 480, 119] + ZOrder -6 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 120 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 121 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 122 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 123 + SrcBlock "jointVel" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "nu_b" + ZOrder 124 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "nu_b" + DstPort 1 + } + Line { + ZOrder 125 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 126 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Feet Jacobians" + SID "4216" + Ports [2, 2] + Position [55, -191, 165, -149] + ZOrder -21 + RequestExecContextInheritance off + System { + Name "Feet Jacobians" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "527" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos" + SID "4217" + Position [585, 598, 615, 612] + ZOrder 673 + } + Block { + BlockType Inport + Name "w_H_b" + SID "4218" + Position [585, 483, 615, 497] + ZOrder 680 + Port "2" + } + Block { + BlockType Reference + Name "Jacobian l_sole" + SID "4219" + Ports [2, 1] + Position [745, 475, 905, 530] + ZOrder 687 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" + SourceType "Jacobian" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "Jacobian r_sole" + SID "4220" + Ports [2, 1] + Position [745, 565, 905, 620] + ZOrder 688 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" + SourceType "Jacobian" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.RIGHT_FOOT" + } + Block { + BlockType Outport + Name "J_l_sole" + SID "4221" + Position [935, 498, 965, 512] + ZOrder 676 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_r_sole" + SID "4222" + Position [935, 588, 965, 602] + ZOrder 677 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "Jacobian l_sole" + SrcPort 1 + DstBlock "J_l_sole" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "Jacobian r_sole" + SrcPort 1 + DstBlock "J_r_sole" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "w_H_b" + SrcPort 1 + Points [56, 0] + Branch { + ZOrder 4 + Points [0, 90] + DstBlock "Jacobian r_sole" + DstPort 1 + } + Branch { + ZOrder 5 + DstBlock "Jacobian l_sole" + DstPort 1 + } + } + Line { + ZOrder 6 + SrcBlock "jointPos" + SrcPort 1 + Points [82, 0] + Branch { + ZOrder 7 + Points [0, -90] + DstBlock "Jacobian l_sole" + DstPort 2 + } + Branch { + ZOrder 8 + DstBlock "Jacobian r_sole" + DstPort 2 + } + } + } + } + Block { + BlockType Mux + Name "Mux" + SID "4224" + Ports [2, 1] + Position [630, -182, 635, -53] + ZOrder -12 + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Outport + Name "nu" + SID "4225" + Position [660, -122, 690, -108] + ZOrder -15 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 20 + SrcBlock "jointVel" + SrcPort 1 + Points [44, 0] + Branch { + ZOrder 11 + DstBlock "Mux" + DstPort 2 + } + Branch { + ZOrder 3 + Points [0, -35] + DstBlock "Compute Base Velocity" + DstPort 4 + } + } + Line { + ZOrder 4 + SrcBlock "Mux" + SrcPort 1 + DstBlock "nu" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "feetContactStatus\n" + SrcPort 1 + DstBlock "Compute Base Velocity" + DstPort 3 + } + Line { + ZOrder 6 + SrcBlock "Compute Base Velocity" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + ZOrder 7 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "Feet Jacobians" + DstPort 1 + } + Line { + ZOrder 8 + SrcBlock "w_H_b" + SrcPort 1 + DstBlock "Feet Jacobians" + DstPort 2 + } + Line { + ZOrder 9 + SrcBlock "Feet Jacobians" + SrcPort 1 + DstBlock "Compute Base Velocity" + DstPort 1 + } + Line { + ZOrder 10 + SrcBlock "Feet Jacobians" + SrcPort 2 + DstBlock "Compute Base Velocity" + DstPort 2 + } + } + } + Block { + BlockType SubSystem + Name "Compute base to fixed link transform" + SID "4307" + Ports [3, 2] + Position [-455, 2653, -330, 2767] + ZOrder 902 + RequestExecContextInheritance off + System { + Name "Compute base to fixed link transform" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "257" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "neckPos" + SID "4980" + Position [50, -32, 80, -18] + ZOrder 957 + } + Block { + BlockType Inport + Name "jointPos" + SID "4310" + Position [50, 38, 80, 52] + ZOrder 902 + Port "2" + } + Block { + BlockType Inport + Name "IMU_meas" + SID "4311" + Position [245, 183, 275, 197] + ZOrder 903 + Port "3" + } + Block { + BlockType Constant + Name "Constant7" + SID "4244" + Position [100, 22, 130, 38] + ZOrder 895 + ShowName off + Value "eye(4)" + } + Block { + BlockType Goto + Name "Goto" + SID "4829" + Position [155, 182, 205, 198] + ZOrder 955 + ShowName off + GotoTag "jointPos" + TagVisibility "global" + } + Block { + BlockType SubSystem + Name "LFoot to base link transform " + SID "4245" + Ports [5, 1] + Position [455, 4, 600, 56] + ZOrder 893 + ForegroundColor "yellow" + RequestExecContextInheritance off + System { + Name "LFoot to base link transform " + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "neck_pos" + SID "4981" + Position [1000, 313, 1030, 327] + ZOrder 753 + } + Block { + BlockType Inport + Name "jointPos" + SID "4246" + Position [470, 227, 495, 243] + ZOrder 736 + Port "2" + } + Block { + BlockType Inport + Name "inertial" + SID "4247" + Position [1000, 212, 1030, 228] + ZOrder 741 + Port "3" + } + Block { + BlockType Inport + Name "w_H_link" + SID "4248" + Position [655, 132, 680, 148] + ZOrder 744 + Port "4" + } + Block { + BlockType Inport + Name "w_H_base_fixed" + SID "4249" + Position [470, 32, 495, 48] + ZOrder 745 + Port "5" + } + Block { + BlockType Reference + Name "Fixe d base to root link transform" + SID "4251" + Ports [2, 1] + Position [575, 190, 740, 250] + ZOrder 737 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.BASE" + } + Block { + BlockType Reference + Name "Fixed base to imu transform" + SID "4250" + Ports [2, 1] + Position [575, 25, 740, 85] + ZOrder 729 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.IMU" + } + Block { + BlockType SubSystem + Name "Get Base Rotation From IMU" + SID "4257" + Ports [6, 1] + Position [1315, 47, 1550, 343] + ZOrder 733 + HideAutomaticName off + LibraryVersion "1.30" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Get Base Rotation From IMU" + Location [219, 337, 814, 775] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "3839" + SIDPrevWatermark "9" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "imu_H_fixedLink" + SID "4257::112" + Position [20, 101, 40, 119] + ZOrder 82 + } + Block { + BlockType Inport + Name "imu_H_fixedLink_0" + SID "4257::113" + Position [20, 136, 40, 154] + ZOrder 83 + Port "2" + } + Block { + BlockType Inport + Name "fixedLink_H_base" + SID "4257::111" + Position [20, 171, 40, 189] + ZOrder 81 + Port "3" + } + Block { + BlockType Inport + Name "rpyFromIMU_0" + SID "4257::115" + Position [20, 206, 40, 224] + ZOrder 85 + Port "4" + } + Block { + BlockType Inport + Name "rpyFromIMU" + SID "4257::93" + Position [20, 246, 40, 264] + ZOrder 69 + Port "5" + } + Block { + BlockType Inport + Name "neck_pos" + SID "4257::118" + Position [20, 281, 40, 299] + ZOrder 86 + Port "6" + } + Block { + BlockType Demux + Name " Demux " + SID "4257::3838" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 238 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4257::3837" + Tag "Stateflow S-Function 8" + Ports [6, 2] + Position [180, 102, 230, 243] + ZOrder 237 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[6 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "w_H_b" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4257::3839" + Position [460, 241, 480, 259] + ZOrder 239 + } + Block { + BlockType Outport + Name "w_H_b" + SID "4257::106" + Position [460, 101, 480, 119] + ZOrder 80 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 154 + SrcBlock "imu_H_fixedLink" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 155 + SrcBlock "imu_H_fixedLink_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 156 + SrcBlock "fixedLink_H_base" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 157 + SrcBlock "rpyFromIMU_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 158 + SrcBlock "rpyFromIMU" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 159 + SrcBlock "neck_pos" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + Name "w_H_b" + ZOrder 160 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "w_H_b" + DstPort 1 + } + Line { + ZOrder 161 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 162 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Product + Name "Inv" + SID "4252" + Ports [2, 1] + Position [825, 40, 895, 95] + ZOrder 731 + ShowName off + HideAutomaticName off + Inputs "/*" + Multiplication "Matrix(*)" + Port { + PortNumber 1 + Name "imu_H_link" + } + } + Block { + BlockType Product + Name "Inv_2" + SID "4253" + Ports [2, 1] + Position [825, 184, 895, 231] + ZOrder 738 + ShowName off + HideAutomaticName off + Inputs "/*" + Multiplication "Matrix(*)" + Port { + PortNumber 1 + Name "link_H_root" + } + } + Block { + BlockType Selector + Name "Selector1" + SID "4849" + Ports [1, 1] + Position [1250, 260, 1275, 280] + ZOrder 749 + ShowName off + InputPortWidth "Ports.IMU_PORT_SIZE" + IndexOptions "Index vector (dialog)" + Indices "Ports.IMU_PORT_ORIENTATION_INDEX" + OutputSizes "1" + } + Block { + BlockType Selector + Name "Selector2" + SID "4848" + Ports [1, 1] + Position [1250, 210, 1275, 230] + ZOrder 747 + ShowName off + InputPortWidth "Ports.IMU_PORT_SIZE" + IndexOptions "Index vector (dialog)" + Indices "Ports.IMU_PORT_ORIENTATION_INDEX" + OutputSizes "1" + } + Block { + BlockType Switch + Name "Switch6" + SID "4255" + Position [1760, 151, 1840, 419] + ZOrder 734 + ShowName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Constant + Name "USE_IMU4EST_BASE" + SID "4256" + Position [1570, 275, 1725, 295] + ZOrder 735 + ShowName off + Value "Config.USE_IMU4EST_BASE" + } + Block { + BlockType Reference + Name "holder 1" + SID "4982" + Ports [1, 1] + Position [1150, 109, 1220, 131] + ZOrder 754 + LibraryVersion "3.11" + SourceBlock "utilityBlocks_lib/Holder" + SourceType "SubSystem" + SourceProductName "WholeBodyControllers" + ShowPortLabels "FromPortIcon" + SystemSampleTime "Config.tStep" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off + } + Block { + BlockType Reference + Name "holder 2" + SID "4983" + Ports [1, 1] + Position [1150, 209, 1220, 231] + ZOrder 755 + LibraryVersion "3.11" + SourceBlock "utilityBlocks_lib/Holder" + SourceType "SubSystem" + SourceProductName "WholeBodyControllers" + ShowPortLabels "FromPortIcon" + SystemSampleTime "Config.tStep" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off + } + Block { + BlockType SubSystem + Name "neck transform" + SID "4987" + Ports [1, 1] + Position [1125, 305, 1250, 335] + ZOrder 756 + RequestExecContextInheritance off + System { + Name "neck transform" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "413" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "neck port" + SID "4988" + Position [120, 18, 150, 32] + ZOrder 725 + NameLocation "top" + Port { + PortNumber 1 + Name "neck pos" + } + } + Block { + BlockType Constant + Name "Constant" + SID "4989" + Position [95, 85, 170, 105] + ZOrder 720 + ShowName off + Value "zeros(3,1)" + } + Block { + BlockType Switch + Name "Switch" + SID "4990" + Position [300, 7, 375, 113] + ZOrder 722 + ShowName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Constant + Name "USE_IMU4EST_BASE1" + SID "4991" + Position [60, 50, 210, 70] + ZOrder 719 + ShowName off + Value "Config.CORRECT_NECK_IMU" + } + Block { + BlockType Outport + Name "neck position" + SID "4992" + Position [425, 53, 455, 67] + ZOrder 726 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "USE_IMU4EST_BASE1" + SrcPort 1 + DstBlock "Switch" + DstPort 2 + } + Line { + ZOrder 2 + SrcBlock "Switch" + SrcPort 1 + DstBlock "neck position" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "Constant" + SrcPort 1 + DstBlock "Switch" + DstPort 3 + } + Line { + Name "neck pos" + ZOrder 4 + Labels [-1, 0] + SrcBlock "neck port" + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "link_H_b" + SID "4268" + Position [1875, 278, 1905, 292] + ZOrder 732 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "Switch6" + SrcPort 1 + DstBlock "link_H_b" + DstPort 1 + } + Line { + Name "link_H_root" + ZOrder 2 + Labels [0, 1] + SrcBlock "Inv_2" + SrcPort 1 + Points [65, 0] + Branch { + ZOrder 3 + Points [0, 165] + DstBlock "Switch6" + DstPort 3 + } + Branch { + ZOrder 4 + Points [0, -40] + DstBlock "Get Base Rotation From IMU" + DstPort 3 + } + } + Line { + ZOrder 5 + SrcBlock "USE_IMU4EST_BASE" + SrcPort 1 + DstBlock "Switch6" + DstPort 2 + } + Line { + Name "imu_H_link" + ZOrder 6 + Labels [0, 0] + SrcBlock "Inv" + SrcPort 1 + Points [65, 0] + Branch { + ZOrder 41 + Points [0, 50] + DstBlock "holder 1" + DstPort 1 + } + Branch { + ZOrder 7 + DstBlock "Get Base Rotation From IMU" + DstPort 1 + } + } + Line { + ZOrder 42 + SrcBlock "holder 2" + SrcPort 1 + DstBlock "Selector2" + DstPort 1 + } + Line { + ZOrder 10 + SrcBlock "Fixed base to imu transform" + SrcPort 1 + DstBlock "Inv" + DstPort 1 + } + Line { + ZOrder 12 + SrcBlock "jointPos" + SrcPort 1 + Points [42, 0] + Branch { + ZOrder 13 + Points [0, -165] + DstBlock "Fixed base to imu transform" + DstPort 2 + } + Branch { + ZOrder 14 + DstBlock "Fixe d base to root link transform" + DstPort 2 + } + } + Line { + ZOrder 15 + SrcBlock "Get Base Rotation From IMU" + SrcPort 1 + DstBlock "Switch6" + DstPort 1 + } + Line { + ZOrder 16 + SrcBlock "Fixe d base to root link transform" + SrcPort 1 + DstBlock "Inv_2" + DstPort 2 + } + Line { + ZOrder 17 + SrcBlock "inertial" + SrcPort 1 + Points [96, 0] + Branch { + ZOrder 64 + Points [0, 50] + DstBlock "Selector1" + DstPort 1 + } + Branch { + ZOrder 43 + DstBlock "holder 2" + DstPort 1 + } + } + Line { + ZOrder 36 + SrcBlock "neck_pos" + SrcPort 1 + DstBlock "neck transform" + DstPort 1 + } + Line { + ZOrder 22 + SrcBlock "w_H_link" + SrcPort 1 + Points [98, 0] + Branch { + ZOrder 23 + Points [0, 55] + DstBlock "Inv_2" + DstPort 1 + } + Branch { + ZOrder 24 + Points [0, -60] + DstBlock "Inv" + DstPort 2 + } + } + Line { + ZOrder 25 + SrcBlock "w_H_base_fixed" + SrcPort 1 + Points [57, 0] + Branch { + ZOrder 26 + Points [0, 165] + DstBlock "Fixe d base to root link transform" + DstPort 1 + } + Branch { + ZOrder 27 + DstBlock "Fixed base to imu transform" + DstPort 1 + } + } + Line { + ZOrder 28 + SrcBlock "Selector2" + SrcPort 1 + DstBlock "Get Base Rotation From IMU" + DstPort 4 + } + Line { + ZOrder 37 + SrcBlock "holder 1" + SrcPort 1 + DstBlock "Get Base Rotation From IMU" + DstPort 2 + } + Line { + ZOrder 31 + SrcBlock "Selector1" + SrcPort 1 + DstBlock "Get Base Rotation From IMU" + DstPort 5 + } + Line { + ZOrder 44 + SrcBlock "neck transform" + SrcPort 1 + DstBlock "Get Base Rotation From IMU" + DstPort 6 + } + } + } + Block { + BlockType SubSystem + Name "RFoot to base link transform" + SID "4850" + Ports [5, 1] + Position [455, 87, 600, 143] + ZOrder 956 + ForegroundColor "yellow" + RequestExecContextInheritance off + System { + Name "RFoot to base link transform" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "140" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "neck_pos" + SID "4986" + Position [1000, 313, 1030, 327] + ZOrder 754 + } + Block { + BlockType Inport + Name "jointPos" + SID "4851" + Position [470, 227, 495, 243] + ZOrder 736 + Port "2" + } + Block { + BlockType Inport + Name "inertial" + SID "4852" + Position [1000, 212, 1030, 228] + ZOrder 741 + Port "3" + } + Block { + BlockType Inport + Name "w_H_link" + SID "4853" + Position [655, 132, 680, 148] + ZOrder 744 + Port "4" + } + Block { + BlockType Inport + Name "w_H_base_fixed" + SID "4854" + Position [470, 32, 495, 48] + ZOrder 745 + Port "5" + } + Block { + BlockType Reference + Name "Fixed base to imu transform" + SID "4855" + Ports [2, 1] + Position [575, 25, 740, 85] + ZOrder 729 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.IMU" + } + Block { + BlockType Reference + Name "Fixed base to root link transform" + SID "4856" + Ports [2, 1] + Position [575, 190, 740, 250] + ZOrder 737 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.BASE" + } + Block { + BlockType SubSystem + Name "Get Base Rotation From IMU" + SID "4857" + Ports [6, 1] + Position [1315, 47, 1550, 343] + ZOrder 733 + HideAutomaticName off + LibraryVersion "1.30" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Get Base Rotation From IMU" + Location [219, 337, 814, 775] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "3839" + SIDPrevWatermark "9" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "imu_H_fixedLink" + SID "4857::112" + Position [20, 101, 40, 119] + ZOrder 82 + } + Block { + BlockType Inport + Name "imu_H_fixedLink_0" + SID "4857::113" + Position [20, 136, 40, 154] + ZOrder 83 + Port "2" + } + Block { + BlockType Inport + Name "fixedLink_H_base" + SID "4857::111" + Position [20, 171, 40, 189] + ZOrder 81 + Port "3" + } + Block { + BlockType Inport + Name "rpyFromIMU_0" + SID "4857::115" + Position [20, 206, 40, 224] + ZOrder 85 + Port "4" + } + Block { + BlockType Inport + Name "rpyFromIMU" + SID "4857::93" + Position [20, 246, 40, 264] + ZOrder 69 + Port "5" + } + Block { + BlockType Inport + Name "neck_pos" + SID "4857::118" + Position [20, 281, 40, 299] + ZOrder 86 + Port "6" + } + Block { + BlockType Demux + Name " Demux " + SID "4857::3838" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 238 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4857::3837" + Tag "Stateflow S-Function 3" + Ports [6, 2] + Position [180, 102, 230, 243] + ZOrder 237 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[6 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "w_H_b" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4857::3839" + Position [460, 241, 480, 259] + ZOrder 239 + } + Block { + BlockType Outport + Name "w_H_b" + SID "4857::106" + Position [460, 101, 480, 119] + ZOrder 80 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 154 + SrcBlock "imu_H_fixedLink" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 155 + SrcBlock "imu_H_fixedLink_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 156 + SrcBlock "fixedLink_H_base" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 157 + SrcBlock "rpyFromIMU_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 158 + SrcBlock "rpyFromIMU" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 159 + SrcBlock "neck_pos" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + Name "w_H_b" + ZOrder 160 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "w_H_b" + DstPort 1 + } + Line { + ZOrder 161 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 162 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Product + Name "Inv" + SID "4858" + Ports [2, 1] + Position [825, 40, 895, 95] + ZOrder 731 + ShowName off + HideAutomaticName off + Inputs "/*" + Multiplication "Matrix(*)" + Port { + PortNumber 1 + Name "imu_H_link" + } + } + Block { + BlockType Product + Name "Inv_2" + SID "4859" + Ports [2, 1] + Position [825, 184, 895, 231] + ZOrder 738 + ShowName off + HideAutomaticName off + Inputs "/*" + Multiplication "Matrix(*)" + Port { + PortNumber 1 + Name "link_H_root" + } + } + Block { + BlockType Selector + Name "Selector1" + SID "4861" + Ports [1, 1] + Position [1250, 260, 1275, 280] + ZOrder 749 + ShowName off + InputPortWidth "Ports.IMU_PORT_SIZE" + IndexOptions "Index vector (dialog)" + Indices "Ports.IMU_PORT_ORIENTATION_INDEX" + OutputSizes "1" + } + Block { + BlockType Selector + Name "Selector2" + SID "4862" + Ports [1, 1] + Position [1250, 210, 1275, 230] + ZOrder 747 + ShowName off + InputPortWidth "Ports.IMU_PORT_SIZE" + IndexOptions "Index vector (dialog)" + Indices "Ports.IMU_PORT_ORIENTATION_INDEX" + OutputSizes "1" + } + Block { + BlockType Switch + Name "Switch6" + SID "4863" + Position [1760, 151, 1840, 419] + ZOrder 734 + ShowName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Constant + Name "USE_IMU4EST_BASE" + SID "4864" + Position [1570, 275, 1725, 295] + ZOrder 735 + ShowName off + Value "Config.USE_IMU4EST_BASE" + } + Block { + BlockType Reference + Name "holder 3" + SID "4984" + Ports [1, 1] + Position [1170, 112, 1225, 128] + ZOrder 750 + LibraryVersion "3.11" + SourceBlock "utilityBlocks_lib/Holder" + SourceType "SubSystem" + SourceProductName "WholeBodyControllers" + ShowPortLabels "FromPortIcon" + SystemSampleTime "Config.tStep" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off + } + Block { + BlockType Reference + Name "holder 4" + SID "4985" + Ports [1, 1] + Position [1170, 212, 1225, 228] + ZOrder 751 + LibraryVersion "3.11" + SourceBlock "utilityBlocks_lib/Holder" + SourceType "SubSystem" + SourceProductName "WholeBodyControllers" + ShowPortLabels "FromPortIcon" + SystemSampleTime "Config.tStep" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off + } + Block { + BlockType SubSystem + Name "neck transform" + SID "4867" + Ports [1, 1] + Position [1135, 305, 1260, 335] + ZOrder 743 + RequestExecContextInheritance off + System { + Name "neck transform" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "413" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "neck port" + SID "4868" + Position [120, 18, 150, 32] + ZOrder 725 + NameLocation "top" + Port { + PortNumber 1 + Name "neck pos" + } + } + Block { + BlockType Constant + Name "Constant" + SID "4869" + Position [95, 85, 170, 105] + ZOrder 720 + ShowName off + Value "zeros(3,1)" + } + Block { + BlockType Switch + Name "Switch" + SID "4872" + Position [300, 7, 375, 113] + ZOrder 722 + ShowName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Constant + Name "USE_IMU4EST_BASE1" + SID "4873" + Position [60, 50, 210, 70] + ZOrder 719 + ShowName off + Value "Config.CORRECT_NECK_IMU" + } + Block { + BlockType Outport + Name "neck position" + SID "4874" + Position [425, 53, 455, 67] + ZOrder 726 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 2 + SrcBlock "USE_IMU4EST_BASE1" + SrcPort 1 + DstBlock "Switch" + DstPort 2 + } + Line { + ZOrder 3 + SrcBlock "Switch" + SrcPort 1 + DstBlock "neck position" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "Constant" + SrcPort 1 + DstBlock "Switch" + DstPort 3 + } + Line { + Name "neck pos" + ZOrder 8 + Labels [-1, 0] + SrcBlock "neck port" + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "link_H_b" + SID "4875" + Position [1875, 278, 1905, 292] + ZOrder 732 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "Switch6" + SrcPort 1 + DstBlock "link_H_b" + DstPort 1 + } + Line { + Name "link_H_root" + ZOrder 2 + Labels [0, 1] + SrcBlock "Inv_2" + SrcPort 1 + Points [65, 0] + Branch { + ZOrder 3 + Points [0, 165] + DstBlock "Switch6" + DstPort 3 + } + Branch { + ZOrder 4 + Points [0, -40] + DstBlock "Get Base Rotation From IMU" + DstPort 3 + } + } + Line { + ZOrder 5 + SrcBlock "USE_IMU4EST_BASE" + SrcPort 1 + DstBlock "Switch6" + DstPort 2 + } + Line { + Name "imu_H_link" + ZOrder 6 + Labels [0, 0] + SrcBlock "Inv" + SrcPort 1 + Points [65, 0] + Branch { + ZOrder 31 + Points [0, 50] + DstBlock "holder 3" + DstPort 1 + } + Branch { + ZOrder 7 + DstBlock "Get Base Rotation From IMU" + DstPort 1 + } + } + Line { + ZOrder 32 + SrcBlock "holder 4" + SrcPort 1 + DstBlock "Selector2" + DstPort 1 + } + Line { + ZOrder 10 + SrcBlock "Fixed base to imu transform" + SrcPort 1 + DstBlock "Inv" + DstPort 1 + } + Line { + ZOrder 11 + SrcBlock "jointPos" + SrcPort 1 + Points [42, 0] + Branch { + ZOrder 12 + Points [0, -165] + DstBlock "Fixed base to imu transform" + DstPort 2 + } + Branch { + ZOrder 13 + DstBlock "Fixed base to root link transform" + DstPort 2 + } + } + Line { + ZOrder 14 + SrcBlock "Get Base Rotation From IMU" + SrcPort 1 + DstBlock "Switch6" + DstPort 1 + } + Line { + ZOrder 15 + SrcBlock "Fixed base to root link transform" + SrcPort 1 + DstBlock "Inv_2" + DstPort 2 + } + Line { + ZOrder 16 + SrcBlock "inertial" + SrcPort 1 + Points [96, 0] + Branch { + ZOrder 33 + DstBlock "holder 4" + DstPort 1 + } + Branch { + ZOrder 18 + Points [0, 50] + DstBlock "Selector1" + DstPort 1 + } + } + Line { + ZOrder 35 + SrcBlock "neck_pos" + SrcPort 1 + DstBlock "neck transform" + DstPort 1 + } + Line { + ZOrder 20 + SrcBlock "neck transform" + SrcPort 1 + DstBlock "Get Base Rotation From IMU" + DstPort 6 + } + Line { + ZOrder 21 + SrcBlock "w_H_link" + SrcPort 1 + Points [98, 0] + Branch { + ZOrder 22 + Points [0, 55] + DstBlock "Inv_2" + DstPort 1 + } + Branch { + ZOrder 23 + Points [0, -60] + DstBlock "Inv" + DstPort 2 + } + } + Line { + ZOrder 24 + SrcBlock "w_H_base_fixed" + SrcPort 1 + Points [57, 0] + Branch { + ZOrder 25 + Points [0, 165] + DstBlock "Fixed base to root link transform" + DstPort 1 + } + Branch { + ZOrder 26 + DstBlock "Fixed base to imu transform" + DstPort 1 + } + } + Line { + ZOrder 27 + SrcBlock "Selector2" + SrcPort 1 + DstBlock "Get Base Rotation From IMU" + DstPort 4 + } + Line { + ZOrder 30 + SrcBlock "holder 3" + SrcPort 1 + DstBlock "Get Base Rotation From IMU" + DstPort 2 + } + Line { + ZOrder 29 + SrcBlock "Selector1" + SrcPort 1 + DstBlock "Get Base Rotation From IMU" + DstPort 5 + } + } + } + Block { + BlockType Reference + Name "Relative transform l_sole_H_base" + SID "4269" + Ports [2, 1] + Position [180, 22, 345, 53] + ZOrder 894 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "Relative transform r_sole_H_base" + SID "4306" + Ports [2, 1] + Position [180, 107, 345, 138] + ZOrder 899 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.RIGHT_FOOT" + } + Block { + BlockType Outport + Name "fixed_l_sole_H_b" + SID "4318" + Position [650, 23, 680, 37] + ZOrder 910 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "fixed_r_sole_H_b" + SID "4320" + Position [650, 108, 680, 122] + ZOrder 912 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "Constant7" + SrcPort 1 + Points [10, 0] + Branch { + ZOrder 2 + Points [0, -22; 255, 0; 0, 42] + Branch { + ZOrder 59 + Points [0, 85] + DstBlock "RFoot to base link transform" + DstPort 5 + } + Branch { + ZOrder 4 + DstBlock "LFoot to base link transform " + DstPort 5 + } + } + Branch { + ZOrder 5 + Points [0, 85] + DstBlock "Relative transform r_sole_H_base" + DstPort 1 + } + Branch { + ZOrder 6 + DstBlock "Relative transform l_sole_H_base" + DstPort 1 + } + } + Line { + ZOrder 35 + SrcBlock "RFoot to base link transform" + SrcPort 1 + DstBlock "fixed_r_sole_H_b" + DstPort 1 + } + Line { + ZOrder 8 + SrcBlock "LFoot to base link transform " + SrcPort 1 + DstBlock "fixed_l_sole_H_b" + DstPort 1 + } + Line { + ZOrder 9 + SrcBlock "Relative transform l_sole_H_base" + SrcPort 1 + DstBlock "LFoot to base link transform " + DstPort 4 + } + Line { + ZOrder 33 + SrcBlock "Relative transform r_sole_H_base" + SrcPort 1 + DstBlock "RFoot to base link transform" + DstPort 4 + } + Line { + ZOrder 11 + SrcBlock "jointPos" + SrcPort 1 + Points [51, 0] + Branch { + ZOrder 12 + Points [0, 85] + Branch { + ZOrder 13 + Points [0, 37] + Branch { + ZOrder 22 + Points [0, 23] + DstBlock "Goto" + DstPort 1 + } + Branch { + ZOrder 21 + Points [274, 0; 0, -62] + Branch { + ZOrder 70 + Points [0, -85] + DstBlock "LFoot to base link transform " + DstPort 2 + } + Branch { + ZOrder 31 + DstBlock "RFoot to base link transform" + DstPort 2 + } + } + } + Branch { + ZOrder 16 + DstBlock "Relative transform r_sole_H_base" + DstPort 2 + } + } + Branch { + ZOrder 17 + DstBlock "Relative transform l_sole_H_base" + DstPort 2 + } + } + Line { + ZOrder 18 + SrcBlock "IMU_meas" + SrcPort 1 + Points [104, 0; 0, -75] + Branch { + ZOrder 69 + Points [0, -85] + DstBlock "LFoot to base link transform " + DstPort 3 + } + Branch { + ZOrder 32 + DstBlock "RFoot to base link transform" + DstPort 3 + } + } + Line { + ZOrder 64 + SrcBlock "neckPos" + SrcPort 1 + Points [343, 0; 0, 35] + Branch { + ZOrder 74 + Points [0, 85] + DstBlock "RFoot to base link transform" + DstPort 1 + } + Branch { + ZOrder 73 + DstBlock "LFoot to base link transform " + DstPort 1 + } + } + } + } + Block { + BlockType Goto + Name "Goto3" + SID "4497" + Position [10, 2913, 65, 2927] + ZOrder 864 + ShowName off + HideAutomaticName off + GotoTag "state" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto5" + SID "4799" + Position [330, 2497, 380, 2513] + ZOrder 956 + ShowName off + GotoTag "nu" + TagVisibility "global" + } + Block { + BlockType SubSystem + Name "State Machine" + SID "2163" + Ports [5, 10] + Position [-275, 2597, -65, 2883] + ZOrder 506 + ForegroundColor "blue" + BackgroundColor "cyan" + Priority "-100" + RequestExecContextInheritance off + System { + Name "State Machine" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "142" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos" + SID "3166" + Position [-575, 263, -545, 277] + ZOrder 676 + } + Block { + BlockType Inport + Name "fixed_l_sole_H_b" + SID "4730" + Position [-575, 403, -545, 417] + ZOrder 902 + Port "2" + } + Block { + BlockType Inport + Name "fixed_r_sole_H_b" + SID "4731" + Position [-575, 438, -545, 452] + ZOrder 903 + Port "3" + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "4876" + Position [-375, 333, -345, 347] + ZOrder 955 + Port "4" + } + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "4877" + Position [-375, 368, -345, 382] + ZOrder 956 + Port "5" + } + Block { + BlockType Demux + Name "Demux1" + SID "3690" + Ports [1, 3] + Position [475, 248, 480, 362] + ZOrder 695 + ShowName off + Outputs "[ROBOT_DOF,3,3]" + } + Block { + BlockType From + Name "From" + SID "4999" + Position [-140, 298, -100, 312] + ZOrder 960 + ForegroundColor "blue" + BackgroundColor "cyan" + GotoTag "time" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto" + SID "4788" + Position [540, 148, 640, 162] + ZOrder 944 + ShowName off + GotoTag "feetContactStatus" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto1" + SID "4789" + Position [540, 183, 645, 197] + ZOrder 945 + ShowName off + GotoTag "KP_postural_diag" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto2" + SID "4496" + Position [355, 78, 405, 92] + ZOrder 928 + ShowName off + HideAutomaticName off + GotoTag "w_H_b" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto3" + SID "4792" + Position [-270, 348, -175, 362] + ZOrder 953 + ShowName off + GotoTag "wrench_leftFoot" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto4" + SID "4794" + Position [-270, 298, -175, 312] + ZOrder 954 + ShowName off + GotoTag "wrench_rightFoot" + TagVisibility "global" + } + Block { + BlockType Reference + Name "MinimumJerkTrajectoryGenerator" + SID "2176" + Ports [1, 1] + Position [330, 295, 430, 315] + ZOrder 594 + ShowName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + SourceType "MinimumJerkTrajectoryGenerator" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + externalSettlingTime off + settlingTime "Config.SmoothingTimeGainScheduling" + sampleTime "Config.tStep" + resetOnSettlingTime off + firstDerivatives off + secondDerivatives off + explicitInitialValue off + } + Block { + BlockType Mux + Name "Mux" + SID "3689" + Ports [3, 1] + Position [305, 250, 310, 360] + ZOrder 694 + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType SubSystem + Name "STATE MACHINE" + SID "2220" + Ports [10, 10] + Position [-65, 107, 250, 468] + ZOrder 497 + ForegroundColor "red" + LibraryVersion "1.30" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "STATE MACHINE" + Location [219, 337, 814, 775] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "3827" + SIDPrevWatermark "9" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "pos_CoM_0" + SID "2220::58" + Position [20, 101, 40, 119] + ZOrder 34 + } + Block { + BlockType Inport + Name "jointPos_0" + SID "2220::82" + Position [20, 136, 40, 154] + ZOrder 58 + Port "2" + } + Block { + BlockType Inport + Name "pos_CoM_fixed_l_sole" + SID "2220::92" + Position [20, 171, 40, 189] + ZOrder 68 + Port "3" + } + Block { + BlockType Inport + Name "pos_CoM_fixed_r_sole" + SID "2220::107" + Position [20, 206, 40, 224] + ZOrder 81 + Port "4" + } + Block { + BlockType Inport + Name "jointPos" + SID "2220::97" + Position [20, 246, 40, 264] + ZOrder 72 + Port "5" + } + Block { + BlockType Inport + Name "time" + SID "2220::84" + Position [20, 281, 40, 299] + ZOrder 60 + Port "6" + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "2220::93" + Position [20, 316, 40, 334] + ZOrder 69 + Port "7" + } + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "2220::103" + Position [20, 351, 40, 369] + ZOrder 77 + Port "8" + } + Block { + BlockType Inport + Name "l_sole_H_b" + SID "2220::101" + Position [20, 386, 40, 404] + ZOrder 75 + Port "9" + } + Block { + BlockType Inport + Name "r_sole_H_b" + SID "2220::108" + Position [20, 426, 40, 444] + ZOrder 82 + Port "10" + } + Block { + BlockType Demux + Name " Demux " + SID "2220::3826" + Ports [1, 1] + Position [270, 495, 320, 535] + ZOrder 224 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "2220::3825" + Tag "Stateflow S-Function 13" + Ports [10, 11] + Position [180, 100, 230, 340] + ZOrder 223 + FunctionName "sf_sfun" + Parameters "Config,Gain,StateMachine" + PortCounts "[10 11]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "w_H_b" + } + Port { + PortNumber 3 + Name "pos_CoM_des" + } + Port { + PortNumber 4 + Name "jointPos_des" + } + Port { + PortNumber 5 + Name "feetContactStatus" + } + Port { + PortNumber 6 + Name "KP_postural_diag" + } + Port { + PortNumber 7 + Name "KP_CoM_diag" + } + Port { + PortNumber 8 + Name "KD_CoM_diag" + } + Port { + PortNumber 9 + Name "state" + } + Port { + PortNumber 10 + Name "smoothingTimeJoints" + } + Port { + PortNumber 11 + Name "smoothingTimeCoM" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "2220::3827" + Position [460, 506, 480, 524] + ZOrder 225 + } + Block { + BlockType Outport + Name "w_H_b" + SID "2220::106" + Position [460, 101, 480, 119] + ZOrder 80 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "pos_CoM_des" + SID "2220::77" + Position [460, 136, 480, 154] + ZOrder 53 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "jointPos_des" + SID "2220::67" + Position [460, 171, 480, 189] + ZOrder 43 + Port "3" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "feetContactStatus" + SID "2220::64" + Position [460, 206, 480, 224] + ZOrder 40 + Port "4" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_postural_diag" + SID "2220::94" + Position [460, 246, 480, 264] + ZOrder 70 + Port "5" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_CoM_diag" + SID "2220::112" + Position [460, 281, 480, 299] + ZOrder 84 + Port "6" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KD_CoM_diag" + SID "2220::113" + Position [460, 316, 480, 334] + ZOrder 85 + Port "7" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "state" + SID "2220::96" + Position [460, 351, 480, 369] + ZOrder 71 + Port "8" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "smoothingTimeJoints" + SID "2220::111" + Position [460, 386, 480, 404] + ZOrder 83 + Port "9" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "smoothingTimeCoM" + SID "2220::3776" + Position [460, 426, 480, 444] + ZOrder 174 + Port "10" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1168 + SrcBlock "pos_CoM_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 1169 + SrcBlock "jointPos_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 1170 + SrcBlock "pos_CoM_fixed_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 1171 + SrcBlock "pos_CoM_fixed_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 1172 + SrcBlock "jointPos" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 1173 + SrcBlock "time" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + ZOrder 1174 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock " SFunction " + DstPort 7 + } + Line { + ZOrder 1175 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock " SFunction " + DstPort 8 + } + Line { + ZOrder 1176 + SrcBlock "l_sole_H_b" + SrcPort 1 + DstBlock " SFunction " + DstPort 9 + } + Line { + ZOrder 1177 + SrcBlock "r_sole_H_b" + SrcPort 1 + DstBlock " SFunction " + DstPort 10 + } + Line { + Name "w_H_b" + ZOrder 1178 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "w_H_b" + DstPort 1 + } + Line { + Name "pos_CoM_des" + ZOrder 1179 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "pos_CoM_des" + DstPort 1 + } + Line { + Name "jointPos_des" + ZOrder 1180 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "jointPos_des" + DstPort 1 + } + Line { + Name "feetContactStatus" + ZOrder 1181 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "feetContactStatus" + DstPort 1 + } + Line { + Name "KP_postural_diag" + ZOrder 1182 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "KP_postural_diag" + DstPort 1 + } + Line { + Name "KP_CoM_diag" + ZOrder 1183 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 7 + DstBlock "KP_CoM_diag" + DstPort 1 + } + Line { + Name "KD_CoM_diag" + ZOrder 1184 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 8 + DstBlock "KD_CoM_diag" + DstPort 1 + } + Line { + Name "state" + ZOrder 1185 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 9 + DstBlock "state" + DstPort 1 + } + Line { + Name "smoothingTimeJoints" + ZOrder 1186 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 10 + DstBlock "smoothingTimeJoints" + DstPort 1 + } + Line { + Name "smoothingTimeCoM" + ZOrder 1187 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 11 + DstBlock "smoothingTimeCoM" + DstPort 1 + } + Line { + ZOrder 1188 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 1189 + SrcBlock " SFunction " + SrcPort 1 + Points [20, 0] + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "holder 5" + SID "4993" + Ports [1, 1] + Position [-170, 122, -115, 138] + ZOrder 957 + LibraryVersion "3.11" + SourceBlock "utilityBlocks_lib/Holder" + SourceType "SubSystem" + SourceProductName "WholeBodyControllers" + ShowPortLabels "FromPortIcon" + SystemSampleTime "Config.tStep" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off + } + Block { + BlockType Reference + Name "holder 6" + SID "4994" + Ports [1, 1] + Position [-170, 157, -115, 173] + ZOrder 958 + LibraryVersion "3.11" + SourceBlock "utilityBlocks_lib/Holder" + SourceType "SubSystem" + SourceProductName "WholeBodyControllers" + ShowPortLabels "FromPortIcon" + SystemSampleTime "Config.tStep" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off + } + Block { + BlockType SubSystem + Name "xCom" + SID "4226" + Ports [2, 1] + Position [-385, 184, -235, 211] + ZOrder 890 + ShowName off + RequestExecContextInheritance off + System { + Name "xCom" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "464" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "w_H_b" + SID "4227" + Position [30, 237, 60, 253] + ZOrder 581 + NameLocation "top" + } + Block { + BlockType Inport + Name "jointPos" + SID "4228" + Position [30, 267, 60, 283] + ZOrder 582 + Port "2" + } + Block { + BlockType Reference + Name "CoM" + SID "4229" + Ports [2, 1] + Position [110, 230, 275, 290] + ZOrder 578 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.COM" + } + Block { + BlockType Selector + Name "CoM6D -> \nCoMXYZ2" + SID "4230" + Ports [1, 1] + Position [305, 241, 345, 279] + ZOrder 580 + ShowName off + NumberOfDimensions "2" + InputPortWidth "7" + IndexOptions "Index vector (dialog),Index vector (dialog)" + Indices "[1 2 3],[4]" + OutputSizes "1,1" + } + Block { + BlockType Outport + Name "pos_CoM_fixed_l_sole " + SID "4231" + Position [400, 253, 430, 267] + ZOrder 583 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "CoM" + DstPort 2 + } + Line { + ZOrder 2 + SrcBlock "CoM6D -> \nCoMXYZ2" + SrcPort 1 + DstBlock "pos_CoM_fixed_l_sole " + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "CoM" + SrcPort 1 + DstBlock "CoM6D -> \nCoMXYZ2" + DstPort 1 + } + Line { + ZOrder 4 + SrcBlock "w_H_b" + SrcPort 1 + DstBlock "CoM" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "xCom2" + SID "4732" + Ports [2, 1] + Position [-385, 219, -235, 246] + ZOrder 904 + ShowName off + RequestExecContextInheritance off + System { + Name "xCom2" + Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "823" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "w_H_b" + SID "4733" + Position [30, 237, 60, 253] + ZOrder 581 + NameLocation "top" + } + Block { + BlockType Inport + Name "jointPos" + SID "4734" + Position [30, 267, 60, 283] + ZOrder 582 + Port "2" + } + Block { + BlockType Reference + Name "CoM" + SID "4735" + Ports [2, 1] + Position [110, 230, 275, 290] + ZOrder 578 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + frameName "Frames.COM" + } + Block { + BlockType Selector + Name "CoM6D -> \nCoMXYZ2" + SID "4736" + Ports [1, 1] + Position [305, 241, 345, 279] + ZOrder 580 + ShowName off + NumberOfDimensions "2" + InputPortWidth "7" + IndexOptions "Index vector (dialog),Index vector (dialog)" + Indices "[1 2 3],[4]" + OutputSizes "1,1" + } + Block { + BlockType Outport + Name "pos_CoM_fixed_r_sole " + SID "4737" + Position [400, 253, 430, 267] + ZOrder 583 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "CoM" + DstPort 2 + } + Line { + ZOrder 2 + SrcBlock "CoM6D -> \nCoMXYZ2" + SrcPort 1 + DstBlock "pos_CoM_fixed_r_sole " + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "CoM" + SrcPort 1 + DstBlock "CoM6D -> \nCoMXYZ2" + DstPort 1 + } + Line { + ZOrder 4 + SrcBlock "w_H_b" + SrcPort 1 + DstBlock "CoM" + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "w_H_b" + SID "2241" + Position [365, 124, 395, 136] + ZOrder 578 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "feetContactStatus" + SID "2236" + Position [580, 229, 610, 241] + ZOrder 480 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "pos_CoM_des" + SID "2234" + Position [365, 158, 395, 172] + ZOrder 478 + Port "3" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "jointPos_des" + SID "2235" + Position [365, 193, 395, 207] + ZOrder 479 + Port "4" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_postural_diag" + SID "2237" + Position [580, 257, 615, 273] + ZOrder 545 + Port "5" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "smoothingTime_Joints" + SID "3181" + Position [365, 403, 395, 417] + ZOrder 680 + Port "6" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_CoM_diag" + SID "3316" + Position [580, 297, 615, 313] + ZOrder 689 + Port "7" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KD_CoM_diag" + SID "3318" + Position [580, 335, 615, 355] + ZOrder 691 + Port "8" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "smoothingTime_CoM" + SID "4738" + Position [365, 438, 395, 452] + ZOrder 905 + Port "9" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "state" + SID "2238" + Position [365, 367, 395, 383] + ZOrder 550 + Port "10" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 173 + SrcBlock "xCom" + SrcPort 1 + Points [13, 0] + Branch { + ZOrder 513 + Points [0, -70] + DstBlock "holder 5" + DstPort 1 + } + Branch { + ZOrder 415 + DstBlock "STATE MACHINE" + DstPort 3 + } + } + Line { + ZOrder 10 + SrcBlock "STATE MACHINE" + SrcPort 3 + DstBlock "jointPos_des" + DstPort 1 + } + Line { + ZOrder 377 + SrcBlock "STATE MACHINE" + SrcPort 2 + DstBlock "pos_CoM_des" + DstPort 1 + } + Line { + ZOrder 14 + SrcBlock "STATE MACHINE" + SrcPort 8 + DstBlock "state" + DstPort 1 + } + Line { + ZOrder 15 + SrcBlock "STATE MACHINE" + SrcPort 4 + Points [218, 0] + Branch { + ZOrder 459 + DstBlock "feetContactStatus" + DstPort 1 + } + Branch { + ZOrder 458 + Points [0, -80] + DstBlock "Goto" + DstPort 1 + } + } + Line { + ZOrder 53 + SrcBlock "STATE MACHINE" + SrcPort 9 + DstBlock "smoothingTime_Joints" + DstPort 1 + } + Line { + ZOrder 60 + SrcBlock "STATE MACHINE" + SrcPort 6 + DstBlock "Mux" + DstPort 2 + } + Line { + ZOrder 61 + SrcBlock "Demux1" + SrcPort 3 + DstBlock "KD_CoM_diag" + DstPort 1 + } + Line { + ZOrder 62 + SrcBlock "STATE MACHINE" + SrcPort 7 + DstBlock "Mux" + DstPort 3 + } + Line { + ZOrder 63 + SrcBlock "STATE MACHINE" + SrcPort 5 + DstBlock "Mux" + DstPort 1 + } + Line { + ZOrder 64 + SrcBlock "Mux" + SrcPort 1 + DstBlock "MinimumJerkTrajectoryGenerator" + DstPort 1 + } + Line { + ZOrder 65 + SrcBlock "MinimumJerkTrajectoryGenerator" + SrcPort 1 + DstBlock "Demux1" + DstPort 1 + } + Line { + ZOrder 66 + SrcBlock "Demux1" + SrcPort 1 + Points [17, 0] + Branch { + ZOrder 456 + Points [0, -75] + DstBlock "Goto1" + DstPort 1 + } + Branch { + ZOrder 455 + DstBlock "KP_postural_diag" + DstPort 1 + } + } + Line { + ZOrder 67 + SrcBlock "Demux1" + SrcPort 2 + DstBlock "KP_CoM_diag" + DstPort 1 + } + Line { + ZOrder 32 + SrcBlock "jointPos" + SrcPort 1 + Points [54, 0] + Branch { + ZOrder 420 + DstBlock "STATE MACHINE" + DstPort 5 + } + Branch { + ZOrder 419 + Points [0, -30] + Branch { + ZOrder 413 + Points [0, -35] + Branch { + ZOrder 515 + Points [0, -40] + DstBlock "holder 6" + DstPort 1 + } + Branch { + ZOrder 262 + DstBlock "xCom" + DstPort 2 + } + } + Branch { + ZOrder 256 + DstBlock "xCom2" + DstPort 2 + } + } + } + Line { + ZOrder 368 + SrcBlock "fixed_l_sole_H_b" + SrcPort 1 + Points [83, 0] + Branch { + ZOrder 426 + DstBlock "STATE MACHINE" + DstPort 9 + } + Branch { + ZOrder 424 + Points [0, -220] + DstBlock "xCom" + DstPort 1 + } + } + Line { + ZOrder 369 + SrcBlock "fixed_r_sole_H_b" + SrcPort 1 + Points [105, 0] + Branch { + ZOrder 429 + Points [0, -220] + DstBlock "xCom2" + DstPort 1 + } + Branch { + ZOrder 428 + DstBlock "STATE MACHINE" + DstPort 10 + } + } + Line { + ZOrder 500 + SrcBlock "wrench_leftFoot" + SrcPort 1 + Points [35, 0] + Branch { + ZOrder 465 + Points [0, -20] + DstBlock "Goto3" + DstPort 1 + } + Branch { + ZOrder 464 + DstBlock "STATE MACHINE" + DstPort 8 + } + } + Line { + ZOrder 514 + SrcBlock "holder 5" + SrcPort 1 + DstBlock "STATE MACHINE" + DstPort 1 + } + Line { + ZOrder 516 + SrcBlock "holder 6" + SrcPort 1 + DstBlock "STATE MACHINE" + DstPort 2 + } + Line { + ZOrder 418 + SrcBlock "xCom2" + SrcPort 1 + DstBlock "STATE MACHINE" + DstPort 4 + } + Line { + ZOrder 430 + SrcBlock "STATE MACHINE" + SrcPort 10 + DstBlock "smoothingTime_CoM" + DstPort 1 + } + Line { + ZOrder 376 + SrcBlock "STATE MACHINE" + SrcPort 1 + Points [42, 0] + Branch { + ZOrder 454 + Points [0, -45] + DstBlock "Goto2" + DstPort 1 + } + Branch { + ZOrder 453 + DstBlock "w_H_b" + DstPort 1 + } + } + Line { + ZOrder 499 + SrcBlock "wrench_rightFoot" + SrcPort 1 + Points [37, 0] + Branch { + ZOrder 463 + Points [0, -35] + DstBlock "Goto4" + DstPort 1 + } + Branch { + ZOrder 462 + DstBlock "STATE MACHINE" + DstPort 7 + } + } + Line { + ZOrder 561 + SrcBlock "From" + SrcPort 1 + DstBlock "STATE MACHINE" + DstPort 6 + } + } + } + Block { + BlockType SubSystem + Name "Update Gains and References" + SID "4030" + Ports [7, 5] + Position [80, 2650, 285, 2860] + ZOrder 702 + RequestExecContextInheritance off + System { + Name "Update Gains and References" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "329" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "pos_CoM_des" + SID "4743" + Position [315, -137, 345, -123] + ZOrder 909 + } + Block { + BlockType Inport + Name "jointPos_des" + SID "4742" + Position [315, -252, 345, -238] + ZOrder 908 + Port "2" + } + Block { + BlockType Inport + Name "KP_postural_diag" + SID "4748" + Position [315, -17, 345, -3] + ZOrder 914 + Port "3" + } + Block { + BlockType Inport + Name "smoothingTime_Joints" + SID "4741" + Position [315, -222, 345, -208] + ZOrder 907 + Port "4" + } + Block { + BlockType Inport + Name "KP_CoM_diag" + SID "4739" + Position [315, 13, 345, 27] + ZOrder 905 + Port "5" + } + Block { + BlockType Inport + Name "KD_CoM_diag" + SID "4740" + Position [315, 43, 345, 57] + ZOrder 906 + Port "6" + } + Block { + BlockType Inport + Name "smoothingTime_CoM" + SID "4033" + Position [315, -102, 345, -88] + ZOrder 704 + Port "7" + } + Block { + BlockType Goto + Name "Goto4" + SID "4498" + Position [815, -273, 895, -257] + ZOrder 928 + ShowName off + HideAutomaticName off + GotoTag "jointPos_des" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto7" + SID "4501" + Position [815, -164, 895, -146] + ZOrder 934 + ShowName off + HideAutomaticName off + GotoTag "pos_CoM_des" + TagVisibility "global" + } + Block { + BlockType SubSystem + Name "Reshape Gains Matrices" + SID "4774" + Ports [3, 3] + Position [455, -26, 655, 66] + ZOrder 933 + RequestExecContextInheritance off + System { + Name "Reshape Gains Matrices" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "532" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "KP_postural_diag" + SID "4775" + Position [20, -82, 50, -68] + ZOrder 915 + } + Block { + BlockType Inport + Name "KP_CoM_diag" + SID "4776" + Position [20, -12, 50, 2] + ZOrder 916 + Port "2" + } + Block { + BlockType Inport + Name "KD_CoM_diag" + SID "4777" + Position [20, 63, 50, 77] + ZOrder 917 + Port "3" + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "4782" + Ports [1, 1] + Position [145, -103, 330, -47] + ZOrder 921 + ShowName off + HideAutomaticName off + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + ContentPreviewEnabled on + System { + Name "MATLAB Function" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "72" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "d" + SID "4782::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Demux + Name " Demux " + SID "4782::71" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 62 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4782::70" + Tag "Stateflow S-Function 2" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 61 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "D" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4782::72" + Position [460, 241, 480, 259] + ZOrder 63 + } + Block { + BlockType Outport + Name "D" + SID "4782::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 73 + SrcBlock "d" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "D" + ZOrder 74 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "D" + DstPort 1 + } + Line { + ZOrder 75 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 76 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "MATLAB Function1" + SID "4783" + Ports [1, 1] + Position [145, -33, 330, 23] + ZOrder 922 + ShowName off + HideAutomaticName off + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + ContentPreviewEnabled on + System { + Name "MATLAB Function1" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "72" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "d" + SID "4783::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Demux + Name " Demux " + SID "4783::71" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 62 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4783::70" + Tag "Stateflow S-Function 1" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 61 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "D" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4783::72" + Position [460, 241, 480, 259] + ZOrder 63 + } + Block { + BlockType Outport + Name "D" + SID "4783::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 69 + SrcBlock "d" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "D" + ZOrder 70 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "D" + DstPort 1 + } + Line { + ZOrder 71 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 72 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "MATLAB Function2" + SID "4784" + Ports [1, 1] + Position [145, 42, 330, 98] + ZOrder 923 + ShowName off + HideAutomaticName off + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + ContentPreviewEnabled on + System { + Name "MATLAB Function2" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "72" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "d" + SID "4784::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Demux + Name " Demux " + SID "4784::71" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 62 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4784::70" + Tag "Stateflow S-Function 4" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 61 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "D" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4784::72" + Position [460, 241, 480, 259] + ZOrder 63 + } + Block { + BlockType Outport + Name "D" + SID "4784::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 69 + SrcBlock "d" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "D" + ZOrder 70 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "D" + DstPort 1 + } + Line { + ZOrder 71 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 72 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "KP_postural" + SID "4778" + Position [420, -82, 450, -68] + ZOrder 918 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_CoM" + SID "4779" + Position [420, -12, 450, 2] + ZOrder 919 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KD_CoM" + SID "4780" + Position [420, 63, 450, 77] + ZOrder 920 + Port "3" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "KP_postural_diag" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "KP_CoM_diag" + SrcPort 1 + DstBlock "MATLAB Function1" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "KD_CoM_diag" + SrcPort 1 + DstBlock "MATLAB Function2" + DstPort 1 + } + Line { + ZOrder 4 + SrcBlock "MATLAB Function" + SrcPort 1 + DstBlock "KP_postural" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "MATLAB Function1" + SrcPort 1 + DstBlock "KP_CoM" + DstPort 1 + } + Line { + ZOrder 6 + SrcBlock "MATLAB Function2" + SrcPort 1 + DstBlock "KD_CoM" + DstPort 1 + } + } + } + Block { + BlockType Selector + Name "Selector1" + SID "4502" + Ports [1, 1] + Position [750, -164, 775, -146] + ZOrder 935 + ShowName off + HideAutomaticName off + NumberOfDimensions "2" + InputPortWidth "3" + IndexOptions "Index vector (dialog),Index vector (dialog)" + Indices "[1 2 3],[1]" + OutputSizes "1,1" + } + Block { + BlockType SubSystem + Name "Smooth reference CoM" + SID "4767" + Ports [2, 1] + Position [455, -147, 660, -78] + ZOrder 932 + RequestExecContextInheritance off + System { + Name "Smooth reference CoM" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "245" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "pos_CoM_des" + SID "4768" + Position [55, -257, 85, -243] + ZOrder 910 + } + Block { + BlockType Inport + Name "smoothingTime_CoM" + SID "4771" + Position [55, -202, 85, -188] + ZOrder 913 + Port "2" + } + Block { + BlockType Constant + Name "Constant" + SID "4882" + Position [265, -78, 320, -42] + ZOrder 916 + Value "zeros(6,1)" + } + Block { + BlockType Reference + Name "MinimumJerkTrajectoryGenerator2" + SID "2153" + Ports [2, 3] + Position [215, -278, 365, -172] + ZOrder 597 + ShowName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + SourceType "MinimumJerkTrajectoryGenerator" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + externalSettlingTime on + settlingTime "0.01" + sampleTime "Config.tStep" + resetOnSettlingTime off + firstDerivatives on + secondDerivatives on + explicitInitialValue off + } + Block { + BlockType Mux + Name "Mux" + SID "2154" + Ports [3, 1] + Position [445, -279, 450, -171] + ZOrder 486 + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType Mux + Name "Mux1" + SID "4883" + Ports [2, 1] + Position [445, -142, 450, -33] + ZOrder 917 + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Reshape + Name "Reshape" + SID "2159" + Ports [1, 1] + Position [845, -175, 890, -135] + ZOrder 456 + ShowName off + OutputDimensionality "Customize" + OutputDimensions "[3,3]" + } + Block { + BlockType Constant + Name "SMOOTH_DES_COM" + SID "2160" + Position [515, -165, 655, -145] + ZOrder 488 + ShowName off + Value "Config.SMOOTH_COM_DES" + } + Block { + BlockType Switch + Name "Switch3" + SID "2246" + Position [695, -263, 785, -47] + ZOrder 487 + ShowName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "desired_pos_vel_acc_CoM" + SID "4773" + Position [965, -162, 995, -148] + ZOrder 915 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 615 + SrcBlock "smoothingTime_CoM" + SrcPort 1 + DstBlock "MinimumJerkTrajectoryGenerator2" + DstPort 2 + } + Line { + ZOrder 11 + SrcBlock "Switch3" + SrcPort 1 + DstBlock "Reshape" + DstPort 1 + } + Line { + ZOrder 614 + SrcBlock "pos_CoM_des" + SrcPort 1 + Points [85, 0] + Branch { + ZOrder 629 + Points [0, 135] + DstBlock "Mux1" + DstPort 1 + } + Branch { + ZOrder 628 + DstBlock "MinimumJerkTrajectoryGenerator2" + DstPort 1 + } + } + Line { + ZOrder 12 + SrcBlock "SMOOTH_DES_COM" + SrcPort 1 + DstBlock "Switch3" + DstPort 2 + } + Line { + ZOrder 10 + SrcBlock "MinimumJerkTrajectoryGenerator2" + SrcPort 3 + DstBlock "Mux" + DstPort 3 + } + Line { + ZOrder 83 + SrcBlock "Mux" + SrcPort 1 + DstBlock "Switch3" + DstPort 1 + } + Line { + ZOrder 9 + SrcBlock "MinimumJerkTrajectoryGenerator2" + SrcPort 2 + DstBlock "Mux" + DstPort 2 + } + Line { + ZOrder 613 + SrcBlock "Reshape" + SrcPort 1 + DstBlock "desired_pos_vel_acc_CoM" + DstPort 1 + } + Line { + ZOrder 8 + SrcBlock "MinimumJerkTrajectoryGenerator2" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + ZOrder 626 + SrcBlock "Mux1" + SrcPort 1 + DstBlock "Switch3" + DstPort 3 + } + Line { + ZOrder 627 + SrcBlock "Constant" + SrcPort 1 + DstBlock "Mux1" + DstPort 2 + } + } + } + Block { + BlockType SubSystem + Name "Smooth reference joint position" + SID "4761" + Ports [2, 1] + Position [455, -262, 660, -198] + ZOrder 931 + HideAutomaticName off + RequestExecContextInheritance off + System { + Name "Smooth reference joint position" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "292" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos_des" + SID "4762" + Position [-65, -97, -35, -83] + ZOrder 905 + } + Block { + BlockType Inport + Name "smoothingTime_Joints" + SID "4763" + Position [-65, -42, -35, -28] + ZOrder 906 + Port "2" + } + Block { + BlockType Goto + Name "Go To \"Compute Joint Torques\"" + SID "4722" + Position [300, -35, 385, -25] + ZOrder 904 + HideAutomaticName off + GotoTag "jointAcc_des" + TagVisibility "global" + } + Block { + BlockType Reference + Name "MinimumJerkTrajectoryGenerator1" + SID "2152" + Ports [2, 3] + Position [90, -120, 240, -10] + ZOrder 598 + ShowName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + SourceType "MinimumJerkTrajectoryGenerator" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + externalSettlingTime on + settlingTime "0.01" + sampleTime "Config.tStep" + resetOnSettlingTime off + firstDerivatives on + secondDerivatives on + explicitInitialValue off + } + Block { + BlockType Constant + Name "SMOOTH_DES_COM2" + SID "2161" + Position [380, -78, 520, -62] + ZOrder 492 + ShowName off + Value "Config.SMOOTH_JOINT_DES" + } + Block { + BlockType Switch + Name "Switch5" + SID "2248" + Position [575, -113, 630, -27] + ZOrder 491 + ShowName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Terminator + Name "Terminator" + SID "4656" + Position [310, -71, 325, -59] + ZOrder 901 + ShowName off + HideAutomaticName off + } + Block { + BlockType Outport + Name "jointPos_des_smoothed" + SID "4766" + Position [685, -77, 715, -63] + ZOrder 909 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 13 + SrcBlock "SMOOTH_DES_COM2" + SrcPort 1 + DstBlock "Switch5" + DstPort 2 + } + Line { + ZOrder 608 + SrcBlock "Switch5" + SrcPort 1 + DstBlock "jointPos_des_smoothed" + DstPort 1 + } + Line { + ZOrder 604 + SrcBlock "jointPos_des" + SrcPort 1 + Points [74, 0] + Branch { + ZOrder 612 + DstBlock "MinimumJerkTrajectoryGenerator1" + DstPort 1 + } + Branch { + ZOrder 611 + Points [0, 132; 400, 0; 0, -82] + DstBlock "Switch5" + DstPort 3 + } + } + Line { + ZOrder 605 + SrcBlock "smoothingTime_Joints" + SrcPort 1 + DstBlock "MinimumJerkTrajectoryGenerator1" + DstPort 2 + } + Line { + ZOrder 90 + SrcBlock "MinimumJerkTrajectoryGenerator1" + SrcPort 1 + DstBlock "Switch5" + DstPort 1 + } + Line { + ZOrder 609 + SrcBlock "MinimumJerkTrajectoryGenerator1" + SrcPort 3 + DstBlock "Go To \"Compute Joint Torques\"" + DstPort 1 + } + Line { + ZOrder 562 + SrcBlock "MinimumJerkTrajectoryGenerator1" + SrcPort 2 + DstBlock "Terminator" + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "KP_postural" + SID "4034" + Position [745, -17, 775, -3] + ZOrder 705 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_CoM" + SID "4035" + Position [745, 13, 775, 27] + ZOrder 706 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KD_CoM" + SID "4036" + Position [745, 43, 775, 57] + ZOrder 707 + Port "3" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "desired_pos_vel_acc_CoM" + SID "4041" + Position [745, -117, 775, -103] + ZOrder 712 + Port "4" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "jointPos_des_smoothed" + SID "4042" + Position [745, -237, 775, -223] + ZOrder 713 + Port "5" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 604 + SrcBlock "Smooth reference joint position" + SrcPort 1 + Points [34, 0] + Branch { + ZOrder 623 + Points [0, -35] + DstBlock "Goto4" + DstPort 1 + } + Branch { + ZOrder 622 + DstBlock "jointPos_des_smoothed" + DstPort 1 + } + } + Line { + ZOrder 605 + SrcBlock "jointPos_des" + SrcPort 1 + DstBlock "Smooth reference joint position" + DstPort 1 + } + Line { + ZOrder 606 + SrcBlock "smoothingTime_Joints" + SrcPort 1 + DstBlock "Smooth reference joint position" + DstPort 2 + } + Line { + ZOrder 610 + SrcBlock "pos_CoM_des" + SrcPort 1 + DstBlock "Smooth reference CoM" + DstPort 1 + } + Line { + ZOrder 611 + SrcBlock "smoothingTime_CoM" + SrcPort 1 + DstBlock "Smooth reference CoM" + DstPort 2 + } + Line { + ZOrder 612 + SrcBlock "Smooth reference CoM" + SrcPort 1 + Points [26, 0] + Branch { + ZOrder 621 + Points [0, -45] + DstBlock "Selector1" + DstPort 1 + } + Branch { + ZOrder 620 + DstBlock "desired_pos_vel_acc_CoM" + DstPort 1 + } + } + Line { + ZOrder 613 + SrcBlock "KP_postural_diag" + SrcPort 1 + DstBlock "Reshape Gains Matrices" + DstPort 1 + } + Line { + ZOrder 614 + SrcBlock "KP_CoM_diag" + SrcPort 1 + DstBlock "Reshape Gains Matrices" + DstPort 2 + } + Line { + ZOrder 615 + SrcBlock "KD_CoM_diag" + SrcPort 1 + DstBlock "Reshape Gains Matrices" + DstPort 3 + } + Line { + ZOrder 616 + SrcBlock "Reshape Gains Matrices" + SrcPort 1 + DstBlock "KP_postural" + DstPort 1 + } + Line { + ZOrder 617 + SrcBlock "Reshape Gains Matrices" + SrcPort 2 + DstBlock "KP_CoM" + DstPort 1 + } + Line { + ZOrder 618 + SrcBlock "Reshape Gains Matrices" + SrcPort 3 + DstBlock "KD_CoM" + DstPort 1 + } + Line { + ZOrder 619 + SrcBlock "Selector1" + SrcPort 1 + DstBlock "Goto7" + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "w_H_b" + SID "2269" + Position [25, 2524, 55, 2536] + ZOrder 578 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "nu" + SID "2271" + Position [340, 2539, 370, 2551] + ZOrder 577 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "state" + SID "3201" + Position [25, 2870, 55, 2880] + ZOrder 647 + Port "3" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "feetContactStatus" + SID "2272" + Position [80, 2565, 110, 2575] + ZOrder 410 + Port "4" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_postural" + SID "2268" + Position [340, 2670, 370, 2680] + ZOrder 517 + Port "5" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_CoM" + SID "3330" + Position [340, 2710, 370, 2720] + ZOrder 692 + Port "6" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KD_CoM" + SID "3331" + Position [340, 2750, 370, 2760] + ZOrder 693 + Port "7" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "desired_pos_vel_acc_CoM" + SID "2274" + Position [340, 2790, 370, 2800] + ZOrder 323 + Port "8" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "jointPos_des" + SID "2273" + Position [340, 2830, 370, 2840] + ZOrder 418 + Port "9" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 545 + SrcBlock "Compute base to fixed link transform" + SrcPort 1 + DstBlock "State Machine" + DstPort 2 + } + Line { + ZOrder 546 + SrcBlock "Compute base to fixed link transform" + SrcPort 2 + DstBlock "State Machine" + DstPort 3 + } + Line { + ZOrder 558 + SrcBlock "State Machine" + SrcPort 1 + Points [67, 0; 0, -45] + Branch { + ZOrder 689 + DstBlock "Compute State Velocity" + DstPort 3 + } + Branch { + ZOrder 687 + Points [0, -30] + DstBlock "w_H_b" + DstPort 1 + } + } + Line { + ZOrder 559 + SrcBlock "State Machine" + SrcPort 2 + Points [94, 0; 0, -45] + Branch { + ZOrder 686 + DstBlock "Compute State Velocity" + DstPort 4 + } + Branch { + ZOrder 685 + Points [0, -20] + DstBlock "feetContactStatus" + DstPort 1 + } + } + Line { + ZOrder 627 + SrcBlock "State Machine" + SrcPort 3 + DstBlock "Update Gains and References" + DstPort 1 + } + Line { + ZOrder 628 + SrcBlock "State Machine" + SrcPort 4 + DstBlock "Update Gains and References" + DstPort 2 + } + Line { + ZOrder 629 + SrcBlock "State Machine" + SrcPort 5 + DstBlock "Update Gains and References" + DstPort 3 + } + Line { + ZOrder 630 + SrcBlock "State Machine" + SrcPort 6 + DstBlock "Update Gains and References" + DstPort 4 + } + Line { + ZOrder 631 + SrcBlock "State Machine" + SrcPort 7 + DstBlock "Update Gains and References" + DstPort 5 + } + Line { + ZOrder 632 + SrcBlock "State Machine" + SrcPort 8 + DstBlock "Update Gains and References" + DstPort 6 + } + Line { + ZOrder 633 + SrcBlock "State Machine" + SrcPort 9 + DstBlock "Update Gains and References" + DstPort 7 + } + Line { + ZOrder 634 + SrcBlock "State Machine" + SrcPort 10 + Points [42, 0] + Branch { + ZOrder 636 + Points [0, 45] + DstBlock "Goto3" + DstPort 1 + } + Branch { + ZOrder 635 + DstBlock "state" + DstPort 1 + } + } + Line { + ZOrder 639 + SrcBlock "Compute State Velocity" + SrcPort 1 + Points [22, 0] + Branch { + ZOrder 655 + Points [0, -40] + DstBlock "Goto5" + DstPort 1 + } + Branch { + ZOrder 654 + DstBlock "nu" + DstPort 1 + } + } + Line { + ZOrder 640 + SrcBlock "Update Gains and References" + SrcPort 1 + DstBlock "KP_postural" + DstPort 1 + } + Line { + ZOrder 641 + SrcBlock "Update Gains and References" + SrcPort 2 + DstBlock "KP_CoM" + DstPort 1 + } + Line { + ZOrder 642 + SrcBlock "Update Gains and References" + SrcPort 3 + DstBlock "KD_CoM" + DstPort 1 + } + Line { + ZOrder 652 + SrcBlock "Update Gains and References" + SrcPort 4 + DstBlock "desired_pos_vel_acc_CoM" + DstPort 1 + } + Line { + ZOrder 653 + SrcBlock "Update Gains and References" + SrcPort 5 + DstBlock "jointPos_des" + DstPort 1 + } + Line { + ZOrder 671 + SrcBlock "IMU_meas" + SrcPort 1 + DstBlock "Compute base to fixed link transform" + DstPort 3 + } + Line { + ZOrder 672 + SrcBlock "jointPos" + SrcPort 1 + Points [31, 0] + Branch { + ZOrder 723 + Points [0, -80] + Branch { + ZOrder 692 + Points [0, -130] + DstBlock "Compute State Velocity" + DstPort 1 + } + Branch { + ZOrder 691 + DstBlock "State Machine" + DstPort 1 + } + } + Branch { + ZOrder 673 + DstBlock "Compute base to fixed link transform" + DstPort 2 + } + } + Line { + ZOrder 677 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock "State Machine" + DstPort 5 + } + Line { + ZOrder 680 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock "State Machine" + DstPort 4 + } + Line { + ZOrder 690 + SrcBlock "jointVel" + SrcPort 1 + DstBlock "Compute State Velocity" + DstPort 2 + } + Line { + ZOrder 724 + SrcBlock "neckPos" + SrcPort 1 + DstBlock "Compute base to fixed link transform" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "emergency stop: joint limits" + SID "4913" + Ports [1] + Position [305, 211, 430, 239] + ZOrder 966 + BackgroundColor "red" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 111 + $ClassName "Simulink.Mask" + Display "disp('EMERGENCY STOP')" + RunInitForIconRedraw "off" + } + System { + Name "emergency stop: joint limits" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "330" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos" + SID "4914" + Position [150, 238, 180, 252] + ZOrder -1 + } + Block { + BlockType Constant + Name "ON_GAZEBO\n1" + SID "4915" + Position [232, 190, 488, 210] + ZOrder 553 + BlockRotation 270 + BlockMirror on + NameLocation "left" + ShowName off + Value "Config.EMERGENCY_STOP_WITH_JOINTS_LIMITS" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n2" + SID "4916" + Position [224, 295, 496, 315] + ZOrder 555 + BlockRotation 270 + BlockMirror on + NameLocation "left" + ShowName off + Value "Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES" + } + Block { + BlockType SubSystem + Name "STOP IF JOINTS HIT THE LIMITS" + SID "4917" + Ports [1, 0, 1] + Position [285, 229, 440, 261] + ZOrder 554 + RequestExecContextInheritance off + System { + Name "STOP IF JOINTS HIT THE LIMITS" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "433" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos" + SID "4918" + Position [60, 103, 90, 117] + ZOrder 552 + } + Block { + BlockType EnablePort + Name "Enable" + SID "4919" + Ports [] + Position [227, -20, 247, 0] + ZOrder 553 + ShowName off + HideAutomaticName off + } + Block { + BlockType Assertion + Name "Assertion" + SID "4920" + Position [360, 37, 420, 83] + ZOrder 207 + HideAutomaticName off + } + Block { + BlockType Reference + Name "GetLimits" + SID "4921" + Ports [0, 2] + Position [15, 23, 135, 92] + ZOrder 560 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + ShowName off + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/States/GetLimits" + SourceType "GetLimits" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + limitsSource "ControlBoard" + limitsType "Position" + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "4922" + Ports [4, 2] + Position [180, 22, 300, 163] + ZOrder 205 + ShowName off + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "MATLAB Function" + Location [121, 45, 868, 1245] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "3816" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "umin" + SID "4922::18" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Inport + Name "umax" + SID "4922::19" + Position [20, 136, 40, 154] + ZOrder -2 + Port "2" + } + Block { + BlockType Inport + Name "u" + SID "4922::1" + Position [20, 171, 40, 189] + ZOrder -3 + Port "3" + } + Block { + BlockType Inport + Name "tol" + SID "4922::20" + Position [20, 206, 40, 224] + ZOrder -4 + Port "4" + } + Block { + BlockType Demux + Name " Demux " + SID "4922::3815" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 165 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4922::3814" + Tag "Stateflow S-Function 18" + Ports [4, 3] + Position [180, 102, 230, 203] + ZOrder 164 + FunctionName "sf_sfun" + PortCounts "[4 3]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "inRange" + } + Port { + PortNumber 3 + Name "res_check_range" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4922::3816" + Position [460, 241, 480, 259] + ZOrder 166 + } + Block { + BlockType Outport + Name "inRange" + SID "4922::5" + Position [460, 101, 480, 119] + ZOrder -8 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "res_check_range" + SID "4922::3789" + Position [460, 136, 480, 154] + ZOrder 139 + Port "2" + } + Line { + ZOrder 73 + SrcBlock "umin" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 74 + SrcBlock "umax" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 75 + SrcBlock "u" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 76 + SrcBlock "tol" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "inRange" + ZOrder 77 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "inRange" + DstPort 1 + } + Line { + Name "res_check_range" + ZOrder 78 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "res_check_range" + DstPort 1 + } + Line { + ZOrder 79 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 80 + SrcBlock " SFunction " + SrcPort 1 + Points [20, 0] + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "4923" + Ports [1] + Position [350, 117, 425, 143] + ZOrder 556 + ShowName off + HideAutomaticName off + VariableName "res_check_range" + MaxDataPoints "inf" + SaveFormat "Structure With Time" + FixptAsFi on + SampleTime "Config.tStep" + } + Block { + BlockType UnitDelay + Name "Unit Delay" + SID "4934" + Position [320, 48, 340, 72] + ZOrder 559 + ShowName off + HideAutomaticName off + InitialCondition "1" + SampleTime "Config.tStep" + HasFrameUpgradeWarning on + } + Block { + BlockType Constant + Name "index1" + SID "4924" + Position [55, 137, 95, 153] + ZOrder 204 + ShowName off + Value "0.01" + VectorParams1D off + } + Line { + ZOrder 1 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 3 + } + Line { + ZOrder 2 + SrcBlock "MATLAB Function" + SrcPort 1 + DstBlock "Unit Delay" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "index1" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 4 + } + Line { + ZOrder 6 + SrcBlock "MATLAB Function" + SrcPort 2 + DstBlock "To Workspace" + DstPort 1 + } + Line { + ZOrder 7 + SrcBlock "Unit Delay" + SrcPort 1 + DstBlock "Assertion" + DstPort 1 + } + Line { + ZOrder 13 + SrcBlock "GetLimits" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 14 + SrcBlock "GetLimits" + SrcPort 2 + DstBlock "MATLAB Function" + DstPort 2 + } + } + } + Block { + BlockType SubSystem + Name "STOP IF THERE ARE SPIKES IN THE ENCODERS" + SID "4925" + Ports [1, 0, 1] + Position [285, 339, 440, 371] + ZOrder 556 + RequestExecContextInheritance off + System { + Name "STOP IF THERE ARE SPIKES IN THE ENCODERS" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "380" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos" + SID "4926" + Position [-10, 53, 20, 67] + ZOrder 552 + } + Block { + BlockType EnablePort + Name "Enable" + SID "4927" + Ports [] + Position [172, -15, 192, 5] + ZOrder 553 + ShowName off + HideAutomaticName off + } + Block { + BlockType Assertion + Name "Assertion" + SID "4928" + Position [335, 37, 400, 83] + ZOrder 556 + HideAutomaticName off + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "4929" + Ports [2, 2] + Position [105, 26, 260, 164] + ZOrder 205 + ShowName off + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "MATLAB Function" + Location [121, 45, 868, 1245] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "3816" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "u" + SID "4929::1" + Position [20, 101, 40, 119] + ZOrder -3 + } + Block { + BlockType Inport + Name "delta_u_max" + SID "4929::18" + Position [20, 136, 40, 154] + ZOrder -1 + Port "2" + } + Block { + BlockType Demux + Name " Demux " + SID "4929::3815" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 165 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4929::3814" + Tag "Stateflow S-Function 14" + Ports [2, 3] + Position [180, 100, 230, 180] + ZOrder 164 + FunctionName "sf_sfun" + PortCounts "[2 3]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "noSpikes" + } + Port { + PortNumber 3 + Name "res_check_spikes" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4929::3816" + Position [460, 241, 480, 259] + ZOrder 166 + } + Block { + BlockType Outport + Name "noSpikes" + SID "4929::5" + Position [460, 101, 480, 119] + ZOrder -8 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "res_check_spikes" + SID "4929::3789" + Position [460, 136, 480, 154] + ZOrder 139 + Port "2" + } + Line { + ZOrder 55 + SrcBlock "u" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 56 + SrcBlock "delta_u_max" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "noSpikes" + ZOrder 57 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "noSpikes" + DstPort 1 + } + Line { + Name "res_check_spikes" + ZOrder 58 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "res_check_spikes" + DstPort 1 + } + Line { + ZOrder 59 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 60 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "4930" + Ports [1] + Position [330, 117, 405, 143] + ZOrder 555 + ShowName off + HideAutomaticName off + VariableName "res_check_spikes" + MaxDataPoints "inf" + SaveFormat "Structure With Time" + FixptAsFi on + SampleTime "Config.tStep" + } + Block { + BlockType UnitDelay + Name "Unit Delay" + SID "4933" + Position [290, 48, 310, 72] + ZOrder 558 + ShowName off + HideAutomaticName off + InitialCondition "1" + SampleTime "Config.tStep" + HasFrameUpgradeWarning on + } + Block { + BlockType Constant + Name "index1" + SID "4931" + Position [-65, 121, 75, 139] + ZOrder 554 + ShowName off + Value "Sat.maxJointsPositionDelta" + VectorParams1D off + } + Line { + ZOrder 1 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 6 + SrcBlock "MATLAB Function" + SrcPort 1 + DstBlock "Unit Delay" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "index1" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 2 + } + Line { + ZOrder 4 + SrcBlock "MATLAB Function" + SrcPort 2 + DstBlock "To Workspace" + DstPort 1 + } + Line { + ZOrder 7 + SrcBlock "Unit Delay" + SrcPort 1 + DstBlock "Assertion" + DstPort 1 + } + } + } + Line { + ZOrder 1 + SrcBlock "ON_GAZEBO\n1" + SrcPort 1 + DstBlock "STOP IF JOINTS HIT THE LIMITS" + DstPort enable + } + Line { + ZOrder 2 + SrcBlock "jointPos" + SrcPort 1 + Points [20, 0] + Branch { + ZOrder 3 + Points [0, 110] + DstBlock "STOP IF THERE ARE SPIKES IN THE ENCODERS" + DstPort 1 + } + Branch { + ZOrder 4 + DstBlock "STOP IF JOINTS HIT THE LIMITS" + DstPort 1 + } + } + Line { + ZOrder 5 + SrcBlock "ON_GAZEBO\n2" + SrcPort 1 + DstBlock "STOP IF THERE ARE SPIKES IN THE ENCODERS" + DstPort enable + } + } + } + Block { + BlockType Outport + Name "jointTorques_star" + SID "4841" + Position [1160, 298, 1190, 312] + ZOrder 560 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 94 + SrcBlock "Dynamics and Kinematics" + SrcPort 4 + DstBlock "Balancing Controller QP" + DstPort 4 + } + Line { + ZOrder 102 + SrcBlock "Dynamics and Kinematics" + SrcPort 11 + DstBlock "Balancing Controller QP" + DstPort 11 + } + Line { + ZOrder 100 + SrcBlock "Dynamics and Kinematics" + SrcPort 9 + DstBlock "Balancing Controller QP" + DstPort 9 + } + Line { + ZOrder 91 + SrcBlock "Dynamics and Kinematics" + SrcPort 2 + DstBlock "Balancing Controller QP" + DstPort 2 + } + Line { + ZOrder 99 + SrcBlock "Dynamics and Kinematics" + SrcPort 8 + DstBlock "Balancing Controller QP" + DstPort 8 + } + Line { + ZOrder 95 + SrcBlock "Dynamics and Kinematics" + SrcPort 5 + DstBlock "Balancing Controller QP" + DstPort 5 + } + Line { + ZOrder 121 + SrcBlock "Robot State and References" + SrcPort 2 + Points [72, 0] + Branch { + ZOrder 331 + Points [0, -240] + DstBlock "Dynamics and Kinematics" + DstPort 2 + } + Branch { + ZOrder 286 + DstBlock "Balancing Controller QP" + DstPort 14 + } + } + Line { + ZOrder 134 + SrcBlock "Robot State and References" + SrcPort 7 + DstBlock "Balancing Controller QP" + DstPort 19 + } + Line { + ZOrder 279 + SrcBlock "jointVel" + SrcPort 1 + DstBlock "Robot State and References" + DstPort 3 + } + Line { + ZOrder 282 + SrcBlock "Joint Torque Saturation" + SrcPort 1 + DstBlock "jointTorques_star" + DstPort 1 + } + Line { + ZOrder 98 + SrcBlock "Dynamics and Kinematics" + SrcPort 7 + DstBlock "Balancing Controller QP" + DstPort 7 + } + Line { + ZOrder 129 + SrcBlock "Robot State and References" + SrcPort 3 + DstBlock "Balancing Controller QP" + DstPort 15 + } + Line { + ZOrder 204 + SrcBlock "Balancing Controller QP" + SrcPort 1 + DstBlock "Joint Torque Saturation" + DstPort 1 + } + Line { + ZOrder 97 + SrcBlock "Dynamics and Kinematics" + SrcPort 6 + DstBlock "Balancing Controller QP" + DstPort 6 + } + Line { + ZOrder 90 + SrcBlock "Dynamics and Kinematics" + SrcPort 1 + DstBlock "Balancing Controller QP" + DstPort 1 + } + Line { + ZOrder 132 + SrcBlock "Robot State and References" + SrcPort 6 + DstBlock "Balancing Controller QP" + DstPort 18 + } + Line { + ZOrder 92 + SrcBlock "Dynamics and Kinematics" + SrcPort 3 + DstBlock "Balancing Controller QP" + DstPort 3 + } + Line { + ZOrder 136 + SrcBlock "Robot State and References" + SrcPort 9 + DstBlock "Balancing Controller QP" + DstPort 21 + } + Line { + ZOrder 130 + SrcBlock "Robot State and References" + SrcPort 4 + DstBlock "Balancing Controller QP" + DstPort 16 + } + Line { + ZOrder 281 + SrcBlock "IMU_meas" + SrcPort 1 + DstBlock "Robot State and References" + DstPort 4 + } + Line { + ZOrder 119 + SrcBlock "Robot State and References" + SrcPort 1 + Points [34, 0; 0, -320] + DstBlock "Dynamics and Kinematics" + DstPort 1 + } + Line { + ZOrder 131 + SrcBlock "Robot State and References" + SrcPort 5 + DstBlock "Balancing Controller QP" + DstPort 17 + } + Line { + ZOrder 101 + SrcBlock "Dynamics and Kinematics" + SrcPort 10 + DstBlock "Balancing Controller QP" + DstPort 10 + } + Line { + ZOrder 135 + SrcBlock "Robot State and References" + SrcPort 8 + DstBlock "Balancing Controller QP" + DstPort 20 + } + Line { + ZOrder 278 + SrcBlock "jointPos" + SrcPort 1 + Points [36, 0] + Branch { + ZOrder 360 + Points [0, 145] + DstBlock "Robot State and References" + DstPort 2 + } + Branch { + ZOrder 293 + Points [324, 0] + Branch { + ZOrder 361 + Points [0, 70] + DstBlock "Balancing Controller QP" + DstPort 12 + } + Branch { + ZOrder 289 + DstBlock "Dynamics and Kinematics" + DstPort 3 + } + } + Branch { + ZOrder 322 + Points [0, -40] + DstBlock "emergency stop: joint limits" + DstPort 1 + } + } + Line { + ZOrder 313 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock "Robot State and References" + DstPort 5 + } + Line { + ZOrder 314 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock "Robot State and References" + DstPort 6 + } + Line { + ZOrder 329 + SrcBlock "jointAcc" + SrcPort 1 + Points [308, 0; 0, 210] + DstBlock "Balancing Controller QP" + DstPort 13 + } + Line { + ZOrder 332 + SrcBlock "neckPos" + SrcPort 1 + DstBlock "Robot State and References" + DstPort 1 + } + Line { + ZOrder 371 + SrcBlock "time" + SrcPort 1 + DstBlock "Goto" + DstPort 1 + } + } + } + Block { + BlockType Memory + Name "Memory" + SID "5002" + Position [1125, 485, 1155, 515] + ZOrder 1286 + InitialCondition "zeros(Config.N_DOF,1)" + InheritSampleTime on + } + Block { + BlockType SubSystem + Name "Robot Visualizer" + SID "5014" + Ports [1] + Position [1465, 245, 1725, 365] + ZOrder 1291 + TreatAsAtomicUnit on + SystemSampleTime "confVisualizer.tStep" + RequestExecContextInheritance off + ContentPreviewEnabled on + System { + Name "Robot Visualizer" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "203" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "In1" + SID "5015" + Position [-35, 73, -5, 87] + ZOrder 1276 + } + Block { + BlockType BusSelector + Name "Bus\nSelector" + SID "4943" + Ports [1, 4] + Position [45, 24, 50, 136] + ZOrder 1275 + OutputSignals "w_H_b,basePose_dot,joints_position,joints_velocity" + Port { + PortNumber 1 + Name "" + } + Port { + PortNumber 2 + Name "" + } + Port { + PortNumber 3 + Name "" + } + Port { + PortNumber 4 + Name "" + } + } + Block { + BlockType Reference + Name "RobotVisualizer" + SID "4939" + Ports [4] + Position [210, 22, 430, 138] + ZOrder 1271 + LibraryVersion "1.4" + SourceBlock "mwbs_visualizers_lib/RobotVisualizer" + SourceType "mwbs.Visualizers.robotVisualizer.RobotVisualizer" + SourceProductName "Matlab Whole-body Simulator" + config "confVisualizer" + SimulateUsing "Interpreted execution" + } + Line { + Name "" + ZOrder 308 + Labels [0, 0] + SrcBlock "Bus\nSelector" + SrcPort 2 + DstBlock "RobotVisualizer" + DstPort 2 + } + Line { + ZOrder 446 + SrcBlock "In1" + SrcPort 1 + DstBlock "Bus\nSelector" + DstPort 1 + } + Line { + Name "" + ZOrder 307 + Labels [0, 0] + SrcBlock "Bus\nSelector" + SrcPort 3 + DstBlock "RobotVisualizer" + DstPort 3 + } + Line { + Name "" + ZOrder 306 + Labels [0, 0] + SrcBlock "Bus\nSelector" + SrcPort 1 + DstBlock "RobotVisualizer" + DstPort 1 + } + Line { + Name "" + ZOrder 309 + Labels [0, 0] + SrcBlock "Bus\nSelector" + SrcPort 4 + DstBlock "RobotVisualizer" + DstPort 4 + } + } + } + Block { + BlockType Selector + Name "Selector" + SID "4963" + Ports [1, 1] + Position [840, 473, 880, 497] + ZOrder 1280 + InputPortWidth "6+robot_config.N_DOF" + IndexOptions "Index vector (dialog)" + Indices "[7:6+Config.N_DOF]" + OutputSizes "1" + } + Block { + BlockType Selector + Name "Selector1" + SID "4975" + Ports [1, 1] + Position [840, 443, 880, 467] + ZOrder 1281 + InputPortWidth "robot_config.N_DOF" + IndexOptions "Index vector (dialog)" + Indices "[1:Config.N_DOF]" + OutputSizes "1" + } + Block { + BlockType Selector + Name "Selector2" + SID "4976" + Ports [1, 1] + Position [840, 413, 880, 437] + ZOrder 1282 + InputPortWidth "robot_config.N_DOF" + IndexOptions "Index vector (dialog)" + Indices "[1:Config.N_DOF]" + OutputSizes "1" + } + Block { + BlockType SubSystem + Name "Subsystem" + SID "4955" + Ports [1, 5] + Position [1190, 427, 1385, 573] + ZOrder 1279 + ForegroundColor "red" + BackgroundColor "yellow" + TreatAsAtomicUnit on + SystemSampleTime "Config.tStepSim" + RequestExecContextInheritance off + ContentPreviewEnabled on + System { + Name "Subsystem" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "208" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "joints torque" + SID "4956" + Position [-100, 288, -70, 302] + ZOrder 1275 + } + Block { + BlockType BusSelector + Name "Bus\nSelector1" + SID "4942" + Ports [1, 4] + Position [445, 393, 450, 507] + ZOrder 1274 + ShowName off + OutputSignals "w_H_b,s,nu,nuDot" + Port { + PortNumber 1 + Name "" + } + Port { + PortNumber 2 + Name "" + } + Port { + PortNumber 3 + Name "" + } + Port { + PortNumber 4 + Name "" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector2" + SID "5017" + Ports [1, 1] + Position [295, 156, 300, 194] + ZOrder 1286 + BlockMirror on + OutputSignals "joints_velocity" + Port { + PortNumber 1 + Name "" + } + } + Block { + BlockType Reference + Name "Configuration" + SID "4962" + Ports [] + Position [180, 110, 255, 140] + ZOrder 1281 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Utilities/Configuration" + SourceType "" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled on + ConfigSource "Workspace" + ConfigObject "'WBTConfigRobotSim'" + } + Block { + BlockType Constant + Name "Constant1" + SID "4940" + Position [120, 285, 150, 315] + ZOrder 1272 + Value "zeros(robot_config.N_DOF + 6,1)" + } + Block { + BlockType Constant + Name "Constant2" + SID "4941" + Position [120, 320, 150, 350] + ZOrder 1273 + Value "motorsReflectedInertia" + VectorParams1D off + } + Block { + BlockType Reference + Name "IMUsensor" + SID "4938" + Ports [4, 1] + Position [545, 392, 765, 508] + ZOrder 1270 + LibraryVersion "1.17" + SourceBlock "mwbs_robotSensors_lib/IMUsensor" + SourceType "IMUsensor" + SourceProductName "Matlab Whole-body Simulator" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled on + frameName "robot_config.robotFrames.IMU" + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "5016" + Ports [2, 1] + Position [-5, 197, 110, 328] + ZOrder 1285 + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + ContentPreviewEnabled on + System { + Name "MATLAB Function" + Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility on + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "23" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointVelocities" + SID "5016::22" + Position [20, 101, 40, 119] + ZOrder 13 + } + Block { + BlockType Inport + Name "motorJointTorques" + SID "5016::1" + Position [20, 136, 40, 154] + ZOrder -1 + Port "2" + } + Block { + BlockType Demux + Name " Demux " + SID "5016::20" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 11 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "5016::19" + Tag "Stateflow S-Function 5" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 10 + FunctionName "sf_sfun" + Parameters "jointFrictionMat" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "jointTorques" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "5016::21" + Position [460, 241, 480, 259] + ZOrder 12 + } + Block { + BlockType Outport + Name "jointTorques" + SID "5016::5" + Position [460, 101, 480, 119] + ZOrder -5 + } + Line { + ZOrder 25 + SrcBlock "jointVelocities" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 26 + SrcBlock "motorJointTorques" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "jointTorques" + ZOrder 27 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "jointTorques" + DstPort 1 + } + Line { + ZOrder 28 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 29 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Memory + Name "Memory" + SID "5018" + Position [140, 160, 170, 190] + ZOrder 1287 + BlockMirror on + InitialCondition "robot_config.initialConditions.s_dot" + InheritSampleTime on + } + Block { + BlockType Reference + Name "RobotDynWithContacts" + SID "4937" + Ports [3, 4] + Position [180, 244, 375, 356] + ZOrder 1231 + ForegroundColor "red" + BackgroundColor "yellow" + LibraryVersion "3.20" + SourceBlock "mwbs_robotDynamicsWithContacts_lib/RobotDynWithContacts" + SourceType "" + SourceProductName "Matlab Whole-body Simulator" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled on + robot_config "robot_config" + contact_config "contact_config" + physics_config "physics_config" + motorReflectedInertiaFormat "matrix" + } + Block { + BlockType Outport + Name "State" + SID "4957" + Position [870, 248, 900, 262] + ZOrder 1276 + } + Block { + BlockType Outport + Name "wrench_LFoot" + SID "4958" + Position [870, 278, 900, 292] + ZOrder 1277 + Port "2" + } + Block { + BlockType Outport + Name "wrench_RFoot" + SID "4959" + Position [870, 308, 900, 322] + ZOrder 1278 + Port "3" + } + Block { + BlockType Outport + Name "" + SID "4960" + Position [870, 338, 900, 352] + ZOrder 1279 + Port "4" + } + Block { + BlockType Outport + Name "imuOut" + SID "4961" + Position [870, 368, 900, 382] + ZOrder 1280 + Port "5" + } + Line { + ZOrder 324 + SrcBlock "RobotDynWithContacts" + SrcPort 3 + DstBlock "wrench_RFoot" + DstPort 1 + } + Line { + ZOrder 322 + SrcBlock "RobotDynWithContacts" + SrcPort 1 + Points [81, 0] + Branch { + ZOrder 351 + Points [0, -80] + DstBlock "Bus\nSelector2" + DstPort 1 + } + Branch { + ZOrder 350 + DstBlock "State" + DstPort 1 + } + } + Line { + ZOrder 326 + SrcBlock "IMUsensor" + SrcPort 1 + Points [48, 0; 0, -75] + DstBlock "imuOut" + DstPort 1 + } + Line { + Name "" + ZOrder 296 + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 1 + DstBlock "IMUsensor" + DstPort 1 + } + Line { + ZOrder 289 + SrcBlock "Constant1" + SrcPort 1 + DstBlock "RobotDynWithContacts" + DstPort 2 + } + Line { + Name "" + ZOrder 297 + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 2 + DstBlock "IMUsensor" + DstPort 2 + } + Line { + ZOrder 290 + SrcBlock "Constant2" + SrcPort 1 + DstBlock "RobotDynWithContacts" + DstPort 3 + } + Line { + Name "" + ZOrder 298 + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 3 + DstBlock "IMUsensor" + DstPort 3 + } + Line { + ZOrder 300 + SrcBlock "RobotDynWithContacts" + SrcPort 4 + Points [23, 0; 0, 105] + DstBlock "Bus\nSelector1" + DstPort 1 + } + Line { + Name "" + ZOrder 299 + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 4 + Points [49, 0] + Branch { + ZOrder 325 + Points [0, -150] + DstBlock "" + DstPort 1 + } + Branch { + ZOrder 318 + DstBlock "IMUsensor" + DstPort 4 + } + } + Line { + ZOrder 323 + SrcBlock "RobotDynWithContacts" + SrcPort 2 + DstBlock "wrench_LFoot" + DstPort 1 + } + Line { + ZOrder 347 + SrcBlock "joints torque" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 2 + } + Line { + ZOrder 348 + SrcBlock "MATLAB Function" + SrcPort 1 + DstBlock "RobotDynWithContacts" + DstPort 1 + } + Line { + Name "" + ZOrder 352 + Labels [0, 0] + SrcBlock "Bus\nSelector2" + SrcPort 1 + DstBlock "Memory" + DstPort 1 + } + Line { + ZOrder 357 + SrcBlock "Memory" + SrcPort 1 + Points [-164, 0; 0, 55] + DstBlock "MATLAB Function" + DstPort 1 + } + } + } + Line { + ZOrder 321 + SrcBlock "Bus\nCreator" + SrcPort 1 + Points [19, 0; 0, 161; -1039, 0; 0, -146] + DstBlock "Bus\nSelector2" + DstPort 1 + } + Line { + ZOrder 408 + SrcBlock "MOMENTUM BASED TORQUE CONTROL" + SrcPort 1 + DstBlock "Memory" + DstPort 1 + } + Line { + ZOrder 362 + SrcBlock "Subsystem" + SrcPort 1 + Points [19, 0; 0, -15] + Branch { + ZOrder 449 + Points [0, -120] + DstBlock "Robot Visualizer" + DstPort 1 + } + Branch { + ZOrder 441 + DstBlock "Bus\nSelector1" + DstPort 1 + } + } + Line { + ZOrder 363 + SrcBlock "Subsystem" + SrcPort 2 + DstBlock "Bus\nCreator" + DstPort 3 + } + Line { + ZOrder 364 + SrcBlock "Subsystem" + SrcPort 3 + DstBlock "Bus\nCreator" + DstPort 4 + } + Line { + ZOrder 365 + SrcBlock "Subsystem" + SrcPort 4 + DstBlock "Bus\nCreator" + DstPort 5 + } + Line { + ZOrder 366 + SrcBlock "Subsystem" + SrcPort 5 + DstBlock "Bus\nCreator" + DstPort 6 + } + Line { + Name "" + ZOrder 379 + Labels [0, 0] + SrcBlock "Bus\nSelector2" + SrcPort 4 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 5 + } + Line { + Name "" + ZOrder 380 + Labels [0, 0] + SrcBlock "Bus\nSelector2" + SrcPort 5 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 6 + } + Line { + Name "" + ZOrder 381 + Labels [0, 0] + SrcBlock "Bus\nSelector2" + SrcPort 6 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 7 + } + Line { + Name "" + ZOrder 388 + Labels [0, 0] + SrcBlock "Bus\nSelector2" + SrcPort 1 + DstBlock "Selector2" + DstPort 1 + } + Line { + Name "" + ZOrder 390 + Labels [0, 0] + SrcBlock "Bus\nSelector2" + SrcPort 2 + DstBlock "Selector1" + DstPort 1 + } + Line { + Name "" + ZOrder 391 + Labels [0, 0] + SrcBlock "Bus\nSelector2" + SrcPort 3 + DstBlock "Selector" + DstPort 1 + } + Line { + ZOrder 392 + SrcBlock "Selector" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 4 + } + Line { + ZOrder 393 + SrcBlock "Selector1" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 3 + } + Line { + ZOrder 394 + SrcBlock "Selector2" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 2 + } + Line { + ZOrder 428 + SrcBlock "Constant" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 1 + } + Line { + ZOrder 437 + SrcBlock "Memory" + SrcPort 1 + DstBlock "Subsystem" + DstPort 1 + } + Line { + ZOrder 406 + SrcBlock "Clock1" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 8 + } + Line { + Name "" + ZOrder 442 + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 1 + DstBlock "Bus\nCreator" + DstPort 1 + } + Line { + Name "" + ZOrder 443 + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 2 + DstBlock "Bus\nCreator" + DstPort 2 + } + } +} +#Finite State Machines +# +# Stateflow 80000036 +# +# +Stateflow { + machine { + id 1 + name "torqueControlBalancingWithSimu" + sfVersion 80000036 + firstTarget 246 + } + chart { + id 2 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and References/Reshape Gains Matr" + "ices/MATLAB Function1" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 3 0 0] + viewObj 2 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 1 + disableImplicitCasting 1 + eml { + name "diagonalMatrixFromVectorFCN" + } + firstData 4 + firstTransition 7 + firstJunction 6 + } + state { + id 3 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 2 + treeNode [2 0 0 0] + superState SUBCHART + subviewer 2 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function D = diagonalMatrixFromVectorFCN(d)\n\n D = wbc.diagonalMatrixFromVector(d);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 4 + ssIdNumber 4 + name "d" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [2 0 5] + } + data { + id 5 + ssIdNumber 5 + name "D" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [2 4 0] + } + junction { + id 6 + position [23.5747 49.5747 7] + chart 2 + subviewer 2 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [2 0 0] + } + transition { + id 7 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 6 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 2 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 2 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [2 0 0] + } + instance { + id 8 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and References/Reshape Gains Matr" + "ices/MATLAB Function1" + chart 2 + } + chart { + id 9 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and References/Reshape Gains Matr" + "ices/MATLAB Function" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 10 0 0] + viewObj 9 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 2 + disableImplicitCasting 1 + eml { + name "diagonalMatrixFromVectorFCN" + } + firstData 11 + firstTransition 14 + firstJunction 13 + } + state { + id 10 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 9 + treeNode [9 0 0 0] + superState SUBCHART + subviewer 9 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function D = diagonalMatrixFromVectorFCN(d)\n\n D = wbc.diagonalMatrixFromVector(d);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 11 + ssIdNumber 4 + name "d" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [9 0 12] + } + data { + id 12 + ssIdNumber 5 + name "D" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [9 11 0] + } + junction { + id 13 + position [23.5747 49.5747 7] + chart 9 + subviewer 9 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [9 0 0] + } + transition { + id 14 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 13 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 9 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 9 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [9 0 0] + } + instance { + id 15 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and References/Reshape Gains Matr" + "ices/MATLAB Function" + chart 9 + } + chart { + id 16 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fixed link transform/RFoot to " + "base link transform/Get Base Rotation From IMU" + windowPosition [326.89 491.339 161 328] + viewLimits [0 156.75 0 153.75] + screen [1 1 1280 1024 1.000677131425054] + treeNode [0 17 0 0] + viewObj 16 + ssIdHighWaterMark 88 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 3 + disableImplicitCasting 1 + eml { + name "getWorldToBaseTransformFCN" + } + firstData 18 + firstTransition 27 + firstJunction 26 + } + state { + id 17 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 16 + treeNode [16 0 0 0] + superState SUBCHART + subviewer 16 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function w_H_b = getWorldToBaseTransformFCN(imu_H_fixedLink, imu_H_fixedLink_0, fixedLink_H_base," + " rpyFromIMU_0, rpyFromIMU, neck_pos, Config)\n\n FILTER_IMU_YAW = Config.FILTER_IMU_YAW;\n \n [w_H_b, w" + "Imu_H_base, wImu_H_fixedLink_0] = wbc.worldToBaseTransformWithIMU(imu_H_fixedLink, imu_H_fixedLink_0, fixedLink_" + "H_base, rpyFromIMU_0, rpyFromIMU, FILTER_IMU_YAW);\n \n % Correct head IMU with neck positions (HEAD IMU o" + "nly)\n if Config.CORRECT_NECK_IMU\n \n wImu_H_wImuAssumingNeckToZero = wbc.correctIMUWithNeckPo" + "s(neck_pos);\n wImu_H_base = wImu_H_wImuAssumingNeckToZero * wImu_H_base;\n w_H_" + "b = wImu_H_fixedLink_0\\wImu_H_base; \n end\nend\n" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 18 + ssIdNumber 82 + name "imu_H_fixedLink" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 0 19] + } + data { + id 19 + ssIdNumber 83 + name "imu_H_fixedLink_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 18 20] + } + data { + id 20 + ssIdNumber 81 + name "fixedLink_H_base" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 19 21] + } + data { + id 21 + ssIdNumber 85 + name "rpyFromIMU_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 20 22] + } + data { + id 22 + ssIdNumber 65 + name "rpyFromIMU" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 21 23] + } + data { + id 23 + ssIdNumber 88 + name "neck_pos" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 22 24] + } + data { + id 24 + ssIdNumber 71 + name "w_H_b" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 23 25] + } + data { + id 25 + ssIdNumber 87 + name "Config" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 24 0] + } + junction { + id 26 + position [23.5747 49.5747 7] + chart 16 + subviewer 16 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [16 0 0] + } + transition { + id 27 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 26 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 16 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 16 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [16 0 0] + } + instance { + id 28 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fixed link transform/RFoot to " + "base link transform/Get Base Rotation From IMU" + chart 16 + } + chart { + id 29 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and References/Reshape Gains Matr" + "ices/MATLAB Function2" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 30 0 0] + viewObj 29 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 4 + disableImplicitCasting 1 + eml { + name "diagonalMatrixFromVectorFCN" + } + firstData 31 + firstTransition 34 + firstJunction 33 + } + state { + id 30 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 29 + treeNode [29 0 0 0] + superState SUBCHART + subviewer 29 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function D = diagonalMatrixFromVectorFCN(d)\n\n D = wbc.diagonalMatrixFromVector(d);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 31 + ssIdNumber 4 + name "d" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [29 0 32] + } + data { + id 32 + ssIdNumber 5 + name "D" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [29 31 0] + } + junction { + id 33 + position [23.5747 49.5747 7] + chart 29 + subviewer 29 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [29 0 0] + } + transition { + id 34 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 33 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 29 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 29 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [29 0 0] + } + instance { + id 35 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and References/Reshape Gains Matr" + "ices/MATLAB Function2" + chart 29 + } + chart { + id 36 + machine 1 + name "Subsystem/MATLAB Function" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 37 0 0] + viewObj 36 + ssIdHighWaterMark 7 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 5 + disableImplicitCasting 1 + eml { + name "frictionModel" + } + firstData 38 + firstTransition 43 + firstJunction 42 + } + state { + id 37 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 36 + treeNode [36 0 0 0] + superState SUBCHART + subviewer 36 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function jointTorques = frictionModel(jointVelocities,motorJointTorques,jointFrictionMat)\n\njoin" + "tTorques = motorJointTorques - jointFrictionMat*jointVelocities;\n" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 38 + ssIdNumber 6 + name "jointVelocities" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [36 0 39] + } + data { + id 39 + ssIdNumber 4 + name "motorJointTorques" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [36 38 40] + } + data { + id 40 + ssIdNumber 5 + name "jointTorques" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [36 39 41] + } + data { + id 41 + ssIdNumber 7 + name "jointFrictionMat" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [36 40 0] + } + junction { + id 42 + position [23.5747 49.5747 7] + chart 36 + subviewer 36 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [36 0 0] + } + transition { + id 43 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 42 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 36 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 36 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [36 0 0] + } + instance { + id 44 + machine 1 + name "Subsystem/MATLAB Function" + chart 36 + } + chart { + id 45 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/Add motors reflected inertia/Add motor r" + "eflected inertias" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 46 0 0] + viewObj 45 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 6 + disableImplicitCasting 1 + eml { + name "addMotorsInertiaFCN" + } + firstData 47 + firstTransition 51 + firstJunction 50 + } + state { + id 46 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 45 + treeNode [45 0 0 0] + superState SUBCHART + subviewer 45 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function M_with_inertia = addMotorsInertiaFCN(M,Config)\n\n Gamma = Config.Gamma;\n T =" + " Config.T;\n I_m = Config.I_m;\n \n M_with_inertia = wbc.addMotorsReflectedInertia(M,Gamma,T,I_m);\ne" + "nd" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 47 + ssIdNumber 4 + name "M" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [45 0 48] + } + data { + id 48 + ssIdNumber 5 + name "M_with_inertia" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [45 47 49] + } + data { + id 49 + ssIdNumber 6 + name "Config" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [45 48 0] + } + junction { + id 50 + position [23.5747 49.5747 7] + chart 45 + subviewer 45 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [45 0 0] + } + transition { + id 51 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 50 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 45 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 45 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [45 0 0] + } + instance { + id 52 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/Add motors reflected inertia/Add motor r" + "eflected inertias" + chart 45 + } + chart { + id 53 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute State Velocity/Compute Base Velocity" + windowPosition [420.256 -293.125 200 760] + viewLimits [0 156.75 0 153.75] + screen [1 1 1680 1050 1.25] + treeNode [0 54 0 0] + viewObj 53 + ssIdHighWaterMark 10 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 7 + disableImplicitCasting 1 + eml { + name "computeBaseVelocityFCN" + } + firstData 55 + firstTransition 62 + firstJunction 61 + } + state { + id 54 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 53 + treeNode [53 0 0 0] + superState SUBCHART + subviewer 53 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function nu_b = computeBaseVelocityFCN(J_l_sole, J_r_sole, feetContactStatus, jointVel, Reg)\n\n " + " pinvDamp_baseVel = Reg.pinvDamp_baseVel;\n \n nu_b = wbc.computeBaseVelocityWithFeetContact(J_l_sole, J" + "_r_sole, feetContactStatus, jointVel, pinvDamp_baseVel);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 55 + ssIdNumber 4 + name "J_l_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [53 0 56] + } + data { + id 56 + ssIdNumber 8 + name "J_r_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [53 55 57] + } + data { + id 57 + ssIdNumber 9 + name "feetContactStatus" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [53 56 58] + } + data { + id 58 + ssIdNumber 5 + name "nu_b" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [53 57 59] + } + data { + id 59 + ssIdNumber 6 + name "jointVel" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [53 58 60] + } + data { + id 60 + ssIdNumber 10 + name "Reg" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [53 59 0] + } + junction { + id 61 + position [23.5747 49.5747 7] + chart 53 + subviewer 53 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [53 0 0] + } + transition { + id 62 + labelString "{eML_blk_kernel();}" + labelPosition [48.125 43.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 61 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 53 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 53 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [53 0 0] + } + instance { + id 63 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute State Velocity/Compute Base Velocity" + chart 53 + } + chart { + id 64 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fixed link transform/LFoot to " + "base link transform /Get Base Rotation From IMU" + windowPosition [326.89 491.339 161 328] + viewLimits [0 156.75 0 153.75] + screen [1 1 1280 1024 1.000677131425054] + treeNode [0 65 0 0] + viewObj 64 + ssIdHighWaterMark 88 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 8 + disableImplicitCasting 1 + eml { + name "getWorldToBaseTransformFCN" + } + firstData 66 + firstTransition 75 + firstJunction 74 + } + state { + id 65 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 64 + treeNode [64 0 0 0] + superState SUBCHART + subviewer 64 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function w_H_b = getWorldToBaseTransformFCN(imu_H_fixedLink, imu_H_fixedLink_0, fixedLink_H_base," + " rpyFromIMU_0, rpyFromIMU, neck_pos, Config)\n\n FILTER_IMU_YAW = Config.FILTER_IMU_YAW;\n \n [w_H_b, w" + "Imu_H_base, wImu_H_fixedLink_0] = wbc.worldToBaseTransformWithIMU(imu_H_fixedLink, imu_H_fixedLink_0, fixedLink_" + "H_base, rpyFromIMU_0, rpyFromIMU, FILTER_IMU_YAW);\n \n % Correct head IMU with neck positions (HEAD IMU o" + "nly)\n if Config.CORRECT_NECK_IMU\n \n wImu_H_wImuAssumingNeckToZero = wbc.correctIMUWithNeckPo" + "s(neck_pos);\n wImu_H_base = wImu_H_wImuAssumingNeckToZero * wImu_H_base;\n w_H_" + "b = wImu_H_fixedLink_0\\wImu_H_base; \n end\nend\n" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 66 + ssIdNumber 82 + name "imu_H_fixedLink" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [64 0 67] + } + data { + id 67 + ssIdNumber 83 + name "imu_H_fixedLink_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [64 66 68] + } + data { + id 68 + ssIdNumber 81 + name "fixedLink_H_base" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [64 67 69] + } + data { + id 69 + ssIdNumber 85 + name "rpyFromIMU_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [64 68 70] + } + data { + id 70 + ssIdNumber 65 + name "rpyFromIMU" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [64 69 71] + } + data { + id 71 + ssIdNumber 88 + name "neck_pos" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [64 70 72] + } + data { + id 72 + ssIdNumber 71 + name "w_H_b" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [64 71 73] + } + data { + id 73 + ssIdNumber 87 + name "Config" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [64 72 0] + } + junction { + id 74 + position [23.5747 49.5747 7] + chart 64 + subviewer 64 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [64 0 0] + } + transition { + id 75 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 74 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 64 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 64 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [64 0 0] + } + instance { + id 76 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fixed link transform/LFoot to " + "base link transform /Get Base Rotation From IMU" + chart 64 + } + chart { + id 77 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/From tau_QP to Joint Torques (motor reflected ine" + "rtia)/(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 78 0 0] + viewObj 77 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 9 + disableImplicitCasting 1 + eml { + name "computeMotorsInertiaFCN" + } + firstData 79 + firstTransition 82 + firstJunction 81 + } + state { + id 78 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 77 + treeNode [77 0 0 0] + superState SUBCHART + subviewer 77 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function reflectedInertia = computeMotorsInertiaFCN(Config)\n \n Gamma = Config.Gamma;\n " + " T = Config.T;\n I_m = Config.I_m;\n \n reflectedInertia = wbc.computeMotorsReflectedInertia(Gam" + "ma,T,I_m);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 79 + ssIdNumber 5 + name "reflectedInertia" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [77 0 80] + } + data { + id 80 + ssIdNumber 4 + name "Config" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [77 79 0] + } + junction { + id 81 + position [23.5747 49.5747 7] + chart 77 + subviewer 77 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [77 0 0] + } + transition { + id 82 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 81 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 77 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 77 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [77 0 0] + } + instance { + id 83 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/From tau_QP to Joint Torques (motor reflected ine" + "rtia)/(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" + chart 77 + } + chart { + id 84 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/Saturate Torque Derivative" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 85 0 0] + viewObj 84 + ssIdHighWaterMark 8 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 11 + disableImplicitCasting 1 + eml { + name "saturateInputDerivativeFCN" + } + firstData 86 + firstTransition 92 + firstJunction 91 + } + state { + id 85 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 84 + treeNode [84 0 0 0] + superState SUBCHART + subviewer 84 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function uSat = saturateInputDerivativeFCN(u, u_0, tStep, uDotMax)\n\n uSat = wbc.saturateInpu" + "tDerivative(u, u_0, tStep, uDotMax);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 86 + ssIdNumber 4 + name "u" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [84 0 87] + } + data { + id 87 + ssIdNumber 5 + name "uSat" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [84 86 88] + } + data { + id 88 + ssIdNumber 6 + name "u_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [84 87 89] + } + data { + id 89 + ssIdNumber 7 + name "tStep" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [84 88 90] + } + data { + id 90 + ssIdNumber 8 + name "uDotMax" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [84 89 0] + } + junction { + id 91 + position [23.5747 49.5747 7] + chart 84 + subviewer 84 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [84 0 0] + } + transition { + id 92 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 91 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 84 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 84 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [84 0 0] + } + instance { + id 93 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/Saturate Torque Derivative" + chart 84 + } + chart { + id 94 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/STATE MACHINE" + windowPosition [326.89 491.339 161 328] + viewLimits [0 156.75 0 153.75] + screen [1 1 1280 1024 1.000677131425054] + treeNode [0 95 0 0] + viewObj 94 + ssIdHighWaterMark 86 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 13 + disableImplicitCasting 1 + eml { + name "stateMachineMomentumControlFCN" + } + firstData 96 + firstTransition 120 + firstJunction 119 + } + state { + id 95 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 94 + treeNode [94 0 0 0] + superState SUBCHART + subviewer 94 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function [w_H_b, pos_CoM_des, jointPos_des, feetContactStatus, KP_postural_diag, KP_CoM_diag, KD_" + "CoM_diag, state, smoothingTimeJoints, smoothingTimeCoM] = ...\n stateMachineMomentumControlFCN(pos_" + "CoM_0, jointPos_0, pos_CoM_fixed_l_sole, pos_CoM_fixed_r_sole, jointPos, ...\n " + " time, wrench_rightFoot, wrench_leftFoot, l_sole_H_b, r_sole_H_b, StateMachine, Gain, Config)\n \n " + "[w_H_b, pos_CoM_des, jointPos_des, feetContactStatus, KP_postural_diag, KP_CoM_diag, KD_CoM_diag, state, smoothi" + "ngTimeJoints, smoothingTimeCoM] = ...\n stateMachineMomentumControl(pos_CoM_0, jointPos_0, pos_CoM_fixed_" + "l_sole, pos_CoM_fixed_r_sole, jointPos, ...\n time, wrench_rightFoot, wrench_" + "leftFoot, l_sole_H_b, r_sole_H_b, StateMachine, Gain, Config);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 96 + ssIdNumber 37 + name "pos_CoM_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 0 97] + } + data { + id 97 + ssIdNumber 54 + name "jointPos_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 96 98] + } + data { + id 98 + ssIdNumber 64 + name "pos_CoM_fixed_l_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 97 99] + } + data { + id 99 + ssIdNumber 77 + name "pos_CoM_fixed_r_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 98 100] + } + data { + id 100 + ssIdNumber 69 + name "jointPos" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 99 101] + } + data { + id 101 + ssIdNumber 56 + name "time" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 100 102] + } + data { + id 102 + ssIdNumber 65 + name "wrench_rightFoot" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 101 103] + } + data { + id 103 + ssIdNumber 74 + name "wrench_leftFoot" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 102 104] + } + data { + id 104 + ssIdNumber 71 + name "w_H_b" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 103 105] + } + data { + id 105 + ssIdNumber 52 + name "pos_CoM_des" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 104 106] + } + data { + id 106 + ssIdNumber 46 + name "jointPos_des" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 105 107] + } + data { + id 107 + ssIdNumber 43 + name "feetContactStatus" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 106 108] + } + data { + id 108 + ssIdNumber 72 + name "l_sole_H_b" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 107 109] + } + data { + id 109 + ssIdNumber 78 + name "r_sole_H_b" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 108 110] + } + data { + id 110 + ssIdNumber 55 + name "StateMachine" + scope PARAMETER_DATA + paramIndexForInitFromWorkspace 2 + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 109 111] + } + data { + id 111 + ssIdNumber 66 + name "KP_postural_diag" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 110 112] + } + data { + id 112 + ssIdNumber 82 + name "KP_CoM_diag" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 111 113] + } + data { + id 113 + ssIdNumber 83 + name "KD_CoM_diag" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 112 114] + } + data { + id 114 + ssIdNumber 67 + name "Gain" + scope PARAMETER_DATA + paramIndexForInitFromWorkspace 1 + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 113 115] + } + data { + id 115 + ssIdNumber 68 + name "state" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 114 116] + } + data { + id 116 + ssIdNumber 81 + name "smoothingTimeJoints" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 115 117] + } + data { + id 117 + ssIdNumber 86 + name "smoothingTimeCoM" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 116 118] + } + data { + id 118 + ssIdNumber 85 + name "Config" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [94 117 0] + } + junction { + id 119 + position [23.5747 49.5747 7] + chart 94 + subviewer 94 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [94 0 0] + } + transition { + id 120 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 119 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 94 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 94 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [94 0 0] + } + instance { + id 121 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/STATE MACHINE" + chart 94 + } + chart { + id 122 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/MAT" + "LAB Function" + windowPosition [369.958 -65.92 200 534.4] + viewLimits [0 156.75 0 153.75] + screen [1 1 1366 768 1.25] + treeNode [0 123 0 0] + viewObj 122 + ssIdHighWaterMark 9 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 14 + disableImplicitCasting 1 + eml { + name "checkInputSpikesFCN" + } + supportVariableSizing 0 + firstData 124 + firstTransition 129 + firstJunction 128 + } + state { + id 123 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 122 + treeNode [122 0 0 0] + superState SUBCHART + subviewer 122 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function [noSpikes, res_check_spikes] = checkInputSpikesFCN(u, delta_u_max)\n\n [noSpikes, re" + "s_check_spikes] = wbc.checkInputSpikes(u, delta_u_max);\nend" + editorLayout "100 M4x1[10 5 700 500]" + fimathString "fimath(...\n'RoundMode', 'floor',...\n'OverflowMode', 'wrap',...\n'ProductMode', 'KeepLSB', " + "'ProductWordLength', 32,...\n'SumMode', 'KeepLSB', 'SumWordLength', 32,...\n'CastBeforeSum', true)" + fimathForFiConstructors FimathMatlabFactoryDefault + emlDefaultFimath FimathUserSpecified + } + } + data { + id 124 + ssIdNumber 6 + name "u" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [122 0 125] + } + data { + id 125 + ssIdNumber 4 + name "delta_u_max" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [122 124 126] + } + data { + id 126 + ssIdNumber 7 + name "noSpikes" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [122 125 127] + } + data { + id 127 + ssIdNumber 9 + name "res_check_spikes" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [122 126 0] + } + junction { + id 128 + position [23.5747 49.5747 7] + chart 122 + subviewer 122 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [122 0 0] + } + transition { + id 129 + labelString "{eML_blk_kernel();}" + labelPosition [40.125 31.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 128 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 122 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 122 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [122 0 0] + } + instance { + id 130 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/MAT" + "LAB Function" + chart 122 + } + chart { + id 131 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/One Foot Two Fee" + "t QP Selector" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 132 0 0] + viewObj 131 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 15 + disableImplicitCasting 1 + eml { + name "robotIsOnSingleSupportQP_FCN" + } + firstData 133 + firstTransition 136 + firstJunction 135 + } + state { + id 132 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 131 + treeNode [131 0 0 0] + superState SUBCHART + subviewer 131 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function onOneFoot = robotIsOnSingleSupportQP_FCN(feetContactStatus)\n\n onOneFoot = wbc.robot" + "IsOnSingleSupportQP(feetContactStatus);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 133 + ssIdNumber 4 + name "feetContactStatus" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [131 0 134] + } + data { + id 134 + ssIdNumber 5 + name "onOneFoot" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [131 133 0] + } + junction { + id 135 + position [23.5747 49.5747 7] + chart 131 + subviewer 131 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [131 0 0] + } + transition { + id 136 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 135 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 131 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 131 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [131 0 0] + } + instance { + id 137 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/One Foot Two Fee" + "t QP Selector" + chart 131 + } + chart { + id 138 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Momentum Based Balancing Controller\n" + windowPosition [352.761 488.141 161 328] + viewLimits [0 156.75 0 153.75] + screen [1 1 1280 1024 1.000677131425054] + treeNode [0 139 0 0] + viewObj 138 + ssIdHighWaterMark 82 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 17 + disableImplicitCasting 1 + eml { + name "momentumBasedControllerFCN" + } + firstData 140 + firstTransition 178 + firstJunction 177 + } + state { + id 139 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 138 + treeNode [138 0 0 0] + superState SUBCHART + subviewer 138 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function [HessianMatrixOneFoot, gradientOneFoot, ConstraintsMatrixOneFoot, bVectorConstraintsOneF" + "oot, ...\n HessianMatrixTwoFeet, gradientTwoFeet, ConstraintsMatrixTwoFeet, bVectorConstraintsTwoFeet, " + "...\n tauModel, Sigma, Na, f_LDot] = ...\n momentumBasedControllerFCN(feetContactStatus, " + "ConstraintsMatrix, bVectorConstraints, jointPos, jointPos_des, nu, M, h, L, intL_angMomError, w_H_l_sole, w_H_r_" + "sole, ...\n J_l_sole, J_r_sole, JDot_l_sole_nu, JDot_r_sole_nu, pos_CoM," + " J_CoM, desired_pos_vel_acc_CoM, KP_CoM, KD_CoM, KP_postural, Reg, Gain, Config)\n " + " \n [HessianMatrixOneFoot, gradientOneFoot, ConstraintsMatrixOneFoot, bVectorConstraintsOne" + "Foot, ...\n HessianMatrixTwoFeet, gradientTwoFeet, ConstraintsMatrixTwoFeet, bVectorConstraintsTwoFeet, ...\n" + " tauModel, Sigma, Na, f_LDot] = ...\n momentumBasedController(feetContactStatus, ConstraintsMatrix, " + "bVectorConstraints, jointPos, jointPos_des, nu, M, h, L, intL_angMomError, w_H_l_sole, w_H_r_sole, ...\n " + " J_l_sole, J_r_sole, JDot_l_sole_nu, JDot_r_sole_nu, pos_CoM, J_CoM, desired_pos_vel_acc_" + "CoM, KP_CoM, KD_CoM, KP_postural, Config, Reg, Gain);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 140 + ssIdNumber 66 + name "HessianMatrixOneFoot" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 0 141] + } + data { + id 141 + ssIdNumber 64 + name "gradientOneFoot" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 140 142] + } + data { + id 142 + ssIdNumber 5 + name "ConstraintsMatrixOneFoot" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 141 143] + } + data { + id 143 + ssIdNumber 52 + name "bVectorConstraintsOneFoot" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 142 144] + } + data { + id 144 + ssIdNumber 69 + name "HessianMatrixTwoFeet" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 143 145] + } + data { + id 145 + ssIdNumber 70 + name "gradientTwoFeet" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 144 146] + } + data { + id 146 + ssIdNumber 53 + name "ConstraintsMatrixTwoFeet" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 145 147] + } + data { + id 147 + ssIdNumber 54 + name "bVectorConstraintsTwoFeet" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 146 148] + } + data { + id 148 + ssIdNumber 57 + name "tauModel" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 147 149] + } + data { + id 149 + ssIdNumber 58 + name "Sigma" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 148 150] + } + data { + id 150 + ssIdNumber 62 + name "Na" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 149 151] + } + data { + id 151 + ssIdNumber 63 + name "f_LDot" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 150 152] + } + data { + id 152 + ssIdNumber 13 + name "feetContactStatus" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 151 153] + } + data { + id 153 + ssIdNumber 50 + name "ConstraintsMatrix" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 152 154] + } + data { + id 154 + ssIdNumber 51 + name "bVectorConstraints" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 153 155] + } + data { + id 155 + ssIdNumber 14 + name "jointPos" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 154 156] + } + data { + id 156 + ssIdNumber 4 + name "jointPos_des" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 155 157] + } + data { + id 157 + ssIdNumber 7 + name "nu" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 156 158] + } + data { + id 158 + ssIdNumber 8 + name "M" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 157 159] + } + data { + id 159 + ssIdNumber 9 + name "h" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 158 160] + } + data { + id 160 + ssIdNumber 11 + name "L" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 159 161] + } + data { + id 161 + ssIdNumber 6 + name "intL_angMomError" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 160 162] + } + data { + id 162 + ssIdNumber 42 + name "w_H_l_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 161 163] + } + data { + id 163 + ssIdNumber 12 + name "w_H_r_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 162 164] + } + data { + id 164 + ssIdNumber 77 + name "J_l_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 163 165] + } + data { + id 165 + ssIdNumber 38 + name "J_r_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 164 166] + } + data { + id 166 + ssIdNumber 32 + name "JDot_l_sole_nu" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 165 167] + } + data { + id 167 + ssIdNumber 33 + name "JDot_r_sole_nu" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 166 168] + } + data { + id 168 + ssIdNumber 40 + name "pos_CoM" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 167 169] + } + data { + id 169 + ssIdNumber 16 + name "J_CoM" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 168 170] + } + data { + id 170 + ssIdNumber 15 + name "desired_pos_vel_acc_CoM" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 169 171] + } + data { + id 171 + ssIdNumber 17 + name "KP_CoM" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 170 172] + } + data { + id 172 + ssIdNumber 81 + name "KD_CoM" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 171 173] + } + data { + id 173 + ssIdNumber 82 + name "KP_postural" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 172 174] + } + data { + id 174 + ssIdNumber 20 + name "Reg" + scope PARAMETER_DATA + paramIndexForInitFromWorkspace 2 + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 173 175] + } + data { + id 175 + ssIdNumber 47 + name "Gain" + scope PARAMETER_DATA + paramIndexForInitFromWorkspace 1 + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 174 176] + } + data { + id 176 + ssIdNumber 19 + name "Config" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [138 175 0] + } + junction { + id 177 + position [23.5747 49.5747 7] + chart 138 + subviewer 138 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [138 0 0] + } + transition { + id 178 + labelString "{eML_blk_kernel();}" + labelPosition [36.125 25.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 177 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 138 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 138 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [138 0 0] + } + instance { + id 179 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Momentum Based Balancing Controller\n" + chart 138 + } + chart { + id 180 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" + windowPosition [369.958 -65.92 200 534.4] + viewLimits [0 156.75 0 153.75] + screen [1 1 1366 768 1.25] + treeNode [0 181 0 0] + viewObj 180 + ssIdHighWaterMark 9 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 18 + disableImplicitCasting 1 + eml { + name "checkInputRangeFCN" + } + supportVariableSizing 0 + firstData 182 + firstTransition 189 + firstJunction 188 + } + state { + id 181 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 180 + treeNode [180 0 0 0] + superState SUBCHART + subviewer 180 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function [inRange, res_check_range] = checkInputRangeFCN(umin, umax, u, tol)\n\n [inRange, re" + "s_check_range] = wbc.checkInputRange(umin, umax, u, tol);\nend" + editorLayout "100 M4x1[10 5 700 500]" + fimathString "fimath(...\n'RoundMode', 'floor',...\n'OverflowMode', 'wrap',...\n'ProductMode', 'KeepLSB', " + "'ProductWordLength', 32,...\n'SumMode', 'KeepLSB', 'SumWordLength', 32,...\n'CastBeforeSum', true)" + fimathForFiConstructors FimathMatlabFactoryDefault + emlDefaultFimath FimathUserSpecified + } + } + data { + id 182 + ssIdNumber 4 + name "umin" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [180 0 183] + } + data { + id 183 + ssIdNumber 5 + name "umax" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [180 182 184] + } + data { + id 184 + ssIdNumber 6 + name "u" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [180 183 185] + } + data { + id 185 + ssIdNumber 7 + name "inRange" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [180 184 186] + } + data { + id 186 + ssIdNumber 8 + name "tol" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [180 185 187] + } + data { + id 187 + ssIdNumber 9 + name "res_check_range" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [180 186 0] + } + junction { + id 188 + position [23.5747 49.5747 7] + chart 180 + subviewer 180 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [180 0 0] + } + transition { + id 189 + labelString "{eML_blk_kernel();}" + labelPosition [40.125 31.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 188 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 180 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 180 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [180 0 0] + } + instance { + id 190 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" + chart 180 + } + chart { + id 191 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Select ba" + "se to world transform" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 192 0 0] + viewObj 191 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 19 + disableImplicitCasting 1 + eml { + name "footOnGround" + } + firstData 193 + firstTransition 196 + firstJunction 195 + } + state { + id 192 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 191 + treeNode [191 0 0 0] + superState SUBCHART + subviewer 191 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function booleanState = footOnGround(state)\n\n % output: a boolean variable whose value is 1 " + "if the foot on ground is left\n % foot, and 0 if the foot on ground is the right foot. If both feet are on\n " + " % ground, the variable is setted to 1.\n\n % states in which right foot is on ground: 9,10,11,12 (HARD COD" + "ED)\n booleanState = 1;\n\n if state == 9 || state == 10 || state == 11 || state == 12\n\n booleanS" + "tate = 0;\n end\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 193 + ssIdNumber 4 + name "state" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [191 0 194] + } + data { + id 194 + ssIdNumber 5 + name "booleanState" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [191 193 0] + } + junction { + id 195 + position [23.5747 49.5747 7] + chart 191 + subviewer 191 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [191 0 0] + } + transition { + id 196 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 195 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 191 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 191 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [191 0 0] + } + instance { + id 197 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Select ba" + "se to world transform" + chart 191 + } + chart { + id 198 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Get Equiv" + "alent Base Velocity" + windowPosition [357.12 483.496 167 391] + viewLimits [0 156.75 0 153.75] + screen [1 1 1280 1024 1.041666666666667] + treeNode [0 199 0 0] + viewObj 198 + ssIdHighWaterMark 10 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 20 + disableImplicitCasting 1 + eml { + name "getEquivalentBaseVel_FCN" + } + firstData 200 + firstTransition 207 + firstJunction 206 + } + state { + id 199 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 198 + treeNode [198 0 0 0] + superState SUBCHART + subviewer 198 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function baseVel_equivalent = getEquivalentBaseVel_FCN(J_l_sole, J_r_sole, feetContactStatus, joi" + "ntPos_err, Reg)\n\n if feetContactStatus(1) == 1\n \n pinvJb = (J_l_sole(:,1:6)'*J_l_so" + "le(:,1:6) + Reg.pinvDamp_baseVel*eye(6))\\J_l_sole(:,1:6)';\n baseVel_equivalent = -pinvJb*J_l_sole(:,7:e" + "nd)*jointPos_err;\n else\n pinvJb = (J_r_sole(:,1:6)'*J_r_sole(:,1:6) + Reg.pinvDamp_baseV" + "el*eye(6))\\J_r_sole(:,1:6)';\n baseVel_equivalent = -pinvJb*J_r_sole(:,7:end)*jointPos_err;\n end\nen" + "d" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 200 + ssIdNumber 4 + name "J_l_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [198 0 201] + } + data { + id 201 + ssIdNumber 6 + name "J_r_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [198 200 202] + } + data { + id 202 + ssIdNumber 9 + name "feetContactStatus" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [198 201 203] + } + data { + id 203 + ssIdNumber 10 + name "jointPos_err" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [198 202 204] + } + data { + id 204 + ssIdNumber 7 + name "baseVel_equivalent" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [198 203 205] + } + data { + id 205 + ssIdNumber 8 + name "Reg" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [198 204 0] + } + junction { + id 206 + position [23.5747 49.5747 7] + chart 198 + subviewer 198 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [198 0 0] + } + transition { + id 207 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 206 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 198 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 198 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [198 0 0] + } + instance { + id 208 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Get Equiv" + "alent Base Velocity" + chart 198 + } + chart { + id 209 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Anal" + "ytical Solution Two Feet (unconstrained)" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 210 0 0] + viewObj 209 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 24 + disableImplicitCasting 1 + eml { + name "analyticalSolutionQP_FCN" + } + firstData 211 + firstTransition 215 + firstJunction 214 + } + state { + id 210 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 209 + treeNode [209 0 0 0] + superState SUBCHART + subviewer 209 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function analyticalSolution = analyticalSolutionQP_FCN(H,g)\n\n analyticalSolution = wbc.analy" + "ticalSolutionQP(H,g);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 211 + ssIdNumber 4 + name "H" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [209 0 212] + } + data { + id 212 + ssIdNumber 5 + name "analyticalSolution" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [209 211 213] + } + data { + id 213 + ssIdNumber 6 + name "g" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [209 212 0] + } + junction { + id 214 + position [23.5747 49.5747 7] + chart 209 + subviewer 209 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [209 0 0] + } + transition { + id 215 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 214 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 209 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 209 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [209 0 0] + } + instance { + id 216 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Anal" + "ytical Solution Two Feet (unconstrained)" + chart 209 + } + chart { + id 217 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Proc" + "ess QP output" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 218 0 0] + viewObj 217 + ssIdHighWaterMark 14 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 25 + disableImplicitCasting 1 + eml { + name "processOutputQP_twoFeetFCN" + } + firstData 219 + firstTransition 225 + firstJunction 224 + } + state { + id 218 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 217 + treeNode [217 0 0 0] + superState SUBCHART + subviewer 217 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function f_star = processOutputQP_twoFeetFCN(analyticalSolution,primalSolution,QPStatus,Config)\n" + "\n f_star = processOutputQP_twoFeet(analyticalSolution,primalSolution,QPStatus,Config);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 219 + ssIdNumber 7 + name "analyticalSolution" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [217 0 220] + } + data { + id 220 + ssIdNumber 4 + name "primalSolution" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [217 219 221] + } + data { + id 221 + ssIdNumber 6 + name "QPStatus" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [217 220 222] + } + data { + id 222 + ssIdNumber 5 + name "f_star" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [217 221 223] + } + data { + id 223 + ssIdNumber 10 + name "Config" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [217 222 0] + } + junction { + id 224 + position [23.5747 49.5747 7] + chart 217 + subviewer 217 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [217 0 0] + } + transition { + id 225 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 224 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 217 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 217 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [217 0 0] + } + instance { + id 226 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Proc" + "ess QP output" + chart 217 + } + chart { + id 227 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Anal" + "ytical Solution QP One Foot (unconstrained)" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 228 0 0] + viewObj 227 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 26 + disableImplicitCasting 1 + eml { + name "analyticalSolutionQP_FCN" + } + firstData 229 + firstTransition 233 + firstJunction 232 + } + state { + id 228 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 227 + treeNode [227 0 0 0] + superState SUBCHART + subviewer 227 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function analyticalSolution = analyticalSolutionQP_FCN(H,g)\n\n analyticalSolution = wbc.analy" + "ticalSolutionQP(H,g);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 229 + ssIdNumber 4 + name "H" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [227 0 230] + } + data { + id 230 + ssIdNumber 5 + name "analyticalSolution" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [227 229 231] + } + data { + id 231 + ssIdNumber 6 + name "g" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [227 230 0] + } + junction { + id 232 + position [23.5747 49.5747 7] + chart 227 + subviewer 227 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [227 0 0] + } + transition { + id 233 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 232 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 227 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 227 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [227 0 0] + } + instance { + id 234 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Anal" + "ytical Solution QP One Foot (unconstrained)" + chart 227 + } + chart { + id 235 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Proc" + "ess QP output" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 236 0 0] + viewObj 235 + ssIdHighWaterMark 14 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 27 + disableImplicitCasting 1 + eml { + name "processOutputQP_oneFoot" + } + firstData 237 + firstTransition 244 + firstJunction 243 + } + state { + id 236 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 235 + treeNode [235 0 0 0] + superState SUBCHART + subviewer 235 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function f_star = processOutputQP_oneFoot(analyticalSolution,primalSolution,QPStatus,feetContact" + "Status,Config)\n\n f_star = processOutputQP_oneFoot(analyticalSolution,primalSolution,QPStatus,feetContactS" + "tatus,Config);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 237 + ssIdNumber 7 + name "analyticalSolution" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [235 0 238] + } + data { + id 238 + ssIdNumber 4 + name "primalSolution" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [235 237 239] + } + data { + id 239 + ssIdNumber 6 + name "QPStatus" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [235 238 240] + } + data { + id 240 + ssIdNumber 14 + name "feetContactStatus" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [235 239 241] + } + data { + id 241 + ssIdNumber 5 + name "f_star" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [235 240 242] + } + data { + id 242 + ssIdNumber 10 + name "Config" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [235 241 0] + } + junction { + id 243 + position [23.5747 49.5747 7] + chart 235 + subviewer 235 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [235 0 0] + } + transition { + id 244 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 243 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 235 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 235 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [235 0 0] + } + instance { + id 245 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Proc" + "ess QP output" + chart 235 + } + target { + id 246 + machine 1 + name "sfun" + codeFlags "" + linkNode [1 0 247] + } + target { + id 247 + machine 1 + name "rtw" + linkNode [1 246 0] + } +} diff --git a/library/simulink-library/Utilities/utilityBlocks_lib.slx b/library/simulink-library/Utilities/utilityBlocks_lib.slx index c4fb101f..0a85f1c1 100644 Binary files a/library/simulink-library/Utilities/utilityBlocks_lib.slx and b/library/simulink-library/Utilities/utilityBlocks_lib.slx differ