From 15708f75a036e207e397a94e224b36d079bbd88c Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Tue, 27 Apr 2021 07:06:23 +0200 Subject: [PATCH 01/20] Create floating-base-balancing-torque-control-with-simulator as a duplicate of floating-base-balancing-torque-control --- .../CMakeLists.txt | 1 + .../README.md | 29 + .../app/robots/iCubGazeboV2_5/configRobot.m | 100 + .../iCubGazeboV2_5/configStateMachine.m | 160 + .../iCubGazeboV2_5/gainsAndReferences.m | 342 + .../app/robots/iCubGenova02/configRobot.m | 99 + .../robots/iCubGenova02/configStateMachine.m | 160 + .../robots/iCubGenova02/gainsAndReferences.m | 346 + .../app/robots/iCubGenova04/configRobot.m | 99 + .../robots/iCubGenova04/configStateMachine.m | 160 + .../robots/iCubGenova04/gainsAndReferences.m | 351 + .../app/robots/icubGazeboSim/configRobot.m | 99 + .../robots/icubGazeboSim/configStateMachine.m | 159 + .../robots/icubGazeboSim/gainsAndReferences.m | 238 + .../initTorqueControlBalancing.m | 83 + .../src-static-gui/closeModel.m | 20 + .../src-static-gui/compileModel.m | 24 + .../src/momentumBasedController.m | 328 + .../src/processOutputQP_oneFoot.m | 57 + .../src/processOutputQP_twoFeet.m | 41 + .../src/stateMachineMomentumControl.m | 393 + .../startModelWithStaticGui.m | 29 + .../stopTorqueControlBalancing.m | 93 + .../torqueControlBalancing.mdl | 23398 ++++++++++++++++ 24 files changed, 26809 insertions(+) create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/CMakeLists.txt create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/README.md create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobot.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/configRobot.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/configStateMachine.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/gainsAndReferences.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configStateMachine.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/gainsAndReferences.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/configRobot.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/configStateMachine.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/gainsAndReferences.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancing.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/src-static-gui/closeModel.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/src-static-gui/compileModel.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/src/momentumBasedController.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/src/processOutputQP_oneFoot.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/src/processOutputQP_twoFeet.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/src/stateMachineMomentumControl.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/startModelWithStaticGui.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/stopTorqueControlBalancing.m create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancing.mdl 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..17b0039c --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/CMakeLists.txt @@ -0,0 +1 @@ +register_mdl(MODELNAME "torqueControlBalancing") \ 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..cff9891b --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobot.m @@ -0,0 +1,100 @@ +% 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 + +% 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 = false; + +% 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 = true; + +% 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/configStateMachine.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m new file mode 100644 index 00000000..bace98b0 --- /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; +Reg.pinvDamp = 1; +Reg.pinvTol = 1e-5; +Reg.KP_postural = 0.1; +Reg.KD_postural = 0; +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 = true; % 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..7b2eae33 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m @@ -0,0 +1,342 @@ +% 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 + +Gain.KD_CoM = 2*sqrt(Gain.KP_CoM)/20; + +% 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,:))/20; + +% 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/app/robots/iCubGenova02/configRobot.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/configRobot.m new file mode 100644 index 00000000..5d39ceb4 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/configRobot.m @@ -0,0 +1,99 @@ +% CONFIGROBOT initializes parameters specific of a particular robot +% (e.g., icuGazeboSim) + +%% --- Initialization --- +Config.ON_GAZEBO = false; +ROBOT_DOF = 23; +Config.GRAV_ACC = 9.81; + +% Robot configuration for WBToolbox +WBTConfigRobot = WBToolbox.Configuration; +WBTConfigRobot.RobotName = 'icub'; +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 + +% 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 = true; + +% 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 = true; +Config.INCLUDE_COUPLING = true; +Config.INCLUDE_HARMONIC_DRIVE_INERTIA = true; + +% 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_foot/cartesianEndEffectorWrench:o'; +Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o'; +Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; +Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; + +% Ports dimensions +Ports.NECK_POS_PORT_SIZE = 6; +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/iCubGenova02/configStateMachine.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/configStateMachine.m new file mode 100644 index 00000000..7107ed3c --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/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; +Reg.pinvDamp = 0.07; +Reg.pinvTol = 1e-5; +Reg.KP_postural = 0.1; +Reg.KD_postural = 0; +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/iCubGenova02/gainsAndReferences.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/gainsAndReferences.m new file mode 100644 index 00000000..61318961 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/gainsAndReferences.m @@ -0,0 +1,346 @@ +% GAINSANDREFERENCES compute gains matrices, references and other control +% related quantities for each state of the state machine. + +%% --- Initialization --- + +% CoM gains +Gain.KP_CoM = [50 100 5 % state == 1 TWO FEET BALANCING + 50 100 5 % state == 2 COM TRANSITION TO LEFT + 50 100 5 % state == 3 LEFT FOOT BALANCING + 50 100 5 % state == 4 YOGA LEFT FOOT + 50 100 5 % state == 5 PREPARING FOR SWITCHING + 50 100 5 % state == 6 LOOKING FOR CONTACT + 50 100 5 % state == 7 TRANSITION TO INITIAL POSITION + 50 100 5 % state == 8 COM TRANSITION TO RIGHT FOOT + 50 100 5 % state == 9 RIGHT FOOT BALANCING + 50 100 5 % state == 10 YOGA RIGHT FOOT + 50 100 5 % state == 11 PREPARING FOR SWITCHING + 50 100 5 % state == 12 LOOKING FOR CONTACT + 50 100 5];% state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_CoM = 2*sqrt(Gain.KP_CoM)/20; + +% Angular momentum gains +Gain.KI_AngularMomentum = 3; +Gain.KP_AngularMomentum = 2*sqrt(Gain.KI_AngularMomentum)/5; + +% 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 10,220 550 220 200 65 300 % state == 5 PREPARING FOR SWITCHING + 30 30 30, 10 10 20 10, 10 10 20 10,100 350 20 200 10 100,220 550 220 200 65 300 % state == 6 LOOKING FOR CONTACT + 30 30 30, 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 50 30 60 100 100, 30 30 20 20 100 100 % state == 9 RIGHT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10,100 50 30 100 100 100,100 200 100 100 10 10 % state == 10 YOGA RIGHT FOOT + 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,200 250 20 20 10 10 % state == 11 PREPARING FOR SWITCHING + 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100 % state == 12 LOOKING FOR CONTACT + 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100];% state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_postural = 2*sqrt(Gain.KP_postural(1,:))/20; + +% symmetric gains +Gain.KP_postural(10,18:23) = Gain.KP_postural(10,18:23)*1.5; +Gain.KP_postural(10,1:3) = Gain.KP_postural(10,1:3)*2; +Gain.KP_postural(4,12:17) = Gain.KP_postural(4,12:17)*1.5; +Gain.KP_postural(4,1:3) = Gain.KP_postural(4,1:3)*2; +Gain.KP_postural(4,4:11) = Gain.KP_postural(4,4:11)*1.5; +Gain.KP_postural(7,2:3) = Gain.KP_postural(7,2:3)*1.5; + +%% 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 + 0.65; %% 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 + 0.65; %% state == 10 YOGA RIGHT FOOT + 2; %% state == 11 PREPARING FOR SWITCHING + 5; %% 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 + 0.65; %% 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 + 0.65; %% state == 10 YOGA RIGHT FOOT + 2; %% state == 11 PREPARING FOR SWITCHING + 5; %% 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.005, 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.02, -0.08, 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.01, 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.1060,-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.1279, 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.3273,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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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/app/robots/iCubGenova04/configRobot.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m new file mode 100644 index 00000000..677c8483 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m @@ -0,0 +1,99 @@ +% CONFIGROBOT initializes parameters specific of a particular robot +% (e.g., icuGazeboSim) + +%% --- Initialization --- +Config.ON_GAZEBO = false; +ROBOT_DOF = 23; +Config.GRAV_ACC = 9.81; + +% Robot configuration for WBToolbox +WBTConfigRobot = WBToolbox.Configuration; +WBTConfigRobot.RobotName = 'icub'; +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 + +% 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 = true; + +% 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 = true; +Config.INCLUDE_COUPLING = true; +Config.INCLUDE_HARMONIC_DRIVE_INERTIA = true; + +% 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.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; +Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; +Ports.WRENCH_LEFT_FOOT = '/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o'; +Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o'; + +% Ports dimensions +Ports.NECK_POS_PORT_SIZE = 6; +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/iCubGenova04/configStateMachine.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configStateMachine.m new file mode 100644 index 00000000..6c1d1788 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/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; +Reg.pinvDamp = 0.07; +Reg.pinvTol = 1e-5; +Reg.KP_postural = 0.1; +Reg.KD_postural = 0; +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.02; +StateMachine.joints_thresholdNotInContact = 15; +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 = 1; + +% 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 = true; + +%% 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/iCubGenova04/gainsAndReferences.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/gainsAndReferences.m new file mode 100644 index 00000000..87758ee4 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/gainsAndReferences.m @@ -0,0 +1,351 @@ +% GAINSANDREFERENCES compute gains matrices, references and other control +% related quantities for each state of the state machine. + +%% --- Initialization --- + +% CoM gains +Gain.KP_CoM = [50 100 5 % state == 1 TWO FEET BALANCING + 50 100 5 % state == 2 COM TRANSITION TO LEFT + 50 100 5 % state == 3 LEFT FOOT BALANCING + 50 100 5 % state == 4 YOGA LEFT FOOT + 50 100 5 % state == 5 PREPARING FOR SWITCHING + 50 100 5 % state == 6 LOOKING FOR CONTACT + 50 100 5 % state == 7 TRANSITION TO INITIAL POSITION + 50 150 5 % state == 8 COM TRANSITION TO RIGHT FOOT + 50 100 5 % state == 9 RIGHT FOOT BALANCING + 50 100 5 % state == 10 YOGA RIGHT FOOT + 50 100 5 % state == 11 PREPARING FOR SWITCHING + 50 100 5 % state == 12 LOOKING FOR CONTACT + 50 100 5];% state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_CoM = 2*sqrt(Gain.KP_CoM)/15; + +% Angular momentum gains +Gain.KI_AngularMomentum = 3 ; +Gain.KP_AngularMomentum = 2*sqrt(Gain.KI_AngularMomentum)/5; + +% 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 50 100 100 % state == 4 YOGA LEFT FOOT + 30 30 30, 5 5 10 10, 10 10 20 10,200 250 20 20 10 10,220 550 220 200 65 300 % state == 5 PREPARING FOR SWITCHING + 30 30 30, 10 10 20 10, 10 10 20 10,100 350 20 200 10 100,220 550 220 200 65 300 % 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 50 30 60 100 100, 30 30 20 20 100 100 % state == 9 RIGHT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10,100 50 30 50 100 100,100 200 100 100 10 10 % state == 10 YOGA RIGHT FOOT + 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,200 250 20 20 10 10 % state == 11 PREPARING FOR SWITCHING + 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100 % state == 12 LOOKING FOR CONTACT + 30 40 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100];% state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_postural = 0*sqrt(Gain.KP_postural(1,:))/20; + +% symmetric gains +Gain.KP_postural(4,1:3) = Gain.KP_postural(4,1:3)*3; +Gain.KP_postural(6,18:23) = Gain.KP_postural(6,18:23)*2; +Gain.KP_postural(10,1:3) = Gain.KP_postural(10,1:3)*3; +Gain.KP_postural(13,1:3) = Gain.KP_postural(13,1:3)*3; + +%% 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 + 0.9; %% 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 + 0.9; %% state == 10 YOGA RIGHT FOOT + 2; %% state == 11 PREPARING FOR SWITCHING + 2; %% state == 12 LOOKING FOR CONTACT + 5]; %% 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 + 0.9; %% 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 + 0.9; %% state == 10 YOGA RIGHT FOOT + 2; %% state == 11 PREPARING FOR SWITCHING + 2; %% state == 12 LOOKING FOR CONTACT + 5]; %% 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.005, 0.0; %% state == 3 LEFT FOOT BALANCING + 0.0, 0.005, 0.0; %% state == 4 YOGA LEFT FOOT + 0.0, 0.005, 0.0; %% state == 5 PREPARING FOR SWITCHING + 0.02,-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.005, 0.0; %% state == 9 RIGHT FOOT BALANCING + 0.0, -0.005, 0.0; %% state == 10 YOGA RIGHT FOOT + 0.0, -0.015, 0.0; %% state == 11 PREPARING FOR SWITCHING + 0.02, 0.02, 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.1060,-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.1279, 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.3273,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]; + +q18 = [-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]; + +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),q18]; + +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/app/robots/icubGazeboSim/configRobot.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/configRobot.m new file mode 100644 index 00000000..81647ffb --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/configRobot.m @@ -0,0 +1,99 @@ +% CONFIGROBOT initializes parameters specific of a particular robot +% (e.g., icuGazeboSim) + +%% --- 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 + +% 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 = false; + +% 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 = true; + +% 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 = 6; +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/icubGazeboSim/configStateMachine.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/configStateMachine.m new file mode 100644 index 00000000..edc45cc0 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/configStateMachine.m @@ -0,0 +1,159 @@ +% 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; +Reg.pinvDamp = 1; +Reg.pinvTol = 1e-5; +Reg.KP_postural = 0.1; +Reg.KD_postural = 0; +Reg.HessianQP = 1e-7; + +%% State Machine configuration + +% time between two yoga positions +StateMachine.joints_pauseBetweenYogaMoves = 3; + +% contact forces threshold +StateMachine.wrench_thresholdContactOn = 25; +StateMachine.wrench_thresholdContactOff = 85; + +% 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.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.05 0.10; % xMin, xMax + -0.025 0.025]; % 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/icubGazeboSim/gainsAndReferences.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/gainsAndReferences.m new file mode 100644 index 00000000..3f854278 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/gainsAndReferences.m @@ -0,0 +1,238 @@ +% GAINSANDREFERENCES compute gains matrices, references and other control +% related quantities for each state of the state machine. + +%% --- Initialization --- + +% CoM gains +Gain.KP_CoM = [10 50 10 % state == 1 TWO FEET BALANCING + 10 50 10 % state == 2 COM TRANSITION TO LEFT + 10 50 10 % state == 3 LEFT FOOT BALANCING + 10 50 10 % state == 4 YOGA LEFT FOOT + 10 50 10 % state == 5 PREPARING FOR SWITCHING + 10 50 10 % state == 6 LOOKING FOR CONTACT + 10 50 10 % state == 7 TRANSITION TO INITIAL POSITION + 10 50 10 % state == 8 COM TRANSITION TO RIGHT FOOT + 10 50 10 % state == 9 RIGHT FOOT BALANCING + 10 50 10 % state == 10 YOGA RIGHT FOOT + 10 50 10 % state == 11 PREPARING FOR SWITCHING + 10 50 10 % state == 12 LOOKING FOR CONTACT + 10 50 10]; % state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_CoM = 2*sqrt(Gain.KP_CoM); + +% 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 50 30 60 50 50, 30 50 30 60 50 50 % state == 1 TWO FEET BALANCING + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 2 COM TRANSITION TO LEFT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 3 LEFT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10, 50 50 250 200 50 50, 50 50 50 50 50 50 % state == 4 YOGA LEFT FOOT + 30 30 30, 10 10 10 10, 10 10 10 10, 30 50 300 60 50 50, 30 50 30 60 50 50 % state == 5 PREPARING FOR SWITCHING + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 6 LOOKING FOR CONTACT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 7 TRANSITION TO INITIAL POSITION + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 8 COM TRANSITION TO RIGHT FOOT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 9 RIGHT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10, 50 50 50 50 50 50, 50 50 250 200 50 50 % state == 10 YOGA RIGHT FOOT + 30 30 30, 10 10 10 10, 10 10 10 10, 30 50 30 60 50 50, 30 50 300 60 50 50 % state == 11 PREPARING FOR SWITCHING + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 12 LOOKING FOR CONTACT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50];% state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_postural = 0*sqrt(Gain.KP_postural(1,:)); + +%% Smoothing times + +% Smoothing time gain scheduling +Config.SmoothingTimeGainScheduling = 2; + +% Smoothing time CoM references +StateMachine.CoMSmoothingTime = [5; %% state == 1 TWO FEET BALANCING + 5; %% state == 2 COM TRANSITION TO LEFT FOOT + 3; %% state == 3 LEFT FOOT BALANCING + 4; %% state == 4 YOGA LEFT FOOT + 5; %% state == 5 PREPARING FOR SWITCHING + 5; %% state == 6 LOOKING FOR CONTACT + 4; %% state == 7 TRANSITION INIT POSITION + 5; %% state == 8 COM TRANSITION TO RIGHT FOOT + 3; %% state == 9 RIGHT FOOT BALANCING + 4; %% state == 10 YOGA RIGHT FOOT + 5; %% state == 11 PREPARING FOR SWITCHING + 5; %% state == 12 LOOKING FOR CONTACT + 4]; %% state == 13 TRANSITION INIT POSITION + +% Smoothing time for joints references +StateMachine.jointsSmoothingTime = [5; %% state == 1 TWO FEET BALANCING + 5; %% state == 2 COM TRANSITION TO LEFT FOOT + 3; %% state == 3 LEFT FOOT BALANCING + 4; %% state == 4 YOGA LEFT FOOT + 5; %% state == 5 PREPARING FOR SWITCHING + 5; %% state == 6 LOOKING FOR CONTACT + 4; %% state == 7 TRANSITION INIT POSITION + 5; %% state == 8 COM TRANSITION TO RIGHT FOOT + 3; %% state == 9 RIGHT FOOT BALANCING + 4; %% state == 10 YOGA RIGHT FOOT + 5; %% state == 11 PREPARING FOR SWITCHING + 5; %% state == 12 LOOKING FOR CONTACT + 4]; %% 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.01, 0.0; %% state == 2 COM TRANSITION TO LEFT FOOT + 0.0, 0.00, 0.0; %% state == 3 LEFT FOOT BALANCING + 0.0, 0.01, 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.01, 0.0; %% state == 8 COM TRANSITION TO RIGHT FOOT + 0.0, 0.00, 0.0; %% state == 9 RIGHT FOOT BALANCING + 0.0, -0.00, 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]; + +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]; + +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)]; + +% 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/initTorqueControlBalancing.m b/controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancing.m new file mode 100644 index 00000000..aa9da74c --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancing.m @@ -0,0 +1,83 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% /** +% * 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; + +%% 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 +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')); + +% 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/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/stopTorqueControlBalancing.m b/controllers/floating-base-balancing-torque-control-with-simulator/stopTorqueControlBalancing.m new file mode 100644 index 00000000..17d8c535 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/stopTorqueControlBalancing.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/torqueControlBalancing.mdl b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancing.mdl new file mode 100644 index 00000000..70b3493d --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancing.mdl @@ -0,0 +1,23398 @@ +Model { + Name "torqueControlBalancing" + Version 10.0 + SavedCharacterEncoding "UTF-8" + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.3516" + 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 55 + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Configuration" + Path "torqueControlBalancing/Configuration" + SID "4537" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "torqueControlBalancing/Dump and visualize/Visualizer/GetMeasurement1" + SID "4543" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "torqueControlBalancing/GetMeasurement" + SID "4539" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "torqueControlBalancing/GetMeasurement1" + SID "4538" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "torqueControlBalancing/GetMeasurement2" + SID "4541" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "torqueControlBalancing/IMU_meas" + SID "2630" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MatchSignalSizes" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torque" + "s/QPSolver/QP One Foot/MatchSignalSizes" + SID "4691" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/QP" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torque" + "s/QPSolver/QP One Foot/QP One Foot" + SID "4693" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MatchSignalSizes" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torque" + "s/QPSolver/QP Two Feet/MatchSignalSizes" + SID "4679" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/QP" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torque" + "s/QPSolver/QP Two Feet/QP Two Feet" + SID "4681" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" + "um integral error/CentroidalMomentum" + SID "3718" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" + "um 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" + "um 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" + "um integral error/Jacobian" + SID "3719" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" + "um integral error/Jacobian1" + SID "3720" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/CentroidalMom" + "entum" + SID "2345" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/GetBiasForces" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/GetBiasForces" + SID "2348" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/MassMatrix" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/MassMatrix" + SID "2349" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/CoM" + SID "4363" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/DotJNu" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/DotJNu l_so" + "le" + SID "2375" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/DotJNu" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/DotJNu r_so" + "le" + SID "2376" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Jacobian com" + SID "2378" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Jacobian l_" + "sole" + SID "2379" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Jacobian r_" + "sole" + SID "2380" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/l_sole" + SID "2405" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/r_sole" + SID "2406" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/holder " + SID "4559" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute State Veloc" + "ity/Feet Jacobians/Jacobian l_sole" + SID "4219" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute State Veloc" + "ity/Feet Jacobians/Jacobian r_sole" + SID "4220" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/LFoot to base link transform /Fixed base to imu transform" + SID "4250" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/LFoot to base link transform /Fixed base to root link transform" + SID "4251" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/LFoot to base link transform /Neck Position" + SID "4254" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/LFoot to base link transform /holder 1" + SID "4258" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/LFoot to base link transform /holder 2" + SID "4259" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/RFoot to base link transform/Fixed base to imu transform" + SID "4855" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/RFoot to base link transform/Fixed base to root link transform" + SID "4856" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/RFoot to base link transform/Neck Position" + SID "4860" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/RFoot to base link transform/holder 1" + SID "4865" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/RFoot to base link transform/holder 2" + SID "4866" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/Relative transform l_sole_H_base" + SID "4269" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/Relative transform r_sole_H_base" + SID "4306" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/Minim" + "umJerkTrajectoryGenerator" + SID "2176" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/holde" + "r 1" + SID "2187" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/holde" + "r 2" + SID "2188" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/xCom/" + "CoM" + SID "4229" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/xCom2" + "/CoM" + SID "4735" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and Re" + "ferences/Smooth reference CoM/MinimumJerkTrajectoryGenerator2" + SID "2153" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and Re" + "ferences/Smooth reference joint position/MinimumJerkTrajectoryGenerator1" + SID "2152" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetLimits" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF JOINTS HI" + "T THE LIMITS/GetLimits" + SID "4921" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Actuators/SetReferences" + Path "torqueControlBalancing/SetReferences" + SID "2354" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Simulator Synchronizer" + Path "torqueControlBalancing/synchronizer/GAZEBO_SYNCHRONIZER/Simulator Synchronizer" + SID "2426" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Real Time Synchronizer" + Path "torqueControlBalancing/synchronizer/REAL_TIME_SYNC/Real Time Synchronizer" + SID "2431" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Clock" + Path "torqueControlBalancing/synchronizer/Yarp Clock" + SID "4629" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "torqueControlBalancing/wrench_leftFoot" + SID "2206" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "torqueControlBalancing/wrench_rightFoot" + SID "2218" + Type "LIBRARY_BLOCK" + } + OrderedModelArguments 1 + } + AnimationPlugin "on" + LogicAnalyzerPlugin "on" + WebScopes_FoundationPlugin "on" + SLCCPlugin "on" + DiagnosticSuppressor "on" + NotesPlugin "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)));\ninitTorqueControlBalancing;\n\n%% Try to ed" + "it the GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n set(sl_synch_handles.startB" + "utton,'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)));\nstopTorqueControlBalancing;\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.checkbox_" + "recompile, 'Value') \n set(sl_synch_handles.startButton,'Backgroundcolor',[0.0,1.0,0.0]);\n set(sl_sy" + "nch_handles.startButton,'Enable','on');\n end\n\n %% Update Compile Button\n set(sl_synch_handles.compileBu" + "tton,'Backgroundcolor',[1.0,0.6,0.0]);\n set(sl_synch_handles.compileButton,'Enable','on');\n\n %% Update Exit" + " Button\n set(sl_synch_handles.exitButton,'Backgroundcolor',[0.0,1.0,1.0]);\n set(sl_synch_handles.exitButton," + "'Enable','on');\n\nend\n\n\n" + LastSavedArchitecture "glnxa64" + Object { + $PropName "BdWindowsInfo" + $ObjectID 1 + $ClassName "Simulink.BDWindowsInfo" + Object { + $PropName "WindowsInfo" + $ObjectID 2 + $ClassName "Simulink.WindowInfo" + IsActive [1] + Location [67.0, 27.0, 1853.0, 1053.0] + Object { + $PropName "ModelBrowserInfo" + $ObjectID 3 + $ClassName "Simulink.ModelBrowserInfo" + Visible [0] + DockPosition "Left" + Width [50] + Height [50] + Filter [59] + Minimized "Unset" + } + Object { + $PropName "ExplorerBarInfo" + $ObjectID 4 + $ClassName "Simulink.ExplorerBarInfo" + Visible [1] + } + Array { + Type "Simulink.EditorInfo" + Dimension 12 + Object { + $ObjectID 5 + IsActive [1] + IsTabbed [1] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [1815.0, 821.0] + ZoomFactor [3.61010101010101] + Offset [776.12199216564068, 371.79127028539449] + SceneRectInView [0.0, 0.0, 0.0, 0.0] + } + Object { + $ObjectID 6 + IsActive [0] + IsTabbed [1] + ViewObjType "SimulinkSubsys" + LoadSaveID "4917" + Extents [1815.0, 821.0] + ZoomFactor [4.3275706438106187] + Offset [10.41989407940855, -23.235071395084702] + SceneRectInView [0.0, 0.0, 0.0, 0.0] + } + Object { + $ObjectID 7 + IsActive [0] + IsTabbed [1] + ViewObjType "SimulinkSubsys" + LoadSaveID "4913" + Extents [1815.0, 821.0] + ZoomFactor [4.0050505050505052] + Offset [94.721007009531633, 186.50441361916774] + SceneRectInView [0.0, 0.0, 0.0, 0.0] + } + Object { + $ObjectID 8 + IsActive [0] + IsTabbed [1] + ViewObjType "SimulinkSubsys" + LoadSaveID "4925" + Extents [1815.0, 821.0] + ZoomFactor [3.8000000393627378] + Offset [-68.684210488152473, -33.394738158739756] + SceneRectInView [0.0, 0.0, 0.0, 0.0] + } + Object { + $ObjectID 9 + IsActive [0] + IsTabbed [1] + ViewObjType "SimulinkSubsys" + LoadSaveID "4836" + Extents [1815.0, 821.0] + ZoomFactor [1.2332814930015552] + Offset [-60.865177726986076, -5.3518284993695033] + SceneRectInView [0.0, 0.0, 0.0, 0.0] + } + Object { + $ObjectID 10 + IsActive [0] + IsTabbed [1] + ViewObjType "SimulinkSubsys" + LoadSaveID "2341" + Extents [1815.0, 821.0] + ZoomFactor [1.3863636363636365] + Offset [-1148.4886014344261, -206.09836065573768] + SceneRectInView [0.0, 0.0, 0.0, 0.0] + } + Object { + $ObjectID 11 + IsActive [0] + IsTabbed [1] + ViewObjType "SimulinkSubsys" + LoadSaveID "4321" + Extents [1815.0, 821.0] + ZoomFactor [3.0] + Offset [-211.69612560424537, 280.84982585720286] + SceneRectInView [0.0, 0.0, 0.0, 0.0] + } + Object { + $ObjectID 12 + IsActive [0] + IsTabbed [1] + 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 13 + IsActive [0] + IsTabbed [1] + 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 14 + IsActive [0] + IsTabbed [1] + 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 15 + IsActive [0] + IsTabbed [1] + 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 16 + IsActive [0] + IsTabbed [1] + 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" + } + Object { + $PropName "DockComponentsInfo" + $ObjectID 17 + $ClassName "Simulink.DockComponentInfo" + Type "GLUE2:PropertyInspector" + ID "Property Inspector" + Visible [0] + CreateCallback "" + UserData "" + Floating [0] + DockPosition "Right" + Width [640] + Height [480] + Minimized "Unset" + } + WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABiAP///wAAAAEAAAAAAAAAAPwCA" + "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" + "QBjAHQAbwByAAAAAAD/////AAAAOQD///8AAAc9AAADcgAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAA" + Array { + Type "Cell" + Dimension 0 + PropName "PersistedApps" + } + WindowUuid "" + } + BDUuid "8d887252-6855-49e7-b7fc-16933ad93043" + } + HideAutomaticNames on + Created "Tue Feb 18 10:13:36 2014" + Creator "daniele" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "gnava" + ModifiedDateFormat "%" + LastModifiedDate "Fri Aug 07 14:43:02 2020" + RTWModifiedTimeStamp 518712182 + ModelVersionFormat "1.%" + SampleTimeColors off + SampleTimeAnnotations off + 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 + SortedOrder on + VariantCondition off + ShowLinearizationAnnotations on + ShowVisualizeInsertedRTB on + 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 18 + $ClassName "Simulink.SimulationData.ModelLoggingInfo" + model_ "torqueControlBalancing" + signals_ [] + overrideMode_ [0.0] + Array { + Type "Cell" + Dimension 1 + Cell "torqueControlBalancing" + PropName "logAsSpecifiedByModels_" + } + Array { + Type "Cell" + Dimension 1 + Cell [] + PropName "logAsSpecifiedByModelsSSIDs_" + } + } + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + 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 + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + OrderedModelArguments on + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 19 + Version "19.1.0" + DisabledProps [] + Description "" + Array { + Type "Handle" + Dimension 9 + Simulink.SolverCC { + $ObjectID 20 + Version "19.1.0" + DisabledProps [] + Description "" + Components [] + StartTime "0.0" + StopTime "Config.SIMULATION_TIME " + AbsTol "auto" + AutoScaleAbsTol on + FixedStep "Config.tStep" + 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 on + ConcurrentTasks off + Solver "FixedStepDiscrete" + 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 + SolverOrder 3 + } + Simulink.DataIOCC { + $ObjectID 21 + Version "19.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 22 + Version "19.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" + 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 on + 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 23 + Version "19.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" + 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" + } + Simulink.HardwareCC { + $ObjectID 24 + Version "19.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 25 + Version "19.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 26 + Version "19.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 + SimBuildMode "sf_incremental_build" + SimGenImportedTypeDefs off + ModelFunctionsGlobalVisibility "on" + CompileTimeRecursionLimit 50 + EnableRuntimeRecursion on + MATLABDynamicMemAlloc on + MATLABDynamicMemAllocThreshold 65536 + CustomCodeFunctionArrayLayout [] + DefaultCustomCodeFunctionArrayLayout "NotSpecified" + CustomCodeUndefinedFunction "UseInterfaceOnly" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 27 + Version "19.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++" + 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 + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 28 + Version "19.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 29 + Version "19.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 30 + Version "20.0.1" + Array { + Type "Cell" + Dimension 10 + Cell "Description" + Cell "Components" + Cell "Name" + Cell "GenerateDestructor" + Cell "GenerateExternalIOAccessMethods" + Cell "ParameterMemberVisibility" + Cell "InternalMemberVisibility" + Cell "GenerateParameterAccessMethods" + Cell "GenerateInternalMemberAccessMethods" + Cell "UseOperatorNewForModelRefRegistration" + PropName "DisabledProps" + } + Description "" + Components [] + Name "CPPClassGenComp" + GenerateDestructor on + GenerateExternalIOAccessMethods "None" + ParameterMemberVisibility "private" + InternalMemberVisibility "private" + GenerateParameterAccessMethods "None" + GenerateInternalMemberAccessMethods "None" + UseOperatorNewForModelRefRegistration off + } + PropName "Components" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + GenFloatMathFcnCalls "NOT IN USE" + TargetLangStandard "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 + MultiInstanceERTCode on + 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 31 + Version "19.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 + CovHtmlReporting on + CovForceBlockReductionOff on + CovEnableCumulative on + CovSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovSaveOutputData on + CovOutputDir "slcov_output/$ModelName$" + CovDataFileName "$ModelName$_cvdata" + CovShowResultsExplorer on + 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 "Code Generation" + ConfigPrmDlgPosition [ 328, 140, 1850, 1010 ] + ExtraOptions "-aBlockCommentType=\"BlockPathComment\" -aIgnoreTestpoints=0 " + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 19 + } + Object { + $PropName "DataTransfer" + $ObjectID 32 + $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 + NamePlacement "normal" + 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 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 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 + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + SimViewingDevice off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + Opaque off + MaskHideContents off + SFBlockType "NONE" + Variant off + VariantControlMode "Expression" + GeneratePreprocessorConditionals off + AllowZeroVariantControls off + PropagateVariantConditions off + TreatAsGroupedWhenPropagatingVariantConditions on + ContentPreviewEnabled off + IsWebBlock 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 + CodeGenStateStorageClass "Auto" + HasFrameUpgradeWarning off + } + } + System { + Name "torqueControlBalancing" + Location [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "361" + ReportName "simulink-default.rpt" + SIDHighWatermark "4936" + SimulinkSubDomain "Simulink" + Block { + BlockType Reference + Name "Configuration" + SID "4537" + Ports [] + Position [1180, 535, 1245, 555] + ZOrder 553 + HideAutomaticName off + LibraryVersion "1.675" + 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'" + RobotName "'icubSim'" + UrdfFile "'model.urdf'" + ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" + ControlBoardsNames "{'left_arm','right_arm','torso'}" + LocalName "'WBT'" + GravityVector "[0,0,-9.81]" + } + Block { + BlockType SubSystem + Name "Dump and visualize" + SID "2275" + Ports [] + Position [1155, 381, 1275, 409] + ZOrder 547 + ForegroundColor "blue" + BackgroundColor "[0.333333, 1.000000, 1.000000]" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 33 + $ClassName "Simulink.Mask" + Display "disp('VISUALIZER')" + } + System { + Name "Dump and visualize" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "415" + SimulinkSubDomain "Simulink" + Block { + BlockType SubSystem + Name "Desired and Measured Forces" + SID "3241" + Ports [4, 0, 1] + Position [260, 313, 435, 387] + ZOrder 899 + RequestExecContextInheritance off + System { + Name "Desired and Measured Forces" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "240" + 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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "466" + 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 48 + SrcBlock "Demux6" + SrcPort 2 + DstBlock "des moments r_sole" + DstPort 1 + } + Line { + ZOrder 45 + SrcBlock "Demux5" + SrcPort 2 + DstBlock "des moments l_sole" + DstPort 1 + } + Line { + ZOrder 41 + SrcBlock "Demux1" + SrcPort 1 + DstBlock "measured forces l_sole" + DstPort 1 + } + Line { + ZOrder 44 + SrcBlock "Demux7" + SrcPort 1 + DstBlock "measured forces r_sole" + DstPort 1 + } + Line { + ZOrder 46 + SrcBlock "Demux7" + SrcPort 2 + DstBlock "measured moments r_sole" + DstPort 1 + } + Line { + ZOrder 38 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock "Demux1" + DstPort 1 + } + Line { + ZOrder 26 + SrcBlock "Demux2" + SrcPort 1 + DstBlock "Demux5" + DstPort 1 + } + Line { + ZOrder 39 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock "Demux7" + DstPort 1 + } + Line { + ZOrder 47 + SrcBlock "Demux6" + SrcPort 1 + DstBlock "des forces r_sole" + DstPort 1 + } + Line { + ZOrder 42 + SrcBlock "Demux1" + SrcPort 2 + DstBlock "measured moments l_sole" + DstPort 1 + } + Line { + ZOrder 40 + SrcBlock "f_star" + SrcPort 1 + DstBlock "Demux2" + DstPort 1 + } + Line { + ZOrder 28 + SrcBlock "Demux2" + SrcPort 2 + DstBlock "Demux6" + DstPort 1 + } + Line { + ZOrder 43 + 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 + ShowName 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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ngVariableName','forces_l_sole_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','D" + "ataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedD" + "isplays',{struct('MinYLimReal','-176.20127','MaxYLimReal','526.26568','YLabelReal','','MinYLimMag',' 0.00000','" + "MaxYLimMag','526.26568','LegendVisibility','Off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 " + "0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.06666666666666" + "67;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.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','V" + "isible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" + "lor',[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,'Ma" + "rker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',tru" + "e,'Placement',1),struct('MinYLimReal','-193.24915','MaxYLimReal','450.91468','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.07" + "45098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0" + "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215" + "6863],'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','-','L" + "ineWidth',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,'Plac" + "ement',2),struct('MinYLimReal','-264.65056','MaxYLimReal','550.97623','YLabelReal','','MinYLimMag','0','MaxYLim" + "Mag','10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTick" + "Color',[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.07450980" + "39215686;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','Visible','on'),struct('Color',[1 0 0],'" + "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidt" + "h',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'" + ",'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('C" + "olor',[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,'M" + "arker','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','40.00000','MaxYLimReal','60.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" + "sibility','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.623529411" + "764706 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'),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','no" + "ne','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'U" + "serDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimR" + "eal','0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off'," + "'XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745" + "09803922 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.274509" + "803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProper" + "tiesCache',{{struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefi" + "nedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaul" + "ts',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509" + "803922],'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.058823" + "5294117647 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],'LineSty" + "le','-','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',{{}},'NumLin" + "es',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','Disp" + "layLayoutDimensions',[6 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" + "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Ve" + "rsion','2020a','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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ngVariableName','moments_l_sole_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','" + "DataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'Serialized" + "Displays',{struct('MinYLimReal','-18.14675','MaxYLimReal','18.61074','YLabelReal','','MinYLimMag','0.00000','Ma" + "xYLimMag','18.61074','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.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','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','-" + "','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,'" + "Placement',1),struct('MinYLimReal','-55.51989','MaxYLimReal','35.88107','YLabelReal','','MinYLimMag','0','MaxYL" + "imMag','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.0745098" + "039215686 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]" + ",'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marke" + "r','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'" + ",'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement" + "',2),struct('MinYLimReal','-58.09813','MaxYLimReal','48.08789','YLabelReal','','MinYLimMag','0','MaxYLimMag','1" + "0','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.074509803921568" + "6;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','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," + "'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),stru" + "ct('MinYLimReal','0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibi" + "lity','off','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.6235294117647" + "06 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','%'" + ",'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','-','LineWid" + "th',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')}},'UserD" + "efinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4)},'DisplayPropertyDef" + "aults',struct('YLabelReal','','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.16" + "078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058" + "8235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{stru" + "ct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'Line" + "Style','-','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',{{}},'Num" + "Lines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','D" + "isplayLayoutDimensions',[4 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigation',true,'On" + "ceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ngVariableName','forces_r_sole_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','D" + "ataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedD" + "isplays',{struct('MinYLimReal','-36.45598','MaxYLimReal','219.57258','YLabelReal','','MinYLimMag','0.00000','Ma" + "xYLimMag','219.57258','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.83137254901960" + "8 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980" + "392156863],'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','Vis" + "ible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','" + "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Mark" + "er','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true," + "'Placement',1),struct('MinYLimReal','-33.68004','MaxYLimReal','221.73551','YLabelReal','','MinYLimMag','0','Max" + "YLimMag','10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'Axes" + "TickColor',[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.0745" + "098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" + "3],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n'),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','-','Line" + "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placeme" + "nt',2),struct('MinYLimReal','-113.33241','MaxYLimReal','33.5863','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.07450980392156" + "86 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215" + "686;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','non" + "e','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],'LineS" + "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0." + "5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" + "le','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),st" + "ruct('MinYLimReal','90.00000','MaxYLimReal','110.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Leg" + "endVisibility','off','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.6235" + "29411764706 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','Visib" + "le','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',4),struct('Min" + "YLimReal','40.00000','MaxYLimReal','60.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibil" + "ity','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980392" + "2 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.7176470588235" + "29 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','" + "Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDe" + "finedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal'," + "'0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGri" + "d',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803" + "922 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.27450980392" + "1569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesC" + "ache',{{struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedCh" + "annelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',s" + "truct('YLabelReal','','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.1607843137" + "2549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352941" + "17647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Colo" + "r',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','" + "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Mark" + "er','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',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLa" + "youtDimensions',[6 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop" + "',false,'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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ngVariableName','moments_r_sole_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','" + "DataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'Serialized" + "Displays',{struct('MinYLimReal','-89.58974','MaxYLimReal','373.15892','YLabelReal','','MinYLimMag','0.00000','M" + "axYLimMag','373.15892','LegendVisibility','Off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'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.8313725490196" + "08 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','Vi" + "sible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" + "or',[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,'Mar" + "ker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true" + ",'Placement',1),struct('MinYLimReal','-90.08017','MaxYLimReal','373.36109','YLabelReal','','MinYLimMag','0','Ma" + "xYLimMag','10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'Axe" + "sTickColor',[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.074" + "5098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6509803921568" + "63],'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],'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','no" + "ne','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placem" + "ent',2),struct('MinYLimReal','-23.18842','MaxYLimReal','45.48642','YLabelReal','','MinYLimMag','0','MaxYLimMag'" + ",'10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColo" + "r',[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.074509803921" + "5686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Titl" + "e','%','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'),stru" + "ct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'Line" + "Style','-','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),s" + "truct('MinYLimReal','-39.55039','MaxYLimReal','20.45784','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Le" + "gendVisibility','off','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.623" + "529411764706 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','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','-" + "','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',4)},'DisplayP" + "ropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980392" + "2 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.2745098039215" + "69 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','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','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),'TimeRangeSamples','40','TimeRangeFrame" + "s','40','DisplayLayoutDimensions',[4 1],'DisplayContentCache',{struct('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','-','" + "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'),s" + "truct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" + "s',{{}},'PlotMagPhase',false),struct('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','V" + "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" + "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle'" + ",'-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),str" + "uct('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],'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','no" + "ne','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','Lin" + "ePropertiesCache',{{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],'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','Visi" + "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" + "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color" + "',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marke" + "r','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" + "),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1]" + ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase" + "',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),ext" + "mgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2020a','Location',[401 474 1711 10" + "60])" + NumInputPorts "4" + Floating off + } + Line { + ZOrder 38 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock "Demux Forces nd Moments" + DstPort 1 + } + Line { + ZOrder 39 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock "Demux Forces nd Moments" + DstPort 2 + } + Line { + ZOrder 40 + SrcBlock "f_star" + SrcPort 1 + DstBlock "Demux Forces nd Moments" + DstPort 3 + } + Line { + Name "measured forces l_sole" + ZOrder 41 + SrcBlock "Demux Forces nd Moments" + SrcPort 1 + Points [43, 0; 0, -270; 142, 0] + Branch { + ZOrder 96 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 1 + } + Branch { + ZOrder 45 + DstBlock "Sum" + DstPort 1 + } + } + Line { + Name "desired forces l_sole" + ZOrder 42 + SrcBlock "Demux Forces nd Moments" + SrcPort 2 + Points [66, 0; 0, -255] + Branch { + ZOrder 82 + DstBlock "Sum" + DstPort 2 + } + Branch { + ZOrder 46 + Labels [-1, 0] + Points [0, -25] + DstBlock "left Foot forces" + DstPort 2 + } + } + Line { + Name "error forces l_sole" + ZOrder 43 + Labels [-1, 0] + SrcBlock "Sum" + SrcPort 1 + DstBlock "left Foot forces" + DstPort 3 + } + Line { + Name "thresholdContactOn" + ZOrder 50 + SrcBlock "Constant" + SrcPort 1 + Points [259, 0] + Branch { + ZOrder 91 + Labels [-1, 0] + Points [0, 185] + DstBlock "right Foot forces" + DstPort 5 + } + Branch { + ZOrder 90 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 5 + } + } + Line { + Name "desired forces r_sole" + ZOrder 83 + SrcBlock "Demux Forces nd Moments" + SrcPort 4 + Points [116, 0; 0, -140] + Branch { + ZOrder 86 + DstBlock "Sum1" + DstPort 2 + } + Branch { + ZOrder 84 + Labels [-1, 0] + Points [0, -25] + DstBlock "right Foot forces" + DstPort 2 + } + } + Line { + Name "measured forces r_sole" + ZOrder 81 + SrcBlock "Demux Forces nd Moments" + SrcPort 3 + Points [90, 0; 0, -155; 95, 0] + Branch { + ZOrder 97 + Labels [-1, 0] + DstBlock "right Foot forces" + DstPort 1 + } + Branch { + ZOrder 64 + DstBlock "Sum1" + DstPort 1 + } + } + Line { + Name "error forces r_sole" + ZOrder 74 + Labels [-1, 0] + SrcBlock "Sum1" + SrcPort 1 + DstBlock "right Foot forces" + DstPort 3 + } + Line { + Name "thresholdContactOff" + ZOrder 49 + SrcBlock "Constant2" + SrcPort 1 + Points [280, 0] + Branch { + ZOrder 89 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 4 + } + Branch { + ZOrder 88 + Labels [-1, 0] + Points [0, 185] + DstBlock "right Foot forces" + DstPort 4 + } + } + Line { + Name "measured moments r_sole" + ZOrder 127 + SrcBlock "Demux Forces nd Moments" + SrcPort 7 + Points [68, 0; 0, 110; 117, 0] + Branch { + ZOrder 131 + DstBlock "Sum3" + DstPort 1 + } + Branch { + ZOrder 130 + Labels [0, 0] + DstBlock "right Foot moments" + DstPort 1 + } + } + Line { + Name "state" + ZOrder 51 + SrcBlock "state" + SrcPort 1 + Points [321, 0] + Branch { + ZOrder 95 + Points [0, 185] + Branch { + ZOrder 123 + Points [0, 205; 1, 0] + Branch { + ZOrder 126 + Labels [-1, 0] + Points [0, 180] + DstBlock "right Foot moments" + DstPort 4 + } + Branch { + ZOrder 125 + DstBlock "left Foot moments" + DstPort 4 + } + } + Branch { + ZOrder 122 + Labels [-1, 0] + DstBlock "right Foot forces" + DstPort 6 + } + } + Branch { + ZOrder 94 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 6 + } + } + Line { + Name "desired moments r_sole" + ZOrder 128 + SrcBlock "Demux Forces nd Moments" + SrcPort 8 + Points [40, 0; 0, 110] + Branch { + ZOrder 134 + Labels [-1, 0] + DstBlock "right Foot moments" + DstPort 2 + } + Branch { + ZOrder 133 + Points [0, 35] + DstBlock "Sum3" + DstPort 2 + } + } + Line { + Name "error moments r_sole" + ZOrder 129 + Labels [-1, 0] + SrcBlock "Sum3" + SrcPort 1 + DstBlock "right Foot moments" + DstPort 3 + } + Line { + Name "error moments l_sole" + ZOrder 116 + Labels [-1, 0] + SrcBlock "Sum2" + SrcPort 1 + DstBlock "left Foot moments" + DstPort 3 + } + Line { + Name "desired moments l_sole" + ZOrder 115 + SrcBlock "Demux Forces nd Moments" + SrcPort 6 + Points [121, 0] + Branch { + ZOrder 120 + Points [0, 35] + DstBlock "Sum2" + DstPort 2 + } + Branch { + ZOrder 119 + Labels [-1, 0] + DstBlock "left Foot moments" + DstPort 2 + } + } + Line { + Name "measured moments l_sole" + ZOrder 114 + SrcBlock "Demux Forces nd Moments" + SrcPort 5 + Points [185, 0] + Branch { + ZOrder 118 + DstBlock "Sum2" + DstPort 1 + } + Branch { + ZOrder 117 + 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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "472" + 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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ngVariableName','feetContactStatus_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1" + "','DataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'Seriali" + "zedDisplays',{struct('MinYLimReal','0.9','MaxYLimReal','1.1','YLabelReal','','MinYLimMag','0.9','MaxYLimMag','1" + ".1','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.074509803921568" + "6;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','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," + "'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),stru" + "ct('MinYLimReal','0.9','MaxYLimReal','1.1','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','%','Lin" + "ePropertiesCache',{{struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'U" + "serDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2)},'DisplayPropert" + "yDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68" + "6274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 " + "0.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','-','LineWidt" + "h',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('C" + "olor',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}}," + "'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[2 1],'DisplayConte" + "ntCache',[]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),e" + "xtmgr.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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ngVariableName','KP_postural_diag_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1'" + ",'DataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'Serializ" + "edDisplays',{struct('MinYLimReal','8.75','MaxYLimReal','21.25','YLabelReal','','MinYLimMag','8.75','MaxYLimMag'" + ",'21.25','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickCo" + "lor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980392" + "15686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039" + "215686;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','" + "none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" + "ruct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'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','Vi" + "sible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1)" + ",struct('MinYLimReal','7.75','MaxYLimReal','10.25','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" + "sibility','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.6235294117" + "64706 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','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','-','Line" + "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Us" + "erDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinYLimRe" + "al','7.75','MaxYLimReal','10.25','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XG" + "rid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098" + "03922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764" + "705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803" + "921569 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','-'" + ",'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')}},'UserDefinedChannelNa" + "mes',{{}},'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'," + "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.160784" + "31372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235" + "294117647 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],'LineStyl" + "e','-','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',{{}},'NumLine" + "s',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-1.00000','MaxYLimReal','1.0000" + "0','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],'Colo" + "rOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3" + "92156862745098 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','-','Line" + "Width',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'),struc" + "t('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,'LineNa" + "mes',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','1.00000','MaxYLimReal','3.00000','YLabelRea" + "l','','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.39215686274509" + "8 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098" + "039215686 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,'M" + "arker','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','-','Li" + "neWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'" + "ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTic" + "kColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980" + "39215686 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]," + "'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'," + "'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" + ",1),'DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),ex" + "tmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2020a','Location',[898 828 2209 1" + "517])" + NumInputPorts "6" + Floating off + } + Line { + ZOrder 1 + SrcBlock "state" + SrcPort 1 + Points [85, 0] + Branch { + ZOrder 31 + DstBlock "Feet Contact Activation" + DstPort 2 + } + Branch { + ZOrder 30 + Points [0, 195] + DstBlock "Gain Scheduling Postural" + DstPort 6 + } + } + Line { + ZOrder 8 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock "Feet Contact Activation" + DstPort 1 + } + Line { + ZOrder 9 + SrcBlock "KP_postural_diag" + SrcPort 1 + DstBlock "Demux1" + DstPort 1 + } + Line { + Name "left_leg" + ZOrder 11 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 4 + DstBlock "Gain Scheduling Postural" + DstPort 4 + } + Line { + Name "right_leg" + ZOrder 12 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 5 + DstBlock "Gain Scheduling Postural" + DstPort 5 + } + Line { + Name "right_arm" + ZOrder 13 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 3 + DstBlock "Gain Scheduling Postural" + DstPort 3 + } + Line { + Name "torso" + ZOrder 14 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 1 + DstBlock "Gain Scheduling Postural" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 15 + 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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ngVariableName','QPStatus_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLo" + "ggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispla" + "ys',{struct('MinYLimReal','-2.25','MaxYLimReal','0.25','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','2.2" + "5','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],'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],'LineStyl" + "e','-','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),struc" + "t('MinYLimReal','0.25','MaxYLimReal','7.75','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibilit" + "y','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','%','Li" + "nePropertiesCache',{{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],'Lin" + "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth'," + "0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" + "ible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefin" + "edChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2)},'DisplayPropertyDefault" + "s',struct('YLabelReal','','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.160784" + "31372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235" + "294117647 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],'LineStyl" + "e','-','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',{{}},'NumLine" + "s',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[2 1]),extmgr.Configuration" + "('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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "185" + 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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','base_pose_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDec" + "imation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispl" + "ays',{struct('MinYLimReal','-0.16773','MaxYLimReal','0.72406','YLabelReal','','MinYLimMag','0.00000','MaxYLimMa" + "g','0.72406','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTi" + "ckColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098" + "039215686 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]" + ",'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marke" + "r','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'" + ",'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement" + "',1),struct('MinYLimReal','-1.25','MaxYLimReal','1.25','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Lege" + "ndVisibility','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.623529" + "411764706 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],'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',2),struct('MinYL" + "imReal','0.625','MaxYLimReal','4.375','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.4" + "11764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745" + "09803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProp" + "ertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" + ",3)},'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" + ".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'),stru" + "ct('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','Visi" + "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" + "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','400'," + "'TimeRangeFrames','400','DisplayLayoutDimensions',[3 1],'DisplayContentCache',[]),extmgr.Configuration('Tools'," + "'Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements'" + ",true,'Version','2018a')),'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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','baseVel_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecim" + "ation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplay" + "s',{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.6" + "86274509803922 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." + "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','Vi" + "sible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" + "or',[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,'Mar" + "ker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('" + "MinYLimReal','-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisib" + "ility','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.6235294117647" + "06 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','%'" + ",'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','-','LineWid" + "th',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')}},'UserD" + "efinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',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.6862745" + "09803922 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.274509" + "803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','LinePropertiesCache',{{s" + "truct('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','V" + "isible','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',{{}},'" + "NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3)},'DisplayPropertyDefaults',struct('MinYLimReal" + "','-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','AxesColor',[0 0 0],'Ax" + "esTickColor',[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.07" + "45098039215686;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,'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','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Place" + "ment',1),'DisplayLayoutDimensions',[3 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',fals" + "e),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2020a','Location',[135 478 2" + "065 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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','pos_CoM_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecim" + "ation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplay" + "s',{struct('MinYLimReal','-0.15575','MaxYLimReal','0.70056','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag'" + ",'0.70056','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTick" + "Color',[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.07450980" + "39215686;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','Visible','on'),struct('Color',[1 0 0],'" + "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidt" + "h',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','-0.14783','MaxYLimReal','0.63035','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" + "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68" + "6274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62" + "3529411764706 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','Vis" + "ible','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,'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',2),struct('M" + "inYLimReal','-0.08636','MaxYLimReal','0.08883','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibi" + "lity','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980392" + "2 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.7176470588235" + "29 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','" + "Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDe" + "finedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal'," + "'0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid" + "',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098039" + "22 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705" + "882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921" + "569 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','-','L" + "ineWidth',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)},'DisplayPropertyDefaults',struct('YLa" + "belReal','','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.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]" + ",'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'" + ",'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],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineName" + "s',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','400','TimeRangeFrames','400','DisplayLayoutDime" + "nsions',[4 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY')" + ",extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2020a','Location',[135 239 265" + "0 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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','tauDesired_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDe" + "cimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisp" + "lays',{struct('MinYLimReal','-19.59759','MaxYLimReal','27.79759','YLabelReal','','MinYLimMag','0.00000','MaxYLi" + "mMag','27.79759','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'Ax" + "esTickColor',[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.07" + "45098039215686;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,'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','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Place" + "ment',1),struct('MinYLimReal','-7.66225','MaxYLimReal','8.03648','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.074509803921568" + "6 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392156" + "86;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','Visible','on'),struct('Color',[1 0 0],'LineSt" + "yle','-','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',2),str" + "uct('MinYLimReal','-11.21869','MaxYLimReal','8.29753','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legen" + "dVisibility','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.6235294" + "11764706 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],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','L" + "ineWidth',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('MinYLi" + "mReal','-51.65238','MaxYLimReal','50.72666','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibilit" + "y','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','%','Li" + "nePropertiesCache',{{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],'Lin" + "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth'," + "0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" + "ible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefin" + "edChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-5" + ".64312','MaxYLimReal','14.99269','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XG" + "rid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098" + "03922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764" + "705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803" + "921569 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','-'" + ",'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')}},'UserDefinedChannelNa" + "mes',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-0.5','MaxYLi" + "mReal','14.5','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',t" + "rue,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980" + "3922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843" + "1372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352" + "94117647 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'," + "'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],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames" + "',{{[]}},'ShowContent',true,'Placement',6)},'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.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','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','-" + "','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,'" + "Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configurat" + "ion('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','M" + "easurements',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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','tau_star_afterSat_DATA','DataLoggingSaveFormat','StructureWithTime','DataLo" + "ggingDecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'Seriali" + "zedDisplays',{struct('MinYLimReal','-14.69019','MaxYLimReal','22.24602','YLabelReal','','MinYLimMag','0.00000'," + "'MaxYLimMag','22.24602','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'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.8313725490196" + "08 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','Vi" + "sible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" + "or',[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,'Mar" + "ker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true" + ",'Placement',1),struct('MinYLimReal','-10.12731','MaxYLimReal','6.52875','YLabelReal','','MinYLimMag','0','MaxY" + "LimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTi" + "ckColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098" + "039215686 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]" + ",'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marke" + "r','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'" + ",'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement" + "',2),struct('MinYLimReal','-14.66185','MaxYLimReal','8.30827','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],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" + "olor',[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,'M" + "arker','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','-23.11356','MaxYLimReal','33.28118','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendV" + "isibility','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.623529411" + "764706 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'),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','no" + "ne','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'U" + "serDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimR" + "eal','-30.29324','MaxYLimReal','27.04418','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility'" + ",'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." + "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'),struc" + "t('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','Visib" + "le','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefined" + "ChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','0.25" + "','MaxYLimReal','7.75','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true," + "'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68" + "6274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 " + "0.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','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',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColo" + "r',[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.831372" + "549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 " + "0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','Lin" + "eWidth',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'),stru" + "ct('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',{{}},'NumLines',0,'LineNames',{{[]}},'ShowConten" + "t',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.C" + "onfiguration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('" + "Tools','Measurements',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 Reference + Name "GetMeasurement1" + SID "4543" + Ports [0, 1] + Position [50, 106, 115, 134] + ZOrder 870 + BackgroundColor "[0.827500, 0.576500, 0.603900]" + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetMeasurement" + SourceType "GetMeasurement" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + measuredType "Joints Torque" + } + 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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','jointPos_Desired_DATA','DataLoggingSaveFormat','StructureWithTime','DataLog" + "gingDecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'Serializ" + "edDisplays',{struct('MinYLimReal','-8.36237','MaxYLimReal','29.68131','YLabelReal','','MinYLimMag','0.00000','M" + "axYLimMag','29.68131','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.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','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','-" + "','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,'" + "Placement',1),struct('MinYLimReal','-81.97965','MaxYLimReal','72.42484','YLabelReal','','MinYLimMag','0','MaxYL" + "imMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTic" + "kColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980" + "39215686 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]," + "'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'," + "'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" + ",2),struct('MinYLimReal','-78.54318','MaxYLimReal','93.23402','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],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" + "olor',[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,'M" + "arker','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','-12.71731','MaxYLimReal','14.72789','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendV" + "isibility','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.623529411" + "764706 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'),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','no" + "ne','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'U" + "serDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimR" + "eal','-113.68313','MaxYLimReal','94.48842','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','%','Lin" + "ePropertiesCache',{{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],'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','Visi" + "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" + "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','0.6" + "25','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',tr" + "ue,'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.4117647058823" + "53 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " + "1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','LinePropertiesCache',{{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,'Mar" + "ker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 " + "1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'," + "0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesC" + "olor',[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.831" + "372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07450980392156" + "86 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'),s" + "truct('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',{{[]}},'ShowCon" + "tent',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmg" + "r.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuratio" + "n('Tools','Measurements',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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','jointPos_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDeci" + "mation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispla" + "ys',{struct('MinYLimReal','-2.41385','MaxYLimReal','5.58471','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag" + "','5.58471','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTic" + "kColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980" + "39215686 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]," + "'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'," + "'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" + ",1),struct('MinYLimReal','-39.61854','MaxYLimReal','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','%<" + "SignalLabel>','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" + "lor',[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,'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',2),struct(" + "'MinYLimReal','-39.58369','MaxYLimReal','58.65801','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" + "sibility','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.6235294117" + "64706 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','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','-','Line" + "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Us" + "erDefinedChannelNames',{{}},'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.686" + "274509803922 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','%','LinePr" + "opertiesCache',{{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],'LineSty" + "le','-','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')}},'UserDefinedCh" + "annelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-11.45" + "092','MaxYLimReal','7.90757','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid'" + ",true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980392" + "2 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" + "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039215" + "69 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','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','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',5),struct('MinYLimReal','0.75','MaxYLimRea" + "l','3.25','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.16078431372" + "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411" + "7647 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,'Marke" + "r','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',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesCol" + "or',[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.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','-','Li" + "neWidth',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'),str" + "uct('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',{{[]}},'ShowConte" + "nt',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr." + "Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration(" + "'Tools','Measurements',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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','jointPos_Error_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggi" + "ngDecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'Serialized" + "Displays',{struct('MinYLimReal','-10.85715','MaxYLimReal','13.2441','YLabelReal','','MinYLimMag','0.00000','Max" + "YLimMag','13.2441','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.0" + "745098039215686 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.6509803921" + "56863],'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','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Pla" + "cement',1),struct('MinYLimReal','-24.65677','MaxYLimReal','13.37484','YLabelReal','','MinYLimMag','0','MaxYLimM" + "ag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickCo" + "lor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980392" + "15686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039" + "215686;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','" + "none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" + "ruct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'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','Vi" + "sible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2)" + ",struct('MinYLimReal','-37.11648','MaxYLimReal','33.25109','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" + "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68" + "6274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62" + "3529411764706 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','Vis" + "ible','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,'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('M" + "inYLimReal','-28.71162','MaxYLimReal','40.05399','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisi" + "bility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803" + "922 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.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'" + "),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1]" + ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'User" + "DefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal" + "','-16.66332','MaxYLimReal','13.39274','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','o" + "n','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." + "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','%','LinePro" + "pertiesCache',{{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],'LineStyl" + "e','-','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')}},'UserDefinedCha" + "nnelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-0.375'" + ",'MaxYLimReal','13.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true" + ",'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.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,'Marke" + "r','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',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesCol" + "or',[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.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','-','Li" + "neWidth',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'),str" + "uct('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',{{[]}},'ShowConte" + "nt',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr." + "Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration(" + "'Tools','Measurements',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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','jointVel_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDeci" + "mation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispla" + "ys',{struct('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','1" + "0','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],'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],'LineStyl" + "e','-','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),struc" + "t('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legend" + "Visibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450" + "9803922 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.7176470" + "58823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," + "'on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " + "1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','Li" + "neWidth',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')}},'" + "UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinYLim" + "Real','-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.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Lin" + "ePropertiesCache',{{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],'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','Visi" + "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" + "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-20" + ".50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XG" + "rid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098" + "03922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764" + "705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803" + "921569 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','-'" + ",'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')}},'UserDefinedChannelNa" + "mes',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-20.50153','M" + "axYLimReal','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.68" + "6274509803922],'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','-','LineWidt" + "h',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('C" + "olor',[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','LegendVisibility','on','XGrid',true,'YGrid',tr" + "ue,'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.16078431" + "372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529" + "4117647 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,'Ma" + "rker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" + "on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0" + " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'" + ",0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('MinYLimReal','-20.50" + "153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','AxesColor',[0 0 0],'AxesTickC" + "olor',[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.074509803" + "9215686;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'," + "'none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" + "truct('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','V" + "isible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1" + "),'TimeRangeSamples','60','TimeRangeFrames','60','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools'," + "'Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2018a'))" + ",'Version','2020a','Location',[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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','tauMeasured_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingD" + "ecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDis" + "plays',{struct('MinYLimReal','-13.53306','MaxYLimReal','13.58663','YLabelReal','','MinYLimMag','0.00000','MaxYL" + "imMag','13.58663','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.07" + "45098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0" + "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215" + "6863],'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','-','L" + "ineWidth',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,'Plac" + "ement',1),struct('MinYLimReal','-4.19072','MaxYLimReal','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.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980392156" + "86 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215" + "686;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','non" + "e','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],'LineS" + "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0." + "5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" + "le','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),st" + "ruct('MinYLimReal','-4.21765','MaxYLimReal','4.8762','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legend" + "Visibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450" + "9803922 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.7176470" + "58823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," + "'on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " + "1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','Li" + "neWidth',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')}},'" + "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." + "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'),stru" + "ct('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','Visi" + "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" + "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-44" + ".89002','MaxYLimReal','34.01019','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XG" + "rid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098" + "03922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764" + "705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803" + "921569 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','-'" + ",'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')}},'UserDefinedChannelNa" + "mes',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-0.5','MaxYLi" + "mReal','14.5','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',t" + "rue,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980" + "3922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843" + "1372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352" + "94117647 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'," + "'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],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames" + "',{{[]}},'ShowContent',true,'Placement',6)},'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.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','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','-" + "','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,'" + "Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configurat" + "ion('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','M" + "easurements',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(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','tauError_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDeci" + "mation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispla" + "ys',{struct('MinYLimReal','-23.63249','MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimM" + "ag','27.95301','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'Axes" + "TickColor',[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.0745" + "098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" + "3],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n'),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','-','Line" + "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placeme" + "nt',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabelReal','','MinYLimMag','0','MaxYLimMag','1" + "0','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],'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],'LineStyl" + "e','-','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),struc" + "t('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" + "sibility','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.6235294117" + "64706 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','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','-','Line" + "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Us" + "erDefinedChannelNames',{{}},'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.68" + "6274509803922 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.2" + "74509803921569 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" + "('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" + ",'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')}},'UserDefinedC" + "hannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-66.4" + "4849','MaxYLimReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGri" + "d',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803" + "922 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.27450980392" + "1569 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','-','" + "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'),s" + "truct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" + "s',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','0.25','MaxYLimR" + "eal','7.75','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',tru" + "e,'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.160784313" + "72549;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],'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','V" + "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" + "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle'" + ",'-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames'," + "{{[]}},'ShowContent',true,'Placement',6)},'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.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392" + "156863],'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','-'," + "'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,'Pl" + "acement',1),'TimeRangeSamples','90','TimeRangeFrames','90','DisplayLayoutDimensions',[6 1]),extmgr.Configuratio" + "n('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Mea" + "surements',true,'Version','2018a')),'Version','2020a','Location',[135 169 1990 1170])" + NumInputPorts "6" + Floating off + } + Line { + Name "CoM_Measured" + ZOrder 391 + SrcBlock "pos_CoM" + SrcPort 1 + Points [120, 0] + Branch { + ZOrder 835 + DstBlock "Sum1" + DstPort 1 + } + Branch { + ZOrder 834 + Labels [-1, 0] + DstBlock "CoM Position" + DstPort 1 + } + } + Line { + ZOrder 702 + SrcBlock "Gain" + SrcPort 1 + DstBlock "Demux1" + DstPort 1 + } + Line { + ZOrder 839 + SrcBlock "jointPos" + SrcPort 1 + Points [80, 0] + Branch { + ZOrder 840 + DstBlock "Gain" + DstPort 1 + } + Branch { + ZOrder 7 + DstBlock "Sum3" + DstPort 1 + } + } + Line { + Name "right_leg" + ZOrder 700 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 5 + DstBlock "Joint Position Measured" + DstPort 5 + } + Line { + Name "torso" + ZOrder 698 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 1 + DstBlock "Joint Position Measured" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 697 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 2 + DstBlock "Joint Position Measured" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 701 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 4 + DstBlock "Joint Position Measured" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 699 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 3 + DstBlock "Joint Position Measured" + DstPort 3 + } + Line { + ZOrder 480 + SrcBlock "GetMeasurement1" + SrcPort 1 + Points [100, 0] + Branch { + ZOrder 831 + DstBlock "Sum" + DstPort 2 + } + Branch { + ZOrder 686 + DstBlock "Demux9" + DstPort 1 + } + } + Line { + Name "right_leg" + ZOrder 666 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 5 + DstBlock "Desired Torques" + DstPort 5 + } + Line { + Name "torso" + ZOrder 668 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 1 + DstBlock "Desired Torques" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 667 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 2 + DstBlock "Desired Torques" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 665 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 4 + DstBlock "Desired Torques" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 669 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 3 + DstBlock "Desired Torques" + DstPort 3 + } + Line { + Name "right_leg" + ZOrder 656 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 5 + DstBlock "Torques Error" + DstPort 5 + } + Line { + Name "torso" + ZOrder 659 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 1 + DstBlock "Torques Error" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 658 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 2 + DstBlock "Torques Error" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 657 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 4 + DstBlock "Torques Error" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 655 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 3 + DstBlock "Torques Error" + DstPort 3 + } + Line { + ZOrder 685 + SrcBlock "Sum" + SrcPort 1 + DstBlock "Demux10" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 660 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 5 + DstBlock "Measured Torques" + DstPort 5 + } + Line { + Name "torso" + ZOrder 661 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 1 + DstBlock "Measured Torques" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 662 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 2 + DstBlock "Measured Torques" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 663 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 4 + DstBlock "Measured Torques" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 664 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 3 + DstBlock "Measured Torques" + DstPort 3 + } + Line { + ZOrder 390 + SrcBlock "jointPos_des" + SrcPort 1 + Points [32, 0] + Branch { + ZOrder 52 + Points [0, 160] + DstBlock "Sum3" + DstPort 2 + } + Branch { + ZOrder 53 + DstBlock "Gain3" + DstPort 1 + } + } + Line { + ZOrder 703 + SrcBlock "Gain3" + SrcPort 1 + DstBlock "Demux2" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 694 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 5 + DstBlock "Joint Position Desired" + DstPort 5 + } + Line { + Name "torso" + ZOrder 693 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 1 + DstBlock "Joint Position Desired" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 695 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 2 + DstBlock "Joint Position Desired" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 692 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 4 + DstBlock "Joint Position Desired" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 696 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 3 + DstBlock "Joint Position Desired" + DstPort 3 + } + Line { + ZOrder 704 + SrcBlock "Gain4" + SrcPort 1 + DstBlock "Demux5" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 690 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 5 + DstBlock "Joint Positon Error" + DstPort 5 + } + Line { + Name "torso" + ZOrder 688 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 1 + DstBlock "Joint Positon Error" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 689 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 2 + DstBlock "Joint Positon Error" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 691 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 4 + DstBlock "Joint Positon Error" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 687 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 3 + DstBlock "Joint Positon Error" + DstPort 3 + } + Line { + ZOrder 70 + SrcBlock "Sum3" + SrcPort 1 + DstBlock "Gain4" + DstPort 1 + } + Line { + Name "CoM_Desired" + ZOrder 832 + SrcBlock "pos_CoM_des" + SrcPort 1 + Points [67, 0] + Branch { + ZOrder 837 + Points [0, 35] + DstBlock "Sum1" + DstPort 2 + } + Branch { + ZOrder 836 + Labels [-1, 0] + DstBlock "CoM Position" + DstPort 2 + } + } + Line { + ZOrder 388 + SrcBlock "w_H_b" + SrcPort 1 + Points [77, 0] + Branch { + ZOrder 869 + Points [0, -40] + DstBlock "CoM6D -> \nCoMXYZ2" + DstPort 1 + } + Branch { + ZOrder 868 + DstBlock "CoM6D -> \nCoMXYZ1" + DstPort 1 + } + } + Line { + Name "basePosition" + ZOrder 103 + Labels [-1, 0] + SrcBlock "CoM6D -> \nCoMXYZ2" + SrcPort 1 + DstBlock "Base Pose" + DstPort 1 + } + Line { + Name "baseOrientation" + ZOrder 104 + Labels [-1, 0] + SrcBlock "CoM6D -> \nCoMXYZ1" + SrcPort 1 + DstBlock "Base Pose" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 670 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 4 + DstBlock "Desired Torques After Saturation" + DstPort 4 + } + Line { + Name "right_leg" + ZOrder 673 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 5 + DstBlock "Desired Torques After Saturation" + DstPort 5 + } + Line { + Name "left_arm" + ZOrder 671 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 2 + DstBlock "Desired Torques After Saturation" + DstPort 2 + } + Line { + Name "torso" + ZOrder 674 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 1 + DstBlock "Desired Torques After Saturation" + DstPort 1 + } + Line { + Name "right_arm" + ZOrder 672 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 3 + DstBlock "Desired Torques After Saturation" + DstPort 3 + } + Line { + ZOrder 581 + SrcBlock "tau_star_afterSat" + SrcPort 1 + DstBlock "Demux7" + DstPort 1 + } + Line { + ZOrder 387 + SrcBlock "tau_star" + SrcPort 1 + Points [41, 0] + Branch { + ZOrder 821 + Points [0, 150] + DstBlock "Sum" + DstPort 1 + } + Branch { + ZOrder 684 + DstBlock "Demux8" + DstPort 1 + } + } + Line { + ZOrder 806 + SrcBlock "nu" + SrcPort 1 + Points [26, 0] + Branch { + ZOrder 852 + DstBlock "Selector1" + DstPort 1 + } + Branch { + ZOrder 805 + Points [0, 160] + DstBlock "Selector2" + DstPort 1 + } + } + Line { + ZOrder 807 + SrcBlock "Selector2" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + ZOrder 808 + SrcBlock "Gain1" + SrcPort 1 + DstBlock "Demux3" + DstPort 1 + } + Line { + Name "base linear velocity" + ZOrder 809 + Labels [-1, 0] + SrcBlock "Demux" + SrcPort 1 + DstBlock "Base linear and angular velocity" + DstPort 1 + } + Line { + Name "base angular velocity" + ZOrder 810 + Labels [-1, 0] + SrcBlock "Gain2" + SrcPort 1 + DstBlock "Base linear and angular velocity" + DstPort 2 + } + Line { + Name "right_arm" + ZOrder 811 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 3 + DstBlock "Joint Velocity" + DstPort 3 + } + Line { + Name "right_leg" + ZOrder 812 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 5 + DstBlock "Joint Velocity" + DstPort 5 + } + Line { + ZOrder 813 + SrcBlock "Selector1" + SrcPort 1 + DstBlock "Gain1" + DstPort 1 + } + Line { + Name "torso" + ZOrder 817 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 1 + DstBlock "Joint Velocity" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 818 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 2 + DstBlock "Joint Velocity" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 819 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 4 + DstBlock "Joint Velocity" + DstPort 4 + } + Line { + ZOrder 820 + SrcBlock "Demux" + SrcPort 2 + DstBlock "Gain2" + DstPort 1 + } + Line { + Name "CoM_Error" + ZOrder 838 + Labels [-1, 0] + SrcBlock "Sum1" + SrcPort 1 + DstBlock "CoM Position" + DstPort 3 + } + Line { + Name "state" + ZOrder 389 + SrcBlock "state" + SrcPort 1 + Points [234, 0] + Branch { + ZOrder 827 + Points [0, 145] + Branch { + ZOrder 825 + Points [0, 150] + Branch { + ZOrder 829 + Points [0, 155] + Branch { + ZOrder 828 + Points [0, 185] + Branch { + ZOrder 844 + Points [0, 55; 742, 0] + Branch { + ZOrder 862 + Labels [-1, 0] + DstBlock "Base linear and angular velocity" + DstPort 3 + } + Branch { + ZOrder 861 + Points [0, -160] + Branch { + ZOrder 859 + Labels [-1, 0] + DstBlock "Joint Velocity" + DstPort 6 + } + Branch { + ZOrder 853 + Points [0, -190] + Branch { + ZOrder 858 + Labels [-1, 0] + DstBlock "Joint Positon Error" + DstPort 6 + } + Branch { + ZOrder 848 + Points [0, -160] + Branch { + ZOrder 857 + Labels [-1, 0] + DstBlock "Joint Position Desired" + DstPort 6 + } + Branch { + ZOrder 850 + Points [0, -150; 1, 0] + Branch { + ZOrder 866 + Labels [-1, 0] + Points [0, -170] + DstBlock "Base Pose" + DstPort 3 + } + Branch { + ZOrder 865 + DstBlock "Joint Position Measured" + DstPort 6 + } + } + } + } + } + } + Branch { + ZOrder 843 + Labels [-1, 0] + DstBlock "CoM Position" + DstPort 4 + } + } + Branch { + ZOrder 89 + Labels [-1, 0] + DstBlock "Measured Torques" + DstPort 6 + } + } + Branch { + ZOrder 90 + Labels [-1, 0] + DstBlock "Torques Error" + DstPort 6 + } + } + Branch { + ZOrder 75 + Labels [-1, 0] + DstBlock "Desired Torques" + DstPort 6 + } + } + Branch { + ZOrder 823 + Labels [-1, 0] + DstBlock "Desired Torques After Saturation" + DstPort 6 + } + } + } + } + Line { + ZOrder 423 + SrcBlock "ON_GAZEBO\n4" + SrcPort 1 + Points [100, 0] + DstBlock "Visualizer" + DstPort enable + } + Line { + ZOrder 390 + SrcBlock "From1" + SrcPort 1 + Points [52, 0] + Branch { + ZOrder 450 + DstBlock "Visualize eventual QP failures" + DstPort 2 + } + Branch { + ZOrder 448 + Points [0, 40] + Branch { + ZOrder 474 + Points [0, 135] + DstBlock "Desired and Measured Forces" + DstPort 4 + } + Branch { + ZOrder 455 + DstBlock "Visualizer" + DstPort 6 + } + } + Branch { + ZOrder 449 + Points [0, -130] + DstBlock "Feet Status and Gains" + DstPort 3 + } + } + Line { + ZOrder 395 + SrcBlock "From7" + SrcPort 1 + DstBlock "Visualizer" + DstPort 2 + } + Line { + ZOrder 424 + SrcBlock "From5" + SrcPort 1 + DstBlock "Visualizer" + DstPort 1 + } + Line { + ZOrder 463 + SrcBlock "From12" + SrcPort 1 + DstBlock "Visualizer" + DstPort 5 + } + Line { + ZOrder 415 + SrcBlock "ON_GAZEBO\n2" + SrcPort 1 + Points [165, 0] + DstBlock "Visualize eventual QP failures" + DstPort enable + } + Line { + ZOrder 416 + SrcBlock "From9" + SrcPort 1 + DstBlock "Visualize eventual QP failures" + DstPort 1 + } + Line { + ZOrder 451 + SrcBlock "ON_GAZEBO\n1" + SrcPort 1 + Points [160, 0] + DstBlock "Desired and Measured Forces" + DstPort enable + } + Line { + ZOrder 445 + SrcBlock "ON_GAZEBO\n6" + SrcPort 1 + Points [135, 0] + DstBlock "Feet Status and Gains" + DstPort enable + } + Line { + ZOrder 446 + SrcBlock "From4" + SrcPort 1 + DstBlock "Feet Status and Gains" + DstPort 1 + } + Line { + ZOrder 447 + SrcBlock "From8" + SrcPort 1 + DstBlock "Feet Status and Gains" + DstPort 2 + } + Line { + ZOrder 452 + SrcBlock "From2" + SrcPort 1 + DstBlock "Desired and Measured Forces" + DstPort 3 + } + Line { + ZOrder 453 + SrcBlock "From11" + SrcPort 1 + DstBlock "Desired and Measured Forces" + DstPort 1 + } + Line { + ZOrder 454 + SrcBlock "From10" + SrcPort 1 + DstBlock "Desired and Measured Forces" + DstPort 2 + } + Line { + ZOrder 462 + SrcBlock "From6" + SrcPort 1 + DstBlock "Visualizer" + DstPort 3 + } + Line { + ZOrder 464 + SrcBlock "From3" + SrcPort 1 + DstBlock "Visualizer" + DstPort 4 + } + Line { + ZOrder 465 + SrcBlock "From" + SrcPort 1 + DstBlock "Visualizer" + DstPort 7 + } + Line { + ZOrder 466 + SrcBlock "From13" + SrcPort 1 + DstBlock "Visualizer" + DstPort 8 + } + Line { + ZOrder 475 + SrcBlock "From14" + SrcPort 1 + DstBlock "Visualizer" + DstPort 9 + } + } + } + Block { + BlockType Reference + Name "GetMeasurement" + SID "4539" + Ports [0, 1] + Position [780, 388, 865, 402] + ZOrder 905 + BackgroundColor "[0.925500, 0.870600, 0.133300]" + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetMeasurement" + SourceType "GetMeasurement" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + measuredType "Joints Acceleration" + } + Block { + BlockType Reference + Name "GetMeasurement1" + SID "4538" + Ports [0, 1] + Position [785, 432, 860, 448] + ZOrder 958 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetMeasurement" + SourceType "GetMeasurement" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + measuredType "Joints Position" + } + Block { + BlockType Reference + Name "GetMeasurement2" + SID "4541" + Ports [0, 1] + Position [785, 462, 860, 478] + ZOrder 960 + BackgroundColor "[0.513700, 0.674500, 1.000000]" + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetMeasurement" + SourceType "GetMeasurement" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + measuredType "Joints Velocity" + } + Block { + BlockType Goto + Name "GoTo Motors Inertia" + SID "4831" + Position [955, 387, 1040, 403] + ZOrder 906 + HideAutomaticName off + GotoTag "jointAcc" + TagVisibility "global" + } + Block { + BlockType Reference + Name "IMU_meas" + SID "2630" + Ports [0, 1] + Position [800, 494, 840, 506] + ZOrder 959 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" + SourceType "YARP Read" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + portName "Ports.IMU" + signalSize "Ports.IMU_PORT_SIZE" + timeout "0.5" + blocking off + timestamp off + autoconnect on + errorOnConnection on + } + Block { + BlockType SubSystem + Name "MOMENTUM BASED TORQUE CONTROL" + SID "4836" + Ports [5, 1] + Position [955, 427, 1105, 573] + ZOrder 961 + RequestExecContextInheritance off + System { + Name "MOMENTUM BASED TORQUE CONTROL" + Location [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "123" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos" + SID "4837" + Position [160, 288, 190, 302] + ZOrder 556 + } + Block { + BlockType Inport + Name "jointVel" + SID "4838" + Position [160, 418, 190, 432] + ZOrder 557 + Port "2" + } + Block { + BlockType Inport + Name "IMU_meas" + SID "4840" + Position [160, 478, 190, 492] + ZOrder 559 + Port "3" + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "4880" + Position [160, 538, 190, 552] + ZOrder 963 + Port "4" + } + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "4881" + Position [160, 598, 190, 612] + ZOrder 964 + Port "5" + } + Block { + BlockType SubSystem + Name "Balancing Controller QP" + SID "2355" + Ports [20, 1] + Position [800, 31, 955, 609] + ZOrder 542 + BackgroundColor "lightBlue" + RequestExecContextInheritance off + System { + Name "Balancing Controller QP" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "164" + 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 "nu" + SID "2359" + Position [1065, 123, 1095, 137] + ZOrder 325 + Port "13" + } + Block { + BlockType Inport + Name "state" + SID "4387" + Position [1065, 318, 1095, 332] + ZOrder 840 + Port "14" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "2363" + Position [1065, -102, 1095, -88] + ZOrder 386 + Port "15" + } + Block { + BlockType Inport + Name "KP_postural" + SID "2356" + Position [1065, 843, 1095, 857] + ZOrder 336 + Port "16" + } + Block { + BlockType Inport + Name "KP_CoM" + SID "3328" + Position [1065, 753, 1095, 767] + ZOrder 644 + Port "17" + } + Block { + BlockType Inport + Name "KD_CoM" + SID "3329" + Position [1065, 798, 1095, 812] + ZOrder 645 + Port "18" + } + Block { + BlockType Inport + Name "desired_pos_vel_acc_CoM" + SID "2365" + Position [1065, 708, 1095, 722] + ZOrder 335 + Port "19" + } + Block { + BlockType Inport + Name "jointPos_des" + SID "2364" + Position [1065, 78, 1095, 92] + ZOrder 324 + Port "20" + } + Block { + BlockType SubSystem + Name "Compute Desired Torques" + SID "4393" + Ports [13, 2] + Position [1825, -185, 1990, 865] + ZOrder 846 + RequestExecContextInheritance off + System { + Name "Compute Desired Torques" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "284" + 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 + RequestExecContextInheritance off + System { + Name "QPSolver" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "343" + 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 + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "105" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "feetContactStatus" + SID "4590::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Demux + Name " Demux " + SID "4590::104" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 95 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4590::103" + Tag "Stateflow S-Function 15" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 94 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "onOneFoot" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4590::105" + Position [460, 241, 480, 259] + ZOrder 96 + } + Block { + BlockType Outport + Name "onOneFoot" + SID "4590::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 105 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "onOneFoot" + ZOrder 106 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "onOneFoot" + DstPort 1 + } + Line { + ZOrder 107 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 108 + 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 + NamePlacement "alternate" + TreatAsAtomicUnit on + RequestExecContextInheritance off + System { + Name "QP One Foot" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "341" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "H" + SID "4685" + Position [970, -237, 1000, -223] + ZOrder 750 + } + Block { + BlockType Inport + Name "g" + SID "4686" + Position [970, -207, 1000, -193] + ZOrder 752 + Port "2" + } + Block { + BlockType Inport + Name "A" + SID "4687" + Position [970, -177, 1000, -163] + ZOrder 753 + Port "3" + } + Block { + BlockType Inport + Name "ubA" + SID "4688" + Position [970, -147, 1000, -133] + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "106" + 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::105" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 96 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4690::104" + Tag "Stateflow S-Function 26" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 95 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "analyticalSolution" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4690::106" + Position [460, 241, 480, 259] + ZOrder 97 + } + Block { + BlockType Outport + Name "analyticalSolution" + SID "4690::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 81 + SrcBlock "H" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 82 + SrcBlock "g" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "analyticalSolution" + ZOrder 83 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "analyticalSolution" + DstPort 1 + } + Line { + ZOrder 84 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 85 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "MatchSignalSizes" + SID "4691" + Ports [2, 1] + Position [1340, -277, 1415, -188] + ZOrder 757 + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "114" + 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::113" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 98 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4692::112" + Tag "Stateflow S-Function 27" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 97 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[4 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "f_star" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4692::114" + Position [460, 241, 480, 259] + ZOrder 99 + } + Block { + BlockType Outport + Name "f_star" + SID "4692::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 119 + SrcBlock "analyticalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 120 + SrcBlock "primalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 121 + SrcBlock "QPStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 122 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "f_star" + ZOrder 123 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "f_star" + DstPort 1 + } + Line { + ZOrder 124 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 125 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "QP One Foot" + SID "4693" + Ports [4, 2] + Position [1145, -242, 1250, -128] + ZOrder 751 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/QP" + SourceType "QP" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + lbA off + 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, -150] + DstBlock "Analytical Solution QP One Foot (unconstrained)" + DstPort 1 + } + Branch { + ZOrder 6 + DstBlock "QP One Foot" + DstPort 1 + } + } + Line { + ZOrder 7 + SrcBlock "ubA" + SrcPort 1 + DstBlock "QP One Foot" + DstPort 4 + } + Line { + ZOrder 8 + SrcBlock "g" + SrcPort 1 + Points [51, 0] + Branch { + ZOrder 33 + Points [0, -55] + 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 [34, 0] + Branch { + ZOrder 41 + Points [0, 50] + DstBlock "Process QP output" + DstPort 3 + } + Branch { + ZOrder 26 + 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 + } + } + } + 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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "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, 1113, 1490, 1127] + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "106" + 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::105" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 96 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4678::104" + Tag "Stateflow S-Function 24" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 95 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "analyticalSolution" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4678::106" + Position [460, 241, 480, 259] + ZOrder 97 + } + Block { + BlockType Outport + Name "analyticalSolution" + SID "4678::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 81 + SrcBlock "H" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 82 + SrcBlock "g" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "analyticalSolution" + ZOrder 83 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "analyticalSolution" + DstPort 1 + } + Line { + ZOrder 84 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 85 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "MatchSignalSizes" + SID "4679" + Ports [2, 1] + Position [1735, 984, 1825, 1066] + ZOrder 757 + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "114" + 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::113" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 97 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4680::112" + Tag "Stateflow S-Function 25" + Ports [3, 2] + Position [180, 100, 230, 180] + ZOrder 96 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[3 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "f_star" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4680::114" + Position [460, 241, 480, 259] + ZOrder 98 + } + Block { + BlockType Outport + Name "f_star" + SID "4680::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 116 + SrcBlock "analyticalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 117 + SrcBlock "primalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 118 + SrcBlock "QPStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + Name "f_star" + ZOrder 119 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "f_star" + DstPort 1 + } + Line { + ZOrder 120 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 121 + SrcBlock " SFunction " + SrcPort 1 + Points [20, 0] + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "QP Two Feet" + SID "4681" + Ports [4, 2] + Position [1575, 1016, 1680, 1134] + ZOrder 752 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/QP" + SourceType "QP" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + lbA off + 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 [28, 0] + Branch { + ZOrder 62 + Points [0, 55] + DstBlock "Process QP output" + DstPort 3 + } + Branch { + ZOrder 61 + DstBlock "QPStatus" + DstPort 1 + } + } + 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 + DstBlock "MatchSignalSizes" + DstPort 2 + } + Line { + ZOrder 45 + SrcBlock "H" + SrcPort 1 + Points [22, 0] + Branch { + ZOrder 79 + DstBlock "QP Two Feet" + DstPort 1 + } + Branch { + ZOrder 43 + Points [0, -160] + DstBlock "Analytical Solution Two Feet (unconstrained)" + DstPort 1 + } + } + Line { + ZOrder 50 + SrcBlock "g" + SrcPort 1 + Points [47, 0] + Branch { + ZOrder 49 + Points [0, -55] + Branch { + ZOrder 48 + Points [0, -100] + 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 51 + SrcBlock "ubA" + 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 + NamePlacement "alternate" + 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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "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.675" + 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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "723" + 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.675" + 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.675" + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "3822" + 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::3821" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 146 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "3721::3820" + Tag "Stateflow S-Function 20" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 145 + FunctionName "sf_sfun" + Parameters "Reg" + PortCounts "[4 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "baseVel_equivalent" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "3721::3822" + Position [460, 241, 480, 259] + ZOrder 147 + } + Block { + BlockType Outport + Name "baseVel_equivalent" + SID "3721::24" + Position [460, 101, 480, 119] + ZOrder 10 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 260 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 261 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 262 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 263 + SrcBlock "jointPos_err" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "baseVel_equivalent" + ZOrder 264 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "baseVel_equivalent" + DstPort 1 + } + Line { + ZOrder 265 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 266 + 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.675" + 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.675" + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "135" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "state" + SID "4027::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Demux + Name " Demux " + SID "4027::134" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 125 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4027::133" + Tag "Stateflow S-Function 19" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 124 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "booleanState" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4027::135" + Position [460, 241, 480, 259] + ZOrder 126 + } + Block { + BlockType Outport + Name "booleanState" + SID "4027::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 149 + SrcBlock "state" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "booleanState" + ZOrder 150 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "booleanState" + DstPort 1 + } + Line { + ZOrder 151 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 152 + 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 [1, 1] + Position [2065, 51, 2220, 109] + ZOrder 862 + RequestExecContextInheritance off + System { + Name "From tau_QP to Joint Torques (motor reflected inertia)" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "338" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "tau_QP" + SID "4520" + Position [-30, 3, 0, 17] + ZOrder 591 + } + 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 + NamePlacement "alternate" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" + Location [223, 338, 826, 833] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "154" + SimulinkSubDomain "Simulink" + Block { + BlockType Demux + Name " Demux " + SID "4551::152" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 143 + Outputs "1" + } + Block { + BlockType Ground + Name " Ground " + SID "4551::154" + Position [20, 121, 40, 139] + ZOrder 145 + } + Block { + BlockType S-Function + Name " SFunction " + SID "4551::151" + Tag "Stateflow S-Function 9" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 142 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "reflectedInertia" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4551::153" + Position [460, 241, 480, 259] + ZOrder 144 + } + Block { + BlockType Outport + Name "reflectedInertia" + SID "4551::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + Name "reflectedInertia" + ZOrder 133 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "reflectedInertia" + DstPort 1 + } + Line { + ZOrder 134 + SrcBlock " Ground " + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 135 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 136 + 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 From + Name "From1" + SID "4830" + Position [-460, -108, -380, -92] + ZOrder 903 + ShowName off + HideAutomaticName off + GotoTag "jointAcc" + 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 115 + SrcBlock "From1" + SrcPort 1 + DstBlock "Switch1" + DstPort 3 + } + } + } + Block { + BlockType Goto + Name "Goto1" + SID "4495" + Position [2320, 16, 2390, 34] + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "3821" + 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::3820" + Ports [1, 1] + Position [270, 570, 320, 610] + ZOrder 223 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "2407::3819" + Tag "Stateflow S-Function 17" + Ports [22, 13] + Position [180, 70, 230, 530] + ZOrder 222 + FunctionName "sf_sfun" + Parameters "Config,Gain,Reg" + PortCounts "[22 13]" + SFunctionDeploymentMode off + EnableBusSupport off + 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::3821" + Position [460, 581, 480, 599] + ZOrder 224 + } + 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 2546 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 2547 + SrcBlock "ConstraintsMatrix" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 2548 + SrcBlock "bVectorConstraints" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 2549 + SrcBlock "jointPos" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 2550 + SrcBlock "jointPos_des" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 2551 + SrcBlock "nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + ZOrder 2552 + SrcBlock "M" + SrcPort 1 + DstBlock " SFunction " + DstPort 7 + } + Line { + ZOrder 2553 + SrcBlock "h" + SrcPort 1 + DstBlock " SFunction " + DstPort 8 + } + Line { + ZOrder 2554 + SrcBlock "L" + SrcPort 1 + DstBlock " SFunction " + DstPort 9 + } + Line { + ZOrder 2555 + SrcBlock "intL_angMomError" + SrcPort 1 + DstBlock " SFunction " + DstPort 10 + } + Line { + ZOrder 2556 + SrcBlock "w_H_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 11 + } + Line { + ZOrder 2557 + SrcBlock "w_H_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 12 + } + Line { + ZOrder 2558 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 13 + } + Line { + ZOrder 2559 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 14 + } + Line { + ZOrder 2560 + SrcBlock "JDot_l_sole_nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 15 + } + Line { + ZOrder 2561 + SrcBlock "JDot_r_sole_nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 16 + } + Line { + ZOrder 2562 + SrcBlock "pos_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 17 + } + Line { + ZOrder 2563 + SrcBlock "J_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 18 + } + Line { + ZOrder 2564 + SrcBlock "desired_pos_vel_acc_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 19 + } + Line { + ZOrder 2565 + SrcBlock "KP_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 20 + } + Line { + ZOrder 2566 + SrcBlock "KD_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 21 + } + Line { + ZOrder 2567 + SrcBlock "KP_postural" + SrcPort 1 + DstBlock " SFunction " + DstPort 22 + } + Line { + Name "HessianMatrixOneFoot" + ZOrder 2568 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "HessianMatrixOneFoot" + DstPort 1 + } + Line { + Name "gradientOneFoot" + ZOrder 2569 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "gradientOneFoot" + DstPort 1 + } + Line { + Name "ConstraintsMatrixOneFoot" + ZOrder 2570 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "ConstraintsMatrixOneFoot" + DstPort 1 + } + Line { + Name "bVectorConstraintsOneFoot" + ZOrder 2571 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "bVectorConstraintsOneFoot" + DstPort 1 + } + Line { + Name "HessianMatrixTwoFeet" + ZOrder 2572 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "HessianMatrixTwoFeet" + DstPort 1 + } + Line { + Name "gradientTwoFeet" + ZOrder 2573 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 7 + DstBlock "gradientTwoFeet" + DstPort 1 + } + Line { + Name "ConstraintsMatrixTwoFeet" + ZOrder 2574 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 8 + DstBlock "ConstraintsMatrixTwoFeet" + DstPort 1 + } + Line { + Name "bVectorConstraintsTwoFeet" + ZOrder 2575 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 9 + DstBlock "bVectorConstraintsTwoFeet" + DstPort 1 + } + Line { + Name "tauModel" + ZOrder 2576 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 10 + DstBlock "tauModel" + DstPort 1 + } + Line { + Name "Sigma" + ZOrder 2577 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 11 + DstBlock "Sigma" + DstPort 1 + } + Line { + Name "Na" + ZOrder 2578 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 12 + DstBlock "Na" + DstPort 1 + } + Line { + Name "f_LDot" + ZOrder 2579 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 13 + DstBlock "f_LDot" + DstPort 1 + } + Line { + ZOrder 2580 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 2581 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "tau_star " + SID "2414" + Position [2340, 73, 2370, 87] + 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 + } + } + } + Block { + BlockType SubSystem + Name "Dynamics and Kinematics" + SID "2341" + Ports [3, 11] + Position [605, 23, 735, 347] + ZOrder 541 + BackgroundColor "[0.000000, 1.000000, 0.498039]" + NamePlacement "alternate" + RequestExecContextInheritance off + System { + Name "Dynamics and Kinematics" + Location [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "139" + 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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "522" + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "121" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "M" + SID "4518::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Demux + Name " Demux " + SID "4518::120" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 110 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4518::119" + Tag "Stateflow S-Function 6" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 109 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "M_with_inertia" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4518::121" + Position [460, 241, 480, 259] + ZOrder 111 + } + Block { + BlockType Outport + Name "M_with_inertia" + SID "4518::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 146 + SrcBlock "M" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "M_with_inertia" + ZOrder 147 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "M_with_inertia" + DstPort 1 + } + Line { + ZOrder 148 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 149 + 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.675" + 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.675" + 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.675" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "300" + 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.675" + 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.675" + 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.675" + 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.675" + 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.675" + 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.675" + 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.675" + 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.675" + 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 SubSystem + Name "Joint Torque Saturation" + SID "4553" + Ports [1, 1] + Position [995, 301, 1105, 339] + ZOrder 555 + BackgroundColor "orange" + RequestExecContextInheritance off + System { + Name "Joint Torque Saturation" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "502" + 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 + NamePlacement "alternate" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Saturate Torque Derivative" + Location [223, 338, 826, 833] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "116" + 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::115" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 104 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4556::114" + Tag "Stateflow S-Function 11" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 103 + FunctionName "sf_sfun" + PortCounts "[4 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "uSat" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4556::116" + Position [460, 241, 480, 259] + ZOrder 105 + } + Block { + BlockType Outport + Name "uSat" + SID "4556::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 224 + SrcBlock "u" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 225 + SrcBlock "u_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 226 + SrcBlock "tStep" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 227 + SrcBlock "uDotMax" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "uSat" + ZOrder 228 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "uSat" + DstPort 1 + } + Line { + ZOrder 229 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 230 + 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\n" + SID "4559" + Ports [1, 1] + Position [-20, 13, 15, 27] + ZOrder 15 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + 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 "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 26 + Points [0, -20] + DstBlock "Saturate Torque Derivative" + DstPort 1 + } + Branch { + ZOrder 16 + DstBlock "holder\n" + DstPort 1 + } + } + } + Line { + ZOrder 5 + SrcBlock "Saturate Torque Derivative" + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + Line { + ZOrder 8 + SrcBlock "holder\n" + 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 [5, 9] + Position [285, 338, 445, 632] + ZOrder 548 + BackgroundColor "orange" + Priority "-10" + RequestExecContextInheritance off + System { + Name "Robot State and References" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "357" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos" + SID "4834" + Position [-580, 2678, -550, 2692] + ZOrder 959 + } + Block { + BlockType Inport + Name "jointVel" + SID "4833" + Position [80, 2523, 110, 2537] + ZOrder 958 + Port "2" + } + Block { + BlockType Inport + Name "IMU_meas" + SID "4835" + Position [-580, 2733, -550, 2747] + ZOrder 960 + Port "3" + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "4878" + Position [-580, 2788, -550, 2802] + ZOrder 961 + Port "4" + } + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "4879" + Position [-580, 2843, -550, 2857] + ZOrder 962 + Port "5" + } + 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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "517" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos" + SID "4212" + Position [-10, -187, 20, -173] + ZOrder 562 + NamePlacement "alternate" + } + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "3845" + 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::3844" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 213 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4215::3843" + Tag "Stateflow S-Function 7" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 212 + FunctionName "sf_sfun" + Parameters "Reg" + PortCounts "[4 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "nu_b" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4215::3845" + Position [460, 241, 480, 259] + ZOrder 214 + } + Block { + BlockType Outport + Name "nu_b" + SID "4215::5" + Position [460, 101, 480, 119] + ZOrder -6 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 113 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 114 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 115 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 116 + SrcBlock "jointVel" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "nu_b" + ZOrder 117 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "nu_b" + DstPort 1 + } + Line { + ZOrder 118 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 119 + 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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "937" + 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.675" + 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.675" + 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 [2, 2] + Position [-455, 2656, -330, 2769] + ZOrder 902 + RequestExecContextInheritance off + System { + Name "Compute base to fixed link transform" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "553" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos" + SID "4310" + Position [50, 38, 80, 52] + ZOrder 902 + } + Block { + BlockType Inport + Name "IMU_meas" + SID "4311" + Position [245, 183, 275, 197] + ZOrder 903 + Port "2" + } + 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 [4, 1] + Position [455, 15, 600, 55] + ZOrder 893 + RequestExecContextInheritance off + System { + Name "LFoot to base link transform " + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "248" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos" + SID "4246" + Position [470, 227, 495, 243] + ZOrder 736 + } + Block { + BlockType Inport + Name "inertial" + SID "4247" + Position [1000, 212, 1030, 228] + ZOrder 741 + Port "2" + } + Block { + BlockType Inport + Name "w_H_link" + SID "4248" + Position [655, 132, 680, 148] + ZOrder 744 + Port "3" + } + Block { + BlockType Inport + Name "w_H_base_fixed" + SID "4249" + Position [470, 32, 495, 48] + ZOrder 745 + Port "4" + } + Block { + BlockType Reference + Name "Fixed base to imu transform" + SID "4250" + Ports [2, 1] + Position [575, 25, 740, 85] + ZOrder 729 + LibraryVersion "1.675" + 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 "4251" + Ports [2, 1] + Position [575, 190, 740, 250] + ZOrder 737 + LibraryVersion "1.675" + 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 "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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "3836" + 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::3835" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 235 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4257::3834" + Tag "Stateflow S-Function 8" + Ports [6, 2] + Position [180, 102, 230, 243] + ZOrder 234 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[6 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "w_H_b" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4257::3836" + Position [460, 241, 480, 259] + ZOrder 236 + } + Block { + BlockType Outport + Name "w_H_b" + SID "4257::106" + Position [460, 101, 480, 119] + ZOrder 80 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 145 + SrcBlock "imu_H_fixedLink" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 146 + SrcBlock "imu_H_fixedLink_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 147 + SrcBlock "fixedLink_H_base" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 148 + SrcBlock "rpyFromIMU_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 149 + SrcBlock "rpyFromIMU" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 150 + SrcBlock "neck_pos" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + Name "w_H_b" + ZOrder 151 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "w_H_b" + 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 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 Reference + Name "Neck Position" + SID "4254" + Ports [0, 1] + Position [1000, 310, 1035, 330] + ZOrder 742 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" + SourceType "YARP Read" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + portName "Ports.NECK_POS" + signalSize "Ports.NECK_POS_PORT_SIZE" + timeout "0.5" + blocking off + timestamp off + autoconnect on + errorOnConnection off + } + 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\n1" + SID "4258" + Ports [1, 1] + Position [1175, 113, 1225, 127] + ZOrder 739 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + 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 "holder\n2" + SID "4259" + Ports [1, 1] + Position [1175, 213, 1225, 227] + ZOrder 740 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + 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 "neck transform" + SID "4260" + Ports [1, 1] + Position [1135, 305, 1260, 335] + ZOrder 743 + RequestExecContextInheritance off + System { + Name "neck transform" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "843" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "neck port" + SID "4261" + Position [45, 18, 75, 32] + ZOrder 725 + NamePlacement "alternate" + } + Block { + BlockType Constant + Name "Constant" + SID "4262" + Position [95, 85, 170, 105] + ZOrder 720 + ShowName off + Value "zeros(3,1)" + } + Block { + BlockType Gain + Name "Gain" + SID "4263" + Position [105, 10, 160, 40] + ZOrder 724 + ShowName off + Gain "pi/180" + } + Block { + BlockType Selector + Name "Selector1" + SID "4264" + Ports [1, 1] + Position [190, 13, 230, 37] + ZOrder 723 + ShowName off + InputPortWidth "Ports.NECK_POS_PORT_SIZE" + IndexOptions "Index vector (dialog)" + Indices "[1:3]" + OutputSizes "1" + Port { + PortNumber 1 + Name "neck pos" + } + } + Block { + BlockType Switch + Name "Switch" + SID "4265" + Position [300, 7, 375, 113] + ZOrder 722 + ShowName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Constant + Name "USE_IMU4EST_BASE1" + SID "4266" + Position [60, 50, 210, 70] + ZOrder 719 + ShowName off + Value "Config.CORRECT_NECK_IMU" + } + Block { + BlockType Outport + Name "neck position" + SID "4267" + Position [425, 53, 455, 67] + ZOrder 726 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "neck port" + SrcPort 1 + DstBlock "Gain" + DstPort 1 + } + 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 4 + SrcBlock "Gain" + SrcPort 1 + DstBlock "Selector1" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "Constant" + SrcPort 1 + DstBlock "Switch" + DstPort 3 + } + Line { + Name "neck pos" + ZOrder 6 + Labels [1, 1] + SrcBlock "Selector1" + 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 7 + DstBlock "Get Base Rotation From IMU" + DstPort 1 + } + Branch { + ZOrder 8 + Points [0, 50] + DstBlock "holder\n1" + DstPort 1 + } + } + Line { + ZOrder 9 + SrcBlock "holder\n2" + 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 "Fixed 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 "Fixed base to root link transform" + SrcPort 1 + DstBlock "Inv_2" + DstPort 2 + } + Line { + ZOrder 17 + SrcBlock "inertial" + SrcPort 1 + Points [96, 0] + Branch { + ZOrder 18 + DstBlock "holder\n2" + DstPort 1 + } + Branch { + ZOrder 19 + Points [0, 50] + DstBlock "Selector1" + DstPort 1 + } + } + Line { + ZOrder 20 + SrcBlock "Neck Position" + SrcPort 1 + DstBlock "neck transform" + DstPort 1 + } + Line { + ZOrder 21 + SrcBlock "neck transform" + SrcPort 1 + DstBlock "Get Base Rotation From IMU" + DstPort 6 + } + 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 "Fixed 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 30 + SrcBlock "holder\n1" + 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 + } + } + } + Block { + BlockType SubSystem + Name "RFoot to base link transform" + SID "4850" + Ports [4, 1] + Position [455, 100, 600, 140] + ZOrder 956 + RequestExecContextInheritance off + System { + Name "RFoot to base link transform" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "248" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "jointPos" + SID "4851" + Position [470, 227, 495, 243] + ZOrder 736 + } + Block { + BlockType Inport + Name "inertial" + SID "4852" + Position [1000, 212, 1030, 228] + ZOrder 741 + Port "2" + } + Block { + BlockType Inport + Name "w_H_link" + SID "4853" + Position [655, 132, 680, 148] + ZOrder 744 + Port "3" + } + Block { + BlockType Inport + Name "w_H_base_fixed" + SID "4854" + Position [470, 32, 495, 48] + ZOrder 745 + Port "4" + } + Block { + BlockType Reference + Name "Fixed base to imu transform" + SID "4855" + Ports [2, 1] + Position [575, 25, 740, 85] + ZOrder 729 + LibraryVersion "1.675" + 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.675" + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "3836" + 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::3835" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 235 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4857::3834" + Tag "Stateflow S-Function 3" + Ports [6, 2] + Position [180, 102, 230, 243] + ZOrder 234 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[6 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "w_H_b" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4857::3836" + Position [460, 241, 480, 259] + ZOrder 236 + } + Block { + BlockType Outport + Name "w_H_b" + SID "4857::106" + Position [460, 101, 480, 119] + ZOrder 80 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 145 + SrcBlock "imu_H_fixedLink" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 146 + SrcBlock "imu_H_fixedLink_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 147 + SrcBlock "fixedLink_H_base" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 148 + SrcBlock "rpyFromIMU_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 149 + SrcBlock "rpyFromIMU" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 150 + SrcBlock "neck_pos" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + Name "w_H_b" + ZOrder 151 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "w_H_b" + 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 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 Reference + Name "Neck Position" + SID "4860" + Ports [0, 1] + Position [1000, 310, 1035, 330] + ZOrder 742 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" + SourceType "YARP Read" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + portName "Ports.NECK_POS" + signalSize "Ports.NECK_POS_PORT_SIZE" + timeout "0.5" + blocking off + timestamp off + autoconnect on + errorOnConnection off + } + 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\n1" + SID "4865" + Ports [1, 1] + Position [1175, 113, 1225, 127] + ZOrder 739 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + 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 "holder\n2" + SID "4866" + Ports [1, 1] + Position [1175, 213, 1225, 227] + ZOrder 740 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + 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 "neck transform" + SID "4867" + Ports [1, 1] + Position [1135, 305, 1260, 335] + ZOrder 743 + RequestExecContextInheritance off + System { + Name "neck transform" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "843" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "neck port" + SID "4868" + Position [45, 18, 75, 32] + ZOrder 725 + NamePlacement "alternate" + } + Block { + BlockType Constant + Name "Constant" + SID "4869" + Position [95, 85, 170, 105] + ZOrder 720 + ShowName off + Value "zeros(3,1)" + } + Block { + BlockType Gain + Name "Gain" + SID "4870" + Position [105, 10, 160, 40] + ZOrder 724 + ShowName off + Gain "pi/180" + } + Block { + BlockType Selector + Name "Selector1" + SID "4871" + Ports [1, 1] + Position [190, 13, 230, 37] + ZOrder 723 + ShowName off + InputPortWidth "Ports.NECK_POS_PORT_SIZE" + IndexOptions "Index vector (dialog)" + Indices "[1:3]" + OutputSizes "1" + Port { + PortNumber 1 + Name "neck pos" + } + } + 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 1 + SrcBlock "neck port" + SrcPort 1 + DstBlock "Gain" + DstPort 1 + } + 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 4 + SrcBlock "Gain" + SrcPort 1 + DstBlock "Selector1" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "Constant" + SrcPort 1 + DstBlock "Switch" + DstPort 3 + } + Line { + Name "neck pos" + ZOrder 6 + Labels [1, 1] + SrcBlock "Selector1" + 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 7 + DstBlock "Get Base Rotation From IMU" + DstPort 1 + } + Branch { + ZOrder 8 + Points [0, 50] + DstBlock "holder\n1" + DstPort 1 + } + } + Line { + ZOrder 9 + SrcBlock "holder\n2" + 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 17 + DstBlock "holder\n2" + DstPort 1 + } + Branch { + ZOrder 18 + Points [0, 50] + DstBlock "Selector1" + DstPort 1 + } + } + Line { + ZOrder 19 + SrcBlock "Neck Position" + 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 28 + SrcBlock "holder\n1" + 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.675" + 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.675" + 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, 28, 680, 42] + ZOrder 910 + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "fixed_r_sole_H_b" + SID "4320" + Position [650, 113, 680, 127] + 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 34 + Points [0, 85] + DstBlock "RFoot to base link transform" + DstPort 4 + } + Branch { + ZOrder 4 + DstBlock "LFoot to base link transform " + DstPort 4 + } + } + 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 3 + } + Line { + ZOrder 33 + SrcBlock "Relative transform r_sole_H_base" + SrcPort 1 + DstBlock "RFoot to base link transform" + DstPort 3 + } + 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 31 + DstBlock "RFoot to base link transform" + DstPort 1 + } + Branch { + ZOrder 14 + Points [0, -85] + DstBlock "LFoot to base link transform " + DstPort 1 + } + } + } + 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 32 + DstBlock "RFoot to base link transform" + DstPort 2 + } + Branch { + ZOrder 19 + Points [0, -85] + DstBlock "LFoot to base link transform " + DstPort 2 + } + } + } + } + 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 "red" + Priority "-100" + RequestExecContextInheritance off + System { + Name "State Machine" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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" + 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 Clock + Name "Clock1" + SID "2166" + Position [-150, 295, -130, 315] + ZOrder 532 + ShowName off + } + 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 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.675" + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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" + 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::3823" + Ports [1, 1] + Position [270, 495, 320, 535] + ZOrder 221 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "2220::3822" + Tag "Stateflow S-Function 13" + Ports [10, 11] + Position [180, 100, 230, 340] + ZOrder 220 + FunctionName "sf_sfun" + Parameters "Config,Gain,StateMachine" + PortCounts "[10 11]" + SFunctionDeploymentMode off + EnableBusSupport off + 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::3824" + Position [460, 506, 480, 524] + ZOrder 222 + } + 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 1146 + SrcBlock "pos_CoM_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 1147 + SrcBlock "jointPos_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 1148 + SrcBlock "pos_CoM_fixed_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 1149 + SrcBlock "pos_CoM_fixed_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 1150 + SrcBlock "jointPos" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 1151 + SrcBlock "time" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + ZOrder 1152 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock " SFunction " + DstPort 7 + } + Line { + ZOrder 1153 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock " SFunction " + DstPort 8 + } + Line { + ZOrder 1154 + SrcBlock "l_sole_H_b" + SrcPort 1 + DstBlock " SFunction " + DstPort 9 + } + Line { + ZOrder 1155 + SrcBlock "r_sole_H_b" + SrcPort 1 + DstBlock " SFunction " + DstPort 10 + } + Line { + Name "w_H_b" + ZOrder 1156 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "w_H_b" + DstPort 1 + } + Line { + Name "pos_CoM_des" + ZOrder 1157 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "pos_CoM_des" + DstPort 1 + } + Line { + Name "jointPos_des" + ZOrder 1158 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "jointPos_des" + DstPort 1 + } + Line { + Name "feetContactStatus" + ZOrder 1159 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "feetContactStatus" + DstPort 1 + } + Line { + Name "KP_postural_diag" + ZOrder 1160 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "KP_postural_diag" + DstPort 1 + } + Line { + Name "KP_CoM_diag" + ZOrder 1161 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 7 + DstBlock "KP_CoM_diag" + DstPort 1 + } + Line { + Name "KD_CoM_diag" + ZOrder 1162 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 8 + DstBlock "KD_CoM_diag" + DstPort 1 + } + Line { + Name "state" + ZOrder 1163 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 9 + DstBlock "state" + DstPort 1 + } + Line { + Name "smoothingTimeJoints" + ZOrder 1164 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 10 + DstBlock "smoothingTimeJoints" + DstPort 1 + } + Line { + Name "smoothingTimeCoM" + ZOrder 1165 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 11 + DstBlock "smoothingTimeCoM" + DstPort 1 + } + Line { + ZOrder 1166 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 1167 + SrcBlock " SFunction " + SrcPort 1 + Points [20, 0] + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "holder\n1" + SID "2187" + Ports [1, 1] + Position [-170, 122, -110, 138] + ZOrder 583 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + 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 "holder\n2" + SID "2188" + Ports [1, 1] + Position [-170, 157, -110, 173] + ZOrder 584 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + 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 "xCom" + SID "4226" + Ports [2, 1] + Position [-385, 184, -235, 211] + ZOrder 890 + ShowName off + RequestExecContextInheritance off + System { + Name "xCom" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "824" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "w_H_b" + SID "4227" + Position [30, 237, 60, 253] + ZOrder 581 + NamePlacement "alternate" + } + 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.675" + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 + NamePlacement "alternate" + } + 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.675" + 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 416 + Points [0, -70] + DstBlock "holder\n1" + 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 262 + DstBlock "xCom" + DstPort 2 + } + Branch { + ZOrder 205 + Points [0, -40] + DstBlock "holder\n2" + DstPort 1 + } + } + 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 408 + SrcBlock "Clock1" + SrcPort 1 + DstBlock "STATE MACHINE" + DstPort 6 + } + 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 411 + SrcBlock "holder\n1" + SrcPort 1 + DstBlock "STATE MACHINE" + DstPort 1 + } + Line { + ZOrder 412 + SrcBlock "holder\n2" + 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 + } + } + } + } + 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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "498" + 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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "777" + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "69" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "d" + SID "4782::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Demux + Name " Demux " + SID "4782::68" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 59 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4782::67" + Tag "Stateflow S-Function 2" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 58 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "D" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4782::69" + Position [460, 241, 480, 259] + ZOrder 60 + } + Block { + BlockType Outport + Name "D" + SID "4782::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 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "69" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "d" + SID "4783::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Demux + Name " Demux " + SID "4783::68" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 59 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4783::67" + Tag "Stateflow S-Function 1" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 58 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "D" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4783::69" + Position [460, 241, 480, 259] + ZOrder 60 + } + Block { + BlockType Outport + Name "D" + SID "4783::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 65 + SrcBlock "d" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "D" + ZOrder 66 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "D" + DstPort 1 + } + Line { + ZOrder 67 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 68 + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "69" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "d" + SID "4784::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Demux + Name " Demux " + SID "4784::68" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 59 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4784::67" + Tag "Stateflow S-Function 4" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 58 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "D" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4784::69" + Position [460, 241, 480, 259] + ZOrder 60 + } + Block { + BlockType Outport + Name "D" + SID "4784::5" + Position [460, 101, 480, 119] + ZOrder -5 + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 65 + SrcBlock "d" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "D" + ZOrder 66 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "D" + DstPort 1 + } + Line { + ZOrder 67 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 68 + 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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "357" + 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.675" + 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 [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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_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.675" + 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 2 + } + Line { + ZOrder 672 + SrcBlock "jointPos" + SrcPort 1 + Points [31, 0] + Branch { + ZOrder 674 + Points [0, -55] + 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 1 + } + } + 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 + } + } + } + Block { + BlockType SubSystem + Name "emergency stop: joint limits" + SID "4913" + Ports [1] + Position [305, 241, 430, 269] + ZOrder 966 + BackgroundColor "red" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 34 + $ClassName "Simulink.Mask" + Display "disp('EMERGENCY STOP')" + RunInitForIconRedraw "off" + } + System { + Name "emergency stop: joint limits" + Location [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "401" + 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 + NamePlacement "alternate" + 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 + NamePlacement "alternate" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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.675" + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "3813" + 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::3812" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 162 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4922::3811" + Tag "Stateflow S-Function 18" + Ports [4, 3] + Position [180, 102, 230, 203] + ZOrder 161 + FunctionName "sf_sfun" + PortCounts "[4 3]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "inRange" + } + Port { + PortNumber 3 + Name "res_check_range" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4922::3813" + Position [460, 241, 480, 259] + ZOrder 163 + } + 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 65 + SrcBlock "umin" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 66 + SrcBlock "umax" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 67 + SrcBlock "u" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 68 + SrcBlock "tol" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "inRange" + ZOrder 69 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "inRange" + DstPort 1 + } + Line { + Name "res_check_range" + ZOrder 70 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "res_check_range" + DstPort 1 + } + Line { + ZOrder 71 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 72 + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "3813" + 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::3812" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 162 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4929::3811" + Tag "Stateflow S-Function 14" + Ports [2, 3] + Position [180, 100, 230, 180] + ZOrder 161 + FunctionName "sf_sfun" + PortCounts "[2 3]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "noSpikes" + } + Port { + PortNumber 3 + Name "res_check_spikes" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4929::3813" + Position [460, 241, 480, 259] + ZOrder 163 + } + 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 49 + SrcBlock "u" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 50 + SrcBlock "delta_u_max" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "noSpikes" + ZOrder 51 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "noSpikes" + DstPort 1 + } + Line { + Name "res_check_spikes" + ZOrder 52 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "res_check_spikes" + DstPort 1 + } + Line { + ZOrder 53 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 54 + 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, 313, 1190, 327] + 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 286 + DstBlock "Balancing Controller QP" + DstPort 13 + } + Branch { + ZOrder 284 + Points [0, -210] + DstBlock "Dynamics and Kinematics" + DstPort 2 + } + } + Line { + ZOrder 134 + SrcBlock "Robot State and References" + SrcPort 7 + DstBlock "Balancing Controller QP" + DstPort 18 + } + Line { + ZOrder 279 + SrcBlock "jointVel" + SrcPort 1 + DstBlock "Robot State and References" + DstPort 2 + } + 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 14 + } + 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 17 + } + 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 20 + } + Line { + ZOrder 130 + SrcBlock "Robot State and References" + SrcPort 4 + DstBlock "Balancing Controller QP" + DstPort 15 + } + Line { + ZOrder 281 + SrcBlock "IMU_meas" + SrcPort 1 + DstBlock "Robot State and References" + DstPort 3 + } + Line { + ZOrder 119 + SrcBlock "Robot State and References" + SrcPort 1 + Points [39, 0; 0, -290] + DstBlock "Dynamics and Kinematics" + DstPort 1 + } + Line { + ZOrder 131 + SrcBlock "Robot State and References" + SrcPort 5 + DstBlock "Balancing Controller QP" + DstPort 16 + } + 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 19 + } + Line { + ZOrder 278 + SrcBlock "jointPos" + SrcPort 1 + Points [36, 0] + Branch { + ZOrder 322 + Points [0, -40] + DstBlock "emergency stop: joint limits" + DstPort 1 + } + Branch { + ZOrder 294 + Points [0, 70] + DstBlock "Robot State and References" + DstPort 1 + } + Branch { + ZOrder 293 + Points [324, 0] + Branch { + ZOrder 289 + DstBlock "Dynamics and Kinematics" + DstPort 3 + } + Branch { + ZOrder 283 + Points [0, 70] + DstBlock "Balancing Controller QP" + DstPort 12 + } + } + } + Line { + ZOrder 313 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock "Robot State and References" + DstPort 4 + } + Line { + ZOrder 314 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock "Robot State and References" + DstPort 5 + } + } + } + Block { + BlockType Reference + Name "SetReferences" + SID "2354" + Ports [1] + Position [1185, 487, 1245, 513] + ZOrder 549 + BackgroundColor "[0.827500, 0.576500, 0.603900]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Actuators/SetReferences" + SourceType "SetReferences" + SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + ContentPreviewEnabled off + controlType "Torque" + refSpeed "50" + refAcceleration "1000000*(pi/180)" + } + Block { + BlockType SubSystem + Name "synchronizer" + SID "2423" + Ports [] + Position [1157, 423, 1272, 453] + ZOrder 546 + ForegroundColor "red" + BackgroundColor "green" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 35 + $ClassName "Simulink.Mask" + Display "disp('SYNCHRONIZER')" + } + System { + Name "synchronizer" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 "947" + SimulinkSubDomain "Simulink" + Block { + BlockType SubSystem + Name "GAZEBO_SYNCHRONIZER" + SID "2424" + Ports [0, 0, 1] + Position [5, 15, 130, 75] + ZOrder -7 + RequestExecContextInheritance off + System { + Name "GAZEBO_SYNCHRONIZER" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 EnablePort + Name "Enable" + SID "2425" + Ports [] + Position [177, 85, 197, 105] + ZOrder 538 + } + Block { + BlockType Reference + Name "Simulator Synchronizer" + SID "2426" + Ports [] + Position [120, 24, 250, 61] + ZOrder 539 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" + SourceType "Simulator Synchronizer" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + period "Config.tStep" + serverPortName "'/clock/rpc'" + clientPortName "'/WBT_synchronizer/rpc:o'" + } + } + } + Block { + BlockType Logic + Name "Logical\nOperator" + SID "2427" + Ports [1, 1] + Position [-30, -34, -5, -16] + ZOrder 307 + BlockMirror on + NamePlacement "alternate" + ShowName off + Operator "NOT" + AllPortsSameDT off + OutDataTypeStr "boolean" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n" + SID "2428" + Position [95, -35, 210, -15] + ZOrder 304 + BlockMirror on + NamePlacement "alternate" + ShowName off + Value "Config.ON_GAZEBO" + } + Block { + BlockType SubSystem + Name "REAL_TIME_SYNC" + SID "2429" + Ports [0, 0, 1] + Position [-160, 17, -35, 77] + ZOrder -9 + RequestExecContextInheritance off + System { + Name "REAL_TIME_SYNC" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" + ModelBrowserVisibility off + 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 EnablePort + Name "Enable" + SID "2430" + Ports [] + Position [167, 90, 187, 110] + ZOrder 538 + } + Block { + BlockType Reference + Name "Real Time Synchronizer" + SID "2431" + Ports [] + Position [115, 34, 240, 71] + ZOrder 539 + ForegroundColor "[0.916667, 0.916667, 0.916667]" + BackgroundColor "gray" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" + SourceType "Real Time Synchronizer" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + period "Config.tStep" + } + } + } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "4628" + Ports [1] + Position [125, -95, 185, -75] + ZOrder 699 + ShowName off + HideAutomaticName off + VariableName "yarp_time" + MaxDataPoints "inf" + SaveFormat "Structure With Time" + FixptAsFi on + SampleTime "-1" + } + Block { + BlockType Reference + Name "Yarp Clock" + SID "4629" + Ports [0, 1] + Position [-50, -94, 15, -76] + ZOrder 698 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Yarp Clock" + SourceType "YARP Clock" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + } + Line { + ZOrder 1 + SrcBlock "Logical\nOperator" + SrcPort 1 + Points [-60, 0] + DstBlock "REAL_TIME_SYNC" + DstPort enable + } + Line { + ZOrder 2 + SrcBlock "ON_GAZEBO\n" + SrcPort 1 + Points [-25, 0] + Branch { + ZOrder 6 + DstBlock "GAZEBO_SYNCHRONIZER" + DstPort enable + } + Branch { + ZOrder 4 + DstBlock "Logical\nOperator" + DstPort 1 + } + } + Line { + ZOrder 8 + SrcBlock "Yarp Clock" + SrcPort 1 + DstBlock "To Workspace" + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "wrench_leftFoot" + SID "2206" + Ports [0, 1] + Position [800, 553, 840, 567] + ZOrder 962 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" + SourceType "YARP Read" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + portName "Ports.WRENCH_LEFT_FOOT" + signalSize "Ports.WRENCH_PORT_SIZE" + timeout "0.5" + blocking off + timestamp off + autoconnect on + errorOnConnection on + } + Block { + BlockType Reference + Name "wrench_rightFoot" + SID "2218" + Ports [0, 1] + Position [800, 523, 840, 537] + ZOrder 963 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" + SourceType "YARP Read" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + portName "Ports.WRENCH_RIGHT_FOOT" + signalSize "Ports.WRENCH_PORT_SIZE" + timeout "0.5" + blocking off + timestamp off + autoconnect on + errorOnConnection on + } + Line { + ZOrder 278 + SrcBlock "MOMENTUM BASED TORQUE CONTROL" + SrcPort 1 + DstBlock "SetReferences" + DstPort 1 + } + Line { + ZOrder 279 + SrcBlock "GetMeasurement" + SrcPort 1 + DstBlock "GoTo Motors Inertia" + DstPort 1 + } + Line { + ZOrder 282 + SrcBlock "GetMeasurement2" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 2 + } + Line { + ZOrder 283 + SrcBlock "GetMeasurement1" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 1 + } + Line { + ZOrder 284 + SrcBlock "IMU_meas" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 3 + } + Line { + ZOrder 286 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 4 + } + Line { + ZOrder 287 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 5 + } + } +} +#Finite State Machines +# +# Stateflow 80000032 +# +# +Stateflow { + machine { + id 1 + name "torqueControlBalancing" + sfVersion 80000019 + firstTarget 237 + } + 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 "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 37 0 0] + viewObj 36 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 6 + disableImplicitCasting 1 + eml { + name "addMotorsInertiaFCN" + } + firstData 38 + firstTransition 42 + firstJunction 41 + } + 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 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 38 + 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 [36 0 39] + } + data { + id 39 + 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 [36 38 40] + } + data { + id 40 + 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 [36 39 0] + } + junction { + id 41 + position [23.5747 49.5747 7] + chart 36 + subviewer 36 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [36 0 0] + } + transition { + id 42 + 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 41 + 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 43 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/Add motors reflected inertia/Add motor r" + "eflected inertias" + chart 36 + } + chart { + id 44 + 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 45 0 0] + viewObj 44 + ssIdHighWaterMark 10 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 7 + disableImplicitCasting 1 + eml { + name "computeBaseVelocityFCN" + } + firstData 46 + firstTransition 53 + firstJunction 52 + } + state { + id 45 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 44 + treeNode [44 0 0 0] + superState SUBCHART + subviewer 44 + 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 46 + 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 [44 0 47] + } + data { + id 47 + 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 [44 46 48] + } + data { + id 48 + 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 [44 47 49] + } + data { + id 49 + 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 [44 48 50] + } + data { + id 50 + 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 [44 49 51] + } + data { + id 51 + 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 [44 50 0] + } + junction { + id 52 + position [23.5747 49.5747 7] + chart 44 + subviewer 44 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [44 0 0] + } + transition { + id 53 + 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 52 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 44 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 44 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [44 0 0] + } + instance { + id 54 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute State Velocity/Compute Base Velocity" + chart 44 + } + chart { + id 55 + 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 56 0 0] + viewObj 55 + ssIdHighWaterMark 88 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 8 + disableImplicitCasting 1 + eml { + name "getWorldToBaseTransformFCN" + } + firstData 57 + firstTransition 66 + firstJunction 65 + } + state { + id 56 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 55 + treeNode [55 0 0 0] + superState SUBCHART + subviewer 55 + 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 57 + 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 [55 0 58] + } + data { + id 58 + 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 [55 57 59] + } + data { + id 59 + 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 [55 58 60] + } + data { + id 60 + 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 [55 59 61] + } + data { + id 61 + 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 [55 60 62] + } + data { + id 62 + 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 [55 61 63] + } + data { + id 63 + 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 [55 62 64] + } + data { + id 64 + 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 [55 63 0] + } + junction { + id 65 + position [23.5747 49.5747 7] + chart 55 + subviewer 55 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [55 0 0] + } + transition { + id 66 + 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 65 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 55 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 55 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [55 0 0] + } + instance { + id 67 + 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 55 + } + chart { + id 68 + 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 69 0 0] + viewObj 68 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 9 + disableImplicitCasting 1 + eml { + name "computeMotorsInertiaFCN" + } + firstData 70 + firstTransition 73 + firstJunction 72 + } + state { + id 69 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 68 + treeNode [68 0 0 0] + superState SUBCHART + subviewer 68 + 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 70 + 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 [68 0 71] + } + data { + id 71 + 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 [68 70 0] + } + junction { + id 72 + position [23.5747 49.5747 7] + chart 68 + subviewer 68 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [68 0 0] + } + transition { + id 73 + 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 72 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 68 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 68 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [68 0 0] + } + instance { + id 74 + 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 68 + } + chart { + id 75 + 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 76 0 0] + viewObj 75 + ssIdHighWaterMark 8 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 11 + disableImplicitCasting 1 + eml { + name "saturateInputDerivativeFCN" + } + firstData 77 + firstTransition 83 + firstJunction 82 + } + state { + id 76 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 75 + treeNode [75 0 0 0] + superState SUBCHART + subviewer 75 + 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 77 + 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 [75 0 78] + } + data { + id 78 + 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 [75 77 79] + } + data { + id 79 + 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 [75 78 80] + } + data { + id 80 + 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 [75 79 81] + } + data { + id 81 + 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 [75 80 0] + } + junction { + id 82 + position [23.5747 49.5747 7] + chart 75 + subviewer 75 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [75 0 0] + } + transition { + id 83 + 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 82 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 75 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 75 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [75 0 0] + } + instance { + id 84 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/Saturate Torque Derivative" + chart 75 + } + chart { + id 85 + 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 86 0 0] + viewObj 85 + ssIdHighWaterMark 86 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 13 + disableImplicitCasting 1 + eml { + name "stateMachineMomentumControlFCN" + } + firstData 87 + firstTransition 111 + firstJunction 110 + } + state { + id 86 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 85 + treeNode [85 0 0 0] + superState SUBCHART + subviewer 85 + 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 87 + 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 [85 0 88] + } + data { + id 88 + 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 [85 87 89] + } + data { + id 89 + 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 [85 88 90] + } + data { + id 90 + 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 [85 89 91] + } + data { + id 91 + 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 [85 90 92] + } + data { + id 92 + 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 [85 91 93] + } + data { + id 93 + 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 [85 92 94] + } + data { + id 94 + 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 [85 93 95] + } + data { + id 95 + 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 [85 94 96] + } + data { + id 96 + 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 [85 95 97] + } + data { + id 97 + 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 [85 96 98] + } + data { + id 98 + 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 [85 97 99] + } + data { + id 99 + 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 [85 98 100] + } + data { + id 100 + 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 [85 99 101] + } + data { + id 101 + 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 [85 100 102] + } + data { + id 102 + 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 [85 101 103] + } + data { + id 103 + 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 [85 102 104] + } + data { + id 104 + 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 [85 103 105] + } + data { + id 105 + 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 [85 104 106] + } + data { + id 106 + 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 [85 105 107] + } + data { + id 107 + 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 [85 106 108] + } + data { + id 108 + 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 [85 107 109] + } + data { + id 109 + 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 [85 108 0] + } + junction { + id 110 + position [23.5747 49.5747 7] + chart 85 + subviewer 85 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [85 0 0] + } + transition { + id 111 + 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 110 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 85 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 85 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [85 0 0] + } + instance { + id 112 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/STATE MACHINE" + chart 85 + } + chart { + id 113 + 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 114 0 0] + viewObj 113 + ssIdHighWaterMark 9 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 14 + disableImplicitCasting 1 + eml { + name "checkInputSpikesFCN" + } + supportVariableSizing 0 + firstData 115 + firstTransition 120 + firstJunction 119 + } + state { + id 114 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 113 + treeNode [113 0 0 0] + superState SUBCHART + subviewer 113 + 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 115 + 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 [113 0 116] + } + data { + id 116 + 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 [113 115 117] + } + data { + id 117 + 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 [113 116 118] + } + data { + id 118 + 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 [113 117 0] + } + junction { + id 119 + position [23.5747 49.5747 7] + chart 113 + subviewer 113 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [113 0 0] + } + transition { + id 120 + 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 119 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 113 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 113 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [113 0 0] + } + instance { + id 121 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/MAT" + "LAB Function" + chart 113 + } + chart { + id 122 + 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 123 0 0] + viewObj 122 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 15 + disableImplicitCasting 1 + eml { + name "robotIsOnSingleSupportQP_FCN" + } + firstData 124 + firstTransition 127 + firstJunction 126 + } + 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 onOneFoot = robotIsOnSingleSupportQP_FCN(feetContactStatus)\n\n onOneFoot = wbc.robot" + "IsOnSingleSupportQP(feetContactStatus);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 124 + 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 [122 0 125] + } + data { + id 125 + 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 [122 124 0] + } + junction { + id 126 + position [23.5747 49.5747 7] + chart 122 + subviewer 122 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [122 0 0] + } + transition { + id 127 + 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 126 + 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 128 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/One Foot Two Fee" + "t QP Selector" + chart 122 + } + chart { + id 129 + 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 130 0 0] + viewObj 129 + ssIdHighWaterMark 82 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 17 + disableImplicitCasting 1 + eml { + name "momentumBasedControllerFCN" + } + firstData 131 + firstTransition 169 + firstJunction 168 + } + state { + id 130 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 129 + treeNode [129 0 0 0] + superState SUBCHART + subviewer 129 + 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 131 + 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 [129 0 132] + } + data { + id 132 + 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 [129 131 133] + } + data { + id 133 + 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 [129 132 134] + } + data { + id 134 + 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 [129 133 135] + } + data { + id 135 + 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 [129 134 136] + } + data { + id 136 + 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 [129 135 137] + } + data { + id 137 + 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 [129 136 138] + } + data { + id 138 + 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 [129 137 139] + } + data { + id 139 + 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 [129 138 140] + } + data { + id 140 + 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 [129 139 141] + } + data { + id 141 + 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 [129 140 142] + } + data { + id 142 + 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 [129 141 143] + } + data { + id 143 + 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 [129 142 144] + } + data { + id 144 + 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 [129 143 145] + } + data { + id 145 + 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 [129 144 146] + } + data { + id 146 + 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 [129 145 147] + } + data { + id 147 + 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 [129 146 148] + } + data { + id 148 + 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 [129 147 149] + } + data { + id 149 + 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 [129 148 150] + } + data { + id 150 + 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 [129 149 151] + } + data { + id 151 + 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 [129 150 152] + } + data { + id 152 + 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 [129 151 153] + } + data { + id 153 + 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 [129 152 154] + } + data { + id 154 + 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 [129 153 155] + } + data { + id 155 + 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 [129 154 156] + } + data { + id 156 + 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 [129 155 157] + } + data { + id 157 + 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 [129 156 158] + } + data { + id 158 + 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 [129 157 159] + } + data { + id 159 + 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 [129 158 160] + } + data { + id 160 + 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 [129 159 161] + } + data { + id 161 + 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 [129 160 162] + } + data { + id 162 + 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 [129 161 163] + } + data { + id 163 + 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 [129 162 164] + } + data { + id 164 + 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 [129 163 165] + } + data { + id 165 + 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 [129 164 166] + } + data { + id 166 + 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 [129 165 167] + } + data { + id 167 + 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 [129 166 0] + } + junction { + id 168 + position [23.5747 49.5747 7] + chart 129 + subviewer 129 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [129 0 0] + } + transition { + id 169 + 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 168 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 129 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 129 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [129 0 0] + } + instance { + id 170 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Momentum Based Balancing Controller\n" + chart 129 + } + chart { + id 171 + 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 172 0 0] + viewObj 171 + ssIdHighWaterMark 9 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 18 + disableImplicitCasting 1 + eml { + name "checkInputRangeFCN" + } + supportVariableSizing 0 + firstData 173 + firstTransition 180 + firstJunction 179 + } + state { + id 172 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 171 + treeNode [171 0 0 0] + superState SUBCHART + subviewer 171 + 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 173 + 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 [171 0 174] + } + data { + id 174 + 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 [171 173 175] + } + data { + id 175 + 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 [171 174 176] + } + data { + id 176 + 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 [171 175 177] + } + data { + id 177 + 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 [171 176 178] + } + data { + id 178 + 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 [171 177 0] + } + junction { + id 179 + position [23.5747 49.5747 7] + chart 171 + subviewer 171 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [171 0 0] + } + transition { + id 180 + 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 179 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 171 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 171 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [171 0 0] + } + instance { + id 181 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" + chart 171 + } + chart { + id 182 + 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 183 0 0] + viewObj 182 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 19 + disableImplicitCasting 1 + eml { + name "footOnGround" + } + firstData 184 + firstTransition 187 + firstJunction 186 + } + state { + id 183 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 182 + treeNode [182 0 0 0] + superState SUBCHART + subviewer 182 + 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 184 + 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 [182 0 185] + } + data { + id 185 + 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 [182 184 0] + } + junction { + id 186 + position [23.5747 49.5747 7] + chart 182 + subviewer 182 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [182 0 0] + } + transition { + id 187 + 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 186 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 182 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 182 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [182 0 0] + } + instance { + id 188 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Select ba" + "se to world transform" + chart 182 + } + chart { + id 189 + 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 190 0 0] + viewObj 189 + ssIdHighWaterMark 10 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 20 + disableImplicitCasting 1 + eml { + name "getEquivalentBaseVel_FCN" + } + firstData 191 + firstTransition 198 + firstJunction 197 + } + state { + id 190 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 189 + treeNode [189 0 0 0] + superState SUBCHART + subviewer 189 + 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 191 + 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 [189 0 192] + } + data { + id 192 + 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 [189 191 193] + } + data { + id 193 + 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 [189 192 194] + } + data { + id 194 + 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 [189 193 195] + } + data { + id 195 + 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 [189 194 196] + } + data { + id 196 + 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 [189 195 0] + } + junction { + id 197 + position [23.5747 49.5747 7] + chart 189 + subviewer 189 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [189 0 0] + } + transition { + id 198 + 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 197 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 189 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 189 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [189 0 0] + } + instance { + id 199 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Get Equiv" + "alent Base Velocity" + chart 189 + } + chart { + id 200 + 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 201 0 0] + viewObj 200 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 24 + disableImplicitCasting 1 + eml { + name "analyticalSolutionQP_FCN" + } + firstData 202 + firstTransition 206 + firstJunction 205 + } + state { + id 201 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 200 + treeNode [200 0 0 0] + superState SUBCHART + subviewer 200 + 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 202 + 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 [200 0 203] + } + data { + id 203 + 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 [200 202 204] + } + data { + id 204 + 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 [200 203 0] + } + junction { + id 205 + position [23.5747 49.5747 7] + chart 200 + subviewer 200 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [200 0 0] + } + transition { + id 206 + 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 205 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 200 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 200 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [200 0 0] + } + instance { + id 207 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Anal" + "ytical Solution Two Feet (unconstrained)" + chart 200 + } + chart { + id 208 + 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 209 0 0] + viewObj 208 + ssIdHighWaterMark 14 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 25 + disableImplicitCasting 1 + eml { + name "processOutputQP_twoFeetFCN" + } + firstData 210 + firstTransition 216 + firstJunction 215 + } + state { + id 209 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 208 + treeNode [208 0 0 0] + superState SUBCHART + subviewer 208 + 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 210 + 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 [208 0 211] + } + data { + id 211 + 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 [208 210 212] + } + data { + id 212 + 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 [208 211 213] + } + data { + id 213 + 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 [208 212 214] + } + data { + id 214 + 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 [208 213 0] + } + junction { + id 215 + position [23.5747 49.5747 7] + chart 208 + subviewer 208 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [208 0 0] + } + transition { + id 216 + 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 215 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 208 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 208 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [208 0 0] + } + instance { + id 217 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Proc" + "ess QP output" + chart 208 + } + chart { + id 218 + 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 219 0 0] + viewObj 218 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 26 + disableImplicitCasting 1 + eml { + name "analyticalSolutionQP_FCN" + } + firstData 220 + firstTransition 224 + firstJunction 223 + } + state { + id 219 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 218 + treeNode [218 0 0 0] + superState SUBCHART + subviewer 218 + 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 220 + 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 [218 0 221] + } + data { + id 221 + 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 [218 220 222] + } + data { + id 222 + 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 [218 221 0] + } + junction { + id 223 + position [23.5747 49.5747 7] + chart 218 + subviewer 218 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [218 0 0] + } + transition { + id 224 + 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 223 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 218 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 218 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [218 0 0] + } + instance { + id 225 + 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 218 + } + chart { + id 226 + 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 227 0 0] + viewObj 226 + ssIdHighWaterMark 14 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 27 + disableImplicitCasting 1 + eml { + name "processOutputQP_oneFoot" + } + firstData 228 + firstTransition 235 + firstJunction 234 + } + state { + id 227 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 226 + treeNode [226 0 0 0] + superState SUBCHART + subviewer 226 + 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 228 + 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 [226 0 229] + } + data { + id 229 + 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 [226 228 230] + } + data { + id 230 + 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 [226 229 231] + } + data { + id 231 + 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 [226 230 232] + } + data { + id 232 + 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 [226 231 233] + } + data { + id 233 + 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 [226 232 0] + } + junction { + id 234 + position [23.5747 49.5747 7] + chart 226 + subviewer 226 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [226 0 0] + } + transition { + id 235 + 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 234 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 226 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 226 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [226 0 0] + } + instance { + id 236 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Proc" + "ess QP output" + chart 226 + } + target { + id 237 + machine 1 + name "sfun" + codeFlags "" + linkNode [1 0 238] + } + target { + id 238 + machine 1 + name "rtw" + linkNode [1 237 0] + } +} From a41ac94c1c7e6e68b1240dd98838350f9bd14f5f Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Tue, 27 Apr 2021 22:16:39 +0200 Subject: [PATCH 02/20] Replaced Gazebo target by the matlab simulator library blocks - Replaced Gazebo interface blocks (setTorque, getPosition, etc) by: - RobotDynWithContacts, - IMUsensor, - RobotVisualizer. - Added configRobotSim.m config file specfic to the simulation, making sure there were no collisions with configRobot.m. - Defined Visualizer init configuration. --- .../CMakeLists.txt | 2 +- .../app/robots/iCubGenova04/configRobotSim.m | 100 + ...m => initTorqueControlBalancingWithSimu.m} | 11 +- .../src/initVisualizer.m | 16 + ...m => stopTorqueControlBalancingWithSimu.m} | 0 ...mdl => torqueControlBalancingWithSimu.mdl} | 2453 ++++++++++------- 6 files changed, 1525 insertions(+), 1057 deletions(-) create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m rename controllers/floating-base-balancing-torque-control-with-simulator/{initTorqueControlBalancing.m => initTorqueControlBalancingWithSimu.m} (93%) create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m rename controllers/floating-base-balancing-torque-control-with-simulator/{stopTorqueControlBalancing.m => stopTorqueControlBalancingWithSimu.m} (100%) rename controllers/floating-base-balancing-torque-control-with-simulator/{torqueControlBalancing.mdl => torqueControlBalancingWithSimu.mdl} (93%) diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/CMakeLists.txt b/controllers/floating-base-balancing-torque-control-with-simulator/CMakeLists.txt index 17b0039c..2d0a60ef 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/CMakeLists.txt +++ b/controllers/floating-base-balancing-torque-control-with-simulator/CMakeLists.txt @@ -1 +1 @@ -register_mdl(MODELNAME "torqueControlBalancing") \ No newline at end of file +register_mdl(MODELNAME "torqueControlBalancingWithSimu") \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m new file mode 100644 index 00000000..9e545e48 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m @@ -0,0 +1,100 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% % +% 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'}; +WBTConfigRobotSim.ControlledJoints = []; +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(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.65]; +initialConditions.orientation = diag([-1, -1, 1]); +initialConditions.w_H_b = mwbs.State.Rp2H(initialConditions.orientation, initialConditions.base_position); + +% joint (inital) position +initialConditions.s = [ + 0.1744; 0.0007; 0.0001; ... + -0.1745; 0.4363; 0.6981; 0.2618; ... + -0.1745; 0.4363; 0.6981; 0.2618; ... + 1; 0.0001; -0.0001; -0.0002; -0.0004; 0.0003; ... + 0.0002; 0.0001; -0.0002; 0.0004; -0.5; 0.0003]; + +% 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; + +% 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; + +% 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/initTorqueControlBalancing.m b/controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancingWithSimu.m similarity index 93% rename from controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancing.m rename to controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancingWithSimu.m index aa9da74c..a3e2104f 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancing.m +++ b/controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancingWithSimu.m @@ -38,7 +38,8 @@ Config.SIMULATION_TIME = 600000; % Controller period [s] -Config.tStep = 0.01; +Config.tStep = 0.01; % 10 ms +Config.tStepSim = 0.001; % 1 ms %% PRELIMINARY CONFIGURATION % @@ -68,9 +69,11 @@ Config.PLOT_INTEGRATION_TIME = false; % Run robot-specific configuration parameters -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'),'/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') 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..5c2e6072 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m @@ -0,0 +1,16 @@ +%% configuration for the matlab iDyntree visualizer + +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; + +% size of the square you see around the robot +confVisualizer.aroundRobot = visualizerAroundRobot; + +% refresh rate of the picure +confVisualizer.tStep = 0.040; % here equal to the time step used in the simulink model diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/stopTorqueControlBalancing.m b/controllers/floating-base-balancing-torque-control-with-simulator/stopTorqueControlBalancingWithSimu.m similarity index 100% rename from controllers/floating-base-balancing-torque-control-with-simulator/stopTorqueControlBalancing.m rename to controllers/floating-base-balancing-torque-control-with-simulator/stopTorqueControlBalancingWithSimu.m diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancing.mdl b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl similarity index 93% rename from controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancing.mdl rename to controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl index 70b3493d..e1262f1b 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancing.mdl +++ b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl @@ -1,12 +1,13 @@ Model { - Name "torqueControlBalancing" - Version 10.0 + Name "torqueControlBalancingWithSimu" + Version 10.2 SavedCharacterEncoding "UTF-8" + ModelUUID "79a42ce4-6d16-4906-af88-8f5d6a197ba1" GraphicalInterface { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.3516" + ComputedModelVersion "3.28" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -19,381 +20,351 @@ Model { IsArchitectureModel 0 IsAUTOSARArchitectureModel 0 NumParameterArguments 0 - NumExternalFileReferences 55 + NumExternalFileReferences 49 ExternalFileReference { Reference "WBToolboxLibrary/Utilities/Configuration" - Path "torqueControlBalancing/Configuration" + Path "torqueControlBalancingWithSimu/Configuration" SID "4537" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/States/GetMeasurement" - Path "torqueControlBalancing/Dump and visualize/Visualizer/GetMeasurement1" + Path "torqueControlBalancingWithSimu/Dump and visualize/Visualizer/GetMeasurement1" SID "4543" Type "LIBRARY_BLOCK" } - ExternalFileReference { - Reference "WBToolboxLibrary/States/GetMeasurement" - Path "torqueControlBalancing/GetMeasurement" - SID "4539" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/States/GetMeasurement" - Path "torqueControlBalancing/GetMeasurement1" - SID "4538" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/States/GetMeasurement" - Path "torqueControlBalancing/GetMeasurement2" - SID "4541" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/YarpRead" - Path "torqueControlBalancing/IMU_meas" - SID "2630" - Type "LIBRARY_BLOCK" - } ExternalFileReference { Reference "WBToolboxLibrary/Utilities/MatchSignalSizes" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torque" - "s/QPSolver/QP One Foot/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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torque" - "s/QPSolver/QP One Foot/QP One Foot" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torque" - "s/QPSolver/QP Two Feet/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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torque" - "s/QPSolver/QP Two Feet/QP Two Feet" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" - "um integral error/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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" - "um integral error/Compute base to fixed link transform/l_sole to root_link relative transform" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" - "um integral error/Compute base to fixed link transform/r_sole to root_link relative transform" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" - "um integral error/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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" - "um integral error/Jacobian1" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/CentroidalMom" - "entum" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/Centr" + "oidalMomentum" SID "2345" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/GetBiasForces" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/CoM" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/CoM" SID "4363" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Jacobians/DotJNu" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/DotJNu l_so" - "le" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/DotJNu r_so" - "le" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Jacobian com" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Jacobian l_" - "sole" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Jacobian r_" - "sole" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/l_sole" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/r_sole" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/r_s" + "ole" SID "2406" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/holder " + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/holder " SID "4559" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute State Veloc" - "ity/Feet Jacobians/Jacobian l_sole" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute State Veloc" - "ity/Feet Jacobians/Jacobian r_sole" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" - "ed link transform/LFoot to base link transform /Fixed base to imu transform" + 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 "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" - "ed link transform/LFoot to base link transform /Fixed base to root link transform" + 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 root link transform" SID "4251" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Utilities/YarpRead" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" - "ed link transform/LFoot to base link transform /Neck Position" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/LFoot to base link transform /Neck Position" SID "4254" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" - "ed link transform/LFoot to base link transform /holder 1" + 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 "4258" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" - "ed link transform/LFoot to base link transform /holder 2" + 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 "4259" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" - "ed link transform/RFoot to base link transform/Fixed base to imu transform" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" - "ed link transform/RFoot to base link transform/Fixed base to root link transform" + 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 "WBToolboxLibrary/Utilities/YarpRead" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" - "ed link transform/RFoot to base link transform/Neck Position" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/RFoot to base link transform/Neck Position" SID "4860" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" - "ed link transform/RFoot to base link transform/holder 1" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/RFoot to base link transform/holder 1" SID "4865" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" - "ed link transform/RFoot to base link transform/holder 2" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" + "e to fixed link transform/RFoot to base link transform/holder 2" SID "4866" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" - "ed link transform/Relative transform l_sole_H_base" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" - "ed link transform/Relative transform r_sole_H_base" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/Minim" - "umJerkTrajectoryGenerator" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machi" + "ne/MinimumJerkTrajectoryGenerator" SID "2176" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/holde" - "r 1" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machi" + "ne/holder 1" SID "2187" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/holde" - "r 2" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machi" + "ne/holder 2" SID "2188" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" - Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/xCom/" - "CoM" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/xCom2" - "/CoM" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and Re" - "ferences/Smooth reference CoM/MinimumJerkTrajectoryGenerator2" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and Re" - "ferences/Smooth reference joint position/MinimumJerkTrajectoryGenerator1" + 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 "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF JOINTS HI" - "T THE LIMITS/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 "WBToolboxLibrary/Actuators/SetReferences" - Path "torqueControlBalancing/SetReferences" - SID "2354" + Reference "mwbs_visualizers_lib/RobotVisualizer" + Path "torqueControlBalancingWithSimu/RobotVisualizer" + SID "4939" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Simulator Synchronizer" - Path "torqueControlBalancing/synchronizer/GAZEBO_SYNCHRONIZER/Simulator Synchronizer" - SID "2426" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Real Time Synchronizer" - Path "torqueControlBalancing/synchronizer/REAL_TIME_SYNC/Real Time Synchronizer" - SID "2431" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Clock" - Path "torqueControlBalancing/synchronizer/Yarp Clock" - SID "4629" + Reference "WBToolboxLibrary/Utilities/Configuration" + Path "torqueControlBalancingWithSimu/Subsystem/Configuration" + SID "4962" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/YarpRead" - Path "torqueControlBalancing/wrench_leftFoot" - SID "2206" + Reference "mwbs_robotSensors_lib/IMUsensor" + Path "torqueControlBalancingWithSimu/Subsystem/IMUsensor" + SID "4938" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/YarpRead" - Path "torqueControlBalancing/wrench_rightFoot" - SID "2218" + Reference "mwbs_robotDynamicsWithContacts_lib/RobotDynWithContacts" + Path "torqueControlBalancingWithSimu/Subsystem/RobotDynWithContacts" + SID "4937" Type "LIBRARY_BLOCK" } OrderedModelArguments 1 } - AnimationPlugin "on" - LogicAnalyzerPlugin "on" - WebScopes_FoundationPlugin "on" 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 @@ -406,22 +377,22 @@ Model { 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)));\ninitTorqueControlBalancing;\n\n%% Try to ed" - "it the GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n set(sl_synch_handles.startB" - "utton,'Backgroundcolor',[0.0,1.0,0.0]);\n set(sl_synch_handles.startButton,'Enable','on');\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)));\nstopTorqueControlBalancing;\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.checkbox_" - "recompile, 'Value') \n set(sl_synch_handles.startButton,'Backgroundcolor',[0.0,1.0,0.0]);\n set(sl_sy" - "nch_handles.startButton,'Enable','on');\n end\n\n %% Update Compile Button\n set(sl_synch_handles.compileBu" - "tton,'Backgroundcolor',[1.0,0.6,0.0]);\n set(sl_synch_handles.compileButton,'Enable','on');\n\n %% Update Exit" - " Button\n set(sl_synch_handles.exitButton,'Backgroundcolor',[0.0,1.0,1.0]);\n set(sl_synch_handles.exitButton," - "'Enable','on');\n\nend\n\n\n" - LastSavedArchitecture "glnxa64" + 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 @@ -431,7 +402,7 @@ Model { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [67.0, 27.0, 1853.0, 1053.0] + Location [-75.0, -1417.0, 2560.0, 1417.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -451,88 +422,209 @@ Model { } Array { Type "Simulink.EditorInfo" - Dimension 12 + Dimension 23 Object { $ObjectID 5 IsActive [1] IsTabbed [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [1815.0, 821.0] - ZoomFactor [3.61010101010101] - Offset [776.12199216564068, 371.79127028539449] - SceneRectInView [0.0, 0.0, 0.0, 0.0] + Extents [2522.0, 1188.0] + ZoomFactor [2.3918755287864548] + Offset [714.122328915358, 197.16778184538103] + SceneRectInView [714.122328915358, 197.16778184538103, 1054.4026934710794, 496.68136393483036] } Object { $ObjectID 6 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4917" - Extents [1815.0, 821.0] - ZoomFactor [4.3275706438106187] - Offset [10.41989407940855, -23.235071395084702] - SceneRectInView [0.0, 0.0, 0.0, 0.0] + LoadSaveID "4519" + Extents [2522.0, 1188.0] + ZoomFactor [3.3799999999999994] + Offset [-591.8224852071005, -288.66568047337279] + SceneRectInView [-591.8224852071005, -288.66568047337279, 746.1538461538463, 351.47928994082844] } Object { $ObjectID 7 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4913" - Extents [1815.0, 821.0] - ZoomFactor [4.0050505050505052] - Offset [94.721007009531633, 186.50441361916774] - SceneRectInView [0.0, 0.0, 0.0, 0.0] + LoadSaveID "2087" + Extents [2522.0, 1188.0] + ZoomFactor [2.4432827938801593] + Offset [-610.27687073977552, 2460.8844664695271] + SceneRectInView [-610.27687073977552, 2460.8844664695271, 1032.217803979551, 486.23106706094632] } Object { $ObjectID 8 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4925" - Extents [1815.0, 821.0] - ZoomFactor [3.8000000393627378] - Offset [-68.684210488152473, -33.394738158739756] - SceneRectInView [0.0, 0.0, 0.0, 0.0] + LoadSaveID "2355" + Extents [2522.0, 1188.0] + ZoomFactor [1.0730804810360777] + Offset [529.272979525862, -198.046551724138] + SceneRectInView [529.272979525862, -198.046551724138, 2350.2431034482761, 1107.093103448276] } Object { $ObjectID 9 IsActive [0] - IsTabbed [1] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "3241" + Extents [2522.0, 1188.0] + ZoomFactor [2.3999999999999995] + Offset [-657.48177083333348, 60.999999999999943] + SceneRectInView [-657.48177083333348, 60.999999999999943, 1050.8333333333335, 495.00000000000011] + } + Object { + $ObjectID 10 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4030" + Extents [2522.0, 1188.0] + ZoomFactor [3.2861189801699715] + Offset [203.38965517241377, -277.26034482758621] + SceneRectInView [203.38965517241377, -277.26034482758621, 767.47068965517246, 361.52068965517242] + } + Object { + $ObjectID 11 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4761" + Extents [2522.0, 1188.0] + ZoomFactor [4.26] + Offset [30.744516578638468, -178.45657276995306] + SceneRectInView [30.744516578638468, -178.45657276995306, 592.01877934272306, 278.87323943661971] + } + Object { + $ObjectID 12 + IsActive [0] + IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "4836" + Extents [2522.0, 1188.0] + ZoomFactor [1.8210361067503922] + Offset [-17.736368534482835, 1.3120689655171986] + SceneRectInView [-17.736368534482835, 1.3120689655171986, 1384.9258620689657, 652.3758620689656] + } + Object { + $ObjectID 13 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937" + Extents [2522.0, 1188.0] + ZoomFactor [1.5528781793842035] + Offset [-54.517079741379348, -192.01551724137931] + SceneRectInView [-54.517079741379348, -192.01551724137931, 1624.0810344827587, 765.03103448275863] + } + Object { + $ObjectID 14 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4913" + Extents [2522.0, 1188.0] + ZoomFactor [4.0050505050505052] + Offset [94.721007009531661, 186.50441361916774] + SceneRectInView [94.721007009531661, 186.50441361916774, 629.70491803278685, 296.62547288776796] + } + Object { + $ObjectID 15 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4917" + Extents [2522.0, 1188.0] + ZoomFactor [4.3275706438106187] + Offset [10.41989407940855, -23.235071395084702] + SceneRectInView [10.41989407940855, -23.235071395084702, 582.77500417168619, 274.51891552575859] + } + Object { + $ObjectID 16 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2275" + Extents [2522.0, 1188.0] + ZoomFactor [2.6728110599078341] + Offset [-71.787931034482767, -38.237931034482756] + SceneRectInView [-71.787931034482767, -38.237931034482756, 943.57586206896553, 444.47586206896551] + } + Object { + $ObjectID 17 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4806" + Extents [2522.0, 1188.0] + ZoomFactor [3.0] + Offset [-684.37478764306115, -198.09728183118739] + SceneRectInView [-684.37478764306115, -198.09728183118739, 840.66666666666663, 396.0] + } + Object { + $ObjectID 18 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4955" + Extents [2522.0, 1188.0] + ZoomFactor [3.0728476821192054] + Offset [102.27209051724139, 158.19396551724139] + SceneRectInView [102.27209051724139, 158.19396551724139, 820.73706896551721, 386.61206896551721] + } + Object { + $ObjectID 19 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4938" + Extents [2522.0, 1188.0] + ZoomFactor [3.7866767887644714] + Offset [-245.23623571571773, -105.36577786688051] + SceneRectInView [-245.23623571571773, -105.36577786688051, 666.01934643143545, 313.731555733761] + } + Object { + $ObjectID 20 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4925" Extents [1815.0, 821.0] - ZoomFactor [1.2332814930015552] - Offset [-60.865177726986076, -5.3518284993695033] - SceneRectInView [0.0, 0.0, 0.0, 0.0] + ZoomFactor [3.8000000393627378] + Offset [-68.684210488152473, -33.394738158739756] + SceneRectInView [-68.684210488152473, -33.394738158739756, 477.63157399976672, 216.05262934094131] } Object { - $ObjectID 10 + $ObjectID 21 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "2341" Extents [1815.0, 821.0] ZoomFactor [1.3863636363636365] Offset [-1148.4886014344261, -206.09836065573768] - SceneRectInView [0.0, 0.0, 0.0, 0.0] + SceneRectInView [-1148.4886014344261, -206.09836065573768, 1309.1803278688524, 592.19672131147536] } Object { - $ObjectID 11 + $ObjectID 22 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "4321" Extents [1815.0, 821.0] ZoomFactor [3.0] Offset [-211.69612560424537, 280.84982585720286] - SceneRectInView [0.0, 0.0, 0.0, 0.0] + SceneRectInView [-211.69612560424537, 280.84982585720286, 605.0, 273.66666666666669] } Object { - $ObjectID 12 + $ObjectID 23 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "4886" Extents [1815.0, 821.0] @@ -541,9 +633,9 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 13 + $ObjectID 24 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "4890" Extents [1815.0, 821.0] @@ -552,9 +644,9 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 14 + $ObjectID 25 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "4664" Extents [1815.0, 821.0] @@ -563,9 +655,9 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 15 + $ObjectID 26 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "2416" Extents [1815.0, 821.0] @@ -574,9 +666,9 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 16 + $ObjectID 27 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "4661" Extents [1815.0, 821.0] @@ -586,45 +678,65 @@ Model { } PropName "EditorsInfo" } - Object { - $PropName "DockComponentsInfo" - $ObjectID 17 - $ClassName "Simulink.DockComponentInfo" - Type "GLUE2:PropertyInspector" - ID "Property Inspector" - Visible [0] - CreateCallback "" - UserData "" - Floating [0] - DockPosition "Right" - Width [640] - Height [480] - Minimized "Unset" + Array { + Type "Simulink.DockComponentInfo" + Dimension 2 + Object { + $ObjectID 28 + 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" + } + Object { + $ObjectID 29 + Type "GLUE2:PropertyInspector" + ID "Property Inspector" + Visible [0] + CreateCallback "" + UserData "" + Floating [0] + DockPosition "Right" + Width [640] + Height [480] + Minimized "Unset" + } + PropName "DockComponentsInfo" } - WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAABPsAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABiAP///wAAAAEAAAAAAAAAAPwCA" - "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAAOQD///8AAAc9AAADcgAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAA" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABrAP////sAAABgAFMAaQBtAHUAb" + "ABpAG4AawA6AEUAZABpAHQAbwByADoAUgBlAGYAZQByAGUAbgBjAGUAZABGAGkAbABlAHMALwBSAGUAZgBlAHIAZQBuAGMAZQBkACAARgBpAGwAZ" + "QBzAAAAAAD/////AAAAjwD///8AAAABAAAAAAAAAAD8AgAAAAH7AAAAVABHAEwAVQBFADIAOgBQAHIAbwBwAGUAcgB0AHkASQBuAHMAcABlAGMAd" + "ABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA////AAAKAAAABOEAAAABAAAAAgAAAAEAAAAC/" + "AAAAAEAAAACAAAAAA==" Array { Type "Cell" Dimension 0 PropName "PersistedApps" } - WindowUuid "" + 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 "gnava" + LastModifiedBy "nunoguedelha" ModifiedDateFormat "%" - LastModifiedDate "Fri Aug 07 14:43:02 2020" - RTWModifiedTimeStamp 518712182 - ModelVersionFormat "1.%" + LastModifiedDate "Tue Apr 27 22:15:53 2021" + RTWModifiedTimeStamp 541462537 + ModelVersionFormat "%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -644,7 +756,6 @@ Model { ShowTestPointIcons on ShowSignalResolutionIcons on ShowViewerIcons on - SortedOrder on VariantCondition off ShowLinearizationAnnotations on ShowVisualizeInsertedRTB on @@ -682,15 +793,15 @@ Model { TryForcingSFcnDF off Object { $PropName "DataLoggingOverride" - $ObjectID 18 + $ObjectID 30 $ClassName "Simulink.SimulationData.ModelLoggingInfo" - model_ "torqueControlBalancing" + model_ "torqueControlBalancingWithSimu" signals_ [] overrideMode_ [0.0] Array { Type "Cell" Dimension 1 - Cell "torqueControlBalancing" + Cell "torqueControlBalancingWithSimu" PropName "logAsSpecifiedByModels_" } Array { @@ -706,6 +817,7 @@ Model { ExtModeTrigMode "normal" ExtModeTrigPort "1" ExtModeTrigElement "any" + ExtModeTrigSignalOutputPortIndex 0 ExtModeTrigDuration 1000 ExtModeTrigDurationFloating "auto" ExtModeTrigHoldOff 0 @@ -721,6 +833,7 @@ Model { ExtModeSkipDownloadWhenConnect off ExtModeLogAll on ExtModeAutoUpdateStatusClock on + VariantFading on ShowModelReferenceBlockVersion off ShowModelReferenceBlockIO off OrderedModelArguments on @@ -728,16 +841,16 @@ Model { Type "Handle" Dimension 1 Simulink.ConfigSet { - $ObjectID 19 - Version "19.1.0" + $ObjectID 31 + Version "20.1.0" DisabledProps [] Description "" Array { Type "Handle" Dimension 9 Simulink.SolverCC { - $ObjectID 20 - Version "19.1.0" + $ObjectID 32 + Version "20.1.0" DisabledProps [] Description "" Components [] @@ -745,7 +858,7 @@ Model { StopTime "Config.SIMULATION_TIME " AbsTol "auto" AutoScaleAbsTol on - FixedStep "Config.tStep" + FixedStep "Config.tStepSim" InitialStep "auto" MaxOrder 5 ZcThreshold "auto" @@ -759,7 +872,6 @@ Model { RelTol "1e-3" EnableMultiTasking on ConcurrentTasks off - Solver "FixedStepDiscrete" SolverName "FixedStepDiscrete" SolverJacobianMethodControl "auto" ShapePreserveControl "DisableAll" @@ -776,11 +888,11 @@ Model { SampleTimeProperty [] DecoupledContinuousIntegration off MinimalZcImpactIntegration off - SolverOrder 3 + ODENIntegrationMethod "ode3" } Simulink.DataIOCC { - $ObjectID 21 - Version "19.1.0" + $ObjectID 33 + Version "20.1.0" DisabledProps [] Description "" Components [] @@ -821,8 +933,8 @@ Model { LoggingIntervals "[-inf, inf]" } Simulink.OptimizationCC { - $ObjectID 22 - Version "19.1.0" + $ObjectID 34 + Version "20.1.0" Array { Type "Cell" Dimension 9 @@ -859,6 +971,7 @@ Model { ExpressionFolding on BooleansAsBitfields off BitfieldContainerType "uint_T" + BitwiseOrLogicalOp "Same as modeled" EnableMemcpy on MemcpyThreshold 64 PassReuseOutputArgsAs "Structure reference" @@ -870,7 +983,7 @@ Model { DataBitsets off ActiveStateOutputEnumStorageType "Native Integer" ZeroExternalMemoryAtStartup off - ZeroInternalMemoryAtStartup on + ZeroInternalMemoryAtStartup off InitFltsAndDblsToZero off NoFixptDivByZeroProtection off EfficientFloat2IntCast off @@ -894,8 +1007,8 @@ Model { EfficientTunableParamExpr off } Simulink.DebuggingCC { - $ObjectID 23 - Version "19.1.0" + $ObjectID 35 + Version "20.1.0" Array { Type "Cell" Dimension 1 @@ -972,6 +1085,7 @@ Model { ModelReferenceIOMismatchMessage "none" UnknownTsInhSupMsg "warning" ModelReferenceDataLoggingMessage "warning" + ModelReferenceNoExplicitFinalValueMsg "none" ModelReferenceSymbolNameMessage "warning" ModelReferenceExtraNoncontSigs "error" StateNameClashWarn "warning" @@ -1013,10 +1127,11 @@ Model { UnderSpecifiedDimensionMsg "none" DebugExecutionForFMUViaOutOfProcess off ArithmeticOperatorsInVariantConditions "warning" + VariantConditionMismatch "none" } Simulink.HardwareCC { - $ObjectID 24 - Version "19.1.0" + $ObjectID 36 + Version "20.1.0" DisabledProps [] Description "" Components [] @@ -1065,8 +1180,8 @@ Model { HardwareBoardFeatureSet "EmbeddedCoderHSP" } Simulink.ModelReferenceCC { - $ObjectID 25 - Version "19.1.0" + $ObjectID 37 + Version "20.1.0" DisabledProps [] Description "" Components [] @@ -1085,8 +1200,8 @@ Model { SupportModelReferenceSimTargetCustomCode off } Simulink.SFSimCC { - $ObjectID 26 - Version "19.1.0" + $ObjectID 38 + Version "20.1.0" DisabledProps [] Description "" Components [] @@ -1107,7 +1222,6 @@ Model { SimUseLocalCustomCode off SimParseCustomCode off SimAnalyzeCustomCode off - SimBuildMode "sf_incremental_build" SimGenImportedTypeDefs off ModelFunctionsGlobalVisibility "on" CompileTimeRecursionLimit 50 @@ -1117,11 +1231,21 @@ Model { 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 27 - Version "19.1.0" + $ObjectID 39 + Version "20.1.0" Array { Type "Cell" Dimension 16 @@ -1190,6 +1314,7 @@ Model { CodeProfilingInstrumentation "off" SILDebugging off TargetLang "C++" + GenerateGPUCode "None" IncludeBusHierarchyInRTWFileBlockHierarchyMap off GenerateTraceInfo off GenerateTraceReport off @@ -1205,12 +1330,28 @@ Model { 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 28 - Version "19.1.0" + $ObjectID 40 + Version "20.1.0" Array { Type "Cell" Dimension 28 @@ -1299,8 +1440,8 @@ Model { } Simulink.GRTTargetCC { $BackupClass "Simulink.TargetCC" - $ObjectID 29 - Version "19.1.0" + $ObjectID 41 + Version "20.1.0" Array { Type "Cell" Dimension 18 @@ -1329,8 +1470,8 @@ Model { Type "Handle" Dimension 1 Simulink.CPPComponent { - $ObjectID 30 - Version "20.0.1" + $ObjectID 42 + Version "20.1.0" Array { Type "Cell" Dimension 10 @@ -1350,12 +1491,14 @@ Model { Components [] Name "CPPClassGenComp" GenerateDestructor on + ExternalIOMemberVisibility "private" GenerateExternalIOAccessMethods "None" ParameterMemberVisibility "private" InternalMemberVisibility "private" GenerateParameterAccessMethods "None" GenerateInternalMemberAccessMethods "None" UseOperatorNewForModelRefRegistration off + IncludeModelTypesInModelClass on } PropName "Components" } @@ -1388,7 +1531,6 @@ Model { SupportAbsoluteTime on LogVarNameModifier "rt_" MatFileLogging off - MultiInstanceERTCode on CodeInterfacePackaging "C++ class" PurelyIntegerCode off SupportNonFinite off @@ -1439,8 +1581,8 @@ Model { } } SlCovCC.ConfigComp { - $ObjectID 31 - Version "19.1.0" + $ObjectID 43 + Version "20.1.0" DisabledProps [] Description "Simulink Coverage Configuration Component" Components [] @@ -1456,7 +1598,6 @@ Model { CovFilter "" CovHTMLOptions "" CovNameIncrementing off - CovHtmlReporting on CovForceBlockReductionOff on CovEnableCumulative on CovSaveCumulativeToWorkspaceVar on @@ -1466,7 +1607,6 @@ Model { CovSaveOutputData on CovOutputDir "slcov_output/$ModelName$" CovDataFileName "$ModelName$_cvdata" - CovShowResultsExplorer on CovReportOnPause on CovModelRefEnable "off" CovModelRefExcluded "" @@ -1482,19 +1622,19 @@ Model { PropName "Components" } Name "Configuration" - CurrentDlgPage "Code Generation" - ConfigPrmDlgPosition [ 328, 140, 1850, 1010 ] + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 200, 140, 1722, 1010 ] ExtraOptions "-aBlockCommentType=\"BlockPathComment\" -aIgnoreTestpoints=0 " } PropName "ConfigurationSets" } Simulink.ConfigSet { $PropName "ActiveConfigurationSet" - $ObjectID 19 + $ObjectID 31 } Object { $PropName "DataTransfer" - $ObjectID 32 + $ObjectID 44 $ClassName "Simulink.GlobalDataTransfer" DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" @@ -1506,7 +1646,7 @@ Model { ForegroundColor "black" BackgroundColor "white" DropShadow off - NamePlacement "normal" + NameLocation "bottom" FontName "Helvetica" FontSize 10 FontWeight "normal" @@ -1565,6 +1705,16 @@ Model { StopWhenAssertionFail on SampleTime "-1" } + Block { + BlockType BusCreator + DisplayOption "none" + OutDataTypeStr "Inherit: auto" + NonVirtualBus off + } + Block { + BlockType BusSelector + OutputAsBus off + } Block { BlockType Clock DisplayTime off @@ -1759,12 +1909,14 @@ Model { 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" @@ -1772,13 +1924,14 @@ Model { MaskHideContents off SFBlockType "NONE" Variant off - VariantControlMode "Expression" - GeneratePreprocessorConditionals off + VariantControlMode "expression" + VariantActivationTime "update diagram" AllowZeroVariantControls off PropagateVariantConditions off TreatAsGroupedWhenPropagatingVariantConditions on ContentPreviewEnabled off IsWebBlock off + IsInjectorSS off Latency "0" AutoFrameSizeCalculation off IsWebBlockPanel off @@ -1834,14 +1987,14 @@ Model { InputProcessing "Elements as channels (sample based)" SampleTime "1" StateMustResolveToSignalObject off - CodeGenStateStorageClass "Auto" HasFrameUpgradeWarning off } } System { - Name "torqueControlBalancing" - Location [67, 27, 1920, 1080] - Open off + Name "torqueControlBalancingWithSimu" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open on PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" @@ -1855,19 +2008,88 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "361" + ZoomFactor "239" ReportName "simulink-default.rpt" - SIDHighWatermark "4936" + SIDHighWatermark "4968" 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\nSelector" + SID "4943" + Ports [1, 4] + Position [1350, 254, 1355, 366] + 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 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 Reference Name "Configuration" SID "4537" Ports [] - Position [1180, 535, 1245, 555] + Position [955, 325, 1020, 345] ZOrder 553 HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/Configuration" SourceType "" SourceProductName "WholeBodyToolbox" @@ -1879,19 +2101,13 @@ Model { ContentPreviewEnabled off ConfigSource "Workspace" ConfigObject "'WBTConfigRobot'" - RobotName "'icubSim'" - UrdfFile "'model.urdf'" - ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" - ControlBoardsNames "{'left_arm','right_arm','torso'}" - LocalName "'WBT'" - GravityVector "[0,0,-9.81]" } Block { BlockType SubSystem Name "Dump and visualize" SID "2275" Ports [] - Position [1155, 381, 1275, 409] + Position [955, 266, 1075, 294] ZOrder 547 ForegroundColor "blue" BackgroundColor "[0.333333, 1.000000, 1.000000]" @@ -1899,13 +2115,14 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 33 + $ObjectID 45 $ClassName "Simulink.Mask" Display "disp('VISUALIZER')" } System { Name "Dump and visualize" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -1920,7 +2137,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "415" + ZoomFactor "267" SimulinkSubDomain "Simulink" Block { BlockType SubSystem @@ -1932,7 +2149,8 @@ Model { RequestExecContextInheritance off System { Name "Desired and Measured Forces" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -2062,7 +2280,8 @@ Model { } System { Name "Demux Forces nd Moments" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -2077,7 +2296,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "466" + ZoomFactor "300" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -3022,6 +3241,7 @@ Model { System { Name "Feet Status and Gains" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -3523,6 +3743,7 @@ Model { System { Name "Visualize eventual QP failures" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -3640,6 +3861,7 @@ Model { System { Name "Visualizer" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -4474,7 +4696,7 @@ Model { BackgroundColor "[0.827500, 0.576500, 0.603900]" ShowName off HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/States/GetMeasurement" SourceType "GetMeasurement" SourceProductName "WholeBodyToolbox" @@ -5994,114 +6216,21 @@ Model { } } } - Block { - BlockType Reference - Name "GetMeasurement" - SID "4539" - Ports [0, 1] - Position [780, 388, 865, 402] - ZOrder 905 - BackgroundColor "[0.925500, 0.870600, 0.133300]" - ShowName off - HideAutomaticName off - LibraryVersion "1.675" - SourceBlock "WBToolboxLibrary/States/GetMeasurement" - SourceType "GetMeasurement" - SourceProductName "WholeBodyToolbox" - RTWMemSecFuncInitTerm "Inherit from model" - RTWMemSecFuncExecute "Inherit from model" - RTWMemSecDataConstants "Inherit from model" - RTWMemSecDataInternal "Inherit from model" - RTWMemSecDataParameters "Inherit from model" - ContentPreviewEnabled off - measuredType "Joints Acceleration" - } - Block { - BlockType Reference - Name "GetMeasurement1" - SID "4538" - Ports [0, 1] - Position [785, 432, 860, 448] - ZOrder 958 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - ShowName off - HideAutomaticName off - LibraryVersion "1.675" - SourceBlock "WBToolboxLibrary/States/GetMeasurement" - SourceType "GetMeasurement" - SourceProductName "WholeBodyToolbox" - RTWMemSecFuncInitTerm "Inherit from model" - RTWMemSecFuncExecute "Inherit from model" - RTWMemSecDataConstants "Inherit from model" - RTWMemSecDataInternal "Inherit from model" - RTWMemSecDataParameters "Inherit from model" - ContentPreviewEnabled off - measuredType "Joints Position" - } - Block { - BlockType Reference - Name "GetMeasurement2" - SID "4541" - Ports [0, 1] - Position [785, 462, 860, 478] - ZOrder 960 - BackgroundColor "[0.513700, 0.674500, 1.000000]" - ShowName off - HideAutomaticName off - LibraryVersion "1.675" - SourceBlock "WBToolboxLibrary/States/GetMeasurement" - SourceType "GetMeasurement" - SourceProductName "WholeBodyToolbox" - RTWMemSecFuncInitTerm "Inherit from model" - RTWMemSecFuncExecute "Inherit from model" - RTWMemSecDataConstants "Inherit from model" - RTWMemSecDataInternal "Inherit from model" - RTWMemSecDataParameters "Inherit from model" - ContentPreviewEnabled off - measuredType "Joints Velocity" - } - Block { - BlockType Goto - Name "GoTo Motors Inertia" - SID "4831" - Position [955, 387, 1040, 403] - ZOrder 906 - HideAutomaticName off - GotoTag "jointAcc" - TagVisibility "global" - } - Block { - BlockType Reference - Name "IMU_meas" - SID "2630" - Ports [0, 1] - Position [800, 494, 840, 506] - ZOrder 959 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.675" - SourceBlock "WBToolboxLibrary/Utilities/YarpRead" - SourceType "YARP Read" - SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" - portName "Ports.IMU" - signalSize "Ports.IMU_PORT_SIZE" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on - } Block { BlockType SubSystem Name "MOMENTUM BASED TORQUE CONTROL" SID "4836" - Ports [5, 1] - Position [955, 427, 1105, 573] + Ports [6, 1] + Position [955, 409, 1105, 591] ZOrder 961 + ForegroundColor "red" + BackgroundColor "yellow" + SystemSampleTime "Config.tStep" RequestExecContextInheritance off System { Name "MOMENTUM BASED TORQUE CONTROL" - Location [67, 27, 1920, 1080] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -6116,7 +6245,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "123" + ZoomFactor "182" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -6133,13 +6262,21 @@ Model { ZOrder 557 Port "2" } + Block { + BlockType Inport + Name "jointAcc" + SID "4966" + Position [160, 178, 190, 192] + ZOrder 969 + Port "3" + } Block { BlockType Inport Name "IMU_meas" SID "4840" Position [160, 478, 190, 492] ZOrder 559 - Port "3" + Port "4" } Block { BlockType Inport @@ -6147,7 +6284,7 @@ Model { SID "4880" Position [160, 538, 190, 552] ZOrder 963 - Port "4" + Port "5" } Block { BlockType Inport @@ -6155,7 +6292,7 @@ Model { SID "4881" Position [160, 598, 190, 612] ZOrder 964 - Port "5" + Port "6" } Block { BlockType SubSystem @@ -6168,7 +6305,8 @@ Model { RequestExecContextInheritance off System { Name "Balancing Controller QP" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -6183,7 +6321,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "164" + ZoomFactor "107" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -6355,6 +6493,7 @@ Model { System { Name "Compute Desired Torques" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -6525,6 +6664,7 @@ Model { System { Name "QPSolver" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -6632,6 +6772,7 @@ Model { ZOrder 723 BlockRotation 270 BlockMirror on + NameLocation "right" HideAutomaticName off Operator "NOT" IconShape "distinctive" @@ -6653,6 +6794,7 @@ Model { 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 @@ -6668,7 +6810,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "105" + SIDHighWatermark "108" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -6680,24 +6822,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4590::104" + SID "4590::107" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 95 + ZOrder 98 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4590::103" + SID "4590::106" Tag "Stateflow S-Function 15" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 94 + ZOrder 97 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -6707,9 +6849,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4590::105" + SID "4590::108" Position [460, 241, 480, 259] - ZOrder 96 + ZOrder 99 } Block { BlockType Outport @@ -6720,7 +6862,7 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 105 + ZOrder 109 SrcBlock "feetContactStatus" SrcPort 1 DstBlock " SFunction " @@ -6728,7 +6870,7 @@ Model { } Line { Name "onOneFoot" - ZOrder 106 + ZOrder 110 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -6736,14 +6878,14 @@ Model { DstPort 1 } Line { - ZOrder 107 + ZOrder 111 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 108 + ZOrder 112 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -6758,12 +6900,13 @@ Model { Ports [5, 2, 1] Position [25, -332, 120, -178] ZOrder 736 - NamePlacement "alternate" + NameLocation "top" TreatAsAtomicUnit on RequestExecContextInheritance off System { Name "QP One Foot" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -6844,6 +6987,7 @@ Model { 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 @@ -6859,7 +7003,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "106" + SIDHighWatermark "109" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -6879,24 +7023,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4690::105" + SID "4690::108" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 96 + ZOrder 99 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4690::104" + SID "4690::107" Tag "Stateflow S-Function 26" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 95 + ZOrder 98 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -6906,9 +7050,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4690::106" + SID "4690::109" Position [460, 241, 480, 259] - ZOrder 97 + ZOrder 100 } Block { BlockType Outport @@ -6919,7 +7063,7 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 81 + ZOrder 86 SrcBlock "H" SrcPort 1 Points [120, 0] @@ -6927,7 +7071,7 @@ Model { DstPort 1 } Line { - ZOrder 82 + ZOrder 87 SrcBlock "g" SrcPort 1 DstBlock " SFunction " @@ -6935,7 +7079,7 @@ Model { } Line { Name "analyticalSolution" - ZOrder 83 + ZOrder 88 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -6943,14 +7087,14 @@ Model { DstPort 1 } Line { - ZOrder 84 + ZOrder 89 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 85 + ZOrder 90 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -6967,7 +7111,7 @@ Model { ZOrder 757 ShowName off HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/MatchSignalSizes" SourceType "MatchSignalSizes" SourceProductName "WholeBodyToolbox" @@ -6993,6 +7137,7 @@ Model { 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 @@ -7008,7 +7153,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "114" + SIDHighWatermark "117" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -7044,25 +7189,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4692::113" + SID "4692::116" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 98 + ZOrder 101 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4692::112" + SID "4692::115" Tag "Stateflow S-Function 27" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 97 + ZOrder 100 FunctionName "sf_sfun" Parameters "Config" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -7072,9 +7217,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4692::114" + SID "4692::117" Position [460, 241, 480, 259] - ZOrder 99 + ZOrder 102 } Block { BlockType Outport @@ -7085,28 +7230,28 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 119 + ZOrder 126 SrcBlock "analyticalSolution" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 120 + ZOrder 127 SrcBlock "primalSolution" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 121 + ZOrder 128 SrcBlock "QPStatus" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 122 + ZOrder 129 SrcBlock "feetContactStatus" SrcPort 1 DstBlock " SFunction " @@ -7114,7 +7259,7 @@ Model { } Line { Name "f_star" - ZOrder 123 + ZOrder 130 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -7122,14 +7267,14 @@ Model { DstPort 1 } Line { - ZOrder 124 + ZOrder 131 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 125 + ZOrder 132 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -7144,7 +7289,7 @@ Model { Ports [4, 2] Position [1145, -242, 1250, -128] ZOrder 751 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/QP" SourceType "QP" SourceProductName "WholeBodyToolbox" @@ -7296,6 +7441,7 @@ Model { System { Name "QP Two Feet" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -7368,6 +7514,7 @@ Model { 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 @@ -7383,7 +7530,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "106" + SIDHighWatermark "109" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -7403,24 +7550,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4678::105" + SID "4678::108" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 96 + ZOrder 99 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4678::104" + SID "4678::107" Tag "Stateflow S-Function 24" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 95 + ZOrder 98 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -7430,9 +7577,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4678::106" + SID "4678::109" Position [460, 241, 480, 259] - ZOrder 97 + ZOrder 100 } Block { BlockType Outport @@ -7443,7 +7590,7 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 81 + ZOrder 86 SrcBlock "H" SrcPort 1 Points [120, 0] @@ -7451,7 +7598,7 @@ Model { DstPort 1 } Line { - ZOrder 82 + ZOrder 87 SrcBlock "g" SrcPort 1 DstBlock " SFunction " @@ -7459,7 +7606,7 @@ Model { } Line { Name "analyticalSolution" - ZOrder 83 + ZOrder 88 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -7467,14 +7614,14 @@ Model { DstPort 1 } Line { - ZOrder 84 + ZOrder 89 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 85 + ZOrder 90 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -7491,7 +7638,7 @@ Model { ZOrder 757 ShowName off HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/MatchSignalSizes" SourceType "MatchSignalSizes" SourceProductName "WholeBodyToolbox" @@ -7517,6 +7664,7 @@ Model { 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 @@ -7532,7 +7680,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "114" + SIDHighWatermark "117" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -7560,25 +7708,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4680::113" + SID "4680::116" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 97 + ZOrder 100 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4680::112" + SID "4680::115" Tag "Stateflow S-Function 25" Ports [3, 2] Position [180, 100, 230, 180] - ZOrder 96 + ZOrder 99 FunctionName "sf_sfun" Parameters "Config" PortCounts "[3 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -7588,9 +7736,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4680::114" + SID "4680::117" Position [460, 241, 480, 259] - ZOrder 98 + ZOrder 101 } Block { BlockType Outport @@ -7601,21 +7749,21 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 116 + ZOrder 122 SrcBlock "analyticalSolution" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 117 + ZOrder 123 SrcBlock "primalSolution" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 118 + ZOrder 124 SrcBlock "QPStatus" SrcPort 1 DstBlock " SFunction " @@ -7623,7 +7771,7 @@ Model { } Line { Name "f_star" - ZOrder 119 + ZOrder 125 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -7631,14 +7779,14 @@ Model { DstPort 1 } Line { - ZOrder 120 + ZOrder 126 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 121 + ZOrder 127 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -7654,7 +7802,7 @@ Model { Ports [4, 2] Position [1575, 1016, 1680, 1134] ZOrder 752 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/QP" SourceType "QP" SourceProductName "WholeBodyToolbox" @@ -7981,7 +8129,7 @@ Model { SID "4712" Position [910, 482, 980, 698] ZOrder 653 - NamePlacement "alternate" + NameLocation "top" Criteria "u2 > Threshold" InputSameDT off SaturateOnIntegerOverflow off @@ -8174,6 +8322,7 @@ Model { System { Name "Compute angular momentum integral error" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -8241,7 +8390,7 @@ Model { Position [1175, -194, 1325, 259] ZOrder 73 HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" SourceType "CentroidalMomentum" SourceProductName "WholeBodyToolbox" @@ -8263,6 +8412,7 @@ Model { System { Name "Compute base to fixed link transform" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -8326,7 +8476,7 @@ Model { Ports [2, 1] Position [155, -51, 320, 16] ZOrder 894 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" @@ -8345,7 +8495,7 @@ Model { Ports [2, 1] Position [155, 64, 325, 131] ZOrder 899 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" @@ -8454,6 +8604,7 @@ Model { 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 @@ -8469,7 +8620,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3822" + SIDHighWatermark "3825" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -8505,25 +8656,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "3721::3821" + SID "3721::3824" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 146 + ZOrder 149 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "3721::3820" + SID "3721::3823" Tag "Stateflow S-Function 20" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 145 + ZOrder 148 FunctionName "sf_sfun" Parameters "Reg" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -8533,9 +8684,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "3721::3822" + SID "3721::3825" Position [460, 241, 480, 259] - ZOrder 147 + ZOrder 150 } Block { BlockType Outport @@ -8546,28 +8697,28 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 260 + ZOrder 267 SrcBlock "J_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 261 + ZOrder 268 SrcBlock "J_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 262 + ZOrder 269 SrcBlock "feetContactStatus" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 263 + ZOrder 270 SrcBlock "jointPos_err" SrcPort 1 DstBlock " SFunction " @@ -8575,7 +8726,7 @@ Model { } Line { Name "baseVel_equivalent" - ZOrder 264 + ZOrder 271 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -8583,14 +8734,14 @@ Model { DstPort 1 } Line { - ZOrder 265 + ZOrder 272 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 266 + ZOrder 273 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -8607,7 +8758,7 @@ Model { ZOrder 40 ShowName off HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" SourceType "Jacobian" SourceProductName "WholeBodyToolbox" @@ -8628,7 +8779,7 @@ Model { ZOrder 79 ShowName off HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" SourceType "Jacobian" SourceProductName "WholeBodyToolbox" @@ -8655,6 +8806,7 @@ Model { 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 @@ -8670,7 +8822,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "135" + SIDHighWatermark "138" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -8682,24 +8834,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4027::134" + SID "4027::137" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 125 + ZOrder 128 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4027::133" + SID "4027::136" Tag "Stateflow S-Function 19" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 124 + ZOrder 127 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -8709,9 +8861,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4027::135" + SID "4027::138" Position [460, 241, 480, 259] - ZOrder 126 + ZOrder 129 } Block { BlockType Outport @@ -8722,7 +8874,7 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 149 + ZOrder 153 SrcBlock "state" SrcPort 1 DstBlock " SFunction " @@ -8730,7 +8882,7 @@ Model { } Line { Name "booleanState" - ZOrder 150 + ZOrder 154 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -8738,14 +8890,14 @@ Model { DstPort 1 } Line { - ZOrder 151 + ZOrder 155 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 152 + ZOrder 156 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -8981,7 +9133,8 @@ Model { RequestExecContextInheritance off System { Name "From tau_QP to Joint Torques (motor reflected inertia)" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -9030,7 +9183,7 @@ Model { Ports [0, 1] Position [-500, -219, -340, -181] ZOrder 871 - NamePlacement "alternate" + NameLocation "top" ErrorFcn "Stateflow.Translate.translate" PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on @@ -9039,6 +9192,7 @@ Model { 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 @@ -9054,37 +9208,37 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "154" + SIDHighWatermark "158" SimulinkSubDomain "Simulink" Block { BlockType Demux Name " Demux " - SID "4551::152" + SID "4551::156" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 143 + ZOrder 147 Outputs "1" } Block { BlockType Ground Name " Ground " - SID "4551::154" + SID "4551::158" Position [20, 121, 40, 139] - ZOrder 145 + ZOrder 149 } Block { BlockType S-Function Name " SFunction " - SID "4551::151" + SID "4551::155" Tag "Stateflow S-Function 9" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 142 + ZOrder 146 FunctionName "sf_sfun" Parameters "Config" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -9094,9 +9248,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4551::153" + SID "4551::157" Position [460, 241, 480, 259] - ZOrder 144 + ZOrder 148 } Block { BlockType Outport @@ -9108,7 +9262,7 @@ Model { } Line { Name "reflectedInertia" - ZOrder 133 + ZOrder 137 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -9116,21 +9270,21 @@ Model { DstPort 1 } Line { - ZOrder 134 + ZOrder 138 SrcBlock " Ground " SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 135 + ZOrder 139 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 136 + ZOrder 140 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -9352,6 +9506,7 @@ Model { 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 @@ -9367,7 +9522,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3821" + SIDHighWatermark "3824" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -9547,25 +9702,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "2407::3820" + SID "2407::3823" Ports [1, 1] Position [270, 570, 320, 610] - ZOrder 223 + ZOrder 226 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2407::3819" + SID "2407::3822" Tag "Stateflow S-Function 17" Ports [22, 13] Position [180, 70, 230, 530] - ZOrder 222 + ZOrder 225 FunctionName "sf_sfun" Parameters "Config,Gain,Reg" PortCounts "[22 13]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -9619,9 +9774,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2407::3821" + SID "2407::3824" Position [460, 581, 480, 599] - ZOrder 224 + ZOrder 227 } Block { BlockType Outport @@ -9731,154 +9886,154 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 2546 + ZOrder 2582 SrcBlock "feetContactStatus" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 2547 + ZOrder 2583 SrcBlock "ConstraintsMatrix" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 2548 + ZOrder 2584 SrcBlock "bVectorConstraints" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 2549 + ZOrder 2585 SrcBlock "jointPos" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 2550 + ZOrder 2586 SrcBlock "jointPos_des" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 2551 + ZOrder 2587 SrcBlock "nu" SrcPort 1 DstBlock " SFunction " DstPort 6 } Line { - ZOrder 2552 + ZOrder 2588 SrcBlock "M" SrcPort 1 DstBlock " SFunction " DstPort 7 } Line { - ZOrder 2553 + ZOrder 2589 SrcBlock "h" SrcPort 1 DstBlock " SFunction " DstPort 8 } Line { - ZOrder 2554 + ZOrder 2590 SrcBlock "L" SrcPort 1 DstBlock " SFunction " DstPort 9 } Line { - ZOrder 2555 + ZOrder 2591 SrcBlock "intL_angMomError" SrcPort 1 DstBlock " SFunction " DstPort 10 } Line { - ZOrder 2556 + ZOrder 2592 SrcBlock "w_H_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 11 } Line { - ZOrder 2557 + ZOrder 2593 SrcBlock "w_H_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 12 } Line { - ZOrder 2558 + ZOrder 2594 SrcBlock "J_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 13 } Line { - ZOrder 2559 + ZOrder 2595 SrcBlock "J_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 14 } Line { - ZOrder 2560 + ZOrder 2596 SrcBlock "JDot_l_sole_nu" SrcPort 1 DstBlock " SFunction " DstPort 15 } Line { - ZOrder 2561 + ZOrder 2597 SrcBlock "JDot_r_sole_nu" SrcPort 1 DstBlock " SFunction " DstPort 16 } Line { - ZOrder 2562 + ZOrder 2598 SrcBlock "pos_CoM" SrcPort 1 DstBlock " SFunction " DstPort 17 } Line { - ZOrder 2563 + ZOrder 2599 SrcBlock "J_CoM" SrcPort 1 DstBlock " SFunction " DstPort 18 } Line { - ZOrder 2564 + ZOrder 2600 SrcBlock "desired_pos_vel_acc_CoM" SrcPort 1 DstBlock " SFunction " DstPort 19 } Line { - ZOrder 2565 + ZOrder 2601 SrcBlock "KP_CoM" SrcPort 1 DstBlock " SFunction " DstPort 20 } Line { - ZOrder 2566 + ZOrder 2602 SrcBlock "KD_CoM" SrcPort 1 DstBlock " SFunction " DstPort 21 } Line { - ZOrder 2567 + ZOrder 2603 SrcBlock "KP_postural" SrcPort 1 DstBlock " SFunction " @@ -9886,7 +10041,7 @@ Model { } Line { Name "HessianMatrixOneFoot" - ZOrder 2568 + ZOrder 2604 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -9895,7 +10050,7 @@ Model { } Line { Name "gradientOneFoot" - ZOrder 2569 + ZOrder 2605 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -9904,7 +10059,7 @@ Model { } Line { Name "ConstraintsMatrixOneFoot" - ZOrder 2570 + ZOrder 2606 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -9913,7 +10068,7 @@ Model { } Line { Name "bVectorConstraintsOneFoot" - ZOrder 2571 + ZOrder 2607 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -9922,7 +10077,7 @@ Model { } Line { Name "HessianMatrixTwoFeet" - ZOrder 2572 + ZOrder 2608 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -9931,7 +10086,7 @@ Model { } Line { Name "gradientTwoFeet" - ZOrder 2573 + ZOrder 2609 Labels [0, 0] SrcBlock " SFunction " SrcPort 7 @@ -9940,7 +10095,7 @@ Model { } Line { Name "ConstraintsMatrixTwoFeet" - ZOrder 2574 + ZOrder 2610 Labels [0, 0] SrcBlock " SFunction " SrcPort 8 @@ -9949,7 +10104,7 @@ Model { } Line { Name "bVectorConstraintsTwoFeet" - ZOrder 2575 + ZOrder 2611 Labels [0, 0] SrcBlock " SFunction " SrcPort 9 @@ -9958,7 +10113,7 @@ Model { } Line { Name "tauModel" - ZOrder 2576 + ZOrder 2612 Labels [0, 0] SrcBlock " SFunction " SrcPort 10 @@ -9967,7 +10122,7 @@ Model { } Line { Name "Sigma" - ZOrder 2577 + ZOrder 2613 Labels [0, 0] SrcBlock " SFunction " SrcPort 11 @@ -9976,7 +10131,7 @@ Model { } Line { Name "Na" - ZOrder 2578 + ZOrder 2614 Labels [0, 0] SrcBlock " SFunction " SrcPort 12 @@ -9985,7 +10140,7 @@ Model { } Line { Name "f_LDot" - ZOrder 2579 + ZOrder 2615 Labels [0, 0] SrcBlock " SFunction " SrcPort 13 @@ -9993,14 +10148,14 @@ Model { DstPort 1 } Line { - ZOrder 2580 + ZOrder 2616 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 2581 + ZOrder 2617 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -10342,11 +10497,12 @@ Model { Position [605, 23, 735, 347] ZOrder 541 BackgroundColor "[0.000000, 1.000000, 0.498039]" - NamePlacement "alternate" + NameLocation "top" RequestExecContextInheritance off System { Name "Dynamics and Kinematics" - Location [67, 27, 1920, 1080] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -10397,6 +10553,7 @@ Model { System { Name "Dynamics" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -10447,6 +10604,7 @@ Model { 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 @@ -10485,6 +10643,7 @@ Model { 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 @@ -10500,7 +10659,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "121" + SIDHighWatermark "124" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -10512,25 +10671,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4518::120" + SID "4518::123" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 110 + ZOrder 113 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4518::119" + SID "4518::122" Tag "Stateflow S-Function 6" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 109 + ZOrder 112 FunctionName "sf_sfun" Parameters "Config" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -10540,9 +10699,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4518::121" + SID "4518::124" Position [460, 241, 480, 259] - ZOrder 111 + ZOrder 114 } Block { BlockType Outport @@ -10553,7 +10712,7 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 146 + ZOrder 150 SrcBlock "M" SrcPort 1 DstBlock " SFunction " @@ -10561,7 +10720,7 @@ Model { } Line { Name "M_with_inertia" - ZOrder 147 + ZOrder 151 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -10569,14 +10728,14 @@ Model { DstPort 1 } Line { - ZOrder 148 + ZOrder 152 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 149 + ZOrder 153 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -10660,7 +10819,7 @@ Model { Position [395, 281, 575, 344] ZOrder 223 HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" SourceType "CentroidalMomentum" SourceProductName "WholeBodyToolbox" @@ -10705,7 +10864,7 @@ Model { Position [395, 132, 535, 233] ZOrder 222 HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Dynamics/GetBiasForces" SourceType "Get Generalized Bias Forces" SourceProductName "WholeBodyToolbox" @@ -10724,7 +10883,7 @@ Model { Position [395, 19, 535, 86] ZOrder 221 HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Dynamics/MassMatrix" SourceType "MassMatrix" SourceProductName "WholeBodyToolbox" @@ -10904,7 +11063,8 @@ Model { RequestExecContextInheritance off System { Name "Kinematics" - Location [67, 27, 1920, 1080] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -10953,7 +11113,7 @@ Model { ZOrder 882 ShowName off HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" @@ -11014,7 +11174,7 @@ Model { ZOrder 829 ShowName off HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Jacobians/DotJNu" SourceType "DotJNu" SourceProductName "WholeBodyToolbox" @@ -11035,7 +11195,7 @@ Model { ZOrder 831 ShowName off HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Jacobians/DotJNu" SourceType "DotJNu" SourceProductName "WholeBodyToolbox" @@ -11065,7 +11225,7 @@ Model { Position [60, 260, 225, 300] ZOrder 835 ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" SourceType "Jacobian" SourceProductName "WholeBodyToolbox" @@ -11085,7 +11245,7 @@ Model { Position [60, 160, 225, 195] ZOrder 828 ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" SourceType "Jacobian" SourceProductName "WholeBodyToolbox" @@ -11105,7 +11265,7 @@ Model { Position [60, 210, 225, 245] ZOrder 830 ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" SourceType "Jacobian" SourceProductName "WholeBodyToolbox" @@ -11125,7 +11285,7 @@ Model { Position [60, 35, 220, 75] ZOrder 826 ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" @@ -11145,7 +11305,7 @@ Model { Position [60, 95, 220, 135] ZOrder 827 ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" @@ -11721,6 +11881,16 @@ Model { } } } + Block { + BlockType Goto + Name "GoTo Motors Inertia" + SID "4967" + Position [305, 177, 390, 193] + ZOrder 970 + HideAutomaticName off + GotoTag "jointAcc" + TagVisibility "global" + } Block { BlockType SubSystem Name "Joint Torque Saturation" @@ -11733,6 +11903,7 @@ Model { System { Name "Joint Torque Saturation" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -11803,7 +11974,7 @@ Model { Ports [4, 1] Position [60, -12, 265, 72] ZOrder 1 - NamePlacement "alternate" + NameLocation "top" ErrorFcn "Stateflow.Translate.translate" PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on @@ -11812,6 +11983,7 @@ Model { 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 @@ -11827,7 +11999,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "116" + SIDHighWatermark "119" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -11863,24 +12035,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4556::115" + SID "4556::118" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 104 + ZOrder 107 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4556::114" + SID "4556::117" Tag "Stateflow S-Function 11" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 103 + ZOrder 106 FunctionName "sf_sfun" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -11890,9 +12062,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4556::116" + SID "4556::119" Position [460, 241, 480, 259] - ZOrder 105 + ZOrder 108 } Block { BlockType Outport @@ -11903,28 +12075,28 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 224 + ZOrder 231 SrcBlock "u" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 225 + ZOrder 232 SrcBlock "u_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 226 + ZOrder 233 SrcBlock "tStep" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 227 + ZOrder 234 SrcBlock "uDotMax" SrcPort 1 DstBlock " SFunction " @@ -11932,7 +12104,7 @@ Model { } Line { Name "uSat" - ZOrder 228 + ZOrder 235 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -11940,14 +12112,14 @@ Model { DstPort 1 } Line { - ZOrder 229 + ZOrder 236 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 230 + ZOrder 237 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -11987,7 +12159,7 @@ Model { BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off HideAutomaticName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" SourceProductName "WholeBodyToolbox" @@ -12105,7 +12277,8 @@ Model { RequestExecContextInheritance off System { Name "Robot State and References" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -12120,7 +12293,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "357" + ZoomFactor "244" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -12172,6 +12345,7 @@ Model { System { Name "Compute State Velocity" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -12194,7 +12368,7 @@ Model { SID "4212" Position [-10, -187, 20, -173] ZOrder 562 - NamePlacement "alternate" + NameLocation "top" } Block { BlockType Inport @@ -12236,6 +12410,7 @@ Model { 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 @@ -12251,7 +12426,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3845" + SIDHighWatermark "3848" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -12287,25 +12462,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4215::3844" + SID "4215::3847" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 213 + ZOrder 216 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4215::3843" + SID "4215::3846" Tag "Stateflow S-Function 7" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 212 + ZOrder 215 FunctionName "sf_sfun" Parameters "Reg" PortCounts "[4 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -12315,9 +12490,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4215::3845" + SID "4215::3848" Position [460, 241, 480, 259] - ZOrder 214 + ZOrder 217 } Block { BlockType Outport @@ -12328,28 +12503,28 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 113 + ZOrder 120 SrcBlock "J_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 114 + ZOrder 121 SrcBlock "J_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 115 + ZOrder 122 SrcBlock "feetContactStatus" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 116 + ZOrder 123 SrcBlock "jointVel" SrcPort 1 DstBlock " SFunction " @@ -12357,7 +12532,7 @@ Model { } Line { Name "nu_b" - ZOrder 117 + ZOrder 124 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -12365,14 +12540,14 @@ Model { DstPort 1 } Line { - ZOrder 118 + ZOrder 125 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 119 + ZOrder 126 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -12391,6 +12566,7 @@ Model { System { Name "Feet Jacobians" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -12429,7 +12605,7 @@ Model { Ports [2, 1] Position [745, 475, 905, 530] ZOrder 687 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" SourceType "Jacobian" SourceProductName "WholeBodyToolbox" @@ -12448,7 +12624,7 @@ Model { Ports [2, 1] Position [745, 565, 905, 620] ZOrder 688 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" SourceType "Jacobian" SourceProductName "WholeBodyToolbox" @@ -12625,6 +12801,7 @@ Model { System { Name "Compute base to fixed link transform" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -12686,6 +12863,7 @@ Model { System { Name "LFoot to base link transform " Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -12740,7 +12918,7 @@ Model { Ports [2, 1] Position [575, 25, 740, 85] ZOrder 729 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" @@ -12759,7 +12937,7 @@ Model { Ports [2, 1] Position [575, 190, 740, 250] ZOrder 737 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" @@ -12788,6 +12966,7 @@ Model { 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 @@ -12803,7 +12982,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3836" + SIDHighWatermark "3839" SIDPrevWatermark "9" SimulinkSubDomain "Simulink" Block { @@ -12856,25 +13035,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4257::3835" + SID "4257::3838" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 235 + ZOrder 238 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4257::3834" + SID "4257::3837" Tag "Stateflow S-Function 8" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 234 + ZOrder 237 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -12884,9 +13063,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4257::3836" + SID "4257::3839" Position [460, 241, 480, 259] - ZOrder 236 + ZOrder 239 } Block { BlockType Outport @@ -12897,42 +13076,42 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 145 + ZOrder 154 SrcBlock "imu_H_fixedLink" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 146 + ZOrder 155 SrcBlock "imu_H_fixedLink_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 147 + ZOrder 156 SrcBlock "fixedLink_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 148 + ZOrder 157 SrcBlock "rpyFromIMU_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 149 + ZOrder 158 SrcBlock "rpyFromIMU" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 150 + ZOrder 159 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -12940,7 +13119,7 @@ Model { } Line { Name "w_H_b" - ZOrder 151 + ZOrder 160 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -12948,14 +13127,14 @@ Model { DstPort 1 } Line { - ZOrder 152 + ZOrder 161 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 153 + ZOrder 162 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -13003,7 +13182,7 @@ Model { Position [1000, 310, 1035, 330] ZOrder 742 ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" @@ -13071,7 +13250,7 @@ Model { ZOrder 739 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" SourceProductName "WholeBodyToolbox" @@ -13091,7 +13270,7 @@ Model { ZOrder 740 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" SourceProductName "WholeBodyToolbox" @@ -13113,6 +13292,7 @@ Model { System { Name "neck transform" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -13135,7 +13315,7 @@ Model { SID "4261" Position [45, 18, 75, 32] ZOrder 725 - NamePlacement "alternate" + NameLocation "top" } Block { BlockType Constant @@ -13452,6 +13632,7 @@ Model { System { Name "RFoot to base link transform" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -13506,7 +13687,7 @@ Model { Ports [2, 1] Position [575, 25, 740, 85] ZOrder 729 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" @@ -13525,7 +13706,7 @@ Model { Ports [2, 1] Position [575, 190, 740, 250] ZOrder 737 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" @@ -13554,6 +13735,7 @@ Model { 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 @@ -13569,7 +13751,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3836" + SIDHighWatermark "3839" SIDPrevWatermark "9" SimulinkSubDomain "Simulink" Block { @@ -13622,25 +13804,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "4857::3835" + SID "4857::3838" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 235 + ZOrder 238 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4857::3834" + SID "4857::3837" Tag "Stateflow S-Function 3" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 234 + ZOrder 237 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -13650,9 +13832,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4857::3836" + SID "4857::3839" Position [460, 241, 480, 259] - ZOrder 236 + ZOrder 239 } Block { BlockType Outport @@ -13663,42 +13845,42 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 145 + ZOrder 154 SrcBlock "imu_H_fixedLink" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 146 + ZOrder 155 SrcBlock "imu_H_fixedLink_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 147 + ZOrder 156 SrcBlock "fixedLink_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 148 + ZOrder 157 SrcBlock "rpyFromIMU_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 149 + ZOrder 158 SrcBlock "rpyFromIMU" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 150 + ZOrder 159 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -13706,7 +13888,7 @@ Model { } Line { Name "w_H_b" - ZOrder 151 + ZOrder 160 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -13714,14 +13896,14 @@ Model { DstPort 1 } Line { - ZOrder 152 + ZOrder 161 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 153 + ZOrder 162 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -13769,7 +13951,7 @@ Model { Position [1000, 310, 1035, 330] ZOrder 742 ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" @@ -13837,7 +14019,7 @@ Model { ZOrder 739 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" SourceProductName "WholeBodyToolbox" @@ -13857,7 +14039,7 @@ Model { ZOrder 740 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" SourceProductName "WholeBodyToolbox" @@ -13879,6 +14061,7 @@ Model { System { Name "neck transform" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -13901,7 +14084,7 @@ Model { SID "4868" Position [45, 18, 75, 32] ZOrder 725 - NamePlacement "alternate" + NameLocation "top" } Block { BlockType Constant @@ -14214,7 +14397,7 @@ Model { Ports [2, 1] Position [180, 22, 345, 53] ZOrder 894 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" @@ -14233,7 +14416,7 @@ Model { Ports [2, 1] Position [180, 107, 345, 138] ZOrder 899 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" @@ -14420,6 +14603,7 @@ Model { System { Name "State Machine" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -14552,7 +14736,7 @@ Model { Position [330, 295, 430, 315] ZOrder 594 ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" SourceType "MinimumJerkTrajectoryGenerator" SourceProductName "WholeBodyToolbox" @@ -14593,6 +14777,7 @@ Model { System { Name "STATE MACHINE" Location [219, 337, 814, 775] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -14608,7 +14793,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3824" + SIDHighWatermark "3827" SIDPrevWatermark "9" SimulinkSubDomain "Simulink" Block { @@ -14693,25 +14878,25 @@ Model { Block { BlockType Demux Name " Demux " - SID "2220::3823" + SID "2220::3826" Ports [1, 1] Position [270, 495, 320, 535] - ZOrder 221 + ZOrder 224 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2220::3822" + SID "2220::3825" Tag "Stateflow S-Function 13" Ports [10, 11] Position [180, 100, 230, 340] - ZOrder 220 + ZOrder 223 FunctionName "sf_sfun" Parameters "Config,Gain,StateMachine" PortCounts "[10 11]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -14757,9 +14942,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2220::3824" + SID "2220::3827" Position [460, 506, 480, 524] - ZOrder 222 + ZOrder 225 } Block { BlockType Outport @@ -14851,70 +15036,70 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 1146 + ZOrder 1168 SrcBlock "pos_CoM_0" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 1147 + ZOrder 1169 SrcBlock "jointPos_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 1148 + ZOrder 1170 SrcBlock "pos_CoM_fixed_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 1149 + ZOrder 1171 SrcBlock "pos_CoM_fixed_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 1150 + ZOrder 1172 SrcBlock "jointPos" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 1151 + ZOrder 1173 SrcBlock "time" SrcPort 1 DstBlock " SFunction " DstPort 6 } Line { - ZOrder 1152 + ZOrder 1174 SrcBlock "wrench_rightFoot" SrcPort 1 DstBlock " SFunction " DstPort 7 } Line { - ZOrder 1153 + ZOrder 1175 SrcBlock "wrench_leftFoot" SrcPort 1 DstBlock " SFunction " DstPort 8 } Line { - ZOrder 1154 + ZOrder 1176 SrcBlock "l_sole_H_b" SrcPort 1 DstBlock " SFunction " DstPort 9 } Line { - ZOrder 1155 + ZOrder 1177 SrcBlock "r_sole_H_b" SrcPort 1 DstBlock " SFunction " @@ -14922,7 +15107,7 @@ Model { } Line { Name "w_H_b" - ZOrder 1156 + ZOrder 1178 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -14931,7 +15116,7 @@ Model { } Line { Name "pos_CoM_des" - ZOrder 1157 + ZOrder 1179 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -14940,7 +15125,7 @@ Model { } Line { Name "jointPos_des" - ZOrder 1158 + ZOrder 1180 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -14949,7 +15134,7 @@ Model { } Line { Name "feetContactStatus" - ZOrder 1159 + ZOrder 1181 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -14958,7 +15143,7 @@ Model { } Line { Name "KP_postural_diag" - ZOrder 1160 + ZOrder 1182 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -14967,7 +15152,7 @@ Model { } Line { Name "KP_CoM_diag" - ZOrder 1161 + ZOrder 1183 Labels [0, 0] SrcBlock " SFunction " SrcPort 7 @@ -14976,7 +15161,7 @@ Model { } Line { Name "KD_CoM_diag" - ZOrder 1162 + ZOrder 1184 Labels [0, 0] SrcBlock " SFunction " SrcPort 8 @@ -14985,7 +15170,7 @@ Model { } Line { Name "state" - ZOrder 1163 + ZOrder 1185 Labels [0, 0] SrcBlock " SFunction " SrcPort 9 @@ -14994,7 +15179,7 @@ Model { } Line { Name "smoothingTimeJoints" - ZOrder 1164 + ZOrder 1186 Labels [0, 0] SrcBlock " SFunction " SrcPort 10 @@ -15003,7 +15188,7 @@ Model { } Line { Name "smoothingTimeCoM" - ZOrder 1165 + ZOrder 1187 Labels [0, 0] SrcBlock " SFunction " SrcPort 11 @@ -15011,14 +15196,14 @@ Model { DstPort 1 } Line { - ZOrder 1166 + ZOrder 1188 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 1167 + ZOrder 1189 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -15036,7 +15221,7 @@ Model { ZOrder 583 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" SourceProductName "WholeBodyToolbox" @@ -15056,7 +15241,7 @@ Model { ZOrder 584 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" SourceProductName "WholeBodyToolbox" @@ -15079,6 +15264,7 @@ Model { System { Name "xCom" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -15101,7 +15287,7 @@ Model { SID "4227" Position [30, 237, 60, 253] ZOrder 581 - NamePlacement "alternate" + NameLocation "top" } Block { BlockType Inport @@ -15118,7 +15304,7 @@ Model { Ports [2, 1] Position [110, 230, 275, 290] ZOrder 578 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" @@ -15194,6 +15380,7 @@ Model { System { Name "xCom2" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -15216,7 +15403,7 @@ Model { SID "4733" Position [30, 237, 60, 253] ZOrder 581 - NamePlacement "alternate" + NameLocation "top" } Block { BlockType Inport @@ -15233,7 +15420,7 @@ Model { Ports [2, 1] Position [110, 230, 275, 290] ZOrder 578 - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" @@ -15681,7 +15868,8 @@ Model { RequestExecContextInheritance off System { Name "Update Gains and References" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -15696,7 +15884,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "498" + ZoomFactor "329" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -15786,6 +15974,7 @@ Model { System { Name "Reshape Gains Matrices" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -15843,6 +16032,7 @@ Model { System { Name "MATLAB Function" Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -15858,7 +16048,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "69" + SIDHighWatermark "72" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -15870,24 +16060,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4782::68" + SID "4782::71" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 59 + ZOrder 62 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4782::67" + SID "4782::70" Tag "Stateflow S-Function 2" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 58 + ZOrder 61 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -15897,9 +16087,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4782::69" + SID "4782::72" Position [460, 241, 480, 259] - ZOrder 60 + ZOrder 63 } Block { BlockType Outport @@ -15910,7 +16100,7 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 69 + ZOrder 73 SrcBlock "d" SrcPort 1 DstBlock " SFunction " @@ -15918,7 +16108,7 @@ Model { } Line { Name "D" - ZOrder 70 + ZOrder 74 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -15926,14 +16116,14 @@ Model { DstPort 1 } Line { - ZOrder 71 + ZOrder 75 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 72 + ZOrder 76 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -15959,6 +16149,7 @@ Model { System { Name "MATLAB Function1" Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -15974,7 +16165,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "69" + SIDHighWatermark "72" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -15986,24 +16177,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4783::68" + SID "4783::71" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 59 + ZOrder 62 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4783::67" + SID "4783::70" Tag "Stateflow S-Function 1" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 58 + ZOrder 61 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -16013,9 +16204,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4783::69" + SID "4783::72" Position [460, 241, 480, 259] - ZOrder 60 + ZOrder 63 } Block { BlockType Outport @@ -16026,7 +16217,7 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 65 + ZOrder 69 SrcBlock "d" SrcPort 1 DstBlock " SFunction " @@ -16034,7 +16225,7 @@ Model { } Line { Name "D" - ZOrder 66 + ZOrder 70 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -16042,14 +16233,14 @@ Model { DstPort 1 } Line { - ZOrder 67 + ZOrder 71 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 68 + ZOrder 72 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -16075,6 +16266,7 @@ Model { System { Name "MATLAB Function2" Location [223, 338, 826, 833] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -16090,7 +16282,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "69" + SIDHighWatermark "72" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -16102,24 +16294,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4784::68" + SID "4784::71" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 59 + ZOrder 62 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4784::67" + SID "4784::70" Tag "Stateflow S-Function 4" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 58 + ZOrder 61 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -16129,9 +16321,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4784::69" + SID "4784::72" Position [460, 241, 480, 259] - ZOrder 60 + ZOrder 63 } Block { BlockType Outport @@ -16142,7 +16334,7 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 65 + ZOrder 69 SrcBlock "d" SrcPort 1 DstBlock " SFunction " @@ -16150,7 +16342,7 @@ Model { } Line { Name "D" - ZOrder 66 + ZOrder 70 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -16158,14 +16350,14 @@ Model { DstPort 1 } Line { - ZOrder 67 + ZOrder 71 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 68 + ZOrder 72 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -16269,6 +16461,7 @@ Model { System { Name "Smooth reference CoM" Location [134, 55, 3840, 2160] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -16316,7 +16509,7 @@ Model { Position [215, -278, 365, -172] ZOrder 597 ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" SourceType "MinimumJerkTrajectoryGenerator" SourceProductName "WholeBodyToolbox" @@ -16490,7 +16683,8 @@ Model { RequestExecContextInheritance off System { Name "Smooth reference joint position" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -16540,7 +16734,7 @@ Model { Position [90, -120, 240, -10] ZOrder 598 ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" SourceType "MinimumJerkTrajectoryGenerator" SourceProductName "WholeBodyToolbox" @@ -17117,19 +17311,21 @@ Model { Ports [1] Position [305, 241, 430, 269] ZOrder 966 - BackgroundColor "red" + ForegroundColor "red" + BackgroundColor "yellow" ShowName off RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 34 + $ObjectID 46 $ClassName "Simulink.Mask" Display "disp('EMERGENCY STOP')" RunInitForIconRedraw "off" } System { Name "emergency stop: joint limits" - Location [67, 27, 1920, 1080] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -17161,7 +17357,7 @@ Model { ZOrder 553 BlockRotation 270 BlockMirror on - NamePlacement "alternate" + NameLocation "left" ShowName off Value "Config.EMERGENCY_STOP_WITH_JOINTS_LIMITS" } @@ -17173,7 +17369,7 @@ Model { ZOrder 555 BlockRotation 270 BlockMirror on - NamePlacement "alternate" + NameLocation "left" ShowName off Value "Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES" } @@ -17184,10 +17380,13 @@ Model { Ports [1, 0, 1] Position [285, 229, 440, 261] ZOrder 554 + ForegroundColor "red" + BackgroundColor "yellow" RequestExecContextInheritance off System { Name "STOP IF JOINTS HIT THE LIMITS" - Location [67, 27, 1920, 1080] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -17238,7 +17437,7 @@ Model { ZOrder 560 BackgroundColor "[0.513700, 0.851000, 0.670600]" ShowName off - LibraryVersion "1.675" + LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/States/GetLimits" SourceType "GetLimits" SourceProductName "WholeBodyToolbox" @@ -17258,6 +17457,8 @@ Model { Ports [4, 2] Position [180, 22, 300, 163] ZOrder 205 + ForegroundColor "red" + BackgroundColor "yellow" ShowName off LibraryVersion "1.32" ErrorFcn "Stateflow.Translate.translate" @@ -17268,6 +17469,7 @@ Model { System { Name "MATLAB Function" Location [121, 45, 868, 1245] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -17283,7 +17485,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3813" + SIDHighWatermark "3816" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -17319,24 +17521,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4922::3812" + SID "4922::3815" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 162 + ZOrder 165 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4922::3811" + SID "4922::3814" Tag "Stateflow S-Function 18" Ports [4, 3] Position [180, 102, 230, 203] - ZOrder 161 + ZOrder 164 FunctionName "sf_sfun" PortCounts "[4 3]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -17350,9 +17552,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4922::3813" + SID "4922::3816" Position [460, 241, 480, 259] - ZOrder 163 + ZOrder 166 } Block { BlockType Outport @@ -17371,28 +17573,28 @@ Model { Port "2" } Line { - ZOrder 65 + ZOrder 73 SrcBlock "umin" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 66 + ZOrder 74 SrcBlock "umax" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 67 + ZOrder 75 SrcBlock "u" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 68 + ZOrder 76 SrcBlock "tol" SrcPort 1 DstBlock " SFunction " @@ -17400,7 +17602,7 @@ Model { } Line { Name "inRange" - ZOrder 69 + ZOrder 77 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17409,7 +17611,7 @@ Model { } Line { Name "res_check_range" - ZOrder 70 + ZOrder 78 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -17417,14 +17619,14 @@ Model { DstPort 1 } Line { - ZOrder 71 + ZOrder 79 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 72 + ZOrder 80 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -17531,7 +17733,8 @@ Model { RequestExecContextInheritance off System { Name "STOP IF THERE ARE SPIKES IN THE ENCODERS" - Location [67, 27, 1920, 1080] + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -17590,6 +17793,7 @@ Model { System { Name "MATLAB Function" Location [121, 45, 868, 1245] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -17605,7 +17809,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3813" + SIDHighWatermark "3816" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -17625,24 +17829,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "4929::3812" + SID "4929::3815" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 162 + ZOrder 165 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4929::3811" + SID "4929::3814" Tag "Stateflow S-Function 14" Ports [2, 3] Position [180, 100, 230, 180] - ZOrder 161 + ZOrder 164 FunctionName "sf_sfun" PortCounts "[2 3]" SFunctionDeploymentMode off - EnableBusSupport off + EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -17656,9 +17860,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4929::3813" + SID "4929::3816" Position [460, 241, 480, 259] - ZOrder 163 + ZOrder 166 } Block { BlockType Outport @@ -17677,7 +17881,7 @@ Model { Port "2" } Line { - ZOrder 49 + ZOrder 55 SrcBlock "u" SrcPort 1 Points [120, 0] @@ -17685,7 +17889,7 @@ Model { DstPort 1 } Line { - ZOrder 50 + ZOrder 56 SrcBlock "delta_u_max" SrcPort 1 DstBlock " SFunction " @@ -17693,7 +17897,7 @@ Model { } Line { Name "noSpikes" - ZOrder 51 + ZOrder 57 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17702,7 +17906,7 @@ Model { } Line { Name "res_check_spikes" - ZOrder 52 + ZOrder 58 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -17710,14 +17914,14 @@ Model { DstPort 1 } Line { - ZOrder 53 + ZOrder 59 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 54 + ZOrder 60 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -18066,51 +18270,56 @@ Model { DstBlock "Robot State and References" DstPort 5 } + Line { + ZOrder 329 + SrcBlock "jointAcc" + SrcPort 1 + DstBlock "GoTo Motors Inertia" + DstPort 1 + } } } Block { BlockType Reference - Name "SetReferences" - SID "2354" - Ports [1] - Position [1185, 487, 1245, 513] - ZOrder 549 - BackgroundColor "[0.827500, 0.576500, 0.603900]" - ShowName off - LibraryVersion "1.675" - SourceBlock "WBToolboxLibrary/Actuators/SetReferences" - SourceType "SetReferences" - SourceProductName "WholeBodyToolbox" - RTWMemSecFuncInitTerm "Inherit from model" - RTWMemSecFuncExecute "Inherit from model" - RTWMemSecDataConstants "Inherit from model" - RTWMemSecDataInternal "Inherit from model" - RTWMemSecDataParameters "Inherit from model" - ContentPreviewEnabled off - controlType "Torque" - refSpeed "50" - refAcceleration "1000000*(pi/180)" + Name "RobotVisualizer" + SID "4939" + Ports [4] + Position [1515, 252, 1735, 368] + 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" + } + 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+robot_config.N_DOF]" + OutputSizes "1" } Block { BlockType SubSystem - Name "synchronizer" - SID "2423" - Ports [] - Position [1157, 423, 1272, 453] - ZOrder 546 - ForegroundColor "red" - BackgroundColor "green" - ShowName off + Name "Subsystem" + SID "4955" + Ports [1, 5] + Position [1190, 427, 1385, 573] + ZOrder 1279 + TreatAsAtomicUnit on + SystemSampleTime "Config.tStepSim" RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 35 - $ClassName "Simulink.Mask" - Display "disp('SYNCHRONIZER')" - } + ContentPreviewEnabled on System { - Name "synchronizer" - Location [134, 55, 3840, 2160] + Name "Subsystem" + Location [-75, -1417, 2485, 0] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -18125,311 +18334,448 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "947" + ZoomFactor "307" SimulinkSubDomain "Simulink" Block { - BlockType SubSystem - Name "GAZEBO_SYNCHRONIZER" - SID "2424" - Ports [0, 0, 1] - Position [5, 15, 130, 75] - ZOrder -7 - RequestExecContextInheritance off - System { - Name "GAZEBO_SYNCHRONIZER" - Location [134, 55, 3840, 2160] - Open off - PortBlocksUseCompactNotation off - SetExecutionDomain off - ExecutionDomainType "Deduce" - ModelBrowserVisibility off - 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 EnablePort - Name "Enable" - SID "2425" - Ports [] - Position [177, 85, 197, 105] - ZOrder 538 - } - Block { - BlockType Reference - Name "Simulator Synchronizer" - SID "2426" - Ports [] - Position [120, 24, 250, 61] - ZOrder 539 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - LibraryVersion "1.675" - SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" - SourceType "Simulator Synchronizer" - SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" - period "Config.tStep" - serverPortName "'/clock/rpc'" - clientPortName "'/WBT_synchronizer/rpc:o'" - } - } + BlockType Inport + Name "joints torque" + SID "4956" + Position [120, 258, 150, 272] + ZOrder 1275 } Block { - BlockType Logic - Name "Logical\nOperator" - SID "2427" - Ports [1, 1] - Position [-30, -34, -5, -16] - ZOrder 307 - BlockMirror on - NamePlacement "alternate" + BlockType BusSelector + Name "Bus\nSelector1" + SID "4942" + Ports [1, 4] + Position [445, 393, 450, 507] + ZOrder 1274 ShowName off - Operator "NOT" - AllPortsSameDT off - OutDataTypeStr "boolean" + 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 Reference + Name "Configuration" + SID "4962" + Ports [] + Position [460, 195, 535, 225] + 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 "ON_GAZEBO\n" - SID "2428" - Position [95, -35, 210, -15] - ZOrder 304 - BlockMirror on - NamePlacement "alternate" - ShowName off - Value "Config.ON_GAZEBO" + Name "Constant1" + SID "4940" + Position [120, 285, 150, 315] + ZOrder 1272 + Value "zeros(robot_config.N_DOF + 6,1)" } Block { - BlockType SubSystem - Name "REAL_TIME_SYNC" - SID "2429" - Ports [0, 0, 1] - Position [-160, 17, -35, 77] - ZOrder -9 - RequestExecContextInheritance off - System { - Name "REAL_TIME_SYNC" - Location [134, 55, 3840, 2160] - Open off - PortBlocksUseCompactNotation off - SetExecutionDomain off - ExecutionDomainType "Deduce" - ModelBrowserVisibility off - 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 EnablePort - Name "Enable" - SID "2430" - Ports [] - Position [167, 90, 187, 110] - ZOrder 538 - } - Block { - BlockType Reference - Name "Real Time Synchronizer" - SID "2431" - Ports [] - Position [115, 34, 240, 71] - ZOrder 539 - ForegroundColor "[0.916667, 0.916667, 0.916667]" - BackgroundColor "gray" - ShowName off - LibraryVersion "1.675" - SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" - SourceType "Real Time Synchronizer" - SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" - period "Config.tStep" - } - } + BlockType Constant + Name "Constant2" + SID "4941" + Position [120, 320, 150, 350] + ZOrder 1273 + Value "zeros(robot_config.N_DOF,1)" } Block { - BlockType ToWorkspace - Name "To Workspace" - SID "4628" - Ports [1] - Position [125, -95, 185, -75] - ZOrder 699 - ShowName off - HideAutomaticName off - VariableName "yarp_time" - MaxDataPoints "inf" - SaveFormat "Structure With Time" - FixptAsFi on - SampleTime "-1" + BlockType Reference + Name "IMUsensor" + SID "4938" + Ports [4, 1] + Position [545, 392, 765, 508] + ZOrder 1270 + LibraryVersion "1.2" + SourceBlock "mwbs_robotSensors_lib/IMUsensor" + SourceType "SubSystem" + SourceProductName "Matlab Whole-body Simulator" + 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 Reference - Name "Yarp Clock" - SID "4629" - Ports [0, 1] - Position [-50, -94, 15, -76] - ZOrder 698 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - HideAutomaticName off - LibraryVersion "1.675" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Clock" - SourceType "YARP Clock" - SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" + Name "RobotDynWithContacts" + SID "4937" + Ports [3, 4] + Position [180, 244, 375, 356] + ZOrder 1231 + LibraryVersion "3.3" + 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" + } + 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 1 - SrcBlock "Logical\nOperator" + ZOrder 324 + SrcBlock "RobotDynWithContacts" + SrcPort 3 + DstBlock "wrench_RFoot" + DstPort 1 + } + Line { + ZOrder 322 + SrcBlock "RobotDynWithContacts" SrcPort 1 - Points [-60, 0] - DstBlock "REAL_TIME_SYNC" - DstPort enable + 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 { + ZOrder 321 + SrcBlock "joints torque" + SrcPort 1 + DstBlock "RobotDynWithContacts" + DstPort 1 } Line { - ZOrder 2 - SrcBlock "ON_GAZEBO\n" + Name "" + ZOrder 297 + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 2 + DstBlock "IMUsensor" + DstPort 2 + } + Line { + ZOrder 290 + SrcBlock "Constant2" SrcPort 1 - Points [-25, 0] + 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 6 - DstBlock "GAZEBO_SYNCHRONIZER" - DstPort enable + ZOrder 325 + Points [0, -150] + DstBlock "" + DstPort 1 } Branch { - ZOrder 4 - DstBlock "Logical\nOperator" - DstPort 1 + ZOrder 318 + DstBlock "IMUsensor" + DstPort 4 } } Line { - ZOrder 8 - SrcBlock "Yarp Clock" - SrcPort 1 - DstBlock "To Workspace" + ZOrder 323 + SrcBlock "RobotDynWithContacts" + SrcPort 2 + DstBlock "wrench_LFoot" DstPort 1 } } } - Block { - BlockType Reference - Name "wrench_leftFoot" - SID "2206" - Ports [0, 1] - Position [800, 553, 840, 567] - ZOrder 962 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - HideAutomaticName off - LibraryVersion "1.675" - SourceBlock "WBToolboxLibrary/Utilities/YarpRead" - SourceType "YARP Read" - SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" - portName "Ports.WRENCH_LEFT_FOOT" - signalSize "Ports.WRENCH_PORT_SIZE" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on + Line { + Name "" + ZOrder 308 + Labels [0, 0] + SrcBlock "Bus\nSelector" + SrcPort 2 + DstBlock "RobotVisualizer" + DstPort 2 } - Block { - BlockType Reference - Name "wrench_rightFoot" - SID "2218" - Ports [0, 1] - Position [800, 523, 840, 537] - ZOrder 963 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - HideAutomaticName off - LibraryVersion "1.675" - SourceBlock "WBToolboxLibrary/Utilities/YarpRead" - SourceType "YARP Read" - SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" - portName "Ports.WRENCH_RIGHT_FOOT" - signalSize "Ports.WRENCH_PORT_SIZE" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on + Line { + Name "" + ZOrder 306 + Labels [0, 0] + SrcBlock "Bus\nSelector" + SrcPort 1 + DstBlock "RobotVisualizer" + DstPort 1 } Line { - ZOrder 278 - SrcBlock "MOMENTUM BASED TORQUE CONTROL" + Name "" + ZOrder 307 + Labels [0, 0] + SrcBlock "Bus\nSelector" + SrcPort 3 + Points [113, 0] + Branch { + ZOrder 313 + Points [0, 85] + DstBlock "Bus\nCreator" + DstPort 1 + } + Branch { + ZOrder 312 + DstBlock "RobotVisualizer" + DstPort 3 + } + } + Line { + Name "" + ZOrder 309 + Labels [0, 0] + SrcBlock "Bus\nSelector" + SrcPort 4 + Points [87, 0] + Branch { + ZOrder 315 + Points [0, 85] + DstBlock "Bus\nCreator" + DstPort 2 + } + Branch { + ZOrder 314 + DstBlock "RobotVisualizer" + DstPort 4 + } + } + Line { + ZOrder 321 + SrcBlock "Bus\nCreator" SrcPort 1 - DstBlock "SetReferences" + Points [19, 0; 0, 154; -1039, 0; 0, -139] + DstBlock "Bus\nSelector2" DstPort 1 } Line { - ZOrder 279 - SrcBlock "GetMeasurement" + ZOrder 361 + SrcBlock "MOMENTUM BASED TORQUE CONTROL" SrcPort 1 - DstBlock "GoTo Motors Inertia" + DstBlock "Subsystem" DstPort 1 } Line { - ZOrder 282 - SrcBlock "GetMeasurement2" + ZOrder 362 + SrcBlock "Subsystem" SrcPort 1 + Points [19, 0; 0, -42; -96, 0; 0, -88] + DstBlock "Bus\nSelector" + 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 2 + DstPort 4 } Line { - ZOrder 283 - SrcBlock "GetMeasurement1" - SrcPort 1 + Name "" + ZOrder 380 + Labels [0, 0] + SrcBlock "Bus\nSelector2" + SrcPort 5 DstBlock "MOMENTUM BASED TORQUE CONTROL" - DstPort 1 + DstPort 5 } Line { - ZOrder 284 - SrcBlock "IMU_meas" - SrcPort 1 + Name "" + ZOrder 381 + Labels [0, 0] + SrcBlock "Bus\nSelector2" + SrcPort 6 DstBlock "MOMENTUM BASED TORQUE CONTROL" - DstPort 3 + DstPort 6 } Line { - ZOrder 286 - SrcBlock "wrench_rightFoot" + Name "" + ZOrder 388 + Labels [0, 0] + SrcBlock "Bus\nSelector2" SrcPort 1 DstBlock "MOMENTUM BASED TORQUE CONTROL" - DstPort 4 + DstPort 1 + } + Line { + Name "" + ZOrder 390 + Labels [0, 0] + SrcBlock "Bus\nSelector2" + SrcPort 2 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 2 } Line { - ZOrder 287 - SrcBlock "wrench_leftFoot" + 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 5 + DstPort 3 } } } #Finite State Machines # -# Stateflow 80000032 +# Stateflow 80000036 # # Stateflow { machine { id 1 - name "torqueControlBalancing" - sfVersion 80000019 + name "torqueControlBalancingWithSimu" + sfVersion 80000036 firstTarget 237 } chart { @@ -22071,6 +22417,9 @@ Stateflow { screen [1 1 1366 768 1.25] treeNode [0 172 0 0] viewObj 171 + visible 1 + subviewS { + } ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART From 7e7467304a0e834411f01303dcbcaa32dc4ed705 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Wed, 28 Apr 2021 06:07:58 +0200 Subject: [PATCH 03/20] Main fixes for compiling the model, having the simulator running at 1kHz and the controller at 100Hz - Define additional joints for the simulation robot model: neck joints. The neck joints are not part of the joints controlled by the high-level torque controller. - Fixes in the controller sub-system for settling the same Config.tStep sample time in all the sub-system blcoks: - Set "Inf" sampling time in reflected inertia block. - Replaced the Holder blocks from the WBT library by a re-implementation in the "utilityBlocks" library ("Joint Torque Saturation", "LFoot to base link transform", "RFoot to base link transform", "State Machine"). - In the "neck transform" block, replaced port read of neck joints by input from the simulator. (Routed neck position from main inputs of the controller system) (Added selectors to the controller inputs for removing the neck joints, as these are processed separately) - Added a zero order hold to the clock input of the state machine, otherwise the sampling time type becomes "continuous" and propagates throughout the model, preventing the compilation. - Added a unit delay between the controller output and the simulator. - Other minor fixes in the controller sub-system, inr: - Removed "measured joint torques" from the visualizer. - Routed the joint acceleration "measurements" from the main input to the joint reflected inertia block (tags are not authorized across sub-systems when treating the parent sub-system as an atomic unit). - Treat the controller sub-system as an atomic unit. --- .../app/robots/iCubGenova04/configRobot.m | 3 + .../app/robots/iCubGenova04/configRobotSim.m | 13 +- .../torqueControlBalancingWithSimu.mdl | 1906 ++++++++++------- .../Utilities/utilityBlocks_lib.slx | Bin 36668 -> 60583 bytes 4 files changed, 1176 insertions(+), 746 deletions(-) diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m index 677c8483..9c187aa2 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m @@ -31,6 +31,9 @@ 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'; diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m index 9e545e48..b8af3d8c 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m @@ -16,16 +16,17 @@ 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'}; +WBTConfigRobotSim.ControlBoardsNames = {'torso','left_arm','right_arm','left_leg','right_leg','head'}; WBTConfigRobotSim.ControlledJoints = []; 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'}; +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})]; diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl index e1262f1b..87ea58e8 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl +++ b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl @@ -7,7 +7,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "3.28" + ComputedModelVersion "3.72" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -20,19 +20,13 @@ Model { IsArchitectureModel 0 IsAUTOSARArchitectureModel 0 NumParameterArguments 0 - NumExternalFileReferences 49 + NumExternalFileReferences 46 ExternalFileReference { Reference "WBToolboxLibrary/Utilities/Configuration" Path "torqueControlBalancingWithSimu/Configuration" SID "4537" Type "LIBRARY_BLOCK" } - ExternalFileReference { - Reference "WBToolboxLibrary/States/GetMeasurement" - Path "torqueControlBalancingWithSimu/Dump and visualize/Visualizer/GetMeasurement1" - SID "4543" - Type "LIBRARY_BLOCK" - } ExternalFileReference { Reference "WBToolboxLibrary/Utilities/MatchSignalSizes" Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desire" @@ -173,9 +167,9 @@ Model { Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/holder " - SID "4559" + Reference "utilityBlocks_lib/Holder" + Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/holder 7" + SID "4996" Type "LIBRARY_BLOCK" } ExternalFileReference { @@ -195,36 +189,29 @@ Model { 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 "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 root link transform" + "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/Utilities/YarpRead" + 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 /Neck Position" - SID "4254" + "e to fixed link transform/LFoot to base link transform /Fixed base to imu transform" + SID "4250" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" + 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 "4258" + SID "4982" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" + 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 "4259" + SID "4983" Type "LIBRARY_BLOCK" } ExternalFileReference { @@ -242,24 +229,17 @@ Model { Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/YarpRead" - Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute bas" - "e to fixed link transform/RFoot to base link transform/Neck Position" - SID "4860" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" + 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 1" - SID "4865" + "e to fixed link transform/RFoot to base link transform/holder 3" + SID "4984" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" + 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 2" - SID "4866" + "e to fixed link transform/RFoot to base link transform/holder 4" + SID "4985" Type "LIBRARY_BLOCK" } ExternalFileReference { @@ -284,17 +264,17 @@ Model { Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" + Reference "utilityBlocks_lib/Holder" Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machi" - "ne/holder 1" - SID "2187" + "ne/holder 5" + SID "4993" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" + Reference "utilityBlocks_lib/Holder" Path "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machi" - "ne/holder 2" - SID "2188" + "ne/holder 6" + SID "4994" Type "LIBRARY_BLOCK" } ExternalFileReference { @@ -407,12 +387,12 @@ Model { $PropName "ModelBrowserInfo" $ObjectID 3 $ClassName "Simulink.ModelBrowserInfo" - Visible [0] + Visible [1] DockPosition "Left" Width [50] Height [50] Filter [59] - Minimized "Unset" + Minimized "Off" } Object { $PropName "ExplorerBarInfo" @@ -422,163 +402,526 @@ Model { } Array { Type "Simulink.EditorInfo" - Dimension 23 + Dimension 55 Object { $ObjectID 5 - IsActive [1] + IsActive [0] IsTabbed [1] - ViewObjType "SimulinkTopLevel" - LoadSaveID "0" - Extents [2522.0, 1188.0] - ZoomFactor [2.3918755287864548] - Offset [714.122328915358, 197.16778184538103] - SceneRectInView [714.122328915358, 197.16778184538103, 1054.4026934710794, 496.68136393483036] + ViewObjType "SimulinkSubsys" + LoadSaveID "4553" + Extents [2075.0, 1188.0] + ZoomFactor [2.8272565416779067] + Offset [-123.97523433683438, -125.59766579140205] + SceneRectInView [-123.97523433683438, -125.59766579140205, 733.92703117366875, 420.1953315828041] } Object { $ObjectID 6 - IsActive [0] - IsTabbed [0] + IsActive [1] + IsTabbed [1] ViewObjType "SimulinkSubsys" - LoadSaveID "4519" - Extents [2522.0, 1188.0] - ZoomFactor [3.3799999999999994] - Offset [-591.8224852071005, -288.66568047337279] - SceneRectInView [-591.8224852071005, -288.66568047337279, 746.1538461538463, 351.47928994082844] + LoadSaveID "4836" + Extents [2075.0, 1188.0] + ZoomFactor [1.7391304347826086] + Offset [78.1640625, -29.050000000000011] + SceneRectInView [78.1640625, -29.050000000000011, 1193.125, 683.1] } Object { $ObjectID 7 IsActive [0] - IsTabbed [0] + IsTabbed [1] ViewObjType "SimulinkSubsys" - LoadSaveID "2087" - Extents [2522.0, 1188.0] - ZoomFactor [2.4432827938801593] - Offset [-610.27687073977552, 2460.8844664695271] - SceneRectInView [-610.27687073977552, 2460.8844664695271, 1032.217803979551, 486.23106706094632] + LoadSaveID "4955" + Extents [2075.0, 1188.0] + ZoomFactor [2.4395775512260358] + Offset [102.27209051724134, 108.01522137450439] + SceneRectInView [102.27209051724134, 108.01522137450439, 850.55709705034235, 486.96955725099122] } Object { $ObjectID 8 IsActive [0] - IsTabbed [0] + IsTabbed [1] ViewObjType "SimulinkSubsys" LoadSaveID "2355" - Extents [2522.0, 1188.0] + Extents [2075.0, 1188.0] ZoomFactor [1.0730804810360777] - Offset [529.272979525862, -198.046551724138] - SceneRectInView [529.272979525862, -198.046551724138, 2350.2431034482761, 1107.093103448276] + Offset [942.03160021551719, -198.046551724138] + SceneRectInView [942.03160021551719, -198.046551724138, 1933.6853448275863, 1107.093103448276] } Object { $ObjectID 9 IsActive [0] IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "3241" - Extents [2522.0, 1188.0] - ZoomFactor [2.3999999999999995] - Offset [-657.48177083333348, 60.999999999999943] - SceneRectInView [-657.48177083333348, 60.999999999999943, 1050.8333333333335, 495.00000000000011] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [2075.0, 1188.0] + ZoomFactor [1.75] + Offset [691.44166877486214, 105.0577102335036] + SceneRectInView [691.44166877486214, 105.0577102335036, 1185.7142857142858, 678.85714285714289] } Object { $ObjectID 10 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4030" - Extents [2522.0, 1188.0] - ZoomFactor [3.2861189801699715] - Offset [203.38965517241377, -277.26034482758621] - SceneRectInView [203.38965517241377, -277.26034482758621, 767.47068965517246, 361.52068965517242] + LoadSaveID "4307" + Extents [2075.0, 1188.0] + ZoomFactor [2.56669704897075] + Offset [-29.368338160419114, -144.92583198051949] + SceneRectInView [-29.368338160419114, -144.92583198051949, 808.43198882083823, 462.851663961039] } Object { $ObjectID 11 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4761" - Extents [2522.0, 1188.0] - ZoomFactor [4.26] - Offset [30.744516578638468, -178.45657276995306] - SceneRectInView [30.744516578638468, -178.45657276995306, 592.01877934272306, 278.87323943661971] + LoadSaveID "2163" + Extents [2075.0, 1188.0] + ZoomFactor [2.0] + Offset [-576.99385991695169, 9.249712994626293] + SceneRectInView [-576.99385991695169, 9.249712994626293, 1037.5, 594.0] } Object { $ObjectID 12 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4836" - Extents [2522.0, 1188.0] - ZoomFactor [1.8210361067503922] - Offset [-17.736368534482835, 1.3120689655171986] - SceneRectInView [-17.736368534482835, 1.3120689655171986, 1384.9258620689657, 652.3758620689656] + 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 13 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937" - Extents [2522.0, 1188.0] - ZoomFactor [1.5528781793842035] - Offset [-54.517079741379348, -192.01551724137931] - SceneRectInView [-54.517079741379348, -192.01551724137931, 1624.0810344827587, 765.03103448275863] + LoadSaveID "4216" + Extents [2075.0, 1188.0] + ZoomFactor [5.2673944072532821] + Offset [578.59213941133362, 441.73077216658527] + SceneRectInView [578.59213941133362, 441.73077216658527, 393.93290867733265, 225.53845566682949] } Object { $ObjectID 14 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4913" - Extents [2522.0, 1188.0] - ZoomFactor [4.0050505050505052] - Offset [94.721007009531661, 186.50441361916774] - SceneRectInView [94.721007009531661, 186.50441361916774, 629.70491803278685, 296.62547288776796] + LoadSaveID "2275" + Extents [2075.0, 1188.0] + ZoomFactor [1.8249725000639532] + Offset [-168.50171712924032, -141.48435660218672] + SceneRectInView [-168.50171712924032, -141.48435660218672, 1137.0034342584806, 650.96871320437344] } Object { $ObjectID 15 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4917" - Extents [2522.0, 1188.0] - ZoomFactor [4.3275706438106187] - Offset [10.41989407940855, -23.235071395084702] - SceneRectInView [10.41989407940855, -23.235071395084702, 582.77500417168619, 274.51891552575859] + 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 16 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "2275" - Extents [2522.0, 1188.0] - ZoomFactor [2.6728110599078341] - Offset [-71.787931034482767, -38.237931034482756] - SceneRectInView [-71.787931034482767, -38.237931034482756, 943.57586206896553, 444.47586206896551] + LoadSaveID "2087" + Extents [2075.0, 1188.0] + ZoomFactor [1.659551344359659] + Offset [-719.33693596886076, 2346.0719358766232] + SceneRectInView [-719.33693596886076, 2346.0719358766232, 1250.3379344377215, 715.85612824675331] } Object { $ObjectID 17 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" + LoadSaveID "2341" + Extents [2075.0, 1188.0] + ZoomFactor [1.5849611820686202] + Offset [-1148.4886014344261, -284.77258542366184] + SceneRectInView [-1148.4886014344261, -284.77258542366184, 1309.1803278688524, 749.54517084732367] + } + Object { + $ObjectID 18 + 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 19 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4269" + Extents [2075.0, 1188.0] + ZoomFactor [5.95] + Offset [-25.672433035714278, -45.831932773109244] + SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] + } + Object { + $ObjectID 20 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4226" + Extents [2075.0, 1188.0] + ZoomFactor [4.6350722637938047] + Offset [23.229550867122612, 135.34665821934539] + SceneRectInView [23.229550867122612, 135.34665821934539, 447.67371076575478, 256.30668356130923] + } + Object { + $ObjectID 21 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4913" + Extents [2075.0, 1188.0] + ZoomFactor [3.2951942101426641] + Offset [94.721007009531661, 154.55463376836479] + SceneRectInView [94.721007009531661, 154.55463376836479, 629.70491803278685, 360.52503258937384] + } + Object { + $ObjectID 22 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4519" + Extents [2075.0, 1188.0] + ZoomFactor [1.5723743845222296] + Offset [-676.15435221738494, -478.27262581168833] + SceneRectInView [-676.15435221738494, -478.27262581168833, 1319.6602669347699, 755.54525162337666] + } + Object { + $ObjectID 23 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4211" + Extents [2075.0, 1188.0] + ZoomFactor [2.4071047957371228] + Offset [-92.890717237308081, -373.76948051948051] + SceneRectInView [-92.890717237308081, -373.76948051948051, 862.03143447461616, 493.538961038961] + } + Object { + $ObjectID 24 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4245" + Extents [2075.0, 1188.0] + ZoomFactor [1.0] + Offset [161.34960569474492, -350.98802908437432] + SceneRectInView [161.34960569474492, -350.98802908437432, 2075.0, 1188.0] + } + Object { + $ObjectID 25 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4850" + Extents [2075.0, 1188.0] + ZoomFactor [1.3964802319507956] + Offset [434.56854543234, -203.35510808500248] + SceneRectInView [434.56854543234, -203.35510808500248, 1485.87853413532, 850.710216170005] + } + Object { + $ObjectID 26 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4993" + Extents [2075.0, 1188.0] + ZoomFactor [3.8] + Offset [-145.39473684210532, -70.1842105263158] + SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] + } + Object { + $ObjectID 27 + 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 28 + 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 29 + 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 30 + 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 31 + 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 32 + 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 33 + 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 34 + 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 35 + 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 36 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937" + Extents [2075.0, 1188.0] + ZoomFactor [1.2328521092206539] + Offset [-54.517079741379348, -291.3096149225039] + SceneRectInView [-54.517079741379348, -291.3096149225039, 1683.0891430373663, 963.61922984500779] + } + Object { + $ObjectID 37 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4994" + Extents [2075.0, 1188.0] + ZoomFactor [3.8] + Offset [-145.39473684210532, -70.1842105263158] + SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] + } + Object { + $ObjectID 38 + 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 39 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" LoadSaveID "4806" + Extents [1722.0, 1188.0] + ZoomFactor [2.0483743061062651] + Offset [-684.37478764306115, -290.08334454895737] + SceneRectInView [-684.37478764306115, -290.08334454895737, 840.66666666666652, 579.97212543554] + } + Object { + $ObjectID 40 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "3241" + Extents [1722.0, 1188.0] + ZoomFactor [1.5612382234185733] + Offset [-683.4385237068966, -71.967241379310337] + SceneRectInView [-683.4385237068966, -71.967241379310337, 1102.9706896551725, 760.93448275862067] + } + Object { + $ObjectID 41 + 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 42 + 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 43 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4504" Extents [2522.0, 1188.0] - ZoomFactor [3.0] - Offset [-684.37478764306115, -198.09728183118739] - SceneRectInView [-684.37478764306115, -198.09728183118739, 840.66666666666663, 396.0] + ZoomFactor [1.2045690550363448] + Offset [-371.86694504310344, -519.62241379310342] + SceneRectInView [-371.86694504310344, -519.62241379310342, 2093.6948275862069, 986.24482758620684] } Object { - $ObjectID 18 + $ObjectID 44 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4955" + LoadSaveID "4030" + Extents [1722.0, 1188.0] + ZoomFactor [3.2861189801699715] + Offset [203.38965517241377, -277.26034482758621] + SceneRectInView [203.38965517241377, -277.26034482758621, 524.02241379310351, 361.52068965517242] + } + Object { + $ObjectID 45 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4774" Extents [2522.0, 1188.0] - ZoomFactor [3.0728476821192054] - Offset [102.27209051724139, 158.19396551724139] - SceneRectInView [102.27209051724139, 158.19396551724139, 820.73706896551721, 386.61206896551721] + ZoomFactor [5.3226623982926506] + Offset [-8.0130751303127283, -114.03393582177664] + SceneRectInView [-8.0130751303127283, -114.03393582177664, 473.82302526062546, 223.19657177225341] } Object { - $ObjectID 19 + $ObjectID 46 + 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 47 + 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 48 + 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 49 + 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 50 + 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 51 + 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 52 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -589,7 +932,7 @@ Model { SceneRectInView [-245.23623571571773, -105.36577786688051, 666.01934643143545, 313.731555733761] } Object { - $ObjectID 20 + $ObjectID 53 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -600,18 +943,7 @@ Model { SceneRectInView [-68.684210488152473, -33.394738158739756, 477.63157399976672, 216.05262934094131] } Object { - $ObjectID 21 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "2341" - Extents [1815.0, 821.0] - ZoomFactor [1.3863636363636365] - Offset [-1148.4886014344261, -206.09836065573768] - SceneRectInView [-1148.4886014344261, -206.09836065573768, 1309.1803278688524, 592.19672131147536] - } - Object { - $ObjectID 22 + $ObjectID 54 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -622,7 +954,7 @@ Model { SceneRectInView [-211.69612560424537, 280.84982585720286, 605.0, 273.66666666666669] } Object { - $ObjectID 23 + $ObjectID 55 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -633,7 +965,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 24 + $ObjectID 56 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -644,7 +976,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 25 + $ObjectID 57 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -655,7 +987,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 26 + $ObjectID 58 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -666,7 +998,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 27 + $ObjectID 59 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -682,7 +1014,7 @@ Model { Type "Simulink.DockComponentInfo" Dimension 2 Object { - $ObjectID 28 + $ObjectID 60 Type "Simulink:Editor:ReferencedFiles" ID "Referenced Files" Visible [0] @@ -695,7 +1027,7 @@ Model { Minimized "Unset" } Object { - $ObjectID 29 + $ObjectID 61 Type "GLUE2:PropertyInspector" ID "Property Inspector" Visible [0] @@ -709,12 +1041,12 @@ Model { } PropName "DockComponentsInfo" } - WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAABPsAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAG+AAAE4fwCAAAABPsAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABrAP////sAAABgAFMAaQBtAHUAb" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQBAAAAeQAABOEAAABrAP////sAAABgAFMAaQBtAHUAb" "ABpAG4AawA6AEUAZABpAHQAbwByADoAUgBlAGYAZQByAGUAbgBjAGUAZABGAGkAbABlAHMALwBSAGUAZgBlAHIAZQBuAGMAZQBkACAARgBpAGwAZ" - "QBzAAAAAAD/////AAAAjwD///8AAAABAAAAAAAAAAD8AgAAAAH7AAAAVABHAEwAVQBFADIAOgBQAHIAbwBwAGUAcgB0AHkASQBuAHMAcABlAGMAd" - "ABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA////AAAKAAAABOEAAAABAAAAAgAAAAEAAAAC/" + "QBzAAAAAAD/////AAAAjwD///8AAAABAAABYAAABOH8AgAAAAH7AAAAVABHAEwAVQBFADIAOgBQAHIAbwBwAGUAcgB0AHkASQBuAHMAcABlAGMAd" + "ABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA////AAAIQQAABOEAAAABAAAAAgAAAAEAAAAC/" "AAAAAEAAAACAAAAAA==" Array { Type "Cell" @@ -734,11 +1066,11 @@ Model { ModifiedByFormat "%" LastModifiedBy "nunoguedelha" ModifiedDateFormat "%" - LastModifiedDate "Tue Apr 27 22:15:53 2021" - RTWModifiedTimeStamp 541462537 - ModelVersionFormat "%" - SampleTimeColors off - SampleTimeAnnotations off + LastModifiedDate "Thu Apr 29 00:41:09 2021" + RTWModifiedTimeStamp 541557663 + ModelVersionFormat "%" + SampleTimeColors on + SampleTimeAnnotations on LibraryLinkDisplay "disabled" WideLines off ShowLineDimensions on @@ -758,7 +1090,7 @@ Model { ShowViewerIcons on VariantCondition off ShowLinearizationAnnotations on - ShowVisualizeInsertedRTB on + ShowVisualizeInsertedRTB off ShowMarkup on BlockNameDataTip off BlockParametersDataTip off @@ -793,7 +1125,7 @@ Model { TryForcingSFcnDF off Object { $PropName "DataLoggingOverride" - $ObjectID 30 + $ObjectID 62 $ClassName "Simulink.SimulationData.ModelLoggingInfo" model_ "torqueControlBalancingWithSimu" signals_ [] @@ -841,7 +1173,7 @@ Model { Type "Handle" Dimension 1 Simulink.ConfigSet { - $ObjectID 31 + $ObjectID 63 Version "20.1.0" DisabledProps [] Description "" @@ -849,7 +1181,7 @@ Model { Type "Handle" Dimension 9 Simulink.SolverCC { - $ObjectID 32 + $ObjectID 64 Version "20.1.0" DisabledProps [] Description "" @@ -891,7 +1223,7 @@ Model { ODENIntegrationMethod "ode3" } Simulink.DataIOCC { - $ObjectID 33 + $ObjectID 65 Version "20.1.0" DisabledProps [] Description "" @@ -933,7 +1265,7 @@ Model { LoggingIntervals "[-inf, inf]" } Simulink.OptimizationCC { - $ObjectID 34 + $ObjectID 66 Version "20.1.0" Array { Type "Cell" @@ -1007,7 +1339,7 @@ Model { EfficientTunableParamExpr off } Simulink.DebuggingCC { - $ObjectID 35 + $ObjectID 67 Version "20.1.0" Array { Type "Cell" @@ -1130,7 +1462,7 @@ Model { VariantConditionMismatch "none" } Simulink.HardwareCC { - $ObjectID 36 + $ObjectID 68 Version "20.1.0" DisabledProps [] Description "" @@ -1180,7 +1512,7 @@ Model { HardwareBoardFeatureSet "EmbeddedCoderHSP" } Simulink.ModelReferenceCC { - $ObjectID 37 + $ObjectID 69 Version "20.1.0" DisabledProps [] Description "" @@ -1200,7 +1532,7 @@ Model { SupportModelReferenceSimTargetCustomCode off } Simulink.SFSimCC { - $ObjectID 38 + $ObjectID 70 Version "20.1.0" DisabledProps [] Description "" @@ -1244,7 +1576,7 @@ Model { } Simulink.RTWCC { $BackupClass "Simulink.RTWCC" - $ObjectID 39 + $ObjectID 71 Version "20.1.0" Array { Type "Cell" @@ -1350,7 +1682,7 @@ Model { Type "Handle" Dimension 2 Simulink.CodeAppCC { - $ObjectID 40 + $ObjectID 72 Version "20.1.0" Array { Type "Cell" @@ -1440,7 +1772,7 @@ Model { } Simulink.GRTTargetCC { $BackupClass "Simulink.TargetCC" - $ObjectID 41 + $ObjectID 73 Version "20.1.0" Array { Type "Cell" @@ -1470,7 +1802,7 @@ Model { Type "Handle" Dimension 1 Simulink.CPPComponent { - $ObjectID 42 + $ObjectID 74 Version "20.1.0" Array { Type "Cell" @@ -1581,7 +1913,7 @@ Model { } } SlCovCC.ConfigComp { - $ObjectID 43 + $ObjectID 75 Version "20.1.0" DisabledProps [] Description "Simulink Coverage Configuration Component" @@ -1630,11 +1962,11 @@ Model { } Simulink.ConfigSet { $PropName "ActiveConfigurationSet" - $ObjectID 31 + $ObjectID 63 } Object { $PropName "DataTransfer" - $ObjectID 44 + $ObjectID 76 $ClassName "Simulink.GlobalDataTransfer" DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" @@ -1989,16 +2321,20 @@ Model { StateMustResolveToSignalObject off HasFrameUpgradeWarning off } + Block { + BlockType ZeroOrderHold + SampleTime "1" + } } System { Name "torqueControlBalancingWithSimu" Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] - Open on + Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -2008,9 +2344,9 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "239" + ZoomFactor "175" ReportName "simulink-default.rpt" - SIDHighWatermark "4968" + SIDHighWatermark "4998" SimulinkSubDomain "Simulink" Block { BlockType BusCreator @@ -2053,7 +2389,7 @@ Model { Name "Bus\nSelector2" SID "4945" Ports [1, 6] - Position [760, 404, 765, 596] + Position [760, 419, 765, 611] ZOrder 1277 OutputSignals "joints_position,joints_velocity,nuDot,signal6,wrench_RFoot,wrench_LFoot" Port { @@ -2088,6 +2424,7 @@ Model { Ports [] Position [955, 325, 1020, 345] ZOrder 553 + ForegroundColor "magenta" HideAutomaticName off LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/Configuration" @@ -2109,13 +2446,13 @@ Model { Ports [] Position [955, 266, 1075, 294] ZOrder 547 - ForegroundColor "blue" - BackgroundColor "[0.333333, 1.000000, 1.000000]" + ForegroundColor "red" + BackgroundColor "yellow" ShowName off RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 45 + $ObjectID 77 $ClassName "Simulink.Mask" Display "disp('VISUALIZER')" } @@ -2127,7 +2464,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -2137,7 +2474,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "267" + ZoomFactor "182" SimulinkSubDomain "Simulink" Block { BlockType SubSystem @@ -2146,6 +2483,8 @@ Model { Ports [4, 0, 1] Position [260, 313, 435, 387] ZOrder 899 + ForegroundColor "red" + BackgroundColor "yellow" RequestExecContextInheritance off System { Name "Desired and Measured Forces" @@ -2155,7 +2494,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -2165,7 +2504,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "240" + ZoomFactor "156" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -2286,7 +2625,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -2296,7 +2635,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "300" + ZoomFactor "205" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -2570,7 +2909,10 @@ Model { Ports [2, 1] Position [-40, 420, -20, 440] ZOrder 704 + ForegroundColor "red" + BackgroundColor "yellow" ShowName off + HideAutomaticName off Inputs "-+|" Port { PortNumber 1 @@ -3246,7 +3588,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -3748,7 +4090,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -3860,13 +4202,13 @@ Model { RequestExecContextInheritance off System { Name "Visualizer" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -3876,7 +4218,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "185" + ZoomFactor "120" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -4687,26 +5029,11 @@ Model { Gain "180/pi" } Block { - BlockType Reference - Name "GetMeasurement1" - SID "4543" - Ports [0, 1] - Position [50, 106, 115, 134] - ZOrder 870 - BackgroundColor "[0.827500, 0.576500, 0.603900]" - ShowName off - HideAutomaticName off - LibraryVersion "1.684" - SourceBlock "WBToolboxLibrary/States/GetMeasurement" - SourceType "GetMeasurement" - SourceProductName "WholeBodyToolbox" - RTWMemSecFuncInitTerm "Inherit from model" - RTWMemSecFuncExecute "Inherit from model" - RTWMemSecDataConstants "Inherit from model" - RTWMemSecDataInternal "Inherit from model" - RTWMemSecDataParameters "Inherit from model" - ContentPreviewEnabled off - measuredType "Joints Torque" + BlockType Ground + Name "Ground" + SID "4971" + Position [70, 110, 90, 130] + ZOrder 897 } Block { BlockType Scope @@ -5416,10 +5743,10 @@ Model { DstPort 3 } Line { - ZOrder 480 - SrcBlock "GetMeasurement1" + ZOrder 935 + SrcBlock "Ground" SrcPort 1 - Points [100, 0] + Points [125, 0] Branch { ZOrder 831 DstBlock "Sum" @@ -6220,22 +6547,20 @@ Model { BlockType SubSystem Name "MOMENTUM BASED TORQUE CONTROL" SID "4836" - Ports [6, 1] - Position [955, 409, 1105, 591] + Ports [7, 1] + Position [955, 391, 1105, 609] ZOrder 961 - ForegroundColor "red" - BackgroundColor "yellow" 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 + Open on PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -6245,61 +6570,76 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "182" + ZoomFactor "174" SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "neckPos" + SID "4977" + Position [160, 353, 190, 367] + ZOrder 971 + SampleTime "Config.tStep" + } Block { BlockType Inport Name "jointPos" SID "4837" - Position [160, 288, 190, 302] + Position [160, 258, 190, 272] ZOrder 556 + Port "2" + SampleTime "Config.tStep" } Block { BlockType Inport Name "jointVel" SID "4838" - Position [160, 418, 190, 432] + Position [160, 453, 190, 467] ZOrder 557 - Port "2" + Port "3" + SampleTime "Config.tStep" } Block { BlockType Inport Name "jointAcc" SID "4966" - Position [160, 178, 190, 192] + Position [160, 148, 190, 162] ZOrder 969 - Port "3" + Port "4" + SampleTime "Config.tStep" } Block { BlockType Inport Name "IMU_meas" SID "4840" - Position [160, 478, 190, 492] + Position [160, 503, 190, 517] ZOrder 559 - Port "4" + Port "5" + SampleTime "Config.tStep" } Block { BlockType Inport Name "wrench_rightFoot" SID "4880" - Position [160, 538, 190, 552] + Position [160, 553, 190, 567] ZOrder 963 - Port "5" + Port "6" + SampleTime "Config.tStep" } Block { BlockType Inport Name "wrench_leftFoot" SID "4881" - Position [160, 598, 190, 612] + Position [160, 603, 190, 617] ZOrder 964 - Port "6" + Port "7" + SampleTime "Config.tStep" } Block { BlockType SubSystem Name "Balancing Controller QP" SID "2355" - Ports [20, 1] - Position [800, 31, 955, 609] + Ports [21, 1] + Position [800, 3, 955, 607] ZOrder 542 BackgroundColor "lightBlue" RequestExecContextInheritance off @@ -6307,11 +6647,11 @@ Model { Name "Balancing Controller QP" Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] - Open off + Open on PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -6418,13 +6758,21 @@ Model { 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 "13" + Port "14" } Block { BlockType Inport @@ -6432,7 +6780,7 @@ Model { SID "4387" Position [1065, 318, 1095, 332] ZOrder 840 - Port "14" + Port "15" } Block { BlockType Inport @@ -6440,7 +6788,7 @@ Model { SID "2363" Position [1065, -102, 1095, -88] ZOrder 386 - Port "15" + Port "16" } Block { BlockType Inport @@ -6448,7 +6796,7 @@ Model { SID "2356" Position [1065, 843, 1095, 857] ZOrder 336 - Port "16" + Port "17" } Block { BlockType Inport @@ -6456,7 +6804,7 @@ Model { SID "3328" Position [1065, 753, 1095, 767] ZOrder 644 - Port "17" + Port "18" } Block { BlockType Inport @@ -6464,7 +6812,7 @@ Model { SID "3329" Position [1065, 798, 1095, 812] ZOrder 645 - Port "18" + Port "19" } Block { BlockType Inport @@ -6472,7 +6820,7 @@ Model { SID "2365" Position [1065, 708, 1095, 722] ZOrder 335 - Port "19" + Port "20" } Block { BlockType Inport @@ -6480,7 +6828,7 @@ Model { SID "2364" Position [1065, 78, 1095, 92] ZOrder 324 - Port "20" + Port "21" } Block { BlockType SubSystem @@ -6498,7 +6846,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -6669,7 +7017,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -6799,7 +7147,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -6911,7 +7259,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -6992,7 +7340,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -7142,7 +7490,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -7446,7 +7794,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -7519,7 +7867,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -7669,7 +8017,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -8327,7 +8675,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -8417,7 +8765,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -8609,7 +8957,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -8811,7 +9159,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -9127,8 +9475,8 @@ Model { BlockType SubSystem Name "From tau_QP to Joint Torques (motor reflected inertia)" SID "4519" - Ports [1, 1] - Position [2065, 51, 2220, 109] + Ports [2, 1] + Position [2145, 66, 2300, 124] ZOrder 862 RequestExecContextInheritance off System { @@ -9139,7 +9487,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -9149,7 +9497,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "338" + ZoomFactor "157" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -9158,6 +9506,14 @@ Model { 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 " " @@ -9187,6 +9543,7 @@ Model { ErrorFcn "Stateflow.Translate.translate" PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on + SystemSampleTime "Config.tStep" RequestExecContextInheritance off SFBlockType "MATLAB Function" System { @@ -9197,7 +9554,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -9315,17 +9672,6 @@ Model { GotoTag "jointAcc_des" TagVisibility "global" } - Block { - BlockType From - Name "From1" - SID "4830" - Position [-460, -108, -380, -92] - ZOrder 903 - ShowName off - HideAutomaticName off - GotoTag "jointAcc" - TagVisibility "global" - } Block { BlockType Gain Name "Gain" @@ -9460,8 +9806,8 @@ Model { DstPort 1 } Line { - ZOrder 115 - SrcBlock "From1" + ZOrder 116 + SrcBlock "jointAcc" SrcPort 1 DstBlock "Switch1" DstPort 3 @@ -9472,7 +9818,7 @@ Model { BlockType Goto Name "Goto1" SID "4495" - Position [2320, 16, 2390, 34] + Position [2400, 31, 2470, 49] ZOrder 856 ShowName off HideAutomaticName off @@ -9511,7 +9857,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -10167,7 +10513,7 @@ Model { BlockType Outport Name "tau_star " SID "2414" - Position [2340, 73, 2370, 87] + Position [2420, 88, 2450, 102] ZOrder 279 VectorParamsAs1DForOutWhenUnconnected off } @@ -10487,6 +10833,13 @@ Model { 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 { @@ -10494,7 +10847,7 @@ Model { Name "Dynamics and Kinematics" SID "2341" Ports [3, 11] - Position [605, 23, 735, 347] + Position [605, -7, 735, 317] ZOrder 541 BackgroundColor "[0.000000, 1.000000, 0.498039]" NameLocation "top" @@ -10507,7 +10860,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -10517,7 +10870,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "139" + ZoomFactor "158" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -10558,7 +10911,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -10609,7 +10962,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -10648,7 +11001,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -11069,7 +11422,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -11881,34 +12234,24 @@ Model { } } } - Block { - BlockType Goto - Name "GoTo Motors Inertia" - SID "4967" - Position [305, 177, 390, 193] - ZOrder 970 - HideAutomaticName off - GotoTag "jointAcc" - TagVisibility "global" - } Block { BlockType SubSystem Name "Joint Torque Saturation" SID "4553" Ports [1, 1] - Position [995, 301, 1105, 339] + Position [1000, 286, 1110, 324] ZOrder 555 BackgroundColor "orange" RequestExecContextInheritance off System { Name "Joint Torque Saturation" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] - Open off + Open on PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -11918,7 +12261,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "502" + ZoomFactor "283" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -11988,7 +12331,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -12151,24 +12494,30 @@ Model { } Block { BlockType Reference - Name "holder\n" - SID "4559" + Name "holder 7" + SID "4996" Ports [1, 1] - Position [-20, 13, 15, 27] - ZOrder 15 - BackgroundColor "[0.537255, 0.721569, 1.000000]" + Position [-25, 12, 25, 28] + ZOrder 725 ShowName off HideAutomaticName off - LibraryVersion "1.684" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "WholeBodyToolbox" + 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" - ContentPreviewEnabled off + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off } Block { BlockType Outport @@ -12206,14 +12555,14 @@ Model { ZOrder 7 Points [0, -140] Branch { - ZOrder 26 - Points [0, -20] - DstBlock "Saturate Torque Derivative" + ZOrder 36 + DstBlock "holder 7" DstPort 1 } Branch { - ZOrder 16 - DstBlock "holder\n" + ZOrder 26 + Points [0, -20] + DstBlock "Saturate Torque Derivative" DstPort 1 } } @@ -12226,8 +12575,8 @@ Model { DstPort 1 } Line { - ZOrder 8 - SrcBlock "holder\n" + ZOrder 35 + SrcBlock "holder 7" SrcPort 1 DstBlock "Saturate Torque Derivative" DstPort 2 @@ -12269,7 +12618,7 @@ Model { BlockType SubSystem Name "Robot State and References" SID "2087" - Ports [5, 9] + Ports [6, 9] Position [285, 338, 445, 632] ZOrder 548 BackgroundColor "orange" @@ -12283,7 +12632,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -12293,14 +12642,22 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "244" + ZoomFactor "166" 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, 2678, -550, 2692] + Position [-580, 2703, -550, 2717] ZOrder 959 + Port "2" } Block { BlockType Inport @@ -12308,15 +12665,15 @@ Model { SID "4833" Position [80, 2523, 110, 2537] ZOrder 958 - Port "2" + Port "3" } Block { BlockType Inport Name "IMU_meas" SID "4835" - Position [-580, 2733, -550, 2747] + Position [-580, 2743, -550, 2757] ZOrder 960 - Port "3" + Port "4" } Block { BlockType Inport @@ -12324,7 +12681,7 @@ Model { SID "4878" Position [-580, 2788, -550, 2802] ZOrder 961 - Port "4" + Port "5" } Block { BlockType Inport @@ -12332,7 +12689,7 @@ Model { SID "4879" Position [-580, 2843, -550, 2857] ZOrder 962 - Port "5" + Port "6" } Block { BlockType SubSystem @@ -12344,13 +12701,13 @@ Model { RequestExecContextInheritance off System { Name "Compute State Velocity" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -12360,7 +12717,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "517" + ZoomFactor "241" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -12415,7 +12772,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -12565,13 +12922,13 @@ Model { RequestExecContextInheritance off System { Name "Feet Jacobians" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -12581,7 +12938,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "937" + ZoomFactor "527" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -12794,19 +13151,19 @@ Model { BlockType SubSystem Name "Compute base to fixed link transform" SID "4307" - Ports [2, 2] - Position [-455, 2656, -330, 2769] + Ports [3, 2] + Position [-455, 2653, -330, 2767] ZOrder 902 RequestExecContextInheritance off System { Name "Compute base to fixed link transform" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -12816,14 +13173,22 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "553" + 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 @@ -12831,7 +13196,7 @@ Model { SID "4311" Position [245, 183, 275, 197] ZOrder 903 - Port "2" + Port "3" } Block { BlockType Constant @@ -12856,19 +13221,20 @@ Model { BlockType SubSystem Name "LFoot to base link transform " SID "4245" - Ports [4, 1] - Position [455, 15, 600, 55] + Ports [5, 1] + Position [455, 4, 600, 56] ZOrder 893 + ForegroundColor "yellow" RequestExecContextInheritance off System { Name "LFoot to base link transform " - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -12878,14 +13244,22 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "248" + 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 @@ -12893,7 +13267,7 @@ Model { SID "4247" Position [1000, 212, 1030, 228] ZOrder 741 - Port "2" + Port "3" } Block { BlockType Inport @@ -12901,7 +13275,7 @@ Model { SID "4248" Position [655, 132, 680, 148] ZOrder 744 - Port "3" + Port "4" } Block { BlockType Inport @@ -12909,15 +13283,15 @@ Model { SID "4249" Position [470, 32, 495, 48] ZOrder 745 - Port "4" + Port "5" } Block { BlockType Reference - Name "Fixed base to imu transform" - SID "4250" + Name "Fixe d base to root link transform" + SID "4251" Ports [2, 1] - Position [575, 25, 740, 85] - ZOrder 729 + Position [575, 190, 740, 250] + ZOrder 737 LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" @@ -12928,15 +13302,15 @@ Model { RTWMemSecDataInternal "Inherit from model" RTWMemSecDataParameters "Inherit from model" ContentPreviewEnabled off - frameName "Frames.IMU" + frameName "Frames.BASE" } Block { BlockType Reference - Name "Fixed base to root link transform" - SID "4251" + Name "Fixed base to imu transform" + SID "4250" Ports [2, 1] - Position [575, 190, 740, 250] - ZOrder 737 + Position [575, 25, 740, 85] + ZOrder 729 LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" SourceType "ForwardKinematics" @@ -12947,7 +13321,7 @@ Model { RTWMemSecDataInternal "Inherit from model" RTWMemSecDataParameters "Inherit from model" ContentPreviewEnabled off - frameName "Frames.BASE" + frameName "Frames.IMU" } Block { BlockType SubSystem @@ -12971,7 +13345,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -13175,27 +13549,6 @@ Model { } } Block { - BlockType Reference - Name "Neck Position" - SID "4254" - Ports [0, 1] - Position [1000, 310, 1035, 330] - ZOrder 742 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.684" - SourceBlock "WBToolboxLibrary/Utilities/YarpRead" - SourceType "YARP Read" - SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" - portName "Ports.NECK_POS" - signalSize "Ports.NECK_POS_PORT_SIZE" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - Block { BlockType Selector Name "Selector1" SID "4849" @@ -13243,61 +13596,71 @@ Model { } Block { BlockType Reference - Name "holder\n1" - SID "4258" + Name "holder 1" + SID "4982" Ports [1, 1] - Position [1175, 113, 1225, 127] - ZOrder 739 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.684" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "WholeBodyToolbox" + 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" - ContentPreviewEnabled off + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off } Block { BlockType Reference - Name "holder\n2" - SID "4259" + Name "holder 2" + SID "4983" Ports [1, 1] - Position [1175, 213, 1225, 227] - ZOrder 740 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.684" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "WholeBodyToolbox" + 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" - ContentPreviewEnabled off + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off } Block { BlockType SubSystem Name "neck transform" - SID "4260" + SID "4987" Ports [1, 1] - Position [1135, 305, 1260, 335] - ZOrder 743 + Position [1125, 305, 1250, 335] + ZOrder 756 RequestExecContextInheritance off System { Name "neck transform" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -13307,55 +13670,33 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "843" + ZoomFactor "413" SimulinkSubDomain "Simulink" Block { BlockType Inport Name "neck port" - SID "4261" - Position [45, 18, 75, 32] + SID "4988" + Position [120, 18, 150, 32] ZOrder 725 NameLocation "top" + Port { + PortNumber 1 + Name "neck pos" + } } Block { BlockType Constant Name "Constant" - SID "4262" + SID "4989" Position [95, 85, 170, 105] ZOrder 720 ShowName off Value "zeros(3,1)" } Block { - BlockType Gain - Name "Gain" - SID "4263" - Position [105, 10, 160, 40] - ZOrder 724 - ShowName off - Gain "pi/180" - } - Block { - BlockType Selector - Name "Selector1" - SID "4264" - Ports [1, 1] - Position [190, 13, 230, 37] - ZOrder 723 - ShowName off - InputPortWidth "Ports.NECK_POS_PORT_SIZE" - IndexOptions "Index vector (dialog)" - Indices "[1:3]" - OutputSizes "1" - Port { - PortNumber 1 - Name "neck pos" - } - } - Block { BlockType Switch Name "Switch" - SID "4265" + SID "4990" Position [300, 7, 375, 113] ZOrder 722 ShowName off @@ -13366,7 +13707,7 @@ Model { Block { BlockType Constant Name "USE_IMU4EST_BASE1" - SID "4266" + SID "4991" Position [60, 50, 210, 70] ZOrder 719 ShowName off @@ -13375,41 +13716,27 @@ Model { Block { BlockType Outport Name "neck position" - SID "4267" + SID "4992" Position [425, 53, 455, 67] ZOrder 726 VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 - SrcBlock "neck port" - SrcPort 1 - DstBlock "Gain" - DstPort 1 - } - Line { - ZOrder 2 SrcBlock "USE_IMU4EST_BASE1" SrcPort 1 DstBlock "Switch" DstPort 2 } Line { - ZOrder 3 + ZOrder 2 SrcBlock "Switch" SrcPort 1 DstBlock "neck position" DstPort 1 } Line { - ZOrder 4 - SrcBlock "Gain" - SrcPort 1 - DstBlock "Selector1" - DstPort 1 - } - Line { - ZOrder 5 + ZOrder 3 SrcBlock "Constant" SrcPort 1 DstBlock "Switch" @@ -13417,9 +13744,9 @@ Model { } Line { Name "neck pos" - ZOrder 6 - Labels [1, 1] - SrcBlock "Selector1" + ZOrder 4 + Labels [-1, 0] + SrcBlock "neck port" SrcPort 1 DstBlock "Switch" DstPort 1 @@ -13476,20 +13803,20 @@ Model { SrcPort 1 Points [65, 0] Branch { - ZOrder 7 - DstBlock "Get Base Rotation From IMU" + ZOrder 41 + Points [0, 50] + DstBlock "holder 1" DstPort 1 } Branch { - ZOrder 8 - Points [0, 50] - DstBlock "holder\n1" + ZOrder 7 + DstBlock "Get Base Rotation From IMU" DstPort 1 } } Line { - ZOrder 9 - SrcBlock "holder\n2" + ZOrder 42 + SrcBlock "holder 2" SrcPort 1 DstBlock "Selector2" DstPort 1 @@ -13514,7 +13841,7 @@ Model { } Branch { ZOrder 14 - DstBlock "Fixed base to root link transform" + DstBlock "Fixe d base to root link transform" DstPort 2 } } @@ -13527,7 +13854,7 @@ Model { } Line { ZOrder 16 - SrcBlock "Fixed base to root link transform" + SrcBlock "Fixe d base to root link transform" SrcPort 1 DstBlock "Inv_2" DstPort 2 @@ -13538,32 +13865,25 @@ Model { SrcPort 1 Points [96, 0] Branch { - ZOrder 18 - DstBlock "holder\n2" + ZOrder 64 + Points [0, 50] + DstBlock "Selector1" DstPort 1 } Branch { - ZOrder 19 - Points [0, 50] - DstBlock "Selector1" + ZOrder 43 + DstBlock "holder 2" DstPort 1 } } Line { - ZOrder 20 - SrcBlock "Neck Position" + ZOrder 36 + SrcBlock "neck_pos" SrcPort 1 DstBlock "neck transform" DstPort 1 } Line { - ZOrder 21 - SrcBlock "neck transform" - SrcPort 1 - DstBlock "Get Base Rotation From IMU" - DstPort 6 - } - Line { ZOrder 22 SrcBlock "w_H_link" SrcPort 1 @@ -13589,7 +13909,7 @@ Model { Branch { ZOrder 26 Points [0, 165] - DstBlock "Fixed base to root link transform" + DstBlock "Fixe d base to root link transform" DstPort 1 } Branch { @@ -13606,8 +13926,8 @@ Model { DstPort 4 } Line { - ZOrder 30 - SrcBlock "holder\n1" + ZOrder 37 + SrcBlock "holder 1" SrcPort 1 DstBlock "Get Base Rotation From IMU" DstPort 2 @@ -13619,25 +13939,33 @@ Model { 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 [4, 1] - Position [455, 100, 600, 140] + Ports [5, 1] + Position [455, 87, 600, 143] ZOrder 956 + ForegroundColor "yellow" RequestExecContextInheritance off System { Name "RFoot to base link transform" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -13647,14 +13975,22 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "248" + 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 @@ -13662,7 +13998,7 @@ Model { SID "4852" Position [1000, 212, 1030, 228] ZOrder 741 - Port "2" + Port "3" } Block { BlockType Inport @@ -13670,7 +14006,7 @@ Model { SID "4853" Position [655, 132, 680, 148] ZOrder 744 - Port "3" + Port "4" } Block { BlockType Inport @@ -13678,7 +14014,7 @@ Model { SID "4854" Position [470, 32, 495, 48] ZOrder 745 - Port "4" + Port "5" } Block { BlockType Reference @@ -13740,7 +14076,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -13944,27 +14280,6 @@ Model { } } Block { - BlockType Reference - Name "Neck Position" - SID "4860" - Ports [0, 1] - Position [1000, 310, 1035, 330] - ZOrder 742 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.684" - SourceBlock "WBToolboxLibrary/Utilities/YarpRead" - SourceType "YARP Read" - SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" - portName "Ports.NECK_POS" - signalSize "Ports.NECK_POS_PORT_SIZE" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - Block { BlockType Selector Name "Selector1" SID "4861" @@ -14012,43 +14327,53 @@ Model { } Block { BlockType Reference - Name "holder\n1" - SID "4865" + Name "holder 3" + SID "4984" Ports [1, 1] - Position [1175, 113, 1225, 127] - ZOrder 739 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.684" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "WholeBodyToolbox" + 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" - ContentPreviewEnabled off + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off } Block { BlockType Reference - Name "holder\n2" - SID "4866" + Name "holder 4" + SID "4985" Ports [1, 1] - Position [1175, 213, 1225, 227] - ZOrder 740 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.684" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "WholeBodyToolbox" + 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" - ContentPreviewEnabled off + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off } Block { BlockType SubSystem @@ -14060,13 +14385,13 @@ Model { RequestExecContextInheritance off System { Name "neck transform" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -14076,15 +14401,19 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "843" + ZoomFactor "413" SimulinkSubDomain "Simulink" Block { BlockType Inport Name "neck port" SID "4868" - Position [45, 18, 75, 32] + Position [120, 18, 150, 32] ZOrder 725 NameLocation "top" + Port { + PortNumber 1 + Name "neck pos" + } } Block { BlockType Constant @@ -14092,34 +14421,8 @@ Model { SID "4869" Position [95, 85, 170, 105] ZOrder 720 - ShowName off - Value "zeros(3,1)" - } - Block { - BlockType Gain - Name "Gain" - SID "4870" - Position [105, 10, 160, 40] - ZOrder 724 - ShowName off - Gain "pi/180" - } - Block { - BlockType Selector - Name "Selector1" - SID "4871" - Ports [1, 1] - Position [190, 13, 230, 37] - ZOrder 723 - ShowName off - InputPortWidth "Ports.NECK_POS_PORT_SIZE" - IndexOptions "Index vector (dialog)" - Indices "[1:3]" - OutputSizes "1" - Port { - PortNumber 1 - Name "neck pos" - } + ShowName off + Value "zeros(3,1)" } Block { BlockType Switch @@ -14150,13 +14453,6 @@ Model { VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 1 - SrcBlock "neck port" - SrcPort 1 - DstBlock "Gain" - DstPort 1 - } - Line { ZOrder 2 SrcBlock "USE_IMU4EST_BASE1" SrcPort 1 @@ -14171,13 +14467,6 @@ Model { DstPort 1 } Line { - ZOrder 4 - SrcBlock "Gain" - SrcPort 1 - DstBlock "Selector1" - DstPort 1 - } - Line { ZOrder 5 SrcBlock "Constant" SrcPort 1 @@ -14186,9 +14475,9 @@ Model { } Line { Name "neck pos" - ZOrder 6 - Labels [1, 1] - SrcBlock "Selector1" + ZOrder 8 + Labels [-1, 0] + SrcBlock "neck port" SrcPort 1 DstBlock "Switch" DstPort 1 @@ -14245,20 +14534,20 @@ Model { SrcPort 1 Points [65, 0] Branch { - ZOrder 7 - DstBlock "Get Base Rotation From IMU" + ZOrder 31 + Points [0, 50] + DstBlock "holder 3" DstPort 1 } Branch { - ZOrder 8 - Points [0, 50] - DstBlock "holder\n1" + ZOrder 7 + DstBlock "Get Base Rotation From IMU" DstPort 1 } } Line { - ZOrder 9 - SrcBlock "holder\n2" + ZOrder 32 + SrcBlock "holder 4" SrcPort 1 DstBlock "Selector2" DstPort 1 @@ -14307,8 +14596,8 @@ Model { SrcPort 1 Points [96, 0] Branch { - ZOrder 17 - DstBlock "holder\n2" + ZOrder 33 + DstBlock "holder 4" DstPort 1 } Branch { @@ -14319,8 +14608,8 @@ Model { } } Line { - ZOrder 19 - SrcBlock "Neck Position" + ZOrder 35 + SrcBlock "neck_pos" SrcPort 1 DstBlock "neck transform" DstPort 1 @@ -14375,8 +14664,8 @@ Model { DstPort 4 } Line { - ZOrder 28 - SrcBlock "holder\n1" + ZOrder 30 + SrcBlock "holder 3" SrcPort 1 DstBlock "Get Base Rotation From IMU" DstPort 2 @@ -14432,7 +14721,7 @@ Model { BlockType Outport Name "fixed_l_sole_H_b" SID "4318" - Position [650, 28, 680, 42] + Position [650, 23, 680, 37] ZOrder 910 VectorParamsAs1DForOutWhenUnconnected off } @@ -14440,7 +14729,7 @@ Model { BlockType Outport Name "fixed_r_sole_H_b" SID "4320" - Position [650, 113, 680, 127] + Position [650, 108, 680, 122] ZOrder 912 Port "2" VectorParamsAs1DForOutWhenUnconnected off @@ -14454,15 +14743,15 @@ Model { ZOrder 2 Points [0, -22; 255, 0; 0, 42] Branch { - ZOrder 34 + ZOrder 59 Points [0, 85] DstBlock "RFoot to base link transform" - DstPort 4 + DstPort 5 } Branch { ZOrder 4 DstBlock "LFoot to base link transform " - DstPort 4 + DstPort 5 } } Branch { @@ -14496,14 +14785,14 @@ Model { SrcBlock "Relative transform l_sole_H_base" SrcPort 1 DstBlock "LFoot to base link transform " - DstPort 3 + DstPort 4 } Line { ZOrder 33 SrcBlock "Relative transform r_sole_H_base" SrcPort 1 DstBlock "RFoot to base link transform" - DstPort 3 + DstPort 4 } Line { ZOrder 11 @@ -14526,15 +14815,15 @@ Model { ZOrder 21 Points [274, 0; 0, -62] Branch { - ZOrder 31 - DstBlock "RFoot to base link transform" - DstPort 1 - } - Branch { - ZOrder 14 + ZOrder 70 Points [0, -85] DstBlock "LFoot to base link transform " - DstPort 1 + DstPort 2 + } + Branch { + ZOrder 31 + DstBlock "RFoot to base link transform" + DstPort 2 } } } @@ -14555,16 +14844,33 @@ Model { 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 2 + DstPort 3 } + } + Line { + ZOrder 64 + SrcBlock "neckPos" + SrcPort 1 + Points [343, 0; 0, 35] Branch { - ZOrder 19 - Points [0, -85] + ZOrder 74 + Points [0, 85] + DstBlock "RFoot to base link transform" + DstPort 1 + } + Branch { + ZOrder 73 DstBlock "LFoot to base link transform " - DstPort 2 + DstPort 1 } } } @@ -14602,13 +14908,13 @@ Model { RequestExecContextInheritance off System { Name "State Machine" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -14618,7 +14924,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "292" + ZoomFactor "200" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -14782,7 +15088,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -15212,45 +15518,63 @@ Model { } } } + Block { + BlockType ZeroOrderHold + Name "Zero-Order\nHold" + SID "4995" + Position [-110, 295, -85, 315] + ZOrder 959 + SampleTime "Config.tStep" + } Block { BlockType Reference - Name "holder\n1" - SID "2187" + Name "holder 5" + SID "4993" Ports [1, 1] - Position [-170, 122, -110, 138] - ZOrder 583 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.684" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "WholeBodyToolbox" + 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" - ContentPreviewEnabled off + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off } Block { BlockType Reference - Name "holder\n2" - SID "2188" + Name "holder 6" + SID "4994" Ports [1, 1] - Position [-170, 157, -110, 173] - ZOrder 584 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.684" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "WholeBodyToolbox" + 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" - ContentPreviewEnabled off + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + ContentPreviewEnabled on + Latency "0" + AutoFrameSizeCalculation off } Block { BlockType SubSystem @@ -15263,13 +15587,13 @@ Model { RequestExecContextInheritance off System { Name "xCom" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -15279,7 +15603,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "824" + ZoomFactor "464" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -15385,7 +15709,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -15579,9 +15903,9 @@ Model { SrcPort 1 Points [13, 0] Branch { - ZOrder 416 + ZOrder 513 Points [0, -70] - DstBlock "holder\n1" + DstBlock "holder 5" DstPort 1 } Branch { @@ -15718,16 +16042,16 @@ Model { 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 205 - Points [0, -40] - DstBlock "holder\n2" - DstPort 1 - } } Branch { ZOrder 256 @@ -15774,8 +16098,8 @@ Model { ZOrder 408 SrcBlock "Clock1" SrcPort 1 - DstBlock "STATE MACHINE" - DstPort 6 + DstBlock "Zero-Order\nHold" + DstPort 1 } Line { ZOrder 500 @@ -15795,15 +16119,15 @@ Model { } } Line { - ZOrder 411 - SrcBlock "holder\n1" + ZOrder 514 + SrcBlock "holder 5" SrcPort 1 DstBlock "STATE MACHINE" DstPort 1 } Line { - ZOrder 412 - SrcBlock "holder\n2" + ZOrder 516 + SrcBlock "holder 6" SrcPort 1 DstBlock "STATE MACHINE" DstPort 2 @@ -15856,6 +16180,13 @@ Model { DstPort 7 } } + Line { + ZOrder 517 + SrcBlock "Zero-Order\nHold" + SrcPort 1 + DstBlock "STATE MACHINE" + DstPort 6 + } } } Block { @@ -15874,7 +16205,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -15973,13 +16304,13 @@ Model { RequestExecContextInheritance off System { Name "Reshape Gains Matrices" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -15989,7 +16320,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "777" + ZoomFactor "532" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -16037,7 +16368,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -16154,7 +16485,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -16271,7 +16602,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -16460,13 +16791,13 @@ Model { RequestExecContextInheritance off System { Name "Smooth reference CoM" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -16476,7 +16807,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "357" + ZoomFactor "245" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -16689,7 +17020,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -16699,7 +17030,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "426" + ZoomFactor "292" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -17253,7 +17584,7 @@ Model { SrcBlock "IMU_meas" SrcPort 1 DstBlock "Compute base to fixed link transform" - DstPort 2 + DstPort 3 } Line { ZOrder 672 @@ -17261,8 +17592,8 @@ Model { SrcPort 1 Points [31, 0] Branch { - ZOrder 674 - Points [0, -55] + ZOrder 723 + Points [0, -80] Branch { ZOrder 692 Points [0, -130] @@ -17278,7 +17609,7 @@ Model { Branch { ZOrder 673 DstBlock "Compute base to fixed link transform" - DstPort 1 + DstPort 2 } } Line { @@ -17302,6 +17633,13 @@ Model { DstBlock "Compute State Velocity" DstPort 2 } + Line { + ZOrder 724 + SrcBlock "neckPos" + SrcPort 1 + DstBlock "Compute base to fixed link transform" + DstPort 1 + } } } Block { @@ -17309,15 +17647,14 @@ Model { Name "emergency stop: joint limits" SID "4913" Ports [1] - Position [305, 241, 430, 269] + Position [305, 211, 430, 239] ZOrder 966 - ForegroundColor "red" - BackgroundColor "yellow" + BackgroundColor "red" ShowName off RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 46 + $ObjectID 78 $ClassName "Simulink.Mask" Display "disp('EMERGENCY STOP')" RunInitForIconRedraw "off" @@ -17330,7 +17667,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -17340,7 +17677,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "401" + ZoomFactor "330" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -17380,8 +17717,6 @@ Model { Ports [1, 0, 1] Position [285, 229, 440, 261] ZOrder 554 - ForegroundColor "red" - BackgroundColor "yellow" RequestExecContextInheritance off System { Name "STOP IF JOINTS HIT THE LIMITS" @@ -17391,7 +17726,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -17457,8 +17792,6 @@ Model { Ports [4, 2] Position [180, 22, 300, 163] ZOrder 205 - ForegroundColor "red" - BackgroundColor "yellow" ShowName off LibraryVersion "1.32" ErrorFcn "Stateflow.Translate.translate" @@ -17474,7 +17807,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -17739,7 +18072,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -17798,7 +18131,7 @@ Model { PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -18040,7 +18373,7 @@ Model { BlockType Outport Name "jointTorques_star" SID "4841" - Position [1160, 313, 1190, 327] + Position [1160, 298, 1190, 312] ZOrder 560 VectorParamsAs1DForOutWhenUnconnected off } @@ -18092,30 +18425,30 @@ Model { SrcPort 2 Points [72, 0] Branch { - ZOrder 286 - DstBlock "Balancing Controller QP" - DstPort 13 - } - Branch { - ZOrder 284 - Points [0, -210] + 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 18 + DstPort 19 } Line { ZOrder 279 SrcBlock "jointVel" SrcPort 1 DstBlock "Robot State and References" - DstPort 2 + DstPort 3 } Line { ZOrder 282 @@ -18136,7 +18469,7 @@ Model { SrcBlock "Robot State and References" SrcPort 3 DstBlock "Balancing Controller QP" - DstPort 14 + DstPort 15 } Line { ZOrder 204 @@ -18164,7 +18497,7 @@ Model { SrcBlock "Robot State and References" SrcPort 6 DstBlock "Balancing Controller QP" - DstPort 17 + DstPort 18 } Line { ZOrder 92 @@ -18178,27 +18511,27 @@ Model { SrcBlock "Robot State and References" SrcPort 9 DstBlock "Balancing Controller QP" - DstPort 20 + DstPort 21 } Line { ZOrder 130 SrcBlock "Robot State and References" SrcPort 4 DstBlock "Balancing Controller QP" - DstPort 15 + DstPort 16 } Line { ZOrder 281 SrcBlock "IMU_meas" SrcPort 1 DstBlock "Robot State and References" - DstPort 3 + DstPort 4 } Line { ZOrder 119 SrcBlock "Robot State and References" SrcPort 1 - Points [39, 0; 0, -290] + Points [34, 0; 0, -320] DstBlock "Dynamics and Kinematics" DstPort 1 } @@ -18207,7 +18540,7 @@ Model { SrcBlock "Robot State and References" SrcPort 5 DstBlock "Balancing Controller QP" - DstPort 16 + DstPort 17 } Line { ZOrder 101 @@ -18221,7 +18554,7 @@ Model { SrcBlock "Robot State and References" SrcPort 8 DstBlock "Balancing Controller QP" - DstPort 19 + DstPort 20 } Line { ZOrder 278 @@ -18229,31 +18562,31 @@ Model { SrcPort 1 Points [36, 0] Branch { - ZOrder 322 - Points [0, -40] - DstBlock "emergency stop: joint limits" - DstPort 1 - } - Branch { - ZOrder 294 - Points [0, 70] + ZOrder 360 + Points [0, 145] DstBlock "Robot State and References" - DstPort 1 + DstPort 2 } Branch { ZOrder 293 Points [324, 0] Branch { - ZOrder 289 - DstBlock "Dynamics and Kinematics" - DstPort 3 - } - Branch { - ZOrder 283 + 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 { @@ -18261,20 +18594,28 @@ Model { SrcBlock "wrench_rightFoot" SrcPort 1 DstBlock "Robot State and References" - DstPort 4 + DstPort 5 } Line { ZOrder 314 SrcBlock "wrench_leftFoot" SrcPort 1 DstBlock "Robot State and References" - DstPort 5 + DstPort 6 } Line { ZOrder 329 SrcBlock "jointAcc" SrcPort 1 - DstBlock "GoTo Motors Inertia" + 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 } } @@ -18298,11 +18639,47 @@ Model { Name "Selector" SID "4963" Ports [1, 1] - Position [840, 473, 880, 497] + Position [840, 488, 880, 512] ZOrder 1280 InputPortWidth "6+robot_config.N_DOF" IndexOptions "Index vector (dialog)" - Indices "[7:6+robot_config.N_DOF]" + Indices "[7:6+Config.N_DOF]" + OutputSizes "1" + } + Block { + BlockType Selector + Name "Selector1" + SID "4975" + Ports [1, 1] + Position [840, 458, 880, 482] + 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, 428, 880, 452] + ZOrder 1282 + InputPortWidth "robot_config.N_DOF" + IndexOptions "Index vector (dialog)" + Indices "[1:Config.N_DOF]" + OutputSizes "1" + } + Block { + BlockType Selector + Name "Selector3" + SID "4978" + Ports [1, 1] + Position [840, 398, 880, 422] + ZOrder 1283 + InputPortWidth "robot_config.N_DOF" + IndexOptions "Index vector (dialog)" + Indices "[Config.N_DOF+1:Config.N_DOF+3]" OutputSizes "1" } Block { @@ -18312,6 +18689,8 @@ Model { Ports [1, 5] Position [1190, 427, 1385, 573] ZOrder 1279 + ForegroundColor "red" + BackgroundColor "yellow" TreatAsAtomicUnit on SystemSampleTime "Config.tStepSim" RequestExecContextInheritance off @@ -18320,11 +18699,11 @@ Model { Name "Subsystem" Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] - Open off + Open on PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" - ModelBrowserVisibility off + ModelBrowserVisibility on ModelBrowserWidth 200 ScreenColor "white" PaperOrientation "landscape" @@ -18334,7 +18713,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "307" + ZoomFactor "244" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -18437,6 +18816,8 @@ Model { Ports [3, 4] Position [180, 244, 375, 356] ZOrder 1231 + ForegroundColor "red" + BackgroundColor "yellow" LibraryVersion "3.3" SourceBlock "mwbs_robotDynamicsWithContacts_lib/RobotDynWithContacts" SourceType "" @@ -18596,6 +18977,16 @@ Model { } } } + Block { + BlockType UnitDelay + Name "Unit Delay" + SID "4997" + Position [1130, 483, 1165, 517] + ZOrder 1284 + InitialCondition "zeros(Config.N_DOF,1)" + SampleTime "Config.tStep" + HasFrameUpgradeWarning on + } Line { Name "" ZOrder 308 @@ -18656,7 +19047,7 @@ Model { ZOrder 321 SrcBlock "Bus\nCreator" SrcPort 1 - Points [19, 0; 0, 154; -1039, 0; 0, -139] + Points [19, 0; 0, 183; -1023, 0; 0, -153] DstBlock "Bus\nSelector2" DstPort 1 } @@ -18664,7 +19055,7 @@ Model { ZOrder 361 SrcBlock "MOMENTUM BASED TORQUE CONTROL" SrcPort 1 - DstBlock "Subsystem" + DstBlock "Unit Delay" DstPort 1 } Line { @@ -18710,7 +19101,7 @@ Model { SrcBlock "Bus\nSelector2" SrcPort 4 DstBlock "MOMENTUM BASED TORQUE CONTROL" - DstPort 4 + DstPort 5 } Line { Name "" @@ -18719,7 +19110,7 @@ Model { SrcBlock "Bus\nSelector2" SrcPort 5 DstBlock "MOMENTUM BASED TORQUE CONTROL" - DstPort 5 + DstPort 6 } Line { Name "" @@ -18728,7 +19119,7 @@ Model { SrcBlock "Bus\nSelector2" SrcPort 6 DstBlock "MOMENTUM BASED TORQUE CONTROL" - DstPort 6 + DstPort 7 } Line { Name "" @@ -18736,8 +19127,18 @@ Model { Labels [0, 0] SrcBlock "Bus\nSelector2" SrcPort 1 - DstBlock "MOMENTUM BASED TORQUE CONTROL" - DstPort 1 + Points [42, 0] + Branch { + ZOrder 397 + Points [0, -30] + DstBlock "Selector3" + DstPort 1 + } + Branch { + ZOrder 396 + DstBlock "Selector2" + DstPort 1 + } } Line { Name "" @@ -18745,8 +19146,8 @@ Model { Labels [0, 0] SrcBlock "Bus\nSelector2" SrcPort 2 - DstBlock "MOMENTUM BASED TORQUE CONTROL" - DstPort 2 + DstBlock "Selector1" + DstPort 1 } Line { Name "" @@ -18762,8 +19163,36 @@ Model { 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 395 + SrcBlock "Selector3" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 1 + } + Line { + ZOrder 405 + SrcBlock "Unit Delay" + SrcPort 1 + DstBlock "Subsystem" + DstPort 1 + } } } #Finite State Machines @@ -22417,9 +22846,6 @@ Stateflow { screen [1 1 1366 768 1.25] treeNode [0 172 0 0] viewObj 171 - visible 1 - subviewS { - } ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART diff --git a/library/simulink-library/Utilities/utilityBlocks_lib.slx b/library/simulink-library/Utilities/utilityBlocks_lib.slx index c4fb101f1c94a529069d8192f6571ec16ffbe225..0a85f1c1bb0a42fcdebcf13bc3dff52e659251ba 100644 GIT binary patch literal 60583 zcmaI7Q

ww=DXV*=5_dZQHhOyUVt1+jdo#ZQJfLZ|{A_y<`9TjB#G_VZFtS$hjh8 zW-3U7fT92Z07w8#Z?=*WNDvqqFaR(E3jiSh`>HK$Z|7oa=c2FT>0s)tOXp#0Gm^Gx zhsB5xs&CrThH9NyT3iu85h7L@tc0TUgoJr}RU0DzfC1_IJ_B5$(1`>Dord?ryvq-c z-|G2OsmcNtw~(pkld^OO8C_q&;z|Ae{h;xIA&${5+0h8Db&takd$T#6)&UYtv2H+3 zC*+WaA*>j1lCA*}YQ}WQ3eWP$>8um&33t`0XU||L3qZCis77qd(j9vh2`O)iPAk!F zSSs1c;EC<+R`Rf{$7za$y~$1tgg|Pnko;*>)Yg>XDGvdMIlse&Y6Y-Fr39CdXP(u<|v-o4m zgfFl*@RM5KQtZdI!H>N|np-SNDxUlCW^Qi|`Hh(^idmhf{40$$yUNqmCD;IiDQDImR20U{Gh2a_9@gYPscwnqP4&Me;kB3}6 zCop_GCAYMQ;GPwXHK8_Sm^!<;xZZ9quBY>>t~15M&_@$7c1#no{cWtc6&glv_h0uHo*tI572Mk0&Ql(6Ef0MiG^2ppt7|=|>Qxek?iLsDqPMXxK`#bTJ?>0c` zL^6|6R#xDXvDESb$dyPT2HPbwmcOclaV(KU+f>?^CD@rYOerGtsgbG$ zPc4**=7hk}%D*Z?TBdZrG!B0Nk35+Q@K(x{YIK}OSi|F{?Q5z!sgG6JzG=ejJ^?2J zBYNix!7@iwmOUAAzZvryow6nW&wMS~6B02A>#g=hPIcLmJDTXaa)g4gpXNpg` z?Q2h4kL>MDtj}T}9$qOUxy+=`GL}B`H(M5y^U-2Ca;n+cJymW8@no0A@eSU89e4Gt)XszP+AAWqLt$uANlG&RaxGWI-B%1-dJb&f&P<<(#Eb# z^M6zf{M+T&|ERDvbulzCbTOniws$gBaI$wWb#k#Zb^foN?yIExj~bLO>f+luG_(yNP*Oe6R6-ZjkF2Wx@KDO-118wh_^N!P<9g{a*^&Oz-6s?kQa8S?uV7g)| z<;T>zPYll$N7XwiY6(RMG|x#I#9zo8{Y~?~FvthIdJ-CCSSU9*R~e#f)&!?*=crZW z?OG~2F6&B%f1&GlgW@1L1z9_E@5LSt%-NfXyEM}b+pJ&7vSOhC3xlyzm^1#O_+DOF zaQ&s@>qs=a1Sex#7b$M0=BLeBwbA-(!(!>_jq+QAzbCflUFla5Qb6gO14Id%uG)Ca zeyQ{wXF5(4vtj-p=sNm=(lhcCUN4U+bqW|s2_K}7Bet?&QTOQ3Jg8m|ehfKt+eW2( z5PTA5CeX@u*UN9~Hl)Z8I%V&YNnR9%>(}(_z&zv?@vEQkC2|=4=)dueqEmxsGza4b zGKPBuVYZWn0SgT`(Xfw>=uK$_m(yc;`IZ6xt#lv%i=T=|2>H=}yomqf2={;TW9$BZ zGGrRZXg9*g~=J2#6LL4kT4VST64M<#nmwOaR^e$a{XxN$NCC z8DqH+UbPN6;uPtOdH?I=<+UP(r4TS2T%@Pa%*B<3pG)1VPN$(QqF5DXvZQQ%Jc4nL zY!*)^oQ0=je$HdA+&}9e%={QOaOHVjZEhuT-H76%AQg$Xu`#hlX~To`S3Q!&Jp+Wy zx^+p-u@aH8`1lu7XPmx7&v%fUyW6y+1Fx|Vg3D;(*0+9XR3Lxt7@6Y6HX>a#mTGfs1avEen0rM9v(yKG*H!@KU;R2KNr`0uvPd!Q) zCiag*rYfJ!&0+oM${Ryf#xu4kKS)G^W$R?j#(MYVsn1N*A>c?i>JR-kDmB@~@^50? zSC3>aWild*B0ma+E1BXZY{Z=tKEa!Ch&s>ih|!xem~{}kCsZZX`^kF=+@}wuv!g(E z;h!y6bDkzuzpP1C!o26ooHTW@y|ub-UaGER&s1IBU|b6@#RYb8J8q?3#=MWC3v7FM z{$B!6A0@UJ{D+(EAAXAe8-68I8&gAPQ%O5B`~RwdYwR>!AOk}1EzihAj|*XbVzSd; zgM*P*XBN~-g#384m^gcCt8S01FEckc&F?dGcO1KyDx=EMWLxPdfyV7Q|kf%x`DLKPZUR0xygLG z_MaliL(f3TPLoF%Ua_CAZ_xkLLDZ0~Cgnd5;*AObp#9G}xLCN_8rd0I+R!=JnO|$X zC?hL&iK!F^;CdNg6c0-rDUA7 zS)Yh(=vp7~>b~E?gR+glxbs@I`+w}SqpZbwqr6b9XeY|<63FiR4*65HEfY_nFXobF z$rlcr-;+gmHoUFF-#*|SFPCLEZe1^;IP==L8b4d0t+cO&@sMibhoaAjD6-x^CVcfS zsogyn>MRrW%RJ=e#|8Xf&~|ZM1-HX)bB7_4{JWixOFOYBXS?Em;9mP6>hrIlH)m2u zNEh*~Rcef_D$+;Xs+?zvUS3|1sfgkl2j6Z2<{E;VQXqc3xzB78v~z<}f!SlD`bsN2 z>BFX+I7X$D$)6~38NI|eM!u2b+`_3O|61pel?MBD?QT6ndre(fq21S#@=6|OjdjPD zMHP0hjI-~TC}VA^NZxjI*b~b+jYC^mCoL{4Y}BM~HeOs@AW=z3Hp;5BR2gT{qkch8 z6h9oz(AZDm`TfG{A@nNi?%vwj@p~yNx%^uG-Zv~8r-!>v`_4jEEihD>Y9_mbZ`i8b zE0edkIwkk0g;VXA3J7gXdD`tpbj6w|G}xxo{Ve~D)a}3PW?Rev0~#kIW5OX^$)#5I z^Lp=N$`Q7Cp@gw0^xi@p*x<;hruc;;m|mOK#C}$o8qP>I z+_>a1T2|69)-eP3se9vY-)zOHH?Gpjp|zD=Swe+pI=#X9-t@X&O8YVGqMI&}MJ-6m zeK+Uh>zh-3HNRY8W`ED5JQYc_pPoudHPwlEzchG&T_1^zWpqqZo!`C&2@JHkem^!b zq1EzAvcwg~&&)V4Mx+{=5`oiPK9_{tH%m77a=XjV-4Du1^E}*)PmyyQ=f_qDx0_j8 z#w{jdq?3ygTMcJ+0Z78Z!SP9MH*mW84$Hcxdtz79O!+c7OLmhi&Iv7_K2A=V|K?s> zvMMvw`g0UJ*51Ce%=<#RC2IiOp3aE>(+gQyQ4~~2ma3v+fgVSeKM(%>9r?rzDZRZ% zQxB+jg|@P?W1WQs3Xw>2KOJ9<9k;eJ8j4o-t@#}*$1)Itjg8Ij&+2t>u^%`ubRg&V z{%$KsKXlrI|4GDsI@H?iU_dyOm9+e4bI11G%gd|%OJ-Fqq(qvI0%d~m7Oi_LPby4; zkI~+ttBWz#k!xK)$}ELUILKA653RDM1<&6@%DZJiiiVgN#A7IzwGNEkQ%8SS*YUpH z#BkAEfYjFZ_M$Ajc~e#Quld=W7A6TD?vTIHGc404JY ztQ&jFO)TAz&8l_@`u?$~38#wC`hl!1a1s=H*ps<4yNZ7G4+SYD1qFfQW{xuPd4yr# z{qo+x6fI?1ugJH;-_4LjdT+N=p>w!(Rm3dhF*#F@$pLw3Uf-{(qq6JuiEk%?it)^g z32$wmmnHRp4P@UHXys$XESi&(6E=mByRq?{<+WatnB2|L*=?Sk8fo%G+`F7GMqxb! zEfDSR$9jF@FAJ$wruZ3;l{1t1z1%&D21Q^~%jDuek5(9*cXe1N9KEBU zu_j1vUgRK_1&E05{4xGK82FPtGw!!~uSWJ;s@6B|O+DP6ilTER$H)COvDEa%OjV_A z-AD5@~FH z!*3t?U+qmgjsq9Q#o)?3H!4J(qV8G*LD5w;F%FAH+JTQdQ13l^X8Sg(j&~vS|7FsLbU>z%YMy-&Cx{W z7jdAVn|4AOWf`{|ZcsSI7|zbfu`3_gCRW=3o)Wev+308NT^U%_q26`-#D&A5@i&{sa)t7#5w?YH@Lq0Pg_FSP}z;q+EygjGM4 zEe`Drw%t`NE(w*IY1AK(eq6%WDzXf@b@RF*dBEXp(%Kk zP97eVZ=5|D>@;^+p}cc5qurC!Kb#}uR&q0Q6VEP2D5oDZVmW)2-zcffbr^>)gdb+B*?!E0k4C{T@ZINztwSB2Lz(Qo9 zTyY5HLcZJD-e-0khvIG@al!-oP~>kJ>72kM)`T+$dO|R5;Hts79AP=f4PRG7-bzq< zfH}MRi0wDIIZt9@$V-O#X|nL5&*sfum9F`o7i*ogm_k*H(h**g7E5D3?s;eW*>(c` zY2S<_Cbzi|#7Y_gAhhjlI)v(GsWz=x7M*udqij!p@xBH}h+?NU#9A8ZrBk9(9E<~R z`Gk%ys3=EGOiV>;;bODVUH$^q(|XjVfAHN^ewFp4r1Z!dJw_KyZ1EUC?Q{0Qm&~l5 zf5xO)W;PV|s6jy`e`$tNQq|>;pbWq)3yVjg9=nTG@uY$wCn@}Wd`G+Q;&H#7D?C`y zUzFvWW8=zR26ok|Tax>zbx~ws(bvUDdJ~Zqv-MK(q@XVD=UT)CQx1h$hw!*iE-Uqu`^UWvM9D{pUCBri3O=L2J72yzV;6BD*Y6@V*vm8 zYNPebfmbt!SrFcjC1GK9AqRbYY*rqIb&}VEuJ_d;9hs1`8NYJI<8HoTW_WGBW|8Q< zDnBzn!Xcq>uKC$s+|;PLhe;aj{lenux%|iiKWf0;e#;D1Kz+?|*^i5xJy-KRJRYG8 z2-qW{h@OjNt8@G0z;kaEBvMwUj9mP>tTR!YZ$IGNnU%=(mgLNV(Iqcy1=M$Q;J6UR z6dv9H!^b&env_0-oR^n$6v>uJ=36bTx>s)NbO zVm8k3=cQ*|f`L=f)6?%>lYBoE6GOqkOe)>M{ya@mrAoiePo{MT5T*x=KMgxw1PvIp zIR|`8+}u5cYIU+aObUyc0&Y};hD;f2$S-ddeRkPHySqr3NZ(%T?E7PQOUOYw(5Un0 z;+PWI{n>lES;(F!K3_Kc)^(@LHm9pAB}mKmuLirj6jl9j3thWFtN+mqy2JDtB{E3M@A&I$42)SaD> z9IDmo?I}*`7?FX7K{WLB^^xYNd!9%5Q^IMZqWUUqox{yS@?lCS%wi7;3m-FC zKHRsvqeNRq?ils<_DV}jr`VSs)6mfVyi!== z*&IYWV?Qqge9#O`*%)kfBz@|b8v~H{hr??!T_71($`_6?1}+@N#&WASB3)Z zSTd*bsFfgKn>K;f`_{dfOmv4gOmWNJ-0~~^^if^#UQs7dzWEhb?|{OmPwwv$BZ^4q zbX4&w3VGzGQe9T<%>^GkEy1% z;-)?2@$;^c;W=HERcZd$h2ldQRfZwy_5wGt^!FdXv7WmjVUqibv?3Sf7}$zjJZU4N z%kb>wjWkFG@87))ph?hIHsIjkx(psAg`S&m^_&H&m+UdqQ)4~JHH%{5#L`o&-y-Oo zYs13S)Uk($#{E6bX*AW}I>N`GpImo5XJCtZM#%p9o&H8wNW`FV`a!o3;Iy75_U=}x6*>TwsrR^J=fExy9CV0$uUI#V1q z1#_;9Gt)88FC@?M^2QSOnK;xoaM%)`0_8l;k| z;k7{%$x7-j*%=*vp@p}LO%PXJCMg)V6Xc)w{((7eiIpTsbibbmrKl7vB`NfjiZOny zBH2lg8!p4b1^w+E_-;H&l^FP|LaU`dj8yR^DUIRE%O>f@z-CfDo$egCaJ~DN=l9Xy z2+F>6>p$oaxAY+)UT6CnTfEv|)<^u-~r~3N*y{O43XFk90tgEXpZi&Le zN$`bJqR0}HCe(vIRqb;JgYAo$XLnK8O+^;xy!Lb}3!Q17Jui7RwvO_%o!y?E-rqoc zgar}=Bv<>c?Z`|_y+Ajr1DEW+s;a04P(OHbT{pVfH_?7S->7_c)1Rg&X7Ue}Pra3d7m{0$21hSmPm&Hn$0rRgTIj$7Cg^xFMb)7S)_mvi_|XcE7xP zWG(6r)rX1NT%;drK)q>2t)V!k>pMJNI<#{<+MIdYii})KK^0GF(v1P2+9frbtmZGC z;~N_TmOfD2wO9b}eQ78_{ex1`n4*&%42n-kkW8L$Z_vH|Z{6m5Q`_6y0rwE$y|@bK zf{YC*9nT7IrhV}JC|Yoy@6$#Ex))qrJBMO;+a*Rw8Jjt`$>Q)m=_(>NGdW0fsd3Fazi?Cc)U`|zmzb;UZb2KuJj0JJe=XH^r(RN`~WrS7qXBtU=k5mPH1iH)mZGk z19gRk2SF^q1wt9dK=!%Tin$PE+YdmohVjoY@qZ#M?4j zVd{xsSTnY=C}j!6oFHy?i&1lea(*7KB#INM`S0OLN@n}+`{cf%Y2+mTH$4eITv9n`@>)h*4 z@cC}=^8UmpI$3B6id;pdphk5pQD-8?y{N@hSAz%O`0!*pJ5mcE-Q3*73a9#f%j6uW z@LzYut=@w{KerYxdubf29O+-9prEip*=~UsJ^UtT5)o2}-&7fMrOMmIS43`^DQ%Ck zA=R(g*x2YQ!woR;stq*eL~Q}u2a0uDj_YzDuc%)s(CcDl(<&qrOq*cxZ``niKXZ0- zY{fTKdEtk7m{?W?)O%PkR6=sa)5QQMvef61Fhgj=m1X0bk150_$M|w{uEm=aoG3t9 zthIZ?4J7~CbLd!Jj<#U^sW67D4v=hQ)U=abJYi*mBZpcT~RqnMAMp83V#iQ||zq z=Z=u8wI%5Hpu!jFFAK zv9*b%p}CXcKeJ(L^WT%2o)vxTO$G$OU7gWmFNNmn4ZN%LsuD$U5e=;`OQcV0h-uE; z_++;{KPgPi;o9$z>9}6*-LH5blC)KRYTPe?IJE4PSrTPbzl2Q6xbgFY&(95V)EG<> zbPTxB3ZUWa{o;Y!Vt9Dg#!R)W^exvrQ#c;mKf%7$eeVX}RZh%AvMl>b_M4EzH?My0DqMDHI!){OKe6TR8hMeAIMJR}}x~fAdC4GIc_HqE>gGUq&ZKiBC&7ENZ zN8WB)2F;Gd0PgTkw#wRz*qO!%WzfjbuEvKTGLMq;+=*~ynsoIjGnUoB(a-VsUCZr@ zMjmFpX=dGc;H*!?zJzQaR|i%(uX#=*Yai;nx!Ue$)+JHSEEH*J9_vHl%J$|;Bs|U@ z$6o2O2gHBE{=mbECxHY23%`!UQ^^xN+Yzs;XYcRH1Cjw722P0nDjsu0$% zy2RqN9t;@mw!0IO4r9{n5@faCc-C+WOi?h!z5^zPPTkww8NtEi6Ap>ps$j*wYTBrX z*}W^E3LEe93^PL86qNAjSnVNUkS2h1kpIaK- z>wt{GkS5E0T%xx9M)0Xq;WX+XO@J?uB$LvH0Q)=NA3{Zq(yMIi5Y!q?z2>#Vy&fh5 zQpXT3CN-p#G>LV58h9M{0h4pMP1_NkAY4;vGXmWbYax@zPGujRNo}p#LC>nRu+Y}* z4|V`WGpngVaoC4|1H4mQ#=JL~BKx6qo)}R-B|rzI$cPx2vPu#hN3GWn_**XG{m`Bs z)$fZw5zs1kkZLX4yB&S1cjpB{nUz7r*rSyB6PU`#h~PshO#*oYuCexIiZ764&kj~o zDHQvjWB70k0IXj&)bbFgia>l2rPm7EQ4Q#o-q}E$2;F_&2SPE5yNkenqKNXd>J7+O zcVQ}|@C!RGuOUBUO0v+PqO_-uouq(&Zh>oJDD_~|5l&!>kDTiF*?D-8cU3;wWt=aN zj0@1jcfJKSb@Y4Atu$wJLa8|AP;)NWp~^_QbJni;TX(woFCO)#u4)`2d^|qlK~Jxk zQzprD=gZoKPg>#-r3`b6>|N&!ULK6-R16o4p^T!|Cy8M%cnwPYik5VG8y2}1OOjgD zV%s*Z zHn=;~HT~31q>+BD*?Q8aeufHl$?_(`lOdXXOij-)uQ6#O{ccXlB$H{wLT7$niJeYX zJD67j3^K(+_0Pv)1|BOi&~39)-RT(CEow-k@E!o;9<2V&5jb4xq8nx|iOMkBc^+oW zU7dRM)N;?hdMk2ojHs1oZP7L%m)sf}_VrqJq+`J&`7X@~>Wb7zTWzeYNP91p{+)wI za?KBLg)2D4PJ-ujYIkHcD-wQSJqoZJWitBSj=)EV(e7DM+b3|%2AK-mS6kks$*rzt zh?lkx>t7^nik1<&D*3YR0G__?OFEHm>{lT7H%ejp zv80uw*zVtjnT`newRxYG<=Sq6FU|IdpUe+GOoyt>k1U@O0eMc7pQB>iw$+6fcYrkg z>c;1Di4b4j6}D#MIa|cO}DG)!MLs=+Gc`KdsY-{Ii=>(YE)J6Z0@su?k|$gP8wgHai^;^^=(@i;T2&h%jJ&=9Eue( zj}QLg&hNj3YZp&!VFM}#0AqFai;jSM$Vo+ty<_j{7&_|-Q94iaeY`PWu=G$pJ&{ct zjdof_9a^k?&D`qes2jR=va-jUuD%{>aoi$ZF$opH&l(aQw2~(EknDN~F#Fr~gi@9YXb$ zf-!@1A`#6#en$Izbjsk{p3M}*W-`8h)EgORNPG<+9WLSHn{%n&h_$j z9R7`|BTRD%J{0N>z+`F#>)f5D;)+Hv&ati?q05~5$K5`q#~M4&SD;z`Gb6w;EPfzG zTM(9|6O{1iO41?fnDD^1bD+G&;}$Q>J~N{(Adrz^8*iG3C9rTtxh^gA<9sSg@6{*vjxRFC zDGYNfEPcpHr~fjLwo~V)#emZU^~mx^^8SGQorA?}^qMLtQjtp0>`#w0?!J4bDxJf& z(mibMO#So(L0L+9qYUFauHkFW`SZAu&D}Bc5o=4bcr2;&#&pSB_58KxqWQ0z8Ofb) zHnV$>Fjh7=VUGg{AMXA+ULdqi!ZPOGa24q#k?2fhB7UqFG$9iM+FGP)#;zi8R5Vrw zEpU4@-o;x^UZ||_PDd6HR5Mu3fugG?PP+V0F%nWtiZ#jboP|hlD1*@)E55bN6 zl^RHA-Yo-m2}Z(LNZxbQT*1Je#I8v&V&+%`k+E0hSqhO?rX4MO*e%Q$+zU7eoM`>s zuC-HfHzJ9W`b0YU4@BHIM7F=ckUeRE>)?W{WYa+%m&jUe>Nw9w;y(x7J2$*cJn=`5@&a zbaL57%1r~R|6n$C9@QzYfAn4FJE&DhfQtILbMAM`LA&=Gk}~c#M@Br&eTM#}un&Q- z9y2ajCS1(o@>pZI$D-xkxidkeH_QC9#fmo(^aeMsTZm%=NfP*%lTx|cmbt%|=c;`j z-#|^y>FN5KV_hA4Td{C0oxOL?S))68{NyxQduMmYMMBjk{?WysylrQ&5cF?v=vZzO zqBT_enF3@vHdt^$fYiTW9;G^Dt0Ebcg6g#+fc?jtBvFM3FD^IBg9Xu$bWNc64iqu4 zjZYR~{cWeC_uPf+bd7XWe*s~}bfOzW+4Re7>HxK{@C=7d_e~y?eKs}DTWW5-K?f0ekP$I|!a>g2lmjQieJeQIY*! z*mH1tlpTRg<~kD4rC@ZZxnTc%DVVody1Zal5)Y9>uzeEn^WQ5H94SBU#TT5vB0_^A zlk(yMFgYoBO2}gC~yqo8YVJ!P=o{j7Kk7eY0WRg@I?mTv~ zv-jxT)(*ne7gEh$O?eOy0@eO>e(zl8YFxKjA1vnuqkt%3iokyv8R+0ZksXoKr2<1_ z!`OODuy#^tnjFKe=Q0ICK|61RnZB|vOe?gHeyeq34rB1{Rj#!Et_@yFXE3tr#)~j* zPb4B`EMOQB(<&%Ow8lA^PwwQ%lsrShc?v6*7xb?z9defBAuQ0843m5}5)EOE}|; zNEIKX6|;yoNDm~u{L+-00UwE@Z*Ex#xhbcgkQ*X##N`>7sBip2vgzBZIB9nalpdhd z-1kh2mZdQdn5mP!M1RDM4rLO%$8)P=pZ0!=#2rek5WEm*bj0S-b%45dSO)pTau21p z;ykwk@(!UVFP1k{zfpUI@{GcCocis`vgX1?+*h^tKr&b=;e_F(b8md_jl6AXy=dofwm;p3Ql$ zKKMY!Gd8chnjaaxfNRoulaD(xl2)h*IOMc z5sk~`mg3_x4n+gS_5-!p_Z3!&!|!o^xaSrAKO1Y`NgPk+zuSu95&!GN_WvJI8U8D# znC7*%-;_w)eNl%La#+(Pk-M5YVrq{@Ya}7>OtBG6P8y;?6U(xZ#7VM|O;V};dQw;2 zE*6RSig&lS4;YCxR(~q$+)>^Bd`vrKIk)MUGEVr~>go1ay}qMCQOvIKyGb2+nErF4 z@8$o>g0Rk{2iAS|Rj~Nx^72XzA2qWIeSNVFW(bFrdfG;F%vyzs0&BYNKh#Bre z%#@4DaAef>d)LQM6O`C3)Nz8jXPW<~SiNaC`hC^|vg`c)ng#(1!H%Ik%xnBa2ixZJ z%fUHU7#u8V1h{qQx~I#-JP;8Z%*itNoc{zS!Nt3pCKw3#8!4D(AGLgJ1IYfs91jd^ zcTEi2+!3*97>vzy)}85~ar2`AcQ4JMA(clzili|HL?`oaJF#VxD~4ljD5t;pbA_|u@j1w;?+qBx!X+l!FNVW70oz(CBT!vfU| z&zNU@2G#(Bxh&E5NsnP4rqn>QOr*!a1GxfKKiXu;J?x;8l!1NlG;O@p!`-RfEp-y3 zTp;x|HhU?yi?%wP)^c%m;iQ*i+nK2Jb z-L+_XWn*!-cCTo+(H;ptpB{?qGO>E=)I6$;r;Kk&AqQuEiMUrT0%ZPx* zK3Y#O!)#sI9E2FN`>eT^z8Ly%V4v_Kk>Vqm535m3`DUPdl^X5Wi6E`A8;{Z)vh6T1 z80m|<_RpRHja(FKmjw*|URAI~>pHzJENQ}O_4nJiTRMY?08-RrSS@%Ljx2mK9B}x2Af=xsTsNdL$KQ|H5g6{~`>@YGkmjv2$ODB% zIhYWlYbQ>$gKfv&R2+a?Hj|S^RN7V6%6JEb{Afe(d~?`q79!~v0O{KmeUyRQX5I~cdHF>>Of+MFDkQi!tTMGiaqtPJ- zt!bn~${A|A9-Z-G?c*MYOV0#Mf3r{#VUl(#0pZT@`hzRP_ia3p9rtrk8Sap<)S-WT zf`wMop~mA_Wrc%Ff6_2U^a2B<;(C?CbU%708qq$@nAvLUoG8+9yT|ATGV-{g!ly8wae{D? zNC$&R>s~W$NOXb)#bztOD4Jg zrWJ2y!+~h_2m`R-?qwIYuku#cJi*~4b)2TMd@ig*5pfs4$9f2d-jT@Ou<183O zNqF4vS2~Jd6Y9wfy|hVEaG!jJDS;xJ>Be~RRT`g8Al`#9@Op<|2zki&V+sKsM*%k4 z&A?Au(49={l^XaDEsQ(St<5(1lBlz75opj3B3qQ$1WgTeh|~lu<}%ic9=vc6fWRNJDExxz{+yBW*lf0Af>ZQB)T=9zhfLn z2NrL*nzgZ_m1rp;(e(z4!l~avV_1Py2Jc;`H9B*}Y-Nl{%h%UDkw^vR94rH%n6{No zi2^LiAMlnUDH%nl14xbv>_iuw;c1RL{cI=8!*|A}Mct~m$2n?Rc`r0*gIp6zE_@O=! zcwTHkB03GW@%&bR`!SxQ2-{Q5MCx`*jW={Sgq`R{plpap*k2=-=Sy=OCSKK3<-%P; z05O<)*J^>CWw?=g##=NWFH9soa-l)K-zH@IGjn#YbJKqzz6(m|PeYmhu_~gPR$C73 zaY84I&N9Pzj?z!h_~eB}{HgtQ{bmHN#t7~nD@_Uej>EiOZ&jb&HINxrS)Vu{10E7E zNiLi85kUR(uL0X@o`C5~Oo5Lv!Ju}na-`ChOPX`ASk&BbBqK7V?e$JAvD|9m<$xfvvxLiXr1TsIDXDl&C_SqligjvzlghRRcYKyR3w|-9lo! zKV9NVyf3gu$XNEy%#(-G8@c-!vXsXEV4i-T!K_LqVDuO6Bqm5Y4~%dMt~pc5qLYol zhc%DZ(IHKI0R&?e(=$Ulm%H|WQ7gzB*;)ss%D#%LEV&rKTK1JP(sNZsQ+)l8L%k8j z%gVqVOhmJg%Wg-&DWfRkgcIb`v#FvyF0-kDYOZ6L>EMOx#!S*dURTY7)m9T=@S`du z9|2g-@^ZBN=~%%BLOBh+_HP9uR@H-Gc@OiST5C`RGeXi%GNKoSLsq;FcSqpIC*fj? z80hm}=`CZH^1UF3PD*ApT5=A#Le2yxya%2z04!r9t{DU?b$UZ@AR>|yBSj$f=}Jo# z19m;Ab(QhmHc_o@xb|?xS@8S-%Z##Gu=Q!K(Cik*}V)S0>?nBj7z>Kf9jWl8e=j@8`a=N4lD@76QTGvpiY+= ze`;gc{3ixFbjCk=$;>^We!zohR}10^!v>(nV$L!x{nHfRW%Wx*^pc z81Vi}dxj|6jA=XsDSkT5G3s24Gl9!Ud&X;_?O^9zhg5vmwDxAf>EI8E53ww0%G%|JU#mkm|KQdmqzDf+2pq0&7h;r4Kmk+( zULfHIYD2&*$bfMPV<^PUEWiN*p9OKck=uB=_WV&NK0`(zmis0Ear6STMBKV3K7>F{=NItUzqOI%fG9Yy~GN%YFJY-k~H3MS!}~oJ+goGZ=sJx z>Mia)RTrjzkQ>{IYB9IVPs6#l}{+?aX&u$t93c9p<^ z*tjxLAv+S(KKu#}CVsE)WW{axtfnQ*y@TIBu*e! zL(GUfA_`cB{%bJ$PIgQJbsa7`O;{bR%j223Br6~H8{bV1t&<0v@~Oh zu3;P@Zj%#)B~sR~`j)L%*lN;05H9rGZfDLVWnj|ECo73H;@QsW>az!bdZAw>_~sGl zr_%~iW`3vdn7ca8hBl-rb88a%>;&wG0^q%i0@tQ{WTU;|qf5uY`6gG-@xtgZzDo0H zm*=wM7R2#Hsop5d`R{ct1SUU4`MmAfdfEr|ti)W%vQEYq^ zRbH+#rF0FQIS7lrAj1t`Ogy1j0yg^_v^h_4@0@lX??+~%-nl8B{*0dV&8o*M$c&<` zx~=HfBvWlV7rK$2ZMR}2tcAgaTF zDV8}#MCRVf1)Y}&rTWxGZWDJssnT^#P7muC2TT$Y^YQ|8Y2zlD;=F*c6InC?>Se|E zN>*?UPX1b{377~Uc8ImKJ$>{x0@;(-l2{;66hO_({ALQ!|{uWJ^ z?A9-m&!BPb&8DU9nj`kD0)(%AS`|e zOSZ*91(kkV8Xu{B;S|ln5R{oSx~ZEN_n3)A!?%(h3-!vVOYbXq>0>|i&yi&a>cPN; z-j>eSz_M;l%osX`;KA0R&5R&B6W@xoxw41ggVEHnG^2RMZZUJf zh;XHlRUdZGa`5i!A=}CB?4aN@)3FBHjJ|-{FgQ#1dx8?@_Z4EPV{TRbQy#WC=pSWD zG5TR3)v};_6SiE1>t%Qtv`%_VsfOr(ui!iiS1>MQVVUMniH$!=q)AY$H*Y3yS;~#l zlzcC~f27dRMUlKrh;0xJ>d|&vv}+XKvtnf1Kzvf>?}jvFz%#RJutDeqp^dUKpF~s0 zX#z@Ug0_qA;vh6JZswM*U=*aElmZ$3;PMED>kz`1KQu+qK{PCS2+9@j@oTyUx$G7+ zKyg-ui@Q_9h**D7KM1u^sC2Q;MXOZ@&o|1{^Sp~?KG7nenFG!})PM;n%I-zO%bK8- z+WifHxuXP=>85v=H71db^( z2vwRbfZB5(L2QbAL2KKlm-3z<{GR~b9hx2j9S81pd%lDsz+PP75S?@vhscn)_0$A{ zxT&s2Q=X)Iey)+w?OzTNU(1u3rpGCVFxEP5Dy_4l$WKC4rf1ZGWN9TOKs#i zVEEqkWvELf$*>E$wEU+iVIxVwB}}EO>|!3}-CKKRm}@xjbR-mS{!xqsIlKXxuMn_# zmzS`UL+TPeGCQ0VVS>IV?eAsv0|vpqRsa>|y;4mDAdn!B_ZbB%MDE^UdLjxz&@oarrBcvyvRrNc!g{%twEi zHo}^JOY!}4mH|3H&tFF)RWF=8x58bzyW#Ycy?bKeJ={cED8=twqLR(vlN&8|USSqH ztR&TU!2qFjSke?t&bJDDg)g;yfcX^b|Do-z!t+X!Y*EQ#W@cuzn3+bC%cu`ouNEdMG?_#7j6519jBwn)xO*&wx+CYM1 z8qg5ou`|&W#0}68;eMjq9K`+WFUauhwNV7nS2G#G@~PeUIAE?EJKdZ)PYnr&6VMx) z|^2`+eLv#U-qWiL4(Dh&I>QLC4oiFMgf9pUwlR-?LUvL z0XbOl&kn+RkZ7m!G*?`O3>kCm-gJ@{;BlB)&Yz{-JWiaKwemCxH%xvs${Z#;KXU2L zJiO=h49oAsMHrvFC5wnFqipZ99W7LZ+de4J%6X*@V7V~q9xUO`5vYNBm+{)~-j!&M z(a*$L*tIEgX2v+mguhw89tFNP4Qh83u9e)OdWxzZ+=pL`qC?Xnwv@Qvg%;&rrFEyq z$B=P)ksMJI9=3My!Y$Hxk;_|`ZG)cj zgDK)~#u`6s&ZaZ0+b;S)<|DULNzJx2J^=AZ7#de8SlJ z)+x*D^u`8z_GWZRpQ_y0w5NApIbj?bhEqN8R*s$|2;pvND4G`brGmxfZ75|EbIZ`!}1AGATUlSUq-)M%S5 zr{P8g;;2h=o{}1XWb?d1YZc*msdFt{@y*~{JpEjp>=bHA_%u5$#+J37=S|sOQ>N(> zk>-8k{xAtZ+W4*J=dSz{i`i+0YZvYliVnHxSOCs`kH1|IJ4}0L6xS`8%3-Q~kZnKH zxG!R^WfJN9`0;w^&CYVLG^k#lRbkWN0AM-cCdW=Af;|UxiRqpCYo6n6_MWltYUj|6 zf!vIbqnRdM#S(v1r6DA#;l)=^e*NdVEVq;*N?<$DRVQ15yDx4w zbu1a9V^|Gs-}Bwghk@n9?eK z=_@z9Y1%l2Dq)MUrfQYtGpPWJtpU*@RxUwbu~$L;_8Gqzcp+KLtTsv>x5Hdd#al&N zSw^(V?5lY$45YRX+sQLr*qgqkW}%_uuEaK)ZWQm+5=U6Tf`rpkqi^AhdfmbG{uTkU z`H{b`t}P?S1ST|ctd>RC6+gm9(fa_5FXU0SZrdwb)6?(&I@Q*X}nf&KZ6 zdca+kj@_Z|pg{PT!+m%gYt+vg&$}Hlo(8=5riX`C{?#%|)kP>{|6__`dhF9|@blIyG z2XyZA^1HJccR%LH;zT)G(I-m|4=fZWHR!N$B(-t>EZr6_;gfK^itwND(sOsNQJ3&b zjS8omw5bfornu3(PF+2sR+bBOdAM_3q#H19KFWB0{;rC ztOafBKnLuy;7_Z)oPv1l0J*xjg)L7a)!PS`v22uT%r{X;-xq%B73k-A%wJB^jiL#s zq897_h(vY}e4Qsc*_VGT(63I3R#3|HG55{YH(?YO%f`(GNu6-JZ+g<*KQC0f8FLdP zJ4JgtEXPDr(mjWJ|40h|IAuupYxjoR$N7ltkA?Zia}xjD&ik(ur!`d-1Eccfk~EZK zBjYv7j5D9*6vRd)$7BabBpZ||X~d`N>2GDACq+(!QA+ul2fpeCK@FGD#szm7f>$p-z1=Zv}9PP>u0N=+v0;N9!wlX)c*SB{Qu(x$_Ft(=?{1}yFZ)v6-rV)qFLOaBgJ1tLe&%Bo;bUa}^;^hR#DUhq*>qM#+IE=@zV*Cf zgE0{m+&Bi2X0dXm*SX<<01&d48!S|sYGoitHHSAwcb$NSLgl0YVXMB%-!-V4Avo=x zEp4)SluM9GJx*eu!34KgY!8WB*$s2MLYL<|cso|qkaFnUN58fK^{4TDeWpx9Mib4oTfRG>}@Pu@Ro`xd^3&-X2mbRxJO=2Et3QPWVREe9U zijSuRqAcjp?l!1Vq7*5`)L~zKIDC^ghxDQ{^h_s-U3M2M8D%{26t5cT5dLBGqj0(p zVlFn;2x0r`-B@8D;gz=>A8-C5b|f&($bujBo35>~|eS zVmJ2bUar3D9%PH_pmfCthjm+mqzJzPhPD8PCLiKCLgWKKm38D*Jf39Wv{Am| z`bn%P_d{2v_!@hzYO+ToLGloNus0j2F29R3@Q_Jx*v<4t`IDYj%{m?EnbV_kDVng8 z(PXI0yM74H+_Rb}ne4>J8vr&3W24E1RdPjqpYPA#OrwfBGX=VE6+&3hGuX&{!7vcA znv$jI(F&V5u$&m0mr{P z6VPLTXSwa^+?1>A+i)ePUklo_L$OdUmZ7Kj_G9B}f@%i_o1~OXxVyjvfn&TO@FeP3 zf&rcEuw!w^wAgM|=a_nieB``n1C0Pr_Rw97URRx?B<}$uCjvhRh6}A6ytdRIU5$Cj zNwIHb+B;A(kxg*YU|1Z8D4winXb66j+7B9uX&lmfkvE|-pCK+j$!`j-fjNq99L8o1 zb&FB1cbd%yg)^;~HtN_-vhe!oj7bcKYFYCbDau`Pg;_ceL zo$W6@mLI3RBz{Wc5n0bUnW#V+{*+V{!0aNhUU1bjvED{j0q&LQB)CpG=sC6P0N%0W zrR{b8s_|2EUCGzI4GU!9n>;?ajxGqi93UIA z=Q=y#EDFe5Pgcg3qMAo;BUdMoO%fycaT37=n{q3teru?Hzc(Uo$0mnG`E*fMn}EVz z7!L`y9a-z;llRPkJjO&}gB|2U>_dD1H-R*J5i>s5fSnf;!X?c8trHHO4WG}9kpaye-kKx$*jtFNukUbC=^SEBM7_Kq6*xfPMqU822Rk=kUD^A zf>ip08-8e0}40}beC?v`$95fJuNeRb#=AZg-OS_=CdC>hri!2_5QM3)5}sE zf@S&m$?CM$|%oLl`<3Uv4Q_2ffobNCQhZ}upbaf8D^j8)981>Oiw5YlB5nyb>)U{Syjb2oLsB%IM(A&_^PS90Ibpy(MbPqg>n}E`Mrjq* z{t+WOzA zB`RuXtXaOkkEvX49)fy+r2|IvJ2_X+p4w0GjS-H4u5u#piqzFPL2l#H}P~rA?I|Zk-3P@7H_I$I0MG-+wb(gb7;lnNZ^R()(Wgj_p>GX&8Tj_QwV%uiQ_CT)VmdSYH^>5ps*~<; z;R_&e7wrMbSp>S-AN|-4b9y2|4>0I#JD(<>lp>qHYXpCHK1D@b6P1wh^5o?yFF88P zjS<>H_XFeh(bKs)$v+B7v=A-j$SIIVrJ}v$>knaE-C$xsV>B5c@1muc^vV;;{8@mL zt|F0wLwrQaw8mdTkTe)0M5Zj-qSPGEX0IiSzMaH)`D79`#S`x{f20gm zsrz*R2ggbZ4*vmw9|DEA<8O~9CqA3VA|>5Vdb`Z*HEHi8ORFD?9?C zXC*+DFd<1Y?4%mS-b`){T8os*_~oe@{dl2SO4vHRaTEGJ+QM@S^ToM2%*xh~DO2&F>G%s0e9Zd&ICO54}>2F{k8O3-rkR4hTM^3Db(*Izy1+&JI>tuvXX5 z;Wr};qNj;U;xgaGh~`S7O6!@nz2gf~XzFIMn@_oQ2Y>_TxA+zR59 z5`=Qi6mfi@vmWQ?o?t-tLqUWoouVr=^1G261a|9|0PRlL3X~~6W2qQ48SnhsdYd5# zBj0Q*n6HHM;cK%v(HRQX<3hH+G}f~3tywaMJV!IdU*%jKHxSLTM)>miiaD@~jPS_t zuV`fXz$sjbW1I#JXPK{`)mkWSp7kvT#*jIRIj*?2$!8HFa+ingfwzm~;Nm6HU-t6) zZq#;xqF5mjJPU43`=Ocz{QVUot>6W(&hSS!|o~+XdRaeM?t>0hd^kW8!5#ogyr1ym@4;_S@cM z1TVk)BaR}nw5EZn(?gegO~$Ww+BJnEigUb2ojml}arwMU+FXlJYq@am#Y9SEI@+0h zfmiMO_!nU}zM|s>?KkWy*YAeNqcO%Y5m!ctWeJ@YtuWq@;LukH1|p;2+@?oq6B|c~ zyu88)V&$wg!+38Xe=ZUrpSdEhKEAy(fB*oj|IIh}_l`veE2%%F;{WVgtcxC(`Dj-Q z)+HHAFMz=mFk7SyqNJ>X!i4)?R}hiH8=r1-yd0a6F#UwCTSrC>D`izO%j-DsNp z_DlGtgioEMsoiBm?Ms3;Og+epE=ZMo-`QVkt%w^}*2WJP_I;d7r~Yr;_;(JRRhqO| zrbqrrYQSsyX+8!Dz;w!YV-A5Hqf4&)Tdn7HRsV?IDSLY!blw&=mjt8o=YF(j+kepV z{4re`ED-}_MkGh$?}`WltP~k2_iQ||uo~=U*GsEF!8qz9dDhrC4KUns$%Ng5|IAN> zX$;tmACQ;a)3QQI?PJwemTr2Xi2|2~gjwOE?O%;4+__7mVq-&TKU|IqC#1!Sku5G} zY81gr3L~r3CQsYQU^Q)&I;U0?IRbcB43d;)Ei@~ZJtRyztf9KBVW_NGV+UKnyx!-4EKPi?1zv7X(HHi;Bb z#OC!9T{}fy%BJ%r1Mj(Wr?>t}vH9YjgMnQted*fuP66AaR=7vYL*b>2rYUfs0c50g z$yJvfK-zcNp*9h5OLAeIGk}30|RPMW8mLXZdH6 zDgfhnIR$}WZ||5yisio9P<~!rLMWs)<8Jupm9z60;p+)ePWJrMsKcnQ#sz_GA629oo*y$<*A& zfsWtC+*<#ao8*4`!!J+$$I`^e@Q5@uUjCB5k~8@4w(<${0UW`@rS#v3?Dz z{5O13wx;HW{5JYlZtlkR|8vJ(4$6*8Igdz=6qiZRK>oY;x(F+b5;IM6@c?GZqLf+x zf(|&`5?by9mEp(2|2uRtwvNUQ{|i*ZbmBCy|BglgQS~3sl5$Q|9_BA-qU>sNWIs^o zeJs4cL!&4qDEL1I19A7o`R}lRQ!Vc9FCKsa#2*or{`I}0RHvHO`M~h~W6}K`7FA;d zMZ=Flzz(`1woW!5!-4--?>&U9G^ETJofu7|%P`%jq&PJoCEUNet*cz>AFr5M91l;; zU)*L`3z($w0nztkQT`oN2P;D}V?#?_6LTv^V|(4-BjbM(0~BN8V`9={k+Q=h6yh|Y zbbo`eMBu*=`tZOa{RJVQ4Xx(s2ePRTb?{F?>fosFXl!C->++A03fB6DX681=zp(w* zM&CQ#YSWF646>oh?^4kj=^sl;teFq1UgG!zzKvnvMU5|Y?{s)>Nx6TB2<^+53m9yn ztmzFMM9DuGC+>~ z!|16zCXVD1`fNJZdHD2^U34%GapoaF^KSIQOH z<@G!raAa6Wx>13SVL< zzdXNEJ7kC_$0m1hdA=BuhcxO=IJWQxKI8&HOU4%9TsAxK)fiH0%E=75jUd@DdVv#4 z)}PA65JmQx=TwC79$r}?SBV^E!5h=Sk0CYJ=%B8gS*+GK3j$~^1tJwho01d^}o#t!V)&&APlju&aJ^e;d0{1 z;qb$8%-q8kKp$tLJ@eNFLtw~Evko{gKr@mdWH-P{Id}>)_vGs)czWDnx|81a-vJz2 zm$&xM67>My$c!_tqwqx+YF2@9KvsN;rm@$coO99Vle)p8&TqBn4sC!>wF+D5(9%h8 z(5TV;oS-h^5JLq@L6^HLrJFoA!I)yDv-#M;W18hBf?(XbQQXE4mP@k<>Y@(lyPUqT zBwn7rvP4my{sf}sql(VOoSo=B*ggS?cu4!M3Wq(dRf+O0ON6*P8~;OP`*J4XG4ld8{=uE zE~vLsV1vgDJg&h((c23YtXx`E)vBlpTB=1>^=`6Iu6~QCdR6eGZ59tbX(^D&2Axn# zX4>)+W672bv#Z4%{}GW|Wvo?Rri&)htKSHxH@8@vfHX4e>Bi0g3z#Aop~fVk@eS8g zWQU~l`1;Vm;6Er zAc`BAYfp#SR~@gq?CfLs zqt>eyrb#b=)hD8w7ZnTxml&>rBKiGxtBxWojR!~C1qqMEdzs+4RcgYC8nKScY^OJ?m znfSXu(G9jNDK}Beh#E{kFCmxhNI4KqZ6uKdVhxOz&F55y?C(~F#$~{5B^aQf_^*JJ zS|pS~(@oxxr|TGcqX$2pBgG7aF_p&*-K#rKH%sS~Mr^dFi8U{HGY&$f(xgl;sc~L? zOe|tY;2$F@PZ@4>YGo|1dNX96H&$bq^9+(1xnk|NsZNiPg0ePQ=Q`P+E>^7};5xv6z;5SKr#a{?tLeoJvKEt%v{ z(6c&gbosfz0@XCf!|g%4Ac&pQasGvHeFxy?w1fXmFe+@|)j0ThhIjfTA`pU4CMtGG z>m3@v8qg#G%p*-UN=rAGrxVbg+6B|q5ywr>0wFvDa4MJiyAl6)B|DsO0sc%G)hNI# z@fP6BZITB-ut`|4E=N5XP*pwTN0w~OsQ_V$C8VqL4ReHm5bfP3mr!Cr<+CeoVPJzo zC}jCJ1&~t&&lG&P=?p~H9@QhrhB8^%nZSkZI2KJ%1N2y7JSh-rJ3I?XZw;X z@EY4@c&;!a)3I(U9~TMXWocS=pK_F#?Xz%Pn@`qm2B+wQ8>n7Q>?~ER2ZtjIyx7ub zbR!=z3DCv}ZDBrILBB8|YgnR1wx)+!2{}>`$2m4t_!Hb{IrD6B1klzNUvo>5&jGPs zy<7kwDWnzHem~YbD3lQpILATndq_D7mu#bca4u(dggJ^j1)O@;XkMO0HNb1yM=L%@ z#Fw2(UjU>DRP`M|#h@cOe(oRxeS$%~(rLmK;S7PP7ye;OT@fl?zEF-a`bA>d2^50i z7ZGCYbzsjewg}urd-d@_L4&iBB|vi90CV@y4>)hJ`N5(AWXlrq39%=%si2F= z{X!Cm=4HL67tl;y3}S6re1>38O{z@_V|oHxvbe-0PVLARX48l;W$j+B zVsFt&Zl?_*3GU_bQMWAAh-8?*-kmjag`Nno?`ct-l5pJoOi0I|lPQZ9yGiGEQk^&; zn&*rrZYlHzY9K@z9fmJWp3i6t5l8*m24}mcA53oossqy3w`D;1jpe!oV&o~w__pJ! zt)RW&W-M{<+xqA<(Qeu$~b zIWw_2GNDgs$Rs$(PUz`rI+O32)gSrO0kEbwqoNeE%C)-=X3d*UdX4u>u8kIkA>?P5 z;_k2*NTKBO+QVXs%aR-b`ORjUU0WDkL96+lOckIWg4$^_X_n%`_n47V>-^EJT4I*A z3aL)WF(xu%=16sgpKV&>?j)39uIkXGVuO$5HUug<3~32yl^*!oJrVH08vnMK`DZ3{6nwHC|4*2Z>9$_sEDEJM>wQ61^tX;-1t#{9NI{wag+-yrHyZJfO z`qSk|UsM{cgcA`OM|S(@`DoG)DNDyzmLEa$|HuhNYs{~A11cD^LkV-0BQ9|O}8o$!ZsYV)V)6*=hjhCkI82$~M}*l9!uO^ywj z@Bk}dZ_MM^o}!a%*aC1bG)+E-%o3Z4SOCKLjSAW6P>z@B z@kyUx60UR_aeFmGr0a!cSXI$>^X5)i!c7vU(so1nB}QDCS?3CvYel44D&W2JGC#}l z=Y~w_Tqvz)`A;U;Jz1;pZY%vnh%~fy56Njwt*Q)zI13AcckBdhAVXs z*mevshhKq)D|8pxb`*htg4%o*oe(DOQG6x|hX8{fk0hiFr7zeWI83(1d_LI`vARA* zm0QeSmYF|MivB5wT=E@R{jRg_Tl#EjmoNUewDZ?W03tG@q4beNDoZc++Mr!@O>CRo zsq_W(WxC$|>eRe}G0P)l3xJ4taiX>09Uez>gvj~W$JqmSg9zIBxr$-=-;=_nUrFJv zffEA`&L^g9%&LR9T14)L@Bv9bR@9nZ#~Vq6<|7hVc=HwEB}{ocI0nMH0pstaN($<= zz!C}ZwUOdS+d%Y(AbO%5Lf$9Y8w~fqxbVBN)A-eo>~@nIT9vuzUtEZX#wj>ecK+~|<1a3h?vxl~VkLz6;6gTS z8nvDJrp@*(MF+1G41EK)$Nk_!>ax;0bLnur(ni|u&<`$jG)QO% zab9%p`ERpALx#*KXv?cL7X=1|1%oaQ@CDTXwz*&V;4&00W*`T+~dGVStz zR~{q(r{%H1b;n_e=f>xhpQ;CEZ@Foo5DK$!#EYmd(bRMVK+6%nPIb;=DS zbebu5lU7Tf?3F0V9RS51?ctRdY-#3}IaouQx`1gnjZ`ORxWoV1Xe%=hn*p9F9jpw~B;NrJdMBM-M z4q)?HJ5=9m{zQ)GM0Cd|x~Ys`Ej%XK`@{kr^3B^nzeV+j=q0K_tabGC=&Ue%F>^Ca zZgM`8X`{h+#LqgT5q0J;D{+?RshJO^?{>z3(DY~?4f}WEH1du{@MP?uaz^8V z;@`WwYuJ0xW_T6)o%=2z^3tnf*pamY63x$CTv1}_&mlCkvNNnSRdkWy4%r*H&f@);UWh1?S~ z!f9&+Y~LDrGdE?Z(0YUIk& zIAqoQ3O`pQ{ar!rylUL>nuo~C!E3Me;7FUfOox{Ik-R83t4&AlRZDJ2UQU$Xt2L{~ z`3L!JQBiKr8;G#Dhw!N=S-2Mi^Y=02&k@Wm8LE9{?eqI(c<*MIs-#I% zBicuoIeDMt_dK-SEhJk?$oZ5WyhR^1yXV0|Q8X@Lyn1C#kn5teOY>Y5ItdBy$jn_ldOGydZD5)|_t&X9Sb12Wz+V?^a zdyPj<9F1nKK@xvnb5h`vDx`|l`ISA|x$4y@28~6N$+WJ@d555(*q+cYO;m#!4eHp! zS_5gj-{Qw`r9s&vbb8XRt*>@HP8IRAKr;_Sx5mIfn=1~Ux5LvA@=Iwz_j?H(rh|>HsBlUS)sg| zBE9HqAVkIiFfKvGaQMF?+hIf!bta9@!4W`;RRd?nepJZ8m#E}jR%X({XnNRSu)iu~ zW0EB#$dwCo43Uot`36fp#%JpA+DjeSpaRaQ>N+2GiQs`rn1>Jnq}I2n$Ws;2YBD&Qw$IU6Blo!O3v)~TQT>jwmYdsZy6}>zU~rtJ_;Uef|GmLQx!ls$EF8Y&R7SKP;;k6sm!+9C+UDyZOCKS z9od$EWf7HqRZBZAR-l(TWZ0fvQA?ehz-0baBl~)d@qt7TN_ccCdileO zXm%YosDt>X{w5Y`7uLmUa_&N*|4S@1+)c75j7R>ck$)2l16?J5iG@V>g^iQ_u`32- zQ{4dn5(|<4A{G{0gGvJWOjZ@6dC&9rm0(($u1j~uc1&&AvdP`0 zCnxq2HtKu85e?2+d%s4lGTMS%K_5~jBGmL4l^n241UK=EK7;zSNN)-Q)z%(B!iZ)u zO(MuMf`3yB#s6L{Jo$%O*gNz`weZ264Xb=V%q0)V>5pom_U~$;#)n#%@}U++Kj}gq zbKfP~teNK7{upmT({3#dnjHq2W&f92==le=ko|wE7CQV=3n}qvB>jq!&( zb>rXvsusq6sD)0h`BX0I*vbame^LvlIsd5^R{u*aB>6A3aKNI1fj7MS1q@PrM>s`A zeDj&Q`EZqVGG63DS0+!}iZ^*Da1?b^-MS3NZNR!DEd+l*T4l7it4SkuuIHgD<(RPy z-I7O;%;Zs1FVR|*da!oF|@_V z5JeIK9om}-wlrWNw4uK0gAL`4^^+EAKG;wYSM#?09u{7QRHF`$vRp$lsH*XPCD~)F zL;vcK`C{^O0mHX0Dt=UtPhHtqZp16P5D*?M=n$DVV?Eq#aRgi15ZzT-F7PR3lS2>} z@$g;p6EN6}miiD(I?-h5{ZQc8z!j}s80+Ng_+=z#Evo7pdeZg zjB$DcI^%#}`IV9LZu_^?;-k4RiImUY*QDG@AB_1wbs2~q5U68?2*OtY8TPG!sb@tF0wE=2elRxue? zRh=BZ&i!m|Ye=m+IR*Dhr$swWR1`4j=`ST-(5F0jT@zVhUS=_tt> zKKEt2czg^P**WMWTyra#Cf*fC+UE0&+M*W?!N|iOPFeR04LBKxtUN!dv;~CE1PBs7 zeJMOeDj>KUZ(?_*$v&C#=Mn%?p7}lk20~VM&`)lJ`UvTe7+-(<#lB}|OA*=8(n0am zjL4>+V(*n2{RfCq*$4}jq2=QIT`kQXP=f4M*xh`!ICNSfF*DhLg1wR7TT%dRuxN)u z+vE>9EL+q2y$Ew#+tEcgjOJ4(P6`$`As*QK6S*L#NjMK@2+n$P9q5ueO0`6DfIGt# zOb98Dp2bXM#<59NsoV#tL+9(AiQ|q2uh9jf%Cx|1R1zt^pz|IECUyzFWxtFH1@6S= zbg)LUPxXCp-8w9Lg(*gP3bzWBOD|xxV}rM)kmOQSXwvK`jwd2o5ZANrKnU}9PIZ0J zBK<$B5rY->%U`qu#f3d4^)xMrrGB1hTmy~3MuX*KSnroMAHJ?+tl6Bd&rM_MIqAxK zrxK5|jW#l~vW6X+cL|lKQrmK7S>d%K-X-i|?b){`y=lz-$R#GwFRvLm`%KKGC!v;W z&umUOtD$bgfIlqLu*KE=fS_&%$j(qL_Wgm_lTwZhF^b1n-NUpIelM6h73=wH3U5K< zPpIiQ6|tmMUAn^M_(z;UeLKQn>+|;dQ_CoVuoJ-(=1d{wLR5L%XdZ5SvkP0u*};aPL4xQ&e+*|LjdXLVnJt*Do7=H%5{u%XL-p8x z>(yW>Gdn@5u-^A^bIH`3_aSNoqGM{5IvQM-+GPq z=e#T*A}zr0<2aEsC;$NNA2;UzwT1hK4e#D~X41nX|Ie(NOXflPxNAfCu((4dIG<;hl)p=F5+k(BZ(M0t^2mo^!2@OW8;uW5!6 z#UC%#NRXqkN-U_xg*&jqVsh(uMmVyCsTXBt_n=Z@7!~2GT1L>{eawDRyLrOvD>p~V zcu~Q5o;Y&!YZ9WgOgQ=-*_CU8>#D*uNY>TT&ayNs^&cv%ekpx=c)9XivC8J5*CFrP z-j;ygzn4f+Kt9+Tr_{K#+a*s@{E?>zF(?qN`XyOVEY8S7@A?V`8p*IbC{PX66*uVE zrSoTrplgxf&%~S+-^lL8qc6^2r=H{*n^V)cG;&vJc#Srlc;twOA(_{mYm`DUnwm-#Bt_LnPW zMc#tdMn9ff%D2;O5|HZ0+!mBMZO(PnY4>$K>}LAf~u2yXoP5HrokpDr6O6 z6(r)Ue18xmhgDY8SKj){SRZ}Wj!ts6~ZlkCP>-4c828Xmh(K>+9 z8tX4qh1aq=I6(%&NczT)eb6xpT{a>19p{=nnXS`YC5u(@HE0r zx*S|ZynE>-t8Ydfg-{O;KsQJFUJCfWZX=}6YC!Znfqa*}IACL}-(XLqITuKCjJE7~>pR~5=TO%w z^)#sZ@C^D#xc~Wt%intDw>s<}7ex$1cG1HNeu0HT`~EHJ ztrd`!hIU`)j9p6o>j<$9vKl8<_JBRSk>CtYGTmuflD4n|8BZvmKw*E?^juZ;obl?p3n8tHh zd`uMg&@ghz9pv$hx0XSc7HxEP%XI#^J$7?WQq$;)^oLwP6_OvgC2fDUTME8#^gp-z z_kD?fx0{Om4|X%rFCRK zx=aMLEMS8}blnuXt1snqq>PO7&LNC-XaRn@Hd(&8`m&dEkE*^LJZUpFR-(muDG6j{ zp~XGp`Ii$-Nf=+I|8u*Ue>xNT?*8FIuG0*)E_^X&2uY z6ytAcTfNhntc!gOP)Mffp9hw>OsxxdsISVAzA|+*b&U$%{^gTK6})=-pWDsC_D=>d zjv9vir`^tzXd*6wHeuR{S&A?zV97U|>#P$IQ@bm}+`$ybQOP}FpDayDkkw)Rtd2RX zJs=wPTG)LyimVDrO~t#UpmTlZ;iNB7rXt`Q<5@0 zm@IWx;@)H#cR^2pR{PwWAKUR+l=1!*K`2*1_6x!(ZXyWd&@EEXZ%IUjq}Vlv9CsU`VVRbGT+KP6OrLORZB%;Gtbvs}%bw zjHlo37v1>C#eLQ|tp!o+xBBnX$NTrOr^BKuGN5$uVcA?r0jdNGz{lzGzmJxf zPW8_X_P$c;O<1M_*f9WSTEXc^H1t$qWRlc*D?81KaUcxdfio@)bS>Ls%tq)Xd!r zr`VnQ@SxGtPT(dN@v9pkc_E7nzUu4S4hBRW;A6D;_!hhvF)uvVxvdf|62q6NYJlnI zqkxb_PM@PYW=0qxYM)pWePX{xf%c^0Lj%qeBJeBQW3#CE_T!do1r}VG3OK1Cl)P8t zd@elIU4Hw4?h2<(UXA#(21B^Q5@h5aOfa`odws%fq=;3xbdel|oDB1hoTTk=bcvWWp{+To)Q}BDa;S7TYRmtjpD7 z!YS{g|94}>Ix)bMHIi4o*2`PO`Tf?FZRKkz;GYF%ELWelNIQ11-^QbxVXQ4Y_&Md8 zy#&~c`$2l(TCjVPTtw$=O3DpS85&N^PwM*RZf0-=X>OId_vvlX@YjbXG&ABL)pm#@ zJdQ3(u7Dtt>lwQS z)PH&{xNlacsI;5~ra`I}=*jeaL!P;#Trk5X$!b*PQ)ayfFKSclc)|s|p68b8Xi1|5 z-eD*b>w>OsySo>xyt*{M1sRTKjf!^JS7tnhmEa^=fTs5x1+g^oJyef8iNSITDBb3tcMCNAEwzkc@a8t zbh(7A&19$R!IEJr^)O^z_pZ@$fF1_AT^ba8ly1+CE_`KuQ(=NQS;6Jho)a3a*jrc;-&r z&KqVGZNSol(_1PC_yEqVIP^V*i;f_LF3Ff&f`hNjLSKM;nfq&Q5}Xy<=oi03I|O{` zi-rMLmUJR5yd5_IbUKd)okqL9dYy?7nVW*AkrE{NQYC~8d+IPnWQ`wqk6AFC zV(#mG?A1nA&LNUTx1Ha-zYmY#-g>+L#JFwR_(K15>Ai4v(^1K7v3}dv8V$0wx)D~v z+C6R-1A?69r4v5-$U|Gw1wnoAXmxvT&X|ogPo4L=zFIYUadgIl&EVnn^)*G9+DFj# z69xWf{GoTujl|ZXF3{!7Zk)aMEH3i5mJnUAPf&b454Xr>92<$Aqh%=I@O45LOG0(T z;E8LuPM4f=v8uf$>0gUQ)@~0)fQhV{U7}gUcT-<>q0e(w*;*t0bl+gOFfT$1tfJSy z8guII#Ra%7AutGAlxH2s7y~lkE+-Gv5ym6yzLuoM7{v-`lA+l@y@$5{rRHh}1m79_ zsJAd+{wm)6<#R*-+pg|vqPFc4J#x_bJyn^~5(@J2qY+R^y?VZcv9KZDQ~^YOr*#ct zvwd2`+w&rzDIg-)lD3rKr@)7va|fNssL>6hWZDkdu9|?!?7+xu5b%gau51@UDzt{x z4i1b~OsPyp{;5eS08pe09*Mz;CW9};JBo7$Sk>vUcB`f^7^<~!bU4;=f+3tI5Rb)M4NQ?oZ=KvZb6QdDZBG+bi4Y|3Gc3Zs4ym~#^!k0V z+;P2&)Qi~*3*eEu#wstgJ)Eck3B%*NEDi{oBk?lQQai>N>)&3KjzfTwq7yWycci{m z6E^IsHaay#0}K3yGx2ecHJT+<$-um)4Dkzr@=`lTG~2G-Dkj({B6&JFwQj&rSwI$9tinpX3pA?v_)3r zN-f#FEl6VNh1L6lYMb!)Hj-kbMQ^O>gQW8y@R zU(AmmIh~_;E45+DJWs;->l-~&f){`j*hS2o(|-K)vkSdD$eNb9@!%`LCA*Ehc$~Im zd13FhT3MDgF(7te(O!eNYnp8rr!dRmV@@0}aK(R!R{gp;9}%1?m>`TxT91o&S05x+ zPFB^RI!zRWfLXTkWl6tNO|n;%-XlhJLb?g#R=xivIDDbpFY%L~lz#j!jS{Y}r>kv6 z!U0yasYlm~eBOPL@R_lHX{eI1g=iJXeA#J3=xb1@|6%B(1HI|nNJf%=Krp7khOZv! z;U#gS7QTPhN8atfoH&@OQwU!JcnZEl|C^_Pp7}RlXD0paDR`x(dBlL3gv^JMuNa$- zS@pwi6i+Xt0{cdcg2V1C@$3Cu_8S42&;gP7Zu~9J(Zn_Md%9GTpUU`6Kpe23iWL-H z=m{6$7%-a0%-wdR_YC5eLlomL5T6e)ED$OdnuASMs{>WQr<0}ZbiZnPYGg}+CvpYN=LUA!&MAyvItQl_e`@wO$JwF1 zNhZ=mFT=c4s)UL$#F81JhoxdHG&vqyiSMCPM2TAgi5N;@1>z(>$PRjoYj} z=urkA8B629t!ib#@c5FjtkxJj@q@tbN5vWReP&-8#}KnrUcouzR~(1fS=Q<)u=BD?)Hkx_;l?b%fqf|vH@(f*I6dXxafWJct}jlIfBFv_3qfA#O8~F=xwxBT zR~FnZJoG9`rvwOQ)N0Y5+k9s6SE)nHjB;~|N`pU@Qn%i)wXK{wkzbfAJhAP*!NA&E+O9gX%uHql^KJ{3Vhvlfzw$mn|5+jUp@~Ac z0`7LVe|NVt{&8#t|9Q9P-We$-IaPkKBB?1mWgpQ|` zD)Fk95||FP+n+*Xyam&WI@Rzp0oul*P7^xgJ!-Hg44ttaBMR!+rhZ13-*fby9%PkV z;%^BcPWw;iJ#OoPrSo&!+_eX`~>p}q9M{skgP!5s%a6X3Bf?8F8Kt7-Ql@L z@@KQd!wsHn*d>Iz%~7|Gj7XiMlTqZ|%y6`{qt-08{GA{s^l`tn=n`O+1AnEeFVnZN zkY4XO0m?J$(P(VHaQ1B-+Fp-OAzy>ZP5IIKisr5pPXMNzN~TMo5oKFsA?^O9sJTdt zC>q(M^xc~b8r=)YW^q4uFdunQZJuGr*78iaxTv6O(%Z3~lv90hB=To@Wn71WEIvbx z!%yB%_!f`o8O$N1k6VGl)g&{`b;vZ}!=H`z13GTvWyu7NxKLG$-958BkbeHa@qK@saJ}o~0m6`snS9 z&E9l9XZo;GX3j|Q3;Dl_5HFcCVoN~GxC1!C?4RBKKa$)9>5a`5vZdEXkPx)Q!0M2}WfAzuOs7zo%b);Sew*Pd08;9` z%KNNmDAh-UHO}yp$MB=jD}`=64)Jmyv^);MPo72D!LBbh*_LhM3w!wn$xSq=Oh-C7 zAE_LW6qbU5g+bU0ol#biq*G*~4sqHhb&jDGlbVRwIE6zgNZ3NYt}<|8 zkF&mY31x3>J`KNo(|&eBf2!W*G#%m4K3O|)YzyN7eSmpbHm zpFnKYHbr<%b6@ySv?P{Jw20SF@;L%9TDom|@~Y%tfD*!ig)#kW@V~)nhUlp#hXF^# zW6Q>u0gI^k&)|Q9?~v{DLtz$#Z~g(%PtF{p2tUUn4EW2HvOl=7KPdEyNeN&Y+`E;d z^QEQM94&X*Ih~@pO|W`mFs@|6Mk!MVt$NQH39!vC%Iip`;ce`|#jLxsDgm{3Vu5+1 z#{cXt=82P+fH1pm-G5g&_H}w)Ys5?G^3oM>lyLm%=TY(Tk;>O)K)zw?6E6#C9+=OE zTE8bU1J^G&_q<^9>3k$X8?Xd?$b-Zqf3-Olr@u6b|7F^vv?Q}K}n_o2nuZA05( zZq1G7S#W+}U`|3~Y|&S%*RaA7*q$3IV>mT-o#!1hR~~9YLsg<82bz%8YL?V&2CEeb zaiQ@g_wxjE&?EB83A$?m<_lyRbHfb~rIZQGnnvx4-P=a5KeF%eBKd`zB??>$b( zX_lka^0X(zJ8raiLH!%8f$fU?f3Oet05z(!J%W4W2KrXBD&A)=O41zT%Ru0F_yVax zjzmJyo)Tsly!yH*l zM8BCCbWQECdSxxS4vn-aeKOsmkMu;zJl!6iX2x|jJQ2~6xewTQ#C_j5o)~#!H)g}_ zn5mCXedvL|F7cA$O1@TmIY#su|29#Jz`UO__yeM+`{_2HcMUsBsJRDk4>f|A_?>FOWX`NTfV^@O$d_X0|&QfnMq*gMd?FLmW~*UP%z% z7Z#aI`}Vgok7HN9b{q8<4WP`^_~TgF!YX2}jn1x)xrS#$kJ4`%mXxDPq1 z9F7PLc-7B|(6vVKRcoL zFa)0*x=e1?cDT66Jux?HyRs{|pT4D>>o zef&)~kng=`bhXT&3h?dE#+3*TTV#>ckf$&sBoTNi(YHRj#-Vxn0@g1@A2!fT3j6Y7UO_Lj<^QbOb z?x$u}Th6kP$u$=7dIFDK<}Exr_IHly@CTSwoVOOJuv*Q z(SKyjBtznQhH;??y*)HOj)u295NiTMpiKd$O`+5vN%U;cn@n;aX&NDryevdv*^k2R z@x+aYg8fRdYeNSOR^kp4dMQDO%pA*NW^?RlUiuGx@jDXxewJI&=Eer7Vd5_g(R!E< zXD=jlX)l2xoHpaM9SvH0@pS}|gK0DRM^X7>A*>I^?q%IMyw5|68CjOZ=qAQ6=!!sC zLxQ7`C&P`I`hvIVDPe<|`x-g-$=82pq?rbRb?lRW(~$@RzYyMYD3|Vp9A9GL(Pf6g zJSWkV-GOUZRF5bPYrB9lNB6s^en(kj;gd#}-9SmGQJM)3G&A3W9M~=>s0R-aX4ZZ8 znmeEqsyIJe=VS74U%MY8YI5W9Mm>3g+$xt@!^U#YW1n39>Syl@*9 zts-u@5sgq7Y1v6g!FS|@8Z^qJ_8-JayR=}2zb`*K_siq1{`m&KbAKs^Ww+k@W_8=u zH$~UwPxalW&dYpC!SJdjRgtf1#R_u5Rlitn;+A4D3gE1|^xEwyzbWgf3waE&FS7B`YX#T)Tuk&Q69 zq-~8RYtxkc$}h<7U6bu1qQ<80*w)_sP6M;pI`|pEY5D9?N~>MXjUS4@ z_xUX*f(Qpiw%D2el`-O@Vj{>1tqfcINo0h0r>K&Gt$LUO__GA$aCV|#Mp9KJWNrjR zsc>&7?&(@I1{y_g=j3W7qKu0~-1zvhfTxlx%}J83I>M49g2zq2C6K)VMx9yA3a0$b z5d-SGTWP(r`*i8FUpaZe!&N|jRPS+)WAy#38<9s{yRHKJg7{_v3^bHV$RWtuUrsDS zpxb^hYFcqF41Q}rLD3<(CQ&O;qtRh$xf@Z+7`(U)+@{zFzi4>7KevZ{s;~~%Usa8P z_Np{=Zep6ZM5VIy1GORwl=_|{C83A<*>p~M88DNXNa{3IB*w=joR^zFs)55Ch&BPY zlKV2iCzMM>Am5-hwpz3(WKvweWK!3w&-}xXc*HDAO!<94Atgg17(hI8Hd@ zUaCt1$~(dYXRbXGPBzmZZnCWa^pMah#V~HMm3?GWil7|Eay-=%gd>u8I>B=O z5Xytqx0tG$9@RJYiU4TzbG-|6g5||8bW#n0LWkwX8E{zG5JbDU#`kH&Da0Z%WP#3eS8t$KpA!tK*I_tAQE| z6H};7Nj(d`R2aI5tIww?>sRI^GKDtQ$6ZjGt(o}sYx1AHaaS0+i>tr-7!y5nE4&0u z1a9c_hHn)K*GU)1m<=MfK6~4L{7YS-Dt0iL3_yVd^#5~=``0QFjemGqtp?Vn;ki>s z#gy8}fjAu$t;JZT0}C6cQU5|8Q^0akg-@9tg@;C}hFuM0E!!76?0$*vihx-1us>nY zJlh%j{qyv(BIHm|W$bX#7{SY{qD*|7&gb`u{e_W*IS#^|{lvmdntB;+QR(~W`A3vR ziR?FMBs{Z1_SqQ!$XEzpqc-5cG=a!yqDW0C&;Kel z%Emob%oajqlD@MFxMIb(wsyKOXW?Fu%6uFS)R}gw_6&GC0S*vO>+90hn9rKJC(=eq zDmown=9#oz+rLJH&kfkIQm9FjugS2YLQpL9D`UmPbp~16|1NeZI(~?QY}QFEhcc&o zQgO&+xHX65XEo}~e88qw=drR58i#v}zEX|?wKCUW#lx#*BsE>R`SxjjQ2Xu9^&6Te zNYob*`4@CRQQi>zG@jOEDWTH(WUuk|yL7T4a~6+~_NS*ayN1P9pLDGC&Ud0h%Ubd` z6p6j8%3&-m9;-UnOQ!I}&>_5!d5QLk0?qufGg=1}ZRB5Ek#A~fBugX+_aii|_;erWQ=*bxl-63_+_6cRukiy^^=qxVCMqJ#N@@*{w` zQiAd82P5dp_${lJ6ClV|5-f8d?eoR_0_PWeeqro=c!Vd9+0FmLNyEGXxjmFMO`j0% zXSn9+B-3S6DEkA%?zOH-9GW-#GuEz&OJS>w6ld;UP_C?V&~UgwR0&CKKTIRd=qF$P zFq9*5J|`~Nr>9RVSYLiYBnq{}f%XUG@_1pa887CByBAJrljjL z(z;eICjAN{q}E6=%90?YH}6nf+v^7C-{g{rFa_pXYK-#^$xiXsxyY55Hwr!DX(%k~ z$Sti)>~3Ed0Pj_nKOw=lko&ioE}{nt|X z$xabrlJbgQ*ZQgA?4eIEX|&fc$OBvJj=`90uX7DJ8`Kq0NoRB&7AotH7Zhw+ywDfV z72n+zh;EE6s3D!1kgs~Uwm>puESar?6|cN-%&aZ}iT0ylC3UVCob}TSCRR_+-Cn7$S{T^Qr4j1M~nm#*U}2 zCYQCLjVgr0@N8!r!a;WI~!avDU^=DB* zKq}MACL*>(G=OgE4DzXo?Q8K*5c_hkr!Sj`M@N>;6ZfGlwD&6FlAAnD8E51SFCYccfzg(XFr5}Z) z|9l4b`v>OVXokOSCQ$#UUl==@IJXutO%gjl><|rNXARvxBnpN()5QLWTx^pruLQ%^ zdZCV8APrzlQ9_MZfMZ;X)fRsQY0k0D6I4g?%Dl`)?f}lRKWF6W zVPm-Q4p!e#VuvtJmN+lMIu8-l?(P|Re}Aa1puYuZ21JqXu`l19N4u_;UPqt(gD+r-A5W4wa(I0^H_?>}AVUhOq!jhBhE}4OW zS=5cE)dnaW>I1_>&E_gogMe02>T){43KV;(!s0%NOx0k5d(G0))>G;JXnNQyO0YGnJ*Sr5^9=wj_R3PR^peCL zg1-HJD1vtzR+TTxdP|FnJ5lYOI0!f%XBl@+6hCSYQWCUqLjlE1t}(}{y>ukdb$il* zQ5}=yD200;a~y97Zf+S?hspI+?t>I@8aqAS=&ePP@&j+XXl%I+_FCX!P7-EKY)cKd zQ8u_Co`XQW#6Y&p8SHU)IegCq84_)^+GQ5PHh@j{oe+ew@m>3~yVD!*f^Psv#s*a| zC4w^4Jbrn32gk$dliIugE~Tad5rN(2vd}HHolod+Gn|V{_2Vbq?_>bC={w`w&F6Hc`D+tFz&CiN&eG3+%;~ zdhcLHsWmPLV@h^!VP8?^=1k*b_{Hj>bP5&oHv1l8_|>5cmu4!epl$bHQ@$P}JW`h# zgD}O$sv9;YQ{@N06{}5-=o+Xim84ec0-_mH6#iU9lVK0h_B}DRp?0xr zf?HTH?H5lH;iiQ7p@fkyQ{f}~57eY5dEX)HPu;SNH;q`ERVB%%lq-~RRepG7+_yVC zN>8HDwGt&TzTEii$<)Z~rfh9G8oAOh@kda1`|P^P>fopOgT~c1g{{q%Q)gPi$FSEB ze%a5UUi79wM*;>{S}^(?Ib=^VMj5C8COCVnOjy957XZ8Ei0JoJfg5CE<$xSXGT!{)!cTEd5!FE+aN3%_3vy0 zv-+TXzG|F}2+hC`t=}I}3`NRE`yfiUf^%MZ1ObWxU{4kN7G2PSe{~J@<+>uet9NTtLH1&{lEaM;ghaZN&+pMw z11CQ%(F3=!hQ#Z%;;B|NBT>0>X)kiTzqMQ%$;EJlC`(oXJx@|$M+o7WCsZZS_ zx(Zr&2?V))V&*^*>YpJ6c!S?ZR!`Ues0MA`#kwv$SuJKO-3PP4cg63Y=Z0aMZCX_= zzpkk|Jht|yaSq>jv&diFM1}4PjiUhX-@V1(tii8Nv%euA( z9dIo!W98`X7$4iVx%!#3HuGm9aEg5hEQtjkaAJ+9Lg3~-SdcfhmOtOK!b9!hGD|Di z>~T>N0aQb9gjI$q<|KX8tdcKutxb-wSst0uI7-y;4ji53|7*L zhjV(Z5mm-T0`6R1c*0ZamB3_ua~)YpEP=}=XchQixC!zsrYsf+(x^W*NVi5{1%l6x zAvjratmX{FW`=!qrA#)V%-btDxP6u}sj zJeO*nDV=Aq&EM}=ifdG@D4r$lSA*jgJRTxtys%~3m&4-_L+Lwn=uKL@`}e7Nt&w{1 z_nueTu82SvAyf13t&jPHL^Xs{^P4K8Yh6%7PC+yr$nbro&)}Fya2j|(x1@5^FqhLp zWoey}1P%~Z*TBRZAAZ5qpMqQ{`x;XMWCPa<*-xFNZ6&^+7*G`K;(#|l#t{EYLa7x# zw`G`{5sMIosISQE(yG>dLBYUSibbh@LeQioGbk{ovcT3Ag#!+ax6$Z zmlm>c{_P&jNTqVA0eXRI^)i)!US*cUTZVciD{OHyFEyjoBln^AvO4jdimf&~AXiMaSbkzl@I%il@RbVn6Hg2}&0ko*5r5_J7L3D)ug zNU+ErZv1QVvJCkDjReuZ52y@G-^M&`)t~DCJC;W8i3p^2hW&JW{dkaGGTxghu~utM zAsWbzBT+r5h8&pv;a+_2)5n`yt=cL1NUWtRy<&8bLfd@>a z(IP2$LR8!{qe2?ri#;0^Wi$4`+}>LNgY7&z%YYWOa%a|X_rFKM zn}36Xv8vM?+6(*ZT((Coev9BEY++JfJ?D1gbdeoDh2o3w4s>pSDr(r(1|N;YuOESU zwFWn}LNI^lO_t=ICU(n#91sAE%T45fXfPMCrH$%f^TM#}PGqq(Ah!Z4LV_&s5+oR? z+4@6wMI5p}5UMo}=)%L%LGCfBEvlkAIJ_{NQUW=)rBC_J!qLr})hM-}9)8DDys{!S z9`BI7r$?H|#f?+If>Lz2Ge<}lpipufH7=1ieN);gH<5tADelc2-*0P`38hpNa|3kE z^4ygVD!dR<(E5vo(44_QtJ0D|(&oWMo8QI0rI)93WvG9;oVuOzU6cLQPNDvSyUY|C zJb66gIe!gNT_;-`%xg=2!Zfwc-Eqp$PfyXTsKfd2_?Tag{b4dN*qZgg?)bXad>`Mo z-FM)nMNP%13$?{jrCdo+@h4Oy)+{8A=M}m z%DAbR>uM}1yT>^0tB6PzQ8^LEPP_UsST<6UWteUAoOHjIp_EPj_0W3>lHhFW-M*TO zGbsy$_OUUyAq5|-tOly*+!R#X=e!&S-n`8fRA?1%Si$;qwA@QH&+aV+h9wL(tC)vt z#cbG5sQWCK;%_K80f?QbgqzO4dr79Q-?XPlS_5Jyo=Te*CvF+WvsxTYn!?yK>R_&TtNN~nW~POj}Ih4Xl}!)R75HSKXHq&*nAoMpt*vu`+nea;<9Tv=h2 z^=#ODZ2w7uF(5FdrDGh}(FdYQ3NS@bA^9^E0^KhYEQlH>_nQTieAoDa2Qta}cHI{v{)(PV|3ptr z-nUNhe}}<+Rpq?v7G?4+&_fFx{A%n#SZ9>4#kN7}78_B3=;`El^fd8T^kn__=m}C| zZhjE4(LXt976S=;!RH=UKDpi4xPY$5S)G|`rX(ofBcgKn zfg>wLv3b9tY9a)wQGNQgz`(#C?yF6UQVql8$C8NEAHm_!r_|2-6FB|Oo;3e6dup{* zm40WhZ_WOu=(JKjfuXwb6{q;LR!`~F;=&8#R54-ZtShp8-zi^+@>{yirz*|SmOHdqS(CCnM+;87)f zsu2`cgc}4UZ1^;^ns={~Vov#jV=!y%1>cR+?!bzSyYOHC1qeSm*>;EgFMyEcPZjom z2ZTOr{|6w%mcaZw5Q>EnKLYvefUSQ=#xqlyf0X}7I{ffKx(5=FMr4~l#D{vveIU0e za{$$;&c3;SsmXf3_c<*!11-u{UN-4HOA*%Y_M98gcUT!rYg-~+T&K@s#Al_^j^c$x zjDFM(+ZztgW?EvwvG5BC1a?iURokg=R-)lXEcpdiE3;#juN6lcsZgBR5O+wK-;;(xwjF__9%k~9CPUHQZC2LRBAv(hEM|R5=_z_{DrE6a< z7ON~U|FUqvIV;haA`({dvn5e`TQ%pY4R%a|!#tZ*N91jONJd^5E<>YTt`^%6n|>Jx zs^J!25oTBO%Zh->WSw z)vypUqH0Yw3%|icT|h(kl~melDAx+pst;24_GB@qQN|JHiO+cWVaS~Axfh>3h)VmA zyZ=o;g4T3g?Gm5cBssE^inF})4Y1tS?!}A| zk(@aWQ1}tyim7fFRMSQ9Mq{G+@XrCVE&dNIzHt@rT_6>$d~HfOYB2SQAWd+qZqaY7 zAXOlKO?-SKa^~2t?L}b#Bs4ntXC$0m4}wPoWhjh9nVadsBI4;U9Ji;k2^UOstRbL1yXqQ1!cSvXvB6s48f4lne$zl*PVz!ZBbaGM zdMK_grgI`ry^XcB#95Gtc6)pO!^?en zG3EJ|crlo`UDUIk8_+?@rJgbgIQ_Q&iL{$QypQ06MdAvkeYZ^i*j4n+M*X1+h+|>s zn)rvLj?nj3PjB}ki@NJ$#U{xA1_`SqRcUkzUt_=06`isv`)*C;tjRuygVFDWP_snM z3`6)ZfA%?jUOq@y@Z}EJ2%WTLyZ!{91g;=EG>>cR2FgEQ&p3H6m`Rme3_pY85lx8+ z^|e`hJ4FtKrrlP6$SYGgM;CsqoKk|#=qpCfHpLh71|qucG>}GxCW3%b)+&F_)0v~G zmhyW-wpy(^E=ytg(>+V4m!D(L88JVjXjI}~QER*Pyzg+`<+jP+Zv@If4&5wxy~LrD z$xwCdun|f6j|{Kd^=}Pk*`!m37~}4t+VN(Cfd74$5);uk4@#VcuP>fZ&-*ynjuKVR1n7rld9}nnV3W|PO?OcXyY*#Ii^Py{Wu~4HjG?j}`CmY-m z&oTX?8;C8f9EREX=i7+$ax^->e5+qFsC!E;9Dw9d;Y9A(1z&*#6q_!z@;q+qFbm&$ z-u)@Xyc!UDY^<=MmGYN1K$~igfC*{R@Q_*>%F(5w(JkuWSJd2+o33CQr$(VdF2m=TzrMaqdqbp|Qp?lnp1w2 zv+^xwzaHqiI@4@?(Z19YvY_9BQZ_*LTUmr?2U<}>$DVh-g$S$XjwxE7PgHx45#D>E zN4G?S1vDR|Ie83LIv&F?e$!F$oO(gFyPn4mzheGPH@D>F9znVxde*M5= zD;fhACCB376~FMywkOH>)KOg&mhdC9^m`vsS zAO5k36tv(AtHFNq-^jtJt*NWuMWlZx2mjja^$#ovC?aA0H$|jBat5$W=G5y=m8{kI*Ar~OYm2q+@ekcE8z6jOwijVT?jF)(`@^CF}Ekm`nWmTV|w zP?)^F(=fEFTVc_=ql%w_5L}qmsph~~ASN5200t-`Wd+T`-qu42THQylefKrk;HHNS z@;daw&j?PrS%8l|?BxRod<8}0Y#*`cv$JT zaTh`p<@J@AdC?foEB<*9)PXvct%Q)^Gr zHUaElQ+Z(|c+2*Yy7?pZw+ogZy<}|DWb(@akgN4*EHl0=R)JCy`U+|aF8Uc@0Fm<5 zqO`R^PEmAbA|^A*n?p`c9T}!xvY+U2&m*HsiY2 zrxoWRJKXmh^gH-Kjp^Uf0{^ezne&3tC>-K<<9@9iL?N|S^*;WTMpu$|7jvo{b?fM78uu5wXN_!rr5J2 z;5GVr+@zgQ4#k9IkkB;g`S1C_mCZ@^*{oIG_{ zY%?ut|2CSwM+g5B6_Z@=j7KPjB%yBcOUf*Z5W70g;{t{{)C(F`*F-(EB~E!(rFB-^ zxhPlYZ(taI3;@Hduts+>(ecFkxGe-NGtxDMnN>)pc!flnW$oq94|Q+8=JjWaFA2tP z#X4`)NB`pYvvwoq69IOR{_FqTCH!lH#)Y~SU~C5IVx6wAzk*j)^n z`>Tn>q;5o?S3I~R-7MI)DhW&OWoWEE%2hPe{?552r2Z;}j4(9X$=383 zI|`Hmaj$B~ZPfa;^27P*)bt^RuKiQzY<*tILyyk3!Mh(A&tBc#jtx*M_e8B5w!Ktd z5x*`@7e0KUMjs<=GMlc4pPgyBms{FtsX1s}9VvqRj+C>+-#evPuM@(nYyiSW)*>WA7lR(27xJYD#}IDmo@&dlo>3cQnPE@W9Qd%@n$mm%DJ}y!Y>X}qn2{keund?XXQ>ZT>1I2W zO|c9C z`8V!fy6i&O+S~Kp z6M~41dQC-fbADu>^RaWbZ#W|`@~RT%@TjV}JD;yy=T-M<*7Y$u>qD;Mmdo~yh~IQK znG9up{sqi=^Yfk@@oWl<*r#?1xIasmEq6*UTp~o#=n8uwJ^P*F0QM2HkmPyg(pLvy z2T%EqHb4YxlBeA_&ep$f?o2|6dHNi zWrKp-4e!s7}Azz!1>52ozZ?%L4@F0swa;ctbfv-M%{K}U>{ zs%(hQ7=#a0yt5wSo+B?9xi|-T8Z68jMPIxxZ%I5S8_nt>lwLD$$I;JyM<>?ek9!c4 z8`|Ud7ar8)#`M3Psrwh+_;#OYTj0+Quyx9SV~eFf=!~5VC7{2aU6I9~RVCyA?ZjUpC$Nq&YHnQ#C8x4x)gK%wr0boO*J1*H^@xklMiB+gUD^mC;tFg2~MT` z0>uDiH%)1NLb-+H-!}S8<8qIdIF8+PfaZJ`#vaPA_-SL*P;Z)I$Mqq?pUw-=1g>lR zEJzHcptp5D{~(c#ePhCWP_@PE+X&Ov@jHYDKP^)kcG70>ucYx$#dlegnYo-HIEoGv zWuU|q+)c6wNl4sQPFrgMLD?DhSoz}7W8HZQ*2|0o-YbxK2i)Gw=W|3MxKcWP$#SBh zp0t{z)+9c+Ukg4q2#KGotK@c&q;;9A(ruwHsCGlT3tf=AsNcqN3cq$Av^D}$o`_x)7~m_7 z0zo=dGLC7UKzDsScM#&Qt=^k+<580sd6dXl|A7@5U?dld3{KIqGAmaB)gJKaTND2d zLo6^Y+2FJV`QU=j{+b{oLj#F3DttZ?ya|r9%M4!wB6Wk{*Y7Nxp+V=rxWYO~nqU*o zk0owMbnZx7w6ab*_5!N&3k_1uQgtx z`nue!(saujSI^C$(_r!*DZadsH6?dz21=0k(Y(`g4`%aVy+|Ewa#4zFTw(v|6rHiH zDn-qG{ipiVsd6ab(mYQlI>Rb`rTT%XZVGYunQ6=~(5J0`F2^UTe+>L@n3Vm`!is-j zQkq9*t8NJKya018>1z6quqKudpp9m2>UYHK*dd=+0RNe6CuAmLGOPY||^Cd7ytM?Hq0Rx*VNKnDV z-@_n5KNs38u1l((WA32wPcwHwM%)ST{!LG&#Y1t3xb;j=tzY@wBGGrY@9ao^D)vD# zC+=Oui8Z4Laa{Lc5$oVA2I$GdIYHZbHu#D^da@!6cd^NC7oaCe_W!OYd!I@0UGji3BU8|%unPzPN*1(%t{;T)KuSQCg7;Q z{Bq32HWSkQw%{V=kgPAG@vWkZ=ZqPn4xThy%=uN8A$_VdbYvG-MaIRz3Yg26DlyP*+Ys%)CB8z!Ya8NM?SbDBn^% zJE$>Frak^Ym^3q%0RANjz@*B5F{#7jKQn2#?LRXqGsm3xZwLjfo&N13tsW&~0QvPr zS5cy+jF82}j3h~y2~~D;M>XfU$up>u#Zt)~<;0U-*OSU5{y#s$^fPj{Yk<9tu7GiF zg8yO1nZ1pT^0%~Tofcey+4r{H=>nR)zd_Lv_Q=M&LCwSvLv_F&#Rb?_0mke%GP z%6qt_N2G1&mT+_%u}witjY(ttd@M@DGihOaXgj9$0-=a_X3b+rD=65>Pz%9 z5;T|n#*}FNGgf==?pGZl+A}wQ9<0%t^TC#?utE10gI?fpuht{atea1l3DlsMPA%M3 z4i>fz?0UvSi{_zEnc5Wvgjevn%$s)dd*~}L5p3#)6q+mR?+4jO?Ui2lpgEA2J7U0R zBt`;@oWxyek@vfz3nIV~cQIsn^ziZ?!>ywPZ}nN*1AEgXG`OIW!ds44cz{m8C&D3o zab)I9H3feDlNwG>@UG-RBC7~jUuCbstoZBqtu(q8^-1b4i-r;A-+tF-#8g_zlgwGK=BcMg78Te{%O}i(dgD}{1Qn)C> zpK^MV)==EY1E0Ii^Hd&8Jw27n@Yn`kci(XAkrr^RD5GUlfmYs%R)$wq(T~l-oaq*A zWZFj%S>&+L<(@&~JDiOkz1q_ebqMoM5(@a=HQI?o|#_HVa((FyxUoxIp_TUbIfnpYz-dCcu*0+%2VgK ztM)7z+2i&!;a!QFc$!ISbAgK`t$1DR%7(wAE3=$*Q#+P^|0ygw?#0;kVNaQ0hv2Nz z4Ex5bv#BQ>^V(!(^qpni=qD>{EteLHIP{GjXib2=&3%x@iwY#t7TsZVN>%L0te(HjdPo$C)e}$g8 z=As@&GWsxH(aQ|iUio+&SkEbYnbl|e^FM2g zA1qPe=FIr~^BrlgmIwLe|EsE4_BCZ#r7?TYZy)ml^1>c;x_8)T?^pFU9DgvRx4fcr zB*(R7smgfVvYbFfQxuR8~x%;Oe5mTq6`>eKh_%+>HW z39QBK{bzHXPF|$H_xFk@l~Vgc{^ZUja$|bb3b)%2XIS44vyZv5Ggj=0?Vhlt!J$$4 z_brMoRwE|WmB$w=C0#G5NA7elIJ@|ITu=U+D@8M8hp1_(#jXXZV>b8HMDv8Vs@uE)0&_A4paFqP|GaPkL0VKfWc$ATBOeeRRng z?ViZ^Mbb$n(!2U1;_QBY$P z#+u&#+4xP2nrE`M^84OM3FMQ*kr9^nZASb{8`AzI4O?8w61GJ*cs|)TaJA_3-rRdz z@=^vpUp~}r*n2+lS800HphJkqq5Nd=m4nIE#Og}jfD09uqvjWW9klFj@3_PsZ&_+J z>|m>TtL=|RxeB$X!u1EQm1jn{1s?vU<9@s_y1U~;@e0p%?RoXje!tWk+Tf)?X;tm~ zBsnieti&`mX6Cz6ZSgL(%4#QOnNDJgr;TD!?<0w)troAxI>R=(n65NWzPO{pI&R+A zSq|1>AW+i3z@bQnAXjzF$w?wXN)x=(EKeXPgLioaoXAWRFAIdpp)fhVE|DNjy*KYj z1p~{TBLsi2hQAbnAUClZ3b+B*mqnqo_4r|n;T7|XE3&BIFK>W919^G*1zqtu_<){4 z;gAAI9FiUxVNo1dNGOHHp;6d)H7NWjZQlS^w*+hR0?hIX3he@Qnt~YNKc>oLy)IOO z)g{6DntW)IN&p9Oq146*&`E3>!o;hdTf^d@8epvkCNBs)ub|aSkQQM&QRoyBn=*NV z0QVlqW#h2|4A_o;69_6OI`KVOMnL5RaT)$h5{<4K%A~@E+Tw$Cb)YIzAZ*r>iM7$1 zy#fQmrZKp58Z%@fNWDKDA%_IeNK_Vy!D}Y&m}Zy1PreMK1%O#PO?(GN1`^2#Gmu7g zrf??Ph)aG^o13--G|5kcKv11V_JUv}Q>1@D040#brE_qn$N_Jw=ZnB(K)^lWAleeY zpeB1ln-rFw1=yNd2rYm_w?!D>HJ4#d z)Hk;!aagnn-Yc2~*Pr*3xXPTk$(;Br@AU>`6I++P{U7tELdy8j1~7+B*Aobfrqw{& zR(K6IBlG|Y3(u4NVwy4toco|V45xX?pn+3E4Ydf}#jxo%(;6ytithgyVOS3SK?9f$ zV9@vl?fzC*;r|4WhX=&(=(?k?fM*Q2`P1ZfHvyS}1W@Q3Xk-r9Ux0zZ+hqiu4|a0~ z0s@xl8|Ei>0V$NurP7#ez4c5QgT&(;_;28e$JNe`djKsKEa3S%enDHk54=6qwUgoo zu$?mzHit%b=7xr{C~P*u!ef8QZg~f41n$g-_iZ|!IIImqrIFV&N%Y7l3QKrpCHIBM zI{=XkmW5#96|}NVc&Z)3p|FK(siBwh?1A717Im=j3aYt#;ndOD#==54rsbjC<^zB< zu;}q5P*Cgz9_B{zcP4{l7u$Ce!exTvnaDvRUlz**4J5>aMT;kcg5s&$aB9k?lY=Pa z5b$XcokL;yqGN|QN@7f1uYO>)g1yD`!8h<0aIus!=q4qQSO#~`+>%T5x&uD&Ac8&O z^p2%~E}EKC7)uT!u{gd4{0IT?#>0&w=iUX_s()ZiFb#xaM1H^!7=yE1V;vb0BmzU9 zEQK*RO)Q4(W{AMhq_Hpt=UK$CG?oYq&AJFXF*wr@hAofA#RR4Y zY{7YNFsm;{(3*&III#!k|0a+H2Ug+KF_=0Ac5B!a!BYhi$-pd}cLigWcm9yYvarA` zoYn+m<-qnWy0!vIPGAbI4K9le)%CQkfH--;hYy3JCyLF+6rXA zfLS=D1jfGIjl&8in1CrbTsTH~CX1lZ;NdU^M_tA+zZ4M|8iN_e;Gnh`W_LgYhK9I> zF*w#JhDl}!$Iz_?w%~Yxm~}2wgvH-fU<(cmg<0>jgR|o^T8IXavV1 zz$oQH5ftwEKKwi?W_c9}x6r3k;U>aQ#$nciOCl`XHo}iZU>3bxxP?9>0oMz@JsY$1 zuZghuxqV;@zC{nSB5H+O_y;xkULnj{(BwaXPlu>DacNH@1zAZQHhO+qP{@e{=7eJKxM&_fJ(lPp!f^ zd!KcxYRgH2|3Cu)0f7cFbApmjsjghx2L%E7f(8LW{r6W>$i~{y*xFH7$<5Z-L7UFi z%5pHt2Dr|EG}Ik-yN*1*9?mF|F-JxX{~I$YsuM5Fvq|^?F%xh3{ehdJa-9Ziu8oA= z$^FWET72)~bg@%q7QiC`tb6f_8$!m?cQ7BL`Eqk%enk_-Y8h>!{4Kx(EH|`;*-%UM;i^6kAF&x9}G+n&=8wNCpt1q zQFNk0i2aLd<_m~NlrUNjh7?n^Z$#AU>HU2-rv|T30X*g*TDvX&vd5Lwl(4gEvH14w zUeVuJ194@1bLY&~h5v$M0}2Wu`fRVHbzR!rP=X`MQLf8LDb4lNgi@TY46%=eSv?n( z-s|g-NXy(GzD^jvZzr$n%KaB7jwx3+&@m}+8bLs(9`{=EL_->31C2j zx>5bvNeIC*lv2AX6|bFwm5FG(lqCrMDYo8vc}a`2=}VV-j{^A2)|JsLy6?1%M{JNW zQ;y@A6!Q-=r&CgndtRJB$ReEz*qgBkBIC><8TDlI8pQBa+qclyLiwFQEq>YQma|Zc zL)M~%tCU+etKyFs-INEZ297>QJbqQeJ4y68VKgdPSJrG_DbPTw?pHaJ{3j0zKv42s zS}3BN>&@`O4}vd>iX&L5IhhvZJLb>RmJzeAQWET)`0B!mFgZN~RCK2VZ$VPV{vg>ni!5L5jehDZaOM(`T5z;N_v2j0pZAYa0R7j6`_+<5%Pbo} zW7ySs^nN5Yj=PAqYsJ>O0`r7U1YcHJC&I)j%~eKGE5~iaLur$@GnjR&&+LJ7TYA9d zwxX&dSo>?>K}6J}J9Cxdzx}Selm4di&+n~ZARxH^{BC9JsBfh2s84TbV{a^HZ)0n0 z?`Ur9@Sm8BR2Y}(XF&Q$9C;pa_Lr!QQvzE+O+-TTYiXTgYy6uz!X|zHd?4*d(6Y*0 zU%&r2R387Ug9M9OGLr}g>A(-enA|BVC;4(smw1}1idiQC2W7!mcMshTR7C+v3Gv zk#VVsq{U(?_W&t9N&!D-jCtQZo65MV(j9}rKUt;tSjO~W9wsVT6&cD-+E7jl=W8Em z-Btuy@5OHlF8L_F!LPPZZsutXG2H#qsQV5K1nJEBIfG2qR=vR|sP{OVn>Ndo2;yR% zbq}MDNhlbb3^-lGFREUq92t`FM`?s^6sfUva=n%74Z7{gF`I1hC-jV>iX>0dmOtCW zu(UIb)8XHqAfxIu>k}a5x1AxmoHu$Os7N17mhVwn;ID?83jdEVsFrEf8Tto<=s!sC z|93d7T>fuBjHAb`!5HC%eaObTQ1yh%rB-V5%P2oV;?<;BLQw~I8GU3iXiQG#lBRDv z-JkyoZq(GsE{r=h$AOKKAD_;Rch7$rQbVW`LH$I2vj3fAVCvYhu5?pe5K-uEG|My4 ziDxmXD^ZKj0Oo>xW-DuaFjZ8-w1P}m!dCt$LhEMUAgvU8W@AItL zZ{e9m1h?R?mxwbo8`oarQVaDZjgF`{`&3Kz6%lo1N;Z~81La3^>;*lwXpl~-%c6u% zjPH|QuGH5A|84)n#+86ACI|=s2LuG;f4AS!%*o2YTHoA~&eqy=Q%zM#(JC1LOZiJ! zKp+e{EQd%tOewKUfd&n^bWXAW68cg|&K())CmOmrF%hxwZzz~f2@1giafHs~)z|OV zwD8pQ))ZECY9rbu&ST!|t!eKu+`P(C39qn6G%&v$6;(xd=x*tPYjKH}^y2JnSbin? zg{&4LBo>4MdUYOt1Y9t?E)nh1CBx_g;f`YV{UK0mHEzM{+gY83tE2Bc$+syTEU?Q5)@P= z{--jt^LC6nU9Rb}(u{K4Yu^VU;YOx5+gwBVSU2C-{Zl6=2eg+Mf9< zT%tb|otzWQQls3Jj`*%~~ z736=mw{qm2?fwkv5wUXRJ!{33kdR&Iz^dZ?VCpl^7c{ z#m1)4zU}r&uc)fDLPHUi2o1;NTa%eNpM8~WHVnhNWk;@_%ILPTc-ysQ*w+EuEAQ%Y zkQ_IpcUGa*k!(;8lb4sjbk1YP9LiN-xetA=HY*x!4q>=qYi#hm;vZ@9>q>DUm4xc@ z?qsv^Oi0L_G^OuddV6Ynot|-M*BJ>zm!Jvb*nHfb`Xs%L`Sh|04E)raJV8uOh)Xyz z`?fwG-`D`%JT0SpUzd4!c%VUfqM6yHW@1X|(&B}5etN(8$UJmkizz8Vy&fOiDl8Z2 z3=RERo_apsc={zKf7*B}U)}RJBV*$ai~QN3O`!2?3<(=6>*D5yO$XhGN^fn$;ND7Y zl|ljIg$nwP@nY;ord(%pAnV629?N1H(Gimup?;uFN9{F?WGhKfClhN@{T`w!%HY}ntwHHM^2r?=c+V!Vr(5ti4-W40=Tophy= z`2E{_7%2gZql=lmd``N$*}{_60gUA2MaKPd{j>5&Tc*wPvyA&PO$b0T_w%p@M`B08 ziO1?D*|0N+X@Tzk#s(vZ?uF-T`mEdz)CLY_&ejhKDKi;*M$Xhs@fkU3&Ge~}k-sJc zmxqTPtv5#2IXTQ}=TqGJ^M*rR+&s_}6veh|qhiIOB5L>9fj>-w0-S=B_XQ3b=(20i+lE=u+g1grj`sE-)LfQfBalc+S zi=HzB!OvIgO{Y1Jk`ilQmKQgZ66436Q0gv4-!6H*P;uL1i=KtQ`_&3Qdfn;GpKZ2g z3@J>Kl34U3P>kjLGwXoQ0eB`w#ll|CyhZCM*CnNy2a6J!7o#l{Ip2$fY?_VClfj!v zOl5M4s>NJ9JP&(3PG<{mgHHvx`WTfqaF-!}z`?*CB=I=`PfZn1&8mFH#*t{}FLYXa zN5P%ToHeEs(tqR7aSnQUhB`coiwgeeXlk|tWr&dA%R5lp+ZtThF*R{XR!T}rsf+P~ zFUW{Fjd0=%wVR3=#ZtJHn}gQft`BBV2F^&_K>P=v?q1s8RS&%5V`EQ`PZbpEW$08V z3i3!eKcCu@-;S04Dtq{=8VF^fsfTeyNnBoELxluJ$dX}cqQE4!6U*!CANh`kJ!W}; ziJ)KWZEne)!6)I5PReOuxLwzAlN?WHCu^JQ)ezEhm*?d4{IdSk0j+78IpLngx}{c) zK9MP}pEWKyIoE^`dlxXtDL`{=fvnIw4=c>cX<@u5zm?YQ0eQxlpP+<)HPsjh(y?!n^< z?uiOuSB%rRY&8+@r}{oL(|6YxoMC8Po*W(y>9%R>tlj+MdXwrn-tIgOTH&BI<(Xrp za1_t8IoK!@r0^i({KLnVElriVwS2?Mdrh5QL1`w`z}RWg^3|mhH|x#sS9##4E`8@n zKDNA(>;$G11VlI&pwD(J{7II$OBhWfmJ_V;AST{K&oJ>T%lplDNi@| z2G#bDut!DCv$zgpJyaQ6fiEB9{_u{T3zHK=TSnxRZIm)WfRysw-}k5^V+iD0XvK@w7f|ixetrj}fT#|l z!1_D%5~lQ>@}x+TCqi?+G%oqy)Ky$=kajGj z-sc_A1A%(71Rmjqc%R3EG}ArMCcUs#_n(*bt!u~fluF;p6sYGxmLx)I2}nD>5u{Sq z_n%&58thW+n8AJYBV^{I*D)cV9hrrxeOaYDwVXzSv2Cnpb5sVaPghk*guhp;u4*NI zl;3R~?IA{^Tj!p6$+!D?5(o<7(9+VHnwqwVk6A?_t9!{QbX(niwANhGciBG=a_$Cb z>dD^(#EJbLl*Y-7ya?S*E0DqkC&^bLS6P}i>K_tqcgN|3(|6aiYnZ7C!NvJl<|hhA zi0Mx{JNr4M^JPh*TCM$lk#tv~vh5bIgo~MWDu(?xR zt0&b%ERp8JNr}Fh*@+hCx+r-KR(I&1ebA3^jPU&pzmHkX?@=9N?5`#9Uu|tR=c%Es zmALod+#wzVgUuKi7}SpqunN;r7{Os6j4UZnr*t|}@8uhe#_Oc$lqR3e)0jgFMsmv> z@>juhQ&V;Wkw&f9=tT^N{skfP{e#X|m3m;qp3hYz35khUit;KQnk`1hc6M0)0q{WiDy4+M>G5uPk|0v&|~ z!__}CCB*C3{o6mw8?wQ{VRBZvbg)JjWaE}U;N{^08izv4Cm7fjyhG~jbu2Q-UWwF0HEKWYmq6Ip8P^xtw4521Am87R^JoLibmg0d^}_d;|Kz@fALSc*tWc(sh3)syFozqSTszGZi))X)y!&i6mLvc{oqWh zmIJx+Ftr*oK8&stti6=|@(<(sO{NjZ>aRV-i+2Zcj<62-m-yIbg=w#razRh!6S%h0 zY@>X&#T(>Xm2y=zeKq!eAw{+!i&l7(jCUUeL8xZ7I+bRlEjhC8& zuEWtl4R815>vzdy1!emePpWEauBpk-@{QGqV0f+$z;iM;H-8ceS1WY2lmm-&O^p@^ z2ol=$coRgt_wEd+V-!rx<_LW5zduxkrl+>Zg(_P14@kiVK04@PEC}lWlu*2+Jd`$O z8aNb{8(f*jvu=8|{fU&_!vsGNMz~}4=d9E~tG)D1?;^`#B+-os&Ns`l2F5CbqLy_pwi{;pt$P6^+ID`q?x>d~-N<2UF2>4AFp61LgESS_^ z?-e)nIb?J%KXV9d?T)UH}7{yZ6YtfD1+b(Nn# z3pt{_9B(r-b3g}V?F&xO>@Cf9;g*P?>g!iK0p~bD)uP>}MaksZ*x0g}m@EA~RNLUtJTqp= zv^>8s0&3SKo}NYFlZhMkxP0v`qyl@1XTE4p1bh{}5XnJmyBG#~lG)|2>W^i@`~;+h zgoLibuj+XVH4%r@cA<$yANgC(S7(Ug zV&Gtf!t1F~hK=vtAPC$J&7=8dl?cO;B)#IIxm9W=qJC@Nj3Tu`%za36k|GRCliAp) zqmW0G$Nhwl!KysU-UrD+M4VIJgBh`89&CGacVca%TPrkZ2gP!v10i@ZPm4;!+SQdC`iETP+9q= z=v`r-ZTMQWJa5#W#D8rT6_pLC>k{v(O^I7jPf%aiJtxISgiq!;urESay3F!FCC>@9 zw;xr!b#wm#&6d?(uU?>-hNe4sMM$SophzS+KJm)~$sne)g7-vmXjX+JfcG!VhhZBt zEo?yVi@_V*q;jc>tFU$3G6+Qft< zz9?(~2pX-22PhJoD>%x#l53S)4|qvQ39N&1w#IK*<79XF`0CEsPa^H^G0ikbdlF)l z{`8~I8&Sbi_DWhX4KpxIV`W?c`L`7-l#M}njvHI&(~zOh*J-1TWRb<)*S6MMhCkZ6 z($^hXGugq8JDqD2nzggvO3sa^am#xrXIi>+R|wO*7^Rq)f{L!aHA3W2^8;P#K=p$k zMBwU{KX{qe^Jsj&x3-JenU5yZ*Q=`b7$^}qHh_%fQHMc!GV;saYrek6l!r#@vhpQy zZbCQeWXj5zSSKhU$mlJg1%-sG8~$11f)Ju2GT?H|J@eG1C0geL70v}CJ7UGyIrTA> zy8!L<_o(`~dhyhvptlQuLvC(v0tRkAu8-)RN5<`g>jv7T#pIC(x+())N`kuO)yRDn zxt%}6q04s>`(AwOSA+a3j$=IrwaSaf9JblxUWnEz%F3xVcFjjrDN4b2T zLo~kRuH$rCXlQgK({)yq#7u@=62Minmim1OjN}^X*CtthOQ=qY&6k@cKa{Pvfj)_o zigbEc;_y$B837(!oGY(dPhR%&hCF=;rVsfiT5j1=L^G@ydEq2&fA{q~=IpaxUM~Fp z{aYLbkn7dNV{Z>d#`GZppi}TL$jcko)P(p%&WWfE4VAsali}4fr0B@VNYfZ)<$Ku> z`rH@`J5cFCM1uYSO=G{;)SZUMsRsdCv@}S&sTW|*#TB^)E&Lu@5Tuu^UNsW4;p3DXjjk?{c$1upE*u619x{wDeLb1{HM z^edcbln6`>j_B6qa-PJ&&hgc$en4~iXjs>w?Oe8h_z_CGF^;qm=O7Qh>2<3@28ul~ z-kq-VOB$;^@hnJckJc_SuM=DE_M68wJWm6A+;TP?vP8yL7%Uuyle(E9&f+71gc`qS z^ON8-?3uRk%|mklm0UCgYD%9cH`+mP;#;PnXDFDBM1f1A5CAFQF#vRQ{Fg= zC$?cgh7_oxJ_&(>N~RT2qtIsbmej2j3Ag}V4eIi|iq|M7=mWw<(|0IO{o4HrJ^Flz#4sgc+xrCr%qpMOG$VY&}=WFSG6&AxDTtjx$Ax( zWRKt&tM!Uc!MdBj7+yH0Wi|aJp89Fy=P<9kII)llH>imh|Z#K={{1o=lEp zC%>wqG{I+`bcY}0zu)L7bkukK{^=8KkRTwW|K8{w%&nX(&8>m-|Bq;CU}<9rG&0vW zwb!@$k2vu^Ma<4cD_|c364Xt#!kxFsa8@m9F|m!;#{0uK!A#Rq zuhD}s{rA?V80Xq$XCap>ge0u=2v;Q=T#r~pLa5La3t^^Kt#1ZH?2H+a5iSwNgLQj! zPSE|eTpkmwC#uj-$&7Jp4vJKPUnN1%cU8un?{p)tKJa8bvw_(}q$}Bpzv`F7k@x@D@3! z4BcFVD(VZvPHhABSvzu3Y6a~jrtWC!1p=M7)sii+xO&w_yO7=Q0Z%j9OMy5hd^zA; z1JrnI6KT?r0YYV)>is3AY7+&z4DQ(aSIzg$w451MSe-MJs@d;5ib85(THI@PnDP&b z@66|lj_SyZfO5sytdbyfX9rU|X*o50?Xr7|8_FZMpjP(Iq?5pa!lB2hCfd(!_Kl_v zOeT3q3o4*7F(Q6C*%d?3-fhM?Y%#~Er0Z$2!#n7ILzvZi9p3N{I{beylKtNh8roQ! zn42mZJBnMI*!(XHjiY-l`WfJZbq7znDDm?nkc*pptAOtyL-P3XYHUf%l{UJW68%wc zL0z)nwl9+9tqO@Lt9@1Hy-4po!T^k^;^|};hB-W0>q#jLYf?SH{*81w(4EAA#sIC) zK;OF3lPD~kiu2U|=h7tE@=o!?F4^|M+wc8zgb^{D@}XXvh&bT?NkP6@AR zW*uuc`kSfB_DA|Tapp7(Sy49YUEb2>`cfDo-VVo3(V{EVf17>Cp9o}u0s+DMC-wdl znf^cK8WP`??xyvY*dNQsGj<e%~Oubffm8K6T6lD>(nI6Ht&>h9f(y+9L4EvdOiziYH7y! zskp}4==t`XOQq_hVs2dsz*KSa2tTDpL2ceWk#w?mOl`$I5b=E!A-!LG%@pD!>xhW9 z-c?`?Me2yWe|S=a|IdbMFzR9B*iWn4xiaWoTV*0a^B-z(T)!PDT3OA*$1NdEvQ0)H z4x(<-PN|Z`-fmAOt3@BKj}K;=WoIXg7)DYAW}}|PA(o@#t%sQ5PX!YNm0 z%djTYs*;dHPwS&id+_Yin+oO}$e%{rBsAWVn&<>*dhQ~ELLookc7N*0xO0Q?W%4>m zY0AJTFrxG&CX@b%fy)xY1vih<8QU|Of@p4}2cP2#16HwOyUgc$Yyq1JE1bhP9)FvE zL9iO;(Y`3TOTwkBd>UR0##^z&RQ~!M{1$C@w+{j`zbvHwlE6{yStILzyS7_|WXT#R zy3S_mZ;EIt^_MY?ByT4k^K+@if4agO3n`KRx3~N$Y7@4+lKC$&TPns(Oq?4BTqAuk z*6Nl(vpv^XAMbCM&3VgI24nW(`*mO3%ATNvY;$M}u$y-}^oKPnSw zL&`NGWC-G$rlgQ>q@218@?6#^=4xXS?phllPsrd+B?;mg)beo_Zg4c$;!*^-hMMXj8?R2F zKCogidlWK-)WoAPOQl&l(ovjNm`I_j?#U|n0ZecphW_S3_r6hLC0j>=@nEc6D6J`k zUD^9V-X6cI5)N=Yr68&2$^}0oaDxHXt858$rrw3J@B<0VjBBs z$K58uX-=!G>=WGM08iPUQdwN5$+FbRAt}~@X!bXnfIs+orej{s8TNInctPvIbJNsM zLz0N(VScVKCE)Nbs^BF~esXp|p+b3otg#>1hwxE9ll6Bcq-=LXs<{2FH%5`*fJto6 zUW!my&Ml#+8WeW|q5=Z<>ZvM(cg4@`wdIkob-wo--N+hFj?NF_c_@Rz(L|VWx@V9FR9<~8*C5c!| zA@j;mGmByoPYf%P@1OZcwo-KJ2J%;62Mc;`1gOS$+R-Rm+1WgLea4`Gg+e8$%B?(!&o zicmRR*8vY28f{6enK69I;wCMpQik4sbvnzUouX)c>E6PY$hZ{lgdKMwdb5le^8iRned4R#(CBeve;ff@)*zCw8H5fEU?{lJ z7)ktz*hSzl-1ah|8#@%{sj}{!px)cVT=DigBwXTI2K*hAX@0ajiAZR&cc$Nql^$9D zj<8hdtR^?nHMILZTsm}P+vkpm0)55til(6-#UMfAapj%Ou`rRVym$w0*63xD0V?|8 zFMV4=0qG-=Q7GYZ-S`V`q|~~SUC<`538qXfm4H=?2&5c0-ZkDrgu%IrW8uSqOItXM zj*OfgsGu0sFQ*dLLl4!9`Yz?~I%_2A79uIqZOrlkA%4MUEW<0X5aJOE!%5Gt(p95oOr9?LRo@-SfV14#C&;FyHE=W zMx00S2j&l2pyKOQDip{bV>AI$j)3`99JM+Q2rn=5CNnQ{xA%87KS5FFwO~U^V7;z$ zMDu5+4g+q`{^-S3a;C9oJNL@#azJ+nKjLujgQYXB2%>jH*pv4(zx_?J&h=;UsPuuT z?2G9`^>nB1XUI-+CiS-m17FuOi4PjotohwpX25n8{{>*{+KUcdR>%I61<@H0RkVAe zd6#Z6JD=*qi~tbL*Q;#-M1Kh1kVSOS9(_Uc;!KqxYI(g?@89SXU@sCMe?{NN%My~$@5~!vc*2^v+ivi< zPaRrns|>#NyBv0~z^54^bH`t07^Jzp-|+XXo(k(%DPalx)jMp^`QE%`bE=+;_hIIs zR6|t7D$IS!cmT!vN@sKv{vq}1Sn={>^qCRA7VNrm0i4-yC0;`frcvsL-8le1kJO#_ z=UoyyJAcv?+Cai#Vg3;Lh_&(IemO0a$b@Nmj^}H+^QdZa7iOQg(QcTckK(&)8H;4L zr-yY*lyJ$+ zA8-W;l0ME9z)Xe=dODK=6&}IqD-(S*Jn5l$Xl$5;FtOD+lkVU6SdSO`rQJxF9RQ5m zV_T;1X^{pW+eG2@S0=T{LlF|zx z!485kbM{IMBlAY_R8N0J$vs2zDlg?+xLNDw)w4h<}gm+2iNUfqLN^akG zLk=SxElGFao$O-8rF9~QCY!fSN|rO;;d;ZTljmLSy$B(`UN}0^c+hx{2*#(nMF*1< z=geOKMAZJ9(^I__9EPLmygsc&0Fcp>8rY9E&lZ@x@bz+zlzb*g#cgJvt`@}o{KHGy zxnE}`C3?C_cb38K3obrm)XCn76M}K3Gs~&Qa3p7*b|K~5a4CtAeVQ!yyYC93@PcxuL!yPJcNF%O=IjP8P;y z6Nkf9FYM&6#vkNH?>c(gd5oX3zcxb|F_F+FakXRynAHg=h>iLDzF#bTX*9C_GcJ@DC%sC3*K@|=71QM<(9Nyoezn8@y1K{LR(O*PQ~@^&7Orr739{L& zd} zGHAeRkDf-qIx?jVjIa$2uq)yw=z3p?Z$E#XnIl6XD$nCbp`hNlj5BRLz&A&K4KvL| z_jkSWOF2Q5M`z?T9tBpfDsWn0{5t^K4?F;T;QB)l`JS%0p_+QNCbS_Pj37?IJp3a0 z(^xN}0yGI*Z<}X%Okh2o@9gYdz9AmDjEqSZw&4wXXi_?EEZpK%Xg-y@I8O0-@48z= zPFfEUl&@v(@(dS{w0=9w#b=T9>O2{E{NNjK=Zhh*OF z=G>v3%DU5S+=GHC(bT2tmiiHnbNiY?yqT=R?;_rqSpaVQSTUKNfzjWbRo8 zdswWI5OjNgxW_j%wZ{9zZDs(>{}{d$9Y5X5Cdn^h1TR8{NL{T^!^3E*}8wPx}uI|mp6yJ*B=urn&%M0 zJ3yB;P0|-|JdD&Y?y9;Lt|8Izz~Tl6iS7v1mSk+$vRwMfQQpKDxgdVkI69BXI=FLN z1IOLxad!wUqYH&Z`rC!V$@Cf}g%0Qt6jX&BU1I+7G4$rCzFVH6R_vB3bz2qdSLZ5}gywV=o7qSJe0&>sYfhk{)R`Ezc!^Je#LeSz< z`AUxBow|E7o<^@B40YZS=8A13p11Q(>RMnI#W+2-KTBgVA~M{T8u2J7X}HM*HLBJe ztcPk{rxj0dK8SfzTQ%Iarq&!+%V%j`v95BqoXNLQjrgdxY^WXm7(CpT8Qk$ki{Lyg zf#1Fk#qC^P4b@I?;aw?s&1^dGj_1ds?62h37(eXy0f%A zT@61ZaW$m#O&}eg5nVWkrQ0)@16??mrncI&0pH7JmE-CY{2>U&?+Icg)sXKCC)foM?uc4w_J)9@2}J0-S$4+(^PUfj%nqz2gJX(*B)ZUFCBe7IKCl9 z4v^=|hyAOTT?2cPDku*>7+Yl-WNZ{)=d%cfzv5p;Gq~zK)PMxVEowur_r!(Faz&&B z@vOZ_({gy=mF>acD4Icp*s%|n+oQAqCSDypx`=p0{XnCk<4B+M$dgdRFQw#SNKa%? zqKpYW(fmyC<6>LvYe|VXP5Bl>%@ww%?Z=sK`^_`?K$D~AzE|&GMD6c&d{R+i)Wx-38FSs#&*)EY35o$-6_5t6q&~`crWevyNgG17E5x;dK8j4@kRG*~ ziOxiEN%YY>^sq;3JKils`NPSv6vOYlNs{32oU1I*++3=Yv#F{1_c*qWkkiKv=cn*p z?dIEy*u=8+*t({FGbfHRGC`jp5nQ>qDt@`;q1%}L)rMH#tfVlzznVgFLul$oRo0LH znmXV{D(c@K@5BxNbg!b{P2;F5^j6HOb7pK6NpXOlQH;+G&te zw6==U@}4eir>U{6ZU7GT>CA#xFkM&M4&Z28){~jm^TX2F$r!FbP1%MBy4}*~-;fcx z|M*(P%c(JIIQ}XZHJaU5b~wVBryfCl5i7^lu&TP5h{8x2T(x)l{9iLWDs{sP zIw|G)8A~mB?IqGj?m5nq5AKEHA;tHx$W&Py3Fxzy{|B5x(3Ybj-e3`*a>$*Lx&Rq( zMT*J8fm{g)4M25oSRl$M)1DTpw1K~krpq6$(cCtco=)4fO_`a`R`T|&@()AgQ{8Qi zYjgvrj50v4_NU@~jbB3c{2RYCTQettL;kSPbu6AV(U1fLRaOfwd%hl!d;Fq>O}^K0 zFIrB(tC@&M>RTo6C+)=O4}ix__tzc|{$)xeFN1(8l4Y7++gH9Qt-fp5!b#0jWHJ{f zF&@yx(OK4HP7+;~!oH-w(D&@XYqVTYd?dfjaC1tWZb%%qI0=blgD{0n7*m!IiGO}m z(!v%Tt314u$|K|TGVbnss#(8>jyl4HJyzKoDf)(_nn6H+q^)$IltCy3q9$}022ST* zXfkVn|5`CZ)^$FpsFBBLC|2Zx*W9au#r~4SZnz_<8#KJu@-q11J9kJwbbp4~Ga_k} z+ykqt@l1^MZG4v8yK-IT@iXGP2`9Plg}&U0vEqhna9saDeHVgJBTwHjHmj&vO*9Jb zQGHxP&mOlHgU42LcQvsV0b4a&bF5oI`zhq=kDO>5$ zRk1lRw=$>+1Gfp|^iwq)orOlpv@8xn6=x3LH-?3TIF1)Z|00YY9f_`hQi_w=}$9>Yx zA6Wju*dkgVr$VEX6V_srE?^`xn$=z^!c>JL9kNO@!8u@ab9v;{*nwzGEK^#Y&3D^( zH^1s>F!9w+v6Pf`WQr|?2167?yh>9;Y+6gO1F;1oMLr2Z-bxMotZj<%^L@o7rQ z^XZJ7&jv9dnP(5>mjOF~6nT!9=d(MW5=?U{OR!74uR5o^+3z3J$0MfjkIzoJ^R*)= zZ&g%x9bOEnfGbD^tH(&@pR5=07kynX2x32Ok`UK{fpaj2Dhe0G07I4cXl}SEd5bc? z9Z!ru=D+x|YUzpj=Am3Sv@jz|96P_gpf}c?yXa8=iapR8st?g1^=~*I+^6{)H(2}O_JIABEh!;x**2qwL6A50O_!hfH>iXnf8|ZpI6+(9 zMo8p#FtRKHC3-0=!XQjeS!C5Xfz9X#YA^~mnGtDj#Mluolo2^IA>oST^$QJuzDNg1V;B3;y@1eZ0#456MY+^dzt)1UM zTiu+!gi$dmLj6Um)gi3$F6HpYt) z@!lH-;e2gPxim_hgjt71y)=b({)-yJa~_krNs=we3o24qekU$!L!9ImySI}i*gbB2 z7&12;6xVOVBA=R*Q(sm|*Vq`u%Do6)G4LT~QIhA>tU*U0K5cOvA&z+6GxxF#LnlbYvCt4QRg&6guRZ)KblB?6E3u+kn(}I*;QKS4M=%C``w6qyu zrCl|xp1q7S$F&U50y7EP{w(2;j$bKo>#Nr%!U}bwMCqX01ZsoVD*z$gpp6 zP8LWAyC&JH12Vv<>xf~Y_sHHzf>4tIv6_k2gCK&_>5?$*AN$@(t^uz}#|cla2Cq5| z>iPj_wO}y^1pL0$A^`7+Wjxe^3@YAzgeWi>GZ6nK)YKS>>1l(N5FyU;ZIJdRyb z;HucYr8RQ^a>m;K5Gpg+1^Eoc0!3!8EEsZ}H2KRLz*LKFKSMNKzGqh=m~9FTvIQLy zCdZiCX{UGR4PSJ#)2qUuIK&D z;y^n(2@ySZxF@Tw1W!K@i`q{Ohn)Cp$k>;Sf{XMSEK!>c*JN}m8-azGdu2EDq=skp zRj)#TsBYZ9f@bxVlL;plA4E6D7Kq$ZA;-h4$pNYya1XDaVMBt7VAex(taer$ zTS}%E$%3pfJZF*gH)GK!YD47467knB@XQncIm0(5f9{q|WQl!PNj=$-Gd3y(cv>ZhyAv+=v5XJyb_DRshV=>w-(W@~RAc2A? z0Ht{Czbe*Z&Li5x85eRZ)nrD$*h#r}Ok{PO1u(~7+mei-h(_+}z}wWOHXCTTiNunV z@n;6@0R_gZx#6OU7)|sc7wgT;85x+$tF*|7X%rsq;iA2d(Q#Y7PrT`s94h(YSfJe& zLjOwZ>LU2ZqAf7CX`Hx)%u(1C!ryL?0WO`|_9Ce!B&+yhbe?Ei3AjJ!?msG}_kv(J zQoz>lGF+r527yo1gN?a9Rn96fwj*+76VOn(2W z*{3_dRp<5-Dujup11jk(l#Z*{QN>?9mbkvTb3K+;G!3<`okPT$R-k%TI4PC?NmbRW z8U@YoRtzBW;Ps7-zTHxq;mJm{3PwQ)U8IG6t7{rPxd*KTOqu@@7sPph2tl&Wu}MwCAUHnE9{OfFfA3|*Q6QFUK_udWSnnm4fpR!WnP zrB!Zb7*uu`uxhq*4z9TkS~NL3ggM=XEm~UD7_1us<3T2aSJoVyWL^Jeka74GBg}>1xoCNraik=s=9u+d zwz|eSS=vK&`lqVj(h4V}`tXHoa%rkD*}k(Wq4E-hj@zQ?Nu{Q-J|U7rfa?R386MBg3Fgvl*O7fKEpIwwPPS5Zg4^OLWP4k5Wpr*j}tEQSpE7pM&lE7e_ zluE1eU3&bV?WX_M(M0%pY6)TtZ30~1`Gdu|%H6m0?|H-a-@=?2lS>%e+@Ut9o9{SrY2e#Y3INH=DIB{jX#IhT$h(j*@I@MVw+fUUO;h~Ay`Ci zawg_ZsI;>2kB9%~h&rLwwS{G;z{<=KfH6?fW&VHl6O97?vneuY`@nT1ToWL-MQAGC zPdKN4`@^-|X4TZJZ~S@3(zE1T;C3^@yr~ad@6!U>`f>OE+s%bfO3w7eQjubqYTr`P z{Su(A(F(?{E8p_+;s@vXgR8M-CqUC9rneCRRc6`b{iGtN2zZ>ZMAp^>P84B{sJ#x# zfTTPgX2EUBgi_u2EayYT!5r$c}Od`R~J4gyY{x$IG+)w5mxTYZ6DRWw%{ND=k6kWu4RC z^NotqW^GMM0Z46ZDu-*1RuDB=ML=ePdB@8!A*I)ULt^FU8`ER$-cQ#CFuUa1*z9D| zst|I4@u77^K=xua>S{IVWowyQXPw%#s(*~m{Ksf*+=>;p(=}3o|H0W;M%S?`>6)3D znPQGHW@ffyW@g-GX2%>eGc(1^%*+@w#0)XR+c|UR&fGJ1z4hk((CTjO-O{I0sru`d zRMkM*#-Tgrl3Fbn#5FcErrOCZy<8lS#{TQK)yY~o+}D$}{W^8I<*85dA=Pr`;Wg>i zS@W52W^wZ1`s}(qC0v+TSXoD%R&9Uu+FhDE?vz3mM_l;;$wGT%ri4l;2v>wFGujrzC06uA0a`iUOc78?0gj%h)8` znIN**Ix=TO(?@Put=M{e4}CukTX5MmB<(N#ZELa_t`q^*9IYsqr6X~?R;@LLp$^mh zlUa9NRd)098Hz4X9HI2GD~FJD=v*WEYM!(0uTyT}WqFgZxyG{g6PSPe&iUO8`VSyw zh}OR7GQ{~t-Npsk>hGa#Ky2Vmmp~OcSwr!_%e8KwlXc%uyO?RzHEd#U>B$1-2tNzR z&X^Zl&wm=E%d;MzTH(XjU5{fv;*nmSLgH@tQlDh|o7>s8fAVV0j5uc5%$kL+^m>h} z$OX3M91NvkJ!ttJ1Z5y49C#3HPKjN}WdtYAK(z?J_-Grrc}o1!9pjWNmGn z7d?2&j&-7If$Z-(?`~_$@T1Xva*2$O>N0@z8(vLS738bwnpOvebsQ9c?~3nJG*FGf z-dSuJnufF_uhz1aZw68#fEA6R=YZEt3KTE!>aBy91OtnTUasaZzg~Mi;s{*64`m{# ziT;CKtwH!8jIoS#&aCxX)2fqf9x32hlEpwPnul)u^wQI;4V6xzTZf3IK+re=i z!h}&HmzgkREpoMHl3#hfHu-63j81K@G2;)js~q~H#MCt!(aTVDO>!L^uroF=HF7CV zghSTz1~Dv+PO3FAyN#9xX3?IVS{<(WV5%J)9%rh1jerQ1Uh8?y-HIXWBes%0&BFN} z_2}g(OL@ppu_KQfyqZ6%oD(m<&mFZ~?D(h^Q#0>=uH*G-*;?j$4ZO!xB6@iXuEVvh zhp2R87YH{36(qegJzFGa+=MG#ZDm{?Jh3OB%9|GpJ z@>s_a@r1^gxr_(L}wzZ^-pMX1kHEkw7T>s$(D& z2vp0CO2M7V2#5?BXmItk_x>2%Ked)v0}U=viTa%X7~Fv6`{2hv1{WwQpur7X{$p@= z8~I?Jy9(UT?|3i#UHlsBt70-q!%F(rs*Buh=clgLuyAV55m(hd{fF)zT(9|D5OAW` zs&?Fi_~UF60S_QT-f8aVpdiP|dluUwtQa-6Tud-Ed=}VWi>}wwRf@R&GR7s+kTrZD zf)#9ff0<&vnmWl#VPp^s&OfRutR1;*!!QT`4$cgrwD+I(7Dy!KWY|Uir#g!i?T0gA z*6=}4D9{hKpm%T}IW>nZ+nMNte_JLBROwsnvDRLZQtaT^5>x57zEwWMg0t>$y(Yw~ znRLGs+iMJ4f*-ctgR|_YD!?4siz?85yIyBPS3U0nUeE;|0O3zfgvh2B5d z#o!-yG58m|(EGzKI{so89e=EF=3nf>{SUh^_={Zt&BkQe)nDww{U7WCI12t^7YH?3 zAzC0QgqC+(mEp@x*Bo$CKQ^(2*lEqFLnm(=d1NM_IJL3vK{pmE*KOo;bbNj10nC%L z97!=Rh@=Gh%S%=#x^<+FW@V~?TFp@nR$F#i&kB04p25Ybrq8jam)`7I30ERb638u( zsq9G^nI&HH0Lubf>^VI#2{k*^n{45T^JNX$4kGQbNw3P+kV zGkoA3ir|k(A#5#(=Fm9)4J7G0xmJOmC&M@coP7prz%L+@Rk0i z6Lb#8C}aoB0?s3cD^3N}V86#i%G*`r*u=CW)B-*L@5MOj5o zwg@YWb*0(J4PF{LF9B5Pp#8655vvOw_vnIxl3pIGd9Vsl_>y{{LJ^0iVN|?%9L>P@ z3MnVFKR=YNT5Gym#Ak}STJ$CCK(P=DyE8f~mn1hj((q<4eLtI_;1X%HNfRN>?IQhi zQ|Pva-XZ&@*@K@|AviO=;Xr<&-Du*)>J2CY@J+}-p*&f{okmu5?yqx;*^t|@r5pXyoH0Y)pqk8)9hG^FVQd6^6$jTaqAp#D zST6@84`w9y+6?U-5~NcUr1rQ|;+`_cN2rb9C~(T)@Vdu{I34}y8pp@QGwMn6J55m- zvw{|!oqdC|qR3YHZ2Fqfa;cThmNXHv)6x(ZtkL6!YDiYhU4x;J@aBF5=$v^rk591W zw7B(0-|RrHsN-p!BpaYG>$k2ONMOy^ z4`PLv>zkNqs57PLM#0Cr!IgrZ$AmG68EO0DHUvFG=eZP%Bn75XK-{rIN{5n$9<*0R zA@yiK9ZR0KB)95S4baI|UvIyf@`+4vjjGfS< zQ=V?n4*r{#ZJ}ox=m%?9O{2*X{Ji3rHWUTKZHkOzxx>1;d)2PYlrFL%)2yK>;7)jI2&r3 zqI8EB?h`@Sq5Z4acNb)=tiZ+l_Ct~YteHx5BzNmDk7}@vMnz14)l!xl{IB#H;N4h3 zl~@*qffb-@Y8uw+)mrNJ7wGU#J(UcWESVv8DN$h9dYYs6Fn-8I0b^LrQ5p~Ltx4>e zKkA{pFXI3+RbMrm{A@hOH%5LstD44;;%SUa$9AvFQ zs)zRB_{>O!;#CB)d~-@eL2)#S4i$r_t#7jsQ;2dY z6wb=uNWDUYQH|BBqJx>5b;R<@%{R5&=2ip2o+o0hq8KZf?+;o)8WvT`>IKSh{*{8r&_Gn(x3YWg?O=Mkdq)-|q!h=; z?coH}T<>^yHg~vpPY*|DN8tLOT@fzGQ!T4;wMcQUsQv|fq`=iLzUIb6Y3KfZZc)c? zp?AqAl`&(qb+Z))(udrg!r2o2G+{CR<50J}HTEVFFHt((FFCLDtX*eZx=41I`jU^unL= zq;w9#su-NTkHflElhZ&=zFF=xR~b@}tgoEdBW8uvL5v#qkTL*DBJ(Z+)f$DOot6nS zoY2O+6OOAa@zF4Z-I~e);7B}^tA>CNQlG)hw)4$`ie8S_xG+GT2}8S@GJs26;t?-Y zW}=gP!6HhuYg~Ej!+eSq!H9BHSI2ne$mUznef3gvY~!i!hRP>m$nE9zA$-~&TO8IB zAxic_sX?4v_7b0?swsYQeR8$e8#a3kLH8(oH}l`4q!CWs2td1Dm$s z${mvr_?79T=h@DsQbbD@WiM~oxtHOvjuSa|3?&p!dI0CJ)j~Yz^MGDOZz`*tcu;Sb zc1^G7?CwC&NIE^WWh?wrf)qx05zDo=!(c|;g1SQKFNi$)#09*A5L)CuPvOs0pa+wv zTX*I3m8c3v!@BeVx(i2>xhLa{$tmRYA<*u7wFY%Dy@CS(i^!t~$C(>_kTL>~z z#E;;7ezBZ%@zXKK*}^yKPxkGS zy#c|AD59ee@O`|GvzsPO_+hs`H%0qDISmQIkVEw4g4SEEBENkT@ojjK=S<3{ z#qqHw4*%wo_J)qB1uOYg;K3ZBasL-DT#4b2kNMKrj|8vhn>ztNo_1co6Q99CZrqmt#4lo!4=yyF8+;|T!ws2|EWuSCKd&qB;Ydkgv*H1h4^$&<4adQ}^h z`q@R(O8KK*BQ8ev6i3OD$EdHpxNuMBRa00>j=fijz@Z7`GTl4$mSL07E#JFe*WTOX zpf6MEz=I@H@gn@}2e&;J9EpJ8USqvgI4ab6zV@RD^kEwpV`zKb4ZT@xmf;@TZOM_S zx@A=Kd(L(pasqMhBl+iu-A)_jt8o*8pONE)PX+dm&wV!DQMSP4WGc+S88EcoIHqrP z$ivkVMERv>fn7hfTXz>P#~n+Azma}OoUPfD33zfpj=%7ANU`!|Y#PN1{(SiO*F~l) z5njq7Li4rh*LwqRtZOx*QG)B}@1+Uyxs!3S&G zYwJf7F-L=BaZk%mt>rJSgj>1V-w$u2kQ}&4vPpnc&yUy13Qi8Z)+Gvawe((*&ncE4 znW{xU6npgZQ;RyNk~}zf8r^rLzTPwfha{tPuQKs!FyCf*aK-ym>?TC_z-`j*pmcF~ z1>s5&WNd}9%_CDkm{-#c%ZL-_lNJV2|^wo$AeZ@jwo%e{jY70+Ivgi4wO+*G1(MEw8tF976mPchj zsWL)I@1|%8Bj!_}Mr2e`_dtQoWY0ciz#SQ`-KEUS@ba3|+WASlpqJ;UG)LQ8(7MVt zH?sokcs`cUC?Ki9IkARrg~yLc|DIOHdYMcDADTy{qJ;W6WS9($T`}Fybv{OnN_zk3 zD_mxlF>7s!4|n?so=h^q#KaoSTvOQT_h!VOj=du8?NmIeOqMNOYDIw|U~lmA;EP72 z7&6CR2wI3*h!Q&*<;twR#jF8CJSA^i=W@E@=uC*EIqW0a^ZC}3-QV(!${wkqXK&>T zzGj*No>a?+!4^UQ+@jhU8l463&smiOnG4k=wjiu}YP9xk%Xhw=LxI&@^YDd(i$gjr{&=ZeN!}i5!zMbQ|8{F$&w#PM~055_b_L8RT;gW zqDwo+pc<;VX-hhKkqeE+QHhpAi}~ui?yYD|nrTCICuWH?ASZ?5bR``0V&dLRH-qCu zuRr-EUR}?^1bIRu6T|)D<1Dlr`VD^fl%Ke}jLa|KRVlMdDy>(z5B8AgOQty`V+ZXD zC&Ks6L%4^T5N%-iW?z>xSMV#!SnRfrP|d(ufov|XWy`u#ExIZy*J@P-UH>SZ9>`GL zJ+3aQ&6?Fi+&Ln-LbN$6aq;-XfvrsdLvH}v9!ziy0(6UnYGDmZm*me0#qkBkwNc}n zhJp82B^`Ft2vc}XLX^Khw zv5)8w;0r%#^aGk`nP}qzS#1x^M1*3O%SGZ~h0H6t+7VUt7SY1X@2EHGU1?#1W5e$UZo=OZ@IC}wowdlGBbjicKtp?Y7?8m4I zZT^FFxsQos(4k-xn9#M>GwEy=G{c%OKG+dmV?fL2G77-3pL{PS#)11R?9cmW{CeGS5C zz*{IJ+EUds6|cPW++r>~R%)3_;-fbCTzvAcg%!lx>Ma@BI-o{y+n_BA zUHaL*kmY9|Nm!-Bpbw%!U(aBhP%v(aDV}j12>Ym$f-=Sms(^Uz<@1sF`U94<8Q=EY z92UjJ9m3dJNaaoNr?4LZAKyZuJG2@vg}5JNCsZ7yh)sHDpDFU-)>GIj!W~a{iS|7i zk-}R`1$W_@@wXcTQFTgCq&BM13E$Ujnl!$v(Ztn>X*2Ll9Lf4-)an3Vg*#c(V>HVAXek@H*s>0Jblc zCt^D#m5tB!_eYi8tqnh91x*j#p0rdX|1bet6dwJNYs+pOzu;&db)<05PD1e~;gQA+ z&M|CGdgOd2OsL=GW5%7E_)*4iZ3(#JDZGWTxM%n9T_9Pvn1!(l5vIo;Ftf;52E*C0 z16v9pP4X%*57A`=$F}1taETnr*)Rk9^XiH&jgnD9Q4tOr=$HX!aD#JNVsWFpRw==X z9D_?SN(fWOgECtyo-<&liIO(_)EZyq6rL}Yg}jrQMr|;05ZVqPC8|WkH*&U7ZcEVi zqc+msq%?ves-etOW4OvtI#$m+zFW@s!J2!rO|IQ|qaNn2bZZ?JVj-60;M@iT?W@GXFCj~>eht~G> zo4CH-n7zTTh`DBd#9a1r6v4eL53@}W!L-4|8oY5b!D!AePM4x^uZZBvKg1=r^F%gC zwRB)W2^7X+@^?hTTA7(>jy^NN!StXg*?k^0Zwe&*4H@_;rX2CQNV^uej8J>~@!Bn*-D58U7KO*P$<81r=1kd2a;z9yV z2{prskxLdJj}ZUNL~A>^iPf-J3d@53S*Uer18|&cfV4By{`wi3&VQ;OE){c|@5ghW zt(iPY#+1DOsn}pQFSOiZC?}d2j>>b0HpD3{MwGs~ejYZq_F587BA-re>axt5vf&i! zPlKi4h<7vIgUTBXx8OHYky0wqN-$v@OU*Trkg~ zB=4eOz{ly4#?lO$a>YXnzZ&`< z1iS%sehOtzJf_3KTN|2!7{>%2Mv#|44MHxxae4P;J~!1qikKwXc9YRw&tCiA%8PwJtw@sbmpl6Axh1_u6NkipV7X8tB6_6Rxvd} zYcnx8D<0n&lzR)j$?7n5B2bWS5D>mN37D?KoLR$ly+<6hf1$U)K~=yGqdN*<+aOAW zUd=|i3zoNU)wAe)0(YQ#jI>AzE)pN%+J$xZF%?}Syu)r-G$O|w5$b553LrE{`IX}+~ez%zk%h$S?vSXx#)Ux^5I*#kC)C}kHE z!O=Ybh2*(>Q!-XX68FQbyJx$a+D#uKC~lK63j31#FkgiTX=1S};A^d@zH^ZoRX<2Pypnt?-du0k0g1N+R@KOZfY&8Cfnno0&v@Btj1B#4 zU+Ao*mbI9Wh>Sy)^n3sN! z4GdfnCHJ85^$N#vNj}OYKhDtG4i3K0F?@bmAWB=M{w+1hZ%fWeiikf7OS(B4lkxoZ z2^btz)*x-CE2vxGiPliiQQpd)>x@ zls@O;i0rKM6WxQUU!LGi|ArmeuZ^pql8pazH)LK(W}w=Yp>Gan%Vae=o7SY zMmuAl^%YOzgYe{rsy^f~v~le5_yNe&%LXRmY7N^QT!<6>Xf%mZw7UEx)16{17% zK!l_kMm^Q0qq)cfZpBVwPPLSjf8;hrCLk%CpgjnDfO13s>pH3X(jD{5tPc?rV$ zqQ&tt(5gNZOYtN;T&1Id&~r$FJ9+h9P7HaR_K|hQJeRiK{ zSY5ETk$RoDn2z(oTC1JLn6aMW`mvQ#`{kDz@U#pjK&HM}aC7S|Q{D)y>6OeBe^uvU zCkZp7GzPRz;<_)PXXmYcQ=s_St$Vc>dr3dLXeVVQ|D7ZTS8s?{$WJMBZ4(D)Vs0Ct_$qtdg@K9I>*&{xiXn%N~=oYh`&nP}^; zc{L<|Y))WqiK8Y^K#w%9#heA{5B!LdcT})5B#RK1knFC>qSMIMLJIO1;a-Rm-#f98 zm2fqr!8yJtlo}0@{De#lV~g@E1U~nxBxqJu9WyPX`h>qbUN^i9zcHy6y4B_{35r zy)>X+cY3Lh6|W9Ue3-fSCe=%FDvTg?Bw+5A3Pka{`ESj!;e;uEhxc084ej8V{0$0` z)}2F+iXdrqtJWjOh??@zhgU$!3oZs5q?oKri|GA^P~M9ZN-8ZLbO@3x0R;A*Q1D;^ za_y-uA7XLPjvq9Np8hm=sdt`{V&<0=7DzsZ9&nqf!0UHUFjO6TwVIj*Ii*l*`!`P|9@uim>&q&ed+dIH~JNDQ6=^j<#na8D}iC zo5d?n0~@IR%a@CH(@(wnXSU9`*Y6%*187yX**=5LfTLg3hZHyh%%{AXn9U^FoU5}~?czxcTFSaW~QDQwFh+pz6we2Hiq$(&m@H6#jM z@e|sfTo2ltH0Q^EX(GfXE;5_mf(ZS!4_3Wg<Z~dYXN!d zq#JYQ_bo5#AGmrq%V%i=Qs!RQJEu7C2&M&p z9FsN;b{q>1o}_zmQ};0FFA46Z({|g141WHdLr1#Vh%r3%q=t%FUcUT9x!{X&c(5Y+ zg|P+e<_$dBA2EaJwHhual>I$!MaPoALkw%`OMW&ef%rZ@ZabFz150OKiX@s zZatZ4Nx((;o=Ax_TJ*)DBO8_;RZS|}(pxfLfr@Emgcu){gsgE{A$nX8ihQ+m{2_rX z*KBdlvAQ{&M|v-7(3dSfno898E2o>M0euNsLuG1o^N_#H!@}RKb9@a<$&3jTLw1kn z>gpqnST*Gx=|(5{xAyUcZ;V_yvLm7VI?Juz(%$d$^arFRe2JwE%W_a+f|m)S!sjC< zGbTGd*}%9qMtG72?1UhRejR2O*Ui8y`xQR|*btoBIMvj&Q! zx)(Gl1J7f26yQ;utAKS6pXQyw-+FxFKd#ELa|eoUW{2UnsC*woOMdJ&^$SKtV43#& z7an-ocJK5_{MT-pYbQlIAzRH;N-8O>9P%p_0vCF!fo8@s!LNQkPDt8FE z-h3;yIY}JMP>U$h2I%ilP^IE2yfgS=}KL zhyuF=k|P=7ZuB^>&?yj|zb?STJ8>T;;q!QQT{~lnb%518a-rPKBv?ZBXs^7um|iu zG&sR$uGk&myd4+OW1KLBZABlT3H!X_y5>3Fsd`+Ffh$R6oon;kuDzusIB@t<6HDRs z&W0E-AeM@n>g8Q9q(4hmI1qT!8QKKNSBo)nZ+Ttha}ddqoOjWoL2z9OY^0Kx^yA|; z>O?c&v;8D2tYOsbs-Dq<9xJNO&FJgU@gZ+0s0nK11R;N`tf6o(bGh0Om0DJoV=Q=v zv|j31((*fXRx*MuZd$^TXr$qT;~qhMIKMba94eQ=5=o@7`W9F=gG80`fi4X$=q&IES2&^o5OVPq_A9?;+t-l(c6>ww!|lF2WCjuw z;t~ITcCemusrP<$vsj@XV|H=);t;6E2byC98knqh%$}Ru*`b)p_iH{b<+h-v=W%`G ziU43%D5hXr>r!F@Lq$2>J$-e~H@tfkvV^5&uerri z&QoWiC~Hq5BYcmz6RkPa6FTrtFUh74lTVDacX*<_F!orwV^eEf3a4u?L-u4uzF2h) z*pNG^)g3qJ3XciZL5B(lKJ*Rr5CO#EbQUu?c;v`so#bcMxuzI_}T z*%>!@xd1(Ss!DHIqaknF^6jD3EI~@Vt*!k$Cg^C8bweM#Gp(e%dc#=0Zc18$fC04L zZ;WPN;{;y&Kl_sO8b`iA0bPD&APWyU0SX}=KE)N3ax=IUbGE7d@ZD|n%l2+)kpIc} zqw&szAGiIY2L$$=xxA+QIiiR&XPka>{=&7+VN+pq!Tj@c(Qx!_g2TWi z;mH=X)Je-?YtofWi&jFuLI5eS4XNkiU; zc;D-1$a8NBFT=s5!}Zh)!d-q>dxOdqe|P9R%K*FNuY{k9eLVS&*1g9ve`@qs+0Sv3 z;^pP}kXc@K)djbYd4K$}VzEniv7vML0XFh$niG*I;yK~Lo<0Y1y&(HKvGiw$r?E#8 z=C|1cY}^_F#b2wmkEHN~(AtcS=y5tP7F3RC*0JycD>J^5wYFzk$yb6$&Ga(!ebvNS zZ*8(EDs#=x{fLCl3lm>WB|rVfPc*RewAWkuIDOm05u3(uUf8tTCe}aY5UR-bd+7ex z*)4s){%sGS>y$iT0#0)Mm#?ya&TjpubGC-MnvqE*8W{%KsfpUz1wHF640$j}()okt8m{H!l^mkhIwaTD)m>^K{=G#Jg6Naeg2&nW=JMMgBkj7=7kb!UerfwQUbe)KyPYE_nLZPtwL&s%TqIiyIdz#p?7qX^LDlcrxbnAjrJe*y9?ts@jMKH)CSO$zXH{ z#7T!~;vDY=bT^PYdR4Y4(i#v>TPd<3nM0;Ma1g@=DKuh{JIW#xa#)!Ti{6Rj{N3+% z5(F~{AdIAov$dQp-8RE4ejx^XFWo!&Ep?I)Lcr8Sbu#0MT@MVRD-0PqPBDIZy?X(R z`b1}OysK?dKm_p_J0{wb%}eF3-pd}q|6NJ;(E>vHP`q=!>}7N6du+U&#QDB_8j67k z`-DqIu;vuy=N=crnC|W7XS1LEUP-hy{A2sp%i#kpj?XQNPUbv%aK<^b>B& z2Jbu0)@TQS>=+sIT-hYx4e?VfP(>p$TisLES)qX8vz22kLPi_!%&ZIviFKgBA=cl~ z4}(I{?#z4xp7P zXkXRmCQLFg7=qocjZ4^qfNoDr#yH>Ms8^f&-0Mx6+l+_c3fjLaDUh~zs>D6_26w&g z*sT@fklLlc8h5F#CQUN{L6c!Hk*Xzvf*RR{oM|@{=zmSF@ z0d)*y0}}M|cC>feRO--=8*vY}$BZ+FSJl*g-9!RmP_^pwn+E$s;ha}8^!>d>$sYMA zh~(dT4gQFlJ^nwcGz8V6eN0-qI(^tCm}V!1oSBW0 z@(F!E=X&`Qk>pzkl#4R+q`A!4Ih-XFXn7xc%cB;}0{L@Ku&dY(nr_wP&N3w(lbM55mzR`VF_ zyB+Y?M!QX)=FPE(!?Fet!=v5c>M0-ts()uWb_QO`yxqYC2DxTscZfjJv3xVu^xi2H z+1TlwpeGxs3nUGGl9RTB{*jI@P!O3wte_UQqReEz8Pg__R&o+q*p0BlDmMQqw|HDo zhwV2GM&=CU4OMz(XS7G1YPOm=;RtV&iTt=r#Sp-Rg=di(QoKs-Zo#$1|Jy+>fq=ZdPUEJzSD;;R}AH_rHaBYd0P^^8`wb6=*Bz z|1XmJcQUI=_$vmX*uxtfowbE>2~4SSvlSf$h2XdP*vtW2iEHwkFCnjd@;RS$YYmft zv5EG0d?^y_DM!^bAPRmg zBk5&lwY))7xrG0Qsl9T`Z^7n10k%X=F{&j|E=M|YOFZ&woPig z3J&L&qU%I{xf5OO=AD-o-JJLXYNt=>OvdmC$R9~{j0{n`xpAbG>?uAoCtY~689^pi zZgcTh`DL9oJ5PPj&?rQDAg6DCd)9qwkQ_S(_8m8{9|`_@AKKcRnA(V18ao3c^8ufE zfAn1LyNt{LGe*e6EnVMciAN(l?%I^R1L?P2tvTLe)AS~`GVYHpZdFIXPLLNrQVF+D zC}?Lp<^8+{_&5y-XZq; zhDgesHw8ud6l4};R$HuP>%fF;0EEJ*TQDzREYWwpx-e750trTyX*pF#@Pym`kPxKG z+TI$)_ERt|FUOinW^+?dgmzFBOYvkCW|2APXt4)Qf_4YT#qiy5H%pbSl6yb1`j#Eb zmXRgR@UDEKa=qH&7bNilic(~1CYto!YW?LOvp8=3MZ)4k54zuB7<1X z4bMRS4OA9U#j7R|kS{QO0;Yd@F8)tYqCmIBcJ=^gOJfxm2L~rp0Kneqe}!yRR(5)v ziE&gGiIIVZMw)?fimCp;)ATBRpqWO3DWbMr@ zjfLzCZ9F_po&NWNyB<}TmUW$wnJBB2W`O&j((5CuF-gfWFCYe4sEk$R{2MxmC~E{o zASz>EBKdddj5!*2PUR}heh4gNW~Z!OctOo zZtr3T3^4ourT4H3a&YofOp**SZsSaoGE(%Qw8;NcY<*R-|4=cDM1KB;zlm*n2;|fQ z7c@;g=zE?~4Cm)JHM>79?0eM{C?9Py+KV&eU5rI3mrL9MpZ!cm%Z`du5|yl1P|ea? z62<=f>&f_6QvPkfs`c6LeC53Wi}G-n%TElon1CqTW!0hy5}zHTV%Bk5&}G`DP^z@G z>RWhTWwZ2X6P7efQ9sd}(F0jxZl?UM)5h&)8J}7`nfKeBAS2InCnFtgc?#Li0GD;P zZ{-$*#=s5OypWYgqQrC8-;VJ*^_;h9$qTh8)VDjBlhY?T_wb_bR}wQH=o30VK|uUB2_E91TExsL6Da?S*xhdQQX` z@z0fed#hq1M~^OCdKCE3J!EhO4K1$932gQGrp>`xxvp6zZD@8;;9X2nJ*=4gS`0o6 zFWRtfVNbI%kvt1ET6Kn2;e-r0q_f8-vB-fq;%z}B<6-?MzPnB2)NbEkU8--}Gu+&zudd_Fp^QQ7yJJOHy(XxY=Jyu;tU*`OX?>~om1 zH|P(u2}ZOD{vO>D5RFqG$59)n#-X7;Zw6N}z%kKuV>Dp{#m+WZU#by>t9v2qGYb~I z{dgQhhZW&L!PPw`cDd&9O%uoF3u^5}ye^$GrnY9Kp`(ZK?Mvd1(yF|j48#7su%NcZRTYQ56cgGtR2uH$=_ver3eK!7L$t_}KmRcFQbQH^1%SNzCr z#vbOyCa?AqRT*NsW1AyozkaSG(iEcLY}WK#x~}h#C@e5sW#H{h`{hg~sKE%r^1pg6 z;M3@yrIQ6mbN368&I00}isDnkVrzf%KyaBmoSJ7uTwDi)G@&>9SUVg5_Z~mJ>a&D8 zFo46v)?j?m6V6mDO*|2aW0K7O_N7jdAzUC@haQmnCDu9GXuhvs>_rS3Y|gJk@%xz3 zDv;6em@wb#@a(n}YsH1%XE;H{mBCw~{_ayOUMbs4*+mWO?r+T=!us zK&#TLDvK{F1uIB-RQJq&+9?uVOvAO;*>Eq!z{Yo?Tl|`B@-k?M;6#lTtcjRQZ0dn+z}sltN-@rP!#;YrWE`TF%$d< zc!w_dse0avI>UI1cl?oe!c_o^cPs#dYbrGIU$slST3>d@2fPy(X0o>Y{sY!8@=hRH z8}!X@^RW~AEhU(<}`@u$$UKIn5d;7&hcOThn~{7+kvy}=>x{FK1%1WcfS zApDr0br*>UK+o%|fn}!uQM!Qlpzlgp!3cN6!BO=jCb=KECe8QnbhyVKLStM(+5&${ zX_ow$Z^4}TSdRYbHT$7_GI`RSX#1>BOZiuwkbmmz#_-7jzas{HgMRkg#;3{sh(yc% zAZ7h?chKx*`TaJGMT!gJ-N(#b<}3u_KPw|KNrWT?e}krMZ+y@n1>#kn!rgfOQ%bkk zbIwqe|9iLH>sI7@N`-dbM}6co0zipW+q*jxWYYN2w~rch3;q%w)-S|>&fSlr=mea} zoZBux;9O$KU6SWBpaa_G=Wx?8#$XwV+kAmXdKYoWM9nXxh+zpkc#|K655A4YcjGJX zvaH{T1N~5K=(fj2_B0wCY=ARZNi=&pJ!G-I2J^K}uh)SU#=4u-Y8S{@Hk|%R-SJ8G z&#mxZA~eOJN~pAj4+EkQ!V#wA?v+1l+B@0RzdJ=|(ey*- zx9@3?bNi^&AlG>n_c3)V)uCmnC&e5}1NR@S#NVsJGhSCyA6IshFa>zJ;t6&-w<~r0Ixp;=i8Ly`K#$hKN8Ga=-yK@=Gv62y>-;%yOS|4 zBK(vymXgNsnO+K6mmF$+JrSU^jW_2j{^9}ee~Z^}eAoo91_1(civR+G^WS-uo28uz z@N43KegQ2~Uw6%y#PGSSF}VJ+N&7HfX;GsRn4-5PjW$Wq@C=$q;f$gqNHMysfGH{# zJZ1U|G~}$oI>$$zaCh*bM6Tz|A@fmb#q^0|nB|Kz*)nn`<4bC_Hr`9;!@=$l@+OVU z&<^)6AH&qh)nA;h%4S3o>pAyOZkqeaB=jn;J~k-Q$tTdR@ffvqez!P}J0keFroe?? z#3dIAgS8i( z>gSbyP1w)$;xY3=?~p$Yvh(Kb6v%yJ#)NF52K?Sez3+Uv>pTu1F8orR|J(Cw0r?H+ z(tR`HMpL7yO9f|L2kzDqU-WZKoR1o8Sr^*kw`r_h!ZjU%=-q~x6ny7{V=Af3)29S~ zYp+Jy)fcs%xnW#XkGGD&r1&#RJ&ASLt({zcU$oVWq|Sch0<^6ehC`Kzel{+FBrQiI zQ+Xb~u%eZAnl%KQutygx#zqfKHFo(uXQg>lA=~5bRnM&LJ8w53YJ@-x=VAWazy zE=v6ZRk^4hI;LDQg{Jk4%fU8^{v3DM!qjxVpD{$XeMLPDOnLrk{m3X;FRZXOii;8| zq}Ysy{GOjrDd9=Wu|$R~hVst}-LeI@11Uunz|!4>Lp^gy_sQWG`J7?H`oSMMh@D!t zX+8wE;@c9`W&%U55ihPNl{QB^&hzq!zCy)~h9&pwBSI)~DYC~|GL{w}0tlBf+|B~b zotP&gd)jk&+d2575|($-K1panbk~Av)CKAyCF)n(F27Isp1G%=pKPh~mhqVxG)#{< z5zsZvEZ{}pFIX^oMglF(lqXb(pof1a-4tlIK>P^W!8k*B1Mi1!0mtZb0A&PGCy7SV z9hMX+S`G;lH!!RHF#{6yISSN~8LvM}?4I(M9qZV5K*Fue-4ppD>nJ~VN)4B4zS55G zJKw_yuk`v$eQ{SVWGa#lxK?1Re;G6!0ti9uRgC`)zlxqs9Ve}}SuXh%F>>8lb z1{TT!dKNx*e4gF@khw4FerP!R|7X92_Zw}n`84N;=O?~|3h!4Ix=~9ve_y?&uS(?> zbBppqneAd9lU%9}DcMb~-qh}DI$;iT)&4Wtle1qxzhzT0^}Ao2(xcYGsMVil$$gsr z_xztVd-TLku75H8m+kJmpFCSEw$1luuJil<)j#OXp)&7#t4p*dPi${)Nad+-KK!Vh z@w<5aoP?>DcUOE_-G0ANX3eKfw~AcWY*T3rj^3qnQa5&riTlyMwO+5AS6qHrpy2Mt zyD@3?CZ$te-WQIRdIwqVPSw`l*&2JcY{R&np2=XT*n80{eNfpk5TZ0RemZ-2H(% z_5q7DZm2eh0U|()AS0#V+mg^V#y^<-D-5V|Cr}yi>KZVBXzT$FBIc%+Bm%pXiTcU; zMX3Qrz;0$yNoFeeCL45v*8A;@I|kJ54%97+VvsZ|l0muUgpDdHsj}V!G=c?alp2ar zGQfMG;BG=U%C!O*NX5YJFSY>sa_5~x0q|lTKH!9x7}QJ%B?G**2X1CbMrm$RUSejB zUO`?ux);%JrvmARgw+8#q!AxHcU7UAh<;fT!o+3;3==VC=dea7`b9(tBdZjVj6}Vb z2;E)imnd*~ApRGg~qLzov5LnHP zZVvh+BEp>O$mURSN)g=!=o1?V7aT1@@(G#?2uyRJTY^3ZfUx9$F>Xt+%?O~Ij6O1r zF!^vPlF3-csnJbBAHhYK^t24gB-qd?mhoJ46VZoX5hhlZV=)oeU@W?^=%c0xW2Gvv z7>i>J72Q=NBD^x;N?VVAHOhJWx8 z-Ei~)LWJS9RY)#H87@RO1HIjjFhi>u$qckse1JDAu;u`@^d%SyfaU$Rb`TE$o}4HH From e46e4442bff75c4f96a44fe62f3209839b713358 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Thu, 29 Apr 2021 17:08:16 +0200 Subject: [PATCH 04/20] Fixed sampling rate transitions between sub-systems and treat "MOMENTUM_BASED_TORQUE_CONTROL" as atomic unit. - Moved "Dump and visualze" into "MOMENTUM_BASED_TORQUE_CONTROL" sub-system. - Moved the "STATE_MACHINE" clock outside the controller sub-system. - Removed zero-order holder in front of that clock. - Set all controller input ports sample time to inherited. - Treat "MOMENTUM_BASED_TORQUE_CONTROL" sub-system as an atomic unit. - Replaced delay block by memory block between the controller and the simulator. - Fixed model tasking and sampling time options: Removed "Treat each dicrete rate as a separate task" option, which was causing data rates transition problems between the simulator (1kHz) and the controller (100Hz). - The model is now fully compiling. --- .../torqueControlBalancingWithSimu.mdl | 15052 ++++++++-------- 1 file changed, 7549 insertions(+), 7503 deletions(-) diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl index 87ea58e8..38516187 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl +++ b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl @@ -7,7 +7,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "3.72" + ComputedModelVersion "3.86" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -382,7 +382,7 @@ Model { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [-75.0, -1417.0, 2560.0, 1417.0] + Location [43.0, 23.0, 1749.0, 1097.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -402,28 +402,28 @@ Model { } Array { Type "Simulink.EditorInfo" - Dimension 55 + Dimension 60 Object { $ObjectID 5 IsActive [0] IsTabbed [1] ViewObjType "SimulinkSubsys" - LoadSaveID "4553" - Extents [2075.0, 1188.0] - ZoomFactor [2.8272565416779067] - Offset [-123.97523433683438, -125.59766579140205] - SceneRectInView [-123.97523433683438, -125.59766579140205, 733.92703117366875, 420.1953315828041] + LoadSaveID "4537" + Extents [1792.0, 1120.0] + ZoomFactor [5.1816867469879524] + Offset [-108.23828125, -66.072916666666657] + SceneRectInView [-108.23828125, -66.072916666666657, 345.83333333333331, 216.14583333333331] } Object { $ObjectID 6 IsActive [1] IsTabbed [1] - ViewObjType "SimulinkSubsys" - LoadSaveID "4836" - Extents [2075.0, 1188.0] - ZoomFactor [1.7391304347826086] - Offset [78.1640625, -29.050000000000011] - SceneRectInView [78.1640625, -29.050000000000011, 1193.125, 683.1] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [1360.0, 868.0] + ZoomFactor [1.2548419646450668] + Offset [708.83934245941441, 103.12909008770396] + SceneRectInView [708.83934245941441, 103.12909008770396, 1083.8018159399676, 691.72057076168517] } Object { $ObjectID 7 @@ -431,10 +431,10 @@ Model { IsTabbed [1] ViewObjType "SimulinkSubsys" LoadSaveID "4955" - Extents [2075.0, 1188.0] - ZoomFactor [2.4395775512260358] - Offset [102.27209051724134, 108.01522137450439] - SceneRectInView [102.27209051724134, 108.01522137450439, 850.55709705034235, 486.96955725099122] + Extents [1360.0, 868.0] + ZoomFactor [1.5228707839211457] + Offset [97.634961569579332, 66.511933656957922] + SceneRectInView [97.634961569579332, 66.511933656957922, 893.0501618122978, 569.97613268608416] } Object { $ObjectID 8 @@ -442,46 +442,90 @@ Model { IsTabbed [1] ViewObjType "SimulinkSubsys" LoadSaveID "2355" - Extents [2075.0, 1188.0] - ZoomFactor [1.0730804810360777] - Offset [942.03160021551719, -198.046551724138] - SceneRectInView [942.03160021551719, -198.046551724138, 1933.6853448275863, 1107.093103448276] + Extents [1264.0, 868.0] + ZoomFactor [1.0] + Offset [1012.8742726293103, -129.0] + SceneRectInView [1012.8742726293103, -129.0, 1264.0, 868.0] } Object { $ObjectID 9 IsActive [0] + IsTabbed [1] + ViewObjType "SimulinkSubsys" + LoadSaveID "2163" + Extents [1792.0, 1120.0] + ZoomFactor [1.4201130022789792] + Offset [-608.35758121641425, -114.33481638525888] + SceneRectInView [-608.35758121641425, -114.33481638525888, 1261.8714124328285, 788.66963277051775] + } + Object { + $ObjectID 10 + IsActive [0] + IsTabbed [1] + ViewObjType "SimulinkSubsys" + LoadSaveID "4553" + Extents [1792.0, 1120.0] + ZoomFactor [2.441659625391233] + Offset [-116.92703117366869, -144.85219724177151] + SceneRectInView [-116.92703117366869, -144.85219724177151, 733.92703117366875, 458.704394483543] + } + Object { + $ObjectID 11 + IsActive [0] + IsTabbed [1] + ViewObjType "SimulinkSubsys" + LoadSaveID "4836" + Extents [1360.0, 868.0] + ZoomFactor [1.2349950019557565] + Offset [124.11704673423424, -38.918426238738732] + SceneRectInView [124.11704673423424, -38.918426238738732, 1101.2190315315315, 702.83685247747746] + } + Object { + $ObjectID 12 + IsActive [0] + IsTabbed [1] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937" + Extents [1360.0, 868.0] + ZoomFactor [1.0] + Offset [155.02749177730379, -243.5] + SceneRectInView [155.02749177730379, -243.5, 1360.0, 868.0] + } + Object { + $ObjectID 13 + IsActive [0] IsTabbed [0] - ViewObjType "SimulinkTopLevel" - LoadSaveID "0" + ViewObjType "SimulinkSubsys" + LoadSaveID "4220" Extents [2075.0, 1188.0] - ZoomFactor [1.75] - Offset [691.44166877486214, 105.0577102335036] - SceneRectInView [691.44166877486214, 105.0577102335036, 1185.7142857142858, 678.85714285714289] + ZoomFactor [5.95] + Offset [-25.672433035714278, -45.831932773109244] + SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 10 + $ObjectID 14 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4307" + LoadSaveID "2345" Extents [2075.0, 1188.0] - ZoomFactor [2.56669704897075] - Offset [-29.368338160419114, -144.92583198051949] - SceneRectInView [-29.368338160419114, -144.92583198051949, 808.43198882083823, 462.851663961039] + ZoomFactor [4.8] + Offset [-29.932291666666686, -42.75] + SceneRectInView [-29.932291666666686, -42.75, 432.29166666666669, 247.5] } Object { - $ObjectID 11 + $ObjectID 15 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "2163" + LoadSaveID "4307" Extents [2075.0, 1188.0] - ZoomFactor [2.0] - Offset [-576.99385991695169, 9.249712994626293] - SceneRectInView [-576.99385991695169, 9.249712994626293, 1037.5, 594.0] + ZoomFactor [2.56669704897075] + Offset [-29.368338160419114, -144.92583198051949] + SceneRectInView [-29.368338160419114, -144.92583198051949, 808.43198882083823, 462.851663961039] } Object { - $ObjectID 12 + $ObjectID 16 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -492,7 +536,7 @@ Model { SceneRectInView [10.776074743527147, -15.071488336727981, 259.375, 148.5] } Object { - $ObjectID 13 + $ObjectID 17 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -503,18 +547,7 @@ Model { SceneRectInView [578.59213941133362, 441.73077216658527, 393.93290867733265, 225.53845566682949] } Object { - $ObjectID 14 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "2275" - Extents [2075.0, 1188.0] - ZoomFactor [1.8249725000639532] - Offset [-168.50171712924032, -141.48435660218672] - SceneRectInView [-168.50171712924032, -141.48435660218672, 1137.0034342584806, 650.96871320437344] - } - Object { - $ObjectID 15 + $ObjectID 18 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -525,18 +558,18 @@ Model { SceneRectInView [-25.756466649159648, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 16 + $ObjectID 19 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "2087" Extents [2075.0, 1188.0] - ZoomFactor [1.659551344359659] - Offset [-719.33693596886076, 2346.0719358766232] - SceneRectInView [-719.33693596886076, 2346.0719358766232, 1250.3379344377215, 715.85612824675331] + ZoomFactor [1.75] + Offset [-709.226772339876, 2332.9648668093268] + SceneRectInView [-709.226772339876, 2332.9648668093268, 1185.7142857142858, 678.85714285714289] } Object { - $ObjectID 17 + $ObjectID 20 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -547,18 +580,7 @@ Model { SceneRectInView [-1148.4886014344261, -284.77258542366184, 1309.1803278688524, 749.54517084732367] } Object { - $ObjectID 18 - 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 19 + $ObjectID 21 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -569,7 +591,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 20 + $ObjectID 22 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -580,7 +602,7 @@ Model { SceneRectInView [23.229550867122612, 135.34665821934539, 447.67371076575478, 256.30668356130923] } Object { - $ObjectID 21 + $ObjectID 23 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -591,7 +613,7 @@ Model { SceneRectInView [94.721007009531661, 154.55463376836479, 629.70491803278685, 360.52503258937384] } Object { - $ObjectID 22 + $ObjectID 24 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -602,7 +624,7 @@ Model { SceneRectInView [-676.15435221738494, -478.27262581168833, 1319.6602669347699, 755.54525162337666] } Object { - $ObjectID 23 + $ObjectID 25 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -613,7 +635,7 @@ Model { SceneRectInView [-92.890717237308081, -373.76948051948051, 862.03143447461616, 493.538961038961] } Object { - $ObjectID 24 + $ObjectID 26 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -624,7 +646,18 @@ Model { SceneRectInView [161.34960569474492, -350.98802908437432, 2075.0, 1188.0] } Object { - $ObjectID 25 + $ObjectID 27 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4353" + Extents [2075.0, 1188.0] + ZoomFactor [3.0839924670433145] + Offset [111.71042989741085, -4.1074743527112787] + SceneRectInView [111.71042989741085, -4.1074743527112787, 672.82914020517831, 385.21494870542256] + } + Object { + $ObjectID 28 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -635,7 +668,62 @@ Model { SceneRectInView [434.56854543234, -203.35510808500248, 1485.87853413532, 850.710216170005] } Object { - $ObjectID 26 + $ObjectID 29 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "3241" + Extents [2075.0, 1188.0] + ZoomFactor [1.5612382234185733] + Offset [-796.48981681034491, -71.967241379310337] + SceneRectInView [-796.48981681034491, -71.967241379310337, 1329.0732758620691, 760.93448275862067] + } + Object { + $ObjectID 30 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4104" + Extents [2075.0, 1188.0] + ZoomFactor [3.0933333333333333] + Offset [-113.1169934431619, -154.52586206896552] + SceneRectInView [-113.1169934431619, -154.52586206896552, 670.79741379310349, 384.05172413793105] + } + Object { + $ObjectID 31 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2275" + Extents [2075.0, 1188.0] + ZoomFactor [1.8249725000639532] + Offset [-168.50171712924032, -141.48435660218672] + SceneRectInView [-168.50171712924032, -141.48435660218672, 1137.0034342584806, 650.96871320437344] + } + Object { + $ObjectID 32 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4620" + Extents [2075.0, 1188.0] + ZoomFactor [10.0] + Offset [87.32421875, 140.1] + SceneRectInView [87.32421875, 140.1, 207.5, 118.8] + } + Object { + $ObjectID 33 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4504" + Extents [2075.0, 1188.0] + ZoomFactor [0.99107089183204411] + Offset [-371.86694504310344, -625.85167594516] + SceneRectInView [-371.86694504310344, -625.85167594516, 2093.6948275862069, 1198.70335189032] + } + Object { + $ObjectID 34 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -646,7 +734,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 27 + $ObjectID 35 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -657,7 +745,7 @@ Model { SceneRectInView [-191.40632054176072, -117.29872084273889, 691.66666666666663, 396.0] } Object { - $ObjectID 28 + $ObjectID 36 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -668,7 +756,7 @@ Model { SceneRectInView [-92.7656132430399, -55.8837471783296, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 29 + $ObjectID 37 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -679,7 +767,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 30 + $ObjectID 38 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -690,7 +778,7 @@ Model { SceneRectInView [-99.198645598194133, -48.972911963882623, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 31 + $ObjectID 39 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -701,7 +789,7 @@ Model { SceneRectInView [-80.667161130818016, -31.015048908954071, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 32 + $ObjectID 40 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -712,7 +800,7 @@ Model { SceneRectInView [13.879095336481726, -85.310064935064929, 502.36680932703655, 287.62012987012986] } Object { - $ObjectID 33 + $ObjectID 41 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -723,7 +811,7 @@ Model { SceneRectInView [13.972911622276058, -85.325665859564168, 502.42130750605327, 287.65133171912834] } Object { - $ObjectID 34 + $ObjectID 42 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -734,7 +822,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 35 + $ObjectID 43 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -745,18 +833,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 36 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "4937" - Extents [2075.0, 1188.0] - ZoomFactor [1.2328521092206539] - Offset [-54.517079741379348, -291.3096149225039] - SceneRectInView [-54.517079741379348, -291.3096149225039, 1683.0891430373663, 963.61922984500779] - } - Object { - $ObjectID 37 + $ObjectID 44 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -767,7 +844,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 38 + $ObjectID 45 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -778,7 +855,7 @@ Model { SceneRectInView [-64.168329397874857, -71.912514757969319, 518.75, 297.0] } Object { - $ObjectID 39 + $ObjectID 46 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -789,18 +866,7 @@ Model { SceneRectInView [-684.37478764306115, -290.08334454895737, 840.66666666666652, 579.97212543554] } Object { - $ObjectID 40 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "3241" - Extents [1722.0, 1188.0] - ZoomFactor [1.5612382234185733] - Offset [-683.4385237068966, -71.967241379310337] - SceneRectInView [-683.4385237068966, -71.967241379310337, 1102.9706896551725, 760.93448275862067] - } - Object { - $ObjectID 41 + $ObjectID 47 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -811,7 +877,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 42 + $ObjectID 48 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -822,18 +888,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 43 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "4504" - Extents [2522.0, 1188.0] - ZoomFactor [1.2045690550363448] - Offset [-371.86694504310344, -519.62241379310342] - SceneRectInView [-371.86694504310344, -519.62241379310342, 2093.6948275862069, 986.24482758620684] - } - Object { - $ObjectID 44 + $ObjectID 49 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -844,7 +899,7 @@ Model { SceneRectInView [203.38965517241377, -277.26034482758621, 524.02241379310351, 361.52068965517242] } Object { - $ObjectID 45 + $ObjectID 50 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -855,7 +910,7 @@ Model { SceneRectInView [-8.0130751303127283, -114.03393582177664, 473.82302526062546, 223.19657177225341] } Object { - $ObjectID 46 + $ObjectID 51 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -866,7 +921,7 @@ Model { SceneRectInView [16.019427876904615, -398.80851543704892, 1030.9142692461908, 485.61703087409785] } Object { - $ObjectID 47 + $ObjectID 52 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -877,7 +932,7 @@ Model { SceneRectInView [-105.54956270048115, -242.68495036561927, 864.60693790096229, 407.2771777265437] } Object { - $ObjectID 48 + $ObjectID 53 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -888,7 +943,7 @@ Model { SceneRectInView [-396.83333333333337, -304.36666666666662, 1148.0, 792.0] } Object { - $ObjectID 49 + $ObjectID 54 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -899,7 +954,7 @@ Model { SceneRectInView [34.9092200413223, -92.857244318181813, 438.7831224173554, 302.71448863636363] } Object { - $ObjectID 50 + $ObjectID 55 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -910,7 +965,7 @@ Model { SceneRectInView [-183.3666666666665, -165.10000000000005, 867.6, 475.2] } Object { - $ObjectID 51 + $ObjectID 56 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -921,7 +976,7 @@ Model { SceneRectInView [10.41989407940855, -23.235071395084702, 501.20498970990775, 274.51891552575859] } Object { - $ObjectID 52 + $ObjectID 57 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -932,7 +987,7 @@ Model { SceneRectInView [-245.23623571571773, -105.36577786688051, 666.01934643143545, 313.731555733761] } Object { - $ObjectID 53 + $ObjectID 58 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -943,7 +998,7 @@ Model { SceneRectInView [-68.684210488152473, -33.394738158739756, 477.63157399976672, 216.05262934094131] } Object { - $ObjectID 54 + $ObjectID 59 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -954,7 +1009,7 @@ Model { SceneRectInView [-211.69612560424537, 280.84982585720286, 605.0, 273.66666666666669] } Object { - $ObjectID 55 + $ObjectID 60 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -965,7 +1020,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 56 + $ObjectID 61 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -976,7 +1031,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 57 + $ObjectID 62 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -987,7 +1042,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 58 + $ObjectID 63 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -998,7 +1053,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 59 + $ObjectID 64 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1014,7 +1069,7 @@ Model { Type "Simulink.DockComponentInfo" Dimension 2 Object { - $ObjectID 60 + $ObjectID 65 Type "Simulink:Editor:ReferencedFiles" ID "Referenced Files" Visible [0] @@ -1027,7 +1082,7 @@ Model { Minimized "Unset" } Object { - $ObjectID 61 + $ObjectID 66 Type "GLUE2:PropertyInspector" ID "Property Inspector" Visible [0] @@ -1041,12 +1096,12 @@ Model { } PropName "DockComponentsInfo" } - WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAG+AAAE4fwCAAAABPsAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAFeAAADofwCAAAABPsAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQBAAAAeQAABOEAAABrAP////sAAABgAFMAaQBtAHUAb" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQBAAAAeQAAA6EAAABrAP////sAAABgAFMAaQBtAHUAb" "ABpAG4AawA6AEUAZABpAHQAbwByADoAUgBlAGYAZQByAGUAbgBjAGUAZABGAGkAbABlAHMALwBSAGUAZgBlAHIAZQBuAGMAZQBkACAARgBpAGwAZ" "QBzAAAAAAD/////AAAAjwD///8AAAABAAABYAAABOH8AgAAAAH7AAAAVABHAEwAVQBFADIAOgBQAHIAbwBwAGUAcgB0AHkASQBuAHMAcABlAGMAd" - "ABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA////AAAIQQAABOEAAAABAAAAAgAAAAEAAAAC/" + "ABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA////AAAFdgAAA6EAAAABAAAAAgAAAAEAAAAC/" "AAAAAEAAAACAAAAAA==" Array { Type "Cell" @@ -1066,9 +1121,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "nunoguedelha" ModifiedDateFormat "%" - LastModifiedDate "Thu Apr 29 00:41:09 2021" - RTWModifiedTimeStamp 541557663 - ModelVersionFormat "%" + LastModifiedDate "Thu Apr 29 17:07:41 2021" + RTWModifiedTimeStamp 541615217 + ModelVersionFormat "%" SampleTimeColors on SampleTimeAnnotations on LibraryLinkDisplay "disabled" @@ -1125,7 +1180,7 @@ Model { TryForcingSFcnDF off Object { $PropName "DataLoggingOverride" - $ObjectID 62 + $ObjectID 67 $ClassName "Simulink.SimulationData.ModelLoggingInfo" model_ "torqueControlBalancingWithSimu" signals_ [] @@ -1173,7 +1228,7 @@ Model { Type "Handle" Dimension 1 Simulink.ConfigSet { - $ObjectID 63 + $ObjectID 68 Version "20.1.0" DisabledProps [] Description "" @@ -1181,7 +1236,7 @@ Model { Type "Handle" Dimension 9 Simulink.SolverCC { - $ObjectID 64 + $ObjectID 69 Version "20.1.0" DisabledProps [] Description "" @@ -1202,7 +1257,7 @@ Model { MinStep "auto" MaxConsecutiveMinStep "1" RelTol "1e-3" - EnableMultiTasking on + EnableMultiTasking off ConcurrentTasks off SolverName "FixedStepDiscrete" SolverJacobianMethodControl "auto" @@ -1223,7 +1278,7 @@ Model { ODENIntegrationMethod "ode3" } Simulink.DataIOCC { - $ObjectID 65 + $ObjectID 70 Version "20.1.0" DisabledProps [] Description "" @@ -1265,7 +1320,7 @@ Model { LoggingIntervals "[-inf, inf]" } Simulink.OptimizationCC { - $ObjectID 66 + $ObjectID 71 Version "20.1.0" Array { Type "Cell" @@ -1339,7 +1394,7 @@ Model { EfficientTunableParamExpr off } Simulink.DebuggingCC { - $ObjectID 67 + $ObjectID 72 Version "20.1.0" Array { Type "Cell" @@ -1462,7 +1517,7 @@ Model { VariantConditionMismatch "none" } Simulink.HardwareCC { - $ObjectID 68 + $ObjectID 73 Version "20.1.0" DisabledProps [] Description "" @@ -1512,7 +1567,7 @@ Model { HardwareBoardFeatureSet "EmbeddedCoderHSP" } Simulink.ModelReferenceCC { - $ObjectID 69 + $ObjectID 74 Version "20.1.0" DisabledProps [] Description "" @@ -1532,7 +1587,7 @@ Model { SupportModelReferenceSimTargetCustomCode off } Simulink.SFSimCC { - $ObjectID 70 + $ObjectID 75 Version "20.1.0" DisabledProps [] Description "" @@ -1576,7 +1631,7 @@ Model { } Simulink.RTWCC { $BackupClass "Simulink.RTWCC" - $ObjectID 71 + $ObjectID 76 Version "20.1.0" Array { Type "Cell" @@ -1682,7 +1737,7 @@ Model { Type "Handle" Dimension 2 Simulink.CodeAppCC { - $ObjectID 72 + $ObjectID 77 Version "20.1.0" Array { Type "Cell" @@ -1772,7 +1827,7 @@ Model { } Simulink.GRTTargetCC { $BackupClass "Simulink.TargetCC" - $ObjectID 73 + $ObjectID 78 Version "20.1.0" Array { Type "Cell" @@ -1802,7 +1857,7 @@ Model { Type "Handle" Dimension 1 Simulink.CPPComponent { - $ObjectID 74 + $ObjectID 79 Version "20.1.0" Array { Type "Cell" @@ -1913,7 +1968,7 @@ Model { } } SlCovCC.ConfigComp { - $ObjectID 75 + $ObjectID 80 Version "20.1.0" DisabledProps [] Description "Simulink Coverage Configuration Component" @@ -1962,11 +2017,11 @@ Model { } Simulink.ConfigSet { $PropName "ActiveConfigurationSet" - $ObjectID 63 + $ObjectID 68 } Object { $PropName "DataTransfer" - $ObjectID 76 + $ObjectID 81 $ClassName "Simulink.GlobalDataTransfer" DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" @@ -2144,6 +2199,14 @@ Model { 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" @@ -2321,16 +2384,12 @@ Model { StateMustResolveToSignalObject off HasFrameUpgradeWarning off } - Block { - BlockType ZeroOrderHold - SampleTime "1" - } } System { Name "torqueControlBalancingWithSimu" - Location [-75, -1417, 2485, 0] + Location [43, 23, 1792, 1120] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] - Open off + Open on PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" @@ -2344,9 +2403,9 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "175" + ZoomFactor "125" ReportName "simulink-default.rpt" - SIDHighWatermark "4998" + SIDHighWatermark "5002" SimulinkSubDomain "Simulink" Block { BlockType BusCreator @@ -2389,7 +2448,7 @@ Model { Name "Bus\nSelector2" SID "4945" Ports [1, 6] - Position [760, 419, 765, 611] + Position [760, 404, 765, 596] ZOrder 1277 OutputSignals "joints_position,joints_velocity,nuDot,signal6,wrench_RFoot,wrench_LFoot" Port { @@ -2417,6 +2476,14 @@ Model { Name "" } } + Block { + BlockType Clock + Name "Clock1" + SID "2166" + Position [875, 595, 895, 615] + ZOrder 1285 + ShowName off + } Block { BlockType Reference Name "Configuration" @@ -2441,26 +2508,19 @@ Model { } Block { BlockType SubSystem - Name "Dump and visualize" - SID "2275" - Ports [] - Position [955, 266, 1075, 294] - ZOrder 547 - ForegroundColor "red" - BackgroundColor "yellow" - ShowName off + Name "MOMENTUM BASED TORQUE CONTROL" + SID "4836" + Ports [8, 1] + Position [940, 388, 1090, 612] + ZOrder 961 + TreatAsAtomicUnit on + SystemSampleTime "Config.tStep" RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 77 - $ClassName "Simulink.Mask" - Display "disp('VISUALIZER')" - } System { - Name "Dump and visualize" - Location [-75, -1417, 2485, 0] + Name "MOMENTUM BASED TORQUE CONTROL" + Location [43, 23, 1792, 1120] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] - Open off + Open on PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" @@ -2474,23 +2534,85 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "182" + ZoomFactor "123" 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 "Desired and Measured Forces" - SID "3241" - Ports [4, 0, 1] - Position [260, 313, 435, 387] - ZOrder 899 - ForegroundColor "red" - BackgroundColor "yellow" + Name "Balancing Controller QP" + SID "2355" + Ports [21, 1] + Position [800, 3, 955, 607] + ZOrder 542 + BackgroundColor "lightBlue" RequestExecContextInheritance off System { - Name "Desired and Measured Forces" - Location [-75, -1417, 2485, 0] + Name "Balancing Controller QP" + Location [43, 23, 1792, 1120] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] - Open off + Open on PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" @@ -2504,122 +2626,186 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "156" + ZoomFactor "100" SimulinkSubDomain "Simulink" Block { BlockType Inport - Name "wrench_leftFoot" - SID "3245" - Position [-580, 237, -550, 253] - ZOrder 648 + Name "M" + SID "2360" + Position [1065, 168, 1095, 182] + ZOrder 326 } Block { BlockType Inport - Name "wrench_rightFoot" - SID "3251" - Position [-580, 337, -550, 353] - ZOrder 682 + Name "h" + SID "2361" + Position [1065, 213, 1095, 227] + ZOrder 327 Port "2" } Block { BlockType Inport - Name "f_star" - SID "4510" - Position [-580, 438, -550, 452] - ZOrder 688 + Name "L " + SID "3219" + Position [1065, 258, 1095, 272] + ZOrder 639 Port "3" } Block { BlockType Inport - Name "state" - SID "4785" - Position [-345, 68, -315, 82] - ZOrder 690 + Name "w_H_l_sole" + SID "4384" + Position [1065, 348, 1095, 362] + ZOrder 837 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 + BlockType Inport + Name "w_H_r_sole" + SID "4391" + Position [1065, 393, 1095, 407] + ZOrder 844 + Port "5" } 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" - } + BlockType Inport + Name "J_l_sole" + SID "2357" + Position [1065, 438, 1095, 452] + ZOrder 550 + Port "6" } 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" - } + 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 "Demux Forces nd Moments" - SID "4806" - Ports [3, 8] - Position [-440, 191, -220, 494] - ZOrder 697 + Name "Compute Desired Torques" + SID "4393" + Ports [13, 2] + Position [1825, -185, 1990, 865] + ZOrder 846 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] + Name "Compute Desired Torques" + Location [134, 55, 3840, 2160] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off @@ -2635,4371 +2821,150 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "205" + ZoomFactor "284" SimulinkSubDomain "Simulink" Block { BlockType Inport - Name "wrench_leftFoot" - SID "4807" - Position [-620, -98, -590, -82] - ZOrder 697 + Name "feetContactStatus" + SID "4410" + Position [430, 464, 460, 476] + ZOrder 640 } Block { BlockType Inport - Name "wrench_rightFoot" - SID "4808" - Position [-620, 67, -590, 83] - ZOrder 698 + Name "HessianMatrixOneFoot" + SID "4411" + Position [430, 508, 460, 522] + ZOrder 641 Port "2" } Block { BlockType Inport - Name "f_star" - SID "4809" - Position [-200, -22, -170, -8] - ZOrder 699 + Name "gradientOneFoot" + SID "4412" + Position [430, 553, 460, 567] + ZOrder 642 Port "3" } Block { - BlockType Demux - Name "Demux1" - SID "4801" - Ports [1, 2] - Position [-500, -159, -495, -26] - ZOrder 692 - ShowName off - Outputs "[3;3]" + BlockType Inport + Name "ConstraintsMatrixOneFoot" + SID "4413" + Position [430, 598, 460, 612] + ZOrder 647 + Port "4" } Block { - BlockType Demux - Name "Demux2" - SID "4802" - Ports [1, 2] - Position [-90, -144, -85, 114] - ZOrder 693 - ShowName off - Outputs "[6;6]" + BlockType Inport + Name "bVectorConstraintsOneFoot" + SID "4414" + Position [430, 643, 460, 657] + ZOrder 648 + Port "5" } Block { - BlockType Demux - Name "Demux5" - SID "4803" - Ports [1, 2] - Position [-10, -127, -5, -38] - ZOrder 694 - ShowName off - Outputs "[3;3]" + BlockType Inport + Name "HessianMatrixTwoFeet" + SID "4415" + Position [430, 688, 460, 702] + ZOrder 643 + Port "6" } Block { - BlockType Demux - Name "Demux6" - SID "4804" - Ports [1, 2] - Position [-10, 3, -5, 92] - ZOrder 695 - ShowName off - Outputs "[3;3]" + BlockType Inport + Name "gradientTwoFeet" + SID "4416" + Position [430, 733, 460, 747] + ZOrder 644 + Port "7" } Block { - BlockType Demux - Name "Demux7" - SID "4805" - Ports [1, 2] - Position [-500, 6, -495, 139] - ZOrder 696 - ShowName off - Outputs "[3;3]" + BlockType Inport + Name "ConstraintsMatrixTwoFeet" + SID "4417" + Position [430, 778, 460, 792] + ZOrder 645 + Port "8" } Block { - BlockType Outport - Name "measured forces l_sole" - SID "4810" - Position [-365, -132, -335, -118] - ZOrder 700 - VectorParamsAs1DForOutWhenUnconnected off + BlockType Inport + Name "bVectorConstraintsTwoFeet" + SID "4418" + Position [430, 823, 460, 837] + ZOrder 646 + Port "9" } Block { - BlockType Outport - Name "des forces l_sole" - SID "4812" - Position [75, -112, 105, -98] - ZOrder 702 - Port "2" - VectorParamsAs1DForOutWhenUnconnected off + BlockType Inport + Name "tauModel" + SID "4395" + Position [1235, 553, 1265, 567] + ZOrder 412 + Port "10" } Block { - BlockType Outport - Name "measured forces r_sole" - SID "4813" - Position [-365, 33, -335, 47] - ZOrder 703 - Port "3" - VectorParamsAs1DForOutWhenUnconnected off + BlockType Inport + Name "Sigma" + SID "4396" + Position [1235, 643, 1265, 657] + ZOrder 413 + Port "11" } Block { - BlockType Outport - Name "des forces r_sole" - SID "4816" - Position [75, 18, 105, 32] - ZOrder 706 - Port "4" - VectorParamsAs1DForOutWhenUnconnected off + BlockType Inport + Name "Na" + SID "4397" + Position [835, 743, 865, 757] + ZOrder 414 + Port "12" } Block { - BlockType Outport - Name "measured moments l_sole" - SID "4811" - Position [-365, -67, -335, -53] - ZOrder 701 - Port "5" - VectorParamsAs1DForOutWhenUnconnected off + BlockType Inport + Name "f_LDot" + SID "4420" + Position [835, 653, 865, 667] + ZOrder 650 + Port "13" } Block { - BlockType Outport - Name "des moments l_sole" - SID "4814" - Position [75, -67, 105, -53] - ZOrder 704 - Port "6" - VectorParamsAs1DForOutWhenUnconnected off + BlockType Sum + Name "Add" + SID "2367" + Ports [2, 1] + Position [1525, 506, 1580, 719] + ZOrder 400 + ShowName off + IconShape "rectangular" } Block { - BlockType Outport - Name "measured moments r_sole" - SID "4815" - Position [-365, 98, -335, 112] - ZOrder 705 - Port "7" - VectorParamsAs1DForOutWhenUnconnected off + BlockType Sum + Name "Add1" + SID "2368" + Ports [2, 1] + Position [1055, 500, 1090, 855] + ZOrder 409 + ShowName off + IconShape "rectangular" } Block { - BlockType Outport - Name "des moments r_sole" - SID "4817" - Position [75, 63, 105, 77] - ZOrder 707 - Port "8" - VectorParamsAs1DForOutWhenUnconnected off - } - Line { - ZOrder 48 - SrcBlock "Demux6" - SrcPort 2 - DstBlock "des moments r_sole" - DstPort 1 - } - Line { - ZOrder 45 - SrcBlock "Demux5" - SrcPort 2 - DstBlock "des moments l_sole" - DstPort 1 - } - Line { - ZOrder 41 - SrcBlock "Demux1" - SrcPort 1 - DstBlock "measured forces l_sole" - DstPort 1 - } - Line { - ZOrder 44 - SrcBlock "Demux7" - SrcPort 1 - DstBlock "measured forces r_sole" - DstPort 1 - } - Line { - ZOrder 46 - SrcBlock "Demux7" - SrcPort 2 - DstBlock "measured moments r_sole" - DstPort 1 - } - Line { - ZOrder 38 - SrcBlock "wrench_leftFoot" - SrcPort 1 - DstBlock "Demux1" - DstPort 1 - } - Line { - ZOrder 26 - SrcBlock "Demux2" - SrcPort 1 - DstBlock "Demux5" - DstPort 1 - } - Line { - ZOrder 39 - SrcBlock "wrench_rightFoot" - SrcPort 1 - DstBlock "Demux7" - DstPort 1 - } - Line { - ZOrder 47 - SrcBlock "Demux6" - SrcPort 1 - DstBlock "des forces r_sole" - DstPort 1 - } - Line { - ZOrder 42 - SrcBlock "Demux1" - SrcPort 2 - DstBlock "measured moments l_sole" - DstPort 1 + BlockType Product + Name "Product1" + SID "2381" + Ports [2, 1] + Position [1420, 634, 1485, 696] + ZOrder 397 + ShowName off + Multiplication "Matrix(*)" } - Line { - ZOrder 40 - SrcBlock "f_star" - SrcPort 1 - DstBlock "Demux2" - DstPort 1 - } - Line { - ZOrder 28 - SrcBlock "Demux2" - SrcPort 2 - DstBlock "Demux6" - DstPort 1 - } - Line { - ZOrder 43 - 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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ngVariableName','forces_l_sole_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','D" - "ataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedD" - "isplays',{struct('MinYLimReal','-176.20127','MaxYLimReal','526.26568','YLabelReal','','MinYLimMag',' 0.00000','" - "MaxYLimMag','526.26568','LegendVisibility','Off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 " - "0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.06666666666666" - "67;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.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','V" - "isible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[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,'Ma" - "rker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',tru" - "e,'Placement',1),struct('MinYLimReal','-193.24915','MaxYLimReal','450.91468','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.07" - "45098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0" - "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215" - "6863],'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','-','L" - "ineWidth',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,'Plac" - "ement',2),struct('MinYLimReal','-264.65056','MaxYLimReal','550.97623','YLabelReal','','MinYLimMag','0','MaxYLim" - "Mag','10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTick" - "Color',[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.07450980" - "39215686;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','Visible','on'),struct('Color',[1 0 0],'" - "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidt" - "h',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'" - ",'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('C" - "olor',[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,'M" - "arker','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','40.00000','MaxYLimReal','60.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" - "sibility','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.623529411" - "764706 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'),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','no" - "ne','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'U" - "serDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimR" - "eal','0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off'," - "'XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745" - "09803922 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.274509" - "803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProper" - "tiesCache',{{struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefi" - "nedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaul" - "ts',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509" - "803922],'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.058823" - "5294117647 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],'LineSty" - "le','-','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',{{}},'NumLin" - "es',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','Disp" - "layLayoutDimensions',[6 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" - "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Ve" - "rsion','2020a','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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ngVariableName','moments_l_sole_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','" - "DataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'Serialized" - "Displays',{struct('MinYLimReal','-18.14675','MaxYLimReal','18.61074','YLabelReal','','MinYLimMag','0.00000','Ma" - "xYLimMag','18.61074','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.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','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','-" - "','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,'" - "Placement',1),struct('MinYLimReal','-55.51989','MaxYLimReal','35.88107','YLabelReal','','MinYLimMag','0','MaxYL" - "imMag','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.0745098" - "039215686 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]" - ",'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','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'" - ",'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement" - "',2),struct('MinYLimReal','-58.09813','MaxYLimReal','48.08789','YLabelReal','','MinYLimMag','0','MaxYLimMag','1" - "0','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.074509803921568" - "6;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','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," - "'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),stru" - "ct('MinYLimReal','0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibi" - "lity','off','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.6235294117647" - "06 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','%'" - ",'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','-','LineWid" - "th',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')}},'UserD" - "efinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4)},'DisplayPropertyDef" - "aults',struct('YLabelReal','','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.16" - "078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058" - "8235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{stru" - "ct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'Line" - "Style','-','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',{{}},'Num" - "Lines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','D" - "isplayLayoutDimensions',[4 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigation',true,'On" - "ceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ngVariableName','forces_r_sole_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','D" - "ataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedD" - "isplays',{struct('MinYLimReal','-36.45598','MaxYLimReal','219.57258','YLabelReal','','MinYLimMag','0.00000','Ma" - "xYLimMag','219.57258','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.83137254901960" - "8 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980" - "392156863],'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','Vis" - "ible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','" - "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Mark" - "er','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true," - "'Placement',1),struct('MinYLimReal','-33.68004','MaxYLimReal','221.73551','YLabelReal','','MinYLimMag','0','Max" - "YLimMag','10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'Axes" - "TickColor',[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.0745" - "098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),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','-','Line" - "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placeme" - "nt',2),struct('MinYLimReal','-113.33241','MaxYLimReal','33.5863','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.07450980392156" - "86 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215" - "686;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','non" - "e','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],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0." - "5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" - "le','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),st" - "ruct('MinYLimReal','90.00000','MaxYLimReal','110.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Leg" - "endVisibility','off','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.6235" - "29411764706 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','Visib" - "le','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',4),struct('Min" - "YLimReal','40.00000','MaxYLimReal','60.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibil" - "ity','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980392" - "2 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.7176470588235" - "29 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','" - "Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDe" - "finedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal'," - "'0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGri" - "d',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803" - "922 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.27450980392" - "1569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesC" - "ache',{{struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedCh" - "annelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',s" - "truct('YLabelReal','','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.1607843137" - "2549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352941" - "17647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Colo" - "r',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','" - "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Mark" - "er','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',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLa" - "youtDimensions',[6 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop" - "',false,'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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ngVariableName','moments_r_sole_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','" - "DataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'Serialized" - "Displays',{struct('MinYLimReal','-89.58974','MaxYLimReal','373.15892','YLabelReal','','MinYLimMag','0.00000','M" - "axYLimMag','373.15892','LegendVisibility','Off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'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.8313725490196" - "08 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','Vi" - "sible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" - "or',[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,'Mar" - "ker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true" - ",'Placement',1),struct('MinYLimReal','-90.08017','MaxYLimReal','373.36109','YLabelReal','','MinYLimMag','0','Ma" - "xYLimMag','10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'Axe" - "sTickColor',[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.074" - "5098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6509803921568" - "63],'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],'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','no" - "ne','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placem" - "ent',2),struct('MinYLimReal','-23.18842','MaxYLimReal','45.48642','YLabelReal','','MinYLimMag','0','MaxYLimMag'" - ",'10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColo" - "r',[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.074509803921" - "5686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Titl" - "e','%','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'),stru" - "ct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'Line" - "Style','-','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),s" - "truct('MinYLimReal','-39.55039','MaxYLimReal','20.45784','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Le" - "gendVisibility','off','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.623" - "529411764706 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','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','-" - "','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',4)},'DisplayP" - "ropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980392" - "2 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.2745098039215" - "69 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','-','Li" - "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','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),'TimeRangeSamples','40','TimeRangeFrame" - "s','40','DisplayLayoutDimensions',[4 1],'DisplayContentCache',{struct('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','-','" - "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'),s" - "truct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" - "s',{{}},'PlotMagPhase',false),struct('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','V" - "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),str" - "uct('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],'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','no" - "ne','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','Lin" - "ePropertiesCache',{{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],'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','Visi" - "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color" - "',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase" - "',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),ext" - "mgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2020a','Location',[401 474 1711 10" - "60])" - NumInputPorts "4" - Floating off - } - Line { - ZOrder 38 - SrcBlock "wrench_leftFoot" - SrcPort 1 - DstBlock "Demux Forces nd Moments" - DstPort 1 - } - Line { - ZOrder 39 - SrcBlock "wrench_rightFoot" - SrcPort 1 - DstBlock "Demux Forces nd Moments" - DstPort 2 - } - Line { - ZOrder 40 - SrcBlock "f_star" - SrcPort 1 - DstBlock "Demux Forces nd Moments" - DstPort 3 - } - Line { - Name "measured forces l_sole" - ZOrder 41 - SrcBlock "Demux Forces nd Moments" - SrcPort 1 - Points [43, 0; 0, -270; 142, 0] - Branch { - ZOrder 96 - Labels [-1, 0] - DstBlock "left Foot forces" - DstPort 1 - } - Branch { - ZOrder 45 - DstBlock "Sum" - DstPort 1 - } - } - Line { - Name "desired forces l_sole" - ZOrder 42 - SrcBlock "Demux Forces nd Moments" - SrcPort 2 - Points [66, 0; 0, -255] - Branch { - ZOrder 82 - DstBlock "Sum" - DstPort 2 - } - Branch { - ZOrder 46 - Labels [-1, 0] - Points [0, -25] - DstBlock "left Foot forces" - DstPort 2 - } - } - Line { - Name "error forces l_sole" - ZOrder 43 - Labels [-1, 0] - SrcBlock "Sum" - SrcPort 1 - DstBlock "left Foot forces" - DstPort 3 - } - Line { - Name "thresholdContactOn" - ZOrder 50 - SrcBlock "Constant" - SrcPort 1 - Points [259, 0] - Branch { - ZOrder 91 - Labels [-1, 0] - Points [0, 185] - DstBlock "right Foot forces" - DstPort 5 - } - Branch { - ZOrder 90 - Labels [-1, 0] - DstBlock "left Foot forces" - DstPort 5 - } - } - Line { - Name "desired forces r_sole" - ZOrder 83 - SrcBlock "Demux Forces nd Moments" - SrcPort 4 - Points [116, 0; 0, -140] - Branch { - ZOrder 86 - DstBlock "Sum1" - DstPort 2 - } - Branch { - ZOrder 84 - Labels [-1, 0] - Points [0, -25] - DstBlock "right Foot forces" - DstPort 2 - } - } - Line { - Name "measured forces r_sole" - ZOrder 81 - SrcBlock "Demux Forces nd Moments" - SrcPort 3 - Points [90, 0; 0, -155; 95, 0] - Branch { - ZOrder 97 - Labels [-1, 0] - DstBlock "right Foot forces" - DstPort 1 - } - Branch { - ZOrder 64 - DstBlock "Sum1" - DstPort 1 - } - } - Line { - Name "error forces r_sole" - ZOrder 74 - Labels [-1, 0] - SrcBlock "Sum1" - SrcPort 1 - DstBlock "right Foot forces" - DstPort 3 - } - Line { - Name "thresholdContactOff" - ZOrder 49 - SrcBlock "Constant2" - SrcPort 1 - Points [280, 0] - Branch { - ZOrder 89 - Labels [-1, 0] - DstBlock "left Foot forces" - DstPort 4 - } - Branch { - ZOrder 88 - Labels [-1, 0] - Points [0, 185] - DstBlock "right Foot forces" - DstPort 4 - } - } - Line { - Name "measured moments r_sole" - ZOrder 127 - SrcBlock "Demux Forces nd Moments" - SrcPort 7 - Points [68, 0; 0, 110; 117, 0] - Branch { - ZOrder 131 - DstBlock "Sum3" - DstPort 1 - } - Branch { - ZOrder 130 - Labels [0, 0] - DstBlock "right Foot moments" - DstPort 1 - } - } - Line { - Name "state" - ZOrder 51 - SrcBlock "state" - SrcPort 1 - Points [321, 0] - Branch { - ZOrder 95 - Points [0, 185] - Branch { - ZOrder 123 - Points [0, 205; 1, 0] - Branch { - ZOrder 126 - Labels [-1, 0] - Points [0, 180] - DstBlock "right Foot moments" - DstPort 4 - } - Branch { - ZOrder 125 - DstBlock "left Foot moments" - DstPort 4 - } - } - Branch { - ZOrder 122 - Labels [-1, 0] - DstBlock "right Foot forces" - DstPort 6 - } - } - Branch { - ZOrder 94 - Labels [-1, 0] - DstBlock "left Foot forces" - DstPort 6 - } - } - Line { - Name "desired moments r_sole" - ZOrder 128 - SrcBlock "Demux Forces nd Moments" - SrcPort 8 - Points [40, 0; 0, 110] - Branch { - ZOrder 134 - Labels [-1, 0] - DstBlock "right Foot moments" - DstPort 2 - } - Branch { - ZOrder 133 - Points [0, 35] - DstBlock "Sum3" - DstPort 2 - } - } - Line { - Name "error moments r_sole" - ZOrder 129 - Labels [-1, 0] - SrcBlock "Sum3" - SrcPort 1 - DstBlock "right Foot moments" - DstPort 3 - } - Line { - Name "error moments l_sole" - ZOrder 116 - Labels [-1, 0] - SrcBlock "Sum2" - SrcPort 1 - DstBlock "left Foot moments" - DstPort 3 - } - Line { - Name "desired moments l_sole" - ZOrder 115 - SrcBlock "Demux Forces nd Moments" - SrcPort 6 - Points [121, 0] - Branch { - ZOrder 120 - Points [0, 35] - DstBlock "Sum2" - DstPort 2 - } - Branch { - ZOrder 119 - Labels [-1, 0] - DstBlock "left Foot moments" - DstPort 2 - } - } - Line { - Name "measured moments l_sole" - ZOrder 114 - SrcBlock "Demux Forces nd Moments" - SrcPort 5 - Points [185, 0] - Branch { - ZOrder 118 - DstBlock "Sum2" - DstPort 1 - } - Branch { - ZOrder 117 - 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 [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 "472" - 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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ngVariableName','feetContactStatus_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1" - "','DataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'Seriali" - "zedDisplays',{struct('MinYLimReal','0.9','MaxYLimReal','1.1','YLabelReal','','MinYLimMag','0.9','MaxYLimMag','1" - ".1','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.074509803921568" - "6;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','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," - "'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),stru" - "ct('MinYLimReal','0.9','MaxYLimReal','1.1','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','%','Lin" - "ePropertiesCache',{{struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'U" - "serDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2)},'DisplayPropert" - "yDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68" - "6274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 " - "0.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','-','LineWidt" - "h',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('C" - "olor',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}}," - "'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[2 1],'DisplayConte" - "ntCache',[]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),e" - "xtmgr.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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ngVariableName','KP_postural_diag_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1'" - ",'DataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'Serializ" - "edDisplays',{struct('MinYLimReal','8.75','MaxYLimReal','21.25','YLabelReal','','MinYLimMag','8.75','MaxYLimMag'" - ",'21.25','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickCo" - "lor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980392" - "15686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039" - "215686;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','" - "none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" - "ruct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'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','Vi" - "sible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1)" - ",struct('MinYLimReal','7.75','MaxYLimReal','10.25','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" - "sibility','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.6235294117" - "64706 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','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','-','Line" - "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Us" - "erDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinYLimRe" - "al','7.75','MaxYLimReal','10.25','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XG" - "rid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098" - "03922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764" - "705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803" - "921569 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','-'" - ",'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')}},'UserDefinedChannelNa" - "mes',{{}},'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'," - "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.160784" - "31372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235" - "294117647 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],'LineStyl" - "e','-','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',{{}},'NumLine" - "s',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-1.00000','MaxYLimReal','1.0000" - "0','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],'Colo" - "rOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3" - "92156862745098 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','-','Line" - "Width',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'),struc" - "t('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,'LineNa" - "mes',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','1.00000','MaxYLimReal','3.00000','YLabelRea" - "l','','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.39215686274509" - "8 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098" - "039215686 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,'M" - "arker','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','-','Li" - "neWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'" - "ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTic" - "kColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980" - "39215686 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]," - "'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'," - "'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" - ",1),'DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),ex" - "tmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2020a','Location',[898 828 2209 1" - "517])" - NumInputPorts "6" - Floating off - } - Line { - ZOrder 1 - SrcBlock "state" - SrcPort 1 - Points [85, 0] - Branch { - ZOrder 31 - DstBlock "Feet Contact Activation" - DstPort 2 - } - Branch { - ZOrder 30 - Points [0, 195] - DstBlock "Gain Scheduling Postural" - DstPort 6 - } - } - Line { - ZOrder 8 - SrcBlock "feetContactStatus" - SrcPort 1 - DstBlock "Feet Contact Activation" - DstPort 1 - } - Line { - ZOrder 9 - SrcBlock "KP_postural_diag" - SrcPort 1 - DstBlock "Demux1" - DstPort 1 - } - Line { - Name "left_leg" - ZOrder 11 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 4 - DstBlock "Gain Scheduling Postural" - DstPort 4 - } - Line { - Name "right_leg" - ZOrder 12 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 5 - DstBlock "Gain Scheduling Postural" - DstPort 5 - } - Line { - Name "right_arm" - ZOrder 13 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 3 - DstBlock "Gain Scheduling Postural" - DstPort 3 - } - Line { - Name "torso" - ZOrder 14 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 1 - DstBlock "Gain Scheduling Postural" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 15 - 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 [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 "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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ngVariableName','QPStatus_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLo" - "ggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispla" - "ys',{struct('MinYLimReal','-2.25','MaxYLimReal','0.25','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','2.2" - "5','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],'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],'LineStyl" - "e','-','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),struc" - "t('MinYLimReal','0.25','MaxYLimReal','7.75','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibilit" - "y','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','%','Li" - "nePropertiesCache',{{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],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth'," - "0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" - "ible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefin" - "edChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2)},'DisplayPropertyDefault" - "s',struct('YLabelReal','','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.160784" - "31372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235" - "294117647 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],'LineStyl" - "e','-','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',{{}},'NumLine" - "s',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[2 1]),extmgr.Configuration" - "('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 "120" - 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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ng',true,'DataLoggingVariableName','base_pose_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDec" - "imation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispl" - "ays',{struct('MinYLimReal','-0.16773','MaxYLimReal','0.72406','YLabelReal','','MinYLimMag','0.00000','MaxYLimMa" - "g','0.72406','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTi" - "ckColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098" - "039215686 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]" - ",'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','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'" - ",'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement" - "',1),struct('MinYLimReal','-1.25','MaxYLimReal','1.25','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Lege" - "ndVisibility','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.623529" - "411764706 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],'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',2),struct('MinYL" - "imReal','0.625','MaxYLimReal','4.375','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.4" - "11764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745" - "09803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProp" - "ertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" - ",3)},'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" - ".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'),stru" - "ct('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','Visi" - "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','400'," - "'TimeRangeFrames','400','DisplayLayoutDimensions',[3 1],'DisplayContentCache',[]),extmgr.Configuration('Tools'," - "'Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements'" - ",true,'Version','2018a')),'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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ng',true,'DataLoggingVariableName','baseVel_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecim" - "ation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplay" - "s',{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.6" - "86274509803922 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." - "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','Vi" - "sible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" - "or',[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,'Mar" - "ker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('" - "MinYLimReal','-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisib" - "ility','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.6235294117647" - "06 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','%'" - ",'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','-','LineWid" - "th',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')}},'UserD" - "efinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',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.6862745" - "09803922 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.274509" - "803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','LinePropertiesCache',{{s" - "truct('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','V" - "isible','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',{{}},'" - "NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3)},'DisplayPropertyDefaults',struct('MinYLimReal" - "','-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','AxesColor',[0 0 0],'Ax" - "esTickColor',[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.07" - "45098039215686;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,'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','-','Li" - "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Place" - "ment',1),'DisplayLayoutDimensions',[3 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',fals" - "e),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2020a','Location',[135 478 2" - "065 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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ng',true,'DataLoggingVariableName','pos_CoM_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecim" - "ation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplay" - "s',{struct('MinYLimReal','-0.15575','MaxYLimReal','0.70056','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag'" - ",'0.70056','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTick" - "Color',[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.07450980" - "39215686;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','Visible','on'),struct('Color',[1 0 0],'" - "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidt" - "h',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','-0.14783','MaxYLimReal','0.63035','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" - "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68" - "6274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62" - "3529411764706 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','Vis" - "ible','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,'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',2),struct('M" - "inYLimReal','-0.08636','MaxYLimReal','0.08883','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibi" - "lity','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980392" - "2 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.7176470588235" - "29 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','" - "Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDe" - "finedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal'," - "'0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid" - "',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098039" - "22 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705" - "882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921" - "569 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','-','L" - "ineWidth',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)},'DisplayPropertyDefaults',struct('YLa" - "belReal','','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.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]" - ",'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'" - ",'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],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineName" - "s',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','400','TimeRangeFrames','400','DisplayLayoutDime" - "nsions',[4 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY')" - ",extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2020a','Location',[135 239 265" - "0 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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ng',true,'DataLoggingVariableName','tauDesired_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDe" - "cimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisp" - "lays',{struct('MinYLimReal','-19.59759','MaxYLimReal','27.79759','YLabelReal','','MinYLimMag','0.00000','MaxYLi" - "mMag','27.79759','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'Ax" - "esTickColor',[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.07" - "45098039215686;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,'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','-','Li" - "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Place" - "ment',1),struct('MinYLimReal','-7.66225','MaxYLimReal','8.03648','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.074509803921568" - "6 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392156" - "86;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','Visible','on'),struct('Color',[1 0 0],'LineSt" - "yle','-','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',2),str" - "uct('MinYLimReal','-11.21869','MaxYLimReal','8.29753','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legen" - "dVisibility','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.6235294" - "11764706 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],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','L" - "ineWidth',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('MinYLi" - "mReal','-51.65238','MaxYLimReal','50.72666','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibilit" - "y','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','%','Li" - "nePropertiesCache',{{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],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth'," - "0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" - "ible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefin" - "edChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-5" - ".64312','MaxYLimReal','14.99269','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XG" - "rid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098" - "03922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764" - "705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803" - "921569 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','-'" - ",'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')}},'UserDefinedChannelNa" - "mes',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-0.5','MaxYLi" - "mReal','14.5','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',t" - "rue,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980" - "3922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843" - "1372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352" - "94117647 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'," - "'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],'LineStyl" - "e','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames" - "',{{[]}},'ShowContent',true,'Placement',6)},'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.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','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','-" - "','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,'" - "Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configurat" - "ion('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','M" - "easurements',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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ng',true,'DataLoggingVariableName','tau_star_afterSat_DATA','DataLoggingSaveFormat','StructureWithTime','DataLo" - "ggingDecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'Seriali" - "zedDisplays',{struct('MinYLimReal','-14.69019','MaxYLimReal','22.24602','YLabelReal','','MinYLimMag','0.00000'," - "'MaxYLimMag','22.24602','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'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.8313725490196" - "08 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','Vi" - "sible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" - "or',[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,'Mar" - "ker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true" - ",'Placement',1),struct('MinYLimReal','-10.12731','MaxYLimReal','6.52875','YLabelReal','','MinYLimMag','0','MaxY" - "LimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTi" - "ckColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098" - "039215686 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]" - ",'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','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'" - ",'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement" - "',2),struct('MinYLimReal','-14.66185','MaxYLimReal','8.30827','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],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" - "olor',[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,'M" - "arker','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','-23.11356','MaxYLimReal','33.28118','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendV" - "isibility','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.623529411" - "764706 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'),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','no" - "ne','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'U" - "serDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimR" - "eal','-30.29324','MaxYLimReal','27.04418','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility'" - ",'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." - "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'),struc" - "t('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','Visib" - "le','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefined" - "ChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','0.25" - "','MaxYLimReal','7.75','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true," - "'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68" - "6274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 " - "0.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','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',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColo" - "r',[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.831372" - "549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 " - "0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','Lin" - "eWidth',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'),stru" - "ct('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',{{}},'NumLines',0,'LineNames',{{[]}},'ShowConten" - "t',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.C" - "onfiguration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('" - "Tools','Measurements',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 Ground - Name "Ground" - SID "4971" - Position [70, 110, 90, 130] - ZOrder 897 - } - 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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ng',true,'DataLoggingVariableName','jointPos_Desired_DATA','DataLoggingSaveFormat','StructureWithTime','DataLog" - "gingDecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'Serializ" - "edDisplays',{struct('MinYLimReal','-8.36237','MaxYLimReal','29.68131','YLabelReal','','MinYLimMag','0.00000','M" - "axYLimMag','29.68131','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.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','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','-" - "','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,'" - "Placement',1),struct('MinYLimReal','-81.97965','MaxYLimReal','72.42484','YLabelReal','','MinYLimMag','0','MaxYL" - "imMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTic" - "kColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980" - "39215686 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]," - "'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'," - "'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" - ",2),struct('MinYLimReal','-78.54318','MaxYLimReal','93.23402','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],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" - "olor',[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,'M" - "arker','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','-12.71731','MaxYLimReal','14.72789','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendV" - "isibility','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.623529411" - "764706 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'),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','no" - "ne','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'U" - "serDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimR" - "eal','-113.68313','MaxYLimReal','94.48842','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','%','Lin" - "ePropertiesCache',{{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],'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','Visi" - "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','0.6" - "25','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',tr" - "ue,'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.4117647058823" - "53 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " - "1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','LinePropertiesCache',{{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,'Mar" - "ker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 " - "1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'," - "0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesC" - "olor',[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.831" - "372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07450980392156" - "86 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'),s" - "truct('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',{{[]}},'ShowCon" - "tent',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmg" - "r.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuratio" - "n('Tools','Measurements',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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ng',true,'DataLoggingVariableName','jointPos_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDeci" - "mation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispla" - "ys',{struct('MinYLimReal','-2.41385','MaxYLimReal','5.58471','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag" - "','5.58471','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTic" - "kColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980" - "39215686 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]," - "'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'," - "'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" - ",1),struct('MinYLimReal','-39.61854','MaxYLimReal','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','%<" - "SignalLabel>','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[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,'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',2),struct(" - "'MinYLimReal','-39.58369','MaxYLimReal','58.65801','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" - "sibility','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.6235294117" - "64706 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','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','-','Line" - "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Us" - "erDefinedChannelNames',{{}},'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.686" - "274509803922 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','%','LinePr" - "opertiesCache',{{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],'LineSty" - "le','-','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')}},'UserDefinedCh" - "annelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-11.45" - "092','MaxYLimReal','7.90757','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid'" - ",true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980392" - "2 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" - "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039215" - "69 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','-','Li" - "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','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',5),struct('MinYLimReal','0.75','MaxYLimRea" - "l','3.25','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.16078431372" - "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411" - "7647 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,'Marke" - "r','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',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesCol" - "or',[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.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','-','Li" - "neWidth',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'),str" - "uct('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',{{[]}},'ShowConte" - "nt',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr." - "Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration(" - "'Tools','Measurements',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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ng',true,'DataLoggingVariableName','jointPos_Error_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggi" - "ngDecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'Serialized" - "Displays',{struct('MinYLimReal','-10.85715','MaxYLimReal','13.2441','YLabelReal','','MinYLimMag','0.00000','Max" - "YLimMag','13.2441','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.0" - "745098039215686 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.6509803921" - "56863],'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','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Pla" - "cement',1),struct('MinYLimReal','-24.65677','MaxYLimReal','13.37484','YLabelReal','','MinYLimMag','0','MaxYLimM" - "ag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickCo" - "lor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980392" - "15686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039" - "215686;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','" - "none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" - "ruct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'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','Vi" - "sible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2)" - ",struct('MinYLimReal','-37.11648','MaxYLimReal','33.25109','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" - "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68" - "6274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62" - "3529411764706 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','Vis" - "ible','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,'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('M" - "inYLimReal','-28.71162','MaxYLimReal','40.05399','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisi" - "bility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803" - "922 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.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'" - "),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'User" - "DefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal" - "','-16.66332','MaxYLimReal','13.39274','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','o" - "n','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." - "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','%','LinePro" - "pertiesCache',{{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],'LineStyl" - "e','-','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')}},'UserDefinedCha" - "nnelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-0.375'" - ",'MaxYLimReal','13.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true" - ",'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.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,'Marke" - "r','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',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesCol" - "or',[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.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','-','Li" - "neWidth',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'),str" - "uct('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',{{[]}},'ShowConte" - "nt',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr." - "Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration(" - "'Tools','Measurements',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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ng',true,'DataLoggingVariableName','jointVel_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDeci" - "mation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispla" - "ys',{struct('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','1" - "0','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],'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],'LineStyl" - "e','-','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),struc" - "t('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legend" - "Visibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450" - "9803922 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.7176470" - "58823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " - "1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','Li" - "neWidth',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')}},'" - "UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinYLim" - "Real','-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.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Lin" - "ePropertiesCache',{{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],'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','Visi" - "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-20" - ".50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XG" - "rid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098" - "03922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764" - "705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803" - "921569 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','-'" - ",'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')}},'UserDefinedChannelNa" - "mes',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-20.50153','M" - "axYLimReal','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.68" - "6274509803922],'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','-','LineWidt" - "h',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('C" - "olor',[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','LegendVisibility','on','XGrid',true,'YGrid',tr" - "ue,'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.16078431" - "372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529" - "4117647 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,'Ma" - "rker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0" - " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'" - ",0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('MinYLimReal','-20.50" - "153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','AxesColor',[0 0 0],'AxesTickC" - "olor',[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.074509803" - "9215686;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'," - "'none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" - "truct('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','V" - "isible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1" - "),'TimeRangeSamples','60','TimeRangeFrames','60','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools'," - "'Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2018a'))" - ",'Version','2020a','Location',[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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ng',true,'DataLoggingVariableName','tauMeasured_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingD" - "ecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDis" - "plays',{struct('MinYLimReal','-13.53306','MaxYLimReal','13.58663','YLabelReal','','MinYLimMag','0.00000','MaxYL" - "imMag','13.58663','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.07" - "45098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0" - "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215" - "6863],'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','-','L" - "ineWidth',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,'Plac" - "ement',1),struct('MinYLimReal','-4.19072','MaxYLimReal','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.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980392156" - "86 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215" - "686;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','non" - "e','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],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0." - "5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" - "le','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),st" - "ruct('MinYLimReal','-4.21765','MaxYLimReal','4.8762','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legend" - "Visibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450" - "9803922 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.7176470" - "58823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " - "1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','Li" - "neWidth',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')}},'" - "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." - "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'),stru" - "ct('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','Visi" - "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-44" - ".89002','MaxYLimReal','34.01019','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XG" - "rid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098" - "03922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764" - "705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803" - "921569 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','-'" - ",'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')}},'UserDefinedChannelNa" - "mes',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-0.5','MaxYLi" - "mReal','14.5','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',t" - "rue,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980" - "3922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843" - "1372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352" - "94117647 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'," - "'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],'LineStyl" - "e','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames" - "',{{[]}},'ShowContent',true,'Placement',6)},'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.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','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','-" - "','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,'" - "Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configurat" - "ion('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','M" - "easurements',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(ext" - "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" - "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" - "ng',true,'DataLoggingVariableName','tauError_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDeci" - "mation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispla" - "ys',{struct('MinYLimReal','-23.63249','MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimM" - "ag','27.95301','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'Axes" - "TickColor',[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.0745" - "098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),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','-','Line" - "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placeme" - "nt',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabelReal','','MinYLimMag','0','MaxYLimMag','1" - "0','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],'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],'LineStyl" - "e','-','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),struc" - "t('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" - "sibility','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.6235294117" - "64706 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','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','-','Line" - "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Us" - "erDefinedChannelNames',{{}},'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.68" - "6274509803922 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.2" - "74509803921569 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" - "('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" - ",'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')}},'UserDefinedC" - "hannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-66.4" - "4849','MaxYLimReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGri" - "d',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803" - "922 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.27450980392" - "1569 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','-','" - "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'),s" - "truct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" - "s',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','0.25','MaxYLimR" - "eal','7.75','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',tru" - "e,'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.160784313" - "72549;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],'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','V" - "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames'," - "{{[]}},'ShowContent',true,'Placement',6)},'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.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392" - "156863],'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','-'," - "'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,'Pl" - "acement',1),'TimeRangeSamples','90','TimeRangeFrames','90','DisplayLayoutDimensions',[6 1]),extmgr.Configuratio" - "n('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Mea" - "surements',true,'Version','2018a')),'Version','2020a','Location',[135 169 1990 1170])" - NumInputPorts "6" - Floating off - } - Line { - Name "CoM_Measured" - ZOrder 391 - SrcBlock "pos_CoM" - SrcPort 1 - Points [120, 0] - Branch { - ZOrder 835 - DstBlock "Sum1" - DstPort 1 - } - Branch { - ZOrder 834 - Labels [-1, 0] - DstBlock "CoM Position" - DstPort 1 - } - } - Line { - ZOrder 702 - SrcBlock "Gain" - SrcPort 1 - DstBlock "Demux1" - DstPort 1 - } - Line { - ZOrder 839 - SrcBlock "jointPos" - SrcPort 1 - Points [80, 0] - Branch { - ZOrder 840 - DstBlock "Gain" - DstPort 1 - } - Branch { - ZOrder 7 - DstBlock "Sum3" - DstPort 1 - } - } - Line { - Name "right_leg" - ZOrder 700 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 5 - DstBlock "Joint Position Measured" - DstPort 5 - } - Line { - Name "torso" - ZOrder 698 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 1 - DstBlock "Joint Position Measured" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 697 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 2 - DstBlock "Joint Position Measured" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 701 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 4 - DstBlock "Joint Position Measured" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 699 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 3 - DstBlock "Joint Position Measured" - DstPort 3 - } - Line { - ZOrder 935 - SrcBlock "Ground" - SrcPort 1 - Points [125, 0] - Branch { - ZOrder 831 - DstBlock "Sum" - DstPort 2 - } - Branch { - ZOrder 686 - DstBlock "Demux9" - DstPort 1 - } - } - Line { - Name "right_leg" - ZOrder 666 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 5 - DstBlock "Desired Torques" - DstPort 5 - } - Line { - Name "torso" - ZOrder 668 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 1 - DstBlock "Desired Torques" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 667 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 2 - DstBlock "Desired Torques" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 665 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 4 - DstBlock "Desired Torques" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 669 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 3 - DstBlock "Desired Torques" - DstPort 3 - } - Line { - Name "right_leg" - ZOrder 656 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 5 - DstBlock "Torques Error" - DstPort 5 - } - Line { - Name "torso" - ZOrder 659 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 1 - DstBlock "Torques Error" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 658 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 2 - DstBlock "Torques Error" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 657 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 4 - DstBlock "Torques Error" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 655 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 3 - DstBlock "Torques Error" - DstPort 3 - } - Line { - ZOrder 685 - SrcBlock "Sum" - SrcPort 1 - DstBlock "Demux10" - DstPort 1 - } - Line { - Name "right_leg" - ZOrder 660 - Labels [-1, 0] - SrcBlock "Demux9" - SrcPort 5 - DstBlock "Measured Torques" - DstPort 5 - } - Line { - Name "torso" - ZOrder 661 - Labels [-1, 0] - SrcBlock "Demux9" - SrcPort 1 - DstBlock "Measured Torques" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 662 - Labels [-1, 0] - SrcBlock "Demux9" - SrcPort 2 - DstBlock "Measured Torques" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 663 - Labels [-1, 0] - SrcBlock "Demux9" - SrcPort 4 - DstBlock "Measured Torques" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 664 - Labels [-1, 0] - SrcBlock "Demux9" - SrcPort 3 - DstBlock "Measured Torques" - DstPort 3 - } - Line { - ZOrder 390 - SrcBlock "jointPos_des" - SrcPort 1 - Points [32, 0] - Branch { - ZOrder 52 - Points [0, 160] - DstBlock "Sum3" - DstPort 2 - } - Branch { - ZOrder 53 - DstBlock "Gain3" - DstPort 1 - } - } - Line { - ZOrder 703 - SrcBlock "Gain3" - SrcPort 1 - DstBlock "Demux2" - DstPort 1 - } - Line { - Name "right_leg" - ZOrder 694 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 5 - DstBlock "Joint Position Desired" - DstPort 5 - } - Line { - Name "torso" - ZOrder 693 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 1 - DstBlock "Joint Position Desired" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 695 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 2 - DstBlock "Joint Position Desired" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 692 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 4 - DstBlock "Joint Position Desired" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 696 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 3 - DstBlock "Joint Position Desired" - DstPort 3 - } - Line { - ZOrder 704 - SrcBlock "Gain4" - SrcPort 1 - DstBlock "Demux5" - DstPort 1 - } - Line { - Name "right_leg" - ZOrder 690 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 5 - DstBlock "Joint Positon Error" - DstPort 5 - } - Line { - Name "torso" - ZOrder 688 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 1 - DstBlock "Joint Positon Error" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 689 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 2 - DstBlock "Joint Positon Error" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 691 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 4 - DstBlock "Joint Positon Error" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 687 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 3 - DstBlock "Joint Positon Error" - DstPort 3 - } - Line { - ZOrder 70 - SrcBlock "Sum3" - SrcPort 1 - DstBlock "Gain4" - DstPort 1 - } - Line { - Name "CoM_Desired" - ZOrder 832 - SrcBlock "pos_CoM_des" - SrcPort 1 - Points [67, 0] - Branch { - ZOrder 837 - Points [0, 35] - DstBlock "Sum1" - DstPort 2 - } - Branch { - ZOrder 836 - Labels [-1, 0] - DstBlock "CoM Position" - DstPort 2 - } - } - Line { - ZOrder 388 - SrcBlock "w_H_b" - SrcPort 1 - Points [77, 0] - Branch { - ZOrder 869 - Points [0, -40] - DstBlock "CoM6D -> \nCoMXYZ2" - DstPort 1 - } - Branch { - ZOrder 868 - DstBlock "CoM6D -> \nCoMXYZ1" - DstPort 1 - } - } - Line { - Name "basePosition" - ZOrder 103 - Labels [-1, 0] - SrcBlock "CoM6D -> \nCoMXYZ2" - SrcPort 1 - DstBlock "Base Pose" - DstPort 1 - } - Line { - Name "baseOrientation" - ZOrder 104 - Labels [-1, 0] - SrcBlock "CoM6D -> \nCoMXYZ1" - SrcPort 1 - DstBlock "Base Pose" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 670 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 4 - DstBlock "Desired Torques After Saturation" - DstPort 4 - } - Line { - Name "right_leg" - ZOrder 673 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 5 - DstBlock "Desired Torques After Saturation" - DstPort 5 - } - Line { - Name "left_arm" - ZOrder 671 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 2 - DstBlock "Desired Torques After Saturation" - DstPort 2 - } - Line { - Name "torso" - ZOrder 674 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 1 - DstBlock "Desired Torques After Saturation" - DstPort 1 - } - Line { - Name "right_arm" - ZOrder 672 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 3 - DstBlock "Desired Torques After Saturation" - DstPort 3 - } - Line { - ZOrder 581 - SrcBlock "tau_star_afterSat" - SrcPort 1 - DstBlock "Demux7" - DstPort 1 - } - Line { - ZOrder 387 - SrcBlock "tau_star" - SrcPort 1 - Points [41, 0] - Branch { - ZOrder 821 - Points [0, 150] - DstBlock "Sum" - DstPort 1 - } - Branch { - ZOrder 684 - DstBlock "Demux8" - DstPort 1 - } - } - Line { - ZOrder 806 - SrcBlock "nu" - SrcPort 1 - Points [26, 0] - Branch { - ZOrder 852 - DstBlock "Selector1" - DstPort 1 - } - Branch { - ZOrder 805 - Points [0, 160] - DstBlock "Selector2" - DstPort 1 - } - } - Line { - ZOrder 807 - SrcBlock "Selector2" - SrcPort 1 - DstBlock "Demux" - DstPort 1 - } - Line { - ZOrder 808 - SrcBlock "Gain1" - SrcPort 1 - DstBlock "Demux3" - DstPort 1 - } - Line { - Name "base linear velocity" - ZOrder 809 - Labels [-1, 0] - SrcBlock "Demux" - SrcPort 1 - DstBlock "Base linear and angular velocity" - DstPort 1 - } - Line { - Name "base angular velocity" - ZOrder 810 - Labels [-1, 0] - SrcBlock "Gain2" - SrcPort 1 - DstBlock "Base linear and angular velocity" - DstPort 2 - } - Line { - Name "right_arm" - ZOrder 811 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 3 - DstBlock "Joint Velocity" - DstPort 3 - } - Line { - Name "right_leg" - ZOrder 812 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 5 - DstBlock "Joint Velocity" - DstPort 5 - } - Line { - ZOrder 813 - SrcBlock "Selector1" - SrcPort 1 - DstBlock "Gain1" - DstPort 1 - } - Line { - Name "torso" - ZOrder 817 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 1 - DstBlock "Joint Velocity" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 818 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 2 - DstBlock "Joint Velocity" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 819 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 4 - DstBlock "Joint Velocity" - DstPort 4 - } - Line { - ZOrder 820 - SrcBlock "Demux" - SrcPort 2 - DstBlock "Gain2" - DstPort 1 - } - Line { - Name "CoM_Error" - ZOrder 838 - Labels [-1, 0] - SrcBlock "Sum1" - SrcPort 1 - DstBlock "CoM Position" - DstPort 3 - } - Line { - Name "state" - ZOrder 389 - SrcBlock "state" - SrcPort 1 - Points [234, 0] - Branch { - ZOrder 827 - Points [0, 145] - Branch { - ZOrder 825 - Points [0, 150] - Branch { - ZOrder 829 - Points [0, 155] - Branch { - ZOrder 828 - Points [0, 185] - Branch { - ZOrder 844 - Points [0, 55; 742, 0] - Branch { - ZOrder 862 - Labels [-1, 0] - DstBlock "Base linear and angular velocity" - DstPort 3 - } - Branch { - ZOrder 861 - Points [0, -160] - Branch { - ZOrder 859 - Labels [-1, 0] - DstBlock "Joint Velocity" - DstPort 6 - } - Branch { - ZOrder 853 - Points [0, -190] - Branch { - ZOrder 858 - Labels [-1, 0] - DstBlock "Joint Positon Error" - DstPort 6 - } - Branch { - ZOrder 848 - Points [0, -160] - Branch { - ZOrder 857 - Labels [-1, 0] - DstBlock "Joint Position Desired" - DstPort 6 - } - Branch { - ZOrder 850 - Points [0, -150; 1, 0] - Branch { - ZOrder 866 - Labels [-1, 0] - Points [0, -170] - DstBlock "Base Pose" - DstPort 3 - } - Branch { - ZOrder 865 - DstBlock "Joint Position Measured" - DstPort 6 - } - } - } - } - } - } - Branch { - ZOrder 843 - Labels [-1, 0] - DstBlock "CoM Position" - DstPort 4 - } - } - Branch { - ZOrder 89 - Labels [-1, 0] - DstBlock "Measured Torques" - DstPort 6 - } - } - Branch { - ZOrder 90 - Labels [-1, 0] - DstBlock "Torques Error" - DstPort 6 - } - } - Branch { - ZOrder 75 - Labels [-1, 0] - DstBlock "Desired Torques" - DstPort 6 - } - } - Branch { - ZOrder 823 - Labels [-1, 0] - DstBlock "Desired Torques After Saturation" - DstPort 6 - } - } - } - } - Line { - ZOrder 423 - SrcBlock "ON_GAZEBO\n4" - SrcPort 1 - Points [100, 0] - DstBlock "Visualizer" - DstPort enable - } - Line { - ZOrder 390 - SrcBlock "From1" - SrcPort 1 - Points [52, 0] - Branch { - ZOrder 450 - DstBlock "Visualize eventual QP failures" - DstPort 2 - } - Branch { - ZOrder 448 - Points [0, 40] - Branch { - ZOrder 474 - Points [0, 135] - DstBlock "Desired and Measured Forces" - DstPort 4 - } - Branch { - ZOrder 455 - DstBlock "Visualizer" - DstPort 6 - } - } - Branch { - ZOrder 449 - Points [0, -130] - DstBlock "Feet Status and Gains" - DstPort 3 - } - } - Line { - ZOrder 395 - SrcBlock "From7" - SrcPort 1 - DstBlock "Visualizer" - DstPort 2 - } - Line { - ZOrder 424 - SrcBlock "From5" - SrcPort 1 - DstBlock "Visualizer" - DstPort 1 - } - Line { - ZOrder 463 - SrcBlock "From12" - SrcPort 1 - DstBlock "Visualizer" - DstPort 5 - } - Line { - ZOrder 415 - SrcBlock "ON_GAZEBO\n2" - SrcPort 1 - Points [165, 0] - DstBlock "Visualize eventual QP failures" - DstPort enable - } - Line { - ZOrder 416 - SrcBlock "From9" - SrcPort 1 - DstBlock "Visualize eventual QP failures" - DstPort 1 - } - Line { - ZOrder 451 - SrcBlock "ON_GAZEBO\n1" - SrcPort 1 - Points [160, 0] - DstBlock "Desired and Measured Forces" - DstPort enable - } - Line { - ZOrder 445 - SrcBlock "ON_GAZEBO\n6" - SrcPort 1 - Points [135, 0] - DstBlock "Feet Status and Gains" - DstPort enable - } - Line { - ZOrder 446 - SrcBlock "From4" - SrcPort 1 - DstBlock "Feet Status and Gains" - DstPort 1 - } - Line { - ZOrder 447 - SrcBlock "From8" - SrcPort 1 - DstBlock "Feet Status and Gains" - DstPort 2 - } - Line { - ZOrder 452 - SrcBlock "From2" - SrcPort 1 - DstBlock "Desired and Measured Forces" - DstPort 3 - } - Line { - ZOrder 453 - SrcBlock "From11" - SrcPort 1 - DstBlock "Desired and Measured Forces" - DstPort 1 - } - Line { - ZOrder 454 - SrcBlock "From10" - SrcPort 1 - DstBlock "Desired and Measured Forces" - DstPort 2 - } - Line { - ZOrder 462 - SrcBlock "From6" - SrcPort 1 - DstBlock "Visualizer" - DstPort 3 - } - Line { - ZOrder 464 - SrcBlock "From3" - SrcPort 1 - DstBlock "Visualizer" - DstPort 4 - } - Line { - ZOrder 465 - SrcBlock "From" - SrcPort 1 - DstBlock "Visualizer" - DstPort 7 - } - Line { - ZOrder 466 - SrcBlock "From13" - SrcPort 1 - DstBlock "Visualizer" - DstPort 8 - } - Line { - ZOrder 475 - SrcBlock "From14" - SrcPort 1 - DstBlock "Visualizer" - DstPort 9 - } - } - } - Block { - BlockType SubSystem - Name "MOMENTUM BASED TORQUE CONTROL" - SID "4836" - Ports [7, 1] - Position [955, 391, 1105, 609] - ZOrder 961 - 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 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 "174" - SimulinkSubDomain "Simulink" - Block { - BlockType Inport - Name "neckPos" - SID "4977" - Position [160, 353, 190, 367] - ZOrder 971 - SampleTime "Config.tStep" - } - Block { - BlockType Inport - Name "jointPos" - SID "4837" - Position [160, 258, 190, 272] - ZOrder 556 - Port "2" - SampleTime "Config.tStep" - } - Block { - BlockType Inport - Name "jointVel" - SID "4838" - Position [160, 453, 190, 467] - ZOrder 557 - Port "3" - SampleTime "Config.tStep" - } - Block { - BlockType Inport - Name "jointAcc" - SID "4966" - Position [160, 148, 190, 162] - ZOrder 969 - Port "4" - SampleTime "Config.tStep" - } - Block { - BlockType Inport - Name "IMU_meas" - SID "4840" - Position [160, 503, 190, 517] - ZOrder 559 - Port "5" - SampleTime "Config.tStep" - } - Block { - BlockType Inport - Name "wrench_rightFoot" - SID "4880" - Position [160, 553, 190, 567] - ZOrder 963 - Port "6" - SampleTime "Config.tStep" - } - Block { - BlockType Inport - Name "wrench_leftFoot" - SID "4881" - Position [160, 603, 190, 617] - ZOrder 964 - Port "7" - SampleTime "Config.tStep" - } - Block { - BlockType SubSystem - Name "Balancing Controller QP" - SID "2355" - Ports [21, 1] - Position [800, 3, 955, 607] - ZOrder 542 - BackgroundColor "lightBlue" - RequestExecContextInheritance off - System { - Name "Balancing Controller QP" - 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 "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 - RequestExecContextInheritance off - System { - Name "Compute Desired Torques" - 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 "284" - 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 Product + Name "Product2" + SID "2382" + Ports [2, 1] + Position [915, 734, 975, 796] + ZOrder 410 + ShowName off + Multiplication "Matrix(*)" } Block { BlockType SubSystem @@ -7027,120 +2992,990 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "343" + ZoomFactor "343" + 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 + NameLocation "top" + TreatAsAtomicUnit on + RequestExecContextInheritance off + System { + Name "QP One Foot" + 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 "341" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "H" + SID "4685" + Position [970, -237, 1000, -223] + ZOrder 750 + } + Block { + BlockType Inport + Name "g" + SID "4686" + Position [970, -207, 1000, -193] + ZOrder 752 + Port "2" + } + Block { + BlockType Inport + Name "A" + SID "4687" + Position [970, -177, 1000, -163] + ZOrder 753 + Port "3" + } + Block { + BlockType Inport + Name "ubA" + SID "4688" + Position [970, -147, 1000, -133] + 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 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 [4, 2] + Position [1145, -242, 1250, -128] + ZOrder 751 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Utilities/QP" + SourceType "QP" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + lbA off + 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, -150] + DstBlock "Analytical Solution QP One Foot (unconstrained)" + DstPort 1 + } + Branch { + ZOrder 6 + DstBlock "QP One Foot" + DstPort 1 + } + } + Line { + ZOrder 7 + SrcBlock "ubA" + SrcPort 1 + DstBlock "QP One Foot" + DstPort 4 + } + Line { + ZOrder 8 + SrcBlock "g" + SrcPort 1 + Points [51, 0] + Branch { + ZOrder 33 + Points [0, -55] + 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 [34, 0] + Branch { + ZOrder 41 + Points [0, 50] + DstBlock "Process QP output" + DstPort 3 + } + Branch { + ZOrder 26 + 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 + } + } + } + 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 [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 "426" SimulinkSubDomain "Simulink" Block { BlockType Inport - Name "feetContactStatus" - SID "4580" - Position [-480, -106, -450, -94] - ZOrder 394 + Name "H" + SID "4673" + Position [1460, 1023, 1490, 1037] + ZOrder 751 } Block { BlockType Inport - Name "HessianMatrixOneFoot" - SID "4581" - Position [-230, -322, -200, -308] - ZOrder 395 + Name "g" + SID "4674" + Position [1460, 1053, 1490, 1067] + ZOrder 753 Port "2" } Block { BlockType Inport - Name "gradientOneFoot" - SID "4582" - Position [-230, -292, -200, -278] - ZOrder 396 + Name "A" + SID "4675" + Position [1460, 1083, 1490, 1097] + ZOrder 754 Port "3" } Block { BlockType Inport - Name "ConstraintsMatrixOneFoot" - SID "4583" - Position [-230, -262, -200, -248] - ZOrder 403 + Name "ubA" + SID "4676" + Position [1460, 1113, 1490, 1127] + 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 "bVectorConstraintsOneFoot" - SID "4584" - Position [-230, -232, -200, -218] - ZOrder 404 - Port "5" + Name "H" + SID "4678::1" + Position [20, 101, 40, 119] + ZOrder -1 } Block { BlockType Inport - Name "HessianMatrixTwoFeet" - SID "4585" - Position [-230, 8, -200, 22] - ZOrder 397 - Port "6" + 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 } - Block { - BlockType Inport - Name "gradientTwoFeet" - SID "4586" - Position [-230, 48, -200, 62] - ZOrder 398 - Port "7" + Line { + ZOrder 90 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 } - 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 + BlockType Reference + Name "MatchSignalSizes" + SID "4679" + Ports [2, 1] + Position [1735, 984, 1825, 1066] + ZOrder 757 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" + 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 "One Foot Two Feet QP Selector" - SID "4590" - Ports [1, 1] - Position [-345, -127, -85, -73] - ZOrder 721 + 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 "One Foot Two Feet QP Selector" + Name "Process QP output" Location [223, 338, 826, 833] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off @@ -7158,333 +3993,738 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "108" + SIDHighWatermark "117" SimulinkSubDomain "Simulink" Block { BlockType Inport - Name "feetContactStatus" - SID "4590::1" + 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 "4590::107" + SID "4680::116" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 98 + ZOrder 100 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 + SID "4680::115" + Tag "Stateflow S-Function 25" + Ports [3, 2] + Position [180, 100, 230, 180] + ZOrder 99 FunctionName "sf_sfun" - PortCounts "[1 2]" + Parameters "Config" + PortCounts "[3 2]" SFunctionDeploymentMode off EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 - Name "onOneFoot" + Name "f_star" } } Block { BlockType Terminator Name " Terminator " - SID "4590::108" + SID "4680::117" Position [460, 241, 480, 259] - ZOrder 99 + ZOrder 101 } Block { BlockType Outport - Name "onOneFoot" - SID "4590::5" + Name "f_star" + SID "4680::5" Position [460, 101, 480, 119] ZOrder -5 VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 109 - SrcBlock "feetContactStatus" + 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 [4, 2] + Position [1575, 1016, 1680, 1134] + ZOrder 752 + LibraryVersion "1.684" + SourceBlock "WBToolboxLibrary/Utilities/QP" + SourceType "QP" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + lbA off + 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 [28, 0] + Branch { + ZOrder 62 + Points [0, 55] + DstBlock "Process QP output" + DstPort 3 + } + Branch { + ZOrder 61 + DstBlock "QPStatus" + DstPort 1 + } + } + 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 - DstBlock " SFunction " - DstPort 1 + DstBlock "MatchSignalSizes" + DstPort 2 } Line { - Name "onOneFoot" - ZOrder 110 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "onOneFoot" + ZOrder 45 + SrcBlock "H" + SrcPort 1 + Points [22, 0] + Branch { + ZOrder 79 + DstBlock "QP Two Feet" DstPort 1 } - Line { - ZOrder 111 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " + Branch { + ZOrder 43 + Points [0, -160] + DstBlock "Analytical Solution Two Feet (unconstrained)" DstPort 1 } + } Line { - ZOrder 112 - SrcBlock " SFunction " + ZOrder 50 + SrcBlock "g" SrcPort 1 - DstBlock " Demux " - DstPort 1 + Points [47, 0] + Branch { + ZOrder 49 + Points [0, -55] + Branch { + ZOrder 48 + Points [0, -100] + DstBlock "Analytical Solution Two Feet (unconstrained)" + DstPort 2 } + Branch { + ZOrder 47 + DstBlock "MatchSignalSizes" + DstPort 1 } } - Block { - BlockType SubSystem - Name "QP One Foot" - SID "4696" - Ports [5, 2, 1] - Position [25, -332, 120, -178] - ZOrder 736 - NameLocation "top" - TreatAsAtomicUnit on - RequestExecContextInheritance off - System { - Name "QP One Foot" - 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 "341" - SimulinkSubDomain "Simulink" - Block { - BlockType Inport - Name "H" - SID "4685" - Position [970, -237, 1000, -223] - ZOrder 750 + Branch { + ZOrder 46 + DstBlock "QP Two Feet" + DstPort 2 } - Block { - BlockType Inport - Name "g" - SID "4686" - Position [970, -207, 1000, -193] - ZOrder 752 - Port "2" } - Block { - BlockType Inport - Name "A" - SID "4687" - Position [970, -177, 1000, -163] - ZOrder 753 - Port "3" + Line { + ZOrder 51 + SrcBlock "ubA" + SrcPort 1 + DstBlock "QP Two Feet" + DstPort 4 } - Block { - BlockType Inport - Name "ubA" - SID "4688" - Position [970, -147, 1000, -133] - 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 + 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 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 + BlockType Outport + Name "f_star_oneFoot" + SID "4626" + Position [245, -297, 275, -283] + ZOrder 401 + VectorParamsAs1DForOutWhenUnconnected off } Block { - BlockType Inport - Name "g" - SID "4690::22" - Position [20, 136, 40, 154] - ZOrder 13 + BlockType Outport + Name "onOneFoot" + SID "4717" + Position [-10, -137, 20, -123] + ZOrder 738 Port "2" + VectorParamsAs1DForOutWhenUnconnected off } Block { - BlockType Demux - Name " Demux " - SID "4690::108" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 99 - Outputs "1" + BlockType Outport + Name "f_0_twoFeet" + SID "4627" + Position [240, 108, 270, 122] + ZOrder 728 + Port "3" + VectorParamsAs1DForOutWhenUnconnected off } - 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" + 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 } - Block { - BlockType Terminator - Name " Terminator " - SID "4690::109" - Position [460, 241, 480, 259] - ZOrder 100 + Line { + ZOrder 37 + SrcBlock "ConstraintsMatrixOneFoot" + SrcPort 1 + DstBlock "QP One Foot" + DstPort 3 } - Block { - BlockType Outport - Name "analyticalSolution" - SID "4690::5" - Position [460, 101, 480, 119] - ZOrder -5 - VectorParamsAs1DForOutWhenUnconnected off + Line { + ZOrder 44 + SrcBlock "gradientTwoFeet" + SrcPort 1 + DstBlock "QP Two Feet" + DstPort 2 } Line { - ZOrder 86 - SrcBlock "H" + ZOrder 43 + SrcBlock "ConstraintsMatrixTwoFeet" SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 + DstBlock "QP Two Feet" + DstPort 3 } Line { - ZOrder 87 - SrcBlock "g" + ZOrder 36 + SrcBlock "bVectorConstraintsOneFoot" SrcPort 1 - DstBlock " SFunction " - DstPort 2 + DstBlock "QP One Foot" + DstPort 4 } Line { - Name "analyticalSolution" - ZOrder 88 - Labels [0, 0] - SrcBlock " SFunction " + ZOrder 42 + SrcBlock "HessianMatrixTwoFeet" + SrcPort 1 + DstBlock "QP Two Feet" + DstPort 1 + } + Line { + ZOrder 39 + SrcBlock "QP One Foot" SrcPort 2 - DstBlock "analyticalSolution" + Points [41, 0; 0, 95] + DstBlock "Switch" DstPort 1 } Line { - ZOrder 89 - SrcBlock " Demux " + ZOrder 62 + SrcBlock "QP Two Feet" SrcPort 1 - DstBlock " Terminator " + 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 90 - SrcBlock " SFunction " + ZOrder 50 + SrcBlock "NOT" SrcPort 1 - DstBlock " Demux " + 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 } - 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 + Branch { + ZOrder 55 + DstBlock "QP One Foot" + DstPort enable } - 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] + } + } + 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 [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 "208" + 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 [134, 55, 3840, 2160] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off @@ -7500,295 +4740,183 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "100" - SIDHighWatermark "117" + ZoomFactor "723" 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" - } + Name "jointPos" + SID "4423" + Position [50, 108, 80, 122] + ZOrder 902 } Block { - BlockType Terminator - Name " Terminator " - SID "4692::117" - Position [460, 241, 480, 259] - ZOrder 102 + BlockType Constant + Name "Constant7" + SID "4425" + Position [50, -43, 80, -27] + ZOrder 895 + ShowName off + Value "eye(4)" } 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 + 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 "QP One Foot" - SID "4693" - Ports [4, 2] - Position [1145, -242, 1250, -128] - ZOrder 751 + 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/Utilities/QP" - SourceType "QP" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" - lbA off - ubA on - lb off - ub off - computeObjVal off - stopIfFails off + 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 "f_star_oneFoot" - SID "4695" - Position [1790, -172, 1820, -158] - ZOrder 760 + Name "fixed_l_sole_H_b" + SID "4476" + Position [495, -22, 525, -8] + ZOrder 910 VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport - Name "QPStatus" - SID "4694" - Position [1360, -162, 1390, -148] - ZOrder 755 + Name "fixed_r_sole_H_b" + SID "4477" + Position [495, 93, 525, 107] + ZOrder 912 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" + SrcBlock "Constant7" SrcPort 1 - Points [31, 0] + Points [33, 0] Branch { - ZOrder 34 - Points [0, -150] - DstBlock "Analytical Solution QP One Foot (unconstrained)" + ZOrder 26 + DstBlock "l_sole to root_link relative transform" DstPort 1 } Branch { - ZOrder 6 - DstBlock "QP One Foot" + ZOrder 23 + Points [0, 115] + DstBlock "r_sole to root_link relative transform" DstPort 1 } } Line { - ZOrder 7 - SrcBlock "ubA" - SrcPort 1 - DstBlock "QP One Foot" - DstPort 4 - } - Line { - ZOrder 8 - SrcBlock "g" + ZOrder 28 + SrcBlock "inv 1" SrcPort 1 - Points [51, 0] - Branch { - ZOrder 33 - Points [0, -55] - Branch { - ZOrder 40 - Points [0, -75] - DstBlock "Analytical Solution QP One Foot (unconstrained)" - DstPort 2 - } - Branch { - ZOrder 11 - DstBlock "MatchSignalSizes" + DstBlock "fixed_r_sole_H_b" DstPort 1 } - } - Branch { - ZOrder 12 - DstBlock "QP One Foot" - DstPort 2 - } - } Line { - ZOrder 13 - SrcBlock "Analytical Solution QP One Foot (unconstrained)" + ZOrder 22 + SrcBlock "inv " SrcPort 1 - DstBlock "Process QP output" + DstBlock "fixed_l_sole_H_b" DstPort 1 } Line { - ZOrder 14 - SrcBlock "QP One Foot" - SrcPort 2 - Points [34, 0] + ZOrder 11 + SrcBlock "jointPos" + SrcPort 1 + Points [18, 0] Branch { - ZOrder 41 - Points [0, 50] - DstBlock "Process QP output" - DstPort 3 + ZOrder 25 + DstBlock "r_sole to root_link relative transform" + DstPort 2 } Branch { - ZOrder 26 - DstBlock "QPStatus" - DstPort 1 - } - } - Line { - ZOrder 17 - SrcBlock "QP One Foot" - SrcPort 1 - DstBlock "MatchSignalSizes" + ZOrder 24 + Points [0, -115] + DstBlock "l_sole to root_link relative transform" DstPort 2 } + } Line { - ZOrder 42 - SrcBlock "feetContactStatus" - SrcPort 1 - DstBlock "Process QP output" - DstPort 4 + ZOrder 27 + SrcBlock "l_sole to root_link relative transform" + SrcPort 1 + DstBlock "inv " + DstPort 1 } Line { - ZOrder 43 - SrcBlock "Process QP output" + ZOrder 29 + SrcBlock "r_sole to root_link relative transform" SrcPort 1 - DstBlock "f_star_oneFoot" + DstBlock "inv 1" DstPort 1 } - } - } - 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 [134, 55, 3840, 2160] + } + } + 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 @@ -7804,213 +4932,192 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "426" + ZoomFactor "100" + SIDHighWatermark "3825" SimulinkSubDomain "Simulink" Block { BlockType Inport - Name "H" - SID "4673" - Position [1460, 1023, 1490, 1037] - ZOrder 751 + Name "J_l_sole" + SID "3721::1" + Position [20, 101, 40, 119] + ZOrder -1 } Block { BlockType Inport - Name "g" - SID "4674" - Position [1460, 1053, 1490, 1067] - ZOrder 753 + Name "J_r_sole" + SID "3721::23" + Position [20, 136, 40, 154] + ZOrder 9 Port "2" } Block { BlockType Inport - Name "A" - SID "4675" - Position [1460, 1083, 1490, 1097] - ZOrder 754 + Name "feetContactStatus" + SID "3721::3701" + Position [20, 171, 40, 189] + ZOrder 26 Port "3" } Block { BlockType Inport - Name "ubA" - SID "4676" - Position [1460, 1113, 1490, 1127] - ZOrder 755 + Name "jointPos_err" + SID "3721::3702" + Position [20, 206, 40, 224] + ZOrder 27 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" + SID "3721::3824" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 99 + ZOrder 149 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 + SID "3721::3823" + Tag "Stateflow S-Function 20" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 148 FunctionName "sf_sfun" - PortCounts "[2 2]" + Parameters "Reg" + PortCounts "[4 2]" SFunctionDeploymentMode off EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 - Name "analyticalSolution" + Name "baseVel_equivalent" } } Block { BlockType Terminator Name " Terminator " - SID "4678::109" + SID "3721::3825" Position [460, 241, 480, 259] - ZOrder 100 + ZOrder 150 } Block { BlockType Outport - Name "analyticalSolution" - SID "4678::5" + Name "baseVel_equivalent" + SID "3721::24" Position [460, 101, 480, 119] - ZOrder -5 + ZOrder 10 VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 86 - SrcBlock "H" + ZOrder 267 + SrcBlock "J_l_sole" SrcPort 1 - Points [120, 0] DstBlock " SFunction " DstPort 1 } Line { - ZOrder 87 - SrcBlock "g" + ZOrder 268 + SrcBlock "J_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - Name "analyticalSolution" - ZOrder 88 + 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 "analyticalSolution" + DstBlock "baseVel_equivalent" DstPort 1 } Line { - ZOrder 89 + ZOrder 272 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 90 + ZOrder 273 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " DstPort 1 } - } - } - Block { - BlockType Reference - Name "MatchSignalSizes" - SID "4679" - Ports [2, 1] - Position [1735, 984, 1825, 1066] - 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" + } + } + 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 @@ -8028,648 +5135,1728 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "117" + SIDHighWatermark "138" SimulinkSubDomain "Simulink" Block { BlockType Inport - Name "analyticalSolution" - SID "4680::29" + Name "state" + SID "4027::1" 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" + SID "4027::137" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 100 + ZOrder 128 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 + SID "4027::136" + Tag "Stateflow S-Function 19" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 127 FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[3 2]" + PortCounts "[1 2]" SFunctionDeploymentMode off EnableBusSupport on SFcnIsStateOwnerBlock off Port { PortNumber 2 - Name "f_star" + Name "booleanState" } } Block { BlockType Terminator Name " Terminator " - SID "4680::117" + SID "4027::138" Position [460, 241, 480, 259] - ZOrder 101 + ZOrder 129 } Block { BlockType Outport - Name "f_star" - SID "4680::5" + Name "booleanState" + SID "4027::5" Position [460, 101, 480, 119] ZOrder -5 VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 122 - SrcBlock "analyticalSolution" + ZOrder 153 + SrcBlock "state" 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 + Name "booleanState" + ZOrder 154 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 - DstBlock "f_star" + DstBlock "booleanState" DstPort 1 } Line { - ZOrder 126 + ZOrder 155 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 127 + ZOrder 156 SrcBlock " SFunction " SrcPort 1 - Points [20, 0] DstBlock " Demux " DstPort 1 } - } - } - Block { - BlockType Reference - Name "QP Two Feet" - SID "4681" - Ports [4, 2] - Position [1575, 1016, 1680, 1134] - ZOrder 752 - LibraryVersion "1.684" - SourceBlock "WBToolboxLibrary/Utilities/QP" - SourceType "QP" - SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" - lbA off - 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 [28, 0] - Branch { - ZOrder 62 - Points [0, 55] - DstBlock "Process QP output" - DstPort 3 - } + } + } + 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 61 - DstBlock "QPStatus" - DstPort 1 - } - } - 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 - DstBlock "MatchSignalSizes" - DstPort 2 - } - Line { - ZOrder 45 - SrcBlock "H" - SrcPort 1 - Points [22, 0] + ZOrder 136 + Points [0, 65] Branch { - ZOrder 79 - DstBlock "QP Two Feet" - DstPort 1 - } + ZOrder 131 + Points [0, 35] Branch { - ZOrder 43 - Points [0, -160] - DstBlock "Analytical Solution Two Feet (unconstrained)" + ZOrder 133 + Points [0, 50] + DstBlock "Add" DstPort 1 } - } - Line { - ZOrder 50 - SrcBlock "g" - SrcPort 1 - Points [47, 0] - Branch { - ZOrder 49 - Points [0, -55] Branch { - ZOrder 48 - Points [0, -100] - DstBlock "Analytical Solution Two Feet (unconstrained)" + ZOrder 126 + DstBlock "Jacobian1" DstPort 2 } + } Branch { - ZOrder 47 - DstBlock "MatchSignalSizes" - DstPort 1 + ZOrder 124 + DstBlock "Jacobian" + DstPort 2 } } Branch { - ZOrder 46 - DstBlock "QP Two Feet" + ZOrder 123 + DstBlock "CentroidalMomentum" DstPort 2 } - } - Line { - ZOrder 51 - SrcBlock "ubA" - SrcPort 1 - DstBlock "QP Two Feet" + } + 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 Switch - Name "Switch" - SID "4607" - Position [225, -133, 290, -67] - ZOrder 732 - ShowName off - HideAutomaticName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off + BlockType Ground + Name " Ground " + SID "4551::158" + Position [20, 121, 40, 139] + ZOrder 149 } Block { - BlockType Outport - Name "f_star_oneFoot" - SID "4626" - Position [245, -297, 275, -283] - ZOrder 401 - VectorParamsAs1DForOutWhenUnconnected off + 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 Outport - Name "onOneFoot" - SID "4717" - Position [-10, -137, 20, -123] - ZOrder 738 - Port "2" - VectorParamsAs1DForOutWhenUnconnected off + BlockType Terminator + Name " Terminator " + SID "4551::157" + Position [460, 241, 480, 259] + ZOrder 148 } Block { BlockType Outport - Name "f_0_twoFeet" - SID "4627" - Position [240, 108, 270, 122] - ZOrder 728 - Port "3" + Name "reflectedInertia" + SID "4551::5" + Position [460, 101, 480, 119] + ZOrder -5 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" + Name "reflectedInertia" + ZOrder 137 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "reflectedInertia" DstPort 1 } - } Line { - ZOrder 41 - SrcBlock "HessianMatrixOneFoot" + ZOrder 138 + SrcBlock " Ground " SrcPort 1 - DstBlock "QP One Foot" + DstBlock " SFunction " 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" + ZOrder 139 + SrcBlock " Demux " SrcPort 1 - DstBlock "QP Two Feet" + DstBlock " Terminator " DstPort 1 } Line { - ZOrder 39 - SrcBlock "QP One Foot" - SrcPort 2 - Points [41, 0; 0, 95] - DstBlock "Switch" + ZOrder 140 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " DstPort 1 } - Line { + } + } + 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 - SrcBlock "QP Two Feet" - SrcPort 1 - Points [40, 0; 0, -115] + Points [0, -75] + DstBlock "Add" + DstPort 2 + } + Branch { + ZOrder 93 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 - } } } + 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 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 + BlockType Outport + Name "bVectorConstraintsTwoFeet" + SID "2407::812" + Position [460, 351, 480, 369] + ZOrder 60 + Port "8" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport - Name "tau_QP" - SID "4408" - Position [1630, 608, 1660, 622] - ZOrder 425 + Name "tauModel" + SID "2407::815" + Position [460, 386, 480, 404] + ZOrder 63 + Port "9" VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport - Name "f_star" - SID "4409" - Position [1440, 733, 1470, 747] - ZOrder 639 - Port "2" + 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 293 - SrcBlock "Na" + ZOrder 2582 + SrcBlock "feetContactStatus" SrcPort 1 - DstBlock "Product2" + DstBlock " SFunction " DstPort 1 } Line { - ZOrder 54 - SrcBlock "Product1" + ZOrder 2583 + SrcBlock "ConstraintsMatrix" SrcPort 1 - DstBlock "Add" + DstBlock " SFunction " DstPort 2 } Line { - ZOrder 291 - SrcBlock "tauModel" + ZOrder 2584 + SrcBlock "bVectorConstraints" SrcPort 1 - DstBlock "Add" - DstPort 1 + DstBlock " SFunction " + DstPort 3 } Line { - ZOrder 304 - SrcBlock "Add" + ZOrder 2585 + SrcBlock "jointPos" SrcPort 1 - DstBlock "tau_QP" - DstPort 1 + DstBlock " SFunction " + DstPort 4 } Line { - ZOrder 57 - SrcBlock "Add1" + ZOrder 2586 + SrcBlock "jointPos_des" SrcPort 1 - Points [253, 0] - Branch { - ZOrder 407 - Points [0, 60] - DstBlock "f_star" - DstPort 1 - } - Branch { - ZOrder 383 - DstBlock "Product1" - DstPort 2 - } + DstBlock " SFunction " + DstPort 5 } Line { - ZOrder 406 - SrcBlock "f_LDot is f_star while on One Foot" + ZOrder 2587 + SrcBlock "nu" SrcPort 1 - DstBlock "Add1" - DstPort 1 + DstBlock " SFunction " + DstPort 6 } Line { - ZOrder 66 - SrcBlock "Product2" + ZOrder 2588 + SrcBlock "M" SrcPort 1 - DstBlock "Add1" - DstPort 2 + DstBlock " SFunction " + DstPort 7 } Line { - ZOrder 292 - SrcBlock "Sigma" + ZOrder 2589 + SrcBlock "h" SrcPort 1 - DstBlock "Product1" - DstPort 1 + DstBlock " SFunction " + DstPort 8 } Line { - ZOrder 355 - SrcBlock "QPSolver" - SrcPort 3 - DstBlock "Product2" - DstPort 2 + ZOrder 2590 + SrcBlock "L" + SrcPort 1 + DstBlock " SFunction " + DstPort 9 } Line { - ZOrder 346 - SrcBlock "gradientOneFoot" + ZOrder 2591 + SrcBlock "intL_angMomError" SrcPort 1 - DstBlock "QPSolver" - DstPort 3 + DstBlock " SFunction " + DstPort 10 } Line { - ZOrder 351 - SrcBlock "ConstraintsMatrixTwoFeet" + ZOrder 2592 + SrcBlock "w_H_l_sole" SrcPort 1 - DstBlock "QPSolver" - DstPort 8 + 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 { - ZOrder 344 - SrcBlock "feetContactStatus" - SrcPort 1 - DstBlock "QPSolver" + Name "bVectorConstraintsOneFoot" + ZOrder 2607 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "bVectorConstraintsOneFoot" DstPort 1 } Line { - ZOrder 348 - SrcBlock "bVectorConstraintsOneFoot" - SrcPort 1 - DstBlock "QPSolver" - DstPort 5 + Name "HessianMatrixTwoFeet" + ZOrder 2608 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "HessianMatrixTwoFeet" + DstPort 1 } Line { - ZOrder 345 - SrcBlock "HessianMatrixOneFoot" - SrcPort 1 - DstBlock "QPSolver" - DstPort 2 + Name "gradientTwoFeet" + ZOrder 2609 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 7 + DstBlock "gradientTwoFeet" + DstPort 1 } Line { - ZOrder 347 - SrcBlock "ConstraintsMatrixOneFoot" - SrcPort 1 - DstBlock "QPSolver" - DstPort 4 + Name "ConstraintsMatrixTwoFeet" + ZOrder 2610 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 8 + DstBlock "ConstraintsMatrixTwoFeet" + DstPort 1 } Line { - ZOrder 350 - SrcBlock "gradientTwoFeet" - SrcPort 1 - DstBlock "QPSolver" - DstPort 7 + Name "bVectorConstraintsTwoFeet" + ZOrder 2611 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 9 + DstBlock "bVectorConstraintsTwoFeet" + DstPort 1 } Line { - ZOrder 349 - SrcBlock "HessianMatrixTwoFeet" - SrcPort 1 - DstBlock "QPSolver" - DstPort 6 + Name "tauModel" + ZOrder 2612 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 10 + DstBlock "tauModel" + DstPort 1 } Line { - ZOrder 352 - SrcBlock "bVectorConstraintsTwoFeet" - SrcPort 1 - DstBlock "QPSolver" - DstPort 9 + Name "Sigma" + ZOrder 2613 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 11 + DstBlock "Sigma" + DstPort 1 } Line { - ZOrder 376 - SrcBlock "QPSolver" - SrcPort 2 - Points [27, 0; 0, -60] - DstBlock "f_LDot is f_star while on One Foot" - DstPort 2 + Name "Na" + ZOrder 2614 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 12 + DstBlock "Na" + DstPort 1 } Line { - ZOrder 378 - SrcBlock "f_LDot" + 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 "f_LDot is f_star while on One Foot" - DstPort 3 + DstBlock " Terminator " + DstPort 1 } Line { - ZOrder 405 - SrcBlock "QPSolver" + ZOrder 2617 + SrcBlock " SFunction " SrcPort 1 - DstBlock "f_LDot is f_star while on One Foot" + 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 82 + $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 "Compute angular momentum integral error" - SID "3714" - Ports [4, 1] - Position [1250, 289, 1420, 331] - ZOrder 835 + 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 "Compute angular momentum integral error" - Location [134, 55, 3840, 2160] + Name "Desired and Measured Forces" + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off @@ -8685,273 +6872,122 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "208" + ZoomFactor "156" SimulinkSubDomain "Simulink" Block { BlockType Inport - Name "jointPos_des" - SID "3715" - Position [-230, -147, -200, -133] - ZOrder -1 + Name "wrench_leftFoot" + SID "3245" + Position [-580, 237, -550, 253] + ZOrder 648 } Block { BlockType Inport - Name "jointPos" - SID "3716" - Position [-230, 143, -200, 157] - ZOrder 1 + Name "wrench_rightFoot" + SID "3251" + Position [-580, 337, -550, 353] + ZOrder 682 Port "2" } Block { BlockType Inport - Name "feetContactStatus" - SID "3717" - Position [620, 98, 650, 112] - ZOrder 80 + Name "f_star" + SID "4510" + Position [-580, 438, -550, 452] + ZOrder 688 Port "3" } Block { BlockType Inport Name "state" - SID "4028" - Position [50, -147, 80, -133] - ZOrder 732 + SID "4785" + Position [-345, 68, -315, 82] + ZOrder 690 Port "4" + Port { + PortNumber 1 + Name "state" + } } Block { - BlockType Sum - Name "Add" - SID "4724" - Ports [2, 1] - Position [-40, 114, -5, 161] - ZOrder 902 + BlockType EnablePort + Name "Enable" + SID "3246" + Ports [] + Position [-338, -30, -318, -10] + ZOrder 649 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 + 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 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 [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 "723" - 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 - } + 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 "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 + Name "Demux Forces nd Moments" + SID "4806" + Ports [3, 8] + Position [-440, 191, -220, 494] + ZOrder 697 RequestExecContextInheritance off - SFBlockType "MATLAB Function" + 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 "Get Equivalent Base Velocity" - Location [223, 338, 826, 833] + Name "Demux Forces nd Moments" + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off @@ -8967,891 +7003,1443 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "100" - SIDHighWatermark "3825" + ZoomFactor "205" SimulinkSubDomain "Simulink" Block { BlockType Inport - Name "J_l_sole" - SID "3721::1" - Position [20, 101, 40, 119] - ZOrder -1 + Name "wrench_leftFoot" + SID "4807" + Position [-620, -98, -590, -82] + ZOrder 697 } Block { BlockType Inport - Name "J_r_sole" - SID "3721::23" - Position [20, 136, 40, 154] - ZOrder 9 + Name "wrench_rightFoot" + SID "4808" + Position [-620, 67, -590, 83] + ZOrder 698 Port "2" } Block { BlockType Inport - Name "feetContactStatus" - SID "3721::3701" - Position [20, 171, 40, 189] - ZOrder 26 + Name "f_star" + SID "4809" + Position [-200, -22, -170, -8] + ZOrder 699 Port "3" } Block { - BlockType Inport - Name "jointPos_err" - SID "3721::3702" - Position [20, 206, 40, 224] - ZOrder 27 - Port "4" + 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 " Demux " - SID "3721::3824" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 149 - Outputs "1" + Name "Demux2" + SID "4802" + Ports [1, 2] + Position [-90, -144, -85, 114] + ZOrder 693 + ShowName off + Outputs "[6;6]" } 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" + 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 Terminator - Name " Terminator " - SID "3721::3825" - Position [460, 241, 480, 259] - ZOrder 150 + 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 "baseVel_equivalent" - SID "3721::24" - Position [460, 101, 480, 119] - ZOrder 10 + Name "measured forces l_sole" + SID "4810" + Position [-365, -132, -335, -118] + ZOrder 700 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 + Block { + BlockType Outport + Name "des forces l_sole" + SID "4812" + Position [75, -112, 105, -98] + ZOrder 702 + Port "2" + VectorParamsAs1DForOutWhenUnconnected off } - Line { - Name "baseVel_equivalent" - ZOrder 271 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "baseVel_equivalent" - DstPort 1 + Block { + BlockType Outport + Name "measured forces r_sole" + SID "4813" + Position [-365, 33, -335, 47] + ZOrder 703 + Port "3" + VectorParamsAs1DForOutWhenUnconnected off } - Line { - ZOrder 272 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 + Block { + BlockType Outport + Name "des forces r_sole" + SID "4816" + Position [75, 18, 105, 32] + ZOrder 706 + Port "4" + VectorParamsAs1DForOutWhenUnconnected off } - Line { - ZOrder 273 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 + Block { + BlockType Outport + Name "measured moments l_sole" + SID "4811" + Position [-365, -67, -335, -53] + ZOrder 701 + Port "5" + VectorParamsAs1DForOutWhenUnconnected off } - } - } - 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 + BlockType Outport + Name "des moments l_sole" + SID "4814" + Position [75, -67, 105, -53] + ZOrder 704 + Port "6" + VectorParamsAs1DForOutWhenUnconnected off } Block { - BlockType Demux - Name " Demux " - SID "4027::137" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 128 - Outputs "1" + BlockType Outport + Name "measured moments r_sole" + SID "4815" + Position [-365, 98, -335, 112] + ZOrder 705 + Port "7" + VectorParamsAs1DForOutWhenUnconnected off } 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" + 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 } - Block { - BlockType Terminator - Name " Terminator " - SID "4027::138" - Position [460, 241, 480, 259] - ZOrder 129 + Line { + ZOrder 4 + SrcBlock "Demux7" + SrcPort 1 + DstBlock "measured forces r_sole" + DstPort 1 } - Block { - BlockType Outport - Name "booleanState" - SID "4027::5" - Position [460, 101, 480, 119] - ZOrder -5 - VectorParamsAs1DForOutWhenUnconnected off + Line { + ZOrder 5 + SrcBlock "Demux7" + SrcPort 2 + DstBlock "measured moments r_sole" + DstPort 1 } Line { - ZOrder 153 - SrcBlock "state" + ZOrder 6 + SrcBlock "wrench_leftFoot" SrcPort 1 - DstBlock " SFunction " + DstBlock "Demux1" DstPort 1 } Line { - Name "booleanState" - ZOrder 154 - Labels [0, 0] - SrcBlock " SFunction " + 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 "booleanState" + DstBlock "measured moments l_sole" DstPort 1 } Line { - ZOrder 155 - SrcBlock " Demux " + ZOrder 11 + SrcBlock "f_star" SrcPort 1 - DstBlock " Terminator " + DstBlock "Demux2" DstPort 1 } Line { - ZOrder 156 - SrcBlock " SFunction " + ZOrder 12 + SrcBlock "Demux2" + SrcPort 2 + DstBlock "Demux6" + DstPort 1 + } + Line { + ZOrder 13 + SrcBlock "Demux5" SrcPort 1 - DstBlock " Demux " + DstBlock "des forces l_sole" DstPort 1 } } } Block { - BlockType Selector - Name "Selector" - SID "3722" - Ports [1, 1] - Position [1365, 16, 1405, 54] - ZOrder 78 + BlockType Sum + Name "Sum" + SID "4818" + Ports [2, 1] + Position [-40, -10, -20, 10] + ZOrder 698 ShowName off - InputPortWidth "6" - IndexOptions "Index vector (dialog)" - Indices "[4 5 6]" - OutputSizes "1" + Inputs "-+|" + Port { + PortNumber 1 + Name "error forces l_sole" + } } Block { - BlockType Switch - Name "Switch" - SID "4029" - Position [405, -205, 465, -75] - ZOrder 733 + 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 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off + Inputs "-+|" + Port { + PortNumber 1 + Name "error moments l_sole" + } } Block { - BlockType Outport - Name "intL_angMomError" - SID "3753" - Position [1450, 28, 1480, 42] - ZOrder -2 - VectorParamsAs1DForOutWhenUnconnected off + 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 "jointPos_des" + SrcBlock "wrench_leftFoot" 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 - } + DstBlock "Demux Forces nd Moments" + DstPort 1 } Line { - ZOrder 127 - SrcBlock "jointPos" + ZOrder 2 + SrcBlock "wrench_rightFoot" SrcPort 1 - DstBlock "Add" + DstBlock "Demux Forces nd Moments" DstPort 2 } Line { - ZOrder 129 - SrcBlock "Add" + ZOrder 3 + SrcBlock "f_star" SrcPort 1 - Points [757, 0] + 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 142 - DstBlock "Get Equivalent Base Velocity" - DstPort 4 + ZOrder 5 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 1 } Branch { - ZOrder 141 - Points [0, 65] - DstBlock "CentroidalMomentum" - DstPort 4 + ZOrder 6 + DstBlock "Sum" + DstPort 1 } } 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 + 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 { - ZOrder 19 - SrcBlock "Selector" + Name "error forces l_sole" + ZOrder 10 + Labels [-1, 0] + SrcBlock "Sum" SrcPort 1 - DstBlock "intL_angMomError" - DstPort 1 + DstBlock "left Foot forces" + DstPort 3 } Line { - ZOrder 25 - SrcBlock "state" + Name "thresholdContactOn" + ZOrder 11 + SrcBlock "Constant" SrcPort 1 - DstBlock "Select base to world transform" - DstPort 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 { - ZOrder 27 - SrcBlock "Select base to world transform" - SrcPort 1 - DstBlock "Switch" - DstPort 2 + 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 { - ZOrder 28 - SrcBlock "Switch" - SrcPort 1 - Points [30, 0] + Name "measured forces r_sole" + ZOrder 17 + SrcBlock "Demux Forces nd Moments" + SrcPort 3 + Points [90, 0; 0, -155; 95, 0] Branch { - ZOrder 138 - DstBlock "CentroidalMomentum" + ZOrder 18 + Labels [-1, 0] + DstBlock "right Foot forces" DstPort 1 } Branch { - ZOrder 135 - Points [0, 170] - Branch { - ZOrder 139 - DstBlock "Jacobian" - DstPort 1 - } - Branch { - ZOrder 130 - Points [0, 35] - DstBlock "Jacobian1" + ZOrder 19 + DstBlock "Sum1" DstPort 1 - } } } Line { - ZOrder 82 - SrcBlock "Compute base to fixed link transform" + Name "error forces r_sole" + ZOrder 20 + Labels [-1, 0] + SrcBlock "Sum1" SrcPort 1 - DstBlock "Switch" - DstPort 1 - } - Line { - ZOrder 83 - SrcBlock "Compute base to fixed link transform" - SrcPort 2 - DstBlock "Switch" + DstBlock "right Foot forces" DstPort 3 } Line { - ZOrder 132 - SrcBlock "Jacobian" - SrcPort 1 - DstBlock "Get Equivalent Base Velocity" - DstPort 1 - } - Line { - ZOrder 134 - SrcBlock "Jacobian1" + Name "thresholdContactOff" + ZOrder 21 + SrcBlock "Constant2" SrcPort 1 - DstBlock "Get Equivalent Base Velocity" - DstPort 2 + 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 { - 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" + 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 } - Line { - ZOrder 138 - SrcBlock " Ground " - SrcPort 1 - DstBlock " SFunction " - DstPort 1 + Branch { + ZOrder 31 + DstBlock "left Foot moments" + DstPort 4 } - Line { - ZOrder 139 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 } - Line { - ZOrder 140 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 + Branch { + ZOrder 32 + Labels [-1, 0] + DstBlock "right Foot forces" + DstPort 6 } } + Branch { + ZOrder 33 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 6 + } } - Block { - BlockType Sum - Name "Add" - SID "4531" - Ports [2, 1] - Position [65, -209, 110, -16] - ZOrder 602 - ShowName off - HideAutomaticName off - IconShape "rectangular" - Inputs "-+" + 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 + } } - Block { - BlockType From - Name "From" - SID "4654" - Position [-460, -158, -375, -142] - ZOrder 902 - ShowName off - HideAutomaticName off - GotoTag "jointAcc_des" - TagVisibility "global" + 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 Gain - Name "Gain" - SID "4525" - Position [-60, -175, 30, -145] - ZOrder 596 - ShowName off - HideAutomaticName off - Gain "Config.K_ff" + BlockType Inport + Name "feetContactStatus" + SID "4106" + Position [5, -71, 35, -59] + ZOrder 646 } Block { - BlockType Product - Name "Product" - SID "4526" - Ports [2, 1] - Position [-170, -239, -135, -86] - ZOrder 597 - ShowName off - HideAutomaticName off - Multiplication "Matrix(*)" + BlockType Inport + Name "KP_postural_diag" + SID "4107" + Position [5, 127, 35, 143] + ZOrder 647 + Port "2" } 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 + BlockType Inport + Name "state" + SID "4108" + Position [5, -6, 35, 6] + ZOrder 648 + Port "3" } Block { - BlockType Switch - Name "Switch1" - SID "4650" - Position [-260, -166, -205, -84] - ZOrder 896 + BlockType EnablePort + Name "Enable" + SID "4110" + Ports [] + Position [392, -150, 412, -130] + ZOrder 649 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 + 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" + } } - Line { - ZOrder 41 - SrcBlock "Switch" - SrcPort 1 - DstBlock "tau_joints" - DstPort 1 + 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 20 - SrcBlock "tau_QP" + ZOrder 1 + SrcBlock "state" SrcPort 1 - Points [24, 0] + Points [85, 0] Branch { - ZOrder 62 - Points [0, -75] - DstBlock "Add" + ZOrder 2 + DstBlock "Feet Contact Activation" DstPort 2 } Branch { - ZOrder 93 - DstBlock "Switch" - DstPort 3 + ZOrder 3 + Points [0, 195] + DstBlock "Gain Scheduling Postural" + DstPort 6 } } Line { - ZOrder 52 - SrcBlock "(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" + ZOrder 4 + SrcBlock "feetContactStatus" SrcPort 1 - DstBlock "Product" + DstBlock "Feet Contact Activation" DstPort 1 } Line { - ZOrder 108 - SrcBlock " 1" + ZOrder 5 + SrcBlock "KP_postural_diag" SrcPort 1 - DstBlock "Switch1" - DstPort 2 + DstBlock "Demux1" + DstPort 1 } Line { - ZOrder 109 - SrcBlock "Switch1" - SrcPort 1 - DstBlock "Product" - DstPort 2 + Name "left_leg" + ZOrder 6 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 4 + DstBlock "Gain Scheduling Postural" + DstPort 4 } Line { - ZOrder 114 - SrcBlock "From" + 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 "Switch1" + DstBlock "Gain Scheduling Postural" DstPort 1 } Line { - ZOrder 116 - SrcBlock "jointAcc" - SrcPort 1 - DstBlock "Switch1" - DstPort 3 + Name "left_arm" + ZOrder 10 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 2 + DstBlock "Gain Scheduling Postural" + DstPort 2 } } } Block { - BlockType Goto - Name "Goto1" - SID "4495" - Position [2400, 31, 2470, 49] - ZOrder 856 + BlockType From + Name "From" + SID "4725" + Position [490, 275, 585, 295] + ZOrder 898 ShowName off HideAutomaticName off - GotoTag "tau_star" + GotoTag "tau_star_afterSat" TagVisibility "global" } Block { - BlockType Goto - Name "Goto5" - SID "4499" - Position [2035, 596, 2095, 614] - ZOrder 860 + 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 "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 + Name "Visualize eventual QP failures" + SID "4620" + Ports [2, 0, 1] + Position [290, 161, 405, 219] + ZOrder 717 RequestExecContextInheritance off - SFBlockType "MATLAB Function" System { - Name "Momentum Based Balancing Controller\n" - Location [219, 337, 814, 775] + Name "Visualize eventual QP failures" + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off @@ -9867,978 +8455,2424 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "100" - SIDHighWatermark "3824" + ZoomFactor "1000" SimulinkSubDomain "Simulink" Block { BlockType Inport - Name "feetContactStatus" - SID "2407::28" - Position [20, 101, 40, 119] - ZOrder 14 + Name "QPStatus" + SID "4621" + Position [100, 164, 125, 176] + ZOrder 409 } Block { BlockType Inport - Name "ConstraintsMatrix" - SID "2407::808" - Position [20, 136, 40, 154] - ZOrder 56 + 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 "99" + 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 "bVectorConstraints" - SID "2407::809" - Position [20, 171, 40, 189] - ZOrder 57 + Name "jointPos_des" + SID "4508" + Position [740, -157, 770, -143] + ZOrder 594 Port "3" } Block { - BlockType Inport - Name "jointPos" - SID "2407::29" - Position [20, 206, 40, 224] - ZOrder 15 + 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 "jointPos_des" - SID "2407::1" - Position [20, 246, 40, 264] - ZOrder -1 + 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 "nu" - SID "2407::22" - Position [20, 281, 40, 299] - ZOrder 8 + Name "state" + SID "4507" + Position [65, -277, 95, -263] + ZOrder 593 Port "6" + Port { + PortNumber 1 + Name "state" + PropagatedSignals "state" + } } Block { BlockType Inport - Name "M" - SID "2407::23" - Position [20, 316, 40, 334] - ZOrder 9 + Name "tau_star_afterSat" + SID "4562" + Position [65, -337, 95, -323] + ZOrder 873 Port "7" } Block { BlockType Inport - Name "h" - SID "2407::24" - Position [20, 351, 40, 369] - ZOrder 10 + Name "nu" + SID "4109" + Position [740, 192, 770, 208] + ZOrder 887 Port "8" } Block { BlockType Inport - Name "L" - SID "2407::26" - Position [20, 386, 40, 404] - ZOrder 12 + Name "jointPos" + SID "4826" + Position [740, -307, 770, -293] + ZOrder 895 Port "9" } Block { - BlockType Inport - Name "intL_angMomError" - SID "2407::21" - Position [20, 426, 40, 444] - ZOrder 7 - Port "10" + 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 Inport - Name "w_H_l_sole" - SID "2407::800" - Position [20, 461, 40, 479] - ZOrder 50 - Port "11" + 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 Inport - Name "w_H_r_sole" - SID "2407::27" - Position [20, 496, 40, 514] - ZOrder 13 - Port "12" + 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 Inport - Name "J_l_sole" - SID "2407::841" - Position [20, 531, 40, 549] - ZOrder 84 - Port "13" + BlockType Gain + Name "Gain" + SID "2289" + Position [910, -318, 960, -282] + ZOrder 309 + ShowName off + Gain "180/pi" } Block { - BlockType Inport - Name "J_r_sole" - SID "2407::790" - Position [20, 566, 40, 584] - ZOrder 41 - Port "14" + BlockType Gain + Name "Gain1" + SID "4114" + Position [945, 187, 995, 213] + ZOrder 892 + ShowName off + Gain "180/pi" } Block { - BlockType Inport - Name "JDot_l_sole_nu" - SID "2407::784" - Position [20, 606, 40, 624] - ZOrder 36 - Port "15" + 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 Inport - Name "JDot_r_sole_nu" - SID "2407::785" - Position [20, 641, 40, 659] - ZOrder 37 - Port "16" + BlockType Gain + Name "Gain3" + SID "2290" + Position [910, -168, 960, -132] + ZOrder 500 + ShowName off + Gain "180/pi" } Block { - BlockType Inport - Name "pos_CoM" - SID "2407::798" - Position [20, 676, 40, 694] - ZOrder 48 - Port "17" + BlockType Gain + Name "Gain4" + SID "2291" + Position [910, -8, 960, 28] + ZOrder 503 + ShowName off + Gain "180/pi" } Block { - BlockType Inport - Name "J_CoM" - SID "2407::31" - Position [20, 711, 40, 729] - ZOrder 17 - Port "18" + BlockType Ground + Name "Ground" + SID "4971" + Position [70, 110, 90, 130] + ZOrder 897 } Block { - BlockType Inport - Name "desired_pos_vel_acc_CoM" - SID "2407::30" - Position [20, 746, 40, 764] - ZOrder 16 - Port "19" + 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 Inport - Name "KP_CoM" - SID "2407::32" - Position [20, 786, 40, 804] - ZOrder 18 - Port "20" + 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 Inport - Name "KD_CoM" - SID "2407::845" - Position [20, 821, 40, 839] - ZOrder 88 - Port "21" + 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 Inport - Name "KP_postural" - SID "2407::846" - Position [20, 856, 40, 874] - ZOrder 89 - Port "22" + 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 Demux - Name " Demux " - SID "2407::3823" + 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 [270, 570, 320, 610] - ZOrder 226 - Outputs "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 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" - } + 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 7 - Name "gradientTwoFeet" + PortNumber 1 + Name "CoM_Error" } - Port { - PortNumber 8 - Name "ConstraintsMatrixTwoFeet" + } + 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 } - Port { - PortNumber 9 - Name "bVectorConstraintsTwoFeet" + Branch { + ZOrder 3 + Labels [-1, 0] + DstBlock "CoM Position" + DstPort 1 } - Port { - PortNumber 10 - Name "tauModel" + } + 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 } - Port { - PortNumber 11 - Name "Sigma" + Branch { + ZOrder 7 + DstBlock "Sum3" + DstPort 1 } - Port { - PortNumber 12 - Name "Na" + } + 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 13 + SrcBlock "Ground" + SrcPort 1 + Points [125, 0] + Branch { + ZOrder 14 + DstBlock "Sum" + DstPort 2 } - Port { - PortNumber 13 - Name "f_LDot" + Branch { + ZOrder 15 + DstBlock "Demux9" + DstPort 1 } } - 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 + Line { + Name "right_leg" + ZOrder 16 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 5 + DstBlock "Desired Torques" + DstPort 5 } - Block { - BlockType Outport - Name "gradientOneFoot" - SID "2407::822" - Position [460, 136, 480, 154] - ZOrder 70 - Port "2" - VectorParamsAs1DForOutWhenUnconnected off + Line { + Name "torso" + ZOrder 17 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 1 + DstBlock "Desired Torques" + DstPort 1 } - Block { - BlockType Outport - Name "ConstraintsMatrixOneFoot" - SID "2407::5" - Position [460, 171, 480, 189] - ZOrder -5 - Port "3" - VectorParamsAs1DForOutWhenUnconnected off + Line { + Name "left_arm" + ZOrder 18 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 2 + DstBlock "Desired Torques" + DstPort 2 } - Block { - BlockType Outport - Name "bVectorConstraintsOneFoot" - SID "2407::810" - Position [460, 206, 480, 224] - ZOrder 58 - Port "4" - VectorParamsAs1DForOutWhenUnconnected off + Line { + Name "left_leg" + ZOrder 19 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 4 + DstBlock "Desired Torques" + DstPort 4 } - Block { - BlockType Outport - Name "HessianMatrixTwoFeet" - SID "2407::827" - Position [460, 246, 480, 264] - ZOrder 75 - Port "5" - VectorParamsAs1DForOutWhenUnconnected off + Line { + Name "right_arm" + ZOrder 20 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 3 + DstBlock "Desired Torques" + DstPort 3 } - Block { - BlockType Outport - Name "gradientTwoFeet" - SID "2407::828" - Position [460, 281, 480, 299] - ZOrder 76 - Port "6" - VectorParamsAs1DForOutWhenUnconnected off + Line { + Name "right_leg" + ZOrder 21 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 5 + DstBlock "Torques Error" + DstPort 5 } - Block { - BlockType Outport - Name "ConstraintsMatrixTwoFeet" - SID "2407::811" - Position [460, 316, 480, 334] - ZOrder 59 - Port "7" - VectorParamsAs1DForOutWhenUnconnected off + Line { + Name "torso" + ZOrder 22 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 1 + DstBlock "Torques Error" + DstPort 1 } - Block { - BlockType Outport - Name "bVectorConstraintsTwoFeet" - SID "2407::812" - Position [460, 351, 480, 369] - ZOrder 60 - Port "8" - VectorParamsAs1DForOutWhenUnconnected off + Line { + Name "left_arm" + ZOrder 23 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 2 + DstBlock "Torques Error" + DstPort 2 } - Block { - BlockType Outport - Name "tauModel" - SID "2407::815" - Position [460, 386, 480, 404] - ZOrder 63 - Port "9" - VectorParamsAs1DForOutWhenUnconnected off + Line { + Name "left_leg" + ZOrder 24 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 4 + DstBlock "Torques Error" + DstPort 4 } - Block { - BlockType Outport - Name "Sigma" - SID "2407::816" - Position [460, 426, 480, 444] - ZOrder 64 - Port "10" - VectorParamsAs1DForOutWhenUnconnected off + Line { + Name "right_arm" + ZOrder 25 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 3 + DstBlock "Torques Error" + DstPort 3 } - Block { - BlockType Outport - Name "Na" - SID "2407::820" - Position [460, 461, 480, 479] - ZOrder 68 - Port "11" - VectorParamsAs1DForOutWhenUnconnected off + Line { + ZOrder 26 + SrcBlock "Sum" + SrcPort 1 + DstBlock "Demux10" + DstPort 1 } - Block { - BlockType Outport - Name "f_LDot" - SID "2407::821" - Position [460, 496, 480, 514] - ZOrder 69 - Port "12" - VectorParamsAs1DForOutWhenUnconnected off + Line { + Name "right_leg" + ZOrder 27 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 5 + DstBlock "Measured Torques" + DstPort 5 } Line { - ZOrder 2582 - SrcBlock "feetContactStatus" + Name "torso" + ZOrder 28 + Labels [-1, 0] + SrcBlock "Demux9" SrcPort 1 - DstBlock " SFunction " + DstBlock "Measured Torques" DstPort 1 } Line { - ZOrder 2583 - SrcBlock "ConstraintsMatrix" - SrcPort 1 - DstBlock " SFunction " + Name "left_arm" + ZOrder 29 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 2 + DstBlock "Measured Torques" DstPort 2 } Line { - ZOrder 2584 - SrcBlock "bVectorConstraints" - SrcPort 1 - DstBlock " SFunction " + 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 2585 - SrcBlock "jointPos" + ZOrder 32 + SrcBlock "jointPos_des" SrcPort 1 - DstBlock " SFunction " - DstPort 4 + Points [32, 0] + Branch { + ZOrder 33 + Points [0, 160] + DstBlock "Sum3" + DstPort 2 + } + Branch { + ZOrder 34 + DstBlock "Gain3" + DstPort 1 + } } Line { - ZOrder 2586 - SrcBlock "jointPos_des" + ZOrder 35 + SrcBlock "Gain3" SrcPort 1 - DstBlock " SFunction " + DstBlock "Demux2" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 36 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 5 + DstBlock "Joint Position Desired" DstPort 5 } Line { - ZOrder 2587 - SrcBlock "nu" + Name "torso" + ZOrder 37 + Labels [-1, 0] + SrcBlock "Demux2" SrcPort 1 - DstBlock " SFunction " - DstPort 6 + DstBlock "Joint Position Desired" + DstPort 1 } Line { - ZOrder 2588 - SrcBlock "M" - SrcPort 1 - DstBlock " SFunction " - DstPort 7 + Name "left_arm" + ZOrder 38 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 2 + DstBlock "Joint Position Desired" + DstPort 2 } Line { - ZOrder 2589 - SrcBlock "h" - SrcPort 1 - DstBlock " SFunction " - DstPort 8 + Name "left_leg" + ZOrder 39 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 4 + DstBlock "Joint Position Desired" + DstPort 4 } Line { - ZOrder 2590 - SrcBlock "L" - SrcPort 1 - DstBlock " SFunction " - DstPort 9 + Name "right_arm" + ZOrder 40 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 3 + DstBlock "Joint Position Desired" + DstPort 3 } Line { - ZOrder 2591 - SrcBlock "intL_angMomError" + ZOrder 41 + SrcBlock "Gain4" SrcPort 1 - DstBlock " SFunction " - DstPort 10 + DstBlock "Demux5" + DstPort 1 } Line { - ZOrder 2592 - SrcBlock "w_H_l_sole" - SrcPort 1 - DstBlock " SFunction " - DstPort 11 + Name "right_leg" + ZOrder 42 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 5 + DstBlock "Joint Positon Error" + DstPort 5 } Line { - ZOrder 2593 - SrcBlock "w_H_r_sole" + Name "torso" + ZOrder 43 + Labels [-1, 0] + SrcBlock "Demux5" SrcPort 1 - DstBlock " SFunction " - DstPort 12 + DstBlock "Joint Positon Error" + DstPort 1 } Line { - ZOrder 2594 - SrcBlock "J_l_sole" - SrcPort 1 - DstBlock " SFunction " - DstPort 13 + Name "left_arm" + ZOrder 44 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 2 + DstBlock "Joint Positon Error" + DstPort 2 } Line { - ZOrder 2595 - SrcBlock "J_r_sole" - SrcPort 1 - DstBlock " SFunction " - DstPort 14 + Name "left_leg" + ZOrder 45 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 4 + DstBlock "Joint Positon Error" + DstPort 4 } Line { - ZOrder 2596 - SrcBlock "JDot_l_sole_nu" - SrcPort 1 - DstBlock " SFunction " - DstPort 15 + Name "right_arm" + ZOrder 46 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 3 + DstBlock "Joint Positon Error" + DstPort 3 } Line { - ZOrder 2597 - SrcBlock "JDot_r_sole_nu" + ZOrder 47 + SrcBlock "Sum3" SrcPort 1 - DstBlock " SFunction " - DstPort 16 + DstBlock "Gain4" + DstPort 1 } Line { - ZOrder 2598 - SrcBlock "pos_CoM" + Name "CoM_Desired" + ZOrder 48 + SrcBlock "pos_CoM_des" SrcPort 1 - DstBlock " SFunction " - DstPort 17 + 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 2599 - SrcBlock "J_CoM" + ZOrder 51 + SrcBlock "w_H_b" SrcPort 1 - DstBlock " SFunction " - DstPort 18 + Points [77, 0] + Branch { + ZOrder 52 + Points [0, -40] + DstBlock "CoM6D -> \nCoMXYZ2" + DstPort 1 + } + Branch { + ZOrder 53 + DstBlock "CoM6D -> \nCoMXYZ1" + DstPort 1 + } } Line { - ZOrder 2600 - SrcBlock "desired_pos_vel_acc_CoM" + Name "basePosition" + ZOrder 54 + Labels [-1, 0] + SrcBlock "CoM6D -> \nCoMXYZ2" SrcPort 1 - DstBlock " SFunction " - DstPort 19 + DstBlock "Base Pose" + DstPort 1 } Line { - ZOrder 2601 - SrcBlock "KP_CoM" + Name "baseOrientation" + ZOrder 55 + Labels [-1, 0] + SrcBlock "CoM6D -> \nCoMXYZ1" SrcPort 1 - DstBlock " SFunction " - DstPort 20 + DstBlock "Base Pose" + DstPort 2 } Line { - ZOrder 2602 - SrcBlock "KD_CoM" - SrcPort 1 - DstBlock " SFunction " - DstPort 21 + Name "left_leg" + ZOrder 56 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 4 + DstBlock "Desired Torques After Saturation" + DstPort 4 } Line { - ZOrder 2603 - SrcBlock "KP_postural" - SrcPort 1 - DstBlock " SFunction " - DstPort 22 + Name "right_leg" + ZOrder 57 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 5 + DstBlock "Desired Torques After Saturation" + DstPort 5 } Line { - Name "HessianMatrixOneFoot" - ZOrder 2604 - Labels [0, 0] - SrcBlock " SFunction " + Name "left_arm" + ZOrder 58 + Labels [-1, 0] + SrcBlock "Demux7" SrcPort 2 - DstBlock "HessianMatrixOneFoot" + 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 "gradientOneFoot" - ZOrder 2605 - Labels [0, 0] - SrcBlock " SFunction " + Name "right_arm" + ZOrder 60 + Labels [-1, 0] + SrcBlock "Demux7" SrcPort 3 - DstBlock "gradientOneFoot" - DstPort 1 + DstBlock "Desired Torques After Saturation" + DstPort 3 } Line { - Name "ConstraintsMatrixOneFoot" - ZOrder 2606 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 4 - DstBlock "ConstraintsMatrixOneFoot" + ZOrder 61 + SrcBlock "tau_star_afterSat" + SrcPort 1 + DstBlock "Demux7" DstPort 1 } Line { - Name "bVectorConstraintsOneFoot" - ZOrder 2607 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 5 - DstBlock "bVectorConstraintsOneFoot" - DstPort 1 + ZOrder 62 + SrcBlock "tau_star" + SrcPort 1 + Points [41, 0] + Branch { + ZOrder 63 + Points [0, 150] + DstBlock "Sum" + DstPort 1 + } + Branch { + ZOrder 64 + DstBlock "Demux8" + DstPort 1 + } } Line { - Name "HessianMatrixTwoFeet" - ZOrder 2608 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 6 - DstBlock "HessianMatrixTwoFeet" - DstPort 1 + 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 { - Name "gradientTwoFeet" - ZOrder 2609 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 7 - DstBlock "gradientTwoFeet" + ZOrder 68 + SrcBlock "Selector2" + SrcPort 1 + DstBlock "Demux" DstPort 1 } Line { - Name "ConstraintsMatrixTwoFeet" - ZOrder 2610 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 8 - DstBlock "ConstraintsMatrixTwoFeet" + ZOrder 69 + SrcBlock "Gain1" + SrcPort 1 + DstBlock "Demux3" DstPort 1 } Line { - Name "bVectorConstraintsTwoFeet" - ZOrder 2611 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 9 - DstBlock "bVectorConstraintsTwoFeet" + Name "base linear velocity" + ZOrder 70 + Labels [-1, 0] + SrcBlock "Demux" + SrcPort 1 + DstBlock "Base linear and angular velocity" DstPort 1 } Line { - Name "tauModel" - ZOrder 2612 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 10 - DstBlock "tauModel" - DstPort 1 + Name "base angular velocity" + ZOrder 71 + Labels [-1, 0] + SrcBlock "Gain2" + SrcPort 1 + DstBlock "Base linear and angular velocity" + DstPort 2 } Line { - Name "Sigma" - ZOrder 2613 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 11 - DstBlock "Sigma" - DstPort 1 + Name "right_arm" + ZOrder 72 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 3 + DstBlock "Joint Velocity" + DstPort 3 } Line { - Name "Na" - ZOrder 2614 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 12 - DstBlock "Na" - DstPort 1 + Name "right_leg" + ZOrder 73 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 5 + DstBlock "Joint Velocity" + DstPort 5 } Line { - Name "f_LDot" - ZOrder 2615 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 13 - DstBlock "f_LDot" + ZOrder 74 + SrcBlock "Selector1" + SrcPort 1 + DstBlock "Gain1" DstPort 1 } Line { - ZOrder 2616 - SrcBlock " Demux " + Name "torso" + ZOrder 75 + Labels [-1, 0] + SrcBlock "Demux3" SrcPort 1 - DstBlock " Terminator " + DstBlock "Joint Velocity" DstPort 1 } Line { - ZOrder 2617 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 + Name "left_arm" + ZOrder 76 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 2 + DstBlock "Joint Velocity" + DstPort 2 } - } - } - 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 + Line { + Name "left_leg" + ZOrder 77 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 4 + DstBlock "Joint Velocity" + DstPort 4 } - Branch { - ZOrder 446 - Points [0, -45] - DstBlock "Compute Desired Torques" + Line { + ZOrder 78 + SrcBlock "Demux" + SrcPort 2 + DstBlock "Gain2" 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 { + 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 420 - SrcBlock "Compute angular momentum integral error" + ZOrder 1 + SrcBlock "ON_GAZEBO\n4" SrcPort 1 - DstBlock "Momentum Based Balancing Controller\n" - DstPort 10 + Points [100, 0] + DstBlock "Visualizer" + DstPort enable } Line { - ZOrder 421 - SrcBlock "w_H_l_sole" + ZOrder 2 + SrcBlock "From1" SrcPort 1 - DstBlock "Momentum Based Balancing Controller\n" - DstPort 11 + 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 422 - SrcBlock "w_H_r_sole" + ZOrder 8 + SrcBlock "From7" SrcPort 1 - DstBlock "Momentum Based Balancing Controller\n" - DstPort 12 + DstBlock "Visualizer" + DstPort 2 } Line { - ZOrder 423 - SrcBlock "J_l_sole" + ZOrder 9 + SrcBlock "From5" SrcPort 1 - DstBlock "Momentum Based Balancing Controller\n" - DstPort 13 + DstBlock "Visualizer" + DstPort 1 } Line { - ZOrder 424 - SrcBlock "J_r_sole" + ZOrder 10 + SrcBlock "From12" SrcPort 1 - DstBlock "Momentum Based Balancing Controller\n" - DstPort 14 + DstBlock "Visualizer" + DstPort 5 } Line { - ZOrder 425 - SrcBlock "JDot_l_sole_nu" + ZOrder 11 + SrcBlock "ON_GAZEBO\n2" SrcPort 1 - DstBlock "Momentum Based Balancing Controller\n" - DstPort 15 + Points [165, 0] + DstBlock "Visualize eventual QP failures" + DstPort enable } Line { - ZOrder 426 - SrcBlock "JDot_r_sole_nu" + ZOrder 12 + SrcBlock "From9" SrcPort 1 - DstBlock "Momentum Based Balancing Controller\n" - DstPort 16 + DstBlock "Visualize eventual QP failures" + DstPort 1 } Line { - ZOrder 427 - SrcBlock "pos_CoM" + ZOrder 13 + SrcBlock "ON_GAZEBO\n1" SrcPort 1 - DstBlock "Momentum Based Balancing Controller\n" - DstPort 17 + Points [160, 0] + DstBlock "Desired and Measured Forces" + DstPort enable } Line { - ZOrder 428 - SrcBlock "J_CoM" + ZOrder 14 + SrcBlock "ON_GAZEBO\n6" SrcPort 1 - DstBlock "Momentum Based Balancing Controller\n" - DstPort 18 + Points [135, 0] + DstBlock "Feet Status and Gains" + DstPort enable } Line { - ZOrder 429 - SrcBlock "desired_pos_vel_acc_CoM" + ZOrder 15 + SrcBlock "From4" SrcPort 1 - DstBlock "Momentum Based Balancing Controller\n" - DstPort 19 + DstBlock "Feet Status and Gains" + DstPort 1 } Line { - ZOrder 430 - SrcBlock "KP_CoM" + ZOrder 16 + SrcBlock "From8" SrcPort 1 - DstBlock "Momentum Based Balancing Controller\n" - DstPort 20 + DstBlock "Feet Status and Gains" + DstPort 2 } Line { - ZOrder 431 - SrcBlock "KD_CoM" + ZOrder 17 + SrcBlock "From2" SrcPort 1 - DstBlock "Momentum Based Balancing Controller\n" - DstPort 21 + DstBlock "Desired and Measured Forces" + DstPort 3 } Line { - ZOrder 432 - SrcBlock "KP_postural" + ZOrder 18 + SrcBlock "From11" SrcPort 1 - DstBlock "Momentum Based Balancing Controller\n" - DstPort 22 + DstBlock "Desired and Measured Forces" + DstPort 1 } Line { - ZOrder 433 - SrcBlock "Momentum Based Balancing Controller\n" + ZOrder 19 + SrcBlock "From10" SrcPort 1 - DstBlock "Compute Desired Torques" + DstBlock "Desired and Measured Forces" DstPort 2 } Line { - ZOrder 434 - SrcBlock "Momentum Based Balancing Controller\n" - SrcPort 2 - DstBlock "Compute Desired Torques" + ZOrder 20 + SrcBlock "From6" + SrcPort 1 + DstBlock "Visualizer" DstPort 3 } Line { - ZOrder 435 - SrcBlock "Momentum Based Balancing Controller\n" - SrcPort 3 - DstBlock "Compute Desired Torques" + ZOrder 21 + SrcBlock "From3" + SrcPort 1 + DstBlock "Visualizer" 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" + ZOrder 22 + SrcBlock "From" + SrcPort 1 + DstBlock "Visualizer" 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" + ZOrder 23 + SrcBlock "From13" SrcPort 1 - DstBlock "Compute angular momentum integral error" - DstPort 4 + DstBlock "Visualizer" + DstPort 8 } Line { - ZOrder 533 - SrcBlock "jointAcc" + ZOrder 24 + SrcBlock "From14" SrcPort 1 - DstBlock "From tau_QP to Joint Torques (motor reflected inertia)" - DstPort 2 + DstBlock "Visualizer" + DstPort 9 } } } @@ -10905,7 +10939,7 @@ Model { RequestExecContextInheritance off System { Name "Dynamics" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off @@ -10921,7 +10955,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "522" + ZoomFactor "308" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -12234,6 +12268,15 @@ Model { } } } + 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" @@ -12245,7 +12288,7 @@ Model { RequestExecContextInheritance off System { Name "Joint Torque Saturation" - Location [-75, -1417, 2485, 0] + Location [43, 23, 1792, 1120] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open on PortBlocksUseCompactNotation off @@ -12261,7 +12304,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "283" + ZoomFactor "244" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -12621,7 +12664,8 @@ Model { Ports [6, 9] Position [285, 338, 445, 632] ZOrder 548 - BackgroundColor "orange" + ForegroundColor "blue" + BackgroundColor "cyan" Priority "-10" RequestExecContextInheritance off System { @@ -12642,7 +12686,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "166" + ZoomFactor "175" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -14903,14 +14947,15 @@ Model { Ports [5, 10] Position [-275, 2597, -65, 2883] ZOrder 506 - ForegroundColor "red" + ForegroundColor "blue" + BackgroundColor "cyan" Priority "-100" RequestExecContextInheritance off System { Name "State Machine" - Location [-75, -1417, 2485, 0] + Location [43, 23, 1792, 1120] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] - Open off + Open on PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" @@ -14924,7 +14969,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "200" + ZoomFactor "142" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -14965,14 +15010,6 @@ Model { ZOrder 956 Port "5" } - Block { - BlockType Clock - Name "Clock1" - SID "2166" - Position [-150, 295, -130, 315] - ZOrder 532 - ShowName off - } Block { BlockType Demux Name "Demux1" @@ -14983,6 +15020,17 @@ Model { 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" @@ -15518,14 +15566,6 @@ Model { } } } - Block { - BlockType ZeroOrderHold - Name "Zero-Order\nHold" - SID "4995" - Position [-110, 295, -85, 315] - ZOrder 959 - SampleTime "Config.tStep" - } Block { BlockType Reference Name "holder 5" @@ -16094,13 +16134,6 @@ Model { DstPort 10 } } - Line { - ZOrder 408 - SrcBlock "Clock1" - SrcPort 1 - DstBlock "Zero-Order\nHold" - DstPort 1 - } Line { ZOrder 500 SrcBlock "wrench_leftFoot" @@ -16181,8 +16214,8 @@ Model { } } Line { - ZOrder 517 - SrcBlock "Zero-Order\nHold" + ZOrder 561 + SrcBlock "From" SrcPort 1 DstBlock "STATE MACHINE" DstPort 6 @@ -17654,7 +17687,7 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 78 + $ObjectID 83 $ClassName "Simulink.Mask" Display "disp('EMERGENCY STOP')" RunInitForIconRedraw "off" @@ -18618,8 +18651,24 @@ Model { 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 Reference Name "RobotVisualizer" @@ -18639,7 +18688,7 @@ Model { Name "Selector" SID "4963" Ports [1, 1] - Position [840, 488, 880, 512] + Position [840, 473, 880, 497] ZOrder 1280 InputPortWidth "6+robot_config.N_DOF" IndexOptions "Index vector (dialog)" @@ -18651,7 +18700,7 @@ Model { Name "Selector1" SID "4975" Ports [1, 1] - Position [840, 458, 880, 482] + Position [840, 443, 880, 467] ZOrder 1281 InputPortWidth "robot_config.N_DOF" IndexOptions "Index vector (dialog)" @@ -18663,7 +18712,7 @@ Model { Name "Selector2" SID "4976" Ports [1, 1] - Position [840, 428, 880, 452] + Position [840, 413, 880, 437] ZOrder 1282 InputPortWidth "robot_config.N_DOF" IndexOptions "Index vector (dialog)" @@ -18675,7 +18724,7 @@ Model { Name "Selector3" SID "4978" Ports [1, 1] - Position [840, 398, 880, 422] + Position [840, 383, 880, 407] ZOrder 1283 InputPortWidth "robot_config.N_DOF" IndexOptions "Index vector (dialog)" @@ -18697,7 +18746,7 @@ Model { ContentPreviewEnabled on System { Name "Subsystem" - Location [-75, -1417, 2485, 0] + Location [43, 23, 1792, 1120] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open on PortBlocksUseCompactNotation off @@ -18713,7 +18762,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "244" + ZoomFactor "152" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -18977,16 +19026,6 @@ Model { } } } - Block { - BlockType UnitDelay - Name "Unit Delay" - SID "4997" - Position [1130, 483, 1165, 517] - ZOrder 1284 - InitialCondition "zeros(Config.N_DOF,1)" - SampleTime "Config.tStep" - HasFrameUpgradeWarning on - } Line { Name "" ZOrder 308 @@ -19047,15 +19086,15 @@ Model { ZOrder 321 SrcBlock "Bus\nCreator" SrcPort 1 - Points [19, 0; 0, 183; -1023, 0; 0, -153] + Points [19, 0; 0, 161; -1039, 0; 0, -146] DstBlock "Bus\nSelector2" DstPort 1 } Line { - ZOrder 361 + ZOrder 408 SrcBlock "MOMENTUM BASED TORQUE CONTROL" SrcPort 1 - DstBlock "Unit Delay" + DstBlock "Memory" DstPort 1 } Line { @@ -19187,12 +19226,19 @@ Model { DstPort 1 } Line { - ZOrder 405 - SrcBlock "Unit Delay" + ZOrder 407 + SrcBlock "Memory" SrcPort 1 DstBlock "Subsystem" DstPort 1 } + Line { + ZOrder 406 + SrcBlock "Clock1" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 8 + } } } #Finite State Machines From 632212ea022efb999cb82f0599d5f5e52a552587 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Sun, 2 May 2021 07:58:14 +0200 Subject: [PATCH 05/20] Fixed visualizer configuration and removed neck joints from controlled joints in simulation --- .../app/robots/iCubGenova04/configRobotSim.m | 4 +- .../images/initNeckPosBlock.jpeg | Bin 0 -> 14107 bytes .../src/initVisualizer.m | 23 +- .../torqueControlBalancingWithSimu.mdl | 388 ++++++++++-------- 4 files changed, 240 insertions(+), 175 deletions(-) create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/images/initNeckPosBlock.jpeg diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m index b8af3d8c..f8309010 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m @@ -16,7 +16,7 @@ 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.ControlBoardsNames = {'torso','left_arm','right_arm','left_leg','right_leg'}; %,'head'}; WBTConfigRobotSim.ControlledJoints = []; numOfJointsForEachControlboard = []; @@ -26,7 +26,7 @@ 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'}; +% ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{6}) = {'neck_pitch','neck_roll','neck_yaw'}; for n = 1:length(WBTConfigRobotSim.ControlBoardsNames) WBTConfigRobotSim.ControlledJoints = [WBTConfigRobotSim.ControlledJoints, ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{n})]; 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 0000000000000000000000000000000000000000..2d1b18ebad32fbf569f32e7b4b163eb54fb0dd8d GIT binary patch literal 14107 zcmch;30P8X*EYOyLQON%95SWSv@+*82gC^x6&w(U%p4IFaRwE^EH%?Kt@O^yu~IWN zb1E}u(?%`FoW({>h3+y^gpd1q-uHWt=lJgDKmPwYj_+S0djos3_qwiit+mc`Ex1$M zMPQGE9l{O(fdIgs_Xlv7z+_u8J_G=qoPgs100;x1OgtdK+v6Pq?C1RhfIvY800sas zgtw0_0|ETJE$FY$%lQ8LNRVn7|9|fTX@8vrbF)Su{@TJ|4vC&&{t+JD0RdKVc6JVW zQT`@L+Fou!CQ*jQae={+?&Qb-H-~WTCE(gm}2UUL3(y z%hSUflcpheUVLPuJW0#QT{2a`xw zSE8a_Q6@x#Ae^CofHmIGEz~A9+%?i9f}|H} z@12P7N2v$s8ydtW=?Cd0X*)UDpkXk$jvFRIl&qIzkHXmNdE!G*uG+fD_#i{CSUqG! zuz|Wsl7U`eh?9Fjpt=o?=pL8k80{Zm9AWPt<>ZQU$C5m-1`g4pk&dxC1P?pASZy4} zFdXM)O$^jV5y+vDm}tDa6Vcf%$O-3ZPqH#F#<|#=MB&4*_Kwybao)O4c-JroD-Y42 z5CivUlr;{c7vvQW$9Y9zk^We+7dgo_!GmW*UIDs#h$v?Q$_o?c=4@@?VH>1t6C3Rn zi?PGv9K5sG;F($e4HsM3ig1eyEP8 zt8N@l%gNI-(Ml%}rsHlz^t6d`uyVz?X=7u(kz~>Eh-h_hv{h_`4Hh40?Lmmel60)m zWMm*g$Jo`#kQf$f9gU4Oz(lyZMu%ct<89;NXyY&!Bkdr!7#jr2RTO0qpy%r3qziY@ zxAJ!n^Gb+C>DqW;+^uX9qDg@Up*U~eDhUX)h7t5UY@F<{b|e$He}q?{gO_cPldeO8 zXppXU5W&OQ+Y{{{6cl8y3wO6RiU`%#(ZwU<<6-)Go-r7*ZU{yT7pmuF5aj5Q;AjVn zw~4_K-6BcoAe@b8cnA@L3n9i^VZDOG^`oNvlRRwH;rhBdPKcyLcO%a@TYnOPgm(>& zhI>UuMxg@ztqhQf2@b}Q-ob$;7;DiOefJ2fnAk9Tukhdmy+n*-fR1sPwN4mPD;^)~ zY7ne$?Hy+9VUXadXM)E$nS^PFMOq_u4Ru|Oy$o>1>S3Y@aBJNlxQ=eHJ{nCn@K3Vy z)Uo$)!8#)GTA_)-wn&m5J~3R|#63*Q)-A!q*v$yzXpJIMc#1kBJrcc5 z$ar*=QB({fUM~oxqmyJ1g9r{mx#_tlMv<*Nv_p)-bhV>UdNF!hNS#Ok;hc4?9gUFQWP^BEXk45@l#P)QZyekm+`OVa48lD8Ba98P zaAU*p7;EfacD;z!V~9`fb$NqLaM`bjA7pTqSi(vXFZ&K0?tbd=AaHs zGKP8T+s2XpdUkMv5!7Kp>eg6wn5Qw%ZNRih&IZP4I}E}mz{@Hj&?X=*RMZX~U>y>M zgSl(lIv6Iv0v(;4$m%w(#&%JL_DROJj>c9F#`YeDHb%xMw7!d;K01VC#j~megqw+f zScE@b)H}un9)oeR)+M@HM_cLW5M9tjlufjQel!kkkwV1kZ`OPF-gnSQymu)!q+I;Lhohx+ z&*qm4D_b@8>~np~P|*vBNfQy3k(HC*e?(R7=&|Ga28Kq)CMT_JZ0+Fo2nRQJj0e`! z%R4YAI0P4p4~vb9Pau&Klg^z_&&a%xb+O>e)oX=Ce_SuFsJwmWZdG;7y@yTBEv;>j z9=G@Q^$$E79D4rZ-RRi(#N^cb=_UH|hmW5=fBCw?{Ql>UpPRo}zqkINv>#Z17TuKRW(m31^>DJ= zFWZlwUWGqOi+qnyaz9uNzk>#SjnDO`ljm}s(jy1hoNf{Pef`S3Z*}`6+7Ro+0!ldSh;%D z5>n*H=!Ud{ca)%6T@0BGE>IW;dddX^nwgXR3vy&PddYPrbYLqlr@_pCP8vP)WUQ)y zB-tnD*7OQ@^nPE$h_gtU#Q5=lxh1F+EmVOx4PMq_OCR4}1o!3TBrWv$9)^nhtb}jM z;|c`HBSgM=lSPG_T^di?tSyz8jLFR-JJlTuOmJVx2_|&SjSi9ey5D@~>D>G^^QSKc zQp7YB*JfMG52s%iuR7WXE-#a4?>tNiXTca(zAe*fIY07tJzbm=NV@dR@Vi50+S_N} zSaEc3Or1@;*9|e>LIs!FQT5%1=humD|q_tqA1nX~Ls#=KcWvCTe{H zZ~Hx0e$EBNv+0;wM0ayLfP292Rwg#gQrm1{7QuI_EQ~2KtUF9)4_2ncS$b32{IW}N zh}D{MWd5N7FUrI2)Tqx@-T-%G+ms`s#syS;mNvFUR=T!a$3R=YmP**=(p*0w3%Qkq zcCCIt#n2M@F|8>7wy~C7$zHymi`ocZpGew;oa7O(pZ~I!NLc%adHHugYfWE!PfsX( zB^k%z5iwK{}1cn|IWJ?7#C22Dsq8uhr6MdR|4b*{L_9?OjpQXBJ(EykGX{y3M7)yFPF!czERNQcdlkqgKnu zvv1NnuZ&h8!5y2M#JC_Tjp=pD8gS2fW zO)l_5$`Vqq|6iv2&kYqmMSBs@0@;=<;sTbrsZiTa3)^UDcf*B|=>sUWjGys2T%bAP z9Q4Yxk^miZ75a>w5K;@vSb1QupI|_U{m$jr0@4zdUeSlr!r} z2ng9<>m_6~bMf<<+4Ac*Y|7(7X;3<&su$8!Iv~N%KFSiTVav9*-58h$>&>lBb}IUS zrKYE*&l%iWwaL?e^{2eph2PKQ+FlQVeVEbl8`A7Z7HE49E7LQ-LtQKLh6mHy>&eU| zLflJIhPp?TbxN_K4 z&NS9_ynQk8uBDxC;`=>9=zA}{md$nBgZv+c^Pr3x&Vj$Ch^;&I+=4}p^w$14x2X8AQDGla@7|$c;FrYv(Y~%-v~ZtQ==86_>6F~3^xA+e z-;5$N(3RfRU{mOO#Uto4Bcb45vHlOE&b3QcQ&Ec{_RlZr!3fR%io9rAchM_ckH`EZleF{^+j$%85$es zDb|&&v|V|7Rxgv2Hu{RmjID+(OxU95e>ZLpIY>;;M}jzX7yKg3sh+ue?FVKo4EOCr-&-UHrvVL=&=S} z7;p0Kkry!S;|9-LCL6Oh&bQ~@k5u;jobT<~?_%Ew6 zt8Gz2>Op+;_iilQeBM@y_;8IyO1+`@m7Wkz`})$4daY;hR_0S4fNE zMlLi=BD*!8T!bm=$8JG0zili_O@}67F6XEuw0_UOuIfuBUyk->EK zk-O}J%#yB((1b0*$H|=dm$64%JtJ+Kb;~lq#=B(>b?4kX_C_ey5^|T9H1I;IKp7Xv zgYaC`rkO7n_@bH$Jr(*d`58O!0T;MB5C+{;*5aI11gK4?-+QlkVE3o|2nY|aopz`s<6eEQCiQpg`OVE%<@WBxwXg@SnwiMEZLlVIx-t*H=Ck)@6>kjd~Xo<^?mAIK2-KoKnVn+H+T|w;>vo)tR zxIoq?#_P6BtqrY>dcyq*zR~B!bk*E{J!6Ha-jGSMc1)H?!;%W*N~X zz~P1MsDX`htuhRsu$cQ-@LO5Q(K40cab+qQ^Ix;`54xx%XCMENZ>dz-Kqf604XkO- z^qjVM8%G4Q5)#j(gdi1N39g?uD zEm|{1u$}Vz5flS1Al#W%fm|AHLf)I#EI(2=&~tr*?~+M{m%E{igm!J9Q1IL}U>FK; z4B5szwFX_h2*@|uH$ql;*4nz<->zC-?K@-s>5hWMnw4PMGxE|O#{l-LBc?CcaAIXj zt4yh0T60Jv-L8Wb%#^TUOk4=Cl_}yVU2*-xLGS6SRa&I>tUsmG+zFseZo}BX&b>}e zwi#nE?Jg61o^4$dTB7=pDP4T4_jD-!<5%`TNMY1qb)@FN^(wu3A!!@xPauU0RKBcz zeaG9WVFngl(_tTZrsY=snOs+k_*~pYp?uh)p3V7Ohuo_mCh#6sl=sWp`8z)*+GI6MeV5`UvQ*HPdx*itJ%2>LNF|lFg8LvByA-IqRu{mW z{XSo+%`k+`$7|+rIMyIFEBEBj7NY`Bw^)TK-qmeOjC!U@{b7z{LZ5(f~p@of6nl?>dwTJ5T-+@k+wD3|C0&}Iri zh0}VDLYD`ZyLR$qXHG8}y@&cizs0mt3F>&dbgI3sz9!)WOp4mlvk1 zIPG4vFCnQ=?%3bEmj6K_|B}`HH$3_4O-TL%!$E?@WbrU+;J^YWEtLP~vws1>5<}Y( zza2d)xj=v}&k;+GfLGK1iEw!5HhyN`HrlM=0`-M{Tk>TR-0}av?}QpkQDRjx!#Agy zrMYJC#V&=@?2|0`lA?o?lw*p_{Oh0QA?y1jj=U-|8aZ{_MKCVdepw(Bl@?twc#0UuJ5SCsv7v_jVqXVZI-(e((S4vahwUXU7-z<0$663`rAZ6*TyZ zIdAg*k13e=d&MEL)G0`d?LxOHRE#CDw0;eeWpR{k^JG6Q^T&IIJ-2gTLZ<5zF88XC z&_Z^9mi}?P=_vTkLi6w?E`Z;dnp}L^_3SBEy0Zb)KGUFji_sW8=EYhCf~gA zuy0>)O0lKf;E7z-934X45U?Z4QZ?h{m7=sBR|>o{1v%gry6L*o)UM+A>tSyL^!fe@ z@p^|;>8O)vaP>by>c24!@COom?>UYc)szcNF-bfVECzO>c?5|?1Z!X$#ey$3WNCEh zG?0A;lI8}+@6Ba3JMI4L(u%>w36I(sih!&fJ|EnJa zc0A~6&hn=f5Q_GLQE*-!u)|9%$Z5v3AdpI+IP^li8Zky@-|&ZuRI?|W9qlQ~9MP<8 z!GKG@=cqh@zrX0okBdtprrE8Y4U%mvDe0q*3sI!ENt z{=se8FODLrPHvdMLKXhaG-Ghta@n$pIigHr^f>Oqc_MY3 z+=89=Qs34j)_Yc4ld~3bmM)i6ri#!OD^9LoVU}*5Wr4R3eI-RNnE$#EFk!ImIg<C7s!V0VPnC)S~ApOrDG#`>C_`(l2iKA8P^R~ z@4I^W`lVDTE6#N)dW=_u`oll2;$SFuMAsKm8D+gNNjb8PI2?7a`46xs*U0BXaX)Z|QEf zWcAAmWc}>4qSdy?*xI<`DsC4-8Vv)T5i2-x*YQ#rWyaF5Lt_~=eHfDGJ=}wmp9}qt zf6HKM4R5C%?Vlb|SC#Q$<aj>)C;jQ}YUmJk6XQ)~0$ zI5a{+k~-{-x)`S*zIV6}Nl!*zk2_Ycxz}Wa`l|3DsM@X`9dzApZei9NSmW?hN2of? z(r12I3r(NwR_nE>eF2@hb02Y+mh;EeQeQ+ZL=UNj zf;F~JP`p{6nLc#zfChxVyP^E~ZMG3paUs*y9EvEJP97Cq_sJ+kzAv;8e&w&w^?GDP zXmIT%Gn?Ml4LL_(`YgpyTvZt*Y7puP2FPQPg^PZu1H#W$u-bC3g0l|RuD`K36$AQ7 zN?$WW2A0owv-(_H$A1RRefIVCHptNp&hmYrKPjmF2&`*pKP=x3eBsf%5<@hW%6L00 zkU0}*GI%Lc>Qh4G9z%&&C^fT^s+=olEg=oO;0S)r8zyab0EVFWZfyf4aCJ2VJc56W33XzW$9PM~*j8^j%iye0?;zrUGs* zv{^hA`<_)MF?A7OXYAw>-a;2EAT8Fl7B+>mvH=!}qyuU8i+M%cm1oZm0JMM0+Jcx3 zi@2VyEUB+lGuEX~q3sRl`&x=FoM@464quh~_}kZZts`IiQrjNP9}TafKxzjeSNxhh ziWqtxJl?8=)#kwv=adqchWYT~!FNs}s#ETzV6hz{;{9FtM}>&`kGPVLj_Gq3gh4Z@ z0Mw47M)6_!RZu5NcTslLv+_`F6Pj^#Nh8xpGye$!><1TkXnj!aV{)lR|IsO3$#jAI z%Myj(IMS?zO~OuXry@mz(NfdSQeW1Ro+oF*PZj7Zx|%~=eCo`NyVIhzZtN`pM0;TB z%G!L?2_;>YI&C|VBF@;zV`Hm4Jwua-7p{0N)Y5P_!0k=g7N6_%V%0YM15x#pMFLUQ zVyQ_By|jxo{!STILr+&z-nXC2zFN=PlysSzjWx4_;fXsnJGbtd$$FpX1iy-XLFqX> zxL3d4_t#$c#bV$CG;j@Nzms3j#&7FWBE}9e=6aZd>rG2@KInYHH-o2!tV4dPpM0w) zwXa&l*|z262m!^wwp3r~{$`>qtKsxSocgv1bwA6;5!b;~F!nLgV)wqgEt}r2C3s(_ z@X_VqeREiHP&@!BfHwJcZ)DP1B+gQGwKCc9N2>Z2^#^|YlkXl_x+)Ydf~~>bEO%tSOhL$)sWl8$eD!x-kWOSFcx2Wu5=G?h27o1dG-X?i zj@rrKF(vghbD8k;xw^245|vo#-QO2ckmB6Rh|7NX$^DW2mJ$%P+Y)%o(u&{$a!`WA z0nTJ=*aS`NhlE&FMqG;P2L*Dx9m0dY@?9vZyv|?!aAMWRh#FU`){))dO|zX!3lQ0D z5Yn7mM<-sGhrGN069w^G@Iu7a%viN0+&}9Pp0aNCWvXWS?K-&4B;7ULXLDdrakBWcFf(j+ox08WJn|MSb}b?5TE@Wu^fKD4)%e6 z3=O`4r!_Tv+b&;8@Se|N4EAnOEAPIzGCe!-2gyu1g<@U3jA2DTdP|O)vnaJC{DxS;e>Pi5V3VZ>|!2c{4fjp23$LcFO z3+Ig~T|M+vD zW0&3;x!u<^lE&^UYFP9+=6Y~S`{%wHqYmNsNqu9!^E5raPjTkizYjOyuQ(Ut2u;-N}P~Jk-^K+KJ<>?2*^J@bqtJIYEFj$MoAQVL0e#&On&Gx>fm+PzDM5&%oIKlqLDUTl8_< z_Eq!HT29xNKpbc*d$~(WD~&BdhsbpV1dlYYKKXurQM)Dm&P>Vs8D8if;e}Dy45FI) zhI)i`?q1t;{ID#!!KI_Kof>q#KPvB!U%y|rjpx+?F%ZR{HJx%i*4>jMzPhEVvCJIK zoJ5RETDZii6t1i2CS40o&D;9W4BFM2 zD0T6ncmKNw;8_iTisfDMXq>s<;AtgR{x+7(&K{Pd*iWja&S`JACX{>L_S(+BezOeL z?TMxdcMFkvYFGVu^>)_<9&Nn;V_V@bI15>MH$AfTfq6d{2p2tk6@CC#f)Ght!$`}R zrls- z0%usH%_@boIMgXca!ClGw5KZsA*TpUQsy~cd9 z6w<(`SXj=X9%^f5m=83}VBI?A@Oyingq(7gyj=L(>VcRKAa?mE|7%|1F1+FV7Ah0E zWWwB6|FoQCOV@+fhwWGV*>XNBr+k(vqP#as8Sf%vg+Uhv3-B#mf`)6IE0ri`5C$)* zjR)inGQFNx7<68uCA1aziI4bH&cF`ul%EAZf5U-26E54`MyCyKWJ18rrMts8X2l#{ zu!&bJyd~IqBQcR=FxGh81JdMUt5SOiO?55%5XfVJd`5|tV?VZi9ppY{m_91NAePC& zE{8Mt>}SDt@_p(6Y>`)Ndd1PEgfrB-OT|j%+3@65-@UpvlL^J~b8L^rJ^apufT91=&2d*koS$DX9;q{5*Z}y>V0ZN75sgvu>Zypko9@1o)yt`Ko#w zF3}`94JhUiDb4{ui@I|CqG)ig>YvE>LvM#F4(=*&_z1`cABWFNgNE1!J5?NM_9+W> z6wp~$?X*~VJgm_oyL#rOV86*i#NJTJk`HG%A3;~3eU`G+z0`NmT~y<*7KbJ{3aFYe zkJED3*BVNkRJW$;UODi(7$?Y*w{Hm<^`E^(I;wm^7u3a|EfZ5YCqs=+tFaP~{MaJg z>6_M{Y>)p`vOhEnA)`Z*wnJ%*RbCy!3OWl907>HjkJb2($|d>i@OPRN7=t)iEMBaC zWyM>lhOob39#=eJgEoI|h6aD`VoOf1tCzkPS;|S9=sY}DNej{GQI%Mvrk6a>EiW zy^7J#k>I^^x6YrwI#a`$48D7LQulYe>x^{C3iQ?$qhpfB;+M58)oDG4I%KP-sBe;n z2mJO7{qi2{Poe%o7p%BlSGzd1b5pPJW7e%mxss37{JCRnL3+da)AH}2god*It0YZgE1diFRkvJToH#;4YLu4ga5dB zF6WS%v2mHiYpofPGRE+^uAl}MHsK1ZKwR$`^?k7P+vH29Q$hltAbEIXwwkmLd|0Bv4eRI0K0$% zU4&jJk!98Jh%&X5iMv`BviNLB)|t-4(luVKJbUhibto>kU{>-#K_qzS&(xlLfisX8 z>R6|WpBP&$yL(#22$8s){fC(mLFQVx#_KVeB2g#y1j9$&-l^q$?9dJArE5v?WfGUU zK#+K{HOqP%LqYG9G;8pjXdg$6`A{v#Oaoty|8>&un&x<>Yg@*y>SaQC>d}1(7wFJb z{tlnzWUZ;~&9}_;C)G9!jBH9Uqq+F!o%SeICM0v&|Hyr1PSPnG(t((F_UA?cB|paS zDJJO~f|*SB>)nv_z2%2tU0-?vk@=G0@rqQUeY5V;$A=J~+TYfKpCcS@C8ttb**XmK z(mqsXrxHW)>fI%M)1!ovgzaBa$MXzHKND}yDVKcf9|6bna?%;kpPCM3g-hROE^78X9eTsq!@t3s z)W6BNCbncbBGJq%OS~?NAoE>T%*?0-#RBPcdaAbBv28=L z*3v>BN{XVv@~>GrE%C0ysoHl%L$ie79j@9LtY;glNvO&?eEf;Dex!@%w=-7PfThj@ zoaaUmkxs=a%5hdQGy7RPG~-9hb&WH&40eG{(~Cciy*|DzIqf@@=BI4@*Dqea#dW<8 zU-Iqh_TN6qDp2p*$MTjHT574WabV|#K9Ro1Qe(t%Y9iJ{QT zV^#H^@}ro~S{mk0k*P9S#1lW0>y(o3<_GRY#^~Q|z6cMLi&VzA`UF&~paqB17x+75 z85zsycE+9grkJf_yMjwt?=gd5zQY5QmZv2y^1pK(&XnklpuxLtQ(t)}D=)#XpuvH~nm-Xbl=SsY{ym$C63 zyv_#2$&89(1&zZdCo&51=jGB*-O3a zt7NXPY&cE3+pWdR^gc|xeL1)0_?euOyZh5XYga)im$A4V2&#R&CA|9n{fLUhbxpPS z+@k@PkY)v@d+yyUD-5s%f=0jvv_3d>*zbqCnFbd)_XNT*wS?T_d1XZMjpWzL%hx{M ztTB^fO@FfgaOs?Kr6%TDrof&U;gzHOKj;6op7?AIH^7l%pxFCnTqfAKIhjuh|oZmO1ri(wC4@%GrW3~X+LBuvLqFnISp_Mx)v`ie1}^crcEV5c8)jzI}iV* z&6xkUCGToaxWMxdD}I|o?|6-PK4|cZg~l%Stvp^ud3*m4+KxJ}_pbFX^;p8CJCxE5 f$Y)E)qXmXF7x;aYCQbXcuZmZi{e6_5`~H6ci?N@M literal 0 HcmV?d00001 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 index 5c2e6072..4615d4f6 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m +++ b/controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m @@ -1,5 +1,20 @@ -%% configuration for the matlab iDyntree visualizer +% 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.040; % 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; @@ -8,9 +23,3 @@ % initial joints configuration specified in configRobot confVisualizer.joints_positions = robot_config.initialConditions.s; confVisualizer.world_H_base = robot_config.initialConditions.w_H_b; - -% size of the square you see around the robot -confVisualizer.aroundRobot = visualizerAroundRobot; - -% refresh rate of the picure -confVisualizer.tStep = 0.040; % here equal to the time step used in the simulink model diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl index 38516187..07c23bea 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl +++ b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl @@ -7,7 +7,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "3.86" + ComputedModelVersion "3.88" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -382,7 +382,7 @@ Model { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [43.0, 23.0, 1749.0, 1097.0] + Location [-75.0, -1417.0, 2560.0, 1417.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -402,88 +402,110 @@ Model { } Array { Type "Simulink.EditorInfo" - Dimension 60 + Dimension 63 Object { $ObjectID 5 - IsActive [0] - IsTabbed [1] - ViewObjType "SimulinkSubsys" - LoadSaveID "4537" - Extents [1792.0, 1120.0] - ZoomFactor [5.1816867469879524] - Offset [-108.23828125, -66.072916666666657] - SceneRectInView [-108.23828125, -66.072916666666657, 345.83333333333331, 216.14583333333331] - } - Object { - $ObjectID 6 IsActive [1] IsTabbed [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [1360.0, 868.0] - ZoomFactor [1.2548419646450668] - Offset [708.83934245941441, 103.12909008770396] - SceneRectInView [708.83934245941441, 103.12909008770396, 1083.8018159399676, 691.72057076168517] + Extents [2171.0, 1188.0] + ZoomFactor [2.0283661825359989] + Offset [713.06692179479285, 156.04796368372479] + SceneRectInView [713.06692179479285, 156.04796368372479, 1070.3195599946705, 585.693061848765] + } + Object { + $ObjectID 6 + 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 7 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4955" - Extents [1360.0, 868.0] - ZoomFactor [1.5228707839211457] - Offset [97.634961569579332, 66.511933656957922] - SceneRectInView [97.634961569579332, 66.511933656957922, 893.0501618122978, 569.97613268608416] + 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 8 IsActive [0] - IsTabbed [1] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4955" + Extents [2171.0, 1188.0] + ZoomFactor [1.25] + Offset [54.774666262135952, 11.467273462783169] + SceneRectInView [54.774666262135952, 11.467273462783169, 1736.8, 950.4] + } + Object { + $ObjectID 9 + IsActive [0] + IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "2355" - Extents [1264.0, 868.0] + Extents [2171.0, 1188.0] ZoomFactor [1.0] Offset [1012.8742726293103, -129.0] - SceneRectInView [1012.8742726293103, -129.0, 1264.0, 868.0] + SceneRectInView [1012.8742726293103, -129.0, 2171.0, 1188.0] } Object { - $ObjectID 9 + $ObjectID 10 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "2163" - Extents [1792.0, 1120.0] + Extents [2171.0, 1188.0] ZoomFactor [1.4201130022789792] Offset [-608.35758121641425, -114.33481638525888] - SceneRectInView [-608.35758121641425, -114.33481638525888, 1261.8714124328285, 788.66963277051775] + SceneRectInView [-608.35758121641425, -114.33481638525888, 1528.7515828078519, 836.55314618872774] } Object { - $ObjectID 10 + $ObjectID 11 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "4553" - Extents [1792.0, 1120.0] + Extents [2171.0, 1188.0] ZoomFactor [2.441659625391233] Offset [-116.92703117366869, -144.85219724177151] - SceneRectInView [-116.92703117366869, -144.85219724177151, 733.92703117366875, 458.704394483543] + SceneRectInView [-116.92703117366869, -144.85219724177151, 889.14932180693916, 486.55430414861524] } Object { - $ObjectID 11 + $ObjectID 12 IsActive [0] - IsTabbed [1] + 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 13 + IsActive [0] + IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "4836" - Extents [1360.0, 868.0] + Extents [2171.0, 1188.0] ZoomFactor [1.2349950019557565] Offset [124.11704673423424, -38.918426238738732] - SceneRectInView [124.11704673423424, -38.918426238738732, 1101.2190315315315, 702.83685247747746] + SceneRectInView [124.11704673423424, -38.918426238738732, 1757.9018510698197, 961.94721283783781] } Object { - $ObjectID 12 + $ObjectID 14 IsActive [0] - IsTabbed [1] + IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "4937" Extents [1360.0, 868.0] @@ -492,7 +514,18 @@ Model { SceneRectInView [155.02749177730379, -243.5, 1360.0, 868.0] } Object { - $ObjectID 13 + $ObjectID 15 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4696" + Extents [2171.0, 1188.0] + ZoomFactor [1.7499999999999998] + Offset [909.16303109605917, -448.321921182266] + SceneRectInView [909.16303109605917, -448.321921182266, 1240.5714285714287, 678.85714285714289] + } + Object { + $ObjectID 16 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -503,7 +536,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 14 + $ObjectID 17 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -514,7 +547,7 @@ Model { SceneRectInView [-29.932291666666686, -42.75, 432.29166666666669, 247.5] } Object { - $ObjectID 15 + $ObjectID 18 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -525,7 +558,7 @@ Model { SceneRectInView [-29.368338160419114, -144.92583198051949, 808.43198882083823, 462.851663961039] } Object { - $ObjectID 16 + $ObjectID 19 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -536,7 +569,7 @@ Model { SceneRectInView [10.776074743527147, -15.071488336727981, 259.375, 148.5] } Object { - $ObjectID 17 + $ObjectID 20 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -547,7 +580,7 @@ Model { SceneRectInView [578.59213941133362, 441.73077216658527, 393.93290867733265, 225.53845566682949] } Object { - $ObjectID 18 + $ObjectID 21 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -558,7 +591,7 @@ Model { SceneRectInView [-25.756466649159648, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 19 + $ObjectID 22 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -569,7 +602,7 @@ Model { SceneRectInView [-709.226772339876, 2332.9648668093268, 1185.7142857142858, 678.85714285714289] } Object { - $ObjectID 20 + $ObjectID 23 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -580,7 +613,7 @@ Model { SceneRectInView [-1148.4886014344261, -284.77258542366184, 1309.1803278688524, 749.54517084732367] } Object { - $ObjectID 21 + $ObjectID 24 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -591,7 +624,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 22 + $ObjectID 25 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -602,7 +635,7 @@ Model { SceneRectInView [23.229550867122612, 135.34665821934539, 447.67371076575478, 256.30668356130923] } Object { - $ObjectID 23 + $ObjectID 26 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -613,7 +646,7 @@ Model { SceneRectInView [94.721007009531661, 154.55463376836479, 629.70491803278685, 360.52503258937384] } Object { - $ObjectID 24 + $ObjectID 27 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -624,7 +657,7 @@ Model { SceneRectInView [-676.15435221738494, -478.27262581168833, 1319.6602669347699, 755.54525162337666] } Object { - $ObjectID 25 + $ObjectID 28 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -635,7 +668,7 @@ Model { SceneRectInView [-92.890717237308081, -373.76948051948051, 862.03143447461616, 493.538961038961] } Object { - $ObjectID 26 + $ObjectID 29 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -646,7 +679,7 @@ Model { SceneRectInView [161.34960569474492, -350.98802908437432, 2075.0, 1188.0] } Object { - $ObjectID 27 + $ObjectID 30 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -657,7 +690,7 @@ Model { SceneRectInView [111.71042989741085, -4.1074743527112787, 672.82914020517831, 385.21494870542256] } Object { - $ObjectID 28 + $ObjectID 31 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -668,7 +701,7 @@ Model { SceneRectInView [434.56854543234, -203.35510808500248, 1485.87853413532, 850.710216170005] } Object { - $ObjectID 29 + $ObjectID 32 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -679,7 +712,7 @@ Model { SceneRectInView [-796.48981681034491, -71.967241379310337, 1329.0732758620691, 760.93448275862067] } Object { - $ObjectID 30 + $ObjectID 33 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -690,7 +723,7 @@ Model { SceneRectInView [-113.1169934431619, -154.52586206896552, 670.79741379310349, 384.05172413793105] } Object { - $ObjectID 31 + $ObjectID 34 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -701,7 +734,7 @@ Model { SceneRectInView [-168.50171712924032, -141.48435660218672, 1137.0034342584806, 650.96871320437344] } Object { - $ObjectID 32 + $ObjectID 35 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -712,7 +745,7 @@ Model { SceneRectInView [87.32421875, 140.1, 207.5, 118.8] } Object { - $ObjectID 33 + $ObjectID 36 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -723,7 +756,7 @@ Model { SceneRectInView [-371.86694504310344, -625.85167594516, 2093.6948275862069, 1198.70335189032] } Object { - $ObjectID 34 + $ObjectID 37 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -734,7 +767,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 35 + $ObjectID 38 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -745,7 +778,7 @@ Model { SceneRectInView [-191.40632054176072, -117.29872084273889, 691.66666666666663, 396.0] } Object { - $ObjectID 36 + $ObjectID 39 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -756,7 +789,7 @@ Model { SceneRectInView [-92.7656132430399, -55.8837471783296, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 37 + $ObjectID 40 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -767,7 +800,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 38 + $ObjectID 41 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -778,7 +811,7 @@ Model { SceneRectInView [-99.198645598194133, -48.972911963882623, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 39 + $ObjectID 42 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -789,7 +822,7 @@ Model { SceneRectInView [-80.667161130818016, -31.015048908954071, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 40 + $ObjectID 43 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -800,7 +833,7 @@ Model { SceneRectInView [13.879095336481726, -85.310064935064929, 502.36680932703655, 287.62012987012986] } Object { - $ObjectID 41 + $ObjectID 44 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -811,7 +844,7 @@ Model { SceneRectInView [13.972911622276058, -85.325665859564168, 502.42130750605327, 287.65133171912834] } Object { - $ObjectID 42 + $ObjectID 45 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -822,7 +855,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 43 + $ObjectID 46 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -833,7 +866,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 44 + $ObjectID 47 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -844,7 +877,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 45 + $ObjectID 48 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -855,7 +888,7 @@ Model { SceneRectInView [-64.168329397874857, -71.912514757969319, 518.75, 297.0] } Object { - $ObjectID 46 + $ObjectID 49 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -866,7 +899,7 @@ Model { SceneRectInView [-684.37478764306115, -290.08334454895737, 840.66666666666652, 579.97212543554] } Object { - $ObjectID 47 + $ObjectID 50 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -877,7 +910,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 48 + $ObjectID 51 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -888,7 +921,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 49 + $ObjectID 52 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -899,7 +932,7 @@ Model { SceneRectInView [203.38965517241377, -277.26034482758621, 524.02241379310351, 361.52068965517242] } Object { - $ObjectID 50 + $ObjectID 53 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -910,7 +943,7 @@ Model { SceneRectInView [-8.0130751303127283, -114.03393582177664, 473.82302526062546, 223.19657177225341] } Object { - $ObjectID 51 + $ObjectID 54 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -921,7 +954,7 @@ Model { SceneRectInView [16.019427876904615, -398.80851543704892, 1030.9142692461908, 485.61703087409785] } Object { - $ObjectID 52 + $ObjectID 55 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -932,7 +965,7 @@ Model { SceneRectInView [-105.54956270048115, -242.68495036561927, 864.60693790096229, 407.2771777265437] } Object { - $ObjectID 53 + $ObjectID 56 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -943,7 +976,7 @@ Model { SceneRectInView [-396.83333333333337, -304.36666666666662, 1148.0, 792.0] } Object { - $ObjectID 54 + $ObjectID 57 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -954,7 +987,7 @@ Model { SceneRectInView [34.9092200413223, -92.857244318181813, 438.7831224173554, 302.71448863636363] } Object { - $ObjectID 55 + $ObjectID 58 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -965,7 +998,7 @@ Model { SceneRectInView [-183.3666666666665, -165.10000000000005, 867.6, 475.2] } Object { - $ObjectID 56 + $ObjectID 59 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -976,7 +1009,7 @@ Model { SceneRectInView [10.41989407940855, -23.235071395084702, 501.20498970990775, 274.51891552575859] } Object { - $ObjectID 57 + $ObjectID 60 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -987,7 +1020,7 @@ Model { SceneRectInView [-245.23623571571773, -105.36577786688051, 666.01934643143545, 313.731555733761] } Object { - $ObjectID 58 + $ObjectID 61 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -998,7 +1031,7 @@ Model { SceneRectInView [-68.684210488152473, -33.394738158739756, 477.63157399976672, 216.05262934094131] } Object { - $ObjectID 59 + $ObjectID 62 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1009,7 +1042,7 @@ Model { SceneRectInView [-211.69612560424537, 280.84982585720286, 605.0, 273.66666666666669] } Object { - $ObjectID 60 + $ObjectID 63 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1020,7 +1053,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 61 + $ObjectID 64 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1031,7 +1064,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 62 + $ObjectID 65 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1042,7 +1075,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 63 + $ObjectID 66 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1053,7 +1086,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 64 + $ObjectID 67 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1069,7 +1102,7 @@ Model { Type "Simulink.DockComponentInfo" Dimension 2 Object { - $ObjectID 65 + $ObjectID 68 Type "Simulink:Editor:ReferencedFiles" ID "Referenced Files" Visible [0] @@ -1082,7 +1115,7 @@ Model { Minimized "Unset" } Object { - $ObjectID 66 + $ObjectID 69 Type "GLUE2:PropertyInspector" ID "Property Inspector" Visible [0] @@ -1096,12 +1129,12 @@ Model { } PropName "DockComponentsInfo" } - WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAFeAAADofwCAAAABPsAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAFeAAAE4fwCAAAABPsAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQBAAAAeQAAA6EAAABrAP////sAAABgAFMAaQBtAHUAb" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQBAAAAeQAABOEAAABrAP////sAAABgAFMAaQBtAHUAb" "ABpAG4AawA6AEUAZABpAHQAbwByADoAUgBlAGYAZQByAGUAbgBjAGUAZABGAGkAbABlAHMALwBSAGUAZgBlAHIAZQBuAGMAZQBkACAARgBpAGwAZ" "QBzAAAAAAD/////AAAAjwD///8AAAABAAABYAAABOH8AgAAAAH7AAAAVABHAEwAVQBFADIAOgBQAHIAbwBwAGUAcgB0AHkASQBuAHMAcABlAGMAd" - "ABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA////AAAFdgAAA6EAAAABAAAAAgAAAAEAAAAC/" + "ABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA////AAAIoQAABOEAAAABAAAAAgAAAAEAAAAC/" "AAAAAEAAAACAAAAAA==" Array { Type "Cell" @@ -1121,9 +1154,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "nunoguedelha" ModifiedDateFormat "%" - LastModifiedDate "Thu Apr 29 17:07:41 2021" - RTWModifiedTimeStamp 541615217 - ModelVersionFormat "%" + LastModifiedDate "Thu Apr 29 22:13:17 2021" + RTWModifiedTimeStamp 541632409 + ModelVersionFormat "%" SampleTimeColors on SampleTimeAnnotations on LibraryLinkDisplay "disabled" @@ -1180,7 +1213,7 @@ Model { TryForcingSFcnDF off Object { $PropName "DataLoggingOverride" - $ObjectID 67 + $ObjectID 70 $ClassName "Simulink.SimulationData.ModelLoggingInfo" model_ "torqueControlBalancingWithSimu" signals_ [] @@ -1228,7 +1261,7 @@ Model { Type "Handle" Dimension 1 Simulink.ConfigSet { - $ObjectID 68 + $ObjectID 71 Version "20.1.0" DisabledProps [] Description "" @@ -1236,7 +1269,7 @@ Model { Type "Handle" Dimension 9 Simulink.SolverCC { - $ObjectID 69 + $ObjectID 72 Version "20.1.0" DisabledProps [] Description "" @@ -1278,7 +1311,7 @@ Model { ODENIntegrationMethod "ode3" } Simulink.DataIOCC { - $ObjectID 70 + $ObjectID 73 Version "20.1.0" DisabledProps [] Description "" @@ -1320,7 +1353,7 @@ Model { LoggingIntervals "[-inf, inf]" } Simulink.OptimizationCC { - $ObjectID 71 + $ObjectID 74 Version "20.1.0" Array { Type "Cell" @@ -1394,7 +1427,7 @@ Model { EfficientTunableParamExpr off } Simulink.DebuggingCC { - $ObjectID 72 + $ObjectID 75 Version "20.1.0" Array { Type "Cell" @@ -1517,7 +1550,7 @@ Model { VariantConditionMismatch "none" } Simulink.HardwareCC { - $ObjectID 73 + $ObjectID 76 Version "20.1.0" DisabledProps [] Description "" @@ -1567,7 +1600,7 @@ Model { HardwareBoardFeatureSet "EmbeddedCoderHSP" } Simulink.ModelReferenceCC { - $ObjectID 74 + $ObjectID 77 Version "20.1.0" DisabledProps [] Description "" @@ -1587,7 +1620,7 @@ Model { SupportModelReferenceSimTargetCustomCode off } Simulink.SFSimCC { - $ObjectID 75 + $ObjectID 78 Version "20.1.0" DisabledProps [] Description "" @@ -1631,7 +1664,7 @@ Model { } Simulink.RTWCC { $BackupClass "Simulink.RTWCC" - $ObjectID 76 + $ObjectID 79 Version "20.1.0" Array { Type "Cell" @@ -1737,7 +1770,7 @@ Model { Type "Handle" Dimension 2 Simulink.CodeAppCC { - $ObjectID 77 + $ObjectID 80 Version "20.1.0" Array { Type "Cell" @@ -1827,7 +1860,7 @@ Model { } Simulink.GRTTargetCC { $BackupClass "Simulink.TargetCC" - $ObjectID 78 + $ObjectID 81 Version "20.1.0" Array { Type "Cell" @@ -1857,7 +1890,7 @@ Model { Type "Handle" Dimension 1 Simulink.CPPComponent { - $ObjectID 79 + $ObjectID 82 Version "20.1.0" Array { Type "Cell" @@ -1968,7 +2001,7 @@ Model { } } SlCovCC.ConfigComp { - $ObjectID 80 + $ObjectID 83 Version "20.1.0" DisabledProps [] Description "Simulink Coverage Configuration Component" @@ -2017,11 +2050,11 @@ Model { } Simulink.ConfigSet { $PropName "ActiveConfigurationSet" - $ObjectID 68 + $ObjectID 71 } Object { $PropName "DataTransfer" - $ObjectID 81 + $ObjectID 84 $ClassName "Simulink.GlobalDataTransfer" DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" @@ -2387,7 +2420,7 @@ Model { } System { Name "torqueControlBalancingWithSimu" - Location [43, 23, 1792, 1120] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open on PortBlocksUseCompactNotation off @@ -2403,9 +2436,9 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "125" + ZoomFactor "203" ReportName "simulink-default.rpt" - SIDHighWatermark "5002" + SIDHighWatermark "5003" SimulinkSubDomain "Simulink" Block { BlockType BusCreator @@ -2506,6 +2539,37 @@ Model { 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 85 + $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 86 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 87 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + } + } Block { BlockType SubSystem Name "MOMENTUM BASED TORQUE CONTROL" @@ -2513,14 +2577,16 @@ Model { 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 [43, 23, 1792, 1120] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] - Open on + Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" @@ -2606,13 +2672,14 @@ Model { Ports [21, 1] Position [800, 3, 955, 607] ZOrder 542 - BackgroundColor "lightBlue" + ForegroundColor "red" + BackgroundColor "yellow" RequestExecContextInheritance off System { Name "Balancing Controller QP" - Location [43, 23, 1792, 1120] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] - Open on + Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" @@ -2802,10 +2869,12 @@ Model { Ports [13, 2] Position [1825, -185, 1990, 865] ZOrder 846 + ForegroundColor "red" + BackgroundColor "yellow" RequestExecContextInheritance off System { Name "Compute Desired Torques" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off @@ -2821,7 +2890,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "284" + ZoomFactor "167" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -2973,10 +3042,12 @@ Model { Ports [9, 3] Position [565, 452, 760, 848] ZOrder 652 + ForegroundColor "red" + BackgroundColor "yellow" RequestExecContextInheritance off System { Name "QPSolver" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off @@ -2992,7 +3063,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "343" + ZoomFactor "225" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -3213,12 +3284,14 @@ Model { 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 [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off @@ -3234,7 +3307,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "341" + ZoomFactor "175" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -3602,6 +3675,8 @@ Model { Ports [4, 2] Position [1145, -242, 1250, -128] ZOrder 751 + ForegroundColor "red" + BackgroundColor "yellow" LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/QP" SourceType "QP" @@ -6820,7 +6895,7 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 82 + $ObjectID 88 $ClassName "Simulink.Mask" Display "disp('VISUALIZER')" } @@ -12288,9 +12363,9 @@ Model { RequestExecContextInheritance off System { Name "Joint Torque Saturation" - Location [43, 23, 1792, 1120] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] - Open on + Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" @@ -14953,9 +15028,9 @@ Model { RequestExecContextInheritance off System { Name "State Machine" - Location [43, 23, 1792, 1120] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] - Open on + Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" @@ -17687,7 +17762,7 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 83 + $ObjectID 89 $ClassName "Simulink.Mask" Display "disp('EMERGENCY STOP')" RunInitForIconRedraw "off" @@ -18719,18 +18794,6 @@ Model { Indices "[1:Config.N_DOF]" OutputSizes "1" } - Block { - BlockType Selector - Name "Selector3" - SID "4978" - Ports [1, 1] - Position [840, 383, 880, 407] - ZOrder 1283 - InputPortWidth "robot_config.N_DOF" - IndexOptions "Index vector (dialog)" - Indices "[Config.N_DOF+1:Config.N_DOF+3]" - OutputSizes "1" - } Block { BlockType SubSystem Name "Subsystem" @@ -18746,9 +18809,9 @@ Model { ContentPreviewEnabled on System { Name "Subsystem" - Location [43, 23, 1792, 1120] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] - Open on + Open off PortBlocksUseCompactNotation off SetExecutionDomain off ExecutionDomainType "Deduce" @@ -18762,7 +18825,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "152" + ZoomFactor "125" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -19166,18 +19229,8 @@ Model { Labels [0, 0] SrcBlock "Bus\nSelector2" SrcPort 1 - Points [42, 0] - Branch { - ZOrder 397 - Points [0, -30] - DstBlock "Selector3" - DstPort 1 - } - Branch { - ZOrder 396 - DstBlock "Selector2" - DstPort 1 - } + DstBlock "Selector2" + DstPort 1 } Line { Name "" @@ -19219,8 +19272,8 @@ Model { DstPort 2 } Line { - ZOrder 395 - SrcBlock "Selector3" + ZOrder 428 + SrcBlock "Constant" SrcPort 1 DstBlock "MOMENTUM BASED TORQUE CONTROL" DstPort 1 @@ -21881,6 +21934,9 @@ Stateflow { screen [1 1 1280 1024 1.000677131425054] treeNode [0 130 0 0] viewObj 129 + visible 1 + subviewS { + } ssIdHighWaterMark 82 decomposition CLUSTER_CHART type EML_CHART From 075d33b19c52bfc3ced96213ba7017d9f49b4562 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Sun, 2 May 2021 09:44:23 +0200 Subject: [PATCH 06/20] Fixed the IMU sensor block mask init configuration (frame name) --- .../torqueControlBalancingWithSimu.mdl | 195 +++++++++--------- 1 file changed, 93 insertions(+), 102 deletions(-) diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl index 07c23bea..df759bef 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl +++ b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl @@ -7,7 +7,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "3.88" + ComputedModelVersion "3.90" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -410,48 +410,59 @@ Model { ViewObjType "SimulinkTopLevel" LoadSaveID "0" Extents [2171.0, 1188.0] - ZoomFactor [2.0283661825359989] - Offset [713.06692179479285, 156.04796368372479] - SceneRectInView [713.06692179479285, 156.04796368372479, 1070.3195599946705, 585.693061848765] + ZoomFactor [2.0625332048278269] + Offset [713.05800716783153, 160.99178344110948] + SceneRectInView [713.05800716783153, 160.99178344110948, 1052.5891146471156, 575.990726946464] } Object { $ObjectID 6 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4579" + LoadSaveID "4938" Extents [2171.0, 1188.0] - ZoomFactor [2.2524271844660193] - Offset [-544.74878771551721, -352.2155172413793] - SceneRectInView [-544.74878771551721, -352.2155172413793, 963.84913793103453, 527.43103448275861] + ZoomFactor [2.9013369436452869] + Offset [-326.36442414255714, -208.23320111992533] + SceneRectInView [-326.36442414255714, -208.23320111992533, 748.27572328511428, 409.46640223985065] } Object { $ObjectID 7 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4537" + LoadSaveID "4955" Extents [2171.0, 1188.0] - ZoomFactor [5.1816867469879524] - Offset [-108.23828125, -66.072916666666657] - SceneRectInView [-108.23828125, -66.072916666666657, 418.97553943452374, 229.26897321428569] + ZoomFactor [2.6403819497920837] + Offset [101.52586181754549, 133.53254783014467] + SceneRectInView [101.52586181754549, 133.53254783014467, 822.229526364909, 449.93490433971067] } Object { $ObjectID 8 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4955" + LoadSaveID "4579" Extents [2171.0, 1188.0] - ZoomFactor [1.25] - Offset [54.774666262135952, 11.467273462783169] - SceneRectInView [54.774666262135952, 11.467273462783169, 1736.8, 950.4] + ZoomFactor [2.2524271844660193] + Offset [-544.74878771551721, -352.2155172413793] + SceneRectInView [-544.74878771551721, -352.2155172413793, 963.84913793103453, 527.43103448275861] } Object { $ObjectID 9 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 10 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" LoadSaveID "2355" Extents [2171.0, 1188.0] ZoomFactor [1.0] @@ -459,7 +470,7 @@ Model { SceneRectInView [1012.8742726293103, -129.0, 2171.0, 1188.0] } Object { - $ObjectID 10 + $ObjectID 11 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -470,7 +481,7 @@ Model { SceneRectInView [-608.35758121641425, -114.33481638525888, 1528.7515828078519, 836.55314618872774] } Object { - $ObjectID 11 + $ObjectID 12 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -481,7 +492,7 @@ Model { SceneRectInView [-116.92703117366869, -144.85219724177151, 889.14932180693916, 486.55430414861524] } Object { - $ObjectID 12 + $ObjectID 13 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -492,7 +503,7 @@ Model { SceneRectInView [374.06441393490434, 302.07339302379842, 1297.2149221301913, 709.85321395240317] } Object { - $ObjectID 13 + $ObjectID 14 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -503,7 +514,7 @@ Model { SceneRectInView [124.11704673423424, -38.918426238738732, 1757.9018510698197, 961.94721283783781] } Object { - $ObjectID 14 + $ObjectID 15 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -514,7 +525,7 @@ Model { SceneRectInView [155.02749177730379, -243.5, 1360.0, 868.0] } Object { - $ObjectID 15 + $ObjectID 16 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -525,7 +536,7 @@ Model { SceneRectInView [909.16303109605917, -448.321921182266, 1240.5714285714287, 678.85714285714289] } Object { - $ObjectID 16 + $ObjectID 17 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -536,7 +547,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 17 + $ObjectID 18 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -547,7 +558,7 @@ Model { SceneRectInView [-29.932291666666686, -42.75, 432.29166666666669, 247.5] } Object { - $ObjectID 18 + $ObjectID 19 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -558,7 +569,7 @@ Model { SceneRectInView [-29.368338160419114, -144.92583198051949, 808.43198882083823, 462.851663961039] } Object { - $ObjectID 19 + $ObjectID 20 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -569,7 +580,7 @@ Model { SceneRectInView [10.776074743527147, -15.071488336727981, 259.375, 148.5] } Object { - $ObjectID 20 + $ObjectID 21 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -580,7 +591,7 @@ Model { SceneRectInView [578.59213941133362, 441.73077216658527, 393.93290867733265, 225.53845566682949] } Object { - $ObjectID 21 + $ObjectID 22 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -591,7 +602,7 @@ Model { SceneRectInView [-25.756466649159648, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 22 + $ObjectID 23 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -602,7 +613,7 @@ Model { SceneRectInView [-709.226772339876, 2332.9648668093268, 1185.7142857142858, 678.85714285714289] } Object { - $ObjectID 23 + $ObjectID 24 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -613,7 +624,7 @@ Model { SceneRectInView [-1148.4886014344261, -284.77258542366184, 1309.1803278688524, 749.54517084732367] } Object { - $ObjectID 24 + $ObjectID 25 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -624,7 +635,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 25 + $ObjectID 26 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -635,7 +646,7 @@ Model { SceneRectInView [23.229550867122612, 135.34665821934539, 447.67371076575478, 256.30668356130923] } Object { - $ObjectID 26 + $ObjectID 27 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -646,7 +657,7 @@ Model { SceneRectInView [94.721007009531661, 154.55463376836479, 629.70491803278685, 360.52503258937384] } Object { - $ObjectID 27 + $ObjectID 28 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -657,7 +668,7 @@ Model { SceneRectInView [-676.15435221738494, -478.27262581168833, 1319.6602669347699, 755.54525162337666] } Object { - $ObjectID 28 + $ObjectID 29 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -668,7 +679,7 @@ Model { SceneRectInView [-92.890717237308081, -373.76948051948051, 862.03143447461616, 493.538961038961] } Object { - $ObjectID 29 + $ObjectID 30 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -679,7 +690,7 @@ Model { SceneRectInView [161.34960569474492, -350.98802908437432, 2075.0, 1188.0] } Object { - $ObjectID 30 + $ObjectID 31 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -690,7 +701,7 @@ Model { SceneRectInView [111.71042989741085, -4.1074743527112787, 672.82914020517831, 385.21494870542256] } Object { - $ObjectID 31 + $ObjectID 32 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -701,7 +712,7 @@ Model { SceneRectInView [434.56854543234, -203.35510808500248, 1485.87853413532, 850.710216170005] } Object { - $ObjectID 32 + $ObjectID 33 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -712,7 +723,7 @@ Model { SceneRectInView [-796.48981681034491, -71.967241379310337, 1329.0732758620691, 760.93448275862067] } Object { - $ObjectID 33 + $ObjectID 34 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -723,7 +734,7 @@ Model { SceneRectInView [-113.1169934431619, -154.52586206896552, 670.79741379310349, 384.05172413793105] } Object { - $ObjectID 34 + $ObjectID 35 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -734,7 +745,7 @@ Model { SceneRectInView [-168.50171712924032, -141.48435660218672, 1137.0034342584806, 650.96871320437344] } Object { - $ObjectID 35 + $ObjectID 36 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -745,7 +756,7 @@ Model { SceneRectInView [87.32421875, 140.1, 207.5, 118.8] } Object { - $ObjectID 36 + $ObjectID 37 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -756,7 +767,7 @@ Model { SceneRectInView [-371.86694504310344, -625.85167594516, 2093.6948275862069, 1198.70335189032] } Object { - $ObjectID 37 + $ObjectID 38 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -767,7 +778,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 38 + $ObjectID 39 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -778,7 +789,7 @@ Model { SceneRectInView [-191.40632054176072, -117.29872084273889, 691.66666666666663, 396.0] } Object { - $ObjectID 39 + $ObjectID 40 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -789,7 +800,7 @@ Model { SceneRectInView [-92.7656132430399, -55.8837471783296, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 40 + $ObjectID 41 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -800,7 +811,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 41 + $ObjectID 42 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -811,7 +822,7 @@ Model { SceneRectInView [-99.198645598194133, -48.972911963882623, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 42 + $ObjectID 43 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -822,7 +833,7 @@ Model { SceneRectInView [-80.667161130818016, -31.015048908954071, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 43 + $ObjectID 44 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -833,7 +844,7 @@ Model { SceneRectInView [13.879095336481726, -85.310064935064929, 502.36680932703655, 287.62012987012986] } Object { - $ObjectID 44 + $ObjectID 45 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -844,7 +855,7 @@ Model { SceneRectInView [13.972911622276058, -85.325665859564168, 502.42130750605327, 287.65133171912834] } Object { - $ObjectID 45 + $ObjectID 46 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -855,7 +866,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 46 + $ObjectID 47 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -866,7 +877,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 47 + $ObjectID 48 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -877,7 +888,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 48 + $ObjectID 49 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -888,7 +899,7 @@ Model { SceneRectInView [-64.168329397874857, -71.912514757969319, 518.75, 297.0] } Object { - $ObjectID 49 + $ObjectID 50 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -899,7 +910,7 @@ Model { SceneRectInView [-684.37478764306115, -290.08334454895737, 840.66666666666652, 579.97212543554] } Object { - $ObjectID 50 + $ObjectID 51 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -910,7 +921,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 51 + $ObjectID 52 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -921,7 +932,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 52 + $ObjectID 53 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -932,7 +943,7 @@ Model { SceneRectInView [203.38965517241377, -277.26034482758621, 524.02241379310351, 361.52068965517242] } Object { - $ObjectID 53 + $ObjectID 54 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -943,7 +954,7 @@ Model { SceneRectInView [-8.0130751303127283, -114.03393582177664, 473.82302526062546, 223.19657177225341] } Object { - $ObjectID 54 + $ObjectID 55 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -954,7 +965,7 @@ Model { SceneRectInView [16.019427876904615, -398.80851543704892, 1030.9142692461908, 485.61703087409785] } Object { - $ObjectID 55 + $ObjectID 56 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -965,7 +976,7 @@ Model { SceneRectInView [-105.54956270048115, -242.68495036561927, 864.60693790096229, 407.2771777265437] } Object { - $ObjectID 56 + $ObjectID 57 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -976,7 +987,7 @@ Model { SceneRectInView [-396.83333333333337, -304.36666666666662, 1148.0, 792.0] } Object { - $ObjectID 57 + $ObjectID 58 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -987,7 +998,7 @@ Model { SceneRectInView [34.9092200413223, -92.857244318181813, 438.7831224173554, 302.71448863636363] } Object { - $ObjectID 58 + $ObjectID 59 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -998,7 +1009,7 @@ Model { SceneRectInView [-183.3666666666665, -165.10000000000005, 867.6, 475.2] } Object { - $ObjectID 59 + $ObjectID 60 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1008,17 +1019,6 @@ Model { Offset [10.41989407940855, -23.235071395084702] SceneRectInView [10.41989407940855, -23.235071395084702, 501.20498970990775, 274.51891552575859] } - Object { - $ObjectID 60 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "4938" - Extents [2522.0, 1188.0] - ZoomFactor [3.7866767887644714] - Offset [-245.23623571571773, -105.36577786688051] - SceneRectInView [-245.23623571571773, -105.36577786688051, 666.01934643143545, 313.731555733761] - } Object { $ObjectID 61 IsActive [0] @@ -1103,26 +1103,26 @@ Model { Dimension 2 Object { $ObjectID 68 - Type "Simulink:Editor:ReferencedFiles" - ID "Referenced Files" + Type "GLUE2:PropertyInspector" + ID "Property Inspector" Visible [0] CreateCallback "" - UserData "{\"filterShowRefModels\":\"true\",\"filterShowRefSubs\":\"true\",\"filterShowOnlyDirtyFiles\":\"false\"}\n" + UserData "" Floating [0] - DockPosition "Left" + DockPosition "Right" Width [640] Height [480] Minimized "Unset" } Object { $ObjectID 69 - Type "GLUE2:PropertyInspector" - ID "Property Inspector" + Type "Simulink:Editor:ReferencedFiles" + ID "Referenced Files" Visible [0] CreateCallback "" - UserData "" + UserData "{\"filterShowRefModels\":\"true\",\"filterShowRefSubs\":\"true\",\"filterShowOnlyDirtyFiles\":\"false\"}\n" Floating [0] - DockPosition "Right" + DockPosition "Left" Width [640] Height [480] Minimized "Unset" @@ -1154,9 +1154,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "nunoguedelha" ModifiedDateFormat "%" - LastModifiedDate "Thu Apr 29 22:13:17 2021" - RTWModifiedTimeStamp 541632409 - ModelVersionFormat "%" + LastModifiedDate "Sun May 02 09:43:17 2021" + RTWModifiedTimeStamp 541849128 + ModelVersionFormat "%" SampleTimeColors on SampleTimeAnnotations on LibraryLinkDisplay "disabled" @@ -2436,7 +2436,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "203" + ZoomFactor "206" ReportName "simulink-default.rpt" SIDHighWatermark "5003" SimulinkSubDomain "Simulink" @@ -18825,7 +18825,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "125" + ZoomFactor "264" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -18903,23 +18903,17 @@ Model { Ports [4, 1] Position [545, 392, 765, 508] ZOrder 1270 - LibraryVersion "1.2" + LibraryVersion "1.17" SourceBlock "mwbs_robotSensors_lib/IMUsensor" - SourceType "SubSystem" + SourceType "IMUsensor" SourceProductName "Matlab Whole-body Simulator" - 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 + frameName "robot_config.robotFrames.IMU" } Block { BlockType Reference @@ -21934,9 +21928,6 @@ Stateflow { screen [1 1 1280 1024 1.000677131425054] treeNode [0 130 0 0] viewObj 129 - visible 1 - subviewS { - } ssIdHighWaterMark 82 decomposition CLUSTER_CHART type EML_CHART From 19badf1f619e6a1ec65b0fc541765e16a536792e Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Sun, 2 May 2021 23:20:42 +0200 Subject: [PATCH 07/20] Fixed QP wbtoolbox block issue (unsupported number of port dimensions) - Used QP interface with constraints lower bound input "lbA". Set lbA=-Inf. - For more details refer to original issue tracker https://github.com/dic-iit/element_ironcub-control/issues/164 --- .../torqueControlBalancingWithSimu.mdl | 425 ++++++++++-------- 1 file changed, 240 insertions(+), 185 deletions(-) diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl index df759bef..b70a7f34 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl +++ b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl @@ -7,7 +7,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "3.90" + ComputedModelVersion "3.98" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -402,7 +402,7 @@ Model { } Array { Type "Simulink.EditorInfo" - Dimension 63 + Dimension 65 Object { $ObjectID 5 IsActive [1] @@ -410,53 +410,53 @@ Model { ViewObjType "SimulinkTopLevel" LoadSaveID "0" Extents [2171.0, 1188.0] - ZoomFactor [2.0625332048278269] - Offset [713.05800716783153, 160.99178344110948] - SceneRectInView [713.05800716783153, 160.99178344110948, 1052.5891146471156, 575.990726946464] + ZoomFactor [2.0285911286881269] + Offset [713.18484189255014, 156.19184020956072] + SceneRectInView [713.18484189255014, 156.19184020956072, 1070.2008745369835, 585.62811559186389] } Object { $ObjectID 6 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4938" + LoadSaveID "4537" Extents [2171.0, 1188.0] - ZoomFactor [2.9013369436452869] - Offset [-326.36442414255714, -208.23320111992533] - SceneRectInView [-326.36442414255714, -208.23320111992533, 748.27572328511428, 409.46640223985065] + ZoomFactor [5.1816867469879524] + Offset [-108.23828125, -66.072916666666657] + SceneRectInView [-108.23828125, -66.072916666666657, 418.97553943452374, 229.26897321428569] } Object { $ObjectID 7 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4955" + LoadSaveID "4579" Extents [2171.0, 1188.0] - ZoomFactor [2.6403819497920837] - Offset [101.52586181754549, 133.53254783014467] - SceneRectInView [101.52586181754549, 133.53254783014467, 822.229526364909, 449.93490433971067] + ZoomFactor [2.2524271844660193] + Offset [-544.74878771551721, -352.2155172413793] + SceneRectInView [-544.74878771551721, -352.2155172413793, 963.84913793103453, 527.43103448275861] } Object { $ObjectID 8 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4579" + LoadSaveID "4393" Extents [2171.0, 1188.0] - ZoomFactor [2.2524271844660193] - Offset [-544.74878771551721, -352.2155172413793] - SceneRectInView [-544.74878771551721, -352.2155172413793, 963.84913793103453, 527.43103448275861] + ZoomFactor [1.6735854351990824] + Offset [374.06441393490434, 302.07339302379842] + SceneRectInView [374.06441393490434, 302.07339302379842, 1297.2149221301913, 709.85321395240317] } Object { $ObjectID 9 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4537" + LoadSaveID "4836" Extents [2171.0, 1188.0] - ZoomFactor [5.1816867469879524] - Offset [-108.23828125, -66.072916666666657] - SceneRectInView [-108.23828125, -66.072916666666657, 418.97553943452374, 229.26897321428569] + ZoomFactor [1.2349950019557565] + Offset [124.11704673423424, -38.918426238738732] + SceneRectInView [124.11704673423424, -38.918426238738732, 1757.9018510698197, 961.94721283783781] } Object { $ObjectID 10 @@ -474,72 +474,116 @@ Model { IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "2163" + LoadSaveID "2275" Extents [2171.0, 1188.0] - ZoomFactor [1.4201130022789792] - Offset [-608.35758121641425, -114.33481638525888] - SceneRectInView [-608.35758121641425, -114.33481638525888, 1528.7515828078519, 836.55314618872774] + ZoomFactor [1.8249725000639532] + Offset [-194.803483319316, -141.48435660218672] + SceneRectInView [-194.803483319316, -141.48435660218672, 1189.606966638632, 650.96871320437344] } Object { $ObjectID 12 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4553" + LoadSaveID "4696" Extents [2171.0, 1188.0] - ZoomFactor [2.441659625391233] - Offset [-116.92703117366869, -144.85219724177151] - SceneRectInView [-116.92703117366869, -144.85219724177151, 889.14932180693916, 486.55430414861524] + ZoomFactor [2.2264875239923225] + Offset [917.234213362069, -427.28793103448277] + SceneRectInView [917.234213362069, -427.28793103448277, 975.078448275862, 533.57586206896553] } Object { $ObjectID 13 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4393" + LoadSaveID "4684" Extents [2171.0, 1188.0] - ZoomFactor [1.6735854351990824] - Offset [374.06441393490434, 302.07339302379842] - SceneRectInView [374.06441393490434, 302.07339302379842, 1297.2149221301913, 709.85321395240317] + ZoomFactor [2.7884615384615383] + Offset [1438.5531788793103, 818.97931034482758] + SceneRectInView [1438.5531788793103, 818.97931034482758, 778.56551724137933, 426.04137931034484] } Object { $ObjectID 14 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4836" + LoadSaveID "4519" Extents [2171.0, 1188.0] - ZoomFactor [1.2349950019557565] - Offset [124.11704673423424, -38.918426238738732] - SceneRectInView [124.11704673423424, -38.918426238738732, 1757.9018510698197, 961.94721283783781] + ZoomFactor [1.5723743845222296] + Offset [-706.68143309105676, -478.27262581168833] + SceneRectInView [-706.68143309105676, -478.27262581168833, 1380.7144286821135, 755.54525162337666] } Object { $ObjectID 15 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937" - Extents [1360.0, 868.0] - ZoomFactor [1.0] - Offset [155.02749177730379, -243.5] - SceneRectInView [155.02749177730379, -243.5, 1360.0, 868.0] + LoadSaveID "4955" + Extents [2171.0, 1188.0] + ZoomFactor [2.6403819497920837] + Offset [101.52586181754549, 133.53254783014467] + SceneRectInView [101.52586181754549, 133.53254783014467, 822.229526364909, 449.93490433971067] } Object { $ObjectID 16 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4696" + LoadSaveID "3714" Extents [2171.0, 1188.0] - ZoomFactor [1.7499999999999998] - Offset [909.16303109605917, -448.321921182266] - SceneRectInView [909.16303109605917, -448.321921182266, 1240.5714285714287, 678.85714285714289] + ZoomFactor [2.08] + Offset [109.74429086538464, -263.57692307692309] + SceneRectInView [109.74429086538464, -263.57692307692309, 1043.75, 571.15384615384619] } Object { $ObjectID 17 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 18 + 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 19 + 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 20 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937" + Extents [1360.0, 868.0] + ZoomFactor [1.0] + Offset [155.02749177730379, -243.5] + SceneRectInView [155.02749177730379, -243.5, 1360.0, 868.0] + } + Object { + $ObjectID 21 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" LoadSaveID "4220" Extents [2075.0, 1188.0] ZoomFactor [5.95] @@ -547,7 +591,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 18 + $ObjectID 22 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -558,7 +602,7 @@ Model { SceneRectInView [-29.932291666666686, -42.75, 432.29166666666669, 247.5] } Object { - $ObjectID 19 + $ObjectID 23 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -569,7 +613,7 @@ Model { SceneRectInView [-29.368338160419114, -144.92583198051949, 808.43198882083823, 462.851663961039] } Object { - $ObjectID 20 + $ObjectID 24 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -580,7 +624,7 @@ Model { SceneRectInView [10.776074743527147, -15.071488336727981, 259.375, 148.5] } Object { - $ObjectID 21 + $ObjectID 25 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -591,7 +635,7 @@ Model { SceneRectInView [578.59213941133362, 441.73077216658527, 393.93290867733265, 225.53845566682949] } Object { - $ObjectID 22 + $ObjectID 26 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -602,7 +646,7 @@ Model { SceneRectInView [-25.756466649159648, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 23 + $ObjectID 27 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -613,7 +657,7 @@ Model { SceneRectInView [-709.226772339876, 2332.9648668093268, 1185.7142857142858, 678.85714285714289] } Object { - $ObjectID 24 + $ObjectID 28 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -624,7 +668,7 @@ Model { SceneRectInView [-1148.4886014344261, -284.77258542366184, 1309.1803278688524, 749.54517084732367] } Object { - $ObjectID 25 + $ObjectID 29 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -635,7 +679,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 26 + $ObjectID 30 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -646,7 +690,7 @@ Model { SceneRectInView [23.229550867122612, 135.34665821934539, 447.67371076575478, 256.30668356130923] } Object { - $ObjectID 27 + $ObjectID 31 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -657,18 +701,7 @@ Model { SceneRectInView [94.721007009531661, 154.55463376836479, 629.70491803278685, 360.52503258937384] } Object { - $ObjectID 28 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "4519" - Extents [2075.0, 1188.0] - ZoomFactor [1.5723743845222296] - Offset [-676.15435221738494, -478.27262581168833] - SceneRectInView [-676.15435221738494, -478.27262581168833, 1319.6602669347699, 755.54525162337666] - } - Object { - $ObjectID 29 + $ObjectID 32 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -679,7 +712,7 @@ Model { SceneRectInView [-92.890717237308081, -373.76948051948051, 862.03143447461616, 493.538961038961] } Object { - $ObjectID 30 + $ObjectID 33 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -690,7 +723,7 @@ Model { SceneRectInView [161.34960569474492, -350.98802908437432, 2075.0, 1188.0] } Object { - $ObjectID 31 + $ObjectID 34 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -701,7 +734,7 @@ Model { SceneRectInView [111.71042989741085, -4.1074743527112787, 672.82914020517831, 385.21494870542256] } Object { - $ObjectID 32 + $ObjectID 35 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -712,7 +745,7 @@ Model { SceneRectInView [434.56854543234, -203.35510808500248, 1485.87853413532, 850.710216170005] } Object { - $ObjectID 33 + $ObjectID 36 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -723,7 +756,7 @@ Model { SceneRectInView [-796.48981681034491, -71.967241379310337, 1329.0732758620691, 760.93448275862067] } Object { - $ObjectID 34 + $ObjectID 37 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -734,18 +767,7 @@ Model { SceneRectInView [-113.1169934431619, -154.52586206896552, 670.79741379310349, 384.05172413793105] } Object { - $ObjectID 35 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "2275" - Extents [2075.0, 1188.0] - ZoomFactor [1.8249725000639532] - Offset [-168.50171712924032, -141.48435660218672] - SceneRectInView [-168.50171712924032, -141.48435660218672, 1137.0034342584806, 650.96871320437344] - } - Object { - $ObjectID 36 + $ObjectID 38 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -756,7 +778,7 @@ Model { SceneRectInView [87.32421875, 140.1, 207.5, 118.8] } Object { - $ObjectID 37 + $ObjectID 39 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -767,7 +789,7 @@ Model { SceneRectInView [-371.86694504310344, -625.85167594516, 2093.6948275862069, 1198.70335189032] } Object { - $ObjectID 38 + $ObjectID 40 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -778,7 +800,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 39 + $ObjectID 41 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -789,7 +811,7 @@ Model { SceneRectInView [-191.40632054176072, -117.29872084273889, 691.66666666666663, 396.0] } Object { - $ObjectID 40 + $ObjectID 42 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -800,7 +822,7 @@ Model { SceneRectInView [-92.7656132430399, -55.8837471783296, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 41 + $ObjectID 43 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -811,7 +833,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 42 + $ObjectID 44 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -822,7 +844,7 @@ Model { SceneRectInView [-99.198645598194133, -48.972911963882623, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 43 + $ObjectID 45 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -833,7 +855,7 @@ Model { SceneRectInView [-80.667161130818016, -31.015048908954071, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 44 + $ObjectID 46 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -844,7 +866,7 @@ Model { SceneRectInView [13.879095336481726, -85.310064935064929, 502.36680932703655, 287.62012987012986] } Object { - $ObjectID 45 + $ObjectID 47 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -855,7 +877,7 @@ Model { SceneRectInView [13.972911622276058, -85.325665859564168, 502.42130750605327, 287.65133171912834] } Object { - $ObjectID 46 + $ObjectID 48 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -866,7 +888,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 47 + $ObjectID 49 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -877,7 +899,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 48 + $ObjectID 50 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -888,7 +910,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 49 + $ObjectID 51 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -899,7 +921,7 @@ Model { SceneRectInView [-64.168329397874857, -71.912514757969319, 518.75, 297.0] } Object { - $ObjectID 50 + $ObjectID 52 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -910,7 +932,7 @@ Model { SceneRectInView [-684.37478764306115, -290.08334454895737, 840.66666666666652, 579.97212543554] } Object { - $ObjectID 51 + $ObjectID 53 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -921,7 +943,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 52 + $ObjectID 54 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -932,7 +954,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 53 + $ObjectID 55 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -943,7 +965,7 @@ Model { SceneRectInView [203.38965517241377, -277.26034482758621, 524.02241379310351, 361.52068965517242] } Object { - $ObjectID 54 + $ObjectID 56 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -954,7 +976,7 @@ Model { SceneRectInView [-8.0130751303127283, -114.03393582177664, 473.82302526062546, 223.19657177225341] } Object { - $ObjectID 55 + $ObjectID 57 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -965,7 +987,7 @@ Model { SceneRectInView [16.019427876904615, -398.80851543704892, 1030.9142692461908, 485.61703087409785] } Object { - $ObjectID 56 + $ObjectID 58 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -976,7 +998,7 @@ Model { SceneRectInView [-105.54956270048115, -242.68495036561927, 864.60693790096229, 407.2771777265437] } Object { - $ObjectID 57 + $ObjectID 59 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -987,7 +1009,7 @@ Model { SceneRectInView [-396.83333333333337, -304.36666666666662, 1148.0, 792.0] } Object { - $ObjectID 58 + $ObjectID 60 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -998,7 +1020,7 @@ Model { SceneRectInView [34.9092200413223, -92.857244318181813, 438.7831224173554, 302.71448863636363] } Object { - $ObjectID 59 + $ObjectID 61 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1009,7 +1031,7 @@ Model { SceneRectInView [-183.3666666666665, -165.10000000000005, 867.6, 475.2] } Object { - $ObjectID 60 + $ObjectID 62 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1020,7 +1042,7 @@ Model { SceneRectInView [10.41989407940855, -23.235071395084702, 501.20498970990775, 274.51891552575859] } Object { - $ObjectID 61 + $ObjectID 63 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1031,7 +1053,7 @@ Model { SceneRectInView [-68.684210488152473, -33.394738158739756, 477.63157399976672, 216.05262934094131] } Object { - $ObjectID 62 + $ObjectID 64 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1042,7 +1064,7 @@ Model { SceneRectInView [-211.69612560424537, 280.84982585720286, 605.0, 273.66666666666669] } Object { - $ObjectID 63 + $ObjectID 65 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1053,7 +1075,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 64 + $ObjectID 66 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1064,7 +1086,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 65 + $ObjectID 67 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1075,7 +1097,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 66 + $ObjectID 68 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1086,7 +1108,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 67 + $ObjectID 69 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1102,7 +1124,7 @@ Model { Type "Simulink.DockComponentInfo" Dimension 2 Object { - $ObjectID 68 + $ObjectID 70 Type "GLUE2:PropertyInspector" ID "Property Inspector" Visible [0] @@ -1115,7 +1137,7 @@ Model { Minimized "Unset" } Object { - $ObjectID 69 + $ObjectID 71 Type "Simulink:Editor:ReferencedFiles" ID "Referenced Files" Visible [0] @@ -1154,9 +1176,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "nunoguedelha" ModifiedDateFormat "%" - LastModifiedDate "Sun May 02 09:43:17 2021" - RTWModifiedTimeStamp 541849128 - ModelVersionFormat "%" + LastModifiedDate "Sun May 02 23:19:18 2021" + RTWModifiedTimeStamp 541898351 + ModelVersionFormat "%" SampleTimeColors on SampleTimeAnnotations on LibraryLinkDisplay "disabled" @@ -1213,7 +1235,7 @@ Model { TryForcingSFcnDF off Object { $PropName "DataLoggingOverride" - $ObjectID 70 + $ObjectID 72 $ClassName "Simulink.SimulationData.ModelLoggingInfo" model_ "torqueControlBalancingWithSimu" signals_ [] @@ -1261,7 +1283,7 @@ Model { Type "Handle" Dimension 1 Simulink.ConfigSet { - $ObjectID 71 + $ObjectID 73 Version "20.1.0" DisabledProps [] Description "" @@ -1269,7 +1291,7 @@ Model { Type "Handle" Dimension 9 Simulink.SolverCC { - $ObjectID 72 + $ObjectID 74 Version "20.1.0" DisabledProps [] Description "" @@ -1311,7 +1333,7 @@ Model { ODENIntegrationMethod "ode3" } Simulink.DataIOCC { - $ObjectID 73 + $ObjectID 75 Version "20.1.0" DisabledProps [] Description "" @@ -1353,7 +1375,7 @@ Model { LoggingIntervals "[-inf, inf]" } Simulink.OptimizationCC { - $ObjectID 74 + $ObjectID 76 Version "20.1.0" Array { Type "Cell" @@ -1427,7 +1449,7 @@ Model { EfficientTunableParamExpr off } Simulink.DebuggingCC { - $ObjectID 75 + $ObjectID 77 Version "20.1.0" Array { Type "Cell" @@ -1550,7 +1572,7 @@ Model { VariantConditionMismatch "none" } Simulink.HardwareCC { - $ObjectID 76 + $ObjectID 78 Version "20.1.0" DisabledProps [] Description "" @@ -1600,7 +1622,7 @@ Model { HardwareBoardFeatureSet "EmbeddedCoderHSP" } Simulink.ModelReferenceCC { - $ObjectID 77 + $ObjectID 79 Version "20.1.0" DisabledProps [] Description "" @@ -1620,7 +1642,7 @@ Model { SupportModelReferenceSimTargetCustomCode off } Simulink.SFSimCC { - $ObjectID 78 + $ObjectID 80 Version "20.1.0" DisabledProps [] Description "" @@ -1664,7 +1686,7 @@ Model { } Simulink.RTWCC { $BackupClass "Simulink.RTWCC" - $ObjectID 79 + $ObjectID 81 Version "20.1.0" Array { Type "Cell" @@ -1770,7 +1792,7 @@ Model { Type "Handle" Dimension 2 Simulink.CodeAppCC { - $ObjectID 80 + $ObjectID 82 Version "20.1.0" Array { Type "Cell" @@ -1860,7 +1882,7 @@ Model { } Simulink.GRTTargetCC { $BackupClass "Simulink.TargetCC" - $ObjectID 81 + $ObjectID 83 Version "20.1.0" Array { Type "Cell" @@ -1890,7 +1912,7 @@ Model { Type "Handle" Dimension 1 Simulink.CPPComponent { - $ObjectID 82 + $ObjectID 84 Version "20.1.0" Array { Type "Cell" @@ -2001,7 +2023,7 @@ Model { } } SlCovCC.ConfigComp { - $ObjectID 83 + $ObjectID 85 Version "20.1.0" DisabledProps [] Description "Simulink Coverage Configuration Component" @@ -2050,11 +2072,11 @@ Model { } Simulink.ConfigSet { $PropName "ActiveConfigurationSet" - $ObjectID 71 + $ObjectID 73 } Object { $PropName "DataTransfer" - $ObjectID 84 + $ObjectID 86 $ClassName "Simulink.GlobalDataTransfer" DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" @@ -2436,9 +2458,9 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "206" + ZoomFactor "203" ReportName "simulink-default.rpt" - SIDHighWatermark "5003" + SIDHighWatermark "5007" SimulinkSubDomain "Simulink" Block { BlockType BusCreator @@ -2548,7 +2570,7 @@ Model { Value "zeros(3,1)" Object { $PropName "MaskObject" - $ObjectID 85 + $ObjectID 87 $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." @@ -2556,12 +2578,12 @@ Model { RunInitForIconRedraw "off" Object { $PropName "DialogControls" - $ObjectID 86 + $ObjectID 88 $ClassName "Simulink.dialog.Group" Prompt "%" Object { $PropName "DialogControls" - $ObjectID 87 + $ObjectID 89 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -3307,20 +3329,20 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "175" + ZoomFactor "223" SimulinkSubDomain "Simulink" Block { BlockType Inport Name "H" SID "4685" - Position [970, -237, 1000, -223] + Position [970, -242, 1000, -228] ZOrder 750 } Block { BlockType Inport Name "g" SID "4686" - Position [970, -207, 1000, -193] + Position [970, -212, 1000, -198] ZOrder 752 Port "2" } @@ -3328,7 +3350,7 @@ Model { BlockType Inport Name "A" SID "4687" - Position [970, -177, 1000, -163] + Position [970, -182, 1000, -168] ZOrder 753 Port "3" } @@ -3336,7 +3358,7 @@ Model { BlockType Inport Name "ubA" SID "4688" - Position [970, -147, 1000, -133] + Position [970, -122, 1000, -108] ZOrder 754 Port "4" } @@ -3489,6 +3511,14 @@ Model { } } 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" @@ -3672,8 +3702,8 @@ Model { BlockType Reference Name "QP One Foot" SID "4693" - Ports [4, 2] - Position [1145, -242, 1250, -128] + Ports [5, 2] + Position [1140, -251, 1240, -99] ZOrder 751 ForegroundColor "red" BackgroundColor "yellow" @@ -3682,7 +3712,7 @@ Model { SourceType "QP" SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" - lbA off + lbA on ubA on lb off ub off @@ -3727,7 +3757,7 @@ Model { Points [31, 0] Branch { ZOrder 34 - Points [0, -150] + Points [0, -145] DstBlock "Analytical Solution QP One Foot (unconstrained)" DstPort 1 } @@ -3738,11 +3768,11 @@ Model { } } Line { - ZOrder 7 + ZOrder 54 SrcBlock "ubA" SrcPort 1 DstBlock "QP One Foot" - DstPort 4 + DstPort 5 } Line { ZOrder 8 @@ -3751,7 +3781,7 @@ Model { Points [51, 0] Branch { ZOrder 33 - Points [0, -55] + Points [0, -50] Branch { ZOrder 40 Points [0, -75] @@ -3781,15 +3811,16 @@ Model { ZOrder 14 SrcBlock "QP One Foot" SrcPort 2 - Points [34, 0] + Points [44, 0] Branch { - ZOrder 41 - Points [0, 50] + ZOrder 59 + Points [0, 30] DstBlock "Process QP output" DstPort 3 } Branch { - ZOrder 26 + ZOrder 58 + Points [0, -20] DstBlock "QPStatus" DstPort 1 } @@ -3815,6 +3846,13 @@ Model { DstBlock "f_star_oneFoot" DstPort 1 } + Line { + ZOrder 55 + SrcBlock "Constant" + SrcPort 1 + DstBlock "QP One Foot" + DstPort 4 + } } } Block { @@ -3828,7 +3866,7 @@ Model { RequestExecContextInheritance off System { Name "QP Two Feet" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off @@ -3844,7 +3882,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "426" + ZoomFactor "279" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -3873,7 +3911,7 @@ Model { BlockType Inport Name "ubA" SID "4676" - Position [1460, 1113, 1490, 1127] + Position [1460, 1143, 1490, 1157] ZOrder 755 Port "4" } @@ -4018,11 +4056,19 @@ Model { } } 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, 984, 1825, 1066] + Position [1735, 986, 1825, 1079] ZOrder 757 ShowName off HideAutomaticName off @@ -4187,15 +4233,15 @@ Model { BlockType Reference Name "QP Two Feet" SID "4681" - Ports [4, 2] - Position [1575, 1016, 1680, 1134] + Ports [5, 2] + Position [1575, 1015, 1685, 1165] ZOrder 752 LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/QP" SourceType "QP" SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" - lbA off + lbA on ubA on lb off ub off @@ -4223,18 +4269,19 @@ Model { ZOrder 37 SrcBlock "QP Two Feet" SrcPort 2 - Points [28, 0] + Points [23, 0] + Branch { + ZOrder 91 + Points [0, -25] + DstBlock "QPStatus" + DstPort 1 + } Branch { ZOrder 62 - Points [0, 55] + Points [0, 30] DstBlock "Process QP output" DstPort 3 } - Branch { - ZOrder 61 - DstBlock "QPStatus" - DstPort 1 - } } Line { ZOrder 38 @@ -4261,6 +4308,7 @@ Model { ZOrder 41 SrcBlock "MatchSignalSizes" SrcPort 1 + Points [43, 0; 0, -10] DstBlock "Process QP output" DstPort 2 } @@ -4277,14 +4325,14 @@ Model { SrcPort 1 Points [22, 0] Branch { - ZOrder 79 - DstBlock "QP Two Feet" + ZOrder 90 + Points [0, -160] + DstBlock "Analytical Solution Two Feet (unconstrained)" DstPort 1 } Branch { - ZOrder 43 - Points [0, -160] - DstBlock "Analytical Solution Two Feet (unconstrained)" + ZOrder 79 + DstBlock "QP Two Feet" DstPort 1 } } @@ -4294,11 +4342,11 @@ Model { SrcPort 1 Points [47, 0] Branch { - ZOrder 49 - Points [0, -55] + ZOrder 89 + Points [0, -50] Branch { - ZOrder 48 - Points [0, -100] + ZOrder 92 + Points [0, -105] DstBlock "Analytical Solution Two Feet (unconstrained)" DstPort 2 } @@ -4315,10 +4363,17 @@ Model { } } Line { - ZOrder 51 + 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 } } @@ -4709,7 +4764,7 @@ Model { RequestExecContextInheritance off System { Name "Compute angular momentum integral error" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off @@ -6895,7 +6950,7 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 88 + $ObjectID 90 $ClassName "Simulink.Mask" Display "disp('VISUALIZER')" } @@ -17762,7 +17817,7 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 89 + $ObjectID 91 $ClassName "Simulink.Mask" Display "disp('EMERGENCY STOP')" RunInitForIconRedraw "off" From 28f39757efebf4942016db9bdc42eafafd983a55 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Mon, 3 May 2021 04:35:49 +0200 Subject: [PATCH 08/20] Deactivate Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES --- .../app/robots/iCubGenova04/configRobot.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m index 9c187aa2..efee6ea0 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m @@ -50,7 +50,7 @@ % 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 = true; +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 From 33bb0b91d50f9e7016d0f9c8ae3be679268fae98 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Mon, 3 May 2021 06:24:52 +0200 Subject: [PATCH 09/20] Fixed the robot initial configuration and base position (set to yoga++ configuration) - Base position was set to [0 0 0.619], tuned to get positive contact forces from the very first simulation step. --- .../app/robots/iCubGenova04/configRobotSim.m | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m index f8309010..ed6d82e0 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m @@ -47,17 +47,17 @@ robot_config.N_DOF_MATRIX = eye(robot_config.N_DOF); % Initial condition of iCub and for the integrators. -initialConditions.base_position = [0; 0; 0.65]; +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.1744; 0.0007; 0.0001; ... - -0.1745; 0.4363; 0.6981; 0.2618; ... - -0.1745; 0.4363; 0.6981; 0.2618; ... - 1; 0.0001; -0.0001; -0.0002; -0.0004; 0.0003; ... - 0.0002; 0.0001; -0.0002; 0.0004; -0.5; 0.0003]; + 0; 0; 0; ... + -35.97; 29.97; 0.06; 50.00; ... + -35.97; 29.97; 0.06; 50.00; ... + 0 ; 0 ; 0 ; 0 ; 0; 0; ... + 0 ; 0 ; 0 ; 0 ; 0; 0]*pi/180; % velocty initial conditions initialConditions.base_linear_velocity = [0; 0; 0]; From c495b10bb7776de0c495db5e210d9647cb2aced8 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Mon, 3 May 2021 22:12:41 +0200 Subject: [PATCH 10/20] Added motor reflected inertias --- .../app/robots/iCubGenova04/configRobotSim.m | 60 ++- .../torqueControlBalancingWithSimu.mdl | 459 +++++++++++++----- 2 files changed, 390 insertions(+), 129 deletions(-) diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m index ed6d82e0..8b1e8be1 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m @@ -69,6 +69,7 @@ % Reflected inertia robot_config.SIMULATE_MOTOR_REFLECTED_INERTIA = true; +INCLUDE_COUPLING = true; % Robot frames list FramesSim.BASE = 'root_link'; @@ -95,7 +96,64 @@ % friction coefficient for the feet contact_config.friction_coefficient = 0.1; -% size of the square you see around the robot +%% 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_DOF); +end + +motorsReflectedInertia = wbc.computeMotorsReflectedInertia(Gamma,T,I_m); + +%% 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/torqueControlBalancingWithSimu.mdl b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl index b70a7f34..7f88c080 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl +++ b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl @@ -7,9 +7,24 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "3.98" + ComputedModelVersion "3.107" NumModelReferences 0 - NumTestPointedSignals 0 + NumTestPointedSignals 2 + TestPointedSignal { + SignalName "" + FullBlockPath "torqueControlBalancingWithSimu/Subsystem/Bus Selector" + LogSignal 1 + MaxPoints 5000 + Decimation 2 + } + TestPointedSignal { + SignalName "" + FullBlockPath "torqueControlBalancingWithSimu/Subsystem/Bus Selector" + PortIndex 2 + LogSignal 1 + MaxPoints 5000 + Decimation 2 + } NumProvidedFunctions 0 NumRequiredFunctions 0 NumResetEvents 0 @@ -402,7 +417,7 @@ Model { } Array { Type "Simulink.EditorInfo" - Dimension 65 + Dimension 69 Object { $ObjectID 5 IsActive [1] @@ -419,6 +434,17 @@ Model { IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" + LoadSaveID "4955" + Extents [2171.0, 1188.0] + ZoomFactor [2.5053995680345573] + Offset [79.376400862068976, 189.41206896551725] + SceneRectInView [79.376400862068976, 189.41206896551725, 866.528448275862, 474.1758620689655] + } + Object { + $ObjectID 7 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" LoadSaveID "4537" Extents [2171.0, 1188.0] ZoomFactor [5.1816867469879524] @@ -426,7 +452,7 @@ Model { SceneRectInView [-108.23828125, -66.072916666666657, 418.97553943452374, 229.26897321428569] } Object { - $ObjectID 7 + $ObjectID 8 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -437,7 +463,7 @@ Model { SceneRectInView [-544.74878771551721, -352.2155172413793, 963.84913793103453, 527.43103448275861] } Object { - $ObjectID 8 + $ObjectID 9 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -448,7 +474,62 @@ Model { SceneRectInView [374.06441393490434, 302.07339302379842, 1297.2149221301913, 709.85321395240317] } Object { - $ObjectID 9 + $ObjectID 10 + 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 11 + 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 12 + 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 13 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937:74" + Extents [2171.0, 1188.0] + ZoomFactor [2.79] + Offset [-168.88888888888886, -63.903225806451616] + SceneRectInView [-168.88888888888886, -63.903225806451616, 778.13620071684591, 425.80645161290323] + } + Object { + $ObjectID 14 + 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 15 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -459,7 +540,7 @@ Model { SceneRectInView [124.11704673423424, -38.918426238738732, 1757.9018510698197, 961.94721283783781] } Object { - $ObjectID 10 + $ObjectID 16 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -470,7 +551,7 @@ Model { SceneRectInView [1012.8742726293103, -129.0, 2171.0, 1188.0] } Object { - $ObjectID 11 + $ObjectID 17 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -481,7 +562,7 @@ Model { SceneRectInView [-194.803483319316, -141.48435660218672, 1189.606966638632, 650.96871320437344] } Object { - $ObjectID 12 + $ObjectID 18 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -492,7 +573,7 @@ Model { SceneRectInView [917.234213362069, -427.28793103448277, 975.078448275862, 533.57586206896553] } Object { - $ObjectID 13 + $ObjectID 19 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -503,7 +584,7 @@ Model { SceneRectInView [1438.5531788793103, 818.97931034482758, 778.56551724137933, 426.04137931034484] } Object { - $ObjectID 14 + $ObjectID 20 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -514,18 +595,7 @@ Model { SceneRectInView [-706.68143309105676, -478.27262581168833, 1380.7144286821135, 755.54525162337666] } Object { - $ObjectID 15 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "4955" - Extents [2171.0, 1188.0] - ZoomFactor [2.6403819497920837] - Offset [101.52586181754549, 133.53254783014467] - SceneRectInView [101.52586181754549, 133.53254783014467, 822.229526364909, 449.93490433971067] - } - Object { - $ObjectID 16 + $ObjectID 21 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -536,7 +606,7 @@ Model { SceneRectInView [109.74429086538464, -263.57692307692309, 1043.75, 571.15384615384619] } Object { - $ObjectID 17 + $ObjectID 22 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -547,7 +617,7 @@ Model { SceneRectInView [-326.36442414255714, -208.23320111992533, 748.27572328511428, 409.46640223985065] } Object { - $ObjectID 18 + $ObjectID 23 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -558,7 +628,7 @@ Model { SceneRectInView [-608.35758121641425, -114.33481638525888, 1528.7515828078519, 836.55314618872774] } Object { - $ObjectID 19 + $ObjectID 24 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -569,18 +639,7 @@ Model { SceneRectInView [-116.92703117366869, -144.85219724177151, 889.14932180693916, 486.55430414861524] } Object { - $ObjectID 20 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "4937" - Extents [1360.0, 868.0] - ZoomFactor [1.0] - Offset [155.02749177730379, -243.5] - SceneRectInView [155.02749177730379, -243.5, 1360.0, 868.0] - } - Object { - $ObjectID 21 + $ObjectID 25 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -591,7 +650,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 22 + $ObjectID 26 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -602,7 +661,7 @@ Model { SceneRectInView [-29.932291666666686, -42.75, 432.29166666666669, 247.5] } Object { - $ObjectID 23 + $ObjectID 27 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -613,7 +672,7 @@ Model { SceneRectInView [-29.368338160419114, -144.92583198051949, 808.43198882083823, 462.851663961039] } Object { - $ObjectID 24 + $ObjectID 28 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -624,7 +683,7 @@ Model { SceneRectInView [10.776074743527147, -15.071488336727981, 259.375, 148.5] } Object { - $ObjectID 25 + $ObjectID 29 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -635,7 +694,7 @@ Model { SceneRectInView [578.59213941133362, 441.73077216658527, 393.93290867733265, 225.53845566682949] } Object { - $ObjectID 26 + $ObjectID 30 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -646,7 +705,7 @@ Model { SceneRectInView [-25.756466649159648, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 27 + $ObjectID 31 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -657,7 +716,7 @@ Model { SceneRectInView [-709.226772339876, 2332.9648668093268, 1185.7142857142858, 678.85714285714289] } Object { - $ObjectID 28 + $ObjectID 32 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -668,7 +727,7 @@ Model { SceneRectInView [-1148.4886014344261, -284.77258542366184, 1309.1803278688524, 749.54517084732367] } Object { - $ObjectID 29 + $ObjectID 33 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -679,7 +738,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 30 + $ObjectID 34 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -690,7 +749,7 @@ Model { SceneRectInView [23.229550867122612, 135.34665821934539, 447.67371076575478, 256.30668356130923] } Object { - $ObjectID 31 + $ObjectID 35 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -701,7 +760,7 @@ Model { SceneRectInView [94.721007009531661, 154.55463376836479, 629.70491803278685, 360.52503258937384] } Object { - $ObjectID 32 + $ObjectID 36 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -712,7 +771,7 @@ Model { SceneRectInView [-92.890717237308081, -373.76948051948051, 862.03143447461616, 493.538961038961] } Object { - $ObjectID 33 + $ObjectID 37 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -723,7 +782,7 @@ Model { SceneRectInView [161.34960569474492, -350.98802908437432, 2075.0, 1188.0] } Object { - $ObjectID 34 + $ObjectID 38 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -734,7 +793,7 @@ Model { SceneRectInView [111.71042989741085, -4.1074743527112787, 672.82914020517831, 385.21494870542256] } Object { - $ObjectID 35 + $ObjectID 39 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -745,7 +804,7 @@ Model { SceneRectInView [434.56854543234, -203.35510808500248, 1485.87853413532, 850.710216170005] } Object { - $ObjectID 36 + $ObjectID 40 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -756,7 +815,7 @@ Model { SceneRectInView [-796.48981681034491, -71.967241379310337, 1329.0732758620691, 760.93448275862067] } Object { - $ObjectID 37 + $ObjectID 41 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -767,7 +826,7 @@ Model { SceneRectInView [-113.1169934431619, -154.52586206896552, 670.79741379310349, 384.05172413793105] } Object { - $ObjectID 38 + $ObjectID 42 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -778,7 +837,7 @@ Model { SceneRectInView [87.32421875, 140.1, 207.5, 118.8] } Object { - $ObjectID 39 + $ObjectID 43 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -789,7 +848,7 @@ Model { SceneRectInView [-371.86694504310344, -625.85167594516, 2093.6948275862069, 1198.70335189032] } Object { - $ObjectID 40 + $ObjectID 44 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -800,7 +859,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 41 + $ObjectID 45 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -811,7 +870,7 @@ Model { SceneRectInView [-191.40632054176072, -117.29872084273889, 691.66666666666663, 396.0] } Object { - $ObjectID 42 + $ObjectID 46 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -822,7 +881,7 @@ Model { SceneRectInView [-92.7656132430399, -55.8837471783296, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 43 + $ObjectID 47 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -833,7 +892,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 44 + $ObjectID 48 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -844,7 +903,7 @@ Model { SceneRectInView [-99.198645598194133, -48.972911963882623, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 45 + $ObjectID 49 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -855,7 +914,7 @@ Model { SceneRectInView [-80.667161130818016, -31.015048908954071, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 46 + $ObjectID 50 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -866,7 +925,7 @@ Model { SceneRectInView [13.879095336481726, -85.310064935064929, 502.36680932703655, 287.62012987012986] } Object { - $ObjectID 47 + $ObjectID 51 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -877,7 +936,7 @@ Model { SceneRectInView [13.972911622276058, -85.325665859564168, 502.42130750605327, 287.65133171912834] } Object { - $ObjectID 48 + $ObjectID 52 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -888,7 +947,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 49 + $ObjectID 53 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -899,7 +958,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 50 + $ObjectID 54 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -910,7 +969,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 51 + $ObjectID 55 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -921,7 +980,7 @@ Model { SceneRectInView [-64.168329397874857, -71.912514757969319, 518.75, 297.0] } Object { - $ObjectID 52 + $ObjectID 56 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -932,7 +991,7 @@ Model { SceneRectInView [-684.37478764306115, -290.08334454895737, 840.66666666666652, 579.97212543554] } Object { - $ObjectID 53 + $ObjectID 57 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -943,7 +1002,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 54 + $ObjectID 58 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -954,7 +1013,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 55 + $ObjectID 59 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -965,7 +1024,7 @@ Model { SceneRectInView [203.38965517241377, -277.26034482758621, 524.02241379310351, 361.52068965517242] } Object { - $ObjectID 56 + $ObjectID 60 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -976,7 +1035,7 @@ Model { SceneRectInView [-8.0130751303127283, -114.03393582177664, 473.82302526062546, 223.19657177225341] } Object { - $ObjectID 57 + $ObjectID 61 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -987,7 +1046,7 @@ Model { SceneRectInView [16.019427876904615, -398.80851543704892, 1030.9142692461908, 485.61703087409785] } Object { - $ObjectID 58 + $ObjectID 62 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -998,7 +1057,7 @@ Model { SceneRectInView [-105.54956270048115, -242.68495036561927, 864.60693790096229, 407.2771777265437] } Object { - $ObjectID 59 + $ObjectID 63 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1009,7 +1068,7 @@ Model { SceneRectInView [-396.83333333333337, -304.36666666666662, 1148.0, 792.0] } Object { - $ObjectID 60 + $ObjectID 64 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1020,7 +1079,7 @@ Model { SceneRectInView [34.9092200413223, -92.857244318181813, 438.7831224173554, 302.71448863636363] } Object { - $ObjectID 61 + $ObjectID 65 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1031,7 +1090,7 @@ Model { SceneRectInView [-183.3666666666665, -165.10000000000005, 867.6, 475.2] } Object { - $ObjectID 62 + $ObjectID 66 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1042,7 +1101,7 @@ Model { SceneRectInView [10.41989407940855, -23.235071395084702, 501.20498970990775, 274.51891552575859] } Object { - $ObjectID 63 + $ObjectID 67 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1053,7 +1112,7 @@ Model { SceneRectInView [-68.684210488152473, -33.394738158739756, 477.63157399976672, 216.05262934094131] } Object { - $ObjectID 64 + $ObjectID 68 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1064,7 +1123,7 @@ Model { SceneRectInView [-211.69612560424537, 280.84982585720286, 605.0, 273.66666666666669] } Object { - $ObjectID 65 + $ObjectID 69 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1075,7 +1134,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 66 + $ObjectID 70 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1086,7 +1145,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 67 + $ObjectID 71 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1097,7 +1156,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 68 + $ObjectID 72 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1108,7 +1167,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 69 + $ObjectID 73 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1124,27 +1183,27 @@ Model { Type "Simulink.DockComponentInfo" Dimension 2 Object { - $ObjectID 70 - Type "GLUE2:PropertyInspector" - ID "Property Inspector" + $ObjectID 74 + Type "Simulink:Editor:ReferencedFiles" + ID "Referenced Files" Visible [0] CreateCallback "" - UserData "" + UserData "{\"filterShowRefModels\":\"true\",\"filterShowRefSubs\":\"true\",\"filterShowOnlyDirtyFiles\":\"false\"}\n" Floating [0] - DockPosition "Right" + DockPosition "Left" Width [640] Height [480] Minimized "Unset" } Object { - $ObjectID 71 - Type "Simulink:Editor:ReferencedFiles" - ID "Referenced Files" + $ObjectID 75 + Type "GLUE2:PropertyInspector" + ID "Property Inspector" Visible [0] CreateCallback "" - UserData "{\"filterShowRefModels\":\"true\",\"filterShowRefSubs\":\"true\",\"filterShowOnlyDirtyFiles\":\"false\"}\n" + UserData "" Floating [0] - DockPosition "Left" + DockPosition "Right" Width [640] Height [480] Minimized "Unset" @@ -1176,9 +1235,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "nunoguedelha" ModifiedDateFormat "%" - LastModifiedDate "Sun May 02 23:19:18 2021" - RTWModifiedTimeStamp 541898351 - ModelVersionFormat "%" + LastModifiedDate "Mon May 03 15:03:03 2021" + RTWModifiedTimeStamp 541954205 + ModelVersionFormat "%" SampleTimeColors on SampleTimeAnnotations on LibraryLinkDisplay "disabled" @@ -1235,7 +1294,7 @@ Model { TryForcingSFcnDF off Object { $PropName "DataLoggingOverride" - $ObjectID 72 + $ObjectID 76 $ClassName "Simulink.SimulationData.ModelLoggingInfo" model_ "torqueControlBalancingWithSimu" signals_ [] @@ -1253,6 +1312,64 @@ Model { PropName "logAsSpecifiedByModelsSSIDs_" } } + Object { + $PropName "InstrumentedSignals" + $ObjectID 77 + $ClassName "Simulink.HMI.InstrumentedSignals" + Array { + Type "Struct" + Dimension 2 + MATStruct { + UUID "361a222e-8799-4264-88ff-83ceed2da1a7" + BlockPath_ "Subsystem/Bus Selector" + SID_ "5009" + SubPath_ "" + OutputPortIndex_ [1.0] + LogicalPortIndex_ [0.0] + SignalName_ "" + SubSysPath_ "" + Decimation_ [1.0] + MaxPoints_ [1024.0] + TargetBufferedStreaming_ [0.0] + IsFrameBased_ [0.0] + HideInSDI_ [0.0] + DomainType_ "" + Array { + Type "Struct" + Dimension 1 + MATStruct { + } + PropName "DomainParams_" + } + VisualType_ "" + } + MATStruct { + UUID "dddab1a3-2010-48e7-907b-c669add332b6" + BlockPath_ "Subsystem/Bus Selector" + SID_ "5009" + SubPath_ "" + OutputPortIndex_ [2.0] + LogicalPortIndex_ [0.0] + SignalName_ "" + SubSysPath_ "" + Decimation_ [1.0] + MaxPoints_ [1024.0] + TargetBufferedStreaming_ [0.0] + IsFrameBased_ [0.0] + HideInSDI_ [0.0] + DomainType_ "" + Array { + Type "Struct" + Dimension 1 + MATStruct { + } + PropName "DomainParams_" + } + VisualType_ "" + } + PropName "Persistence" + } + } ExtModeBatchMode off ExtModeEnableFloating on ExtModeTrigType "manual" @@ -1283,7 +1400,7 @@ Model { Type "Handle" Dimension 1 Simulink.ConfigSet { - $ObjectID 73 + $ObjectID 78 Version "20.1.0" DisabledProps [] Description "" @@ -1291,7 +1408,7 @@ Model { Type "Handle" Dimension 9 Simulink.SolverCC { - $ObjectID 74 + $ObjectID 79 Version "20.1.0" DisabledProps [] Description "" @@ -1333,7 +1450,7 @@ Model { ODENIntegrationMethod "ode3" } Simulink.DataIOCC { - $ObjectID 75 + $ObjectID 80 Version "20.1.0" DisabledProps [] Description "" @@ -1375,7 +1492,7 @@ Model { LoggingIntervals "[-inf, inf]" } Simulink.OptimizationCC { - $ObjectID 76 + $ObjectID 81 Version "20.1.0" Array { Type "Cell" @@ -1449,7 +1566,7 @@ Model { EfficientTunableParamExpr off } Simulink.DebuggingCC { - $ObjectID 77 + $ObjectID 82 Version "20.1.0" Array { Type "Cell" @@ -1572,7 +1689,7 @@ Model { VariantConditionMismatch "none" } Simulink.HardwareCC { - $ObjectID 78 + $ObjectID 83 Version "20.1.0" DisabledProps [] Description "" @@ -1622,7 +1739,7 @@ Model { HardwareBoardFeatureSet "EmbeddedCoderHSP" } Simulink.ModelReferenceCC { - $ObjectID 79 + $ObjectID 84 Version "20.1.0" DisabledProps [] Description "" @@ -1642,7 +1759,7 @@ Model { SupportModelReferenceSimTargetCustomCode off } Simulink.SFSimCC { - $ObjectID 80 + $ObjectID 85 Version "20.1.0" DisabledProps [] Description "" @@ -1686,7 +1803,7 @@ Model { } Simulink.RTWCC { $BackupClass "Simulink.RTWCC" - $ObjectID 81 + $ObjectID 86 Version "20.1.0" Array { Type "Cell" @@ -1792,7 +1909,7 @@ Model { Type "Handle" Dimension 2 Simulink.CodeAppCC { - $ObjectID 82 + $ObjectID 87 Version "20.1.0" Array { Type "Cell" @@ -1882,7 +1999,7 @@ Model { } Simulink.GRTTargetCC { $BackupClass "Simulink.TargetCC" - $ObjectID 83 + $ObjectID 88 Version "20.1.0" Array { Type "Cell" @@ -1912,7 +2029,7 @@ Model { Type "Handle" Dimension 1 Simulink.CPPComponent { - $ObjectID 84 + $ObjectID 89 Version "20.1.0" Array { Type "Cell" @@ -2023,7 +2140,7 @@ Model { } } SlCovCC.ConfigComp { - $ObjectID 85 + $ObjectID 90 Version "20.1.0" DisabledProps [] Description "Simulink Coverage Configuration Component" @@ -2072,11 +2189,11 @@ Model { } Simulink.ConfigSet { $PropName "ActiveConfigurationSet" - $ObjectID 73 + $ObjectID 78 } Object { $PropName "DataTransfer" - $ObjectID 86 + $ObjectID 91 $ClassName "Simulink.GlobalDataTransfer" DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" @@ -2181,6 +2298,14 @@ Model { DisplayOption "bar" BusSelectionMode off } + Block { + BlockType Display + Format "short" + Decimation "10" + Floating off + Lockdown off + SampleTime "-1" + } Block { BlockType EnablePort StatesWhenEnabling "held" @@ -2460,7 +2585,7 @@ Model { ShowPageBoundaries off ZoomFactor "203" ReportName "simulink-default.rpt" - SIDHighWatermark "5007" + SIDHighWatermark "5012" SimulinkSubDomain "Simulink" Block { BlockType BusCreator @@ -2570,7 +2695,7 @@ Model { Value "zeros(3,1)" Object { $PropName "MaskObject" - $ObjectID 87 + $ObjectID 92 $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." @@ -2578,12 +2703,12 @@ Model { RunInitForIconRedraw "off" Object { $PropName "DialogControls" - $ObjectID 88 + $ObjectID 93 $ClassName "Simulink.dialog.Group" Prompt "%" Object { $PropName "DialogControls" - $ObjectID 89 + $ObjectID 94 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -2592,6 +2717,14 @@ Model { } } } + Block { + BlockType Constant + Name "Constant1" + SID "5012" + Position [1055, 320, 1115, 350] + ZOrder 1289 + Value "zeros(23,1)" + } Block { BlockType SubSystem Name "MOMENTUM BASED TORQUE CONTROL" @@ -4234,7 +4367,7 @@ Model { Name "QP Two Feet" SID "4681" Ports [5, 2] - Position [1575, 1015, 1685, 1165] + Position [1575, 1019, 1685, 1171] ZOrder 752 LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/QP" @@ -4269,7 +4402,7 @@ Model { ZOrder 37 SrcBlock "QP Two Feet" SrcPort 2 - Points [23, 0] + Points [0, -5; 23, 0] Branch { ZOrder 91 Points [0, -25] @@ -4287,6 +4420,7 @@ Model { ZOrder 38 SrcBlock "A" SrcPort 1 + Points [65, 0] DstBlock "QP Two Feet" DstPort 3 } @@ -4316,6 +4450,7 @@ Model { ZOrder 42 SrcBlock "QP Two Feet" SrcPort 1 + Points [0, -5] DstBlock "MatchSignalSizes" DstPort 2 } @@ -4332,6 +4467,7 @@ Model { } Branch { ZOrder 79 + Points [43, 0] DstBlock "QP Two Feet" DstPort 1 } @@ -4358,6 +4494,7 @@ Model { } Branch { ZOrder 46 + Points [18, 0] DstBlock "QP Two Feet" DstPort 2 } @@ -4366,6 +4503,7 @@ Model { ZOrder 93 SrcBlock "ubA" SrcPort 1 + Points [65, 0] DstBlock "QP Two Feet" DstPort 5 } @@ -4373,6 +4511,7 @@ Model { ZOrder 94 SrcBlock "Constant" SrcPort 1 + Points [30, 0] DstBlock "QP Two Feet" DstPort 4 } @@ -6950,7 +7089,7 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 90 + $ObjectID 95 $ClassName "Simulink.Mask" Display "disp('VISUALIZER')" } @@ -17817,7 +17956,7 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 91 + $ObjectID 96 $ClassName "Simulink.Mask" Display "disp('EMERGENCY STOP')" RunInitForIconRedraw "off" @@ -18880,7 +19019,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "264" + ZoomFactor "251" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -18889,6 +19028,23 @@ Model { Position [120, 258, 150, 272] ZOrder 1275 } + Block { + BlockType BusSelector + Name "Bus\nSelector" + SID "5009" + Ports [1, 2] + Position [120, 556, 125, 594] + ZOrder 1282 + OutputSignals "fc,left_right_foot_in_contact" + Port { + PortNumber 1 + Name "" + } + Port { + PortNumber 2 + Name "" + } + } Block { BlockType BusSelector Name "Bus\nSelector1" @@ -18949,7 +19105,26 @@ Model { SID "4941" Position [120, 320, 150, 350] ZOrder 1273 - Value "zeros(robot_config.N_DOF,1)" + Value "motorsReflectedInertia" + VectorParams1D off + } + Block { + BlockType Display + Name "Display" + SID "5010" + Ports [1] + Position [340, 472, 420, 658] + ZOrder 1283 + Decimation "1" + } + Block { + BlockType Display + Name "Display1" + SID "5011" + Ports [1] + Position [215, 602, 305, 638] + ZOrder 1284 + Decimation "1" } Block { BlockType Reference @@ -19107,8 +19282,17 @@ Model { SrcBlock "RobotDynWithContacts" SrcPort 4 Points [23, 0; 0, 105] - DstBlock "Bus\nSelector1" - DstPort 1 + Branch { + ZOrder 339 + Points [-307, 0; 0, 125] + DstBlock "Bus\nSelector" + DstPort 1 + } + Branch { + ZOrder 338 + DstBlock "Bus\nSelector1" + DstPort 1 + } } Line { Name "" @@ -19136,6 +19320,25 @@ Model { DstBlock "wrench_LFoot" DstPort 1 } + Line { + Name "" + ZOrder 340 + Labels [1, 1] + SrcBlock "Bus\nSelector" + SrcPort 1 + DstBlock "Display" + DstPort 1 + } + Line { + Name "" + ZOrder 341 + Labels [-1, 1] + SrcBlock "Bus\nSelector" + SrcPort 2 + Points [38, 0; 0, 35] + DstBlock "Display1" + DstPort 1 + } } } Line { @@ -19328,7 +19531,7 @@ Model { DstPort 1 } Line { - ZOrder 407 + ZOrder 437 SrcBlock "Memory" SrcPort 1 DstBlock "Subsystem" From 34c82b905e04c8941151e92b7b0b316bc891dfcf Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Mon, 3 May 2021 23:36:45 +0200 Subject: [PATCH 11/20] Add friction torques to the simulator input, and treat RobotVisualizer as an atomic unit at 40ms sampling time --- .../app/robots/iCubGenova04/configRobotSim.m | 5 +- .../torqueControlBalancingWithSimu.mdl | 2128 ++++++++++------- 2 files changed, 1308 insertions(+), 825 deletions(-) diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m index 8b1e8be1..6a5ea839 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m @@ -148,11 +148,14 @@ if INCLUDE_COUPLING T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12)); else - T = eye(ROBOT_DOF); + T = eye(robot_config.N_DOF); end motorsReflectedInertia = wbc.computeMotorsReflectedInertia(Gamma,T,I_m); +KvmechMat = diag(0.5*ones(1,robot_config.N_DOF)); +jointFrictionMat = wbc.computeMotorsReflectedInertia(eye(robot_config.N_DOF),T,KvmechMat); + %% size of the square you see around the robot visualizerAroundRobot = 1; % mt diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl index 7f88c080..6b47dd7d 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl +++ b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl @@ -7,7 +7,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "3.107" + ComputedModelVersion "3.116" NumModelReferences 0 NumTestPointedSignals 2 TestPointedSignal { @@ -329,7 +329,7 @@ Model { } ExternalFileReference { Reference "mwbs_visualizers_lib/RobotVisualizer" - Path "torqueControlBalancingWithSimu/RobotVisualizer" + Path "torqueControlBalancingWithSimu/Robot Visualizer/RobotVisualizer" SID "4939" Type "LIBRARY_BLOCK" } @@ -417,7 +417,7 @@ Model { } Array { Type "Simulink.EditorInfo" - Dimension 69 + Dimension 73 Object { $ObjectID 5 IsActive [1] @@ -425,81 +425,103 @@ Model { ViewObjType "SimulinkTopLevel" LoadSaveID "0" Extents [2171.0, 1188.0] - ZoomFactor [2.0285911286881269] - Offset [713.18484189255014, 156.19184020956072] - SceneRectInView [713.18484189255014, 156.19184020956072, 1070.2008745369835, 585.62811559186389] + ZoomFactor [2.0281179051220715] + Offset [712.93674073651232, 152.61242825136725] + SceneRectInView [712.93674073651232, 152.61242825136725, 1070.4505859925971, 585.76476101299193] } Object { $ObjectID 6 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4955" + LoadSaveID "5014" Extents [2171.0, 1188.0] - ZoomFactor [2.5053995680345573] - Offset [79.376400862068976, 189.41206896551725] - SceneRectInView [79.376400862068976, 189.41206896551725, 866.528448275862, 474.1758620689655] + ZoomFactor [2.03] + Offset [-310.81869612068976, -180.61083743842369] + SceneRectInView [-310.81869612068976, -180.61083743842369, 1069.4581280788179, 585.22167487684737] } Object { $ObjectID 7 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4537" + LoadSaveID "4937:34" Extents [2171.0, 1188.0] - ZoomFactor [5.1816867469879524] - Offset [-108.23828125, -66.072916666666657] - SceneRectInView [-108.23828125, -66.072916666666657, 418.97553943452374, 229.26897321428569] + ZoomFactor [2.92] + Offset [-164.07534246575341, -91.924657534246592] + SceneRectInView [-164.07534246575341, -91.924657534246592, 743.49315068493149, 406.84931506849318] } Object { $ObjectID 8 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4579" + LoadSaveID "4836" Extents [2171.0, 1188.0] - ZoomFactor [2.2524271844660193] - Offset [-544.74878771551721, -352.2155172413793] - SceneRectInView [-544.74878771551721, -352.2155172413793, 963.84913793103453, 527.43103448275861] + ZoomFactor [1.2349950019557565] + Offset [124.11704673423424, -38.918426238738732] + SceneRectInView [124.11704673423424, -38.918426238738732, 1757.9018510698197, 961.94721283783781] } Object { $ObjectID 9 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4393" + LoadSaveID "4537" Extents [2171.0, 1188.0] - ZoomFactor [1.6735854351990824] - Offset [374.06441393490434, 302.07339302379842] - SceneRectInView [374.06441393490434, 302.07339302379842, 1297.2149221301913, 709.85321395240317] + ZoomFactor [5.1816867469879524] + Offset [-108.23828125, -66.072916666666657] + SceneRectInView [-108.23828125, -66.072916666666657, 418.97553943452374, 229.26897321428569] } Object { $ObjectID 10 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937:27" + LoadSaveID "4937:14" Extents [2171.0, 1188.0] ZoomFactor [2.85] - Offset [-173.20175438596488, -115.42105263157893] - SceneRectInView [-173.20175438596488, -115.42105263157893, 761.75438596491222, 416.84210526315786] + Offset [-165.70175438596488, -113.92105263157893] + SceneRectInView [-165.70175438596488, -113.92105263157893, 761.75438596491222, 416.84210526315786] } Object { $ObjectID 11 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937:21" + LoadSaveID "4955" Extents [2171.0, 1188.0] - ZoomFactor [2.85] - Offset [-165.70175438596488, -113.92105263157893] - SceneRectInView [-165.70175438596488, -113.92105263157893, 761.75438596491222, 416.84210526315786] + ZoomFactor [2.0773052223433903] + Offset [-119.91137569995328, 98.052613159122757] + SceneRectInView [-119.91137569995328, 98.052613159122757, 1045.1040013999066, 571.89477368175449] } Object { $ObjectID 12 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 13 + 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 14 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" LoadSaveID "4937" Extents [2171.0, 1188.0] ZoomFactor [1.0] @@ -507,40 +529,73 @@ Model { SceneRectInView [-327.9765625, -403.5, 2171.0, 1188.0] } Object { - $ObjectID 13 + $ObjectID 15 + 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 16 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" LoadSaveID "4937:74" Extents [2171.0, 1188.0] - ZoomFactor [2.79] - Offset [-168.88888888888886, -63.903225806451616] - SceneRectInView [-168.88888888888886, -63.903225806451616, 778.13620071684591, 425.80645161290323] + ZoomFactor [3.0] + Offset [-132.82012310290415, -16.387940803946396] + SceneRectInView [-132.82012310290415, -16.387940803946396, 723.66666666666663, 396.0] } Object { - $ObjectID 14 + $ObjectID 17 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937:14" + 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 18 + 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 19 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4937:27" Extents [2171.0, 1188.0] ZoomFactor [2.85] - Offset [-165.70175438596488, -113.92105263157893] - SceneRectInView [-165.70175438596488, -113.92105263157893, 761.75438596491222, 416.84210526315786] + Offset [-173.20175438596488, -115.42105263157893] + SceneRectInView [-173.20175438596488, -115.42105263157893, 761.75438596491222, 416.84210526315786] } Object { - $ObjectID 15 + $ObjectID 20 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4836" + LoadSaveID "4937:21" Extents [2171.0, 1188.0] - ZoomFactor [1.2349950019557565] - Offset [124.11704673423424, -38.918426238738732] - SceneRectInView [124.11704673423424, -38.918426238738732, 1757.9018510698197, 961.94721283783781] + ZoomFactor [2.85] + Offset [-165.70175438596488, -113.92105263157893] + SceneRectInView [-165.70175438596488, -113.92105263157893, 761.75438596491222, 416.84210526315786] } Object { - $ObjectID 16 + $ObjectID 21 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -551,7 +606,7 @@ Model { SceneRectInView [1012.8742726293103, -129.0, 2171.0, 1188.0] } Object { - $ObjectID 17 + $ObjectID 22 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -562,7 +617,7 @@ Model { SceneRectInView [-194.803483319316, -141.48435660218672, 1189.606966638632, 650.96871320437344] } Object { - $ObjectID 18 + $ObjectID 23 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -573,7 +628,7 @@ Model { SceneRectInView [917.234213362069, -427.28793103448277, 975.078448275862, 533.57586206896553] } Object { - $ObjectID 19 + $ObjectID 24 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -584,7 +639,7 @@ Model { SceneRectInView [1438.5531788793103, 818.97931034482758, 778.56551724137933, 426.04137931034484] } Object { - $ObjectID 20 + $ObjectID 25 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -595,7 +650,7 @@ Model { SceneRectInView [-706.68143309105676, -478.27262581168833, 1380.7144286821135, 755.54525162337666] } Object { - $ObjectID 21 + $ObjectID 26 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -606,18 +661,7 @@ Model { SceneRectInView [109.74429086538464, -263.57692307692309, 1043.75, 571.15384615384619] } Object { - $ObjectID 22 - 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 23 + $ObjectID 27 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -628,7 +672,7 @@ Model { SceneRectInView [-608.35758121641425, -114.33481638525888, 1528.7515828078519, 836.55314618872774] } Object { - $ObjectID 24 + $ObjectID 28 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -639,7 +683,7 @@ Model { SceneRectInView [-116.92703117366869, -144.85219724177151, 889.14932180693916, 486.55430414861524] } Object { - $ObjectID 25 + $ObjectID 29 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -650,7 +694,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 26 + $ObjectID 30 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -661,7 +705,7 @@ Model { SceneRectInView [-29.932291666666686, -42.75, 432.29166666666669, 247.5] } Object { - $ObjectID 27 + $ObjectID 31 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -672,7 +716,7 @@ Model { SceneRectInView [-29.368338160419114, -144.92583198051949, 808.43198882083823, 462.851663961039] } Object { - $ObjectID 28 + $ObjectID 32 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -683,7 +727,7 @@ Model { SceneRectInView [10.776074743527147, -15.071488336727981, 259.375, 148.5] } Object { - $ObjectID 29 + $ObjectID 33 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -694,7 +738,7 @@ Model { SceneRectInView [578.59213941133362, 441.73077216658527, 393.93290867733265, 225.53845566682949] } Object { - $ObjectID 30 + $ObjectID 34 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -705,7 +749,7 @@ Model { SceneRectInView [-25.756466649159648, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 31 + $ObjectID 35 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -716,7 +760,7 @@ Model { SceneRectInView [-709.226772339876, 2332.9648668093268, 1185.7142857142858, 678.85714285714289] } Object { - $ObjectID 32 + $ObjectID 36 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -727,7 +771,7 @@ Model { SceneRectInView [-1148.4886014344261, -284.77258542366184, 1309.1803278688524, 749.54517084732367] } Object { - $ObjectID 33 + $ObjectID 37 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -738,7 +782,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 34 + $ObjectID 38 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -749,7 +793,7 @@ Model { SceneRectInView [23.229550867122612, 135.34665821934539, 447.67371076575478, 256.30668356130923] } Object { - $ObjectID 35 + $ObjectID 39 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -760,7 +804,7 @@ Model { SceneRectInView [94.721007009531661, 154.55463376836479, 629.70491803278685, 360.52503258937384] } Object { - $ObjectID 36 + $ObjectID 40 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -771,7 +815,7 @@ Model { SceneRectInView [-92.890717237308081, -373.76948051948051, 862.03143447461616, 493.538961038961] } Object { - $ObjectID 37 + $ObjectID 41 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -782,7 +826,7 @@ Model { SceneRectInView [161.34960569474492, -350.98802908437432, 2075.0, 1188.0] } Object { - $ObjectID 38 + $ObjectID 42 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -793,7 +837,7 @@ Model { SceneRectInView [111.71042989741085, -4.1074743527112787, 672.82914020517831, 385.21494870542256] } Object { - $ObjectID 39 + $ObjectID 43 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -804,7 +848,7 @@ Model { SceneRectInView [434.56854543234, -203.35510808500248, 1485.87853413532, 850.710216170005] } Object { - $ObjectID 40 + $ObjectID 44 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -815,7 +859,7 @@ Model { SceneRectInView [-796.48981681034491, -71.967241379310337, 1329.0732758620691, 760.93448275862067] } Object { - $ObjectID 41 + $ObjectID 45 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -826,7 +870,7 @@ Model { SceneRectInView [-113.1169934431619, -154.52586206896552, 670.79741379310349, 384.05172413793105] } Object { - $ObjectID 42 + $ObjectID 46 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -837,7 +881,7 @@ Model { SceneRectInView [87.32421875, 140.1, 207.5, 118.8] } Object { - $ObjectID 43 + $ObjectID 47 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -848,7 +892,7 @@ Model { SceneRectInView [-371.86694504310344, -625.85167594516, 2093.6948275862069, 1198.70335189032] } Object { - $ObjectID 44 + $ObjectID 48 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -859,7 +903,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 45 + $ObjectID 49 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -870,7 +914,7 @@ Model { SceneRectInView [-191.40632054176072, -117.29872084273889, 691.66666666666663, 396.0] } Object { - $ObjectID 46 + $ObjectID 50 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -881,7 +925,7 @@ Model { SceneRectInView [-92.7656132430399, -55.8837471783296, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 47 + $ObjectID 51 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -892,7 +936,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 48 + $ObjectID 52 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -903,7 +947,7 @@ Model { SceneRectInView [-99.198645598194133, -48.972911963882623, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 49 + $ObjectID 53 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -914,7 +958,7 @@ Model { SceneRectInView [-80.667161130818016, -31.015048908954071, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 50 + $ObjectID 54 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -925,7 +969,7 @@ Model { SceneRectInView [13.879095336481726, -85.310064935064929, 502.36680932703655, 287.62012987012986] } Object { - $ObjectID 51 + $ObjectID 55 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -936,7 +980,7 @@ Model { SceneRectInView [13.972911622276058, -85.325665859564168, 502.42130750605327, 287.65133171912834] } Object { - $ObjectID 52 + $ObjectID 56 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -947,7 +991,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 53 + $ObjectID 57 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -958,7 +1002,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 54 + $ObjectID 58 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -969,7 +1013,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 55 + $ObjectID 59 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -980,7 +1024,7 @@ Model { SceneRectInView [-64.168329397874857, -71.912514757969319, 518.75, 297.0] } Object { - $ObjectID 56 + $ObjectID 60 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -991,7 +1035,7 @@ Model { SceneRectInView [-684.37478764306115, -290.08334454895737, 840.66666666666652, 579.97212543554] } Object { - $ObjectID 57 + $ObjectID 61 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1002,7 +1046,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 58 + $ObjectID 62 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1013,7 +1057,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 59 + $ObjectID 63 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1024,7 +1068,7 @@ Model { SceneRectInView [203.38965517241377, -277.26034482758621, 524.02241379310351, 361.52068965517242] } Object { - $ObjectID 60 + $ObjectID 64 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1035,7 +1079,7 @@ Model { SceneRectInView [-8.0130751303127283, -114.03393582177664, 473.82302526062546, 223.19657177225341] } Object { - $ObjectID 61 + $ObjectID 65 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1046,7 +1090,7 @@ Model { SceneRectInView [16.019427876904615, -398.80851543704892, 1030.9142692461908, 485.61703087409785] } Object { - $ObjectID 62 + $ObjectID 66 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1057,7 +1101,7 @@ Model { SceneRectInView [-105.54956270048115, -242.68495036561927, 864.60693790096229, 407.2771777265437] } Object { - $ObjectID 63 + $ObjectID 67 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1068,7 +1112,7 @@ Model { SceneRectInView [-396.83333333333337, -304.36666666666662, 1148.0, 792.0] } Object { - $ObjectID 64 + $ObjectID 68 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1079,7 +1123,7 @@ Model { SceneRectInView [34.9092200413223, -92.857244318181813, 438.7831224173554, 302.71448863636363] } Object { - $ObjectID 65 + $ObjectID 69 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1090,7 +1134,7 @@ Model { SceneRectInView [-183.3666666666665, -165.10000000000005, 867.6, 475.2] } Object { - $ObjectID 66 + $ObjectID 70 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1101,7 +1145,7 @@ Model { SceneRectInView [10.41989407940855, -23.235071395084702, 501.20498970990775, 274.51891552575859] } Object { - $ObjectID 67 + $ObjectID 71 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1112,7 +1156,7 @@ Model { SceneRectInView [-68.684210488152473, -33.394738158739756, 477.63157399976672, 216.05262934094131] } Object { - $ObjectID 68 + $ObjectID 72 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1123,7 +1167,7 @@ Model { SceneRectInView [-211.69612560424537, 280.84982585720286, 605.0, 273.66666666666669] } Object { - $ObjectID 69 + $ObjectID 73 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1134,7 +1178,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 70 + $ObjectID 74 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1145,7 +1189,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 71 + $ObjectID 75 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1156,7 +1200,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 72 + $ObjectID 76 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1167,7 +1211,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 73 + $ObjectID 77 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1183,27 +1227,27 @@ Model { Type "Simulink.DockComponentInfo" Dimension 2 Object { - $ObjectID 74 - Type "Simulink:Editor:ReferencedFiles" - ID "Referenced Files" + $ObjectID 78 + Type "GLUE2:PropertyInspector" + ID "Property Inspector" Visible [0] CreateCallback "" - UserData "{\"filterShowRefModels\":\"true\",\"filterShowRefSubs\":\"true\",\"filterShowOnlyDirtyFiles\":\"false\"}\n" + UserData "" Floating [0] - DockPosition "Left" + DockPosition "Right" Width [640] Height [480] Minimized "Unset" } Object { - $ObjectID 75 - Type "GLUE2:PropertyInspector" - ID "Property Inspector" + $ObjectID 79 + Type "Simulink:Editor:ReferencedFiles" + ID "Referenced Files" Visible [0] CreateCallback "" - UserData "" + UserData "{\"filterShowRefModels\":\"true\",\"filterShowRefSubs\":\"true\",\"filterShowOnlyDirtyFiles\":\"false\"}\n" Floating [0] - DockPosition "Right" + DockPosition "Left" Width [640] Height [480] Minimized "Unset" @@ -1235,9 +1279,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "nunoguedelha" ModifiedDateFormat "%" - LastModifiedDate "Mon May 03 15:03:03 2021" - RTWModifiedTimeStamp 541954205 - ModelVersionFormat "%" + LastModifiedDate "Mon May 03 23:34:59 2021" + RTWModifiedTimeStamp 541984483 + ModelVersionFormat "%" SampleTimeColors on SampleTimeAnnotations on LibraryLinkDisplay "disabled" @@ -1294,7 +1338,7 @@ Model { TryForcingSFcnDF off Object { $PropName "DataLoggingOverride" - $ObjectID 76 + $ObjectID 80 $ClassName "Simulink.SimulationData.ModelLoggingInfo" model_ "torqueControlBalancingWithSimu" signals_ [] @@ -1314,7 +1358,7 @@ Model { } Object { $PropName "InstrumentedSignals" - $ObjectID 77 + $ObjectID 81 $ClassName "Simulink.HMI.InstrumentedSignals" Array { Type "Struct" @@ -1400,7 +1444,7 @@ Model { Type "Handle" Dimension 1 Simulink.ConfigSet { - $ObjectID 78 + $ObjectID 82 Version "20.1.0" DisabledProps [] Description "" @@ -1408,7 +1452,7 @@ Model { Type "Handle" Dimension 9 Simulink.SolverCC { - $ObjectID 79 + $ObjectID 83 Version "20.1.0" DisabledProps [] Description "" @@ -1450,7 +1494,7 @@ Model { ODENIntegrationMethod "ode3" } Simulink.DataIOCC { - $ObjectID 80 + $ObjectID 84 Version "20.1.0" DisabledProps [] Description "" @@ -1492,7 +1536,7 @@ Model { LoggingIntervals "[-inf, inf]" } Simulink.OptimizationCC { - $ObjectID 81 + $ObjectID 85 Version "20.1.0" Array { Type "Cell" @@ -1566,7 +1610,7 @@ Model { EfficientTunableParamExpr off } Simulink.DebuggingCC { - $ObjectID 82 + $ObjectID 86 Version "20.1.0" Array { Type "Cell" @@ -1689,7 +1733,7 @@ Model { VariantConditionMismatch "none" } Simulink.HardwareCC { - $ObjectID 83 + $ObjectID 87 Version "20.1.0" DisabledProps [] Description "" @@ -1739,7 +1783,7 @@ Model { HardwareBoardFeatureSet "EmbeddedCoderHSP" } Simulink.ModelReferenceCC { - $ObjectID 84 + $ObjectID 88 Version "20.1.0" DisabledProps [] Description "" @@ -1759,7 +1803,7 @@ Model { SupportModelReferenceSimTargetCustomCode off } Simulink.SFSimCC { - $ObjectID 85 + $ObjectID 89 Version "20.1.0" DisabledProps [] Description "" @@ -1803,7 +1847,7 @@ Model { } Simulink.RTWCC { $BackupClass "Simulink.RTWCC" - $ObjectID 86 + $ObjectID 90 Version "20.1.0" Array { Type "Cell" @@ -1909,7 +1953,7 @@ Model { Type "Handle" Dimension 2 Simulink.CodeAppCC { - $ObjectID 87 + $ObjectID 91 Version "20.1.0" Array { Type "Cell" @@ -1999,7 +2043,7 @@ Model { } Simulink.GRTTargetCC { $BackupClass "Simulink.TargetCC" - $ObjectID 88 + $ObjectID 92 Version "20.1.0" Array { Type "Cell" @@ -2029,7 +2073,7 @@ Model { Type "Handle" Dimension 1 Simulink.CPPComponent { - $ObjectID 89 + $ObjectID 93 Version "20.1.0" Array { Type "Cell" @@ -2140,7 +2184,7 @@ Model { } } SlCovCC.ConfigComp { - $ObjectID 90 + $ObjectID 94 Version "20.1.0" DisabledProps [] Description "Simulink Coverage Configuration Component" @@ -2189,11 +2233,11 @@ Model { } Simulink.ConfigSet { $PropName "ActiveConfigurationSet" - $ObjectID 78 + $ObjectID 82 } Object { $PropName "DataTransfer" - $ObjectID 91 + $ObjectID 95 $ClassName "Simulink.GlobalDataTransfer" DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" @@ -2585,7 +2629,7 @@ Model { ShowPageBoundaries off ZoomFactor "203" ReportName "simulink-default.rpt" - SIDHighWatermark "5012" + SIDHighWatermark "5018" SimulinkSubDomain "Simulink" Block { BlockType BusCreator @@ -2600,27 +2644,19 @@ Model { } Block { BlockType BusSelector - Name "Bus\nSelector" - SID "4943" - Ports [1, 4] - Position [1350, 254, 1355, 366] - ZOrder 1275 - OutputSignals "w_H_b,basePose_dot,joints_position,joints_velocity" + Name "Bus\nSelector1" + SID "5013" + Ports [1, 2] + Position [1465, 396, 1470, 454] + ZOrder 1290 + OutputSignals "joints_position,joints_velocity" Port { PortNumber 1 - Name "" + Name "" } Port { PortNumber 2 - Name "" - } - Port { - PortNumber 3 - Name "" - } - Port { - PortNumber 4 - Name "" + Name "" } } Block { @@ -2695,7 +2731,7 @@ Model { Value "zeros(3,1)" Object { $PropName "MaskObject" - $ObjectID 92 + $ObjectID 96 $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." @@ -2703,12 +2739,12 @@ Model { RunInitForIconRedraw "off" Object { $PropName "DialogControls" - $ObjectID 93 + $ObjectID 97 $ClassName "Simulink.dialog.Group" Prompt "%" Object { $PropName "DialogControls" - $ObjectID 94 + $ObjectID 98 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -7089,7 +7125,7 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 95 + $ObjectID 99 $ClassName "Simulink.Mask" Display "disp('VISUALIZER')" } @@ -17956,7 +17992,7 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 96 + $ObjectID 100 $ClassName "Simulink.Mask" Display "disp('EMERGENCY STOP')" RunInitForIconRedraw "off" @@ -18939,18 +18975,126 @@ Model { InheritSampleTime on } Block { - BlockType Reference - Name "RobotVisualizer" - SID "4939" - Ports [4] - Position [1515, 252, 1735, 368] - 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" + 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 @@ -19019,13 +19163,13 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "251" + ZoomFactor "208" SimulinkSubDomain "Simulink" Block { BlockType Inport Name "joints torque" SID "4956" - Position [120, 258, 150, 272] + Position [-100, 288, -70, 302] ZOrder 1275 } Block { @@ -19071,12 +19215,26 @@ Model { 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 [460, 195, 535, 225] + Position [180, 110, 255, 140] ZOrder 1281 LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/Configuration" @@ -19145,6 +19303,146 @@ 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" @@ -19218,8 +19516,18 @@ Model { ZOrder 322 SrcBlock "RobotDynWithContacts" SrcPort 1 - DstBlock "State" - DstPort 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 @@ -19245,13 +19553,6 @@ Model { DstBlock "RobotDynWithContacts" DstPort 2 } - Line { - ZOrder 321 - SrcBlock "joints torque" - SrcPort 1 - DstBlock "RobotDynWithContacts" - DstPort 1 - } Line { Name "" ZOrder 297 @@ -19339,62 +19640,37 @@ Model { DstBlock "Display1" DstPort 1 } - } - } - Line { - Name "" - ZOrder 308 - Labels [0, 0] - SrcBlock "Bus\nSelector" - SrcPort 2 - DstBlock "RobotVisualizer" - DstPort 2 - } - Line { - Name "" - ZOrder 306 - Labels [0, 0] - SrcBlock "Bus\nSelector" - SrcPort 1 - DstBlock "RobotVisualizer" - DstPort 1 - } - Line { - Name "" - ZOrder 307 - Labels [0, 0] - SrcBlock "Bus\nSelector" - SrcPort 3 - Points [113, 0] - Branch { - ZOrder 313 - Points [0, 85] - DstBlock "Bus\nCreator" - DstPort 1 - } - Branch { - ZOrder 312 - DstBlock "RobotVisualizer" - DstPort 3 - } - } - Line { - Name "" - ZOrder 309 - Labels [0, 0] - SrcBlock "Bus\nSelector" - SrcPort 4 - Points [87, 0] - Branch { - ZOrder 315 - Points [0, 85] - DstBlock "Bus\nCreator" - DstPort 2 - } - Branch { - ZOrder 314 - DstBlock "RobotVisualizer" - DstPort 4 + 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 { @@ -19416,9 +19692,18 @@ Model { ZOrder 362 SrcBlock "Subsystem" SrcPort 1 - Points [19, 0; 0, -42; -96, 0; 0, -88] - DstBlock "Bus\nSelector" - DstPort 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 @@ -19544,6 +19829,24 @@ Model { 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 @@ -19556,7 +19859,7 @@ Stateflow { id 1 name "torqueControlBalancingWithSimu" sfVersion 80000036 - firstTarget 237 + firstTarget 246 } chart { id 2 @@ -20222,24 +20525,26 @@ Stateflow { chart { id 36 machine 1 - name "MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/Add motors reflected inertia/Add motor r" - "eflected inertias" + 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 6 + visible 1 + subviewS { + } + ssIdHighWaterMark 7 decomposition CLUSTER_CHART type EML_CHART - chartFileNumber 6 + chartFileNumber 5 disableImplicitCasting 1 eml { - name "addMotorsInertiaFCN" + name "frictionModel" } firstData 38 - firstTransition 42 - firstJunction 41 + firstTransition 43 + firstJunction 42 } state { id 37 @@ -20255,16 +20560,15 @@ Stateflow { 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" + 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 4 - name "M" + ssIdNumber 6 + name "jointVelocities" scope INPUT_DATA machine 1 props { @@ -20274,8 +20578,11 @@ Stateflow { type { method SF_INHERITED_TYPE primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" } complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED unit { name "inherit" } @@ -20285,8 +20592,30 @@ Stateflow { } 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 "M_with_inertia" + name "jointTorques" scope OUTPUT_DATA machine 1 props { @@ -20304,12 +20633,12 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [36 38 40] + linkNode [36 39 41] } data { - id 40 - ssIdNumber 6 - name "Config" + id 41 + ssIdNumber 7 + name "jointFrictionMat" scope PARAMETER_DATA machine 1 props { @@ -20329,10 +20658,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [36 39 0] + linkNode [36 40 0] } junction { - id 41 + id 42 position [23.5747 49.5747 7] chart 36 subviewer 36 @@ -20341,7 +20670,7 @@ Stateflow { linkNode [36 0 0] } transition { - id 42 + id 43 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -20349,7 +20678,7 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 41 + id 42 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] @@ -20365,21 +20694,172 @@ Stateflow { linkNode [36 0 0] } instance { - id 43 + 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" - chart 36 + 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 44 + 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 45 0 0] - viewObj 44 + treeNode [0 54 0 0] + viewObj 53 ssIdHighWaterMark 10 decomposition CLUSTER_CHART type EML_CHART @@ -20388,19 +20868,19 @@ Stateflow { eml { name "computeBaseVelocityFCN" } - firstData 46 - firstTransition 53 - firstJunction 52 + firstData 55 + firstTransition 62 + firstJunction 61 } state { - id 45 + id 54 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 44 - treeNode [44 0 0 0] + chart 53 + treeNode [53 0 0 0] superState SUBCHART - subviewer 44 + subviewer 53 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -20413,7 +20893,7 @@ Stateflow { } } data { - id 46 + id 55 ssIdNumber 4 name "J_l_sole" scope INPUT_DATA @@ -20432,10 +20912,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [44 0 47] + linkNode [53 0 56] } data { - id 47 + id 56 ssIdNumber 8 name "J_r_sole" scope INPUT_DATA @@ -20457,10 +20937,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [44 46 48] + linkNode [53 55 57] } data { - id 48 + id 57 ssIdNumber 9 name "feetContactStatus" scope INPUT_DATA @@ -20482,10 +20962,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [44 47 49] + linkNode [53 56 58] } data { - id 49 + id 58 ssIdNumber 5 name "nu_b" scope OUTPUT_DATA @@ -20505,10 +20985,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [44 48 50] + linkNode [53 57 59] } data { - id 50 + id 59 ssIdNumber 6 name "jointVel" scope INPUT_DATA @@ -20530,10 +21010,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [44 49 51] + linkNode [53 58 60] } data { - id 51 + id 60 ssIdNumber 10 name "Reg" scope PARAMETER_DATA @@ -20555,19 +21035,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [44 50 0] + linkNode [53 59 0] } junction { - id 52 + id 61 position [23.5747 49.5747 7] - chart 44 - subviewer 44 + chart 53 + subviewer 53 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [44 0 0] + linkNode [53 0 0] } transition { - id 53 + id 62 labelString "{eML_blk_kernel();}" labelPosition [48.125 43.875 102.544 14.964] fontSize 12 @@ -20575,37 +21055,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 52 + id 61 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 44 + chart 53 dataLimits [21.175 25.975 14.625 42.575] - subviewer 44 + subviewer 53 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [44 0 0] + linkNode [53 0 0] } instance { - id 54 + id 63 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute State Velocity/Compute Base Velocity" - chart 44 + chart 53 } chart { - id 55 + 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 56 0 0] - viewObj 55 + treeNode [0 65 0 0] + viewObj 64 ssIdHighWaterMark 88 decomposition CLUSTER_CHART type EML_CHART @@ -20614,19 +21094,19 @@ Stateflow { eml { name "getWorldToBaseTransformFCN" } - firstData 57 - firstTransition 66 - firstJunction 65 + firstData 66 + firstTransition 75 + firstJunction 74 } state { - id 56 + id 65 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 55 - treeNode [55 0 0 0] + chart 64 + treeNode [64 0 0 0] superState SUBCHART - subviewer 55 + subviewer 64 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -20643,7 +21123,7 @@ Stateflow { } } data { - id 57 + id 66 ssIdNumber 82 name "imu_H_fixedLink" scope INPUT_DATA @@ -20665,10 +21145,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [55 0 58] + linkNode [64 0 67] } data { - id 58 + id 67 ssIdNumber 83 name "imu_H_fixedLink_0" scope INPUT_DATA @@ -20690,10 +21170,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [55 57 59] + linkNode [64 66 68] } data { - id 59 + id 68 ssIdNumber 81 name "fixedLink_H_base" scope INPUT_DATA @@ -20715,10 +21195,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [55 58 60] + linkNode [64 67 69] } data { - id 60 + id 69 ssIdNumber 85 name "rpyFromIMU_0" scope INPUT_DATA @@ -20740,10 +21220,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [55 59 61] + linkNode [64 68 70] } data { - id 61 + id 70 ssIdNumber 65 name "rpyFromIMU" scope INPUT_DATA @@ -20765,10 +21245,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [55 60 62] + linkNode [64 69 71] } data { - id 62 + id 71 ssIdNumber 88 name "neck_pos" scope INPUT_DATA @@ -20790,10 +21270,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [55 61 63] + linkNode [64 70 72] } data { - id 63 + id 72 ssIdNumber 71 name "w_H_b" scope OUTPUT_DATA @@ -20815,10 +21295,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [55 62 64] + linkNode [64 71 73] } data { - id 64 + id 73 ssIdNumber 87 name "Config" scope PARAMETER_DATA @@ -20840,19 +21320,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [55 63 0] + linkNode [64 72 0] } junction { - id 65 + id 74 position [23.5747 49.5747 7] - chart 55 - subviewer 55 + chart 64 + subviewer 64 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [55 0 0] + linkNode [64 0 0] } transition { - id 66 + id 75 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -20860,38 +21340,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 65 + id 74 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 55 + chart 64 dataLimits [21.175 25.975 14.625 42.575] - subviewer 55 + subviewer 64 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [55 0 0] + linkNode [64 0 0] } instance { - id 67 + 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 55 + chart 64 } chart { - id 68 + 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 69 0 0] - viewObj 68 + treeNode [0 78 0 0] + viewObj 77 ssIdHighWaterMark 5 decomposition CLUSTER_CHART type EML_CHART @@ -20900,19 +21380,19 @@ Stateflow { eml { name "computeMotorsInertiaFCN" } - firstData 70 - firstTransition 73 - firstJunction 72 + firstData 79 + firstTransition 82 + firstJunction 81 } state { - id 69 + id 78 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 68 - treeNode [68 0 0 0] + chart 77 + treeNode [77 0 0 0] superState SUBCHART - subviewer 68 + subviewer 77 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -20925,7 +21405,7 @@ Stateflow { } } data { - id 70 + id 79 ssIdNumber 5 name "reflectedInertia" scope OUTPUT_DATA @@ -20945,10 +21425,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [68 0 71] + linkNode [77 0 80] } data { - id 71 + id 80 ssIdNumber 4 name "Config" scope PARAMETER_DATA @@ -20967,19 +21447,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [68 70 0] + linkNode [77 79 0] } junction { - id 72 + id 81 position [23.5747 49.5747 7] - chart 68 - subviewer 68 + chart 77 + subviewer 77 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [68 0 0] + linkNode [77 0 0] } transition { - id 73 + id 82 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -20987,37 +21467,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 72 + id 81 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 68 + chart 77 dataLimits [21.175 25.975 14.625 42.575] - subviewer 68 + subviewer 77 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [68 0 0] + linkNode [77 0 0] } instance { - id 74 + 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 68 + chart 77 } chart { - id 75 + 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 76 0 0] - viewObj 75 + treeNode [0 85 0 0] + viewObj 84 ssIdHighWaterMark 8 decomposition CLUSTER_CHART type EML_CHART @@ -21026,19 +21506,19 @@ Stateflow { eml { name "saturateInputDerivativeFCN" } - firstData 77 - firstTransition 83 - firstJunction 82 + firstData 86 + firstTransition 92 + firstJunction 91 } state { - id 76 + id 85 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 75 - treeNode [75 0 0 0] + chart 84 + treeNode [84 0 0 0] superState SUBCHART - subviewer 75 + subviewer 84 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -21050,7 +21530,7 @@ Stateflow { } } data { - id 77 + id 86 ssIdNumber 4 name "u" scope INPUT_DATA @@ -21069,10 +21549,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [75 0 78] + linkNode [84 0 87] } data { - id 78 + id 87 ssIdNumber 5 name "uSat" scope OUTPUT_DATA @@ -21092,10 +21572,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [75 77 79] + linkNode [84 86 88] } data { - id 79 + id 88 ssIdNumber 6 name "u_0" scope INPUT_DATA @@ -21117,10 +21597,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [75 78 80] + linkNode [84 87 89] } data { - id 80 + id 89 ssIdNumber 7 name "tStep" scope INPUT_DATA @@ -21142,10 +21622,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [75 79 81] + linkNode [84 88 90] } data { - id 81 + id 90 ssIdNumber 8 name "uDotMax" scope INPUT_DATA @@ -21167,19 +21647,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [75 80 0] + linkNode [84 89 0] } junction { - id 82 + id 91 position [23.5747 49.5747 7] - chart 75 - subviewer 75 + chart 84 + subviewer 84 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [75 0 0] + linkNode [84 0 0] } transition { - id 83 + id 92 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -21187,36 +21667,36 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 82 + id 91 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 75 + chart 84 dataLimits [21.175 25.975 14.625 42.575] - subviewer 75 + subviewer 84 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [75 0 0] + linkNode [84 0 0] } instance { - id 84 + id 93 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/Saturate Torque Derivative" - chart 75 + chart 84 } chart { - id 85 + 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 86 0 0] - viewObj 85 + treeNode [0 95 0 0] + viewObj 94 ssIdHighWaterMark 86 decomposition CLUSTER_CHART type EML_CHART @@ -21225,19 +21705,19 @@ Stateflow { eml { name "stateMachineMomentumControlFCN" } - firstData 87 - firstTransition 111 - firstJunction 110 + firstData 96 + firstTransition 120 + firstJunction 119 } state { - id 86 + id 95 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 85 - treeNode [85 0 0 0] + chart 94 + treeNode [94 0 0 0] superState SUBCHART - subviewer 85 + subviewer 94 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -21255,7 +21735,7 @@ Stateflow { } } data { - id 87 + id 96 ssIdNumber 37 name "pos_CoM_0" scope INPUT_DATA @@ -21277,10 +21757,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 0 88] + linkNode [94 0 97] } data { - id 88 + id 97 ssIdNumber 54 name "jointPos_0" scope INPUT_DATA @@ -21302,10 +21782,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 87 89] + linkNode [94 96 98] } data { - id 89 + id 98 ssIdNumber 64 name "pos_CoM_fixed_l_sole" scope INPUT_DATA @@ -21327,10 +21807,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 88 90] + linkNode [94 97 99] } data { - id 90 + id 99 ssIdNumber 77 name "pos_CoM_fixed_r_sole" scope INPUT_DATA @@ -21352,10 +21832,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 89 91] + linkNode [94 98 100] } data { - id 91 + id 100 ssIdNumber 69 name "jointPos" scope INPUT_DATA @@ -21377,10 +21857,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 90 92] + linkNode [94 99 101] } data { - id 92 + id 101 ssIdNumber 56 name "time" scope INPUT_DATA @@ -21402,10 +21882,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 91 93] + linkNode [94 100 102] } data { - id 93 + id 102 ssIdNumber 65 name "wrench_rightFoot" scope INPUT_DATA @@ -21427,10 +21907,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 92 94] + linkNode [94 101 103] } data { - id 94 + id 103 ssIdNumber 74 name "wrench_leftFoot" scope INPUT_DATA @@ -21452,10 +21932,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 93 95] + linkNode [94 102 104] } data { - id 95 + id 104 ssIdNumber 71 name "w_H_b" scope OUTPUT_DATA @@ -21477,10 +21957,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 94 96] + linkNode [94 103 105] } data { - id 96 + id 105 ssIdNumber 52 name "pos_CoM_des" scope OUTPUT_DATA @@ -21502,10 +21982,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 95 97] + linkNode [94 104 106] } data { - id 97 + id 106 ssIdNumber 46 name "jointPos_des" scope OUTPUT_DATA @@ -21527,10 +22007,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 96 98] + linkNode [94 105 107] } data { - id 98 + id 107 ssIdNumber 43 name "feetContactStatus" scope OUTPUT_DATA @@ -21552,10 +22032,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 97 99] + linkNode [94 106 108] } data { - id 99 + id 108 ssIdNumber 72 name "l_sole_H_b" scope INPUT_DATA @@ -21577,10 +22057,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 98 100] + linkNode [94 107 109] } data { - id 100 + id 109 ssIdNumber 78 name "r_sole_H_b" scope INPUT_DATA @@ -21602,10 +22082,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 99 101] + linkNode [94 108 110] } data { - id 101 + id 110 ssIdNumber 55 name "StateMachine" scope PARAMETER_DATA @@ -21628,10 +22108,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 100 102] + linkNode [94 109 111] } data { - id 102 + id 111 ssIdNumber 66 name "KP_postural_diag" scope OUTPUT_DATA @@ -21653,10 +22133,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 101 103] + linkNode [94 110 112] } data { - id 103 + id 112 ssIdNumber 82 name "KP_CoM_diag" scope OUTPUT_DATA @@ -21678,10 +22158,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 102 104] + linkNode [94 111 113] } data { - id 104 + id 113 ssIdNumber 83 name "KD_CoM_diag" scope OUTPUT_DATA @@ -21703,10 +22183,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 103 105] + linkNode [94 112 114] } data { - id 105 + id 114 ssIdNumber 67 name "Gain" scope PARAMETER_DATA @@ -21729,10 +22209,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 104 106] + linkNode [94 113 115] } data { - id 106 + id 115 ssIdNumber 68 name "state" scope OUTPUT_DATA @@ -21754,10 +22234,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 105 107] + linkNode [94 114 116] } data { - id 107 + id 116 ssIdNumber 81 name "smoothingTimeJoints" scope OUTPUT_DATA @@ -21779,10 +22259,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 106 108] + linkNode [94 115 117] } data { - id 108 + id 117 ssIdNumber 86 name "smoothingTimeCoM" scope OUTPUT_DATA @@ -21804,10 +22284,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 107 109] + linkNode [94 116 118] } data { - id 109 + id 118 ssIdNumber 85 name "Config" scope PARAMETER_DATA @@ -21829,19 +22309,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [85 108 0] + linkNode [94 117 0] } junction { - id 110 + id 119 position [23.5747 49.5747 7] - chart 85 - subviewer 85 + chart 94 + subviewer 94 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [85 0 0] + linkNode [94 0 0] } transition { - id 111 + id 120 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -21849,37 +22329,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 110 + id 119 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 85 + chart 94 dataLimits [21.175 25.975 14.625 42.575] - subviewer 85 + subviewer 94 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [85 0 0] + linkNode [94 0 0] } instance { - id 112 + id 121 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/STATE MACHINE" - chart 85 + chart 94 } chart { - id 113 + 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 114 0 0] - viewObj 113 + treeNode [0 123 0 0] + viewObj 122 ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART @@ -21889,19 +22369,19 @@ Stateflow { name "checkInputSpikesFCN" } supportVariableSizing 0 - firstData 115 - firstTransition 120 - firstJunction 119 + firstData 124 + firstTransition 129 + firstJunction 128 } state { - id 114 + id 123 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 113 - treeNode [113 0 0 0] + chart 122 + treeNode [122 0 0 0] superState SUBCHART - subviewer 113 + subviewer 122 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -21917,7 +22397,7 @@ Stateflow { } } data { - id 115 + id 124 ssIdNumber 6 name "u" scope INPUT_DATA @@ -21936,10 +22416,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [113 0 116] + linkNode [122 0 125] } data { - id 116 + id 125 ssIdNumber 4 name "delta_u_max" scope INPUT_DATA @@ -21961,10 +22441,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [113 115 117] + linkNode [122 124 126] } data { - id 117 + id 126 ssIdNumber 7 name "noSpikes" scope OUTPUT_DATA @@ -21984,10 +22464,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [113 116 118] + linkNode [122 125 127] } data { - id 118 + id 127 ssIdNumber 9 name "res_check_spikes" scope OUTPUT_DATA @@ -22009,19 +22489,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [113 117 0] + linkNode [122 126 0] } junction { - id 119 + id 128 position [23.5747 49.5747 7] - chart 113 - subviewer 113 + chart 122 + subviewer 122 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [113 0 0] + linkNode [122 0 0] } transition { - id 120 + id 129 labelString "{eML_blk_kernel();}" labelPosition [40.125 31.875 102.544 14.964] fontSize 12 @@ -22029,38 +22509,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 119 + id 128 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 113 + chart 122 dataLimits [21.175 25.975 14.625 42.575] - subviewer 113 + subviewer 122 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [113 0 0] + linkNode [122 0 0] } instance { - id 121 + 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 113 + chart 122 } chart { - id 122 + 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 123 0 0] - viewObj 122 + treeNode [0 132 0 0] + viewObj 131 ssIdHighWaterMark 5 decomposition CLUSTER_CHART type EML_CHART @@ -22069,19 +22549,19 @@ Stateflow { eml { name "robotIsOnSingleSupportQP_FCN" } - firstData 124 - firstTransition 127 - firstJunction 126 + firstData 133 + firstTransition 136 + firstJunction 135 } state { - id 123 + id 132 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 122 - treeNode [122 0 0 0] + chart 131 + treeNode [131 0 0 0] superState SUBCHART - subviewer 122 + subviewer 131 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -22093,7 +22573,7 @@ Stateflow { } } data { - id 124 + id 133 ssIdNumber 4 name "feetContactStatus" scope INPUT_DATA @@ -22112,10 +22592,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [122 0 125] + linkNode [131 0 134] } data { - id 125 + id 134 ssIdNumber 5 name "onOneFoot" scope OUTPUT_DATA @@ -22135,19 +22615,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [122 124 0] + linkNode [131 133 0] } junction { - id 126 + id 135 position [23.5747 49.5747 7] - chart 122 - subviewer 122 + chart 131 + subviewer 131 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [122 0 0] + linkNode [131 0 0] } transition { - id 127 + id 136 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -22155,37 +22635,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 126 + id 135 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 122 + chart 131 dataLimits [21.175 25.975 14.625 42.575] - subviewer 122 + subviewer 131 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [122 0 0] + linkNode [131 0 0] } instance { - id 128 + id 137 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/One Foot Two Fee" "t QP Selector" - chart 122 + chart 131 } chart { - id 129 + 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 130 0 0] - viewObj 129 + treeNode [0 139 0 0] + viewObj 138 ssIdHighWaterMark 82 decomposition CLUSTER_CHART type EML_CHART @@ -22194,19 +22674,19 @@ Stateflow { eml { name "momentumBasedControllerFCN" } - firstData 131 - firstTransition 169 - firstJunction 168 + firstData 140 + firstTransition 178 + firstJunction 177 } state { - id 130 + id 139 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 129 - treeNode [129 0 0 0] + chart 138 + treeNode [138 0 0 0] superState SUBCHART - subviewer 129 + subviewer 138 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -22228,7 +22708,7 @@ Stateflow { } } data { - id 131 + id 140 ssIdNumber 66 name "HessianMatrixOneFoot" scope OUTPUT_DATA @@ -22250,10 +22730,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 0 132] + linkNode [138 0 141] } data { - id 132 + id 141 ssIdNumber 64 name "gradientOneFoot" scope OUTPUT_DATA @@ -22275,10 +22755,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 131 133] + linkNode [138 140 142] } data { - id 133 + id 142 ssIdNumber 5 name "ConstraintsMatrixOneFoot" scope OUTPUT_DATA @@ -22298,10 +22778,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 132 134] + linkNode [138 141 143] } data { - id 134 + id 143 ssIdNumber 52 name "bVectorConstraintsOneFoot" scope OUTPUT_DATA @@ -22323,10 +22803,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 133 135] + linkNode [138 142 144] } data { - id 135 + id 144 ssIdNumber 69 name "HessianMatrixTwoFeet" scope OUTPUT_DATA @@ -22348,10 +22828,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 134 136] + linkNode [138 143 145] } data { - id 136 + id 145 ssIdNumber 70 name "gradientTwoFeet" scope OUTPUT_DATA @@ -22373,10 +22853,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 135 137] + linkNode [138 144 146] } data { - id 137 + id 146 ssIdNumber 53 name "ConstraintsMatrixTwoFeet" scope OUTPUT_DATA @@ -22398,10 +22878,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 136 138] + linkNode [138 145 147] } data { - id 138 + id 147 ssIdNumber 54 name "bVectorConstraintsTwoFeet" scope OUTPUT_DATA @@ -22423,10 +22903,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 137 139] + linkNode [138 146 148] } data { - id 139 + id 148 ssIdNumber 57 name "tauModel" scope OUTPUT_DATA @@ -22448,10 +22928,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 138 140] + linkNode [138 147 149] } data { - id 140 + id 149 ssIdNumber 58 name "Sigma" scope OUTPUT_DATA @@ -22473,10 +22953,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 139 141] + linkNode [138 148 150] } data { - id 141 + id 150 ssIdNumber 62 name "Na" scope OUTPUT_DATA @@ -22498,10 +22978,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 140 142] + linkNode [138 149 151] } data { - id 142 + id 151 ssIdNumber 63 name "f_LDot" scope OUTPUT_DATA @@ -22523,10 +23003,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 141 143] + linkNode [138 150 152] } data { - id 143 + id 152 ssIdNumber 13 name "feetContactStatus" scope INPUT_DATA @@ -22548,10 +23028,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 142 144] + linkNode [138 151 153] } data { - id 144 + id 153 ssIdNumber 50 name "ConstraintsMatrix" scope INPUT_DATA @@ -22573,10 +23053,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 143 145] + linkNode [138 152 154] } data { - id 145 + id 154 ssIdNumber 51 name "bVectorConstraints" scope INPUT_DATA @@ -22598,10 +23078,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 144 146] + linkNode [138 153 155] } data { - id 146 + id 155 ssIdNumber 14 name "jointPos" scope INPUT_DATA @@ -22623,10 +23103,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 145 147] + linkNode [138 154 156] } data { - id 147 + id 156 ssIdNumber 4 name "jointPos_des" scope INPUT_DATA @@ -22645,10 +23125,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 146 148] + linkNode [138 155 157] } data { - id 148 + id 157 ssIdNumber 7 name "nu" scope INPUT_DATA @@ -22670,10 +23150,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 147 149] + linkNode [138 156 158] } data { - id 149 + id 158 ssIdNumber 8 name "M" scope INPUT_DATA @@ -22695,10 +23175,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 148 150] + linkNode [138 157 159] } data { - id 150 + id 159 ssIdNumber 9 name "h" scope INPUT_DATA @@ -22720,10 +23200,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 149 151] + linkNode [138 158 160] } data { - id 151 + id 160 ssIdNumber 11 name "L" scope INPUT_DATA @@ -22745,10 +23225,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 150 152] + linkNode [138 159 161] } data { - id 152 + id 161 ssIdNumber 6 name "intL_angMomError" scope INPUT_DATA @@ -22770,10 +23250,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 151 153] + linkNode [138 160 162] } data { - id 153 + id 162 ssIdNumber 42 name "w_H_l_sole" scope INPUT_DATA @@ -22795,10 +23275,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 152 154] + linkNode [138 161 163] } data { - id 154 + id 163 ssIdNumber 12 name "w_H_r_sole" scope INPUT_DATA @@ -22820,10 +23300,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 153 155] + linkNode [138 162 164] } data { - id 155 + id 164 ssIdNumber 77 name "J_l_sole" scope INPUT_DATA @@ -22845,10 +23325,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 154 156] + linkNode [138 163 165] } data { - id 156 + id 165 ssIdNumber 38 name "J_r_sole" scope INPUT_DATA @@ -22870,10 +23350,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 155 157] + linkNode [138 164 166] } data { - id 157 + id 166 ssIdNumber 32 name "JDot_l_sole_nu" scope INPUT_DATA @@ -22895,10 +23375,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 156 158] + linkNode [138 165 167] } data { - id 158 + id 167 ssIdNumber 33 name "JDot_r_sole_nu" scope INPUT_DATA @@ -22920,10 +23400,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 157 159] + linkNode [138 166 168] } data { - id 159 + id 168 ssIdNumber 40 name "pos_CoM" scope INPUT_DATA @@ -22945,10 +23425,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 158 160] + linkNode [138 167 169] } data { - id 160 + id 169 ssIdNumber 16 name "J_CoM" scope INPUT_DATA @@ -22970,10 +23450,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 159 161] + linkNode [138 168 170] } data { - id 161 + id 170 ssIdNumber 15 name "desired_pos_vel_acc_CoM" scope INPUT_DATA @@ -22995,10 +23475,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 160 162] + linkNode [138 169 171] } data { - id 162 + id 171 ssIdNumber 17 name "KP_CoM" scope INPUT_DATA @@ -23020,10 +23500,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 161 163] + linkNode [138 170 172] } data { - id 163 + id 172 ssIdNumber 81 name "KD_CoM" scope INPUT_DATA @@ -23045,10 +23525,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 162 164] + linkNode [138 171 173] } data { - id 164 + id 173 ssIdNumber 82 name "KP_postural" scope INPUT_DATA @@ -23070,10 +23550,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 163 165] + linkNode [138 172 174] } data { - id 165 + id 174 ssIdNumber 20 name "Reg" scope PARAMETER_DATA @@ -23096,10 +23576,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 164 166] + linkNode [138 173 175] } data { - id 166 + id 175 ssIdNumber 47 name "Gain" scope PARAMETER_DATA @@ -23122,10 +23602,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 165 167] + linkNode [138 174 176] } data { - id 167 + id 176 ssIdNumber 19 name "Config" scope PARAMETER_DATA @@ -23147,19 +23627,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 166 0] + linkNode [138 175 0] } junction { - id 168 + id 177 position [23.5747 49.5747 7] - chart 129 - subviewer 129 + chart 138 + subviewer 138 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [129 0 0] + linkNode [138 0 0] } transition { - id 169 + id 178 labelString "{eML_blk_kernel();}" labelPosition [36.125 25.875 102.544 14.964] fontSize 12 @@ -23167,36 +23647,36 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 168 + id 177 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 129 + chart 138 dataLimits [21.175 25.975 14.625 42.575] - subviewer 129 + subviewer 138 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [129 0 0] + linkNode [138 0 0] } instance { - id 170 + id 179 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Momentum Based Balancing Controller\n" - chart 129 + chart 138 } chart { - id 171 + 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 172 0 0] - viewObj 171 + treeNode [0 181 0 0] + viewObj 180 ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART @@ -23206,19 +23686,19 @@ Stateflow { name "checkInputRangeFCN" } supportVariableSizing 0 - firstData 173 - firstTransition 180 - firstJunction 179 + firstData 182 + firstTransition 189 + firstJunction 188 } state { - id 172 + id 181 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 171 - treeNode [171 0 0 0] + chart 180 + treeNode [180 0 0 0] superState SUBCHART - subviewer 171 + subviewer 180 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -23234,7 +23714,7 @@ Stateflow { } } data { - id 173 + id 182 ssIdNumber 4 name "umin" scope INPUT_DATA @@ -23256,10 +23736,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [171 0 174] + linkNode [180 0 183] } data { - id 174 + id 183 ssIdNumber 5 name "umax" scope INPUT_DATA @@ -23281,10 +23761,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [171 173 175] + linkNode [180 182 184] } data { - id 175 + id 184 ssIdNumber 6 name "u" scope INPUT_DATA @@ -23303,10 +23783,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [171 174 176] + linkNode [180 183 185] } data { - id 176 + id 185 ssIdNumber 7 name "inRange" scope OUTPUT_DATA @@ -23326,10 +23806,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [171 175 177] + linkNode [180 184 186] } data { - id 177 + id 186 ssIdNumber 8 name "tol" scope INPUT_DATA @@ -23351,10 +23831,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [171 176 178] + linkNode [180 185 187] } data { - id 178 + id 187 ssIdNumber 9 name "res_check_range" scope OUTPUT_DATA @@ -23376,19 +23856,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [171 177 0] + linkNode [180 186 0] } junction { - id 179 + id 188 position [23.5747 49.5747 7] - chart 171 - subviewer 171 + chart 180 + subviewer 180 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [171 0 0] + linkNode [180 0 0] } transition { - id 180 + id 189 labelString "{eML_blk_kernel();}" labelPosition [40.125 31.875 102.544 14.964] fontSize 12 @@ -23396,37 +23876,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 179 + id 188 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 171 + chart 180 dataLimits [21.175 25.975 14.625 42.575] - subviewer 171 + subviewer 180 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [171 0 0] + linkNode [180 0 0] } instance { - id 181 + id 190 machine 1 name "MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" - chart 171 + chart 180 } chart { - id 182 + 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 183 0 0] - viewObj 182 + treeNode [0 192 0 0] + viewObj 191 ssIdHighWaterMark 5 decomposition CLUSTER_CHART type EML_CHART @@ -23435,19 +23915,19 @@ Stateflow { eml { name "footOnGround" } - firstData 184 - firstTransition 187 - firstJunction 186 + firstData 193 + firstTransition 196 + firstJunction 195 } state { - id 183 + id 192 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 182 - treeNode [182 0 0 0] + chart 191 + treeNode [191 0 0 0] superState SUBCHART - subviewer 182 + subviewer 191 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -23462,7 +23942,7 @@ Stateflow { } } data { - id 184 + id 193 ssIdNumber 4 name "state" scope INPUT_DATA @@ -23481,10 +23961,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [182 0 185] + linkNode [191 0 194] } data { - id 185 + id 194 ssIdNumber 5 name "booleanState" scope OUTPUT_DATA @@ -23504,19 +23984,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [182 184 0] + linkNode [191 193 0] } junction { - id 186 + id 195 position [23.5747 49.5747 7] - chart 182 - subviewer 182 + chart 191 + subviewer 191 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [182 0 0] + linkNode [191 0 0] } transition { - id 187 + id 196 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -23524,38 +24004,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 186 + id 195 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 182 + chart 191 dataLimits [21.175 25.975 14.625 42.575] - subviewer 182 + subviewer 191 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [182 0 0] + linkNode [191 0 0] } instance { - id 188 + id 197 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Select ba" "se to world transform" - chart 182 + chart 191 } chart { - id 189 + 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 190 0 0] - viewObj 189 + treeNode [0 199 0 0] + viewObj 198 ssIdHighWaterMark 10 decomposition CLUSTER_CHART type EML_CHART @@ -23564,19 +24044,19 @@ Stateflow { eml { name "getEquivalentBaseVel_FCN" } - firstData 191 - firstTransition 198 - firstJunction 197 + firstData 200 + firstTransition 207 + firstJunction 206 } state { - id 190 + id 199 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 189 - treeNode [189 0 0 0] + chart 198 + treeNode [198 0 0 0] superState SUBCHART - subviewer 189 + subviewer 198 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -23592,7 +24072,7 @@ Stateflow { } } data { - id 191 + id 200 ssIdNumber 4 name "J_l_sole" scope INPUT_DATA @@ -23611,10 +24091,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [189 0 192] + linkNode [198 0 201] } data { - id 192 + id 201 ssIdNumber 6 name "J_r_sole" scope INPUT_DATA @@ -23636,10 +24116,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [189 191 193] + linkNode [198 200 202] } data { - id 193 + id 202 ssIdNumber 9 name "feetContactStatus" scope INPUT_DATA @@ -23661,10 +24141,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [189 192 194] + linkNode [198 201 203] } data { - id 194 + id 203 ssIdNumber 10 name "jointPos_err" scope INPUT_DATA @@ -23686,10 +24166,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [189 193 195] + linkNode [198 202 204] } data { - id 195 + id 204 ssIdNumber 7 name "baseVel_equivalent" scope OUTPUT_DATA @@ -23711,10 +24191,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [189 194 196] + linkNode [198 203 205] } data { - id 196 + id 205 ssIdNumber 8 name "Reg" scope PARAMETER_DATA @@ -23736,19 +24216,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [189 195 0] + linkNode [198 204 0] } junction { - id 197 + id 206 position [23.5747 49.5747 7] - chart 189 - subviewer 189 + chart 198 + subviewer 198 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [189 0 0] + linkNode [198 0 0] } transition { - id 198 + id 207 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -23756,38 +24236,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 197 + id 206 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 189 + chart 198 dataLimits [21.175 25.975 14.625 42.575] - subviewer 189 + subviewer 198 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [189 0 0] + linkNode [198 0 0] } instance { - id 199 + id 208 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Get Equiv" "alent Base Velocity" - chart 189 + chart 198 } chart { - id 200 + 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 201 0 0] - viewObj 200 + treeNode [0 210 0 0] + viewObj 209 ssIdHighWaterMark 6 decomposition CLUSTER_CHART type EML_CHART @@ -23796,19 +24276,19 @@ Stateflow { eml { name "analyticalSolutionQP_FCN" } - firstData 202 - firstTransition 206 - firstJunction 205 + firstData 211 + firstTransition 215 + firstJunction 214 } state { - id 201 + id 210 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 200 - treeNode [200 0 0 0] + chart 209 + treeNode [209 0 0 0] superState SUBCHART - subviewer 200 + subviewer 209 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -23820,7 +24300,7 @@ Stateflow { } } data { - id 202 + id 211 ssIdNumber 4 name "H" scope INPUT_DATA @@ -23839,10 +24319,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [200 0 203] + linkNode [209 0 212] } data { - id 203 + id 212 ssIdNumber 5 name "analyticalSolution" scope OUTPUT_DATA @@ -23862,10 +24342,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [200 202 204] + linkNode [209 211 213] } data { - id 204 + id 213 ssIdNumber 6 name "g" scope INPUT_DATA @@ -23887,19 +24367,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [200 203 0] + linkNode [209 212 0] } junction { - id 205 + id 214 position [23.5747 49.5747 7] - chart 200 - subviewer 200 + chart 209 + subviewer 209 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [200 0 0] + linkNode [209 0 0] } transition { - id 206 + id 215 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -23907,38 +24387,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 205 + id 214 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 200 + chart 209 dataLimits [21.175 25.975 14.625 42.575] - subviewer 200 + subviewer 209 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [200 0 0] + linkNode [209 0 0] } instance { - id 207 + 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 200 + chart 209 } chart { - id 208 + 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 209 0 0] - viewObj 208 + treeNode [0 218 0 0] + viewObj 217 ssIdHighWaterMark 14 decomposition CLUSTER_CHART type EML_CHART @@ -23947,19 +24427,19 @@ Stateflow { eml { name "processOutputQP_twoFeetFCN" } - firstData 210 - firstTransition 216 - firstJunction 215 + firstData 219 + firstTransition 225 + firstJunction 224 } state { - id 209 + id 218 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 208 - treeNode [208 0 0 0] + chart 217 + treeNode [217 0 0 0] superState SUBCHART - subviewer 208 + subviewer 217 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -23971,7 +24451,7 @@ Stateflow { } } data { - id 210 + id 219 ssIdNumber 7 name "analyticalSolution" scope INPUT_DATA @@ -23993,10 +24473,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [208 0 211] + linkNode [217 0 220] } data { - id 211 + id 220 ssIdNumber 4 name "primalSolution" scope INPUT_DATA @@ -24015,10 +24495,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [208 210 212] + linkNode [217 219 221] } data { - id 212 + id 221 ssIdNumber 6 name "QPStatus" scope INPUT_DATA @@ -24040,10 +24520,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [208 211 213] + linkNode [217 220 222] } data { - id 213 + id 222 ssIdNumber 5 name "f_star" scope OUTPUT_DATA @@ -24063,10 +24543,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [208 212 214] + linkNode [217 221 223] } data { - id 214 + id 223 ssIdNumber 10 name "Config" scope PARAMETER_DATA @@ -24088,19 +24568,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [208 213 0] + linkNode [217 222 0] } junction { - id 215 + id 224 position [23.5747 49.5747 7] - chart 208 - subviewer 208 + chart 217 + subviewer 217 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [208 0 0] + linkNode [217 0 0] } transition { - id 216 + id 225 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -24108,38 +24588,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 215 + id 224 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 208 + chart 217 dataLimits [21.175 25.975 14.625 42.575] - subviewer 208 + subviewer 217 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [208 0 0] + linkNode [217 0 0] } instance { - id 217 + id 226 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Proc" "ess QP output" - chart 208 + chart 217 } chart { - id 218 + 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 219 0 0] - viewObj 218 + treeNode [0 228 0 0] + viewObj 227 ssIdHighWaterMark 6 decomposition CLUSTER_CHART type EML_CHART @@ -24148,19 +24628,19 @@ Stateflow { eml { name "analyticalSolutionQP_FCN" } - firstData 220 - firstTransition 224 - firstJunction 223 + firstData 229 + firstTransition 233 + firstJunction 232 } state { - id 219 + id 228 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 218 - treeNode [218 0 0 0] + chart 227 + treeNode [227 0 0 0] superState SUBCHART - subviewer 218 + subviewer 227 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -24172,7 +24652,7 @@ Stateflow { } } data { - id 220 + id 229 ssIdNumber 4 name "H" scope INPUT_DATA @@ -24191,10 +24671,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [218 0 221] + linkNode [227 0 230] } data { - id 221 + id 230 ssIdNumber 5 name "analyticalSolution" scope OUTPUT_DATA @@ -24214,10 +24694,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [218 220 222] + linkNode [227 229 231] } data { - id 222 + id 231 ssIdNumber 6 name "g" scope INPUT_DATA @@ -24239,19 +24719,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [218 221 0] + linkNode [227 230 0] } junction { - id 223 + id 232 position [23.5747 49.5747 7] - chart 218 - subviewer 218 + chart 227 + subviewer 227 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [218 0 0] + linkNode [227 0 0] } transition { - id 224 + id 233 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -24259,38 +24739,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 223 + id 232 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 218 + chart 227 dataLimits [21.175 25.975 14.625 42.575] - subviewer 218 + subviewer 227 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [218 0 0] + linkNode [227 0 0] } instance { - id 225 + 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 218 + chart 227 } chart { - id 226 + 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 227 0 0] - viewObj 226 + treeNode [0 236 0 0] + viewObj 235 ssIdHighWaterMark 14 decomposition CLUSTER_CHART type EML_CHART @@ -24299,19 +24779,19 @@ Stateflow { eml { name "processOutputQP_oneFoot" } - firstData 228 - firstTransition 235 - firstJunction 234 + firstData 237 + firstTransition 244 + firstJunction 243 } state { - id 227 + id 236 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 226 - treeNode [226 0 0 0] + chart 235 + treeNode [235 0 0 0] superState SUBCHART - subviewer 226 + subviewer 235 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -24324,7 +24804,7 @@ Stateflow { } } data { - id 228 + id 237 ssIdNumber 7 name "analyticalSolution" scope INPUT_DATA @@ -24346,10 +24826,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [226 0 229] + linkNode [235 0 238] } data { - id 229 + id 238 ssIdNumber 4 name "primalSolution" scope INPUT_DATA @@ -24368,10 +24848,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [226 228 230] + linkNode [235 237 239] } data { - id 230 + id 239 ssIdNumber 6 name "QPStatus" scope INPUT_DATA @@ -24393,10 +24873,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [226 229 231] + linkNode [235 238 240] } data { - id 231 + id 240 ssIdNumber 14 name "feetContactStatus" scope INPUT_DATA @@ -24418,10 +24898,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [226 230 232] + linkNode [235 239 241] } data { - id 232 + id 241 ssIdNumber 5 name "f_star" scope OUTPUT_DATA @@ -24441,10 +24921,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [226 231 233] + linkNode [235 240 242] } data { - id 233 + id 242 ssIdNumber 10 name "Config" scope PARAMETER_DATA @@ -24466,19 +24946,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [226 232 0] + linkNode [235 241 0] } junction { - id 234 + id 243 position [23.5747 49.5747 7] - chart 226 - subviewer 226 + chart 235 + subviewer 235 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [226 0 0] + linkNode [235 0 0] } transition { - id 235 + id 244 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -24486,39 +24966,39 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 234 + id 243 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 226 + chart 235 dataLimits [21.175 25.975 14.625 42.575] - subviewer 226 + subviewer 235 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [226 0 0] + linkNode [235 0 0] } instance { - id 236 + id 245 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Proc" "ess QP output" - chart 226 + chart 235 } target { - id 237 + id 246 machine 1 name "sfun" codeFlags "" - linkNode [1 0 238] + linkNode [1 0 247] } target { - id 238 + id 247 machine 1 name "rtw" - linkNode [1 237 0] + linkNode [1 246 0] } } From f5a92f33b96c148e2807a5b37fde4201397db023 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Mon, 3 May 2021 23:58:49 +0200 Subject: [PATCH 12/20] Propagated the changes to iCubGazeboV2_5 model --- .../app/robots/iCubGazeboV2_5/configRobot.m | 5 +- .../robots/iCubGazeboV2_5/configRobotSim.m | 162 ++++++++++++++++++ 2 files changed, 166 insertions(+), 1 deletion(-) create mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobotSim.m 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 index cff9891b..a1511360 100644 --- 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 @@ -32,6 +32,9 @@ 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'; @@ -48,7 +51,7 @@ % 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 = true; +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 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..6a5ea839 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobotSim.m @@ -0,0 +1,162 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% % +% 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; ... + 0 ; 0 ; 0 ; 0 ; 0; 0; ... + 0 ; 0 ; 0 ; 0 ; 0; 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); + +KvmechMat = diag(0.5*ones(1,robot_config.N_DOF)); +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 From 8e44679d86a37dacc840645cb90c2adb83dbe5a9 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Tue, 4 May 2021 05:39:31 +0200 Subject: [PATCH 13/20] Defined friction coeff per joint, setup YOGA balancing only, RobotVisualizer at 10ms - Tuning tests 1 to 3 (Refer to https://github.com/robotology/whole-body-controllers/issues/121) - Tuning tests 4,5,6 Controller Motor reflected inertia ON/OFF / Harmonic Drive inertia ON/OFF / Coupling ON/OFF Simulator Motor reflected inertia ON Simulator Friction = 0.2 --- .../robots/iCubGazeboV2_5/configRobotSim.m | 29 ++++++++++++++++++- .../iCubGazeboV2_5/configStateMachine.m | 2 +- .../src/initVisualizer.m | 2 +- 3 files changed, 30 insertions(+), 3 deletions(-) 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 index 6a5ea839..ea702f53 100644 --- 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 @@ -153,7 +153,34 @@ motorsReflectedInertia = wbc.computeMotorsReflectedInertia(Gamma,T,I_m); -KvmechMat = diag(0.5*ones(1,robot_config.N_DOF)); +% 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.2 0.2]; +KvmechMappingRightLeg = [0.2 0.2 0.2 0.2 0.2 0.2]; + +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 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 index bace98b0..608d2264 100644 --- 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 @@ -48,7 +48,7 @@ StateMachine.initialState = 1; % other configuration parameters for state machine -StateMachine.tBalancing = 1; +StateMachine.tBalancing = 1000; StateMachine.tBalancingBeforeYoga = 1; StateMachine.yogaExtended = true; StateMachine.skipYoga = false; 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 index 4615d4f6..8d7d8ae9 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m +++ b/controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m @@ -9,7 +9,7 @@ confVisualizer.aroundRobot = 1; % [m] % refresh rate of the picure -confVisualizer.tStep = 0.040; % here equal to the time step used in the simulink model +confVisualizer.tStep = 0.010; % here equal to the time step used in the simulink model %% Parameters copied from robot_config From afb0f4aed6186a3f332e729297bfb5e81f42c409 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Tue, 4 May 2021 06:33:34 +0200 Subject: [PATCH 14/20] Re-activated YOGA right foot support, increased ankle friction. - Test 7 Controller Motor reflected inertia OFF Simulator Motor reflected inertia ON Simulator Friction = 0.2 for all except the ankle pitch and roll = 0.6 => More stable for the first 5s then same instability and divergence. --- .../app/robots/iCubGazeboV2_5/configRobotSim.m | 4 ++-- .../app/robots/iCubGazeboV2_5/configStateMachine.m | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) 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 index ea702f53..35efe5b2 100644 --- 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 @@ -166,8 +166,8 @@ 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.2 0.2]; -KvmechMappingRightLeg = [0.2 0.2 0.2 0.2 0.2 0.2]; +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, ... 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 index 608d2264..bace98b0 100644 --- 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 @@ -48,7 +48,7 @@ StateMachine.initialState = 1; % other configuration parameters for state machine -StateMachine.tBalancing = 1000; +StateMachine.tBalancing = 1; StateMachine.tBalancingBeforeYoga = 1; StateMachine.yogaExtended = true; StateMachine.skipYoga = false; From b34ce47aaf4c0f055458f15d9d0f322f378a17ab Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Tue, 4 May 2021 15:46:10 +0200 Subject: [PATCH 15/20] Skip YOGA, just do contact switching. Saturate torque derivative --- .../app/robots/iCubGazeboV2_5/configRobot.m | 2 +- .../iCubGazeboV2_5/configStateMachine.m | 2 +- .../torqueControlBalancingWithSimu.mdl | 1585 ++++++++++------- 3 files changed, 978 insertions(+), 611 deletions(-) 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 index a1511360..55304cd7 100644 --- 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 @@ -45,7 +45,7 @@ % 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 = false; +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 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 index bace98b0..8c9df41c 100644 --- 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 @@ -51,7 +51,7 @@ StateMachine.tBalancing = 1; StateMachine.tBalancingBeforeYoga = 1; StateMachine.yogaExtended = true; -StateMachine.skipYoga = false; +StateMachine.skipYoga = true; StateMachine.demoOnlyBalancing = false; StateMachine.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first StateMachine.yogaAlsoOnRightFoot = true; % TO DO: yoga on both feet starting from right foot (not available for now) diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl index 6b47dd7d..a1f504ba 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl +++ b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl @@ -7,9 +7,32 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "3.116" + ComputedModelVersion "3.120" NumModelReferences 0 - NumTestPointedSignals 2 + NumTestPointedSignals 5 + TestPointedSignal { + SignalName "" + FullBlockPath "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL" + LogSignal 1 + MaxPoints 5000 + Decimation 2 + } + TestPointedSignal { + SignalName "" + FullBlockPath "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/MATLAB Function" + LogSignal 1 + MaxPoints 5000 + Decimation 2 + } + TestPointedSignal { + SignalName "" + FullBlockPath "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Sta" + "te Machine" + PortIndex 2 + LogSignal 1 + MaxPoints 5000 + Decimation 2 + } TestPointedSignal { SignalName "" FullBlockPath "torqueControlBalancingWithSimu/Subsystem/Bus Selector" @@ -425,81 +448,147 @@ Model { ViewObjType "SimulinkTopLevel" LoadSaveID "0" Extents [2171.0, 1188.0] - ZoomFactor [2.0281179051220715] - Offset [712.93674073651232, 152.61242825136725] - SceneRectInView [712.93674073651232, 152.61242825136725, 1070.4505859925971, 585.76476101299193] + ZoomFactor [2.0281179327233292] + Offset [712.93675521064722, 152.64617823095665] + SceneRectInView [712.93675521064722, 152.64617823095665, 1070.4505714245181, 585.76475304114581] } Object { $ObjectID 6 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "5014" + LoadSaveID "4836" Extents [2171.0, 1188.0] - ZoomFactor [2.03] - Offset [-310.81869612068976, -180.61083743842369] - SceneRectInView [-310.81869612068976, -180.61083743842369, 1069.4581280788179, 585.22167487684737] + ZoomFactor [1.7499999999999998] + Offset [89.479636723777276, -28.60714285714289] + SceneRectInView [89.479636723777276, -28.60714285714289, 1240.5714285714287, 678.85714285714289] } Object { $ObjectID 7 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937:34" + LoadSaveID "2087" Extents [2171.0, 1188.0] - ZoomFactor [2.92] - Offset [-164.07534246575341, -91.924657534246592] - SceneRectInView [-164.07534246575341, -91.924657534246592, 743.49315068493149, 406.84931506849318] + ZoomFactor [1.75] + Offset [-736.65534376844744, 2332.9648668093268] + SceneRectInView [-736.65534376844744, 2332.9648668093268, 1240.5714285714287, 678.85714285714289] } Object { $ObjectID 8 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4836" + LoadSaveID "4537" Extents [2171.0, 1188.0] - ZoomFactor [1.2349950019557565] - Offset [124.11704673423424, -38.918426238738732] - SceneRectInView [124.11704673423424, -38.918426238738732, 1757.9018510698197, 961.94721283783781] + ZoomFactor [5.1816867469879524] + Offset [-108.23828125, -66.072916666666657] + SceneRectInView [-108.23828125, -66.072916666666657, 418.97553943452374, 229.26897321428569] } Object { $ObjectID 9 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4537" + LoadSaveID "2341" Extents [2171.0, 1188.0] - ZoomFactor [5.1816867469879524] - Offset [-108.23828125, -66.072916666666657] - SceneRectInView [-108.23828125, -66.072916666666657, 418.97553943452374, 229.26897321428569] + ZoomFactor [1.5849611820686202] + Offset [-1178.7732548019947, -284.77258542366184] + SceneRectInView [-1178.7732548019947, -284.77258542366184, 1369.7496346039895, 749.54517084732367] } Object { $ObjectID 10 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937:14" + LoadSaveID "4955" Extents [2171.0, 1188.0] - ZoomFactor [2.85] - Offset [-165.70175438596488, -113.92105263157893] - SceneRectInView [-165.70175438596488, -113.92105263157893, 761.75438596491222, 416.84210526315786] + ZoomFactor [2.0773052223433903] + Offset [-119.91137569995328, 98.052613159122757] + SceneRectInView [-119.91137569995328, 98.052613159122757, 1045.1040013999066, 571.89477368175449] } Object { $ObjectID 11 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4955" + LoadSaveID "4030" Extents [2171.0, 1188.0] - ZoomFactor [2.0773052223433903] - Offset [-119.91137569995328, 98.052613159122757] - SceneRectInView [-119.91137569995328, 98.052613159122757, 1045.1040013999066, 571.89477368175449] + ZoomFactor [3.2861189801699715] + Offset [135.07198275862066, -277.26034482758621] + SceneRectInView [135.07198275862066, -277.26034482758621, 660.65775862068972, 361.52068965517242] } Object { $ObjectID 12 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 13 + 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 14 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2355" + Extents [2171.0, 1188.0] + ZoomFactor [1.0] + Offset [1012.8742726293103, -129.0] + SceneRectInView [1012.8742726293103, -129.0, 2171.0, 1188.0] + } + Object { + $ObjectID 15 + 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 16 + 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 17 + 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 18 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" LoadSaveID "4937:67" Extents [2171.0, 1188.0] ZoomFactor [2.87] @@ -507,7 +596,7 @@ Model { SceneRectInView [-165.54878048780483, -115.96864111498257, 756.44599303135885, 413.93728222996515] } Object { - $ObjectID 13 + $ObjectID 19 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -518,7 +607,7 @@ Model { SceneRectInView [-326.36442414255714, -208.23320111992533, 748.27572328511428, 409.46640223985065] } Object { - $ObjectID 14 + $ObjectID 20 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -529,7 +618,7 @@ Model { SceneRectInView [-327.9765625, -403.5, 2171.0, 1188.0] } Object { - $ObjectID 15 + $ObjectID 21 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -540,7 +629,7 @@ Model { SceneRectInView [-116.23828124999997, -57.0, 361.83333333333331, 198.0] } Object { - $ObjectID 16 + $ObjectID 22 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -551,7 +640,7 @@ Model { SceneRectInView [-132.82012310290415, -16.387940803946396, 723.66666666666663, 396.0] } Object { - $ObjectID 17 + $ObjectID 23 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -562,7 +651,7 @@ Model { SceneRectInView [-544.74878771551721, -352.2155172413793, 963.84913793103453, 527.43103448275861] } Object { - $ObjectID 18 + $ObjectID 24 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -573,7 +662,7 @@ Model { SceneRectInView [374.06441393490434, 302.07339302379842, 1297.2149221301913, 709.85321395240317] } Object { - $ObjectID 19 + $ObjectID 25 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -584,7 +673,7 @@ Model { SceneRectInView [-173.20175438596488, -115.42105263157893, 761.75438596491222, 416.84210526315786] } Object { - $ObjectID 20 + $ObjectID 26 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -595,18 +684,7 @@ Model { SceneRectInView [-165.70175438596488, -113.92105263157893, 761.75438596491222, 416.84210526315786] } Object { - $ObjectID 21 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "2355" - Extents [2171.0, 1188.0] - ZoomFactor [1.0] - Offset [1012.8742726293103, -129.0] - SceneRectInView [1012.8742726293103, -129.0, 2171.0, 1188.0] - } - Object { - $ObjectID 22 + $ObjectID 27 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -617,7 +695,7 @@ Model { SceneRectInView [-194.803483319316, -141.48435660218672, 1189.606966638632, 650.96871320437344] } Object { - $ObjectID 23 + $ObjectID 28 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -628,7 +706,7 @@ Model { SceneRectInView [917.234213362069, -427.28793103448277, 975.078448275862, 533.57586206896553] } Object { - $ObjectID 24 + $ObjectID 29 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -639,7 +717,7 @@ Model { SceneRectInView [1438.5531788793103, 818.97931034482758, 778.56551724137933, 426.04137931034484] } Object { - $ObjectID 25 + $ObjectID 30 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -650,7 +728,7 @@ Model { SceneRectInView [-706.68143309105676, -478.27262581168833, 1380.7144286821135, 755.54525162337666] } Object { - $ObjectID 26 + $ObjectID 31 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -661,18 +739,7 @@ Model { SceneRectInView [109.74429086538464, -263.57692307692309, 1043.75, 571.15384615384619] } Object { - $ObjectID 27 - 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 28 + $ObjectID 32 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -683,7 +750,7 @@ Model { SceneRectInView [-116.92703117366869, -144.85219724177151, 889.14932180693916, 486.55430414861524] } Object { - $ObjectID 29 + $ObjectID 33 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -694,7 +761,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 30 + $ObjectID 34 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -705,18 +772,7 @@ Model { SceneRectInView [-29.932291666666686, -42.75, 432.29166666666669, 247.5] } Object { - $ObjectID 31 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "4307" - Extents [2075.0, 1188.0] - ZoomFactor [2.56669704897075] - Offset [-29.368338160419114, -144.92583198051949] - SceneRectInView [-29.368338160419114, -144.92583198051949, 808.43198882083823, 462.851663961039] - } - Object { - $ObjectID 32 + $ObjectID 35 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -727,7 +783,7 @@ Model { SceneRectInView [10.776074743527147, -15.071488336727981, 259.375, 148.5] } Object { - $ObjectID 33 + $ObjectID 36 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -738,7 +794,7 @@ Model { SceneRectInView [578.59213941133362, 441.73077216658527, 393.93290867733265, 225.53845566682949] } Object { - $ObjectID 34 + $ObjectID 37 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -749,29 +805,7 @@ Model { SceneRectInView [-25.756466649159648, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 35 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "2087" - Extents [2075.0, 1188.0] - ZoomFactor [1.75] - Offset [-709.226772339876, 2332.9648668093268] - SceneRectInView [-709.226772339876, 2332.9648668093268, 1185.7142857142858, 678.85714285714289] - } - Object { - $ObjectID 36 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "2341" - Extents [2075.0, 1188.0] - ZoomFactor [1.5849611820686202] - Offset [-1148.4886014344261, -284.77258542366184] - SceneRectInView [-1148.4886014344261, -284.77258542366184, 1309.1803278688524, 749.54517084732367] - } - Object { - $ObjectID 37 + $ObjectID 38 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -782,7 +816,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 38 + $ObjectID 39 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -793,7 +827,7 @@ Model { SceneRectInView [23.229550867122612, 135.34665821934539, 447.67371076575478, 256.30668356130923] } Object { - $ObjectID 39 + $ObjectID 40 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -804,7 +838,7 @@ Model { SceneRectInView [94.721007009531661, 154.55463376836479, 629.70491803278685, 360.52503258937384] } Object { - $ObjectID 40 + $ObjectID 41 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -815,7 +849,7 @@ Model { SceneRectInView [-92.890717237308081, -373.76948051948051, 862.03143447461616, 493.538961038961] } Object { - $ObjectID 41 + $ObjectID 42 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -826,7 +860,7 @@ Model { SceneRectInView [161.34960569474492, -350.98802908437432, 2075.0, 1188.0] } Object { - $ObjectID 42 + $ObjectID 43 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -837,7 +871,7 @@ Model { SceneRectInView [111.71042989741085, -4.1074743527112787, 672.82914020517831, 385.21494870542256] } Object { - $ObjectID 43 + $ObjectID 44 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -848,7 +882,7 @@ Model { SceneRectInView [434.56854543234, -203.35510808500248, 1485.87853413532, 850.710216170005] } Object { - $ObjectID 44 + $ObjectID 45 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -859,7 +893,7 @@ Model { SceneRectInView [-796.48981681034491, -71.967241379310337, 1329.0732758620691, 760.93448275862067] } Object { - $ObjectID 45 + $ObjectID 46 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -870,7 +904,7 @@ Model { SceneRectInView [-113.1169934431619, -154.52586206896552, 670.79741379310349, 384.05172413793105] } Object { - $ObjectID 46 + $ObjectID 47 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -881,7 +915,7 @@ Model { SceneRectInView [87.32421875, 140.1, 207.5, 118.8] } Object { - $ObjectID 47 + $ObjectID 48 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -892,7 +926,7 @@ Model { SceneRectInView [-371.86694504310344, -625.85167594516, 2093.6948275862069, 1198.70335189032] } Object { - $ObjectID 48 + $ObjectID 49 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -903,7 +937,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 49 + $ObjectID 50 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -914,7 +948,7 @@ Model { SceneRectInView [-191.40632054176072, -117.29872084273889, 691.66666666666663, 396.0] } Object { - $ObjectID 50 + $ObjectID 51 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -925,7 +959,7 @@ Model { SceneRectInView [-92.7656132430399, -55.8837471783296, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 51 + $ObjectID 52 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -936,7 +970,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 52 + $ObjectID 53 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -947,7 +981,7 @@ Model { SceneRectInView [-99.198645598194133, -48.972911963882623, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 53 + $ObjectID 54 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -958,7 +992,7 @@ Model { SceneRectInView [-80.667161130818016, -31.015048908954071, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 54 + $ObjectID 55 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -969,7 +1003,7 @@ Model { SceneRectInView [13.879095336481726, -85.310064935064929, 502.36680932703655, 287.62012987012986] } Object { - $ObjectID 55 + $ObjectID 56 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -980,7 +1014,7 @@ Model { SceneRectInView [13.972911622276058, -85.325665859564168, 502.42130750605327, 287.65133171912834] } Object { - $ObjectID 56 + $ObjectID 57 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -991,7 +1025,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 57 + $ObjectID 58 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1002,7 +1036,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 58 + $ObjectID 59 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1013,7 +1047,7 @@ Model { SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] } Object { - $ObjectID 59 + $ObjectID 60 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1024,7 +1058,7 @@ Model { SceneRectInView [-64.168329397874857, -71.912514757969319, 518.75, 297.0] } Object { - $ObjectID 60 + $ObjectID 61 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1035,7 +1069,7 @@ Model { SceneRectInView [-684.37478764306115, -290.08334454895737, 840.66666666666652, 579.97212543554] } Object { - $ObjectID 61 + $ObjectID 62 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1046,7 +1080,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 62 + $ObjectID 63 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1056,17 +1090,6 @@ Model { Offset [-25.672433035714278, -45.831932773109244] SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } - Object { - $ObjectID 63 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "4030" - Extents [1722.0, 1188.0] - ZoomFactor [3.2861189801699715] - Offset [203.38965517241377, -277.26034482758621] - SceneRectInView [203.38965517241377, -277.26034482758621, 524.02241379310351, 361.52068965517242] - } Object { $ObjectID 64 IsActive [0] @@ -1279,9 +1302,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "nunoguedelha" ModifiedDateFormat "%" - LastModifiedDate "Mon May 03 23:34:59 2021" - RTWModifiedTimeStamp 541984483 - ModelVersionFormat "%" + LastModifiedDate "Tue May 04 15:45:49 2021" + RTWModifiedTimeStamp 542042503 + ModelVersionFormat "%" SampleTimeColors on SampleTimeAnnotations on LibraryLinkDisplay "disabled" @@ -1362,7 +1385,7 @@ Model { $ClassName "Simulink.HMI.InstrumentedSignals" Array { Type "Struct" - Dimension 2 + Dimension 5 MATStruct { UUID "361a222e-8799-4264-88ff-83ceed2da1a7" BlockPath_ "Subsystem/Bus Selector" @@ -1411,6 +1434,78 @@ Model { } VisualType_ "" } + MATStruct { + UUID "cdd1a5b9-cc47-4f84-98a9-874c613b5099" + BlockPath_ "MOMENTUM BASED TORQUE CONTROL" + SID_ "4836" + SubPath_ "" + OutputPortIndex_ [1.0] + LogicalPortIndex_ [0.0] + SignalName_ "" + SubSysPath_ "" + Decimation_ [1.0] + MaxPoints_ [1024.0] + TargetBufferedStreaming_ [0.0] + IsFrameBased_ [0.0] + HideInSDI_ [0.0] + DomainType_ "" + Array { + Type "Struct" + Dimension 1 + MATStruct { + } + PropName "DomainParams_" + } + VisualType_ "" + } + MATStruct { + UUID "f3d563f6-1537-46f0-b052-2788cde11d95" + BlockPath_ "MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine" + SID_ "2163" + SubPath_ "" + OutputPortIndex_ [2.0] + LogicalPortIndex_ [0.0] + SignalName_ "" + SubSysPath_ "" + Decimation_ [1.0] + MaxPoints_ [1024.0] + TargetBufferedStreaming_ [0.0] + IsFrameBased_ [0.0] + HideInSDI_ [0.0] + DomainType_ "" + Array { + Type "Struct" + Dimension 1 + MATStruct { + } + PropName "DomainParams_" + } + VisualType_ "" + } + MATStruct { + UUID "c612eb14-7151-45a8-ab91-9218ca14d03e" + BlockPath_ "MOMENTUM BASED TORQUE CONTROL/MATLAB Function" + SID_ "5019" + SubPath_ "" + OutputPortIndex_ [1.0] + LogicalPortIndex_ [0.0] + SignalName_ "" + SubSysPath_ "" + Decimation_ [1.0] + MaxPoints_ [1024.0] + TargetBufferedStreaming_ [0.0] + IsFrameBased_ [0.0] + HideInSDI_ [0.0] + DomainType_ "" + Array { + Type "Struct" + Dimension 1 + MATStruct { + } + PropName "DomainParams_" + } + VisualType_ "" + } PropName "Persistence" } } @@ -2629,7 +2724,7 @@ Model { ShowPageBoundaries off ZoomFactor "203" ReportName "simulink-default.rpt" - SIDHighWatermark "5018" + SIDHighWatermark "5020" SimulinkSubDomain "Simulink" Block { BlockType BusCreator @@ -2773,6 +2868,9 @@ Model { TreatAsAtomicUnit on SystemSampleTime "Config.tStep" RequestExecContextInheritance off + Port { + PortNumber 1 + } System { Name "MOMENTUM BASED TORQUE CONTROL" Location [-75, -1417, 2485, 0] @@ -2791,7 +2889,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "123" + ZoomFactor "175" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -12962,6 +13060,127 @@ Model { } } } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "5019" + Ports [1, 1] + Position [1000, 2, 1215, 48] + ZOrder 976 + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + ContentPreviewEnabled on + Port { + PortNumber 1 + PropagatedSignals "conditionNumber" + } + 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 "21" + SimulinkSubDomain "Simulink" + Block { + BlockType Inport + Name "M" + SID "5019::1" + Position [20, 101, 40, 119] + ZOrder -1 + } + Block { + BlockType Demux + Name " Demux " + SID "5019::20" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 11 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "5019::19" + Tag "Stateflow S-Function 10" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 10 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport on + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "conditionNumber" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "5019::21" + Position [460, 241, 480, 259] + ZOrder 12 + } + Block { + BlockType Outport + Name "conditionNumber" + SID "5019::5" + Position [460, 101, 480, 119] + ZOrder -5 + } + Line { + ZOrder 5 + SrcBlock "M" + SrcPort 1 + Points [65, 0; 0, 20] + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "conditionNumber" + ZOrder 6 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + Points [121, 0; 0, -35] + DstBlock "conditionNumber" + DstPort 1 + } + Line { + ZOrder 7 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 8 + SrcBlock " SFunction " + SrcPort 1 + Points [16, 0; 0, 135] + DstBlock " Demux " + DstPort 1 + } + } + } Block { BlockType SubSystem Name "Robot State and References" @@ -15256,6 +15475,10 @@ Model { BackgroundColor "cyan" Priority "-100" RequestExecContextInheritance off + Port { + PortNumber 2 + PropagatedSignals "feetContactStatus" + } System { Name "State Machine" Location [-75, -1417, 2485, 0] @@ -17980,6 +18203,13 @@ Model { } } } + Block { + BlockType Terminator + Name "Terminator" + SID "5020" + Position [1270, 15, 1290, 35] + ZOrder 977 + } Block { BlockType SubSystem Name "emergency stop: joint limits" @@ -18827,8 +19057,18 @@ Model { ZOrder 90 SrcBlock "Dynamics and Kinematics" SrcPort 1 - DstBlock "Balancing Controller QP" - DstPort 1 + Points [29, 0] + Branch { + ZOrder 373 + Points [0, -26; 209, 0; 0, 46] + DstBlock "MATLAB Function" + DstPort 1 + } + Branch { + ZOrder 372 + DstBlock "Balancing Controller QP" + DstPort 1 + } } Line { ZOrder 132 @@ -18960,7 +19200,14 @@ Model { ZOrder 371 SrcBlock "time" SrcPort 1 - DstBlock "Goto" + DstBlock "Goto" + DstPort 1 + } + Line { + ZOrder 374 + SrcBlock "MATLAB Function" + SrcPort 1 + DstBlock "Terminator" DstPort 1 } } @@ -19859,7 +20106,7 @@ Stateflow { id 1 name "torqueControlBalancingWithSimu" sfVersion 80000036 - firstTarget 246 + firstTarget 253 } chart { id 2 @@ -20531,9 +20778,6 @@ Stateflow { screen [1 1 3600 1200 1.180555555555556] treeNode [0 37 0 0] viewObj 36 - visible 1 - subviewS { - } ssIdHighWaterMark 7 decomposition CLUSTER_CHART type EML_CHART @@ -21492,23 +21736,23 @@ Stateflow { chart { id 84 machine 1 - name "MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/Saturate Torque Derivative" + name "MOMENTUM BASED TORQUE CONTROL/MATLAB Function" 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 + ssIdHighWaterMark 5 decomposition CLUSTER_CHART type EML_CHART - chartFileNumber 11 + chartFileNumber 10 disableImplicitCasting 1 eml { - name "saturateInputDerivativeFCN" + name "matConditionNumber" } firstData 86 - firstTransition 92 - firstJunction 91 + firstTransition 89 + firstJunction 88 } state { id 85 @@ -21524,15 +21768,14 @@ Stateflow { 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" + script "function conditionNumber = matConditionNumber(M)\n\nconditionNumber = cond(M);\n" editorLayout "100 M4x1[10 5 700 500]" } } data { id 86 ssIdNumber 4 - name "u" + name "M" scope INPUT_DATA machine 1 props { @@ -21554,6 +21797,130 @@ Stateflow { data { id 87 ssIdNumber 5 + name "conditionNumber" + 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 0] + } + junction { + id 88 + position [23.5747 49.5747 7] + chart 84 + subviewer 84 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [84 0 0] + } + transition { + id 89 + 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 88 + 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 90 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/MATLAB Function" + chart 84 + } + chart { + id 91 + 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 92 0 0] + viewObj 91 + ssIdHighWaterMark 8 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 11 + disableImplicitCasting 1 + eml { + name "saturateInputDerivativeFCN" + } + firstData 93 + firstTransition 99 + firstJunction 98 + } + state { + id 92 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 91 + treeNode [91 0 0 0] + superState SUBCHART + subviewer 91 + 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 93 + 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 [91 0 94] + } + data { + id 94 + ssIdNumber 5 name "uSat" scope OUTPUT_DATA machine 1 @@ -21572,10 +21939,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [84 86 88] + linkNode [91 93 95] } data { - id 88 + id 95 ssIdNumber 6 name "u_0" scope INPUT_DATA @@ -21597,10 +21964,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [84 87 89] + linkNode [91 94 96] } data { - id 89 + id 96 ssIdNumber 7 name "tStep" scope INPUT_DATA @@ -21622,10 +21989,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [84 88 90] + linkNode [91 95 97] } data { - id 90 + id 97 ssIdNumber 8 name "uDotMax" scope INPUT_DATA @@ -21647,19 +22014,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [84 89 0] + linkNode [91 96 0] } junction { - id 91 + id 98 position [23.5747 49.5747 7] - chart 84 - subviewer 84 + chart 91 + subviewer 91 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [84 0 0] + linkNode [91 0 0] } transition { - id 92 + id 99 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -21667,36 +22034,36 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 91 + id 98 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 84 + chart 91 dataLimits [21.175 25.975 14.625 42.575] - subviewer 84 + subviewer 91 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [84 0 0] + linkNode [91 0 0] } instance { - id 93 + id 100 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/Saturate Torque Derivative" - chart 84 + chart 91 } chart { - id 94 + id 101 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 + treeNode [0 102 0 0] + viewObj 101 ssIdHighWaterMark 86 decomposition CLUSTER_CHART type EML_CHART @@ -21705,19 +22072,19 @@ Stateflow { eml { name "stateMachineMomentumControlFCN" } - firstData 96 - firstTransition 120 - firstJunction 119 + firstData 103 + firstTransition 127 + firstJunction 126 } state { - id 95 + id 102 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 94 - treeNode [94 0 0 0] + chart 101 + treeNode [101 0 0 0] superState SUBCHART - subviewer 94 + subviewer 101 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -21735,7 +22102,7 @@ Stateflow { } } data { - id 96 + id 103 ssIdNumber 37 name "pos_CoM_0" scope INPUT_DATA @@ -21757,10 +22124,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 0 97] + linkNode [101 0 104] } data { - id 97 + id 104 ssIdNumber 54 name "jointPos_0" scope INPUT_DATA @@ -21782,10 +22149,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 96 98] + linkNode [101 103 105] } data { - id 98 + id 105 ssIdNumber 64 name "pos_CoM_fixed_l_sole" scope INPUT_DATA @@ -21807,10 +22174,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 97 99] + linkNode [101 104 106] } data { - id 99 + id 106 ssIdNumber 77 name "pos_CoM_fixed_r_sole" scope INPUT_DATA @@ -21832,10 +22199,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 98 100] + linkNode [101 105 107] } data { - id 100 + id 107 ssIdNumber 69 name "jointPos" scope INPUT_DATA @@ -21857,10 +22224,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 99 101] + linkNode [101 106 108] } data { - id 101 + id 108 ssIdNumber 56 name "time" scope INPUT_DATA @@ -21882,10 +22249,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 100 102] + linkNode [101 107 109] } data { - id 102 + id 109 ssIdNumber 65 name "wrench_rightFoot" scope INPUT_DATA @@ -21907,10 +22274,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 101 103] + linkNode [101 108 110] } data { - id 103 + id 110 ssIdNumber 74 name "wrench_leftFoot" scope INPUT_DATA @@ -21932,10 +22299,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 102 104] + linkNode [101 109 111] } data { - id 104 + id 111 ssIdNumber 71 name "w_H_b" scope OUTPUT_DATA @@ -21957,10 +22324,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 103 105] + linkNode [101 110 112] } data { - id 105 + id 112 ssIdNumber 52 name "pos_CoM_des" scope OUTPUT_DATA @@ -21982,10 +22349,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 104 106] + linkNode [101 111 113] } data { - id 106 + id 113 ssIdNumber 46 name "jointPos_des" scope OUTPUT_DATA @@ -22007,10 +22374,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 105 107] + linkNode [101 112 114] } data { - id 107 + id 114 ssIdNumber 43 name "feetContactStatus" scope OUTPUT_DATA @@ -22032,10 +22399,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 106 108] + linkNode [101 113 115] } data { - id 108 + id 115 ssIdNumber 72 name "l_sole_H_b" scope INPUT_DATA @@ -22057,10 +22424,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 107 109] + linkNode [101 114 116] } data { - id 109 + id 116 ssIdNumber 78 name "r_sole_H_b" scope INPUT_DATA @@ -22082,10 +22449,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 108 110] + linkNode [101 115 117] } data { - id 110 + id 117 ssIdNumber 55 name "StateMachine" scope PARAMETER_DATA @@ -22108,10 +22475,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 109 111] + linkNode [101 116 118] } data { - id 111 + id 118 ssIdNumber 66 name "KP_postural_diag" scope OUTPUT_DATA @@ -22133,10 +22500,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 110 112] + linkNode [101 117 119] } data { - id 112 + id 119 ssIdNumber 82 name "KP_CoM_diag" scope OUTPUT_DATA @@ -22158,10 +22525,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 111 113] + linkNode [101 118 120] } data { - id 113 + id 120 ssIdNumber 83 name "KD_CoM_diag" scope OUTPUT_DATA @@ -22183,10 +22550,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 112 114] + linkNode [101 119 121] } data { - id 114 + id 121 ssIdNumber 67 name "Gain" scope PARAMETER_DATA @@ -22209,10 +22576,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 113 115] + linkNode [101 120 122] } data { - id 115 + id 122 ssIdNumber 68 name "state" scope OUTPUT_DATA @@ -22234,10 +22601,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 114 116] + linkNode [101 121 123] } data { - id 116 + id 123 ssIdNumber 81 name "smoothingTimeJoints" scope OUTPUT_DATA @@ -22259,10 +22626,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 115 117] + linkNode [101 122 124] } data { - id 117 + id 124 ssIdNumber 86 name "smoothingTimeCoM" scope OUTPUT_DATA @@ -22284,10 +22651,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 116 118] + linkNode [101 123 125] } data { - id 118 + id 125 ssIdNumber 85 name "Config" scope PARAMETER_DATA @@ -22309,19 +22676,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [94 117 0] + linkNode [101 124 0] } junction { - id 119 + id 126 position [23.5747 49.5747 7] - chart 94 - subviewer 94 + chart 101 + subviewer 101 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [94 0 0] + linkNode [101 0 0] } transition { - id 120 + id 127 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -22329,37 +22696,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 119 + id 126 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 94 + chart 101 dataLimits [21.175 25.975 14.625 42.575] - subviewer 94 + subviewer 101 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [94 0 0] + linkNode [101 0 0] } instance { - id 121 + id 128 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/STATE MACHINE" - chart 94 + chart 101 } chart { - id 122 + id 129 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 + treeNode [0 130 0 0] + viewObj 129 ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART @@ -22369,19 +22736,19 @@ Stateflow { name "checkInputSpikesFCN" } supportVariableSizing 0 - firstData 124 - firstTransition 129 - firstJunction 128 + firstData 131 + firstTransition 136 + firstJunction 135 } state { - id 123 + id 130 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 122 - treeNode [122 0 0 0] + chart 129 + treeNode [129 0 0 0] superState SUBCHART - subviewer 122 + subviewer 129 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -22397,7 +22764,7 @@ Stateflow { } } data { - id 124 + id 131 ssIdNumber 6 name "u" scope INPUT_DATA @@ -22416,10 +22783,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [122 0 125] + linkNode [129 0 132] } data { - id 125 + id 132 ssIdNumber 4 name "delta_u_max" scope INPUT_DATA @@ -22441,10 +22808,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [122 124 126] + linkNode [129 131 133] } data { - id 126 + id 133 ssIdNumber 7 name "noSpikes" scope OUTPUT_DATA @@ -22464,10 +22831,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [122 125 127] + linkNode [129 132 134] } data { - id 127 + id 134 ssIdNumber 9 name "res_check_spikes" scope OUTPUT_DATA @@ -22489,19 +22856,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [122 126 0] + linkNode [129 133 0] } junction { - id 128 + id 135 position [23.5747 49.5747 7] - chart 122 - subviewer 122 + chart 129 + subviewer 129 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [122 0 0] + linkNode [129 0 0] } transition { - id 129 + id 136 labelString "{eML_blk_kernel();}" labelPosition [40.125 31.875 102.544 14.964] fontSize 12 @@ -22509,38 +22876,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 128 + id 135 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 122 + chart 129 dataLimits [21.175 25.975 14.625 42.575] - subviewer 122 + subviewer 129 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [122 0 0] + linkNode [129 0 0] } instance { - id 130 + id 137 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 129 } chart { - id 131 + id 138 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 + treeNode [0 139 0 0] + viewObj 138 ssIdHighWaterMark 5 decomposition CLUSTER_CHART type EML_CHART @@ -22549,19 +22916,19 @@ Stateflow { eml { name "robotIsOnSingleSupportQP_FCN" } - firstData 133 - firstTransition 136 - firstJunction 135 + firstData 140 + firstTransition 143 + firstJunction 142 } state { - id 132 + id 139 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 131 - treeNode [131 0 0 0] + chart 138 + treeNode [138 0 0 0] superState SUBCHART - subviewer 131 + subviewer 138 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -22573,7 +22940,7 @@ Stateflow { } } data { - id 133 + id 140 ssIdNumber 4 name "feetContactStatus" scope INPUT_DATA @@ -22592,10 +22959,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [131 0 134] + linkNode [138 0 141] } data { - id 134 + id 141 ssIdNumber 5 name "onOneFoot" scope OUTPUT_DATA @@ -22615,19 +22982,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [131 133 0] + linkNode [138 140 0] } junction { - id 135 + id 142 position [23.5747 49.5747 7] - chart 131 - subviewer 131 + chart 138 + subviewer 138 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [131 0 0] + linkNode [138 0 0] } transition { - id 136 + id 143 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -22635,37 +23002,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 135 + id 142 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 131 + chart 138 dataLimits [21.175 25.975 14.625 42.575] - subviewer 131 + subviewer 138 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [131 0 0] + linkNode [138 0 0] } instance { - id 137 + id 144 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/One Foot Two Fee" "t QP Selector" - chart 131 + chart 138 } chart { - id 138 + id 145 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 + treeNode [0 146 0 0] + viewObj 145 ssIdHighWaterMark 82 decomposition CLUSTER_CHART type EML_CHART @@ -22674,19 +23041,19 @@ Stateflow { eml { name "momentumBasedControllerFCN" } - firstData 140 - firstTransition 178 - firstJunction 177 + firstData 147 + firstTransition 185 + firstJunction 184 } state { - id 139 + id 146 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 138 - treeNode [138 0 0 0] + chart 145 + treeNode [145 0 0 0] superState SUBCHART - subviewer 138 + subviewer 145 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -22708,7 +23075,7 @@ Stateflow { } } data { - id 140 + id 147 ssIdNumber 66 name "HessianMatrixOneFoot" scope OUTPUT_DATA @@ -22730,10 +23097,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 0 141] + linkNode [145 0 148] } data { - id 141 + id 148 ssIdNumber 64 name "gradientOneFoot" scope OUTPUT_DATA @@ -22755,10 +23122,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 140 142] + linkNode [145 147 149] } data { - id 142 + id 149 ssIdNumber 5 name "ConstraintsMatrixOneFoot" scope OUTPUT_DATA @@ -22778,10 +23145,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 141 143] + linkNode [145 148 150] } data { - id 143 + id 150 ssIdNumber 52 name "bVectorConstraintsOneFoot" scope OUTPUT_DATA @@ -22803,10 +23170,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 142 144] + linkNode [145 149 151] } data { - id 144 + id 151 ssIdNumber 69 name "HessianMatrixTwoFeet" scope OUTPUT_DATA @@ -22828,10 +23195,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 143 145] + linkNode [145 150 152] } data { - id 145 + id 152 ssIdNumber 70 name "gradientTwoFeet" scope OUTPUT_DATA @@ -22853,10 +23220,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 144 146] + linkNode [145 151 153] } data { - id 146 + id 153 ssIdNumber 53 name "ConstraintsMatrixTwoFeet" scope OUTPUT_DATA @@ -22878,10 +23245,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 145 147] + linkNode [145 152 154] } data { - id 147 + id 154 ssIdNumber 54 name "bVectorConstraintsTwoFeet" scope OUTPUT_DATA @@ -22903,10 +23270,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 146 148] + linkNode [145 153 155] } data { - id 148 + id 155 ssIdNumber 57 name "tauModel" scope OUTPUT_DATA @@ -22928,10 +23295,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 147 149] + linkNode [145 154 156] } data { - id 149 + id 156 ssIdNumber 58 name "Sigma" scope OUTPUT_DATA @@ -22953,10 +23320,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 148 150] + linkNode [145 155 157] } data { - id 150 + id 157 ssIdNumber 62 name "Na" scope OUTPUT_DATA @@ -22978,10 +23345,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 149 151] + linkNode [145 156 158] } data { - id 151 + id 158 ssIdNumber 63 name "f_LDot" scope OUTPUT_DATA @@ -23003,10 +23370,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 150 152] + linkNode [145 157 159] } data { - id 152 + id 159 ssIdNumber 13 name "feetContactStatus" scope INPUT_DATA @@ -23028,10 +23395,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 151 153] + linkNode [145 158 160] } data { - id 153 + id 160 ssIdNumber 50 name "ConstraintsMatrix" scope INPUT_DATA @@ -23053,10 +23420,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 152 154] + linkNode [145 159 161] } data { - id 154 + id 161 ssIdNumber 51 name "bVectorConstraints" scope INPUT_DATA @@ -23078,10 +23445,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 153 155] + linkNode [145 160 162] } data { - id 155 + id 162 ssIdNumber 14 name "jointPos" scope INPUT_DATA @@ -23103,10 +23470,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 154 156] + linkNode [145 161 163] } data { - id 156 + id 163 ssIdNumber 4 name "jointPos_des" scope INPUT_DATA @@ -23125,10 +23492,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 155 157] + linkNode [145 162 164] } data { - id 157 + id 164 ssIdNumber 7 name "nu" scope INPUT_DATA @@ -23150,10 +23517,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 156 158] + linkNode [145 163 165] } data { - id 158 + id 165 ssIdNumber 8 name "M" scope INPUT_DATA @@ -23175,10 +23542,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 157 159] + linkNode [145 164 166] } data { - id 159 + id 166 ssIdNumber 9 name "h" scope INPUT_DATA @@ -23200,10 +23567,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 158 160] + linkNode [145 165 167] } data { - id 160 + id 167 ssIdNumber 11 name "L" scope INPUT_DATA @@ -23225,10 +23592,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 159 161] + linkNode [145 166 168] } data { - id 161 + id 168 ssIdNumber 6 name "intL_angMomError" scope INPUT_DATA @@ -23250,10 +23617,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 160 162] + linkNode [145 167 169] } data { - id 162 + id 169 ssIdNumber 42 name "w_H_l_sole" scope INPUT_DATA @@ -23275,10 +23642,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 161 163] + linkNode [145 168 170] } data { - id 163 + id 170 ssIdNumber 12 name "w_H_r_sole" scope INPUT_DATA @@ -23300,10 +23667,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 162 164] + linkNode [145 169 171] } data { - id 164 + id 171 ssIdNumber 77 name "J_l_sole" scope INPUT_DATA @@ -23325,10 +23692,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 163 165] + linkNode [145 170 172] } data { - id 165 + id 172 ssIdNumber 38 name "J_r_sole" scope INPUT_DATA @@ -23350,10 +23717,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 164 166] + linkNode [145 171 173] } data { - id 166 + id 173 ssIdNumber 32 name "JDot_l_sole_nu" scope INPUT_DATA @@ -23375,10 +23742,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 165 167] + linkNode [145 172 174] } data { - id 167 + id 174 ssIdNumber 33 name "JDot_r_sole_nu" scope INPUT_DATA @@ -23400,10 +23767,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 166 168] + linkNode [145 173 175] } data { - id 168 + id 175 ssIdNumber 40 name "pos_CoM" scope INPUT_DATA @@ -23425,10 +23792,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 167 169] + linkNode [145 174 176] } data { - id 169 + id 176 ssIdNumber 16 name "J_CoM" scope INPUT_DATA @@ -23450,10 +23817,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 168 170] + linkNode [145 175 177] } data { - id 170 + id 177 ssIdNumber 15 name "desired_pos_vel_acc_CoM" scope INPUT_DATA @@ -23475,10 +23842,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 169 171] + linkNode [145 176 178] } data { - id 171 + id 178 ssIdNumber 17 name "KP_CoM" scope INPUT_DATA @@ -23500,10 +23867,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 170 172] + linkNode [145 177 179] } data { - id 172 + id 179 ssIdNumber 81 name "KD_CoM" scope INPUT_DATA @@ -23525,10 +23892,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 171 173] + linkNode [145 178 180] } data { - id 173 + id 180 ssIdNumber 82 name "KP_postural" scope INPUT_DATA @@ -23550,10 +23917,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 172 174] + linkNode [145 179 181] } data { - id 174 + id 181 ssIdNumber 20 name "Reg" scope PARAMETER_DATA @@ -23576,10 +23943,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 173 175] + linkNode [145 180 182] } data { - id 175 + id 182 ssIdNumber 47 name "Gain" scope PARAMETER_DATA @@ -23602,10 +23969,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 174 176] + linkNode [145 181 183] } data { - id 176 + id 183 ssIdNumber 19 name "Config" scope PARAMETER_DATA @@ -23627,19 +23994,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 175 0] + linkNode [145 182 0] } junction { - id 177 + id 184 position [23.5747 49.5747 7] - chart 138 - subviewer 138 + chart 145 + subviewer 145 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [138 0 0] + linkNode [145 0 0] } transition { - id 178 + id 185 labelString "{eML_blk_kernel();}" labelPosition [36.125 25.875 102.544 14.964] fontSize 12 @@ -23647,36 +24014,36 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 177 + id 184 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 138 + chart 145 dataLimits [21.175 25.975 14.625 42.575] - subviewer 138 + subviewer 145 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [138 0 0] + linkNode [145 0 0] } instance { - id 179 + id 186 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Momentum Based Balancing Controller\n" - chart 138 + chart 145 } chart { - id 180 + id 187 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 + treeNode [0 188 0 0] + viewObj 187 ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART @@ -23686,19 +24053,19 @@ Stateflow { name "checkInputRangeFCN" } supportVariableSizing 0 - firstData 182 - firstTransition 189 - firstJunction 188 + firstData 189 + firstTransition 196 + firstJunction 195 } state { - id 181 + id 188 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 180 - treeNode [180 0 0 0] + chart 187 + treeNode [187 0 0 0] superState SUBCHART - subviewer 180 + subviewer 187 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -23714,7 +24081,7 @@ Stateflow { } } data { - id 182 + id 189 ssIdNumber 4 name "umin" scope INPUT_DATA @@ -23736,10 +24103,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [180 0 183] + linkNode [187 0 190] } data { - id 183 + id 190 ssIdNumber 5 name "umax" scope INPUT_DATA @@ -23761,10 +24128,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [180 182 184] + linkNode [187 189 191] } data { - id 184 + id 191 ssIdNumber 6 name "u" scope INPUT_DATA @@ -23783,10 +24150,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [180 183 185] + linkNode [187 190 192] } data { - id 185 + id 192 ssIdNumber 7 name "inRange" scope OUTPUT_DATA @@ -23806,10 +24173,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [180 184 186] + linkNode [187 191 193] } data { - id 186 + id 193 ssIdNumber 8 name "tol" scope INPUT_DATA @@ -23831,10 +24198,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [180 185 187] + linkNode [187 192 194] } data { - id 187 + id 194 ssIdNumber 9 name "res_check_range" scope OUTPUT_DATA @@ -23856,19 +24223,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [180 186 0] + linkNode [187 193 0] } junction { - id 188 + id 195 position [23.5747 49.5747 7] - chart 180 - subviewer 180 + chart 187 + subviewer 187 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [180 0 0] + linkNode [187 0 0] } transition { - id 189 + id 196 labelString "{eML_blk_kernel();}" labelPosition [40.125 31.875 102.544 14.964] fontSize 12 @@ -23876,37 +24243,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 188 + id 195 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 180 + chart 187 dataLimits [21.175 25.975 14.625 42.575] - subviewer 180 + subviewer 187 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [180 0 0] + linkNode [187 0 0] } instance { - id 190 + id 197 machine 1 name "MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" - chart 180 + chart 187 } chart { - id 191 + id 198 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 + treeNode [0 199 0 0] + viewObj 198 ssIdHighWaterMark 5 decomposition CLUSTER_CHART type EML_CHART @@ -23915,19 +24282,19 @@ Stateflow { eml { name "footOnGround" } - firstData 193 - firstTransition 196 - firstJunction 195 + firstData 200 + firstTransition 203 + firstJunction 202 } state { - id 192 + id 199 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 191 - treeNode [191 0 0 0] + chart 198 + treeNode [198 0 0 0] superState SUBCHART - subviewer 191 + subviewer 198 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -23942,7 +24309,7 @@ Stateflow { } } data { - id 193 + id 200 ssIdNumber 4 name "state" scope INPUT_DATA @@ -23961,10 +24328,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [191 0 194] + linkNode [198 0 201] } data { - id 194 + id 201 ssIdNumber 5 name "booleanState" scope OUTPUT_DATA @@ -23984,19 +24351,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [191 193 0] + linkNode [198 200 0] } junction { - id 195 + id 202 position [23.5747 49.5747 7] - chart 191 - subviewer 191 + chart 198 + subviewer 198 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [191 0 0] + linkNode [198 0 0] } transition { - id 196 + id 203 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -24004,38 +24371,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 195 + id 202 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 191 + chart 198 dataLimits [21.175 25.975 14.625 42.575] - subviewer 191 + subviewer 198 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [191 0 0] + linkNode [198 0 0] } instance { - id 197 + id 204 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Select ba" "se to world transform" - chart 191 + chart 198 } chart { - id 198 + id 205 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 + treeNode [0 206 0 0] + viewObj 205 ssIdHighWaterMark 10 decomposition CLUSTER_CHART type EML_CHART @@ -24044,19 +24411,19 @@ Stateflow { eml { name "getEquivalentBaseVel_FCN" } - firstData 200 - firstTransition 207 - firstJunction 206 + firstData 207 + firstTransition 214 + firstJunction 213 } state { - id 199 + id 206 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 198 - treeNode [198 0 0 0] + chart 205 + treeNode [205 0 0 0] superState SUBCHART - subviewer 198 + subviewer 205 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -24072,7 +24439,7 @@ Stateflow { } } data { - id 200 + id 207 ssIdNumber 4 name "J_l_sole" scope INPUT_DATA @@ -24091,10 +24458,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [198 0 201] + linkNode [205 0 208] } data { - id 201 + id 208 ssIdNumber 6 name "J_r_sole" scope INPUT_DATA @@ -24116,10 +24483,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [198 200 202] + linkNode [205 207 209] } data { - id 202 + id 209 ssIdNumber 9 name "feetContactStatus" scope INPUT_DATA @@ -24141,10 +24508,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [198 201 203] + linkNode [205 208 210] } data { - id 203 + id 210 ssIdNumber 10 name "jointPos_err" scope INPUT_DATA @@ -24166,10 +24533,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [198 202 204] + linkNode [205 209 211] } data { - id 204 + id 211 ssIdNumber 7 name "baseVel_equivalent" scope OUTPUT_DATA @@ -24191,10 +24558,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [198 203 205] + linkNode [205 210 212] } data { - id 205 + id 212 ssIdNumber 8 name "Reg" scope PARAMETER_DATA @@ -24216,19 +24583,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [198 204 0] + linkNode [205 211 0] } junction { - id 206 + id 213 position [23.5747 49.5747 7] - chart 198 - subviewer 198 + chart 205 + subviewer 205 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [198 0 0] + linkNode [205 0 0] } transition { - id 207 + id 214 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -24236,38 +24603,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 206 + id 213 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 198 + chart 205 dataLimits [21.175 25.975 14.625 42.575] - subviewer 198 + subviewer 205 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [198 0 0] + linkNode [205 0 0] } instance { - id 208 + id 215 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Get Equiv" "alent Base Velocity" - chart 198 + chart 205 } chart { - id 209 + 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)" 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 + treeNode [0 217 0 0] + viewObj 216 ssIdHighWaterMark 6 decomposition CLUSTER_CHART type EML_CHART @@ -24276,19 +24643,19 @@ Stateflow { eml { name "analyticalSolutionQP_FCN" } - firstData 211 - firstTransition 215 - firstJunction 214 + firstData 218 + firstTransition 222 + firstJunction 221 } state { - id 210 + id 217 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 209 - treeNode [209 0 0 0] + chart 216 + treeNode [216 0 0 0] superState SUBCHART - subviewer 209 + subviewer 216 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -24300,7 +24667,7 @@ Stateflow { } } data { - id 211 + id 218 ssIdNumber 4 name "H" scope INPUT_DATA @@ -24319,10 +24686,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [209 0 212] + linkNode [216 0 219] } data { - id 212 + id 219 ssIdNumber 5 name "analyticalSolution" scope OUTPUT_DATA @@ -24342,10 +24709,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [209 211 213] + linkNode [216 218 220] } data { - id 213 + id 220 ssIdNumber 6 name "g" scope INPUT_DATA @@ -24367,19 +24734,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [209 212 0] + linkNode [216 219 0] } junction { - id 214 + id 221 position [23.5747 49.5747 7] - chart 209 - subviewer 209 + chart 216 + subviewer 216 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [209 0 0] + linkNode [216 0 0] } transition { - id 215 + id 222 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -24387,38 +24754,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 214 + id 221 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 209 + chart 216 dataLimits [21.175 25.975 14.625 42.575] - subviewer 209 + subviewer 216 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [209 0 0] + linkNode [216 0 0] } instance { - id 216 + id 223 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 216 } chart { - id 217 + id 224 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 + treeNode [0 225 0 0] + viewObj 224 ssIdHighWaterMark 14 decomposition CLUSTER_CHART type EML_CHART @@ -24427,19 +24794,19 @@ Stateflow { eml { name "processOutputQP_twoFeetFCN" } - firstData 219 - firstTransition 225 - firstJunction 224 + firstData 226 + firstTransition 232 + firstJunction 231 } state { - id 218 + id 225 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 217 - treeNode [217 0 0 0] + chart 224 + treeNode [224 0 0 0] superState SUBCHART - subviewer 217 + subviewer 224 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -24451,7 +24818,7 @@ Stateflow { } } data { - id 219 + id 226 ssIdNumber 7 name "analyticalSolution" scope INPUT_DATA @@ -24473,10 +24840,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [217 0 220] + linkNode [224 0 227] } data { - id 220 + id 227 ssIdNumber 4 name "primalSolution" scope INPUT_DATA @@ -24495,10 +24862,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [217 219 221] + linkNode [224 226 228] } data { - id 221 + id 228 ssIdNumber 6 name "QPStatus" scope INPUT_DATA @@ -24520,10 +24887,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [217 220 222] + linkNode [224 227 229] } data { - id 222 + id 229 ssIdNumber 5 name "f_star" scope OUTPUT_DATA @@ -24543,10 +24910,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [217 221 223] + linkNode [224 228 230] } data { - id 223 + id 230 ssIdNumber 10 name "Config" scope PARAMETER_DATA @@ -24568,19 +24935,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [217 222 0] + linkNode [224 229 0] } junction { - id 224 + id 231 position [23.5747 49.5747 7] - chart 217 - subviewer 217 + chart 224 + subviewer 224 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [217 0 0] + linkNode [224 0 0] } transition { - id 225 + id 232 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -24588,38 +24955,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 224 + id 231 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 217 + chart 224 dataLimits [21.175 25.975 14.625 42.575] - subviewer 217 + subviewer 224 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [217 0 0] + linkNode [224 0 0] } instance { - id 226 + id 233 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Proc" "ess QP output" - chart 217 + chart 224 } chart { - id 227 + 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)" 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 + treeNode [0 235 0 0] + viewObj 234 ssIdHighWaterMark 6 decomposition CLUSTER_CHART type EML_CHART @@ -24628,19 +24995,19 @@ Stateflow { eml { name "analyticalSolutionQP_FCN" } - firstData 229 - firstTransition 233 - firstJunction 232 + firstData 236 + firstTransition 240 + firstJunction 239 } state { - id 228 + id 235 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 227 - treeNode [227 0 0 0] + chart 234 + treeNode [234 0 0 0] superState SUBCHART - subviewer 227 + subviewer 234 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -24652,7 +25019,7 @@ Stateflow { } } data { - id 229 + id 236 ssIdNumber 4 name "H" scope INPUT_DATA @@ -24671,10 +25038,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [227 0 230] + linkNode [234 0 237] } data { - id 230 + id 237 ssIdNumber 5 name "analyticalSolution" scope OUTPUT_DATA @@ -24694,10 +25061,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [227 229 231] + linkNode [234 236 238] } data { - id 231 + id 238 ssIdNumber 6 name "g" scope INPUT_DATA @@ -24719,19 +25086,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [227 230 0] + linkNode [234 237 0] } junction { - id 232 + id 239 position [23.5747 49.5747 7] - chart 227 - subviewer 227 + chart 234 + subviewer 234 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [227 0 0] + linkNode [234 0 0] } transition { - id 233 + id 240 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -24739,38 +25106,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 232 + id 239 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 227 + chart 234 dataLimits [21.175 25.975 14.625 42.575] - subviewer 227 + subviewer 234 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [227 0 0] + linkNode [234 0 0] } instance { - id 234 + id 241 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 234 } chart { - id 235 + id 242 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 + treeNode [0 243 0 0] + viewObj 242 ssIdHighWaterMark 14 decomposition CLUSTER_CHART type EML_CHART @@ -24779,19 +25146,19 @@ Stateflow { eml { name "processOutputQP_oneFoot" } - firstData 237 - firstTransition 244 - firstJunction 243 + firstData 244 + firstTransition 251 + firstJunction 250 } state { - id 236 + id 243 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 235 - treeNode [235 0 0 0] + chart 242 + treeNode [242 0 0 0] superState SUBCHART - subviewer 235 + subviewer 242 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -24804,7 +25171,7 @@ Stateflow { } } data { - id 237 + id 244 ssIdNumber 7 name "analyticalSolution" scope INPUT_DATA @@ -24826,10 +25193,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [235 0 238] + linkNode [242 0 245] } data { - id 238 + id 245 ssIdNumber 4 name "primalSolution" scope INPUT_DATA @@ -24848,10 +25215,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [235 237 239] + linkNode [242 244 246] } data { - id 239 + id 246 ssIdNumber 6 name "QPStatus" scope INPUT_DATA @@ -24873,10 +25240,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [235 238 240] + linkNode [242 245 247] } data { - id 240 + id 247 ssIdNumber 14 name "feetContactStatus" scope INPUT_DATA @@ -24898,10 +25265,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [235 239 241] + linkNode [242 246 248] } data { - id 241 + id 248 ssIdNumber 5 name "f_star" scope OUTPUT_DATA @@ -24921,10 +25288,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [235 240 242] + linkNode [242 247 249] } data { - id 242 + id 249 ssIdNumber 10 name "Config" scope PARAMETER_DATA @@ -24946,19 +25313,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [235 241 0] + linkNode [242 248 0] } junction { - id 243 + id 250 position [23.5747 49.5747 7] - chart 235 - subviewer 235 + chart 242 + subviewer 242 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [235 0 0] + linkNode [242 0 0] } transition { - id 244 + id 251 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -24966,39 +25333,39 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 243 + id 250 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 235 + chart 242 dataLimits [21.175 25.975 14.625 42.575] - subviewer 235 + subviewer 242 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [235 0 0] + linkNode [242 0 0] } instance { - id 245 + id 252 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Proc" "ess QP output" - chart 235 + chart 242 } target { - id 246 + id 253 machine 1 name "sfun" codeFlags "" - linkNode [1 0 247] + linkNode [1 0 254] } target { - id 247 + id 254 machine 1 name "rtw" - linkNode [1 246 0] + linkNode [1 253 0] } } From 9061b7968f8ec396a533d9174a438cd912fa9e8b Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Tue, 4 May 2021 21:09:46 +0200 Subject: [PATCH 16/20] Changes for lowering the desired CoM - Changed the initial joint configuration in order to bend the knees. - lower by 10cm the desired CoM hight: `StateMachine.CoM_delta(:,3) = -0.10`. --- .../robots/iCubGazeboV2_5/configRobotSim.m | 4 ++-- .../iCubGazeboV2_5/configStateMachine.m | 10 ++++---- .../iCubGazeboV2_5/gainsAndReferences.m | 24 ++++++++++--------- 3 files changed, 20 insertions(+), 18 deletions(-) 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 index 35efe5b2..44b1c405 100644 --- 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 @@ -56,8 +56,8 @@ 0; 0; 0; ... -35.97; 29.97; 0.06; 50.00; ... -35.97; 29.97; 0.06; 50.00; ... - 0 ; 0 ; 0 ; 0 ; 0; 0; ... - 0 ; 0 ; 0 ; 0 ; 0; 0]*pi/180; + 10; 0; 0; -20; -10; 0; ... + 10; 0; 0; -20; -10; 0]*pi/180; % velocty initial conditions initialConditions.base_linear_velocity = [0; 0; 0]; 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 index 8c9df41c..f157e9f1 100644 --- 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 @@ -23,11 +23,11 @@ Sat.maxJointsPositionDelta = 15*pi/180; % [rad] %% Regularization parameters -Reg.pinvDamp_baseVel = 1e-7; -Reg.pinvDamp = 1; -Reg.pinvTol = 1e-5; -Reg.KP_postural = 0.1; -Reg.KD_postural = 0; +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 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 index 7b2eae33..7ad69355 100644 --- 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 @@ -18,6 +18,8 @@ 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)/20; % Angular momentum gains @@ -93,18 +95,18 @@ % 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 + 0.0, 0.00, -0.10; %% state == 2 COM TRANSITION TO LEFT FOOT + 0.0, 0.00, -0.10; %% state == 3 LEFT FOOT BALANCING + 0.0, 0.005,-0.10; %% state == 4 YOGA LEFT FOOT + 0.0, 0.00, -0.10; %% state == 5 PREPARING FOR SWITCHING + 0.0, -0.09, -0.10; %% state == 6 LOOKING FOR CONTACT + 0.0, 0.00, -0.10; %% 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.10; %% state == 8 COM TRANSITION TO RIGHT FOOT + 0.0, 0.00, -0.10; %% state == 9 RIGHT FOOT BALANCING + 0.0, -0.005,-0.10; %% state == 10 YOGA RIGHT FOOT + 0.0, 0.00, -0.10; %% state == 11 PREPARING FOR SWITCHING + 0.0, 0.09, -0.10; %% state == 12 LOOKING FOR CONTACT 0.0, 0.00, 0.0]; %% NOT USED %% Joint references From f2385950fc2fc35f913019fdfb4adbacb1f6b20b Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Tue, 4 May 2021 23:50:23 +0200 Subject: [PATCH 17/20] Do not skip the YOGA anymore. Restore default `StateMachine.CoM_delta`. Increase KD gains. - No need to add an offset to the desired CoM since the initial joint configuration already has the knees bent. There is enough margin to maintain the CoM hight when transitioning the CoM to the left or ight foot. - Increase the Gain.KD_CoM and Gain.KD_postural as the velocity feedback in the simulator is accurate. This stabilizes significantly the joint trajectory. At this point, the YOGA with support on left foot works and is stable. The robot falls when attempting the second YOGA on the right foot. --- .../iCubGazeboV2_5/configStateMachine.m | 2 +- .../iCubGazeboV2_5/gainsAndReferences.m | 26 +++++++++---------- 2 files changed, 14 insertions(+), 14 deletions(-) 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 index f157e9f1..67fa91f2 100644 --- 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 @@ -51,7 +51,7 @@ StateMachine.tBalancing = 1; StateMachine.tBalancingBeforeYoga = 1; StateMachine.yogaExtended = true; -StateMachine.skipYoga = true; +StateMachine.skipYoga = false; StateMachine.demoOnlyBalancing = false; StateMachine.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first StateMachine.yogaAlsoOnRightFoot = true; % TO DO: yoga on both feet starting from right foot (not available for now) 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 index 7ad69355..47555122 100644 --- 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 @@ -20,7 +20,7 @@ % 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)/20; +Gain.KD_CoM = 2*sqrt(Gain.KP_CoM)/2; % Angular momentum gains Gain.KI_AngularMomentum = 0.25 ; @@ -42,7 +42,7 @@ 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,:))/20; +Gain.KD_postural = 2*sqrt(Gain.KP_postural(1,:))/2; % symmetric gains Gain.KP_postural(8:12,:) = Gain.KP_postural(2:6,:); @@ -95,18 +95,18 @@ % 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.10; %% state == 2 COM TRANSITION TO LEFT FOOT - 0.0, 0.00, -0.10; %% state == 3 LEFT FOOT BALANCING - 0.0, 0.005,-0.10; %% state == 4 YOGA LEFT FOOT - 0.0, 0.00, -0.10; %% state == 5 PREPARING FOR SWITCHING - 0.0, -0.09, -0.10; %% state == 6 LOOKING FOR CONTACT - 0.0, 0.00, -0.10; %% state == 7 TWO FEET BALANCING + 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.10; %% state == 8 COM TRANSITION TO RIGHT FOOT - 0.0, 0.00, -0.10; %% state == 9 RIGHT FOOT BALANCING - 0.0, -0.005,-0.10; %% state == 10 YOGA RIGHT FOOT - 0.0, 0.00, -0.10; %% state == 11 PREPARING FOR SWITCHING - 0.0, 0.09, -0.10; %% state == 12 LOOKING FOR CONTACT + 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 From 5f5d7fe717e60f0f6b4fc0f6f456d1cb91098d34 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Thu, 6 May 2021 15:12:32 +0200 Subject: [PATCH 18/20] Cleanup the whole model (debug blocks, extra logging) and activate YOGA left foot only --- .../iCubGazeboV2_5/configStateMachine.m | 2 +- .../torqueControlBalancingWithSimu.mdl | 2255 +++++++---------- 2 files changed, 935 insertions(+), 1322 deletions(-) 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 index 67fa91f2..0c4cc531 100644 --- 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 @@ -54,7 +54,7 @@ StateMachine.skipYoga = false; StateMachine.demoOnlyBalancing = false; StateMachine.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first -StateMachine.yogaAlsoOnRightFoot = true; % TO DO: yoga on both feet starting from right foot (not available for now) +StateMachine.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot (not available for now) %%%% List of possible "Yoga in loop" %%%% diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl index a1f504ba..0618f4b5 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl +++ b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl @@ -7,47 +7,9 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "3.120" + ComputedModelVersion "3.122" NumModelReferences 0 - NumTestPointedSignals 5 - TestPointedSignal { - SignalName "" - FullBlockPath "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL" - LogSignal 1 - MaxPoints 5000 - Decimation 2 - } - TestPointedSignal { - SignalName "" - FullBlockPath "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/MATLAB Function" - LogSignal 1 - MaxPoints 5000 - Decimation 2 - } - TestPointedSignal { - SignalName "" - FullBlockPath "torqueControlBalancingWithSimu/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Sta" - "te Machine" - PortIndex 2 - LogSignal 1 - MaxPoints 5000 - Decimation 2 - } - TestPointedSignal { - SignalName "" - FullBlockPath "torqueControlBalancingWithSimu/Subsystem/Bus Selector" - LogSignal 1 - MaxPoints 5000 - Decimation 2 - } - TestPointedSignal { - SignalName "" - FullBlockPath "torqueControlBalancingWithSimu/Subsystem/Bus Selector" - PortIndex 2 - LogSignal 1 - MaxPoints 5000 - Decimation 2 - } + NumTestPointedSignals 0 NumProvidedFunctions 0 NumRequiredFunctions 0 NumResetEvents 0 @@ -440,7 +402,7 @@ Model { } Array { Type "Simulink.EditorInfo" - Dimension 73 + Dimension 83 Object { $ObjectID 5 IsActive [1] @@ -448,499 +410,642 @@ Model { ViewObjType "SimulinkTopLevel" LoadSaveID "0" Extents [2171.0, 1188.0] - ZoomFactor [2.0281179327233292] - Offset [712.93675521064722, 152.64617823095665] - SceneRectInView [712.93675521064722, 152.64617823095665, 1070.4505714245181, 585.76475304114581] + 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 "4836" + LoadSaveID "4850" Extents [2171.0, 1188.0] - ZoomFactor [1.7499999999999998] - Offset [89.479636723777276, -28.60714285714289] - SceneRectInView [89.479636723777276, -28.60714285714289, 1240.5714285714287, 678.85714285714289] + ZoomFactor [1.3964802319507956] + Offset [400.19641548607717, -203.35510808500248] + SceneRectInView [400.19641548607717, -203.35510808500248, 1554.6227940278457, 850.710216170005] } Object { $ObjectID 7 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "2087" + LoadSaveID "4226" Extents [2171.0, 1188.0] - ZoomFactor [1.75] - Offset [-736.65534376844744, 2332.9648668093268] - SceneRectInView [-736.65534376844744, 2332.9648668093268, 1240.5714285714287, 678.85714285714289] + ZoomFactor [4.6350722637938047] + Offset [12.87372526868586, 135.34665821934539] + SceneRectInView [12.87372526868586, 135.34665821934539, 468.38536196262828, 256.30668356130923] } Object { $ObjectID 8 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4537" + LoadSaveID "4993" Extents [2171.0, 1188.0] - ZoomFactor [5.1816867469879524] - Offset [-108.23828125, -66.072916666666657] - SceneRectInView [-108.23828125, -66.072916666666657, 418.97553943452374, 229.26897321428569] + ZoomFactor [3.8] + Offset [-158.02631578947376, -70.1842105263158] + SceneRectInView [-158.02631578947376, -70.1842105263158, 571.31578947368428, 312.63157894736844] } Object { $ObjectID 9 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "2341" + LoadSaveID "4994" Extents [2171.0, 1188.0] - ZoomFactor [1.5849611820686202] - Offset [-1178.7732548019947, -284.77258542366184] - SceneRectInView [-1178.7732548019947, -284.77258542366184, 1369.7496346039895, 749.54517084732367] + 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 "4955" + LoadSaveID "4913" Extents [2171.0, 1188.0] - ZoomFactor [2.0773052223433903] - Offset [-119.91137569995328, 98.052613159122757] - SceneRectInView [-119.91137569995328, 98.052613159122757, 1045.1040013999066, 571.89477368175449] + ZoomFactor [3.2951942101426641] + Offset [80.154339026122614, 154.55463376836479] + SceneRectInView [80.154339026122614, 154.55463376836479, 658.838253999605, 360.52503258937384] } Object { $ObjectID 11 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4030" + LoadSaveID "4216" Extents [2171.0, 1188.0] - ZoomFactor [3.2861189801699715] - Offset [135.07198275862066, -277.26034482758621] - SceneRectInView [135.07198275862066, -277.26034482758621, 660.65775862068972, 361.52068965517242] + ZoomFactor [5.2673944072532821] + Offset [569.47947453590621, 441.73077216658527] + SceneRectInView [569.47947453590621, 441.73077216658527, 412.15823842818759, 225.53845566682949] } Object { $ObjectID 12 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4307" + LoadSaveID "4269" Extents [2171.0, 1188.0] - ZoomFactor [2.56669704897075] - Offset [-48.06941549217828, -144.92583198051949] - SceneRectInView [-48.06941549217828, -144.92583198051949, 845.83414348435656, 462.851663961039] + ZoomFactor [5.95] + Offset [-33.73965992647058, -45.831932773109244] + SceneRectInView [-33.73965992647058, -45.831932773109244, 364.8739495798319, 199.66386554621849] } Object { $ObjectID 13 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "2163" + LoadSaveID "4620" Extents [2171.0, 1188.0] - ZoomFactor [1.4201130022789792] - Offset [-608.35758121641425, -114.33481638525888] - SceneRectInView [-608.35758121641425, -114.33481638525888, 1528.7515828078519, 836.55314618872774] + ZoomFactor [10.0] + Offset [82.52421875, 140.1] + SceneRectInView [82.52421875, 140.1, 217.1, 118.8] } Object { $ObjectID 14 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "2355" + LoadSaveID "2163" Extents [2171.0, 1188.0] - ZoomFactor [1.0] - Offset [1012.8742726293103, -129.0] - SceneRectInView [1012.8742726293103, -129.0, 2171.0, 1188.0] + ZoomFactor [1.4201130022789792] + Offset [-608.35758121641425, -114.33481638525888] + SceneRectInView [-608.35758121641425, -114.33481638525888, 1528.7515828078519, 836.55314618872774] } Object { $ObjectID 15 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "5014" + LoadSaveID "3241" Extents [2171.0, 1188.0] - ZoomFactor [2.03] - Offset [-310.81869612068976, -180.61083743842369] - SceneRectInView [-310.81869612068976, -180.61083743842369, 1069.4581280788179, 585.22167487684737] + ZoomFactor [1.5612382234185733] + Offset [-827.23464439655174, -71.967241379310337] + SceneRectInView [-827.23464439655174, -71.967241379310337, 1390.5629310344827, 760.93448275862067] } Object { $ObjectID 16 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937:34" + LoadSaveID "4030" Extents [2171.0, 1188.0] - ZoomFactor [2.92] - Offset [-164.07534246575341, -91.924657534246592] - SceneRectInView [-164.07534246575341, -91.924657534246592, 743.49315068493149, 406.84931506849318] + ZoomFactor [3.2861189801699715] + Offset [256.79612068965514, -277.26034482758621] + SceneRectInView [256.79612068965514, -277.26034482758621, 660.65775862068972, 361.52068965517242] } Object { $ObjectID 17 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937:14" + LoadSaveID "4806" Extents [2171.0, 1188.0] - ZoomFactor [2.85] - Offset [-165.70175438596488, -113.92105263157893] - SceneRectInView [-165.70175438596488, -113.92105263157893, 761.75438596491222, 416.84210526315786] + ZoomFactor [2.0483743061062651] + Offset [-793.97389720558533, -290.08334454895737] + SceneRectInView [-793.97389720558533, -290.08334454895737, 1059.8648857917149, 579.97212543554] } Object { $ObjectID 18 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937:67" + LoadSaveID "4245" Extents [2171.0, 1188.0] - ZoomFactor [2.87] - Offset [-165.54878048780483, -115.96864111498257] - SceneRectInView [-165.54878048780483, -115.96864111498257, 756.44599303135885, 413.93728222996515] + ZoomFactor [1.0] + Offset [92.0078125, -350.98802908437432] + SceneRectInView [92.0078125, -350.98802908437432, 2171.0, 1188.0] } Object { $ObjectID 19 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4938" + LoadSaveID "4211" Extents [2171.0, 1188.0] - ZoomFactor [2.9013369436452869] - Offset [-326.36442414255714, -208.23320111992533] - SceneRectInView [-326.36442414255714, -208.23320111992533, 748.27572328511428, 409.46640223985065] + ZoomFactor [2.4071047957371228] + Offset [-112.83168536009441, -373.76948051948051] + SceneRectInView [-112.83168536009441, -373.76948051948051, 901.91337072018882, 493.538961038961] } Object { $ObjectID 20 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937" + LoadSaveID "4307" Extents [2171.0, 1188.0] - ZoomFactor [1.0] - Offset [-327.9765625, -403.5] - SceneRectInView [-327.9765625, -403.5, 2171.0, 1188.0] + ZoomFactor [2.56669704897075] + Offset [-48.06941549217828, -144.92583198051949] + SceneRectInView [-48.06941549217828, -144.92583198051949, 845.83414348435656, 462.851663961039] } Object { $ObjectID 21 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4962" + LoadSaveID "4519" Extents [2171.0, 1188.0] - ZoomFactor [6.0] - Offset [-116.23828124999997, -57.0] - SceneRectInView [-116.23828124999997, -57.0, 361.83333333333331, 198.0] + ZoomFactor [1.5723743845222296] + Offset [-706.68143309105676, -478.27262581168833] + SceneRectInView [-706.68143309105676, -478.27262581168833, 1380.7144286821135, 755.54525162337666] } Object { $ObjectID 22 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937:74" + LoadSaveID "4353" Extents [2171.0, 1188.0] - ZoomFactor [3.0] - Offset [-132.82012310290415, -16.387940803946396] - SceneRectInView [-132.82012310290415, -16.387940803946396, 723.66666666666663, 396.0] + ZoomFactor [3.0839924670433145] + Offset [96.146189545676577, -4.1074743527112787] + SceneRectInView [96.146189545676577, -4.1074743527112787, 703.95762090864685, 385.21494870542256] } Object { $ObjectID 23 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4579" + LoadSaveID "4321" Extents [2171.0, 1188.0] - ZoomFactor [2.2524271844660193] - Offset [-544.74878771551721, -352.2155172413793] - SceneRectInView [-544.74878771551721, -352.2155172413793, 963.84913793103453, 527.43103448275861] + ZoomFactor [3.5884297520661157] + Offset [-211.69612560424537, 252.15114629325387] + SceneRectInView [-211.69612560424537, 252.15114629325387, 605.0, 331.06402579456471] } Object { $ObjectID 24 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4393" + LoadSaveID "4504" Extents [2171.0, 1188.0] - ZoomFactor [1.6735854351990824] - Offset [374.06441393490434, 302.07339302379842] - SceneRectInView [374.06441393490434, 302.07339302379842, 1297.2149221301913, 709.85321395240317] + ZoomFactor [1.0] + Offset [-410.51953125, -620.5] + SceneRectInView [-410.51953125, -620.5, 2171.0, 1188.0] } Object { $ObjectID 25 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937:27" + LoadSaveID "4104" Extents [2171.0, 1188.0] - ZoomFactor [2.85] - Offset [-173.20175438596488, -115.42105263157893] - SceneRectInView [-173.20175438596488, -115.42105263157893, 761.75438596491222, 416.84210526315786] + ZoomFactor [3.0933333333333333] + Offset [-128.63423482247225, -154.52586206896552] + SceneRectInView [-128.63423482247225, -154.52586206896552, 701.83189655172418, 384.05172413793105] } Object { $ObjectID 26 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4937:21" + LoadSaveID "2275" Extents [2171.0, 1188.0] - ZoomFactor [2.85] - Offset [-165.70175438596488, -113.92105263157893] - SceneRectInView [-165.70175438596488, -113.92105263157893, 761.75438596491222, 416.84210526315786] + ZoomFactor [1.8249725000639532] + Offset [-194.803483319316, -141.48435660218672] + SceneRectInView [-194.803483319316, -141.48435660218672, 1189.606966638632, 650.96871320437344] } Object { $ObjectID 27 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "2275" + LoadSaveID "2341" Extents [2171.0, 1188.0] - ZoomFactor [1.8249725000639532] - Offset [-194.803483319316, -141.48435660218672] - SceneRectInView [-194.803483319316, -141.48435660218672, 1189.606966638632, 650.96871320437344] + ZoomFactor [1.5849611820686202] + Offset [-1178.7732548019947, -284.77258542366184] + SceneRectInView [-1178.7732548019947, -284.77258542366184, 1369.7496346039895, 749.54517084732367] } Object { $ObjectID 28 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4696" + LoadSaveID "4553" Extents [2171.0, 1188.0] - ZoomFactor [2.2264875239923225] - Offset [917.234213362069, -427.28793103448277] - SceneRectInView [917.234213362069, -427.28793103448277, 975.078448275862, 533.57586206896553] + ZoomFactor [2.441659625391233] + Offset [-116.92703117366869, -144.85219724177151] + SceneRectInView [-116.92703117366869, -144.85219724177151, 889.14932180693916, 486.55430414861524] } Object { $ObjectID 29 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4684" + LoadSaveID "2087" Extents [2171.0, 1188.0] - ZoomFactor [2.7884615384615383] - Offset [1438.5531788793103, 818.97931034482758] - SceneRectInView [1438.5531788793103, 818.97931034482758, 778.56551724137933, 426.04137931034484] + ZoomFactor [1.7499999999999998] + Offset [-736.65534376844744, 2332.9648668093268] + SceneRectInView [-736.65534376844744, 2332.9648668093268, 1240.5714285714287, 678.85714285714289] } Object { $ObjectID 30 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4519" + LoadSaveID "3720" Extents [2171.0, 1188.0] - ZoomFactor [1.5723743845222296] - Offset [-706.68143309105676, -478.27262581168833] - SceneRectInView [-706.68143309105676, -478.27262581168833, 1380.7144286821135, 755.54525162337666] + ZoomFactor [5.95] + Offset [-33.739659926470551, -42.831932773109244] + SceneRectInView [-33.739659926470551, -42.831932773109244, 364.8739495798319, 199.66386554621849] } Object { $ObjectID 31 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "3714" + LoadSaveID "3719" Extents [2171.0, 1188.0] - ZoomFactor [2.08] - Offset [109.74429086538464, -263.57692307692309] - SceneRectInView [109.74429086538464, -263.57692307692309, 1043.75, 571.15384615384619] + 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 "4553" + LoadSaveID "4422" Extents [2171.0, 1188.0] - ZoomFactor [2.441659625391233] - Offset [-116.92703117366869, -144.85219724177151] - SceneRectInView [-116.92703117366869, -144.85219724177151, 889.14932180693916, 486.55430414861524] + ZoomFactor [4.2554142103630159] + Offset [42.960073495100346, -92.586881707886135] + SceneRectInView [42.960073495100346, -92.586881707886135, 510.17360300979931, 279.17376341577227] } Object { $ObjectID 33 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] + 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 34 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] + 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 35 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] + 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 36 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4216" - Extents [2075.0, 1188.0] - ZoomFactor [5.2673944072532821] - Offset [578.59213941133362, 441.73077216658527] - SceneRectInView [578.59213941133362, 441.73077216658527, 393.93290867733265, 225.53845566682949] + 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 37 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] + 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 38 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4269" - Extents [2075.0, 1188.0] - ZoomFactor [5.95] - Offset [-25.672433035714278, -45.831932773109244] - SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] + 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 39 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4226" - Extents [2075.0, 1188.0] - ZoomFactor [4.6350722637938047] - Offset [23.229550867122612, 135.34665821934539] - SceneRectInView [23.229550867122612, 135.34665821934539, 447.67371076575478, 256.30668356130923] + 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 40 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4913" - Extents [2075.0, 1188.0] - ZoomFactor [3.2951942101426641] - Offset [94.721007009531661, 154.55463376836479] - SceneRectInView [94.721007009531661, 154.55463376836479, 629.70491803278685, 360.52503258937384] + 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 41 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4211" - Extents [2075.0, 1188.0] - ZoomFactor [2.4071047957371228] - Offset [-92.890717237308081, -373.76948051948051] - SceneRectInView [-92.890717237308081, -373.76948051948051, 862.03143447461616, 493.538961038961] + 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 42 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "4245" - Extents [2075.0, 1188.0] - ZoomFactor [1.0] - Offset [161.34960569474492, -350.98802908437432] - SceneRectInView [161.34960569474492, -350.98802908437432, 2075.0, 1188.0] + 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 "4353" - Extents [2075.0, 1188.0] - ZoomFactor [3.0839924670433145] - Offset [111.71042989741085, -4.1074743527112787] - SceneRectInView [111.71042989741085, -4.1074743527112787, 672.82914020517831, 385.21494870542256] + 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 "4850" - Extents [2075.0, 1188.0] - ZoomFactor [1.3964802319507956] - Offset [434.56854543234, -203.35510808500248] - SceneRectInView [434.56854543234, -203.35510808500248, 1485.87853413532, 850.710216170005] + 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 "3241" - Extents [2075.0, 1188.0] - ZoomFactor [1.5612382234185733] - Offset [-796.48981681034491, -71.967241379310337] - SceneRectInView [-796.48981681034491, -71.967241379310337, 1329.0732758620691, 760.93448275862067] + 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 "4104" - Extents [2075.0, 1188.0] - ZoomFactor [3.0933333333333333] - Offset [-113.1169934431619, -154.52586206896552] - SceneRectInView [-113.1169934431619, -154.52586206896552, 670.79741379310349, 384.05172413793105] + 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 "4620" - Extents [2075.0, 1188.0] - ZoomFactor [10.0] - Offset [87.32421875, 140.1] - SceneRectInView [87.32421875, 140.1, 207.5, 118.8] + 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 "4504" - Extents [2075.0, 1188.0] - ZoomFactor [0.99107089183204411] - Offset [-371.86694504310344, -625.85167594516] - SceneRectInView [-371.86694504310344, -625.85167594516, 2093.6948275862069, 1198.70335189032] + 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 "4993" - Extents [2075.0, 1188.0] - ZoomFactor [3.8] - Offset [-145.39473684210532, -70.1842105263158] - SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] + 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] @@ -948,7 +1053,7 @@ Model { SceneRectInView [-191.40632054176072, -117.29872084273889, 691.66666666666663, 396.0] } Object { - $ObjectID 51 + $ObjectID 64 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -959,7 +1064,7 @@ Model { SceneRectInView [-92.7656132430399, -55.8837471783296, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 52 + $ObjectID 65 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -970,7 +1075,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 53 + $ObjectID 66 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -981,7 +1086,7 @@ Model { SceneRectInView [-99.198645598194133, -48.972911963882623, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 54 + $ObjectID 67 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -992,7 +1097,7 @@ Model { SceneRectInView [-80.667161130818016, -31.015048908954071, 468.39729119638827, 268.17155756207677] } Object { - $ObjectID 55 + $ObjectID 68 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1003,7 +1108,7 @@ Model { SceneRectInView [13.879095336481726, -85.310064935064929, 502.36680932703655, 287.62012987012986] } Object { - $ObjectID 56 + $ObjectID 69 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1014,7 +1119,7 @@ Model { SceneRectInView [13.972911622276058, -85.325665859564168, 502.42130750605327, 287.65133171912834] } Object { - $ObjectID 57 + $ObjectID 70 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1025,7 +1130,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 58 + $ObjectID 71 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1036,18 +1141,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 59 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "4994" - Extents [2075.0, 1188.0] - ZoomFactor [3.8] - Offset [-145.39473684210532, -70.1842105263158] - SceneRectInView [-145.39473684210532, -70.1842105263158, 546.0526315789474, 312.63157894736844] - } - Object { - $ObjectID 60 + $ObjectID 72 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1058,18 +1152,7 @@ Model { SceneRectInView [-64.168329397874857, -71.912514757969319, 518.75, 297.0] } Object { - $ObjectID 61 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "4806" - Extents [1722.0, 1188.0] - ZoomFactor [2.0483743061062651] - Offset [-684.37478764306115, -290.08334454895737] - SceneRectInView [-684.37478764306115, -290.08334454895737, 840.66666666666652, 579.97212543554] - } - Object { - $ObjectID 62 + $ObjectID 73 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1080,7 +1163,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 63 + $ObjectID 74 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1091,7 +1174,7 @@ Model { SceneRectInView [-25.672433035714278, -45.831932773109244, 348.7394957983193, 199.66386554621849] } Object { - $ObjectID 64 + $ObjectID 75 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1102,7 +1185,7 @@ Model { SceneRectInView [-8.0130751303127283, -114.03393582177664, 473.82302526062546, 223.19657177225341] } Object { - $ObjectID 65 + $ObjectID 76 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1113,7 +1196,7 @@ Model { SceneRectInView [16.019427876904615, -398.80851543704892, 1030.9142692461908, 485.61703087409785] } Object { - $ObjectID 66 + $ObjectID 77 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1124,7 +1207,7 @@ Model { SceneRectInView [-105.54956270048115, -242.68495036561927, 864.60693790096229, 407.2771777265437] } Object { - $ObjectID 67 + $ObjectID 78 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1135,7 +1218,7 @@ Model { SceneRectInView [-396.83333333333337, -304.36666666666662, 1148.0, 792.0] } Object { - $ObjectID 68 + $ObjectID 79 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1146,7 +1229,7 @@ Model { SceneRectInView [34.9092200413223, -92.857244318181813, 438.7831224173554, 302.71448863636363] } Object { - $ObjectID 69 + $ObjectID 80 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1157,7 +1240,7 @@ Model { SceneRectInView [-183.3666666666665, -165.10000000000005, 867.6, 475.2] } Object { - $ObjectID 70 + $ObjectID 81 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1168,7 +1251,7 @@ Model { SceneRectInView [10.41989407940855, -23.235071395084702, 501.20498970990775, 274.51891552575859] } Object { - $ObjectID 71 + $ObjectID 82 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1179,18 +1262,7 @@ Model { SceneRectInView [-68.684210488152473, -33.394738158739756, 477.63157399976672, 216.05262934094131] } Object { - $ObjectID 72 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "4321" - Extents [1815.0, 821.0] - ZoomFactor [3.0] - Offset [-211.69612560424537, 280.84982585720286] - SceneRectInView [-211.69612560424537, 280.84982585720286, 605.0, 273.66666666666669] - } - Object { - $ObjectID 73 + $ObjectID 83 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1201,7 +1273,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 74 + $ObjectID 84 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1212,7 +1284,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 75 + $ObjectID 85 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1223,7 +1295,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 76 + $ObjectID 86 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1234,7 +1306,7 @@ Model { SceneRectInView [0.0, 0.0, 0.0, 0.0] } Object { - $ObjectID 77 + $ObjectID 87 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -1248,42 +1320,56 @@ Model { } Array { Type "Simulink.DockComponentInfo" - Dimension 2 + Dimension 3 Object { - $ObjectID 78 - Type "GLUE2:PropertyInspector" - ID "Property Inspector" + $ObjectID 88 + Type "GLUE2:SpreadSheet" + ID "ModelData" + Visible [0] + CreateCallback "DataView.createSpreadSheetComponent" + UserData "" + Floating [0] + DockPosition "Bottom" + Width [2205] + Height [206] + Minimized "Unset" + } + Object { + $ObjectID 89 + Type "Simulink:Editor:ReferencedFiles" + ID "Referenced Files" Visible [0] CreateCallback "" - UserData "" + UserData "{\"filterShowRefModels\":\"true\",\"filterShowRefSubs\":\"true\",\"filterShowOnlyDirtyFiles\":\"false\"}\n" Floating [0] - DockPosition "Right" + DockPosition "Left" Width [640] Height [480] Minimized "Unset" } Object { - $ObjectID 79 - Type "Simulink:Editor:ReferencedFiles" - ID "Referenced Files" + $ObjectID 90 + Type "GLUE2:PropertyInspector" + ID "Property Inspector" Visible [0] CreateCallback "" - UserData "{\"filterShowRefModels\":\"true\",\"filterShowRefSubs\":\"true\",\"filterShowOnlyDirtyFiles\":\"false\"}\n" + UserData "" Floating [0] - DockPosition "Left" + DockPosition "Right" Width [640] Height [480] Minimized "Unset" } PropName "DockComponentsInfo" } - WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAFeAAAE4fwCAAAABPsAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + WindowState "AAAA/wAAAAD9AAAAAwAAAAAAAAFeAAAE4fwCAAAABPsAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQBAAAAeQAABOEAAABrAP////sAAABgAFMAaQBtAHUAb" "ABpAG4AawA6AEUAZABpAHQAbwByADoAUgBlAGYAZQByAGUAbgBjAGUAZABGAGkAbABlAHMALwBSAGUAZgBlAHIAZQBuAGMAZQBkACAARgBpAGwAZ" - "QBzAAAAAAD/////AAAAjwD///8AAAABAAABYAAABOH8AgAAAAH7AAAAVABHAEwAVQBFADIAOgBQAHIAbwBwAGUAcgB0AHkASQBuAHMAcABlAGMAd" - "ABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA////AAAIoQAABOEAAAABAAAAAgAAAAEAAAAC/" - "AAAAAEAAAACAAAAAA==" + "QBzAAAAAAD/////AAAAjwD///8AAAABAAABYAAABOH8AgAAAAH8AAAAeQAABOEAAAAAAP////r/////AgAAAAL7AAAAVABHAEwAVQBFADIAOgBQA" + "HIAbwBwAGUAcgB0AHkASQBuAHMAcABlAGMAdABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA/" + "///+/////8AAAAAAP////8AAABpAP///wAAAAMAAAihAAAA6vwBAAAAAfsAAAA2AEcATABVAEUAMgA6AFMAcAByAGUAYQBkAFMAaABlAGUAdAAvA" + "E0AbwBkAGUAbABEAGEAdABhAAAAAV8AAAihAAABSAD///8AAAihAAAE4QAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAA" Array { Type "Cell" Dimension 0 @@ -1302,9 +1388,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "nunoguedelha" ModifiedDateFormat "%" - LastModifiedDate "Tue May 04 15:45:49 2021" - RTWModifiedTimeStamp 542042503 - ModelVersionFormat "%" + LastModifiedDate "Thu May 06 15:11:05 2021" + RTWModifiedTimeStamp 542214505 + ModelVersionFormat "%" SampleTimeColors on SampleTimeAnnotations on LibraryLinkDisplay "disabled" @@ -1361,7 +1447,7 @@ Model { TryForcingSFcnDF off Object { $PropName "DataLoggingOverride" - $ObjectID 80 + $ObjectID 91 $ClassName "Simulink.SimulationData.ModelLoggingInfo" model_ "torqueControlBalancingWithSimu" signals_ [] @@ -1381,133 +1467,9 @@ Model { } Object { $PropName "InstrumentedSignals" - $ObjectID 81 + $ObjectID 92 $ClassName "Simulink.HMI.InstrumentedSignals" - Array { - Type "Struct" - Dimension 5 - MATStruct { - UUID "361a222e-8799-4264-88ff-83ceed2da1a7" - BlockPath_ "Subsystem/Bus Selector" - SID_ "5009" - SubPath_ "" - OutputPortIndex_ [1.0] - LogicalPortIndex_ [0.0] - SignalName_ "" - SubSysPath_ "" - Decimation_ [1.0] - MaxPoints_ [1024.0] - TargetBufferedStreaming_ [0.0] - IsFrameBased_ [0.0] - HideInSDI_ [0.0] - DomainType_ "" - Array { - Type "Struct" - Dimension 1 - MATStruct { - } - PropName "DomainParams_" - } - VisualType_ "" - } - MATStruct { - UUID "dddab1a3-2010-48e7-907b-c669add332b6" - BlockPath_ "Subsystem/Bus Selector" - SID_ "5009" - SubPath_ "" - OutputPortIndex_ [2.0] - LogicalPortIndex_ [0.0] - SignalName_ "" - SubSysPath_ "" - Decimation_ [1.0] - MaxPoints_ [1024.0] - TargetBufferedStreaming_ [0.0] - IsFrameBased_ [0.0] - HideInSDI_ [0.0] - DomainType_ "" - Array { - Type "Struct" - Dimension 1 - MATStruct { - } - PropName "DomainParams_" - } - VisualType_ "" - } - MATStruct { - UUID "cdd1a5b9-cc47-4f84-98a9-874c613b5099" - BlockPath_ "MOMENTUM BASED TORQUE CONTROL" - SID_ "4836" - SubPath_ "" - OutputPortIndex_ [1.0] - LogicalPortIndex_ [0.0] - SignalName_ "" - SubSysPath_ "" - Decimation_ [1.0] - MaxPoints_ [1024.0] - TargetBufferedStreaming_ [0.0] - IsFrameBased_ [0.0] - HideInSDI_ [0.0] - DomainType_ "" - Array { - Type "Struct" - Dimension 1 - MATStruct { - } - PropName "DomainParams_" - } - VisualType_ "" - } - MATStruct { - UUID "f3d563f6-1537-46f0-b052-2788cde11d95" - BlockPath_ "MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine" - SID_ "2163" - SubPath_ "" - OutputPortIndex_ [2.0] - LogicalPortIndex_ [0.0] - SignalName_ "" - SubSysPath_ "" - Decimation_ [1.0] - MaxPoints_ [1024.0] - TargetBufferedStreaming_ [0.0] - IsFrameBased_ [0.0] - HideInSDI_ [0.0] - DomainType_ "" - Array { - Type "Struct" - Dimension 1 - MATStruct { - } - PropName "DomainParams_" - } - VisualType_ "" - } - MATStruct { - UUID "c612eb14-7151-45a8-ab91-9218ca14d03e" - BlockPath_ "MOMENTUM BASED TORQUE CONTROL/MATLAB Function" - SID_ "5019" - SubPath_ "" - OutputPortIndex_ [1.0] - LogicalPortIndex_ [0.0] - SignalName_ "" - SubSysPath_ "" - Decimation_ [1.0] - MaxPoints_ [1024.0] - TargetBufferedStreaming_ [0.0] - IsFrameBased_ [0.0] - HideInSDI_ [0.0] - DomainType_ "" - Array { - Type "Struct" - Dimension 1 - MATStruct { - } - PropName "DomainParams_" - } - VisualType_ "" - } - PropName "Persistence" - } + Persistence [] } ExtModeBatchMode off ExtModeEnableFloating on @@ -1539,7 +1501,7 @@ Model { Type "Handle" Dimension 1 Simulink.ConfigSet { - $ObjectID 82 + $ObjectID 93 Version "20.1.0" DisabledProps [] Description "" @@ -1547,7 +1509,7 @@ Model { Type "Handle" Dimension 9 Simulink.SolverCC { - $ObjectID 83 + $ObjectID 94 Version "20.1.0" DisabledProps [] Description "" @@ -1589,7 +1551,7 @@ Model { ODENIntegrationMethod "ode3" } Simulink.DataIOCC { - $ObjectID 84 + $ObjectID 95 Version "20.1.0" DisabledProps [] Description "" @@ -1631,7 +1593,7 @@ Model { LoggingIntervals "[-inf, inf]" } Simulink.OptimizationCC { - $ObjectID 85 + $ObjectID 96 Version "20.1.0" Array { Type "Cell" @@ -1705,7 +1667,7 @@ Model { EfficientTunableParamExpr off } Simulink.DebuggingCC { - $ObjectID 86 + $ObjectID 97 Version "20.1.0" Array { Type "Cell" @@ -1828,7 +1790,7 @@ Model { VariantConditionMismatch "none" } Simulink.HardwareCC { - $ObjectID 87 + $ObjectID 98 Version "20.1.0" DisabledProps [] Description "" @@ -1878,7 +1840,7 @@ Model { HardwareBoardFeatureSet "EmbeddedCoderHSP" } Simulink.ModelReferenceCC { - $ObjectID 88 + $ObjectID 99 Version "20.1.0" DisabledProps [] Description "" @@ -1898,7 +1860,7 @@ Model { SupportModelReferenceSimTargetCustomCode off } Simulink.SFSimCC { - $ObjectID 89 + $ObjectID 100 Version "20.1.0" DisabledProps [] Description "" @@ -1942,7 +1904,7 @@ Model { } Simulink.RTWCC { $BackupClass "Simulink.RTWCC" - $ObjectID 90 + $ObjectID 101 Version "20.1.0" Array { Type "Cell" @@ -2048,7 +2010,7 @@ Model { Type "Handle" Dimension 2 Simulink.CodeAppCC { - $ObjectID 91 + $ObjectID 102 Version "20.1.0" Array { Type "Cell" @@ -2138,7 +2100,7 @@ Model { } Simulink.GRTTargetCC { $BackupClass "Simulink.TargetCC" - $ObjectID 92 + $ObjectID 103 Version "20.1.0" Array { Type "Cell" @@ -2168,7 +2130,7 @@ Model { Type "Handle" Dimension 1 Simulink.CPPComponent { - $ObjectID 93 + $ObjectID 104 Version "20.1.0" Array { Type "Cell" @@ -2279,7 +2241,7 @@ Model { } } SlCovCC.ConfigComp { - $ObjectID 94 + $ObjectID 105 Version "20.1.0" DisabledProps [] Description "Simulink Coverage Configuration Component" @@ -2320,7 +2282,7 @@ Model { PropName "Components" } Name "Configuration" - CurrentDlgPage "Solver" + CurrentDlgPage "Data Import//Export" ConfigPrmDlgPosition [ 200, 140, 1722, 1010 ] ExtraOptions "-aBlockCommentType=\"BlockPathComment\" -aIgnoreTestpoints=0 " } @@ -2328,11 +2290,11 @@ Model { } Simulink.ConfigSet { $PropName "ActiveConfigurationSet" - $ObjectID 82 + $ObjectID 93 } Object { $PropName "DataTransfer" - $ObjectID 95 + $ObjectID 106 $ClassName "Simulink.GlobalDataTransfer" DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" @@ -2437,14 +2399,6 @@ Model { DisplayOption "bar" BusSelectionMode off } - Block { - BlockType Display - Format "short" - Decimation "10" - Floating off - Lockdown off - SampleTime "-1" - } Block { BlockType EnablePort StatesWhenEnabling "held" @@ -2722,7 +2676,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "203" + ZoomFactor "206" ReportName "simulink-default.rpt" SIDHighWatermark "5020" SimulinkSubDomain "Simulink" @@ -2816,6 +2770,12 @@ Model { ContentPreviewEnabled off ConfigSource "Workspace" ConfigObject "'WBTConfigRobot'" + RobotName "'icubSim'" + UrdfFile "'model.urdf'" + ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" + ControlBoardsNames "{'left_arm','right_arm','torso'}" + LocalName "'WBT'" + GravityVector "[0,0,-9.81]" } Block { BlockType Constant @@ -2826,7 +2786,7 @@ Model { Value "zeros(3,1)" Object { $PropName "MaskObject" - $ObjectID 96 + $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." @@ -2834,12 +2794,12 @@ Model { RunInitForIconRedraw "off" Object { $PropName "DialogControls" - $ObjectID 97 + $ObjectID 108 $ClassName "Simulink.dialog.Group" Prompt "%" Object { $PropName "DialogControls" - $ObjectID 98 + $ObjectID 109 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -2848,14 +2808,6 @@ Model { } } } - Block { - BlockType Constant - Name "Constant1" - SID "5012" - Position [1055, 320, 1115, 350] - ZOrder 1289 - Value "zeros(23,1)" - } Block { BlockType SubSystem Name "MOMENTUM BASED TORQUE CONTROL" @@ -2868,9 +2820,6 @@ Model { TreatAsAtomicUnit on SystemSampleTime "Config.tStep" RequestExecContextInheritance off - Port { - PortNumber 1 - } System { Name "MOMENTUM BASED TORQUE CONTROL" Location [-75, -1417, 2485, 0] @@ -2982,7 +2931,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "100" + ZoomFactor "107" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -4335,7 +4284,7 @@ Model { Name "MatchSignalSizes" SID "4679" Ports [2, 1] - Position [1735, 986, 1825, 1079] + Position [1735, 976, 1825, 1069] ZOrder 757 ShowName off HideAutomaticName off @@ -4501,7 +4450,7 @@ Model { Name "QP Two Feet" SID "4681" Ports [5, 2] - Position [1575, 1019, 1685, 1171] + Position [1575, 1014, 1685, 1166] ZOrder 752 LibraryVersion "1.684" SourceBlock "WBToolboxLibrary/Utilities/QP" @@ -4536,7 +4485,7 @@ Model { ZOrder 37 SrcBlock "QP Two Feet" SrcPort 2 - Points [0, -5; 23, 0] + Points [0, 0; 23, 0] Branch { ZOrder 91 Points [0, -25] @@ -4576,7 +4525,6 @@ Model { ZOrder 41 SrcBlock "MatchSignalSizes" SrcPort 1 - Points [43, 0; 0, -10] DstBlock "Process QP output" DstPort 2 } @@ -4584,7 +4532,7 @@ Model { ZOrder 42 SrcBlock "QP Two Feet" SrcPort 1 - Points [0, -5] + Points [20, 0; 0, -10] DstBlock "MatchSignalSizes" DstPort 2 } @@ -4613,10 +4561,10 @@ Model { Points [47, 0] Branch { ZOrder 89 - Points [0, -50] + Points [0, -60] Branch { ZOrder 92 - Points [0, -105] + Points [0, -95] DstBlock "Analytical Solution Two Feet (unconstrained)" DstPort 2 } @@ -5053,7 +5001,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "208" + ZoomFactor "122" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -5127,7 +5075,7 @@ Model { RequestExecContextInheritance off System { Name "Compute base to fixed link transform" - Location [134, 55, 3840, 2160] + Location [-75, -1417, 2485, 0] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off PortBlocksUseCompactNotation off @@ -5143,7 +5091,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "723" + ZoomFactor "426" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -7223,7 +7171,7 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 99 + $ObjectID 110 $ClassName "Simulink.Mask" Display "disp('VISUALIZER')" } @@ -8975,7 +8923,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "99" + ZoomFactor "100" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -9776,13 +9724,6 @@ Model { ShowName off Gain "180/pi" } - Block { - BlockType Ground - Name "Ground" - SID "4971" - Position [70, 110, 90, 130] - ZOrder 897 - } Block { BlockType Scope Name "Joint Position Desired" @@ -10479,19 +10420,37 @@ Model { DstPort 3 } Line { - ZOrder 13 - SrcBlock "Ground" + ZOrder 62 + SrcBlock "tau_star" SrcPort 1 - Points [125, 0] + 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 { + } + Branch { ZOrder 15 DstBlock "Demux9" DstPort 1 + } + } + Branch { + ZOrder 101 + DstBlock "Sum" + DstPort 1 + } + } + Branch { + ZOrder 64 + DstBlock "Demux8" + DstPort 1 } } Line { @@ -10870,23 +10829,6 @@ Model { DstBlock "Demux7" DstPort 1 } - Line { - ZOrder 62 - SrcBlock "tau_star" - SrcPort 1 - Points [41, 0] - Branch { - ZOrder 63 - Points [0, 150] - DstBlock "Sum" - DstPort 1 - } - Branch { - ZOrder 64 - DstBlock "Demux8" - DstPort 1 - } - } Line { ZOrder 65 SrcBlock "nu" @@ -11869,7 +11811,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "300" + ZoomFactor "359" SimulinkSubDomain "Simulink" Block { BlockType Inport @@ -13060,127 +13002,6 @@ Model { } } } - Block { - BlockType SubSystem - Name "MATLAB Function" - SID "5019" - Ports [1, 1] - Position [1000, 2, 1215, 48] - ZOrder 976 - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - ContentPreviewEnabled on - Port { - PortNumber 1 - PropagatedSignals "conditionNumber" - } - 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 "21" - SimulinkSubDomain "Simulink" - Block { - BlockType Inport - Name "M" - SID "5019::1" - Position [20, 101, 40, 119] - ZOrder -1 - } - Block { - BlockType Demux - Name " Demux " - SID "5019::20" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 11 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "5019::19" - Tag "Stateflow S-Function 10" - Ports [1, 2] - Position [180, 100, 230, 160] - ZOrder 10 - FunctionName "sf_sfun" - PortCounts "[1 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "conditionNumber" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "5019::21" - Position [460, 241, 480, 259] - ZOrder 12 - } - Block { - BlockType Outport - Name "conditionNumber" - SID "5019::5" - Position [460, 101, 480, 119] - ZOrder -5 - } - Line { - ZOrder 5 - SrcBlock "M" - SrcPort 1 - Points [65, 0; 0, 20] - DstBlock " SFunction " - DstPort 1 - } - Line { - Name "conditionNumber" - ZOrder 6 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - Points [121, 0; 0, -35] - DstBlock "conditionNumber" - DstPort 1 - } - Line { - ZOrder 7 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 8 - SrcBlock " SFunction " - SrcPort 1 - Points [16, 0; 0, 135] - DstBlock " Demux " - DstPort 1 - } - } - } Block { BlockType SubSystem Name "Robot State and References" @@ -15475,10 +15296,6 @@ Model { BackgroundColor "cyan" Priority "-100" RequestExecContextInheritance off - Port { - PortNumber 2 - PropagatedSignals "feetContactStatus" - } System { Name "State Machine" Location [-75, -1417, 2485, 0] @@ -18203,13 +18020,6 @@ Model { } } } - Block { - BlockType Terminator - Name "Terminator" - SID "5020" - Position [1270, 15, 1290, 35] - ZOrder 977 - } Block { BlockType SubSystem Name "emergency stop: joint limits" @@ -18222,7 +18032,7 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 100 + $ObjectID 111 $ClassName "Simulink.Mask" Display "disp('EMERGENCY STOP')" RunInitForIconRedraw "off" @@ -19057,18 +18867,8 @@ Model { ZOrder 90 SrcBlock "Dynamics and Kinematics" SrcPort 1 - Points [29, 0] - Branch { - ZOrder 373 - Points [0, -26; 209, 0; 0, 46] - DstBlock "MATLAB Function" - DstPort 1 - } - Branch { - ZOrder 372 - DstBlock "Balancing Controller QP" - DstPort 1 - } + DstBlock "Balancing Controller QP" + DstPort 1 } Line { ZOrder 132 @@ -19203,13 +19003,6 @@ Model { DstBlock "Goto" DstPort 1 } - Line { - ZOrder 374 - SrcBlock "MATLAB Function" - SrcPort 1 - DstBlock "Terminator" - DstPort 1 - } } } Block { @@ -19419,23 +19212,6 @@ Model { Position [-100, 288, -70, 302] ZOrder 1275 } - Block { - BlockType BusSelector - Name "Bus\nSelector" - SID "5009" - Ports [1, 2] - Position [120, 556, 125, 594] - ZOrder 1282 - OutputSignals "fc,left_right_foot_in_contact" - Port { - PortNumber 1 - Name "" - } - Port { - PortNumber 2 - Name "" - } - } Block { BlockType BusSelector Name "Bus\nSelector1" @@ -19495,6 +19271,12 @@ Model { ContentPreviewEnabled on ConfigSource "Workspace" ConfigObject "'WBTConfigRobotSim'" + RobotName "'icubSim'" + UrdfFile "'model.urdf'" + ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" + ControlBoardsNames "{'left_arm','right_arm','torso'}" + LocalName "'WBT'" + GravityVector "[0,0,-9.81]" } Block { BlockType Constant @@ -19513,24 +19295,6 @@ Model { Value "motorsReflectedInertia" VectorParams1D off } - Block { - BlockType Display - Name "Display" - SID "5010" - Ports [1] - Position [340, 472, 420, 658] - ZOrder 1283 - Decimation "1" - } - Block { - BlockType Display - Name "Display1" - SID "5011" - Ports [1] - Position [215, 602, 305, 638] - ZOrder 1284 - Decimation "1" - } Block { BlockType Reference Name "IMUsensor" @@ -19830,17 +19594,8 @@ Model { SrcBlock "RobotDynWithContacts" SrcPort 4 Points [23, 0; 0, 105] - Branch { - ZOrder 339 - Points [-307, 0; 0, 125] - DstBlock "Bus\nSelector" - DstPort 1 - } - Branch { - ZOrder 338 - DstBlock "Bus\nSelector1" - DstPort 1 - } + DstBlock "Bus\nSelector1" + DstPort 1 } Line { Name "" @@ -19862,29 +19617,10 @@ Model { } } Line { - ZOrder 323 - SrcBlock "RobotDynWithContacts" - SrcPort 2 - DstBlock "wrench_LFoot" - DstPort 1 - } - Line { - Name "" - ZOrder 340 - Labels [1, 1] - SrcBlock "Bus\nSelector" - SrcPort 1 - DstBlock "Display" - DstPort 1 - } - Line { - Name "" - ZOrder 341 - Labels [-1, 1] - SrcBlock "Bus\nSelector" + ZOrder 323 + SrcBlock "RobotDynWithContacts" SrcPort 2 - Points [38, 0; 0, 35] - DstBlock "Display1" + DstBlock "wrench_LFoot" DstPort 1 } Line { @@ -20106,7 +19842,7 @@ Stateflow { id 1 name "torqueControlBalancingWithSimu" sfVersion 80000036 - firstTarget 253 + firstTarget 246 } chart { id 2 @@ -21736,23 +21472,23 @@ Stateflow { chart { id 84 machine 1 - name "MOMENTUM BASED TORQUE CONTROL/MATLAB Function" + 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 5 + ssIdHighWaterMark 8 decomposition CLUSTER_CHART type EML_CHART - chartFileNumber 10 + chartFileNumber 11 disableImplicitCasting 1 eml { - name "matConditionNumber" + name "saturateInputDerivativeFCN" } firstData 86 - firstTransition 89 - firstJunction 88 + firstTransition 92 + firstJunction 91 } state { id 85 @@ -21766,129 +21502,6 @@ Stateflow { ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE - eml { - isEML 1 - script "function conditionNumber = matConditionNumber(M)\n\nconditionNumber = cond(M);\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 86 - 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 [84 0 87] - } - data { - id 87 - ssIdNumber 5 - name "conditionNumber" - 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 0] - } - junction { - id 88 - position [23.5747 49.5747 7] - chart 84 - subviewer 84 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [84 0 0] - } - transition { - id 89 - 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 88 - 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 90 - machine 1 - name "MOMENTUM BASED TORQUE CONTROL/MATLAB Function" - chart 84 - } - chart { - id 91 - 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 92 0 0] - viewObj 91 - ssIdHighWaterMark 8 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 11 - disableImplicitCasting 1 - eml { - name "saturateInputDerivativeFCN" - } - firstData 93 - firstTransition 99 - firstJunction 98 - } - state { - id 92 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 91 - treeNode [91 0 0 0] - superState SUBCHART - subviewer 91 - 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" @@ -21897,7 +21510,7 @@ Stateflow { } } data { - id 93 + id 86 ssIdNumber 4 name "u" scope INPUT_DATA @@ -21916,10 +21529,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [91 0 94] + linkNode [84 0 87] } data { - id 94 + id 87 ssIdNumber 5 name "uSat" scope OUTPUT_DATA @@ -21939,10 +21552,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [91 93 95] + linkNode [84 86 88] } data { - id 95 + id 88 ssIdNumber 6 name "u_0" scope INPUT_DATA @@ -21964,10 +21577,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [91 94 96] + linkNode [84 87 89] } data { - id 96 + id 89 ssIdNumber 7 name "tStep" scope INPUT_DATA @@ -21989,10 +21602,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [91 95 97] + linkNode [84 88 90] } data { - id 97 + id 90 ssIdNumber 8 name "uDotMax" scope INPUT_DATA @@ -22014,19 +21627,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [91 96 0] + linkNode [84 89 0] } junction { - id 98 + id 91 position [23.5747 49.5747 7] - chart 91 - subviewer 91 + chart 84 + subviewer 84 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [91 0 0] + linkNode [84 0 0] } transition { - id 99 + id 92 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -22034,36 +21647,36 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 98 + id 91 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 91 + chart 84 dataLimits [21.175 25.975 14.625 42.575] - subviewer 91 + subviewer 84 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [91 0 0] + linkNode [84 0 0] } instance { - id 100 + id 93 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/Saturate Torque Derivative" - chart 91 + chart 84 } chart { - id 101 + 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 102 0 0] - viewObj 101 + treeNode [0 95 0 0] + viewObj 94 ssIdHighWaterMark 86 decomposition CLUSTER_CHART type EML_CHART @@ -22072,19 +21685,19 @@ Stateflow { eml { name "stateMachineMomentumControlFCN" } - firstData 103 - firstTransition 127 - firstJunction 126 + firstData 96 + firstTransition 120 + firstJunction 119 } state { - id 102 + id 95 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 101 - treeNode [101 0 0 0] + chart 94 + treeNode [94 0 0 0] superState SUBCHART - subviewer 101 + subviewer 94 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -22102,7 +21715,7 @@ Stateflow { } } data { - id 103 + id 96 ssIdNumber 37 name "pos_CoM_0" scope INPUT_DATA @@ -22124,10 +21737,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 0 104] + linkNode [94 0 97] } data { - id 104 + id 97 ssIdNumber 54 name "jointPos_0" scope INPUT_DATA @@ -22149,10 +21762,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 103 105] + linkNode [94 96 98] } data { - id 105 + id 98 ssIdNumber 64 name "pos_CoM_fixed_l_sole" scope INPUT_DATA @@ -22174,10 +21787,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 104 106] + linkNode [94 97 99] } data { - id 106 + id 99 ssIdNumber 77 name "pos_CoM_fixed_r_sole" scope INPUT_DATA @@ -22199,10 +21812,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 105 107] + linkNode [94 98 100] } data { - id 107 + id 100 ssIdNumber 69 name "jointPos" scope INPUT_DATA @@ -22224,10 +21837,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 106 108] + linkNode [94 99 101] } data { - id 108 + id 101 ssIdNumber 56 name "time" scope INPUT_DATA @@ -22249,10 +21862,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 107 109] + linkNode [94 100 102] } data { - id 109 + id 102 ssIdNumber 65 name "wrench_rightFoot" scope INPUT_DATA @@ -22274,10 +21887,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 108 110] + linkNode [94 101 103] } data { - id 110 + id 103 ssIdNumber 74 name "wrench_leftFoot" scope INPUT_DATA @@ -22299,10 +21912,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 109 111] + linkNode [94 102 104] } data { - id 111 + id 104 ssIdNumber 71 name "w_H_b" scope OUTPUT_DATA @@ -22324,10 +21937,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 110 112] + linkNode [94 103 105] } data { - id 112 + id 105 ssIdNumber 52 name "pos_CoM_des" scope OUTPUT_DATA @@ -22349,10 +21962,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 111 113] + linkNode [94 104 106] } data { - id 113 + id 106 ssIdNumber 46 name "jointPos_des" scope OUTPUT_DATA @@ -22374,10 +21987,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 112 114] + linkNode [94 105 107] } data { - id 114 + id 107 ssIdNumber 43 name "feetContactStatus" scope OUTPUT_DATA @@ -22399,10 +22012,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 113 115] + linkNode [94 106 108] } data { - id 115 + id 108 ssIdNumber 72 name "l_sole_H_b" scope INPUT_DATA @@ -22424,10 +22037,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 114 116] + linkNode [94 107 109] } data { - id 116 + id 109 ssIdNumber 78 name "r_sole_H_b" scope INPUT_DATA @@ -22449,10 +22062,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 115 117] + linkNode [94 108 110] } data { - id 117 + id 110 ssIdNumber 55 name "StateMachine" scope PARAMETER_DATA @@ -22475,10 +22088,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 116 118] + linkNode [94 109 111] } data { - id 118 + id 111 ssIdNumber 66 name "KP_postural_diag" scope OUTPUT_DATA @@ -22500,10 +22113,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 117 119] + linkNode [94 110 112] } data { - id 119 + id 112 ssIdNumber 82 name "KP_CoM_diag" scope OUTPUT_DATA @@ -22525,10 +22138,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 118 120] + linkNode [94 111 113] } data { - id 120 + id 113 ssIdNumber 83 name "KD_CoM_diag" scope OUTPUT_DATA @@ -22550,10 +22163,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 119 121] + linkNode [94 112 114] } data { - id 121 + id 114 ssIdNumber 67 name "Gain" scope PARAMETER_DATA @@ -22576,10 +22189,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 120 122] + linkNode [94 113 115] } data { - id 122 + id 115 ssIdNumber 68 name "state" scope OUTPUT_DATA @@ -22601,10 +22214,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 121 123] + linkNode [94 114 116] } data { - id 123 + id 116 ssIdNumber 81 name "smoothingTimeJoints" scope OUTPUT_DATA @@ -22626,10 +22239,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 122 124] + linkNode [94 115 117] } data { - id 124 + id 117 ssIdNumber 86 name "smoothingTimeCoM" scope OUTPUT_DATA @@ -22651,10 +22264,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 123 125] + linkNode [94 116 118] } data { - id 125 + id 118 ssIdNumber 85 name "Config" scope PARAMETER_DATA @@ -22676,19 +22289,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [101 124 0] + linkNode [94 117 0] } junction { - id 126 + id 119 position [23.5747 49.5747 7] - chart 101 - subviewer 101 + chart 94 + subviewer 94 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [101 0 0] + linkNode [94 0 0] } transition { - id 127 + id 120 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -22696,37 +22309,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 126 + id 119 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 101 + chart 94 dataLimits [21.175 25.975 14.625 42.575] - subviewer 101 + subviewer 94 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [101 0 0] + linkNode [94 0 0] } instance { - id 128 + id 121 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/STATE MACHINE" - chart 101 + chart 94 } chart { - id 129 + 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 130 0 0] - viewObj 129 + treeNode [0 123 0 0] + viewObj 122 ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART @@ -22736,19 +22349,19 @@ Stateflow { name "checkInputSpikesFCN" } supportVariableSizing 0 - firstData 131 - firstTransition 136 - firstJunction 135 + firstData 124 + firstTransition 129 + firstJunction 128 } state { - id 130 + id 123 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 129 - treeNode [129 0 0 0] + chart 122 + treeNode [122 0 0 0] superState SUBCHART - subviewer 129 + subviewer 122 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -22764,7 +22377,7 @@ Stateflow { } } data { - id 131 + id 124 ssIdNumber 6 name "u" scope INPUT_DATA @@ -22783,10 +22396,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 0 132] + linkNode [122 0 125] } data { - id 132 + id 125 ssIdNumber 4 name "delta_u_max" scope INPUT_DATA @@ -22808,10 +22421,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 131 133] + linkNode [122 124 126] } data { - id 133 + id 126 ssIdNumber 7 name "noSpikes" scope OUTPUT_DATA @@ -22831,10 +22444,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 132 134] + linkNode [122 125 127] } data { - id 134 + id 127 ssIdNumber 9 name "res_check_spikes" scope OUTPUT_DATA @@ -22856,19 +22469,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [129 133 0] + linkNode [122 126 0] } junction { - id 135 + id 128 position [23.5747 49.5747 7] - chart 129 - subviewer 129 + chart 122 + subviewer 122 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [129 0 0] + linkNode [122 0 0] } transition { - id 136 + id 129 labelString "{eML_blk_kernel();}" labelPosition [40.125 31.875 102.544 14.964] fontSize 12 @@ -22876,38 +22489,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 135 + id 128 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 129 + chart 122 dataLimits [21.175 25.975 14.625 42.575] - subviewer 129 + subviewer 122 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [129 0 0] + linkNode [122 0 0] } instance { - id 137 + 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 129 + chart 122 } chart { - id 138 + 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 139 0 0] - viewObj 138 + treeNode [0 132 0 0] + viewObj 131 ssIdHighWaterMark 5 decomposition CLUSTER_CHART type EML_CHART @@ -22916,19 +22529,19 @@ Stateflow { eml { name "robotIsOnSingleSupportQP_FCN" } - firstData 140 - firstTransition 143 - firstJunction 142 + firstData 133 + firstTransition 136 + firstJunction 135 } state { - id 139 + id 132 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 138 - treeNode [138 0 0 0] + chart 131 + treeNode [131 0 0 0] superState SUBCHART - subviewer 138 + subviewer 131 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -22940,7 +22553,7 @@ Stateflow { } } data { - id 140 + id 133 ssIdNumber 4 name "feetContactStatus" scope INPUT_DATA @@ -22959,10 +22572,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 0 141] + linkNode [131 0 134] } data { - id 141 + id 134 ssIdNumber 5 name "onOneFoot" scope OUTPUT_DATA @@ -22982,19 +22595,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [138 140 0] + linkNode [131 133 0] } junction { - id 142 + id 135 position [23.5747 49.5747 7] - chart 138 - subviewer 138 + chart 131 + subviewer 131 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [138 0 0] + linkNode [131 0 0] } transition { - id 143 + id 136 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -23002,37 +22615,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 142 + id 135 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 138 + chart 131 dataLimits [21.175 25.975 14.625 42.575] - subviewer 138 + subviewer 131 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [138 0 0] + linkNode [131 0 0] } instance { - id 144 + id 137 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/One Foot Two Fee" "t QP Selector" - chart 138 + chart 131 } chart { - id 145 + 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 146 0 0] - viewObj 145 + treeNode [0 139 0 0] + viewObj 138 ssIdHighWaterMark 82 decomposition CLUSTER_CHART type EML_CHART @@ -23041,19 +22654,19 @@ Stateflow { eml { name "momentumBasedControllerFCN" } - firstData 147 - firstTransition 185 - firstJunction 184 + firstData 140 + firstTransition 178 + firstJunction 177 } state { - id 146 + id 139 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 145 - treeNode [145 0 0 0] + chart 138 + treeNode [138 0 0 0] superState SUBCHART - subviewer 145 + subviewer 138 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -23075,7 +22688,7 @@ Stateflow { } } data { - id 147 + id 140 ssIdNumber 66 name "HessianMatrixOneFoot" scope OUTPUT_DATA @@ -23097,10 +22710,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 0 148] + linkNode [138 0 141] } data { - id 148 + id 141 ssIdNumber 64 name "gradientOneFoot" scope OUTPUT_DATA @@ -23122,10 +22735,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 147 149] + linkNode [138 140 142] } data { - id 149 + id 142 ssIdNumber 5 name "ConstraintsMatrixOneFoot" scope OUTPUT_DATA @@ -23145,10 +22758,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 148 150] + linkNode [138 141 143] } data { - id 150 + id 143 ssIdNumber 52 name "bVectorConstraintsOneFoot" scope OUTPUT_DATA @@ -23170,10 +22783,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 149 151] + linkNode [138 142 144] } data { - id 151 + id 144 ssIdNumber 69 name "HessianMatrixTwoFeet" scope OUTPUT_DATA @@ -23195,10 +22808,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 150 152] + linkNode [138 143 145] } data { - id 152 + id 145 ssIdNumber 70 name "gradientTwoFeet" scope OUTPUT_DATA @@ -23220,10 +22833,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 151 153] + linkNode [138 144 146] } data { - id 153 + id 146 ssIdNumber 53 name "ConstraintsMatrixTwoFeet" scope OUTPUT_DATA @@ -23245,10 +22858,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 152 154] + linkNode [138 145 147] } data { - id 154 + id 147 ssIdNumber 54 name "bVectorConstraintsTwoFeet" scope OUTPUT_DATA @@ -23270,10 +22883,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 153 155] + linkNode [138 146 148] } data { - id 155 + id 148 ssIdNumber 57 name "tauModel" scope OUTPUT_DATA @@ -23295,10 +22908,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 154 156] + linkNode [138 147 149] } data { - id 156 + id 149 ssIdNumber 58 name "Sigma" scope OUTPUT_DATA @@ -23320,10 +22933,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 155 157] + linkNode [138 148 150] } data { - id 157 + id 150 ssIdNumber 62 name "Na" scope OUTPUT_DATA @@ -23345,10 +22958,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 156 158] + linkNode [138 149 151] } data { - id 158 + id 151 ssIdNumber 63 name "f_LDot" scope OUTPUT_DATA @@ -23370,10 +22983,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 157 159] + linkNode [138 150 152] } data { - id 159 + id 152 ssIdNumber 13 name "feetContactStatus" scope INPUT_DATA @@ -23395,10 +23008,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 158 160] + linkNode [138 151 153] } data { - id 160 + id 153 ssIdNumber 50 name "ConstraintsMatrix" scope INPUT_DATA @@ -23420,10 +23033,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 159 161] + linkNode [138 152 154] } data { - id 161 + id 154 ssIdNumber 51 name "bVectorConstraints" scope INPUT_DATA @@ -23445,10 +23058,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 160 162] + linkNode [138 153 155] } data { - id 162 + id 155 ssIdNumber 14 name "jointPos" scope INPUT_DATA @@ -23470,10 +23083,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 161 163] + linkNode [138 154 156] } data { - id 163 + id 156 ssIdNumber 4 name "jointPos_des" scope INPUT_DATA @@ -23492,10 +23105,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 162 164] + linkNode [138 155 157] } data { - id 164 + id 157 ssIdNumber 7 name "nu" scope INPUT_DATA @@ -23517,10 +23130,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 163 165] + linkNode [138 156 158] } data { - id 165 + id 158 ssIdNumber 8 name "M" scope INPUT_DATA @@ -23542,10 +23155,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 164 166] + linkNode [138 157 159] } data { - id 166 + id 159 ssIdNumber 9 name "h" scope INPUT_DATA @@ -23567,10 +23180,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 165 167] + linkNode [138 158 160] } data { - id 167 + id 160 ssIdNumber 11 name "L" scope INPUT_DATA @@ -23592,10 +23205,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 166 168] + linkNode [138 159 161] } data { - id 168 + id 161 ssIdNumber 6 name "intL_angMomError" scope INPUT_DATA @@ -23617,10 +23230,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 167 169] + linkNode [138 160 162] } data { - id 169 + id 162 ssIdNumber 42 name "w_H_l_sole" scope INPUT_DATA @@ -23642,10 +23255,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 168 170] + linkNode [138 161 163] } data { - id 170 + id 163 ssIdNumber 12 name "w_H_r_sole" scope INPUT_DATA @@ -23667,10 +23280,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 169 171] + linkNode [138 162 164] } data { - id 171 + id 164 ssIdNumber 77 name "J_l_sole" scope INPUT_DATA @@ -23692,10 +23305,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 170 172] + linkNode [138 163 165] } data { - id 172 + id 165 ssIdNumber 38 name "J_r_sole" scope INPUT_DATA @@ -23717,10 +23330,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 171 173] + linkNode [138 164 166] } data { - id 173 + id 166 ssIdNumber 32 name "JDot_l_sole_nu" scope INPUT_DATA @@ -23742,10 +23355,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 172 174] + linkNode [138 165 167] } data { - id 174 + id 167 ssIdNumber 33 name "JDot_r_sole_nu" scope INPUT_DATA @@ -23767,10 +23380,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 173 175] + linkNode [138 166 168] } data { - id 175 + id 168 ssIdNumber 40 name "pos_CoM" scope INPUT_DATA @@ -23792,10 +23405,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 174 176] + linkNode [138 167 169] } data { - id 176 + id 169 ssIdNumber 16 name "J_CoM" scope INPUT_DATA @@ -23817,10 +23430,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 175 177] + linkNode [138 168 170] } data { - id 177 + id 170 ssIdNumber 15 name "desired_pos_vel_acc_CoM" scope INPUT_DATA @@ -23842,10 +23455,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 176 178] + linkNode [138 169 171] } data { - id 178 + id 171 ssIdNumber 17 name "KP_CoM" scope INPUT_DATA @@ -23867,10 +23480,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 177 179] + linkNode [138 170 172] } data { - id 179 + id 172 ssIdNumber 81 name "KD_CoM" scope INPUT_DATA @@ -23892,10 +23505,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 178 180] + linkNode [138 171 173] } data { - id 180 + id 173 ssIdNumber 82 name "KP_postural" scope INPUT_DATA @@ -23917,10 +23530,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 179 181] + linkNode [138 172 174] } data { - id 181 + id 174 ssIdNumber 20 name "Reg" scope PARAMETER_DATA @@ -23943,10 +23556,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 180 182] + linkNode [138 173 175] } data { - id 182 + id 175 ssIdNumber 47 name "Gain" scope PARAMETER_DATA @@ -23969,10 +23582,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 181 183] + linkNode [138 174 176] } data { - id 183 + id 176 ssIdNumber 19 name "Config" scope PARAMETER_DATA @@ -23994,19 +23607,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [145 182 0] + linkNode [138 175 0] } junction { - id 184 + id 177 position [23.5747 49.5747 7] - chart 145 - subviewer 145 + chart 138 + subviewer 138 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [145 0 0] + linkNode [138 0 0] } transition { - id 185 + id 178 labelString "{eML_blk_kernel();}" labelPosition [36.125 25.875 102.544 14.964] fontSize 12 @@ -24014,36 +23627,36 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 184 + id 177 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 145 + chart 138 dataLimits [21.175 25.975 14.625 42.575] - subviewer 145 + subviewer 138 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [145 0 0] + linkNode [138 0 0] } instance { - id 186 + id 179 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Momentum Based Balancing Controller\n" - chart 145 + chart 138 } chart { - id 187 + 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 188 0 0] - viewObj 187 + treeNode [0 181 0 0] + viewObj 180 ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART @@ -24053,19 +23666,19 @@ Stateflow { name "checkInputRangeFCN" } supportVariableSizing 0 - firstData 189 - firstTransition 196 - firstJunction 195 + firstData 182 + firstTransition 189 + firstJunction 188 } state { - id 188 + id 181 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 187 - treeNode [187 0 0 0] + chart 180 + treeNode [180 0 0 0] superState SUBCHART - subviewer 187 + subviewer 180 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -24081,7 +23694,7 @@ Stateflow { } } data { - id 189 + id 182 ssIdNumber 4 name "umin" scope INPUT_DATA @@ -24103,10 +23716,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [187 0 190] + linkNode [180 0 183] } data { - id 190 + id 183 ssIdNumber 5 name "umax" scope INPUT_DATA @@ -24128,10 +23741,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [187 189 191] + linkNode [180 182 184] } data { - id 191 + id 184 ssIdNumber 6 name "u" scope INPUT_DATA @@ -24150,10 +23763,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [187 190 192] + linkNode [180 183 185] } data { - id 192 + id 185 ssIdNumber 7 name "inRange" scope OUTPUT_DATA @@ -24173,10 +23786,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [187 191 193] + linkNode [180 184 186] } data { - id 193 + id 186 ssIdNumber 8 name "tol" scope INPUT_DATA @@ -24198,10 +23811,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [187 192 194] + linkNode [180 185 187] } data { - id 194 + id 187 ssIdNumber 9 name "res_check_range" scope OUTPUT_DATA @@ -24223,19 +23836,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [187 193 0] + linkNode [180 186 0] } junction { - id 195 + id 188 position [23.5747 49.5747 7] - chart 187 - subviewer 187 + chart 180 + subviewer 180 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [187 0 0] + linkNode [180 0 0] } transition { - id 196 + id 189 labelString "{eML_blk_kernel();}" labelPosition [40.125 31.875 102.544 14.964] fontSize 12 @@ -24243,37 +23856,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 195 + id 188 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 187 + chart 180 dataLimits [21.175 25.975 14.625 42.575] - subviewer 187 + subviewer 180 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [187 0 0] + linkNode [180 0 0] } instance { - id 197 + id 190 machine 1 name "MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" - chart 187 + chart 180 } chart { - id 198 + 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 199 0 0] - viewObj 198 + treeNode [0 192 0 0] + viewObj 191 ssIdHighWaterMark 5 decomposition CLUSTER_CHART type EML_CHART @@ -24282,19 +23895,19 @@ Stateflow { eml { name "footOnGround" } - firstData 200 - firstTransition 203 - firstJunction 202 + firstData 193 + firstTransition 196 + firstJunction 195 } state { - id 199 + id 192 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 198 - treeNode [198 0 0 0] + chart 191 + treeNode [191 0 0 0] superState SUBCHART - subviewer 198 + subviewer 191 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -24309,7 +23922,7 @@ Stateflow { } } data { - id 200 + id 193 ssIdNumber 4 name "state" scope INPUT_DATA @@ -24328,10 +23941,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [198 0 201] + linkNode [191 0 194] } data { - id 201 + id 194 ssIdNumber 5 name "booleanState" scope OUTPUT_DATA @@ -24351,19 +23964,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [198 200 0] + linkNode [191 193 0] } junction { - id 202 + id 195 position [23.5747 49.5747 7] - chart 198 - subviewer 198 + chart 191 + subviewer 191 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [198 0 0] + linkNode [191 0 0] } transition { - id 203 + id 196 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -24371,38 +23984,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 202 + id 195 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 198 + chart 191 dataLimits [21.175 25.975 14.625 42.575] - subviewer 198 + subviewer 191 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [198 0 0] + linkNode [191 0 0] } instance { - id 204 + id 197 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Select ba" "se to world transform" - chart 198 + chart 191 } chart { - id 205 + 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 206 0 0] - viewObj 205 + treeNode [0 199 0 0] + viewObj 198 ssIdHighWaterMark 10 decomposition CLUSTER_CHART type EML_CHART @@ -24411,19 +24024,19 @@ Stateflow { eml { name "getEquivalentBaseVel_FCN" } - firstData 207 - firstTransition 214 - firstJunction 213 + firstData 200 + firstTransition 207 + firstJunction 206 } state { - id 206 + id 199 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 205 - treeNode [205 0 0 0] + chart 198 + treeNode [198 0 0 0] superState SUBCHART - subviewer 205 + subviewer 198 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -24439,7 +24052,7 @@ Stateflow { } } data { - id 207 + id 200 ssIdNumber 4 name "J_l_sole" scope INPUT_DATA @@ -24458,10 +24071,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [205 0 208] + linkNode [198 0 201] } data { - id 208 + id 201 ssIdNumber 6 name "J_r_sole" scope INPUT_DATA @@ -24483,10 +24096,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [205 207 209] + linkNode [198 200 202] } data { - id 209 + id 202 ssIdNumber 9 name "feetContactStatus" scope INPUT_DATA @@ -24508,10 +24121,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [205 208 210] + linkNode [198 201 203] } data { - id 210 + id 203 ssIdNumber 10 name "jointPos_err" scope INPUT_DATA @@ -24533,10 +24146,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [205 209 211] + linkNode [198 202 204] } data { - id 211 + id 204 ssIdNumber 7 name "baseVel_equivalent" scope OUTPUT_DATA @@ -24558,10 +24171,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [205 210 212] + linkNode [198 203 205] } data { - id 212 + id 205 ssIdNumber 8 name "Reg" scope PARAMETER_DATA @@ -24583,19 +24196,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [205 211 0] + linkNode [198 204 0] } junction { - id 213 + id 206 position [23.5747 49.5747 7] - chart 205 - subviewer 205 + chart 198 + subviewer 198 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [205 0 0] + linkNode [198 0 0] } transition { - id 214 + id 207 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -24603,38 +24216,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 213 + id 206 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 205 + chart 198 dataLimits [21.175 25.975 14.625 42.575] - subviewer 205 + subviewer 198 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [205 0 0] + linkNode [198 0 0] } instance { - id 215 + id 208 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Get Equiv" "alent Base Velocity" - chart 205 + chart 198 } chart { - id 216 + 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 217 0 0] - viewObj 216 + treeNode [0 210 0 0] + viewObj 209 ssIdHighWaterMark 6 decomposition CLUSTER_CHART type EML_CHART @@ -24643,19 +24256,19 @@ Stateflow { eml { name "analyticalSolutionQP_FCN" } - firstData 218 - firstTransition 222 - firstJunction 221 + firstData 211 + firstTransition 215 + firstJunction 214 } state { - id 217 + id 210 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 216 - treeNode [216 0 0 0] + chart 209 + treeNode [209 0 0 0] superState SUBCHART - subviewer 216 + subviewer 209 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -24667,7 +24280,7 @@ Stateflow { } } data { - id 218 + id 211 ssIdNumber 4 name "H" scope INPUT_DATA @@ -24686,10 +24299,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [216 0 219] + linkNode [209 0 212] } data { - id 219 + id 212 ssIdNumber 5 name "analyticalSolution" scope OUTPUT_DATA @@ -24709,10 +24322,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [216 218 220] + linkNode [209 211 213] } data { - id 220 + id 213 ssIdNumber 6 name "g" scope INPUT_DATA @@ -24734,19 +24347,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [216 219 0] + linkNode [209 212 0] } junction { - id 221 + id 214 position [23.5747 49.5747 7] - chart 216 - subviewer 216 + chart 209 + subviewer 209 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [216 0 0] + linkNode [209 0 0] } transition { - id 222 + id 215 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -24754,38 +24367,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 221 + id 214 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 216 + chart 209 dataLimits [21.175 25.975 14.625 42.575] - subviewer 216 + subviewer 209 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [216 0 0] + linkNode [209 0 0] } instance { - id 223 + 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 216 + chart 209 } chart { - id 224 + 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 225 0 0] - viewObj 224 + treeNode [0 218 0 0] + viewObj 217 ssIdHighWaterMark 14 decomposition CLUSTER_CHART type EML_CHART @@ -24794,19 +24407,19 @@ Stateflow { eml { name "processOutputQP_twoFeetFCN" } - firstData 226 - firstTransition 232 - firstJunction 231 + firstData 219 + firstTransition 225 + firstJunction 224 } state { - id 225 + id 218 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 224 - treeNode [224 0 0 0] + chart 217 + treeNode [217 0 0 0] superState SUBCHART - subviewer 224 + subviewer 217 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -24818,7 +24431,7 @@ Stateflow { } } data { - id 226 + id 219 ssIdNumber 7 name "analyticalSolution" scope INPUT_DATA @@ -24840,10 +24453,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [224 0 227] + linkNode [217 0 220] } data { - id 227 + id 220 ssIdNumber 4 name "primalSolution" scope INPUT_DATA @@ -24862,10 +24475,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [224 226 228] + linkNode [217 219 221] } data { - id 228 + id 221 ssIdNumber 6 name "QPStatus" scope INPUT_DATA @@ -24887,10 +24500,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [224 227 229] + linkNode [217 220 222] } data { - id 229 + id 222 ssIdNumber 5 name "f_star" scope OUTPUT_DATA @@ -24910,10 +24523,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [224 228 230] + linkNode [217 221 223] } data { - id 230 + id 223 ssIdNumber 10 name "Config" scope PARAMETER_DATA @@ -24935,19 +24548,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [224 229 0] + linkNode [217 222 0] } junction { - id 231 + id 224 position [23.5747 49.5747 7] - chart 224 - subviewer 224 + chart 217 + subviewer 217 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [224 0 0] + linkNode [217 0 0] } transition { - id 232 + id 225 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -24955,38 +24568,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 231 + id 224 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 224 + chart 217 dataLimits [21.175 25.975 14.625 42.575] - subviewer 224 + subviewer 217 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [224 0 0] + linkNode [217 0 0] } instance { - id 233 + id 226 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Proc" "ess QP output" - chart 224 + chart 217 } chart { - id 234 + 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 235 0 0] - viewObj 234 + treeNode [0 228 0 0] + viewObj 227 ssIdHighWaterMark 6 decomposition CLUSTER_CHART type EML_CHART @@ -24995,19 +24608,19 @@ Stateflow { eml { name "analyticalSolutionQP_FCN" } - firstData 236 - firstTransition 240 - firstJunction 239 + firstData 229 + firstTransition 233 + firstJunction 232 } state { - id 235 + id 228 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 234 - treeNode [234 0 0 0] + chart 227 + treeNode [227 0 0 0] superState SUBCHART - subviewer 234 + subviewer 227 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -25019,7 +24632,7 @@ Stateflow { } } data { - id 236 + id 229 ssIdNumber 4 name "H" scope INPUT_DATA @@ -25038,10 +24651,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [234 0 237] + linkNode [227 0 230] } data { - id 237 + id 230 ssIdNumber 5 name "analyticalSolution" scope OUTPUT_DATA @@ -25061,10 +24674,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [234 236 238] + linkNode [227 229 231] } data { - id 238 + id 231 ssIdNumber 6 name "g" scope INPUT_DATA @@ -25086,19 +24699,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [234 237 0] + linkNode [227 230 0] } junction { - id 239 + id 232 position [23.5747 49.5747 7] - chart 234 - subviewer 234 + chart 227 + subviewer 227 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [234 0 0] + linkNode [227 0 0] } transition { - id 240 + id 233 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -25106,38 +24719,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 239 + id 232 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 234 + chart 227 dataLimits [21.175 25.975 14.625 42.575] - subviewer 234 + subviewer 227 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [234 0 0] + linkNode [227 0 0] } instance { - id 241 + 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 234 + chart 227 } chart { - id 242 + 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 243 0 0] - viewObj 242 + treeNode [0 236 0 0] + viewObj 235 ssIdHighWaterMark 14 decomposition CLUSTER_CHART type EML_CHART @@ -25146,19 +24759,19 @@ Stateflow { eml { name "processOutputQP_oneFoot" } - firstData 244 - firstTransition 251 - firstJunction 250 + firstData 237 + firstTransition 244 + firstJunction 243 } state { - id 243 + id 236 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 242 - treeNode [242 0 0 0] + chart 235 + treeNode [235 0 0 0] superState SUBCHART - subviewer 242 + subviewer 235 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -25171,7 +24784,7 @@ Stateflow { } } data { - id 244 + id 237 ssIdNumber 7 name "analyticalSolution" scope INPUT_DATA @@ -25193,10 +24806,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [242 0 245] + linkNode [235 0 238] } data { - id 245 + id 238 ssIdNumber 4 name "primalSolution" scope INPUT_DATA @@ -25215,10 +24828,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [242 244 246] + linkNode [235 237 239] } data { - id 246 + id 239 ssIdNumber 6 name "QPStatus" scope INPUT_DATA @@ -25240,10 +24853,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [242 245 247] + linkNode [235 238 240] } data { - id 247 + id 240 ssIdNumber 14 name "feetContactStatus" scope INPUT_DATA @@ -25265,10 +24878,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [242 246 248] + linkNode [235 239 241] } data { - id 248 + id 241 ssIdNumber 5 name "f_star" scope OUTPUT_DATA @@ -25288,10 +24901,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [242 247 249] + linkNode [235 240 242] } data { - id 249 + id 242 ssIdNumber 10 name "Config" scope PARAMETER_DATA @@ -25313,19 +24926,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [242 248 0] + linkNode [235 241 0] } junction { - id 250 + id 243 position [23.5747 49.5747 7] - chart 242 - subviewer 242 + chart 235 + subviewer 235 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [242 0 0] + linkNode [235 0 0] } transition { - id 251 + id 244 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -25333,39 +24946,39 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 250 + id 243 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 242 + chart 235 dataLimits [21.175 25.975 14.625 42.575] - subviewer 242 + subviewer 235 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [242 0 0] + linkNode [235 0 0] } instance { - id 252 + id 245 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Proc" "ess QP output" - chart 242 + chart 235 } target { - id 253 + id 246 machine 1 name "sfun" codeFlags "" - linkNode [1 0 254] + linkNode [1 0 247] } target { - id 254 + id 247 machine 1 name "rtw" - linkNode [1 253 0] + linkNode [1 246 0] } } From 4edd9d3e76439b8682470e09a0f00dca1c406b69 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Fri, 7 May 2021 17:16:47 +0200 Subject: [PATCH 19/20] Removed unsupported models --- .../app/robots/iCubGenova02/configRobot.m | 99 ----- .../robots/iCubGenova02/configStateMachine.m | 160 -------- .../robots/iCubGenova02/gainsAndReferences.m | 346 ----------------- .../app/robots/iCubGenova04/configRobot.m | 102 ----- .../app/robots/iCubGenova04/configRobotSim.m | 162 -------- .../robots/iCubGenova04/configStateMachine.m | 160 -------- .../robots/iCubGenova04/gainsAndReferences.m | 351 ------------------ .../app/robots/icubGazeboSim/configRobot.m | 99 ----- .../robots/icubGazeboSim/configStateMachine.m | 159 -------- .../robots/icubGazeboSim/gainsAndReferences.m | 238 ------------ .../initTorqueControlBalancingWithSimu.m | 6 + 11 files changed, 6 insertions(+), 1876 deletions(-) delete mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/configRobot.m delete mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/configStateMachine.m delete mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/gainsAndReferences.m delete mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m delete mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m delete mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configStateMachine.m delete mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/gainsAndReferences.m delete mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/configRobot.m delete mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/configStateMachine.m delete mode 100644 controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/gainsAndReferences.m diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/configRobot.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/configRobot.m deleted file mode 100644 index 5d39ceb4..00000000 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/configRobot.m +++ /dev/null @@ -1,99 +0,0 @@ -% CONFIGROBOT initializes parameters specific of a particular robot -% (e.g., icuGazeboSim) - -%% --- Initialization --- -Config.ON_GAZEBO = false; -ROBOT_DOF = 23; -Config.GRAV_ACC = 9.81; - -% Robot configuration for WBToolbox -WBTConfigRobot = WBToolbox.Configuration; -WBTConfigRobot.RobotName = 'icub'; -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 - -% 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 = true; - -% 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 = true; -Config.INCLUDE_COUPLING = true; -Config.INCLUDE_HARMONIC_DRIVE_INERTIA = true; - -% 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_foot/cartesianEndEffectorWrench:o'; -Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o'; -Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; -Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; - -% Ports dimensions -Ports.NECK_POS_PORT_SIZE = 6; -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/iCubGenova02/configStateMachine.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/configStateMachine.m deleted file mode 100644 index 7107ed3c..00000000 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/configStateMachine.m +++ /dev/null @@ -1,160 +0,0 @@ -% 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; -Reg.pinvDamp = 0.07; -Reg.pinvTol = 1e-5; -Reg.KP_postural = 0.1; -Reg.KD_postural = 0; -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/iCubGenova02/gainsAndReferences.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/gainsAndReferences.m deleted file mode 100644 index 61318961..00000000 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova02/gainsAndReferences.m +++ /dev/null @@ -1,346 +0,0 @@ -% GAINSANDREFERENCES compute gains matrices, references and other control -% related quantities for each state of the state machine. - -%% --- Initialization --- - -% CoM gains -Gain.KP_CoM = [50 100 5 % state == 1 TWO FEET BALANCING - 50 100 5 % state == 2 COM TRANSITION TO LEFT - 50 100 5 % state == 3 LEFT FOOT BALANCING - 50 100 5 % state == 4 YOGA LEFT FOOT - 50 100 5 % state == 5 PREPARING FOR SWITCHING - 50 100 5 % state == 6 LOOKING FOR CONTACT - 50 100 5 % state == 7 TRANSITION TO INITIAL POSITION - 50 100 5 % state == 8 COM TRANSITION TO RIGHT FOOT - 50 100 5 % state == 9 RIGHT FOOT BALANCING - 50 100 5 % state == 10 YOGA RIGHT FOOT - 50 100 5 % state == 11 PREPARING FOR SWITCHING - 50 100 5 % state == 12 LOOKING FOR CONTACT - 50 100 5];% state == 13 TRANSITION TO INITIAL POSITION - -Gain.KD_CoM = 2*sqrt(Gain.KP_CoM)/20; - -% Angular momentum gains -Gain.KI_AngularMomentum = 3; -Gain.KP_AngularMomentum = 2*sqrt(Gain.KI_AngularMomentum)/5; - -% 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 10,220 550 220 200 65 300 % state == 5 PREPARING FOR SWITCHING - 30 30 30, 10 10 20 10, 10 10 20 10,100 350 20 200 10 100,220 550 220 200 65 300 % state == 6 LOOKING FOR CONTACT - 30 30 30, 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 50 30 60 100 100, 30 30 20 20 100 100 % state == 9 RIGHT FOOT BALANCING - 30 30 30, 10 10 10 10, 10 10 10 10,100 50 30 100 100 100,100 200 100 100 10 10 % state == 10 YOGA RIGHT FOOT - 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,200 250 20 20 10 10 % state == 11 PREPARING FOR SWITCHING - 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100 % state == 12 LOOKING FOR CONTACT - 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100];% state == 13 TRANSITION TO INITIAL POSITION - -Gain.KD_postural = 2*sqrt(Gain.KP_postural(1,:))/20; - -% symmetric gains -Gain.KP_postural(10,18:23) = Gain.KP_postural(10,18:23)*1.5; -Gain.KP_postural(10,1:3) = Gain.KP_postural(10,1:3)*2; -Gain.KP_postural(4,12:17) = Gain.KP_postural(4,12:17)*1.5; -Gain.KP_postural(4,1:3) = Gain.KP_postural(4,1:3)*2; -Gain.KP_postural(4,4:11) = Gain.KP_postural(4,4:11)*1.5; -Gain.KP_postural(7,2:3) = Gain.KP_postural(7,2:3)*1.5; - -%% 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 - 0.65; %% 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 - 0.65; %% state == 10 YOGA RIGHT FOOT - 2; %% state == 11 PREPARING FOR SWITCHING - 5; %% 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 - 0.65; %% 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 - 0.65; %% state == 10 YOGA RIGHT FOOT - 2; %% state == 11 PREPARING FOR SWITCHING - 5; %% 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.005, 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.02, -0.08, 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.01, 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.1060,-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.1279, 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.3273,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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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.2443,-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/app/robots/iCubGenova04/configRobot.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m deleted file mode 100644 index efee6ea0..00000000 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobot.m +++ /dev/null @@ -1,102 +0,0 @@ -% CONFIGROBOT initializes parameters specific of a particular robot -% (e.g., icuGazeboSim) - -%% --- Initialization --- -Config.ON_GAZEBO = false; -ROBOT_DOF = 23; -Config.GRAV_ACC = 9.81; - -% Robot configuration for WBToolbox -WBTConfigRobot = WBToolbox.Configuration; -WBTConfigRobot.RobotName = 'icub'; -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 = true; -Config.INCLUDE_COUPLING = true; -Config.INCLUDE_HARMONIC_DRIVE_INERTIA = true; - -% 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.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; -Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; -Ports.WRENCH_LEFT_FOOT = '/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o'; -Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o'; - -% Ports dimensions -Ports.NECK_POS_PORT_SIZE = 6; -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/iCubGenova04/configRobotSim.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m deleted file mode 100644 index 6a5ea839..00000000 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configRobotSim.m +++ /dev/null @@ -1,162 +0,0 @@ -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% % -% 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; ... - 0 ; 0 ; 0 ; 0 ; 0; 0; ... - 0 ; 0 ; 0 ; 0 ; 0; 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); - -KvmechMat = diag(0.5*ones(1,robot_config.N_DOF)); -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/iCubGenova04/configStateMachine.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configStateMachine.m deleted file mode 100644 index 6c1d1788..00000000 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/configStateMachine.m +++ /dev/null @@ -1,160 +0,0 @@ -% 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; -Reg.pinvDamp = 0.07; -Reg.pinvTol = 1e-5; -Reg.KP_postural = 0.1; -Reg.KD_postural = 0; -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.02; -StateMachine.joints_thresholdNotInContact = 15; -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 = 1; - -% 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 = true; - -%% 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/iCubGenova04/gainsAndReferences.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/gainsAndReferences.m deleted file mode 100644 index 87758ee4..00000000 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGenova04/gainsAndReferences.m +++ /dev/null @@ -1,351 +0,0 @@ -% GAINSANDREFERENCES compute gains matrices, references and other control -% related quantities for each state of the state machine. - -%% --- Initialization --- - -% CoM gains -Gain.KP_CoM = [50 100 5 % state == 1 TWO FEET BALANCING - 50 100 5 % state == 2 COM TRANSITION TO LEFT - 50 100 5 % state == 3 LEFT FOOT BALANCING - 50 100 5 % state == 4 YOGA LEFT FOOT - 50 100 5 % state == 5 PREPARING FOR SWITCHING - 50 100 5 % state == 6 LOOKING FOR CONTACT - 50 100 5 % state == 7 TRANSITION TO INITIAL POSITION - 50 150 5 % state == 8 COM TRANSITION TO RIGHT FOOT - 50 100 5 % state == 9 RIGHT FOOT BALANCING - 50 100 5 % state == 10 YOGA RIGHT FOOT - 50 100 5 % state == 11 PREPARING FOR SWITCHING - 50 100 5 % state == 12 LOOKING FOR CONTACT - 50 100 5];% state == 13 TRANSITION TO INITIAL POSITION - -Gain.KD_CoM = 2*sqrt(Gain.KP_CoM)/15; - -% Angular momentum gains -Gain.KI_AngularMomentum = 3 ; -Gain.KP_AngularMomentum = 2*sqrt(Gain.KI_AngularMomentum)/5; - -% 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 50 100 100 % state == 4 YOGA LEFT FOOT - 30 30 30, 5 5 10 10, 10 10 20 10,200 250 20 20 10 10,220 550 220 200 65 300 % state == 5 PREPARING FOR SWITCHING - 30 30 30, 10 10 20 10, 10 10 20 10,100 350 20 200 10 100,220 550 220 200 65 300 % 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 50 30 60 100 100, 30 30 20 20 100 100 % state == 9 RIGHT FOOT BALANCING - 30 30 30, 10 10 10 10, 10 10 10 10,100 50 30 50 100 100,100 200 100 100 10 10 % state == 10 YOGA RIGHT FOOT - 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,200 250 20 20 10 10 % state == 11 PREPARING FOR SWITCHING - 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100 % state == 12 LOOKING FOR CONTACT - 30 40 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100];% state == 13 TRANSITION TO INITIAL POSITION - -Gain.KD_postural = 0*sqrt(Gain.KP_postural(1,:))/20; - -% symmetric gains -Gain.KP_postural(4,1:3) = Gain.KP_postural(4,1:3)*3; -Gain.KP_postural(6,18:23) = Gain.KP_postural(6,18:23)*2; -Gain.KP_postural(10,1:3) = Gain.KP_postural(10,1:3)*3; -Gain.KP_postural(13,1:3) = Gain.KP_postural(13,1:3)*3; - -%% 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 - 0.9; %% 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 - 0.9; %% state == 10 YOGA RIGHT FOOT - 2; %% state == 11 PREPARING FOR SWITCHING - 2; %% state == 12 LOOKING FOR CONTACT - 5]; %% 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 - 0.9; %% 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 - 0.9; %% state == 10 YOGA RIGHT FOOT - 2; %% state == 11 PREPARING FOR SWITCHING - 2; %% state == 12 LOOKING FOR CONTACT - 5]; %% 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.005, 0.0; %% state == 3 LEFT FOOT BALANCING - 0.0, 0.005, 0.0; %% state == 4 YOGA LEFT FOOT - 0.0, 0.005, 0.0; %% state == 5 PREPARING FOR SWITCHING - 0.02,-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.005, 0.0; %% state == 9 RIGHT FOOT BALANCING - 0.0, -0.005, 0.0; %% state == 10 YOGA RIGHT FOOT - 0.0, -0.015, 0.0; %% state == 11 PREPARING FOR SWITCHING - 0.02, 0.02, 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.1060,-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.1279, 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.3273,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]; - -q18 = [-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]; - -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),q18]; - -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/app/robots/icubGazeboSim/configRobot.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/configRobot.m deleted file mode 100644 index 81647ffb..00000000 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/configRobot.m +++ /dev/null @@ -1,99 +0,0 @@ -% CONFIGROBOT initializes parameters specific of a particular robot -% (e.g., icuGazeboSim) - -%% --- 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 - -% 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 = false; - -% 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 = true; - -% 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 = 6; -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/icubGazeboSim/configStateMachine.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/configStateMachine.m deleted file mode 100644 index edc45cc0..00000000 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/configStateMachine.m +++ /dev/null @@ -1,159 +0,0 @@ -% 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; -Reg.pinvDamp = 1; -Reg.pinvTol = 1e-5; -Reg.KP_postural = 0.1; -Reg.KD_postural = 0; -Reg.HessianQP = 1e-7; - -%% State Machine configuration - -% time between two yoga positions -StateMachine.joints_pauseBetweenYogaMoves = 3; - -% contact forces threshold -StateMachine.wrench_thresholdContactOn = 25; -StateMachine.wrench_thresholdContactOff = 85; - -% 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.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.05 0.10; % xMin, xMax - -0.025 0.025]; % 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/icubGazeboSim/gainsAndReferences.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/gainsAndReferences.m deleted file mode 100644 index 3f854278..00000000 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/icubGazeboSim/gainsAndReferences.m +++ /dev/null @@ -1,238 +0,0 @@ -% GAINSANDREFERENCES compute gains matrices, references and other control -% related quantities for each state of the state machine. - -%% --- Initialization --- - -% CoM gains -Gain.KP_CoM = [10 50 10 % state == 1 TWO FEET BALANCING - 10 50 10 % state == 2 COM TRANSITION TO LEFT - 10 50 10 % state == 3 LEFT FOOT BALANCING - 10 50 10 % state == 4 YOGA LEFT FOOT - 10 50 10 % state == 5 PREPARING FOR SWITCHING - 10 50 10 % state == 6 LOOKING FOR CONTACT - 10 50 10 % state == 7 TRANSITION TO INITIAL POSITION - 10 50 10 % state == 8 COM TRANSITION TO RIGHT FOOT - 10 50 10 % state == 9 RIGHT FOOT BALANCING - 10 50 10 % state == 10 YOGA RIGHT FOOT - 10 50 10 % state == 11 PREPARING FOR SWITCHING - 10 50 10 % state == 12 LOOKING FOR CONTACT - 10 50 10]; % state == 13 TRANSITION TO INITIAL POSITION - -Gain.KD_CoM = 2*sqrt(Gain.KP_CoM); - -% 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 50 30 60 50 50, 30 50 30 60 50 50 % state == 1 TWO FEET BALANCING - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 2 COM TRANSITION TO LEFT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 3 LEFT FOOT BALANCING - 30 30 30, 10 10 10 10, 10 10 10 10, 50 50 250 200 50 50, 50 50 50 50 50 50 % state == 4 YOGA LEFT FOOT - 30 30 30, 10 10 10 10, 10 10 10 10, 30 50 300 60 50 50, 30 50 30 60 50 50 % state == 5 PREPARING FOR SWITCHING - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 6 LOOKING FOR CONTACT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 7 TRANSITION TO INITIAL POSITION - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 8 COM TRANSITION TO RIGHT FOOT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 9 RIGHT FOOT BALANCING - 30 30 30, 10 10 10 10, 10 10 10 10, 50 50 50 50 50 50, 50 50 250 200 50 50 % state == 10 YOGA RIGHT FOOT - 30 30 30, 10 10 10 10, 10 10 10 10, 30 50 30 60 50 50, 30 50 300 60 50 50 % state == 11 PREPARING FOR SWITCHING - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 12 LOOKING FOR CONTACT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50];% state == 13 TRANSITION TO INITIAL POSITION - -Gain.KD_postural = 0*sqrt(Gain.KP_postural(1,:)); - -%% Smoothing times - -% Smoothing time gain scheduling -Config.SmoothingTimeGainScheduling = 2; - -% Smoothing time CoM references -StateMachine.CoMSmoothingTime = [5; %% state == 1 TWO FEET BALANCING - 5; %% state == 2 COM TRANSITION TO LEFT FOOT - 3; %% state == 3 LEFT FOOT BALANCING - 4; %% state == 4 YOGA LEFT FOOT - 5; %% state == 5 PREPARING FOR SWITCHING - 5; %% state == 6 LOOKING FOR CONTACT - 4; %% state == 7 TRANSITION INIT POSITION - 5; %% state == 8 COM TRANSITION TO RIGHT FOOT - 3; %% state == 9 RIGHT FOOT BALANCING - 4; %% state == 10 YOGA RIGHT FOOT - 5; %% state == 11 PREPARING FOR SWITCHING - 5; %% state == 12 LOOKING FOR CONTACT - 4]; %% state == 13 TRANSITION INIT POSITION - -% Smoothing time for joints references -StateMachine.jointsSmoothingTime = [5; %% state == 1 TWO FEET BALANCING - 5; %% state == 2 COM TRANSITION TO LEFT FOOT - 3; %% state == 3 LEFT FOOT BALANCING - 4; %% state == 4 YOGA LEFT FOOT - 5; %% state == 5 PREPARING FOR SWITCHING - 5; %% state == 6 LOOKING FOR CONTACT - 4; %% state == 7 TRANSITION INIT POSITION - 5; %% state == 8 COM TRANSITION TO RIGHT FOOT - 3; %% state == 9 RIGHT FOOT BALANCING - 4; %% state == 10 YOGA RIGHT FOOT - 5; %% state == 11 PREPARING FOR SWITCHING - 5; %% state == 12 LOOKING FOR CONTACT - 4]; %% 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.01, 0.0; %% state == 2 COM TRANSITION TO LEFT FOOT - 0.0, 0.00, 0.0; %% state == 3 LEFT FOOT BALANCING - 0.0, 0.01, 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.01, 0.0; %% state == 8 COM TRANSITION TO RIGHT FOOT - 0.0, 0.00, 0.0; %% state == 9 RIGHT FOOT BALANCING - 0.0, -0.00, 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]; - -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]; - -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)]; - -% 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/initTorqueControlBalancingWithSimu.m b/controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancingWithSimu.m index a3e2104f..9ef27f7a 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancingWithSimu.m +++ b/controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancingWithSimu.m @@ -69,6 +69,12 @@ 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')); From 4e03377f0078d82f6715d2841ae14135642cd238 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Fri, 7 May 2021 17:30:06 +0200 Subject: [PATCH 20/20] Set inout parameter 'input motor reflected inertia format' to 'matrix' - as per https://github.com/dic-iit/matlab-whole-body-simulator/pull/44 changes. --- .../torqueControlBalancingWithSimu.mdl | 154 ++++++++---------- 1 file changed, 69 insertions(+), 85 deletions(-) diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl index 0618f4b5..1b451127 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl +++ b/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl @@ -7,7 +7,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "3.122" + ComputedModelVersion "3.123" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -419,6 +419,17 @@ Model { 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] @@ -426,7 +437,7 @@ Model { SceneRectInView [400.19641548607717, -203.35510808500248, 1554.6227940278457, 850.710216170005] } Object { - $ObjectID 7 + $ObjectID 8 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -437,7 +448,7 @@ Model { SceneRectInView [12.87372526868586, 135.34665821934539, 468.38536196262828, 256.30668356130923] } Object { - $ObjectID 8 + $ObjectID 9 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -448,7 +459,7 @@ Model { SceneRectInView [-158.02631578947376, -70.1842105263158, 571.31578947368428, 312.63157894736844] } Object { - $ObjectID 9 + $ObjectID 10 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -459,7 +470,7 @@ Model { SceneRectInView [-158.02631578947376, -70.1842105263158, 571.31578947368428, 312.63157894736844] } Object { - $ObjectID 10 + $ObjectID 11 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -470,7 +481,7 @@ Model { SceneRectInView [80.154339026122614, 154.55463376836479, 658.838253999605, 360.52503258937384] } Object { - $ObjectID 11 + $ObjectID 12 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -481,7 +492,7 @@ Model { SceneRectInView [569.47947453590621, 441.73077216658527, 412.15823842818759, 225.53845566682949] } Object { - $ObjectID 12 + $ObjectID 13 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -492,7 +503,7 @@ Model { SceneRectInView [-33.73965992647058, -45.831932773109244, 364.8739495798319, 199.66386554621849] } Object { - $ObjectID 13 + $ObjectID 14 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -503,7 +514,7 @@ Model { SceneRectInView [82.52421875, 140.1, 217.1, 118.8] } Object { - $ObjectID 14 + $ObjectID 15 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -514,7 +525,7 @@ Model { SceneRectInView [-608.35758121641425, -114.33481638525888, 1528.7515828078519, 836.55314618872774] } Object { - $ObjectID 15 + $ObjectID 16 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -525,7 +536,7 @@ Model { SceneRectInView [-827.23464439655174, -71.967241379310337, 1390.5629310344827, 760.93448275862067] } Object { - $ObjectID 16 + $ObjectID 17 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -536,7 +547,7 @@ Model { SceneRectInView [256.79612068965514, -277.26034482758621, 660.65775862068972, 361.52068965517242] } Object { - $ObjectID 17 + $ObjectID 18 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -547,7 +558,7 @@ Model { SceneRectInView [-793.97389720558533, -290.08334454895737, 1059.8648857917149, 579.97212543554] } Object { - $ObjectID 18 + $ObjectID 19 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -558,7 +569,7 @@ Model { SceneRectInView [92.0078125, -350.98802908437432, 2171.0, 1188.0] } Object { - $ObjectID 19 + $ObjectID 20 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -569,7 +580,7 @@ Model { SceneRectInView [-112.83168536009441, -373.76948051948051, 901.91337072018882, 493.538961038961] } Object { - $ObjectID 20 + $ObjectID 21 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -580,7 +591,7 @@ Model { SceneRectInView [-48.06941549217828, -144.92583198051949, 845.83414348435656, 462.851663961039] } Object { - $ObjectID 21 + $ObjectID 22 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -591,7 +602,7 @@ Model { SceneRectInView [-706.68143309105676, -478.27262581168833, 1380.7144286821135, 755.54525162337666] } Object { - $ObjectID 22 + $ObjectID 23 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -602,7 +613,7 @@ Model { SceneRectInView [96.146189545676577, -4.1074743527112787, 703.95762090864685, 385.21494870542256] } Object { - $ObjectID 23 + $ObjectID 24 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -613,7 +624,7 @@ Model { SceneRectInView [-211.69612560424537, 252.15114629325387, 605.0, 331.06402579456471] } Object { - $ObjectID 24 + $ObjectID 25 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -624,7 +635,7 @@ Model { SceneRectInView [-410.51953125, -620.5, 2171.0, 1188.0] } Object { - $ObjectID 25 + $ObjectID 26 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -635,7 +646,7 @@ Model { SceneRectInView [-128.63423482247225, -154.52586206896552, 701.83189655172418, 384.05172413793105] } Object { - $ObjectID 26 + $ObjectID 27 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -646,7 +657,7 @@ Model { SceneRectInView [-194.803483319316, -141.48435660218672, 1189.606966638632, 650.96871320437344] } Object { - $ObjectID 27 + $ObjectID 28 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -657,7 +668,7 @@ Model { SceneRectInView [-1178.7732548019947, -284.77258542366184, 1369.7496346039895, 749.54517084732367] } Object { - $ObjectID 28 + $ObjectID 29 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -668,7 +679,7 @@ Model { SceneRectInView [-116.92703117366869, -144.85219724177151, 889.14932180693916, 486.55430414861524] } Object { - $ObjectID 29 + $ObjectID 30 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -679,7 +690,7 @@ Model { SceneRectInView [-736.65534376844744, 2332.9648668093268, 1240.5714285714287, 678.85714285714289] } Object { - $ObjectID 30 + $ObjectID 31 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -690,7 +701,7 @@ Model { SceneRectInView [-33.739659926470551, -42.831932773109244, 364.8739495798319, 199.66386554621849] } Object { - $ObjectID 31 + $ObjectID 32 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -701,7 +712,7 @@ Model { SceneRectInView [-33.739659926470551, -42.831932773109244, 364.8739495798319, 199.66386554621849] } Object { - $ObjectID 32 + $ObjectID 33 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -712,7 +723,7 @@ Model { SceneRectInView [42.960073495100346, -92.586881707886135, 510.17360300979931, 279.17376341577227] } Object { - $ObjectID 33 + $ObjectID 34 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -723,7 +734,7 @@ Model { SceneRectInView [-39.932291666666657, -42.75, 452.29166666666669, 247.5] } Object { - $ObjectID 34 + $ObjectID 35 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -734,7 +745,7 @@ Model { SceneRectInView [-256.01528596010269, -463.59387395006996, 1774.7883844202054, 971.18774790013993] } Object { - $ObjectID 35 + $ObjectID 36 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -745,7 +756,7 @@ Model { SceneRectInView [1438.5531788793103, 818.97931034482758, 778.56551724137933, 426.04137931034484] } Object { - $ObjectID 36 + $ObjectID 37 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -756,7 +767,7 @@ Model { SceneRectInView [917.234213362069, -427.28793103448277, 975.078448275862, 533.57586206896553] } Object { - $ObjectID 37 + $ObjectID 38 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -767,7 +778,7 @@ Model { SceneRectInView [-544.74878771551721, -352.2155172413793, 963.84913793103453, 527.43103448275861] } Object { - $ObjectID 38 + $ObjectID 39 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -778,7 +789,7 @@ Model { SceneRectInView [374.06441393490434, 302.07339302379842, 1297.2149221301913, 709.85321395240317] } Object { - $ObjectID 39 + $ObjectID 40 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -789,7 +800,7 @@ Model { SceneRectInView [732.82082435344819, -198.046551724138, 2023.1474137931036, 1107.093103448276] } Object { - $ObjectID 40 + $ObjectID 41 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -799,17 +810,6 @@ Model { Offset [89.479636723777276, -28.60714285714289] SceneRectInView [89.479636723777276, -28.60714285714289, 1240.5714285714287, 678.85714285714289] } - Object { - $ObjectID 41 - 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 42 IsActive [0] @@ -1323,39 +1323,39 @@ Model { Dimension 3 Object { $ObjectID 88 - Type "GLUE2:SpreadSheet" - ID "ModelData" + Type "GLUE2:PropertyInspector" + ID "Property Inspector" Visible [0] - CreateCallback "DataView.createSpreadSheetComponent" + CreateCallback "" UserData "" Floating [0] - DockPosition "Bottom" - Width [2205] - Height [206] + DockPosition "Right" + Width [640] + Height [480] Minimized "Unset" } Object { $ObjectID 89 - Type "Simulink:Editor:ReferencedFiles" - ID "Referenced Files" + Type "GLUE2:SpreadSheet" + ID "ModelData" Visible [0] - CreateCallback "" - UserData "{\"filterShowRefModels\":\"true\",\"filterShowRefSubs\":\"true\",\"filterShowOnlyDirtyFiles\":\"false\"}\n" + CreateCallback "DataView.createSpreadSheetComponent" + UserData "" Floating [0] - DockPosition "Left" + DockPosition "Bottom" Width [640] Height [480] Minimized "Unset" } Object { $ObjectID 90 - Type "GLUE2:PropertyInspector" - ID "Property Inspector" + Type "Simulink:Editor:ReferencedFiles" + ID "Referenced Files" Visible [0] CreateCallback "" - UserData "" + UserData "{\"filterShowRefModels\":\"true\",\"filterShowRefSubs\":\"true\",\"filterShowOnlyDirtyFiles\":\"false\"}\n" Floating [0] - DockPosition "Right" + DockPosition "Left" Width [640] Height [480] Minimized "Unset" @@ -1366,10 +1366,10 @@ Model { "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQBAAAAeQAABOEAAABrAP////sAAABgAFMAaQBtAHUAb" "ABpAG4AawA6AEUAZABpAHQAbwByADoAUgBlAGYAZQByAGUAbgBjAGUAZABGAGkAbABlAHMALwBSAGUAZgBlAHIAZQBuAGMAZQBkACAARgBpAGwAZ" - "QBzAAAAAAD/////AAAAjwD///8AAAABAAABYAAABOH8AgAAAAH8AAAAeQAABOEAAAAAAP////r/////AgAAAAL7AAAAVABHAEwAVQBFADIAOgBQA" + "QBzAAAAAAD/////AAAAjwD///8AAAABAAABYAAABOH8AgAAAAH8AAAAeQAABOEAAAAAAP////r/////AgAAAAH7AAAAVABHAEwAVQBFADIAOgBQA" "HIAbwBwAGUAcgB0AHkASQBuAHMAcABlAGMAdABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA/" - "///+/////8AAAAAAP////8AAABpAP///wAAAAMAAAihAAAA6vwBAAAAAfsAAAA2AEcATABVAEUAMgA6AFMAcAByAGUAYQBkAFMAaABlAGUAdAAvA" - "E0AbwBkAGUAbABEAGEAdABhAAAAAV8AAAihAAABSAD///8AAAihAAAE4QAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAA" + "///AAAAAwAACKEAAADq/AEAAAAB+wAAADYARwBMAFUARQAyADoAUwBwAHIAZQBhAGQAUwBoAGUAZQB0AC8ATQBvAGQAZQBsAEQAYQB0AGEAAAABX" + "wAACKEAAAE6AP///wAACKEAAAThAAAAAQAAAAIAAAABAAAAAvwAAAABAAAAAgAAAAA=" Array { Type "Cell" Dimension 0 @@ -1388,9 +1388,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "nunoguedelha" ModifiedDateFormat "%" - LastModifiedDate "Thu May 06 15:11:05 2021" - RTWModifiedTimeStamp 542214505 - ModelVersionFormat "%" + LastModifiedDate "Fri May 07 17:27:52 2021" + RTWModifiedTimeStamp 542309261 + ModelVersionFormat "%" SampleTimeColors on SampleTimeAnnotations on LibraryLinkDisplay "disabled" @@ -2770,12 +2770,6 @@ Model { ContentPreviewEnabled off ConfigSource "Workspace" ConfigObject "'WBTConfigRobot'" - RobotName "'icubSim'" - UrdfFile "'model.urdf'" - ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" - ControlBoardsNames "{'left_arm','right_arm','torso'}" - LocalName "'WBT'" - GravityVector "[0,0,-9.81]" } Block { BlockType Constant @@ -4503,7 +4497,6 @@ Model { ZOrder 38 SrcBlock "A" SrcPort 1 - Points [65, 0] DstBlock "QP Two Feet" DstPort 3 } @@ -4549,7 +4542,6 @@ Model { } Branch { ZOrder 79 - Points [43, 0] DstBlock "QP Two Feet" DstPort 1 } @@ -4576,7 +4568,6 @@ Model { } Branch { ZOrder 46 - Points [18, 0] DstBlock "QP Two Feet" DstPort 2 } @@ -4585,7 +4576,6 @@ Model { ZOrder 93 SrcBlock "ubA" SrcPort 1 - Points [65, 0] DstBlock "QP Two Feet" DstPort 5 } @@ -4593,7 +4583,6 @@ Model { ZOrder 94 SrcBlock "Constant" SrcPort 1 - Points [30, 0] DstBlock "QP Two Feet" DstPort 4 } @@ -19271,12 +19260,6 @@ Model { ContentPreviewEnabled on ConfigSource "Workspace" ConfigObject "'WBTConfigRobotSim'" - RobotName "'icubSim'" - UrdfFile "'model.urdf'" - ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" - ControlBoardsNames "{'left_arm','right_arm','torso'}" - LocalName "'WBT'" - GravityVector "[0,0,-9.81]" } Block { BlockType Constant @@ -19463,7 +19446,7 @@ Model { ZOrder 1231 ForegroundColor "red" BackgroundColor "yellow" - LibraryVersion "3.3" + LibraryVersion "3.20" SourceBlock "mwbs_robotDynamicsWithContacts_lib/RobotDynWithContacts" SourceType "" SourceProductName "Matlab Whole-body Simulator" @@ -19476,6 +19459,7 @@ Model { robot_config "robot_config" contact_config "contact_config" physics_config "physics_config" + motorReflectedInertiaFormat "matrix" } Block { BlockType Outport