From 8dd494b1978ded770ff411fd7db70c6871760ebb Mon Sep 17 00:00:00 2001 From: Gabriele Nava Date: Mon, 23 Mar 2020 18:05:10 +0100 Subject: [PATCH] [WIP] WBC release 2.5 (#77) * added data processing utility * documentation of the repo updated * Update README.md * Update README.md * cleanup of documentation, cmake and models * added Simulink balancing simulator * added home positions for the yarpmotorgui * updated installation path of homePos * Revert "updated installation path of homePos" This reverts commit 55928fb5def6ccf82a81d6354adaac3ae9948164. * fixed warning in CMakeLists.txt * added goToWholeBodyControllers.m generation * updated documentation for goToWholeBodyControllers.m * updated GUI (#82) and print warning when joints have spikes or exit range (#81) * added pictures of the simulink models * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update How-to-run-controllers-with-gazebo-simulator.md * Update How-to-run-controllers-with-gazebo-simulator.md * Update How-to-run-controllers-with-gazebo-simulator.md * Update How-to-setup-iCub-for-wbc-experiments.md * Update autogeneration (#92) * Add driver.cpp.in template * Update clang-format * Update main CMakeLists.txt to use the driver.cpp.in template * Minor edit in RegisteMdl.cmake * Update CreateAutogeneratedCodeTargets.cmake.in template * fix block names for autogen * fixed also the model without Gazebo for autogen * modified installation path of +wbc library * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * models exported to 2017b Co-authored-by: Diego Ferigo Co-authored-by: Diego Ferigo --- .clang-format | 6 +- .gitignore | 3 +- CMakeLists.txt | 36 +- README.md | 76 +- cmake/CreateAutogeneratedCodeTargets.cmake.in | 29 +- cmake/RegisterMdl.cmake | 4 + config/CMakeLists.txt | 1 + config/README.md | 4 +- config/createGoToWBC.m | 76 + config/export_WBC.m | 18 +- config/{startup_WBC.m => startup_WBC.m.in} | 24 +- controllers/README.md | 9 +- controllers/data-processing/README.md | 9 + .../data-processing/processDataFromSimulink.m | 130 + .../data-processing/src_plot/custom_plot.m | 28 + .../README.md | 10 +- .../iCubGazeboV2_5/configJointsControl.m | 0 .../app/robots/iCubGazeboV2_5/configRobot.m | 0 .../iCubGazeboV2_5/gainsAndReferences.m | 0 .../robots/iCubGenova02/configJointsControl.m | 0 .../app/robots/iCubGenova02/configRobot.m | 0 .../robots/iCubGenova02/gainsAndReferences.m | 0 .../robots/iCubGenova04/configJointsControl.m | 0 .../app/robots/iCubGenova04/configRobot.m | 0 .../robots/iCubGenova04/gainsAndReferences.m | 0 .../icubGazeboSim/configJointsControl.m | 0 .../app/robots/icubGazeboSim/configRobot.m | 0 .../robots/icubGazeboSim/gainsAndReferences.m | 0 .../initJointsControl.m | 0 .../jointsControl.mdl | 619 +- .../src-static-gui/closeModel.m | 0 .../src-static-gui/compileModel.m | 0 .../startModelWithStaticGui.m | 0 .../stopJointsControl.m | 35 +- .../README.md | 8 +- .../positionControlBalancing.mdl | 1305 +- .../stopPositionControlBalancing.m | 35 +- .../README.md | 8 +- .../stopTorqueControlBalancing.m | 35 +- .../torqueControlBalancing.mdl | 2282 +- .../simulink-balancing-simulator/README.md | 35 + .../app/robots/iCubGazeboV2_5/configRobot.m | 108 + .../iCubGazeboV2_5/configStateMachine.m | 160 + .../iCubGazeboV2_5/gainsAndReferences.m | 347 + .../initTorqueControlBalancingSim.m | 83 + .../src-static-gui/closeModel.m | 20 + .../src-static-gui/compileModel.m | 24 + .../src/computeBasePoseDerivative.m | 20 + .../src/estimateContactForcesFromDynamics.m | 36 + .../src/forwardDynamics.m | 10 + .../src/momentumBasedController.m | 328 + .../src/processOutputQP_oneFoot.m | 57 + .../src/processOutputQP_twoFeet.m | 41 + .../src/referenceGeneratorCoM.m | 18 + .../src/stateMachineMomentumControl.m | 393 + .../startModelWithStaticGui.m | 29 + .../stopTorqueControlBalancingSim.m | 93 + .../torqueControlBalancingSim.mdl | 26374 ++++++++++++++++ doc/How-to-run-controllers-on-real-iCub.md | 19 + ...-run-controllers-with-gazebo-simulator.md} | 17 +- ...How-to-run-torqueBalancing-on-real-iCub.md | 19 - ... How-to-setup-iCub-for-wbc-experiments.md} | 9 +- doc/README.md | 6 +- doc/pics/jointControl.png | Bin 0 -> 87086 bytes doc/pics/positionControl.png | Bin 0 -> 93175 bytes doc/pics/torqueControl.png | Bin 0 -> 95337 bytes doc/pics/torqueControlSim.png | Bin 0 -> 86247 bytes library/CMakeLists.txt | 1 + library/README.md | 6 +- library/matlab-gui/simulinkStaticGUI.fig | Bin 8388 -> 13394 bytes library/matlab-gui/simulinkStaticGUI.m | 20 +- library/matlab-wbc/+wbc/checkInputRange.m | 33 + library/matlab-wbc/+wbc/checkInputSpikes.m | 47 + library/matlab-wbc/+wbc/checkRange.m | 3 + library/matlab-wbc/+wbc/checkSpikes.m | 3 + .../simulink-library/CMM_iCub_23_25_DoFs.mdl | 587 +- src/driver.cpp.in | 57 + utilities/CMakeLists.txt | 1 + utilities/README.md | 6 +- utilities/debug_BoschIMU.mdl | 43 +- utilities/debug_FTExternalForces.mdl | 199 +- utilities/debug_FTMeas.mdl | 86 +- utilities/debug_FTMeas_shoes.mdl | 76 +- utilities/debug_seesawIMU.mdl | 151 +- utilities/debug_xSensIMU.mdl | 79 +- utilities/homePositions/CMakeLists.txt | 1 + utilities/homePositions/README.md | 3 + utilities/homePositions/robots/CMakeLists.txt | 27 + .../robots/bigman/CMakeLists.txt | 5 + .../robots/bigman/homePoseBalancing.ini | 19 + .../robots/bigman/homePoseCalib.ini | 34 + .../robots/bigman/homePoseYogaPP.ini | 100 + .../robots/bigman/torqueBalancing.ini | 19 + .../robots/bigman_only_legs/CMakeLists.txt | 5 + .../bigman_only_legs/homePoseBalancing.ini | 11 + .../bigman_only_legs/homePoseYogaPP.ini | 80 + .../bigman_only_legs/torqueBalancing.ini | 19 + .../robots/iCubDarmstadt01/CMakeLists.txt | 5 + .../homePoseBalancingHeadTorsoLegsZero.ini | 37 + .../homePoseBalancingLeftFoot.ini | 99 + .../homePoseBalancingLegsZero.ini | 37 + .../homePoseBalancingTorsoHeadZero.ini | 37 + .../homePoseBalancingTwoFeet.ini | 37 + .../homePoseFineCalibration.ini | 37 + .../homePoseWholeBodyImpedance.ini | 37 + .../robots/iCubDarmstadt01/homePoseYogaPP.ini | 37 + .../iCubDarmstadt01/oneFootBalancingGains.ini | 2 + .../torqueBalancingLeftFoot.ini | 22 + .../torqueBalancingTwoFeet.ini | 21 + .../iCubDarmstadt01/twoFeetBalancingGains.ini | 8 + .../robots/iCubGazeboV2_5/CMakeLists.txt | 5 + .../iCubGazeboV2_5/homePoseBalancing.ini | 74 + .../robots/iCubGazeboV2_5_plus/CMakeLists.txt | 5 + .../iCubGazeboV2_5_plus/homePoseBalancing.ini | 88 + .../robots/iCubGenova01/CMakeLists.txt | 5 + .../homePoseBalancingLeftFoot.ini | 99 + .../iCubGenova01/homePoseBalancingRedBall.ini | 99 + .../iCubGenova01/homePoseBalancingTwoFeet.ini | 37 + .../iCubGenova01/homePoseFineCalibration.ini | 37 + .../robots/iCubGenova01/homePoseWalking.ini | 37 + .../iCubGenova01/torqueBalancingLeftFoot.ini | 19 + .../torqueBalancingLeftFootYoga.ini | 20 + .../iCubGenova01/torqueBalancingTwoFeet.ini | 27 + .../robots/iCubGenova02/CMakeLists.txt | 5 + .../robots/iCubGenova02/homePoseBalancing.ini | 46 + .../iCubGenova02/homePoseBalancingChair.ini | 37 + .../homePoseBalancingLegsZero.ini | 37 + .../iCubGenova02/homePoseBalancingRedBall.ini | 99 + .../iCubGenova02/homePoseBalancingTwoFeet.ini | 37 + .../iCubGenova02/homePoseFineCalibration.ini | 37 + .../robots/iCubGenova02/homePoseYogaPP.ini | 35 + .../iCubGenova02/torqueBalancingLeftFoot.ini | 19 + .../iCubGenova02/torqueBalancingTwoFeet.ini | 21 + .../iCubGenova02/twoFeetBalancingGains.ini | 2 + .../robots/iCubGenova03/CMakeLists.txt | 5 + .../homePoseBalancingLeftFoot.ini | 99 + .../iCubGenova03/homePoseBalancingRedBall.ini | 99 + .../iCubGenova03/homePoseBalancingTwoFeet.ini | 99 + .../iCubGenova03/homePoseFineCalibration.ini | 37 + .../robots/iCubGenova03/torqueBalancing.ini | 27 + .../robots/iCubGenova04/CMakeLists.txt | 5 + .../robots/iCubGenova04/homePoseBalancing.ini | 104 + .../iCubGenova04/homePoseBalancingChair.ini | 37 + .../homePoseBalancingLeftFoot.ini | 99 + .../homePoseBalancingLegsZero.ini | 37 + .../iCubGenova04/homePoseBalancingRedBall.ini | 99 + .../iCubGenova04/homePoseBalancingTwoFeet.ini | 37 + .../iCubGenova04/homePoseFineCalibration.ini | 37 + .../robots/iCubGenova04/homePoseYogaPP.ini | 35 + .../iCubGenova04/torqueBalancingLeftFoot.ini | 19 + .../iCubGenova04/torqueBalancingTwoFeet.ini | 21 + .../iCubGenova04/twoFeetBalancingGains.ini | 2 + .../robots/iCubGenova05/CMakeLists.txt | 5 + .../homePoseBalancingLeftFoot.ini | 99 + .../homePoseBalancingLegsZero.ini | 37 + .../homePoseBalancingTorsoHeadZero.ini | 37 + .../iCubGenova05/homePoseBalancingTwoFeet.ini | 37 + .../iCubGenova05/homePoseFineCalibration.ini | 37 + .../homePoseWholeBodyImpedance.ini | 37 + .../robots/iCubGenova05/homePoseYogaPP.ini | 37 + .../iCubGenova05/oneFootBalancingGains.ini | 2 + .../iCubGenova05/torqueBalancingLeftFoot.ini | 22 + .../iCubGenova05/torqueBalancingTwoFeet.ini | 21 + .../iCubGenova05/twoFeetBalancingGains.ini | 8 + .../robots/iCubHeidelberg01/CMakeLists.txt | 5 + .../homePoseBalancingLeftFoot.ini | 85 + .../homePoseBalancingRightFoot.ini | 85 + .../homePoseBalancingTwoFeet.ini | 86 + .../iCubHeidelberg01/torqueBalancing.ini | 20 + .../robots/iCubNancy01/CMakeLists.txt | 5 + .../robots/iCubNancy01/homePoseBalancing.ini | 46 + .../iCubNancy01/torqueBalancingLeftFoot.ini | 19 + .../iCubNancy01/torqueBalancingTwoFeet.ini | 21 + .../iCubNancy01/twoFeetBalancingGains.ini | 2 + .../robots/iCubParis01/CMakeLists.txt | 5 + .../iCubParis01/homePoseBalancingLeftFoot.ini | 99 + .../iCubParis01/homePoseBalancingRedBall.ini | 99 + .../homePoseBalancingRightFoot.ini | 99 + .../iCubParis01/homePoseBalancingTwoFeet.ini | 99 + .../iCubParis01/homePoseFineCalibration.ini | 37 + .../robots/iCubParis01/torqueBalancing.ini | 19 + .../robots/iCubParis02/CMakeLists.txt | 5 + .../iCubParis02/homePoseBalancingLeftFoot.ini | 98 + .../iCubParis02/homePoseBalancingRedBall.ini | 98 + .../iCubParis02/homePoseBalancingTwoFeet.ini | 99 + .../iCubParis02/homePoseFineCalibration.ini | 37 + .../homePoseWholeBodyImpedance.ini | 37 + .../robots/iCubParis02/torqueBalancing.ini | 27 + .../robots/icubGazeboSim/CMakeLists.txt | 5 + .../icubGazeboSim/homePoseBalancing.ini | 74 + .../icubGazeboSim/homePoseBalancingChair.ini | 36 + .../homePoseBalancingLegsZero.ini | 37 + .../homePoseBalancingRedBall.ini | 99 + .../homePoseBalancingTwoFeet.ini | 99 + .../icubGazeboSim/homePoseFineCalibration.ini | 37 + .../robots/icubGazeboSim/homePoseYogaPP.ini | 36 + .../robots/icubGazeboSim/torqueBalancing.ini | 19 + .../icubGazeboSim/torqueBalancingLeftFoot.ini | 19 + 198 files changed, 36988 insertions(+), 2082 deletions(-) create mode 100644 config/CMakeLists.txt create mode 100644 config/createGoToWBC.m rename config/{startup_WBC.m => startup_WBC.m.in} (69%) create mode 100644 controllers/data-processing/README.md create mode 100644 controllers/data-processing/processDataFromSimulink.m create mode 100644 controllers/data-processing/src_plot/custom_plot.m rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/README.md (75%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/app/robots/iCubGazeboV2_5/configJointsControl.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/app/robots/iCubGazeboV2_5/configRobot.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/app/robots/iCubGazeboV2_5/gainsAndReferences.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/app/robots/iCubGenova02/configJointsControl.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/app/robots/iCubGenova02/configRobot.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/app/robots/iCubGenova02/gainsAndReferences.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/app/robots/iCubGenova04/configJointsControl.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/app/robots/iCubGenova04/configRobot.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/app/robots/iCubGenova04/gainsAndReferences.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/app/robots/icubGazeboSim/configJointsControl.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/app/robots/icubGazeboSim/configRobot.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/app/robots/icubGazeboSim/gainsAndReferences.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/initJointsControl.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/jointsControl.mdl (94%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/src-static-gui/closeModel.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/src-static-gui/compileModel.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/startModelWithStaticGui.m (100%) rename controllers/{fixed-base-joints-control => fixed-base-joints-torque-control}/stopJointsControl.m (66%) create mode 100644 controllers/simulink-balancing-simulator/README.md create mode 100644 controllers/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/configRobot.m create mode 100644 controllers/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m create mode 100644 controllers/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m create mode 100644 controllers/simulink-balancing-simulator/initTorqueControlBalancingSim.m create mode 100644 controllers/simulink-balancing-simulator/src-static-gui/closeModel.m create mode 100644 controllers/simulink-balancing-simulator/src-static-gui/compileModel.m create mode 100644 controllers/simulink-balancing-simulator/src/computeBasePoseDerivative.m create mode 100644 controllers/simulink-balancing-simulator/src/estimateContactForcesFromDynamics.m create mode 100644 controllers/simulink-balancing-simulator/src/forwardDynamics.m create mode 100644 controllers/simulink-balancing-simulator/src/momentumBasedController.m create mode 100644 controllers/simulink-balancing-simulator/src/processOutputQP_oneFoot.m create mode 100644 controllers/simulink-balancing-simulator/src/processOutputQP_twoFeet.m create mode 100644 controllers/simulink-balancing-simulator/src/referenceGeneratorCoM.m create mode 100644 controllers/simulink-balancing-simulator/src/stateMachineMomentumControl.m create mode 100644 controllers/simulink-balancing-simulator/startModelWithStaticGui.m create mode 100644 controllers/simulink-balancing-simulator/stopTorqueControlBalancingSim.m create mode 100644 controllers/simulink-balancing-simulator/torqueControlBalancingSim.mdl create mode 100644 doc/How-to-run-controllers-on-real-iCub.md rename doc/{How-to-run-torqueBalancing-simulations-with-iCub.md => How-to-run-controllers-with-gazebo-simulator.md} (56%) delete mode 100644 doc/How-to-run-torqueBalancing-on-real-iCub.md rename doc/{How-to-setup-the-robot-for-wbc-experiments.md => How-to-setup-iCub-for-wbc-experiments.md} (97%) create mode 100644 doc/pics/jointControl.png create mode 100644 doc/pics/positionControl.png create mode 100644 doc/pics/torqueControl.png create mode 100644 doc/pics/torqueControlSim.png create mode 100644 library/CMakeLists.txt create mode 100644 library/matlab-wbc/+wbc/checkInputRange.m create mode 100644 library/matlab-wbc/+wbc/checkInputSpikes.m create mode 100644 src/driver.cpp.in create mode 100644 utilities/CMakeLists.txt create mode 100644 utilities/homePositions/CMakeLists.txt create mode 100644 utilities/homePositions/README.md create mode 100644 utilities/homePositions/robots/CMakeLists.txt create mode 100644 utilities/homePositions/robots/bigman/CMakeLists.txt create mode 100644 utilities/homePositions/robots/bigman/homePoseBalancing.ini create mode 100644 utilities/homePositions/robots/bigman/homePoseCalib.ini create mode 100644 utilities/homePositions/robots/bigman/homePoseYogaPP.ini create mode 100644 utilities/homePositions/robots/bigman/torqueBalancing.ini create mode 100644 utilities/homePositions/robots/bigman_only_legs/CMakeLists.txt create mode 100644 utilities/homePositions/robots/bigman_only_legs/homePoseBalancing.ini create mode 100644 utilities/homePositions/robots/bigman_only_legs/homePoseYogaPP.ini create mode 100644 utilities/homePositions/robots/bigman_only_legs/torqueBalancing.ini create mode 100644 utilities/homePositions/robots/iCubDarmstadt01/CMakeLists.txt create mode 100644 utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingHeadTorsoLegsZero.ini create mode 100644 utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingLegsZero.ini create mode 100644 utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingTorsoHeadZero.ini create mode 100644 utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubDarmstadt01/homePoseFineCalibration.ini create mode 100644 utilities/homePositions/robots/iCubDarmstadt01/homePoseWholeBodyImpedance.ini create mode 100644 utilities/homePositions/robots/iCubDarmstadt01/homePoseYogaPP.ini create mode 100644 utilities/homePositions/robots/iCubDarmstadt01/oneFootBalancingGains.ini create mode 100644 utilities/homePositions/robots/iCubDarmstadt01/torqueBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubDarmstadt01/torqueBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubDarmstadt01/twoFeetBalancingGains.ini create mode 100644 utilities/homePositions/robots/iCubGazeboV2_5/CMakeLists.txt create mode 100644 utilities/homePositions/robots/iCubGazeboV2_5/homePoseBalancing.ini create mode 100644 utilities/homePositions/robots/iCubGazeboV2_5_plus/CMakeLists.txt create mode 100644 utilities/homePositions/robots/iCubGazeboV2_5_plus/homePoseBalancing.ini create mode 100644 utilities/homePositions/robots/iCubGenova01/CMakeLists.txt create mode 100644 utilities/homePositions/robots/iCubGenova01/homePoseBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubGenova01/homePoseBalancingRedBall.ini create mode 100644 utilities/homePositions/robots/iCubGenova01/homePoseBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubGenova01/homePoseFineCalibration.ini create mode 100644 utilities/homePositions/robots/iCubGenova01/homePoseWalking.ini create mode 100644 utilities/homePositions/robots/iCubGenova01/torqueBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubGenova01/torqueBalancingLeftFootYoga.ini create mode 100644 utilities/homePositions/robots/iCubGenova01/torqueBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubGenova02/CMakeLists.txt create mode 100644 utilities/homePositions/robots/iCubGenova02/homePoseBalancing.ini create mode 100644 utilities/homePositions/robots/iCubGenova02/homePoseBalancingChair.ini create mode 100644 utilities/homePositions/robots/iCubGenova02/homePoseBalancingLegsZero.ini create mode 100644 utilities/homePositions/robots/iCubGenova02/homePoseBalancingRedBall.ini create mode 100644 utilities/homePositions/robots/iCubGenova02/homePoseBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubGenova02/homePoseFineCalibration.ini create mode 100644 utilities/homePositions/robots/iCubGenova02/homePoseYogaPP.ini create mode 100644 utilities/homePositions/robots/iCubGenova02/torqueBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubGenova02/torqueBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubGenova02/twoFeetBalancingGains.ini create mode 100644 utilities/homePositions/robots/iCubGenova03/CMakeLists.txt create mode 100644 utilities/homePositions/robots/iCubGenova03/homePoseBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubGenova03/homePoseBalancingRedBall.ini create mode 100644 utilities/homePositions/robots/iCubGenova03/homePoseBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubGenova03/homePoseFineCalibration.ini create mode 100644 utilities/homePositions/robots/iCubGenova03/torqueBalancing.ini create mode 100644 utilities/homePositions/robots/iCubGenova04/CMakeLists.txt create mode 100644 utilities/homePositions/robots/iCubGenova04/homePoseBalancing.ini create mode 100644 utilities/homePositions/robots/iCubGenova04/homePoseBalancingChair.ini create mode 100644 utilities/homePositions/robots/iCubGenova04/homePoseBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubGenova04/homePoseBalancingLegsZero.ini create mode 100644 utilities/homePositions/robots/iCubGenova04/homePoseBalancingRedBall.ini create mode 100644 utilities/homePositions/robots/iCubGenova04/homePoseBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubGenova04/homePoseFineCalibration.ini create mode 100644 utilities/homePositions/robots/iCubGenova04/homePoseYogaPP.ini create mode 100644 utilities/homePositions/robots/iCubGenova04/torqueBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubGenova04/torqueBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubGenova04/twoFeetBalancingGains.ini create mode 100644 utilities/homePositions/robots/iCubGenova05/CMakeLists.txt create mode 100644 utilities/homePositions/robots/iCubGenova05/homePoseBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubGenova05/homePoseBalancingLegsZero.ini create mode 100644 utilities/homePositions/robots/iCubGenova05/homePoseBalancingTorsoHeadZero.ini create mode 100644 utilities/homePositions/robots/iCubGenova05/homePoseBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubGenova05/homePoseFineCalibration.ini create mode 100644 utilities/homePositions/robots/iCubGenova05/homePoseWholeBodyImpedance.ini create mode 100644 utilities/homePositions/robots/iCubGenova05/homePoseYogaPP.ini create mode 100644 utilities/homePositions/robots/iCubGenova05/oneFootBalancingGains.ini create mode 100644 utilities/homePositions/robots/iCubGenova05/torqueBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubGenova05/torqueBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubGenova05/twoFeetBalancingGains.ini create mode 100644 utilities/homePositions/robots/iCubHeidelberg01/CMakeLists.txt create mode 100644 utilities/homePositions/robots/iCubHeidelberg01/homePoseBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubHeidelberg01/homePoseBalancingRightFoot.ini create mode 100644 utilities/homePositions/robots/iCubHeidelberg01/homePoseBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubHeidelberg01/torqueBalancing.ini create mode 100644 utilities/homePositions/robots/iCubNancy01/CMakeLists.txt create mode 100644 utilities/homePositions/robots/iCubNancy01/homePoseBalancing.ini create mode 100644 utilities/homePositions/robots/iCubNancy01/torqueBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubNancy01/torqueBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubNancy01/twoFeetBalancingGains.ini create mode 100644 utilities/homePositions/robots/iCubParis01/CMakeLists.txt create mode 100644 utilities/homePositions/robots/iCubParis01/homePoseBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubParis01/homePoseBalancingRedBall.ini create mode 100644 utilities/homePositions/robots/iCubParis01/homePoseBalancingRightFoot.ini create mode 100644 utilities/homePositions/robots/iCubParis01/homePoseBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubParis01/homePoseFineCalibration.ini create mode 100644 utilities/homePositions/robots/iCubParis01/torqueBalancing.ini create mode 100644 utilities/homePositions/robots/iCubParis02/CMakeLists.txt create mode 100644 utilities/homePositions/robots/iCubParis02/homePoseBalancingLeftFoot.ini create mode 100644 utilities/homePositions/robots/iCubParis02/homePoseBalancingRedBall.ini create mode 100644 utilities/homePositions/robots/iCubParis02/homePoseBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/iCubParis02/homePoseFineCalibration.ini create mode 100644 utilities/homePositions/robots/iCubParis02/homePoseWholeBodyImpedance.ini create mode 100644 utilities/homePositions/robots/iCubParis02/torqueBalancing.ini create mode 100644 utilities/homePositions/robots/icubGazeboSim/CMakeLists.txt create mode 100644 utilities/homePositions/robots/icubGazeboSim/homePoseBalancing.ini create mode 100644 utilities/homePositions/robots/icubGazeboSim/homePoseBalancingChair.ini create mode 100644 utilities/homePositions/robots/icubGazeboSim/homePoseBalancingLegsZero.ini create mode 100644 utilities/homePositions/robots/icubGazeboSim/homePoseBalancingRedBall.ini create mode 100644 utilities/homePositions/robots/icubGazeboSim/homePoseBalancingTwoFeet.ini create mode 100644 utilities/homePositions/robots/icubGazeboSim/homePoseFineCalibration.ini create mode 100644 utilities/homePositions/robots/icubGazeboSim/homePoseYogaPP.ini create mode 100644 utilities/homePositions/robots/icubGazeboSim/torqueBalancing.ini create mode 100644 utilities/homePositions/robots/icubGazeboSim/torqueBalancingLeftFoot.ini diff --git a/.clang-format b/.clang-format index 8723d00..c453b01 100644 --- a/.clang-format +++ b/.clang-format @@ -64,6 +64,10 @@ IncludeCategories: Priority: 2 - Regex: '^(<|"(gtest|gmock|isl|json)/)' Priority: 3 + - Regex: '^"rtwtypes' + Priority: -1 + - Regex: '^"tmwtypes' + Priority: -1 - Regex: '.*' Priority: 1 IncludeIsMainRegex: '(Test)?$' @@ -89,7 +93,7 @@ PenaltyExcessCharacter: 1000000 PenaltyReturnTypeOnItsOwnLine: 60 PointerAlignment: Left ReflowComments: true -SortIncludes: false +SortIncludes: true SortUsingDeclarations: true SpaceAfterCStyleCast: true SpaceAfterTemplateKeyword: true diff --git a/.gitignore b/.gitignore index 8aea83d..b999334 100644 --- a/.gitignore +++ b/.gitignore @@ -31,7 +31,8 @@ slprj/ *.mat # Matlab figures -*.fig +*.fig +!/library/matlab-gui/simulinkStaticGUI.fig # Simulink autosave *.autosave diff --git a/CMakeLists.txt b/CMakeLists.txt index b6fdf04..ca82fdf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,10 +5,26 @@ cmake_minimum_required(VERSION 3.5) project(whole-body-controllers) option(WBC_EXPORT_AUTOGENERATED "Enable the target to export code generated with Simulink Coder" OFF) +option(WBC_INSTALL_ALL_HOME_POS "Installation of all available home positions" ON) + +# ====================================== +# Install home positions and WBC library +# ====================================== + +find_package(YARP REQUIRED) +yarp_configure_external_installation(wbc) + +add_subdirectory(utilities) +add_subdirectory(library) +add_subdirectory(config) + +# ====================== +# Autogeneration routine +# ====================== if(NOT WBC_EXPORT_AUTOGENERATED) # Fake installation. - install(CODE "MESSAGE(\"This repository simply acts as a container\")") + install(CODE "MESSAGE(\"Disabled exporting of autogenerated c++ code from Simulink.\")") return() endif() @@ -37,8 +53,7 @@ file(WRITE "${CMAKE_BINARY_DIR}/autogenerated/CMakeLists.txt" "# Exported contro # Find clang-format find_program(ClangFormat_EXECUTABLE DOC "Path to the clang-format executable." - NAMES clang-format clang-format-7.0 clang-format-6.0 clang-format-5.0 clang-format-4.0 - ) + NAMES clang-format clang-format-9 clang-format-8 clang-format-7 clang-format-6.0 clang-format-5.0 clang-format-4.0) configure_file(${CMAKE_SOURCE_DIR}/.clang-format ${CMAKE_BINARY_DIR}/.clang-format COPYONLY) # Copy only h and cpp files @@ -48,17 +63,21 @@ foreach(SOURCEDIR ${AUTOGENERATED_FOLDERS}) file(APPEND "${CMAKE_BINARY_DIR}/autogenerated/CMakeLists.txt" "add_subdirectory(${MDLDIRNAME})\n") file(MAKE_DIRECTORY "${MDLDESTDIR}") string(REGEX MATCH "[^_]*" MDL_NAME ${MDLDIRNAME}) # Remove _gtw_rtw + # Create the CMakeLists.txt of the model configure_file( "${CMAKE_SOURCE_DIR}/cmake/CreateAutogeneratedCodeTargets.cmake.in" "${MDLDESTDIR}/CMakeLists.txt" - @ONLY - ) + @ONLY) + # Copy the main file + configure_file( + "${PROJECT_SOURCE_DIR}/src/driver.cpp.in" + "${MDLDESTDIR}/driver.cpp" + @ONLY) # Copy sources to the build directory add_custom_command(TARGET copy-autogenerated-models PRE_BUILD COMMAND ${CMAKE_COMMAND} -E copy "${SOURCEDIR}/*.h" "${SOURCEDIR}/*.cpp" "${SOURCEDIR}/defines.txt" "${MDLDESTDIR}" - COMMENT "Copying ${MDLDIRNAME} to the build folder." - ) + COMMENT "Copying ${MDLDIRNAME} to the build folder.") # Apply clang-format style if(NOT ${ClangFormat_EXECUTABLE} STREQUAL "ClangFormat_EXECUTABLE-NOTFOUND") add_custom_command(TARGET copy-autogenerated-models @@ -72,5 +91,4 @@ endforeach() add_custom_command(TARGET copy-autogenerated-models POST_BUILD COMMAND ${CMAKE_COMMAND} -E copy_directory "${CMAKE_BINARY_DIR}/autogenerated" "${AUTOGENERATED_WBC_SOURCE_DIR}/autogenerated" - COMMENT "Copying generated files to ${AUTOGENERATED_WBC_SOURCE_DIR}/autogenerated" - ) \ No newline at end of file + COMMENT "Copying generated files to ${AUTOGENERATED_WBC_SOURCE_DIR}/autogenerated") diff --git a/README.md b/README.md index 0237fa5..0dd7d1a 100644 --- a/README.md +++ b/README.md @@ -1,31 +1,50 @@ # whole-body-controllers -The repository contains Simulink-based whole-body controllers for humanoid robots, and configuration and utility Matlab functions to perform balancing simulations with Gazebo simulator and on the Yarp-based robot [iCub](http://www.icub.org/). - -**Warning! This repository is under active development. In future releases, `master` branch may break compatibility with older versions of WBC. If you are interested in retrieving a `stable` version of this repo, `fork the repository` or refer to the following releases:** +**Warning! This repository contains reseach material and therefore is under active development. In future releases, `master` branch may break compatibility with older versions of WBC. If you are interested in retrieving a `stable` version of this repo, `fork the repository` or refer to the following releases:** - [WBC v2.0](https://github.com/robotology/whole-body-controllers/releases/tag/v2.0) - [WBC v1.5](https://github.com/robotology/whole-body-controllers/releases/tag/v1.5) - [WBC v1.0](https://github.com/robotology/whole-body-controllers/releases/tag/v1.0) +## Overview + +The repository contains `Simulink-based whole-body controllers` developed to control the [iCub](http://www.icub.org/) humanoid robot. It can be imagined as a **starting point** and a **support** repository for a user that intends to develop a new Simulink controller (not necessarily for the iCub robot) in within the framework of the [robotology](https://github.com/robotology) organization. It is worth noting that: + +- The controllers stored in this repository are an **overview** of the possibile control frameworks that can be implemented using the `robotology` software infrastructure. Also, the repository contains a [library](library/README.md) of configuration and utility Matlab functions to design simulations with [Gazebo](http://gazebosim.org/) simulator and on the real robot iCub. + +- The robot dynamics and kinematics is computed run-time by means of [WBToolbox](https://github.com/robotology/wb-toolbox), a Simulink library that wraps [iDyntree](https://github.com/robotology/idyntree). For more information on iDyntree library, see also this [README](https://github.com/robotology/idyntree/blob/master/README.md). + +- The Simulink models implement different control strategies both for fixed-base and for floating-base robots. They space from `momentum-based` torque control to `inverse-kinematics-based` position control. Have a look at the [controllers](controllers/README.md) folder for more details. + ## Dependencies This repository depends upon the following Software: -- [Matlab/Simulink](https://it.mathworks.com/products/matlab.html), default version **R2017b** -- [WB-Toolbox](https://github.com/robotology/WB-Toolbox) and [blockfactory](https://github.com/robotology/blockfactory) -- [Gazebo Simulator](http://gazebosim.org/), default version **9.0** -- [gazebo-yarp-plugins](https://github.com/robotology/gazebo-yarp-plugins) +- [CMake](https://cmake.org/), at least version **3.5**. +- [Matlab/Simulink](https://it.mathworks.com/products/matlab.html), default version **R2017b**. +- [WB-Toolbox](https://github.com/robotology/WB-Toolbox) and [blockfactory](https://github.com/robotology/blockfactory). +- [Gazebo Simulator](http://gazebosim.org/), default version **9.0**. +- [gazebo-yarp-plugins](https://github.com/robotology/gazebo-yarp-plugins). - [icub-gazebo](https://github.com/robotology/icub-gazebo), [icub-gazebo-wholebody](https://github.com/robotology-playground/icub-gazebo-wholebody) and [icub-models](https://github.com/robotology/icub-models) to access iCub models. -- [codyco-modules](https://github.com/robotology/codyco-modules) (Optional, for using [home positions](https://github.com/robotology/codyco-modules/tree/master/src/modules/torqueBalancing/app/robots) and [wholeBodyDynamics](https://github.com/robotology/codyco-modules/tree/master/src/devices/wholeBodyDynamics) device). +- [whole-body-estimators](https://github.com/robotology/whole-body-estimators) (**Optional**, for using [wholeBodyDynamics](https://github.com/robotology/whole-body-estimators/tree/master/devices/wholeBodyDynamics) device). +- [YARP](https://github.com/robotology/yarp) and [icub-main](https://github.com/robotology/icub-main). ## Installation and usage -**NOTE:** it is suggested to install `whole-body-controllers` and most of its dependencies (namely, `codyco-modules`,`icub-gazebo`,`icub-gazebo-wholebody`, `icub-models`, `gazebo-yarp-plugins`, `blockfactory` and `WB-Toolbox` and their dependencies) using the [robotology-superbuild](https://github.com/robotology/robotology-superbuild) (enable `ROBOTOLOGY_USES_GAZEBO`, `ROBOTOLOGY_ENABLE_DYNAMICS`, `ROBOTOLOGY_USES_MATLAB` options). +The repository is usually tested and developed on **Ubuntu** and **macOS** operating systems. Some functionalities may not work properly on **Windows**. -- Otherwise, clone the repository on your pc by running on a terminal `git clone https://github.com/robotology/whole-body-controllers`, or dowload the repository. +- **NOTE:** it is suggested to install `whole-body-controllers` and most of its dependencies (namely, `YARP`, `icub-main`, `whole-body-estimators`,`icub-gazebo`,`icub-gazebo-wholebody`, `icub-models`, `gazebo-yarp-plugins`, `blockfactory` and `WB-Toolbox` and their dependencies) using the [robotology-superbuild](https://github.com/robotology/robotology-superbuild) (enable `ROBOTOLOGY_USES_GAZEBO`, `ROBOTOLOGY_ENABLE_DYNAMICS`, `ROBOTOLOGY_USES_MATLAB` options). -- set the environmental variable `YARP_ROBOT_NAME` in the `.bashrc` file to be the name of the robot you want to control. List of supported robot names: +- Otherwise, after installing all the dependencies, **clone the repository** on your pc by running on a terminal `git clone https://github.com/robotology/whole-body-controllers`, or download the repository. Then (on Ubuntu), open a terminal from the folder where you downloaded whole-body-controllers and run: + + ``` + mkdir build + cd build + ccmake .. + ``` + in the GUI that it will open, set the `CMAKE_PREFIX_PATH` as your desired installation folder. Then, run `make install`. + +- Set the environmental variable `YARP_ROBOT_NAME` in your `.bashrc` file (or equivalent) to be the name of the robot you want to control. List of supported robot names: | Robot Names | Associated URDF Model | |:-------------:|:-------------:| @@ -34,23 +53,23 @@ This repository depends upon the following Software: | iCubGazeboV2_5|[model.urdf](https://github.com/robotology/icub-models/blob/master/iCub/robots/iCubGazeboV2_5/model.urdf)| | icubGazeboSim |[model.urdf](https://github.com/robotology/yarp-wholebodyinterface/blob/master/app/robots/icubGazeboSim/model.urdf) | -- to use the Simulink controllers, it is **required** to add the [matlab-wbc](library/matlab-wbc) folder to the Matlab path. There are two different possible ways to add the folder to the path: - - manually add the folder to the Matlab path; - - run **only once** the [startup_WBC.m](config/startup_WBC.m) script. In this case, it is required to **always** start matlab from the folder where the `pathdef.m` file is (usually `~/Documents/MATLAB`). For further information see also the [WBToolbox documentation](https://robotology.github.io/wb-toolbox/mkdocs/install/#matlab). - - **Note**: to use any function inside the package [matlab-wbc/+wbc](library/matlab-wbc/+wbc), add the `wbc` prefix to the function name when the function is invoked, i.e. +- **IMPORTANT!** to use the WBC Simulink controllers, it is **required** to add the **installed** [+wbc](library/matlab-wbc/+wbc) folder to the Matlab path. There are two possible ways to add the folder to the Matlab path: + + **1a.** `manually` and `permanently` add the folder to the Matlab path; - `[outputs] = wbc.myFunction(inputs)`. + **1b.** run **only once** the [startup_WBC.m](config/startup_WBC.m.in) script, which is installed in your `${BUILD}` folder. In this case, path is **not** permanently added to Matlab, and it is required to **always** start Matlab from the folder where your `pathdef.m` file is (usually `~/Documents/MATLAB`). To facilitate the reaching of the WBC working folder from the folder containing the `pathdef.m`, a `goToWholeBodyController.m` script can be [automatically created](config/createGoToWBC.m) in that folder. Run it to jump to the WBC folder. For further information on the installation procedure see also the [WBToolbox documentation](https://robotology.github.io/wb-toolbox/mkdocs/install/#matlab). + **WARNING**: if the repository is installed through the `robotology-superbuild`, **DO NOT** run the `startup_WBC.m` file but instead run the [startup_robotology_superbuild](https://github.com/robotology/robotology-superbuild/blob/master/cmake/template/startup_robotology_superbuild.m.in) file that comes along with robotology-superbuild installation. + - **Note**: to use any function inside the package [matlab-wbc/+wbc](library/matlab-wbc/+wbc), add the `wbc` prefix to the function name when the function is invoked, i.e. `[outputs] = wbc.myFunction(inputs)`. More information on packages can be found in the [Matlab documentation](https://it.mathworks.com/help/matlab/matlab_oop/scoping-classes-with-packages.html). - More information on packages can be found in the [Matlab documentation](https://it.mathworks.com/help/matlab/matlab_oop/scoping-classes-with-packages.html). +- There are some functionalities of the repo such as the [automatic generation of c++ code from Simulink](https://github.com/robotology/whole-body-controllers#automatic-generation-of-c-code-from-simulink) that require to enable not-default cmake options. Check the available options by running `ccmake .` in your `build` directory. ## Troubleshooting Please refer to the [WBToolbox troubleshooting documentation](https://robotology.github.io/wb-toolbox/mkdocs/troubleshooting/). -## Structure of the repo +## Relevant folders of the repo -- **config**: a collection of scripts for correctly configure this repo. [[README]](config/README.md) +- **config**: a collection of scripts to correctly configure this repo. [[README]](config/README.md) - **controllers**: Simulink whole-body position and torque controllers for balancing of humanoid robots. [[README]](controllers/README.md) @@ -62,9 +81,10 @@ Please refer to the [WBToolbox troubleshooting documentation](https://robotology ### Available controllers -- [fixed-base-joints-control](controllers/fixed-base-joints-control/README.md) +- [fixed-base-joints-torque-control](controllers/fixed-base-joints-torque-control/README.md) - [floating-base-balancing-position-control](controllers/floating-base-balancing-position-control/README.md) - [floating-base-balancing-torque-control](controllers/floating-base-balancing-torque-control/README.md) +- [simulink-balancing-simulator](controllers/simulink-balancing-simulator/README.md) ### Matlab functions library @@ -80,16 +100,22 @@ There is the possibility to generate c++ code from the Simulink models using [Si When used for controlling real platforms, heavy Simulink models may violate the user-defined simulation time step, see also [this issue](https://github.com/robotology/wb-toolbox/issues/160). It seems a source of delay is the run-time update of the Simulink interface. For this reason, a [static GUI for running the models](library/matlab-gui) has been developed. If you want to run Simulink with the static GUI, run the [startModelWithStaticGui](controllers/floating-base-balancing-torque-control/startModelWithStaticGui.m) script. +### Home positions for yarpmotorgui + +The repo contains a set of predefined [home positions](utilities/homePositions) to be used with the [yarpmotorgui](https://www.yarp.it/yarpmotorgui.html). By default, if the repo is installed through `robotology-superbuild`, the home positions are installed in the `robotology-superbuild/build/install` directory. Otherwise add the path to the [homePositions](utilities/homePositions) folder to the `YARP_DATA_DIRS` environmental variable in your `.bashrc` file . The command to use the home positions with the yarpmotorgui is `yarpmotorgui --from myHomePosFileName.ini`. + ## Where do I find legacy materials? Official legacy repositories are: [mex-wholebodymodel](https://github.com/robotology/mex-wholebodymodel) and [WBI-Toolbox-controllers](https://github.com/robotology-legacy/WBI-Toolbox-controllers). **Note**: these legacy repos contain undocumented/outdated code, and duplicated or not tested matlab functions. They also contain original code that has been tested on the robot in the past and then never used again, or code that will be ported in the main repository in the future. -- [exploit friction and walking controller](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/whole-body-controllers-legacy/controllers/legacy) -- [seesaw controller](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/whole-body-controllers-legacy/controllers/legacy) -- [automatic gain tuning](https://github.com/robotology/mex-wholebodymodel/tree/master/controllers/torqueBalancingGainTuning) +- [exploit friction controller](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/master/controllers/momentum-based-yoga-friction) +- [walking controller](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/master/controllers/task-based-walking) +- [seesaw controller](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/master/controllers/torqueBalancingOnSeesaw) +- [automatic gain tuning](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/master/controllers/torqueBalancingTuning) and [automatic gain tuning-matlab](https://github.com/robotology/mex-wholebodymodel/tree/master/controllers/torqueBalancingGainTuning) - [elastic joints control](https://github.com/robotology/mex-wholebodymodel/tree/master/controllers/torqueBalancing_JE) -- [walkman control](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/whole-body-controllers-legacy/controllers/legacy/torqueBalancing-walkman) and [walkman control-matlab](https://github.com/robotology/mex-wholebodymodel/tree/master/controllers/torqueBalancingWalkman) +- [walkman control](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/master/controllers/torqueBalancing-walkman) and [walkman control-matlab](https://github.com/robotology/mex-wholebodymodel/tree/master/controllers/torqueBalancingWalkman) - [joint-space control and centroidal transformation](https://github.com/robotology/mex-wholebodymodel/tree/master/controllers/torqueBalancingJointControl) +- [stand-up control 4 contacts](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/master/controllers/torqueBalancingStandup_4Contacts) ## Citing this work diff --git a/cmake/CreateAutogeneratedCodeTargets.cmake.in b/cmake/CreateAutogeneratedCodeTargets.cmake.in index 85c38a1..dd7913c 100644 --- a/cmake/CreateAutogeneratedCodeTargets.cmake.in +++ b/cmake/CreateAutogeneratedCodeTargets.cmake.in @@ -1,22 +1,29 @@ +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +find_package(BlockFactory COMPONENTS SimulinkCoder REQUIRED) + +# =============================================== # Generate a library from the autogenerated class # =============================================== -set(MDL_NAME @MDL_NAME@) -add_generated_code_lib(MODELNAME ${MDL_NAME}) +include(TargetFromGeneratedCode) +set(SIMULINK_MODEL_NAME "@MDL_NAME@") +# This macro creates a target named as the mdl file for the autogenerated class +target_from_generated_code( + MODELNAME ${SIMULINK_MODEL_NAME} + SOURCE_FOLDER ${CMAKE_CURRENT_SOURCE_DIR}) + +# ================================ # Test executable with custom main # ================================ # Set the target name for the custom driver -set(CODER_MAIN "CoderMain_${MDL_NAME}") - -configure_file( - "${PROJECT_SOURCE_DIR}/src/driver.cpp.in" - "${CMAKE_CURRENT_BINARY_DIR}/driver.cpp" - @ONLY) +set(CODER_MAIN "Run${SIMULINK_MODEL_NAME}") add_executable(${CODER_MAIN} - "${CMAKE_CURRENT_BINARY_DIR}/driver.cpp" -) + "${CMAKE_CURRENT_SOURCE_DIR}/driver.cpp") -target_link_libraries(${CODER_MAIN} PUBLIC ${MDL_NAME}_LIB) \ No newline at end of file +target_link_libraries(${CODER_MAIN} PUBLIC ${SIMULINK_MODEL_NAME}) diff --git a/cmake/RegisterMdl.cmake b/cmake/RegisterMdl.cmake index 82ae584..3361109 100644 --- a/cmake/RegisterMdl.cmake +++ b/cmake/RegisterMdl.cmake @@ -4,7 +4,9 @@ function(initialize_mdl_set) # PARSE ARGUMENTS # =============== + set(_options) set(_oneValueArgs NAME) + set(_multiValueArgs) cmake_parse_arguments(PREFIX "${_options}" @@ -31,7 +33,9 @@ function(register_mdl) # PARSE ARGUMENTS # =============== + set(_options) set(_oneValueArgs MODELNAME) + set(_multiValueArgs) cmake_parse_arguments(PREFIX "${_options}" diff --git a/config/CMakeLists.txt b/config/CMakeLists.txt new file mode 100644 index 0000000..05b1e7d --- /dev/null +++ b/config/CMakeLists.txt @@ -0,0 +1 @@ +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/startup_WBC.m.in ${CMAKE_BINARY_DIR}/startup_WBC.m) \ No newline at end of file diff --git a/config/README.md b/config/README.md index 4335ca9..a4913e4 100644 --- a/config/README.md +++ b/config/README.md @@ -4,4 +4,6 @@ A collection of scripts used for configuring the repo. - [export_WBC.m](export_WBC.m): run this script. Then, digit the Matlab version in which you want to export the Simulink models. All models in the repo will be exported to that version. By default, Simulnk models in this repo are written using Matlab 2017b. **Remember: you cannot export a model in a Matlab version newer than the one you are using**! -- [startup_WBC.m](startup_WBC.m): run this script. Then, the path to the `matlab-wbc` folder will be **permanently** added to your `pathdef.m` file, which will be saved inside the Matlab `userpath`. In order to have the `matlab-wbc` folder inside the Matlab path, it is **required** to start Matlab from the folder where the `pathdef.m` file is (i.e., from the folder that the `userpath` is pointing, usually `~/Documents/MATLAB`). \ No newline at end of file +- [startup_WBC.m.in](startup_WBC.m.in): **DO NOT** run this script, but run its installation inside the procjet `${BUILD}` folder. Then, the path to the `+wbc` folder will be **permanently** added to your `pathdef.m` file, which will be saved inside the Matlab `userpath`. **WARNING**! In order to have the `+wbc` folder inside the Matlab path, it is **required** to start Matlab from the folder where the `pathdef.m` file is (i.e., from the folder that the `userpath` is pointing, usually `~/Documents/MATLAB`). + +- [createGoToWBC.m](createGoToWBC.m): run this script. A file named 'goToWholeBodyControllers.m' will be created in the folder pointed by your `userpath`, to facilitate the reaching of WBC source folder when Matlab is started from the `userpath` folder. diff --git a/config/createGoToWBC.m b/config/createGoToWBC.m new file mode 100644 index 0000000..b4b8525 --- /dev/null +++ b/config/createGoToWBC.m @@ -0,0 +1,76 @@ +%% createGoToWBC.m +% +% Run this script once create the goToWholeBodyControllers.m script in your userpath. + +clc +fprintf('\n## whole-body-controllers ##\n'); +fprintf('\nCreating "goToWholeBodyControllers.m" in your userpath...\n\n'); + +% Path to the Matlab userpath +pathToUserpath = userpath; +pathSeparatorLocation = strfind(pathToUserpath, pathsep); + +if isempty(pathToUserpath) + + answer = input('Empty userpath. Do you want to reset the userpath? Y/N ','s'); + + if strcmpi(answer,'Y') + + userpath('reset'); + disp('Resetting userpath..'); + pathToUserpath = userpath; + pathSeparatorLocation = strfind(pathToUserpath, pathsep); + else + error('Please set the userpath before running this script'); + end + +elseif size(pathSeparatorLocation, 2) > 1 + + answer = input('Multiple userpath. Do you want to reset the userpath? Y/N ','s'); + + if strcmpi(answer,'Y') + + userpath('reset'); + disp('Resetting userpath..'); + pathToUserpath = userpath; + pathSeparatorLocation = strfind(pathToUserpath, pathsep); + else + error('Please set a single userpath before running this script'); + end +end + +% check again the userpath +if isempty(pathToUserpath) + + error('userpath is still empty. Please set the userpath before running this script'); + +elseif size(pathSeparatorLocation, 2) > 1 + + error('There are still multiple userpath. Please set a single userpath before running this script'); +end + +% save a script named "goToWholeBodyControllers" inside the pathdef folder, +% to facilitate the user to reach the WBC working folder + +% get the WBC path +[~, currentFolderName, ~] = fileparts(pwd); +currentPath = pwd; +wbcPath = currentPath(1:end-length(currentFolderName)); + +% note: exist returns 2 if a .m file with the specified name is found +if exist([pathToUserpath, filesep, 'goToWholeBodyControllers.m'],'file') == 2 + + fprintf('\n') + warning(['A goToWholeBodyControllers.m file exists already in your ',pathToUserpath, ' folder,' ... + ' and therefore it won''t be created.']) +else + % create the script and write inside the cd command + fid = fopen([pathToUserpath, filesep, 'goToWholeBodyControllers.m'],'w'); + fprintf(fid, ['cd ', wbcPath, ';']); + fclose(fid); + + fprintf('\n') + fprintf(['A file called goToWholeBodyControllers.m has been created in your %s folder.\n', ... + 'This will help to quickly reach the WBC-project folder after ', ... + 'Matlab is launched.\n'], pathToUserpath); +end diff --git a/config/export_WBC.m b/config/export_WBC.m index cdd825f..3c61abd 100644 --- a/config/export_WBC.m +++ b/config/export_WBC.m @@ -4,28 +4,28 @@ % the default version is MATLAB 2017b. IF YOU OWE A MATLAB VERSION GREATER % THAN 2017B, REMEMBER TO EXPORT ALL MODELS TO THE DEFAULT SUPPORTED % VERSION BEFORE PUBLISHING ANY MODIFICATIONS, OTHERWISE OTHER USERS MAY -% NOT BE ABLE TO USE THE TOOLBOX! +% NOT BE ABLE TO USE THE MODELS! % clc clear close all -fprintf('\nWholeBodyControl Toolbox\n'); +fprintf('\nwhole-body-controllers\n'); fprintf('\nExport project to a previous Matlab version\n'); fprintf('\nOldest supported version: R2014a\n'); fprintf('\n######################################################\n'); fprintf('\nWarning: this function exports only Simulink models.\n'); -fprintf('\nIf a .m file requires a dependency (toolbox, new matlab version)\n'); -fprintf('\nthat is not compatible with your installation, the WBC toolbox\n'); -fprintf('\nmay not work properly. Consider running also tests after the export.\n'); +fprintf('\nIf a .m file requires a dependency that is not\n'); +fprintf('\ncompatible with your installation, the WBC controllers\n'); +fprintf('\nmay still not work properly.\n'); fprintf('\n######################################################\n\n'); % list of all simulink models (in this repo only .mdl are allowed) in the project -mdlList = dir('../**/*.mdl'); +mdlList = dir('../**/*.mdl'); % matlab version to which export all models -matlabVer = input('Specify the Matlab version to export models (format: R20XXx) ','s'); +matlabVer = input('Specify the Matlab version to export models (format: R20XXx) ','s'); %% Verify matlab version @@ -45,7 +45,7 @@ if strcmp(matlabVer_list{k},matlabVer) - fprintf(['\nExporting files into Matlab ',matlabVer,'\n\n']); + fprintf(['\nExporting files into Matlab ',matlabVer, '...','\n\n']); matlabVer_found = k; end end @@ -98,4 +98,4 @@ delete([mdlList(k).folder,'/',mdlList(k).name(1:end-4),'_temp.mdl']) end -fprintf('\nDone.\n'); \ No newline at end of file +fprintf('\nDone.\n'); diff --git a/config/startup_WBC.m b/config/startup_WBC.m.in similarity index 69% rename from config/startup_WBC.m rename to config/startup_WBC.m.in index 5187368..fdd7ae0 100644 --- a/config/startup_WBC.m +++ b/config/startup_WBC.m.in @@ -1,22 +1,22 @@ %% startup_WBC.m % -% Run this script once to permanently add the matlab-wbc library to your MATLAB path. +% Run this script once to permanently add the +wbc library to your MATLAB path. +clc fprintf('\n## whole-body-controllers ##\n'); -fprintf('\nAdding "matlab-wbc" library to pathdef.m...\n\n'); +fprintf('\nAdding "+wbc" library to your pathdef.m...\n\n'); % path to whole-body-controllers -pathToWBC = pwd; -pathToWBC = pathToWBC(1:end-6); +pathToInstallDir = '@CMAKE_INSTALL_PREFIX@'; % path to the matlab-wbc library -pathToLibrary = [pathToWBC, filesep, 'library/matlab-wbc']; +pathToLibrary = [pathToInstallDir, filesep, 'mex']; if exist(pathToLibrary, 'dir') addpath(pathToLibrary); else - error('Path to the "matlab-wbc" library not found or not correct.') + error('Path to the "+wbc" library not found or not correct.') end % Path to the Matlab userpath @@ -72,8 +72,14 @@ if (~savepath([pathToUserpath, filesep, 'pathdef.m'])) fprintf(['A file called pathdef.m has been created in your %s folder.\n', ... - 'This should be enough to permanently add matlab-wbc to ', ... + 'This should be enough to permanently add +wbc to ', ... 'your MATLAB installation.\n'], pathToUserpath); else - disp('There was an error generating the pathdef.m. Please manually add the matlab-wbc folder to your matlabpath'); -end \ No newline at end of file + disp('There was an error generating the pathdef.m. Please manually add the +wbc folder to your matlabpath'); +end + +% inform the user that it is possible to generate the goToWholeBodyControllers script +fprintf('\n'); +fprintf(['INFO: you may generate a file called goToWholeBodyControllers.m in your %s folder.\n', ... + 'This will help to quickly reach the WBC-project folder after Matlab is launched.\n', ... + 'To create the file, go to the ''WBC_SOURCE_DIR/config'' and run ''createGoToWBC.m.'' \n'], pathToUserpath); diff --git a/controllers/README.md b/controllers/README.md index 6a7fce2..d0661c8 100644 --- a/controllers/README.md +++ b/controllers/README.md @@ -4,10 +4,15 @@ Simulink controllers for humanoid robots. ## List of available controllers: -- **fixed-base-joints-control** [[README]](fixed-base-joints-control/README.md) +- **fixed-base-joints-torque-control** [[README]](fixed-base-joints-torque-control/README.md) - **floating-base-balancing-position-control** [[README]](floating-base-balancing-position-control/README.md) - **floating-base-balancing-torque-control** [[README]](floating-base-balancing-torque-control/README.md) +- **simulink-balancing-simulator** [[README]](simulink-balancing-simulator/README.md) + +## Plotting Utilities + +- **data-processing** [[README]](data-processing/README.md) ## Usage -See corresponding READMEs, and check the [documentation](https://github.com/robotology-playground/whole-body-controllers/tree/master/doc) of the repo. \ No newline at end of file +See corresponding READMEs, and check the [documentation](https://github.com/robotology-playground/whole-body-controllers/tree/master/doc) or the **wiki** of the repo. diff --git a/controllers/data-processing/README.md b/controllers/data-processing/README.md new file mode 100644 index 0000000..13861ab --- /dev/null +++ b/controllers/data-processing/README.md @@ -0,0 +1,9 @@ +## DATA PROCESSING + +All Simulink controllers in the repo save data with the following convention: + +- data are saved in `structure with time` format; + +- data contains the tag `DATA` in their names. + +Based on these rules, the script `processDataFromSimulink.m` automatically loads from workspace (or from a previously saved `.mat` file) all the data, and plots them according to the user-defined specifications. diff --git a/controllers/data-processing/processDataFromSimulink.m b/controllers/data-processing/processDataFromSimulink.m new file mode 100644 index 0000000..d187c92 --- /dev/null +++ b/controllers/data-processing/processDataFromSimulink.m @@ -0,0 +1,130 @@ +% This script assumes that the variables under interest of analysis are: +% +% 1) SAVED FROM SIMULINK as STRUCTURE WITH TIME format; +% +% 2) contains the suffix "_DATA" in the name. +% +% Then, the script reads the workspace (or loads a .mat file), looks for +% these variables and allows the user to choose which variable to plot. +% +% + +%% DATA TO LOAD (comment if data are in the workspace) +% load('EXAMPLE.mat') + +%% SETUP VISUALIZER +close all +clc + +addpath('./src_plot') + +%%utility variables + +% labels and font size +xAxisLabel = 'Time [s]'; +lineSize = 2; + +% print figures to .png +printFigure = false; + +% dock all figures together +dockFigures = true; + +% desired init and end time in the figures [s] +timeInit = 0; +timeLimit = 120; + +if dockFigures + + set(0,'DefaultFigureWindowStyle','docked') +else + set(0,'DefaultFigureWindowStyle','normal') +end + +%% Get dataset from workspace +dataVariables = whos('*DATA'); +plotList = {}; + +for k = 1:length(dataVariables) + + plotList{k} = dataVariables(k).name; %#ok +end + +[plotNumber, ~] = listdlg('PromptString', 'Choose the variables to plot:', ... + 'ListString', plotList, ... + 'SelectionMode', 'multiple', ... + 'ListSize', [250 150]); + +% regenerate the plot list with the data selected by user +plotList = plotList(plotNumber); + +%% Select which time to use for the plot (if Yarp time is available) +timeList = {'simulink time'}; + +if exist('time_Yarp','var') + + timeList{2} = 'yarp time'; +end + +if length(timeList) > 1 + + [timeNumber, ~] = listdlg('PromptString', 'Choose the time vector to use:', ... + 'ListString', timeList, ... + 'SelectionMode', 'single', ... + 'ListSize', [250 150]); +else + timeNumber = 1; +end + +%% Process data to plot +figNumber = 0; + +for k = 1:length(plotList) + + currentData = eval(plotList{k}); + + if timeNumber == 1 + + time = currentData.time; + + elseif timeNumber == 2 + + time = time_Yarp.signals.values; + end + + % get the user defined time length + timeLimitIndex = sum(time <= timeLimit); + timeInitIndex = sum(time <= timeInit); + tVector = time(timeInitIndex:timeLimitIndex); + + dataStructure = currentData.signals; + figNumber = figNumber + 1; + + for kk = 1:length(currentData.signals) + + valuesToPlot = squeeze(currentData.signals(kk).values); + + % twist data dimensions if necessary + if size(valuesToPlot,1) < length(time) + + valuesToPlot = valuesToPlot'; + end + + valuesToPlot = valuesToPlot(timeInitIndex:timeLimitIndex,:); + yAxisLabel = currentData.signals(kk).label; + + if kk == 1 + + titleLabel = plotList{k}(1:end-5); + else + titleLabel = ''; + end + + figure(figNumber) + subplot(length(currentData.signals),1,kk) + custom_plot(tVector, valuesToPlot, xAxisLabel, yAxisLabel, titleLabel, ... + lineSize, '', figNumber, printFigure, 0); + end +end + +rmpath('./src_plot') diff --git a/controllers/data-processing/src_plot/custom_plot.m b/controllers/data-processing/src_plot/custom_plot.m new file mode 100644 index 0000000..c0297e1 --- /dev/null +++ b/controllers/data-processing/src_plot/custom_plot.m @@ -0,0 +1,28 @@ +% Redefinition of the Matlab "plot" function, with more options available +% and with the possibility of printing the figure to a .pdf file. +% +% +function [] = custom_plot(x, y, label_xaxis, label_yaxis, label_title, ... + lineSize, labels_legend, figNumber, printFigure, useDots) + + % generate nice plots + figure(figNumber) + + if useDots + + plot(x,y,'o','Markersize',10) + else + plot(x,y,'Linewidth',lineSize) + end + + xlabel(label_xaxis) + ylabel(label_yaxis) + title(label_title) + legend(labels_legend,'Location','Best') + grid on + hold all + + if printFigure + print(gcf, [label_title, '.png'], '-dpng','-r600') + end +end diff --git a/controllers/fixed-base-joints-control/README.md b/controllers/fixed-base-joints-torque-control/README.md similarity index 75% rename from controllers/fixed-base-joints-control/README.md rename to controllers/fixed-base-joints-torque-control/README.md index 870d034..2e6c920 100644 --- a/controllers/fixed-base-joints-control/README.md +++ b/controllers/fixed-base-joints-torque-control/README.md @@ -1,6 +1,8 @@ ## Module description -This module implements a simple torque control balancing strategy. The robot is assumed to have its `base_link` fixed on a pole. Input torques are the `gravity toruqes` which allow to perform **gravity compensation**. Optionally, it is also possible to move each controlled joint in order to track a desired joints trajectory. +This module implements a simple torque control balancing strategy. The robot is assumed to have its `base_link` fixed on a pole. Input torques are the `gravity torques` which allow to perform **gravity compensation**. Optionally, it is also possible to move each controlled joint in order to track a desired joints trajectory. + + ### Compatibility @@ -12,10 +14,14 @@ Currently, supported robots are: `iCubGenova04`, `iCunGenova02`, `icubGazeboSim` ## 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 `initJointsControl.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`. \ No newline at end of file +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/fixed-base-joints-control/app/robots/iCubGazeboV2_5/configJointsControl.m b/controllers/fixed-base-joints-torque-control/app/robots/iCubGazeboV2_5/configJointsControl.m similarity index 100% rename from controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/configJointsControl.m rename to controllers/fixed-base-joints-torque-control/app/robots/iCubGazeboV2_5/configJointsControl.m diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/configRobot.m b/controllers/fixed-base-joints-torque-control/app/robots/iCubGazeboV2_5/configRobot.m similarity index 100% rename from controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/configRobot.m rename to controllers/fixed-base-joints-torque-control/app/robots/iCubGazeboV2_5/configRobot.m diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m b/controllers/fixed-base-joints-torque-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m similarity index 100% rename from controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m rename to controllers/fixed-base-joints-torque-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGenova02/configJointsControl.m b/controllers/fixed-base-joints-torque-control/app/robots/iCubGenova02/configJointsControl.m similarity index 100% rename from controllers/fixed-base-joints-control/app/robots/iCubGenova02/configJointsControl.m rename to controllers/fixed-base-joints-torque-control/app/robots/iCubGenova02/configJointsControl.m diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGenova02/configRobot.m b/controllers/fixed-base-joints-torque-control/app/robots/iCubGenova02/configRobot.m similarity index 100% rename from controllers/fixed-base-joints-control/app/robots/iCubGenova02/configRobot.m rename to controllers/fixed-base-joints-torque-control/app/robots/iCubGenova02/configRobot.m diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGenova02/gainsAndReferences.m b/controllers/fixed-base-joints-torque-control/app/robots/iCubGenova02/gainsAndReferences.m similarity index 100% rename from controllers/fixed-base-joints-control/app/robots/iCubGenova02/gainsAndReferences.m rename to controllers/fixed-base-joints-torque-control/app/robots/iCubGenova02/gainsAndReferences.m diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGenova04/configJointsControl.m b/controllers/fixed-base-joints-torque-control/app/robots/iCubGenova04/configJointsControl.m similarity index 100% rename from controllers/fixed-base-joints-control/app/robots/iCubGenova04/configJointsControl.m rename to controllers/fixed-base-joints-torque-control/app/robots/iCubGenova04/configJointsControl.m diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGenova04/configRobot.m b/controllers/fixed-base-joints-torque-control/app/robots/iCubGenova04/configRobot.m similarity index 100% rename from controllers/fixed-base-joints-control/app/robots/iCubGenova04/configRobot.m rename to controllers/fixed-base-joints-torque-control/app/robots/iCubGenova04/configRobot.m diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGenova04/gainsAndReferences.m b/controllers/fixed-base-joints-torque-control/app/robots/iCubGenova04/gainsAndReferences.m similarity index 100% rename from controllers/fixed-base-joints-control/app/robots/iCubGenova04/gainsAndReferences.m rename to controllers/fixed-base-joints-torque-control/app/robots/iCubGenova04/gainsAndReferences.m diff --git a/controllers/fixed-base-joints-control/app/robots/icubGazeboSim/configJointsControl.m b/controllers/fixed-base-joints-torque-control/app/robots/icubGazeboSim/configJointsControl.m similarity index 100% rename from controllers/fixed-base-joints-control/app/robots/icubGazeboSim/configJointsControl.m rename to controllers/fixed-base-joints-torque-control/app/robots/icubGazeboSim/configJointsControl.m diff --git a/controllers/fixed-base-joints-control/app/robots/icubGazeboSim/configRobot.m b/controllers/fixed-base-joints-torque-control/app/robots/icubGazeboSim/configRobot.m similarity index 100% rename from controllers/fixed-base-joints-control/app/robots/icubGazeboSim/configRobot.m rename to controllers/fixed-base-joints-torque-control/app/robots/icubGazeboSim/configRobot.m diff --git a/controllers/fixed-base-joints-control/app/robots/icubGazeboSim/gainsAndReferences.m b/controllers/fixed-base-joints-torque-control/app/robots/icubGazeboSim/gainsAndReferences.m similarity index 100% rename from controllers/fixed-base-joints-control/app/robots/icubGazeboSim/gainsAndReferences.m rename to controllers/fixed-base-joints-torque-control/app/robots/icubGazeboSim/gainsAndReferences.m diff --git a/controllers/fixed-base-joints-control/initJointsControl.m b/controllers/fixed-base-joints-torque-control/initJointsControl.m similarity index 100% rename from controllers/fixed-base-joints-control/initJointsControl.m rename to controllers/fixed-base-joints-torque-control/initJointsControl.m diff --git a/controllers/fixed-base-joints-control/jointsControl.mdl b/controllers/fixed-base-joints-torque-control/jointsControl.mdl similarity index 94% rename from controllers/fixed-base-joints-control/jointsControl.mdl rename to controllers/fixed-base-joints-torque-control/jointsControl.mdl index b4fecbe..5d52090 100644 --- a/controllers/fixed-base-joints-control/jointsControl.mdl +++ b/controllers/fixed-base-joints-torque-control/jointsControl.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.358" + ComputedModelVersion "1.368" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -69,7 +69,7 @@ Model { ExternalFileReference { Reference "WBToolboxLibrary/States/GetLimits" Path "jointsControl/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/GetLimits" - SID "478" + SID "695" Type "LIBRARY_BLOCK" } ExternalFileReference { @@ -92,13 +92,9 @@ Model { } OrderedModelArguments 1 } - DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" - LogicAnalyzerGraphicalSettings "" - LogicAnalyzerPlugin "on" - LogicAnalyzerSignalOrdering "" - CustomCodeFunctionData "" SLCCPlugin "on" + LogicAnalyzerPlugin "on" + DiagnosticSuppressor "on" PostLoadFcn "%% Try to open the static GUI\ntry\n\n staticGUI = simulinkStaticGUI;\n\nend" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on @@ -119,10 +115,12 @@ Model { "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" StopFcn "%% stop the simulation\ncd(fileparts(which(bdroot)));\nstopJointsControl;\n\n%% Try to edit the GUI (the" - " user may have already closed it)\ntry\n\n %% Update Compile Button\n set(sl_synch_handles.compileButton,'Back" - "groundcolor',[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" + " 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_synch_handl" + "es.startButton,'Enable','on');\n end\n\n %% Update Compile Button\n set(sl_synch_handles.compileButton,'Bac" + "kgroundcolor',[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" LastSavedArchitecture "glnxa64" Object { $PropName "BdWindowsInfo" @@ -157,9 +155,9 @@ Model { IsActive [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [1815.0, 876.0] + Extents [1815.0, 821.0] ZoomFactor [3.9274725274725273] - Offset [171.435366536094, 145.97789591494126] + Offset [171.435366536094, 149.41522104085058] } Object { $PropName "DockComponentsInfo" @@ -179,11 +177,7 @@ Model { "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABiAP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAALAD///8AAAc9AAADqgAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACA/////wAAAAAAAAAA/////wEAAADo/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAFo/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAANR/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" + "QBjAHQAbwByAAAAAAD/////AAAAOQD///8AAAc9AAADcgAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAA" } } HideAutomaticNames on @@ -193,9 +187,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Mar 08 16:25:37 2019" - RTWModifiedTimeStamp 473963137 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Mar 23 00:04:21 2020" + RTWModifiedTimeStamp 506822659 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -230,7 +224,6 @@ Model { FunctionConnectors off BrowserLookUnderMasks off SimulationMode "normal" - VisualizeLoggedSignalsWhenLoggingToFile off PauseTimes "5" NumberOfSteps 1 SnapshotBufferSize 10 @@ -302,6 +295,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] StartTime "0.0" StopTime "Config.SIMULATION_TIME" AbsTol "auto" @@ -341,6 +335,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] Decimation "1" ExternalInput "[t, u]" FinalStateName "xFinal" @@ -382,7 +377,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 8 + Dimension 9 Cell "BooleansAsBitfields" Cell "PassReuseOutputArgsAs" Cell "PassReuseOutputArgsThreshold" @@ -391,9 +386,11 @@ Model { Cell "OptimizeModelRefInitCode" Cell "NoFixptDivByZeroProtection" Cell "UseSpecifiedMinMax" + Cell "EfficientTunableParamExpr" PropName "DisabledProps" } Description "" + Components [] BlockReduction on BooleanDataType on ConditionallyExecuteInputs on @@ -449,6 +446,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] RTPrefix "error" ConsistencyChecking "none" ArrayBoundsChecking "none" @@ -559,6 +557,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] ProdBitPerChar 8 ProdBitPerShort 16 ProdBitPerInt 32 @@ -607,6 +606,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" EnableRefExpFcnMdlSchedulingChecks on CheckModelReferenceTargetMessage "error" @@ -626,6 +626,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] SimCustomSourceCode "" SimCustomHeaderCode "" SimCustomInitializer "" @@ -675,6 +676,7 @@ Model { Cell "GenerateMissedCodeReplacementReport" PropName "DisabledProps" } + Description "" SystemTargetFile "grt.tlc" HardwareBoard "None" TLCOptions "" @@ -685,9 +687,7 @@ Model { PackageName "" TemplateMakefile "grt_default_tmf" PostCodeGenCommand "" - Description "" GenerateReport off - SaveLog off RTWVerbose on RetainRTWFile off RTWBuildHooks [] @@ -775,6 +775,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] Comment "" ForceParamTrailComments off GenerateComments on @@ -830,7 +831,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 16 + Dimension 17 Cell "GeneratePreprocessorConditionals" Cell "IncludeMdlTerminateFcn" Cell "SupportNonInlinedSFcns" @@ -847,9 +848,11 @@ Model { Cell "ExistingSharedCode" Cell "RemoveDisableFunc" Cell "RemoveResetFunc" + Cell "PreserveStateflowLocalDataDimensions" PropName "DisabledProps" } Description "" + Components [] TargetFcnLib "ansi_tfl_table_tmw.mat" TargetLibSuffix "" TargetPreCompLibLocation "" @@ -928,6 +931,7 @@ Model { Version "1.17.1" DisabledProps [] Description "Simulink Coverage Configuration Component" + Components [] Name "Simulink Coverage" CovEnable off CovScope "EntireSystem" @@ -966,9 +970,9 @@ Model { PropName "Components" } Name "Configuration" - ExtraOptions "" CurrentDlgPage "Solver" ConfigPrmDlgPosition [ 396, 138, 1456, 890 ] + ExtraOptions "" } PropName "ConfigurationSets" } @@ -984,7 +988,6 @@ Model { DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" DefaultExtrapolationMethodBetweenContTasks "None" - AutoInsertRateTranBlk [0] } ExplicitPartitioning off BlockDefaults { @@ -1159,7 +1162,7 @@ Model { MustResolveToSignalObject off OutputWhenUnConnected off OutputWhenUnconnectedValue "0" - VectorParamsAs1DForOutWhenUnconnected off + VectorParamsAs1DForOutWhenUnconnected on } Block { BlockType Product @@ -1239,7 +1242,6 @@ Model { TreatAsGroupedWhenPropagatingVariantConditions on ContentPreviewEnabled off IsWebBlock off - IsObserver off } Block { BlockType Sum @@ -1266,11 +1268,20 @@ Model { MaxDataPoints "1000" Decimation "1" SaveFormat "Array" - Save2DSignal "Inherit from input (this choice will be removed - see release notes)" + 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 "jointsControl" @@ -1289,7 +1300,7 @@ Model { ShowPageBoundaries off ZoomFactor "393" ReportName "simulink-default.rpt" - SIDHighWatermark "648" + SIDHighWatermark "707" Block { BlockType Reference Name "Configuration" @@ -1302,6 +1313,11 @@ Model { 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'" @@ -1717,7 +1733,7 @@ Model { "[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'),extmgr.Configuration" - "('Tools','Measurements',true,'Version','2018a')),'Version','2018a','Location',[135 184 3841 2132])" + "('Tools','Measurements',true,'Version','2018a')),'Version','2019b','Location',[135 184 3841 2132])" NumInputPorts "5" Floating off } @@ -1828,7 +1844,7 @@ Model { "0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','Line" "Width',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Conf" "iguration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Too" - "ls','Measurements',true,'Version','2018a')),'Version','2018a','Location',[358 457 1618 936])" + "ls','Measurements',true,'Version','2018a')),'Version','2019b','Location',[358 457 1618 936])" NumInputPorts "5" Floating off } @@ -1939,7 +1955,7 @@ Model { ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWi" "dth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Config" "uration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools" - "','Measurements',true,'Version','2018a')),'Version','2018a','Location',[1175 719 2455 1410])" + "','Measurements',true,'Version','2018a')),'Version','2019b','Location',[1175 719 2455 1410])" NumInputPorts "5" Floating off } @@ -2234,7 +2250,7 @@ Model { ",[0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;1 1 0.0666666666666667;0.71764705" "8823529 0.274509803921569 1;0.392156862745098 0.831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0" "745098039215686 0.650980392156863])),extmgr.Configuration('Tools','Plot Navigation',true,'PreviousAutoscale','X" - "Y'),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2018a','Position',[135 1 37" + "Y'),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2019b','Position',[135 1 37" "06 1990])" NumInputPorts "3" Floating off @@ -2569,7 +2585,7 @@ Model { "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidt" "h',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configur" "ation('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools'," - "'Measurements',true,'Version','2018a')),'Version','2018a','Location',[135 169 1415 860])" + "'Measurements',true,'Version','2018a')),'Version','2019b','Location',[135 169 1415 860])" NumInputPorts "5" Floating off } @@ -2680,7 +2696,7 @@ Model { "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidt" "h',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configur" "ation('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools'," - "'Measurements',true,'Version','2018a')),'Version','2018a','Location',[135 169 1425 890])" + "'Measurements',true,'Version','2018a')),'Version','2019b','Location',[135 169 1425 890])" NumInputPorts "5" Floating off } @@ -2791,7 +2807,7 @@ Model { "neStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth'" ",0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configurat" "ion('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','M" - "easurements',true,'Version','2018a')),'Version','2018a','Location',[387 216 1667 907])" + "easurements',true,'Version','2018a')),'Version','2019b','Location',[387 216 1667 907])" NumInputPorts "5" Floating off } @@ -2902,7 +2918,7 @@ Model { "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWid" "th',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configu" "ration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools'" - ",'Measurements',true,'Version','2018a')),'Version','2018a','Location',[135 175 1445 880])" + ",'Measurements',true,'Version','2018a')),'Version','2019b','Location',[135 175 1445 880])" NumInputPorts "5" Floating off } @@ -3210,6 +3226,11 @@ Model { 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" } @@ -3225,6 +3246,11 @@ Model { 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" } @@ -3240,6 +3266,11 @@ Model { 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" } @@ -3376,6 +3407,11 @@ Model { SourceBlock "WBToolboxLibrary/Model/Dynamics/GetGravityForces" SourceType "Gravity bias" SourceProductName "WholeBodyToolbox" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" ContentPreviewEnabled off Port { PortNumber 1 @@ -3402,6 +3438,7 @@ Model { Position [590, 148, 620, 162] ZOrder 640 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 77 @@ -3632,6 +3669,7 @@ Model { Position [855, 123, 885, 137] ZOrder 639 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { Name "Kd*(jointVel_des - jointVel)" @@ -3841,6 +3879,11 @@ Model { 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 { @@ -3877,6 +3920,7 @@ Model { Position [590, 93, 620, 107] ZOrder 680 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -3975,6 +4019,11 @@ Model { 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 { @@ -4018,6 +4067,7 @@ Model { Position [280, -337, 310, -323] ZOrder 663 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4027,6 +4077,7 @@ Model { ZOrder 657 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4036,6 +4087,7 @@ Model { ZOrder 992 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -4157,6 +4209,7 @@ Model { Position [610, 38, 640, 52] ZOrder 388 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 422 @@ -4231,6 +4284,7 @@ Model { Position [700, 113, 730, 127] ZOrder 638 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 396 @@ -4316,6 +4370,11 @@ Model { 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" @@ -4324,10 +4383,10 @@ Model { Block { BlockType SubSystem Name "emergency stop: joint limits" - SID "470" + SID "687" Ports [1] - Position [300, 250, 405, 270] - ZOrder 636 + Position [300, 256, 400, 274] + ZOrder 969 BackgroundColor "red" ShowName off RequestExecContextInheritance off @@ -4340,7 +4399,7 @@ Model { } System { Name "emergency stop: joint limits" - Location [134, 55, 3840, 2160] + Location [67, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -4353,11 +4412,11 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "904" + ZoomFactor "401" Block { BlockType Inport Name "jointPos" - SID "471" + SID "688" Position [150, 238, 180, 252] ZOrder -1 IconDisplay "Port number" @@ -4365,7 +4424,7 @@ Model { Block { BlockType Constant Name "ON_GAZEBO\n1" - SID "472" + SID "689" Position [232, 190, 488, 210] ZOrder 553 BlockRotation 270 @@ -4377,7 +4436,7 @@ Model { Block { BlockType Constant Name "ON_GAZEBO\n2" - SID "473" + SID "690" Position [224, 295, 496, 315] ZOrder 555 BlockRotation 270 @@ -4389,14 +4448,14 @@ Model { Block { BlockType SubSystem Name "STOP IF JOINTS HIT THE LIMITS" - SID "474" + SID "691" Ports [1, 0, 1] Position [285, 229, 440, 261] ZOrder 554 RequestExecContextInheritance off System { Name "STOP IF JOINTS HIT THE LIMITS" - Location [134, 55, 3840, 2160] + Location [67, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -4409,11 +4468,11 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "922" + ZoomFactor "433" Block { BlockType Inport Name "jointPos" - SID "475" + SID "692" Position [60, 103, 90, 117] ZOrder 552 IconDisplay "Port number" @@ -4421,31 +4480,39 @@ Model { Block { BlockType EnablePort Name "Enable" - SID "476" + SID "693" Ports [] - Position [227, -30, 247, -10] + Position [227, -20, 247, 0] ZOrder 553 + ShowName off + HideAutomaticName off } Block { BlockType Assertion Name "Assertion" - SID "477" - Position [340, 72, 400, 118] + SID "694" + Position [360, 37, 420, 83] ZOrder 207 + HideAutomaticName off } Block { BlockType Reference Name "GetLimits" - SID "478" + SID "695" Ports [0, 2] - Position [20, 23, 140, 92] - ZOrder 551 + 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" @@ -4453,8 +4520,8 @@ Model { Block { BlockType SubSystem Name "MATLAB Function" - SID "479" - Ports [4, 1] + SID "696" + Ports [4, 2] Position [180, 22, 300, 163] ZOrder 205 ShowName off @@ -4480,11 +4547,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3782" + SIDHighWatermark "3792" Block { BlockType Inport Name "umin" - SID "479::18" + SID "696::18" Position [20, 101, 40, 119] ZOrder -1 IconDisplay "Port number" @@ -4492,7 +4559,7 @@ Model { Block { BlockType Inport Name "umax" - SID "479::19" + SID "696::19" Position [20, 136, 40, 154] ZOrder -2 Port "2" @@ -4501,7 +4568,7 @@ Model { Block { BlockType Inport Name "u" - SID "479::1" + SID "696::1" Position [20, 171, 40, 189] ZOrder -3 Port "3" @@ -4510,7 +4577,7 @@ Model { Block { BlockType Inport Name "tol" - SID "479::20" + SID "696::20" Position [20, 206, 40, 224] ZOrder -4 Port "4" @@ -4519,22 +4586,22 @@ Model { Block { BlockType Demux Name " Demux " - SID "479::3781" + SID "696::3791" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 131 + ZOrder 141 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "479::3780" + SID "696::3790" Tag "Stateflow S-Function jointsControl 18" - Ports [4, 2] + Ports [4, 3] Position [180, 102, 230, 203] - ZOrder 130 + ZOrder 140 FunctionName "sf_sfun" - PortCounts "[4 2]" + PortCounts "[4 3]" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -4542,45 +4609,59 @@ Model { PortNumber 2 Name "inRange" } + Port { + PortNumber 3 + Name "res_check_range" + } } Block { BlockType Terminator Name " Terminator " - SID "479::3782" + SID "696::3792" Position [460, 241, 480, 259] - ZOrder 132 + ZOrder 142 } Block { BlockType Outport Name "inRange" - SID "479::5" + SID "696::5" Position [460, 101, 480, 119] ZOrder -8 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "res_check_range" + SID "696::3789" + Position [460, 136, 480, 154] + ZOrder 139 + Port "2" + IconDisplay "Port number" } Line { - ZOrder 29 + ZOrder 9 SrcBlock "umin" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 30 + ZOrder 10 SrcBlock "umax" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 31 + ZOrder 11 SrcBlock "u" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 32 + ZOrder 12 SrcBlock "tol" SrcPort 1 DstBlock " SFunction " @@ -4588,7 +4669,7 @@ Model { } Line { Name "inRange" - ZOrder 33 + ZOrder 13 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -4596,26 +4677,63 @@ Model { DstPort 1 } Line { - ZOrder 34 + Name "res_check_range" + ZOrder 14 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "res_check_range" + DstPort 1 + } + Line { + ZOrder 15 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 35 + ZOrder 16 SrcBlock " SFunction " SrcPort 1 + Points [20, 0] DstBlock " Demux " DstPort 1 } } } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "697" + 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 "698" + Position [320, 48, 340, 72] + ZOrder 559 + ShowName off + HideAutomaticName off + InitialCondition "1" + SampleTime "Config.tStep" + HasFrameUpgradeWarning on + } Block { BlockType Constant Name "index1" - SID "480" - Position [70, 140, 95, 150] + SID "699" + Position [55, 137, 95, 153] ZOrder 204 ShowName off Value "0.01" @@ -4632,43 +4750,57 @@ Model { ZOrder 2 SrcBlock "MATLAB Function" SrcPort 1 - DstBlock "Assertion" + DstBlock "Unit Delay" DstPort 1 } Line { ZOrder 3 - SrcBlock "GetLimits" + SrcBlock "index1" SrcPort 1 DstBlock "MATLAB Function" - DstPort 1 + DstPort 4 } Line { ZOrder 4 - SrcBlock "GetLimits" + SrcBlock "MATLAB Function" SrcPort 2 - DstBlock "MATLAB Function" - DstPort 2 + DstBlock "To Workspace" + DstPort 1 } Line { ZOrder 5 - SrcBlock "index1" + SrcBlock "Unit Delay" + SrcPort 1 + DstBlock "Assertion" + DstPort 1 + } + Line { + ZOrder 6 + SrcBlock "GetLimits" SrcPort 1 DstBlock "MATLAB Function" - DstPort 4 + DstPort 1 + } + Line { + ZOrder 7 + SrcBlock "GetLimits" + SrcPort 2 + DstBlock "MATLAB Function" + DstPort 2 } } } Block { BlockType SubSystem Name "STOP IF THERE ARE SPIKES IN THE ENCODERS" - SID "481" + SID "700" Ports [1, 0, 1] Position [285, 339, 440, 371] ZOrder 556 RequestExecContextInheritance off System { Name "STOP IF THERE ARE SPIKES IN THE ENCODERS" - Location [134, 55, 3840, 2160] + Location [67, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -4681,36 +4813,39 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "827" + ZoomFactor "380" Block { BlockType Inport Name "jointPos" - SID "482" - Position [15, 53, 45, 67] + SID "701" + Position [-10, 53, 20, 67] ZOrder 552 IconDisplay "Port number" } Block { BlockType EnablePort Name "Enable" - SID "483" + SID "702" Ports [] - Position [217, -25, 237, -5] + Position [172, -15, 192, 5] ZOrder 553 + ShowName off + HideAutomaticName off } Block { BlockType Assertion Name "Assertion" - SID "484" - Position [340, 72, 400, 118] - ZOrder 207 + SID "703" + Position [335, 37, 400, 83] + ZOrder 556 + HideAutomaticName off } Block { BlockType SubSystem Name "MATLAB Function" - SID "485" - Ports [2, 1] - Position [165, 24, 285, 166] + SID "704" + Ports [2, 2] + Position [105, 26, 260, 164] ZOrder 205 ShowName off LibraryVersion "1.32" @@ -4735,11 +4870,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3782" + SIDHighWatermark "3792" Block { BlockType Inport Name "u" - SID "485::1" + SID "704::1" Position [20, 101, 40, 119] ZOrder -3 IconDisplay "Port number" @@ -4747,7 +4882,7 @@ Model { Block { BlockType Inport Name "delta_u_max" - SID "485::18" + SID "704::18" Position [20, 136, 40, 154] ZOrder -1 Port "2" @@ -4756,22 +4891,22 @@ Model { Block { BlockType Demux Name " Demux " - SID "485::3781" + SID "704::3791" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 131 + ZOrder 141 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "485::3780" + SID "704::3790" Tag "Stateflow S-Function jointsControl 14" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 130 + Ports [2, 3] + Position [180, 100, 230, 180] + ZOrder 140 FunctionName "sf_sfun" - PortCounts "[2 2]" + PortCounts "[2 3]" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -4779,24 +4914,38 @@ Model { PortNumber 2 Name "noSpikes" } + Port { + PortNumber 3 + Name "res_check_spikes" + } } Block { BlockType Terminator Name " Terminator " - SID "485::3782" + SID "704::3792" Position [460, 241, 480, 259] - ZOrder 132 + ZOrder 142 } Block { BlockType Outport Name "noSpikes" - SID "485::5" + SID "704::5" Position [460, 101, 480, 119] ZOrder -8 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "res_check_spikes" + SID "704::3789" + Position [460, 136, 480, 154] + ZOrder 139 + Port "2" + IconDisplay "Port number" } Line { - ZOrder 21 + ZOrder 7 SrcBlock "u" SrcPort 1 Points [120, 0] @@ -4804,7 +4953,7 @@ Model { DstPort 1 } Line { - ZOrder 22 + ZOrder 8 SrcBlock "delta_u_max" SrcPort 1 DstBlock " SFunction " @@ -4812,7 +4961,7 @@ Model { } Line { Name "noSpikes" - ZOrder 23 + ZOrder 9 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -4820,14 +4969,23 @@ Model { DstPort 1 } Line { - ZOrder 24 + Name "res_check_spikes" + ZOrder 10 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "res_check_spikes" + DstPort 1 + } + Line { + ZOrder 11 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 25 + ZOrder 12 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -4835,11 +4993,38 @@ Model { } } } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "705" + 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 "706" + Position [290, 48, 310, 72] + ZOrder 558 + ShowName off + HideAutomaticName off + InitialCondition "1" + SampleTime "Config.tStep" + HasFrameUpgradeWarning on + } Block { BlockType Constant Name "index1" - SID "486" - Position [-40, 124, 100, 136] + SID "707" + Position [-65, 121, 75, 139] ZOrder 554 ShowName off Value "Sat.maxJointsPositionDelta" @@ -4856,7 +5041,7 @@ Model { ZOrder 2 SrcBlock "MATLAB Function" SrcPort 1 - DstBlock "Assertion" + DstBlock "Unit Delay" DstPort 1 } Line { @@ -4866,6 +5051,20 @@ Model { DstBlock "MATLAB Function" DstPort 2 } + Line { + ZOrder 4 + SrcBlock "MATLAB Function" + SrcPort 2 + DstBlock "To Workspace" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "Unit Delay" + SrcPort 1 + DstBlock "Assertion" + DstPort 1 + } } } Line { @@ -4979,7 +5178,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" SourceType "Simulator Synchronizer" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" period "Config.tStep" serverPortName "'/clock/rpc'" clientPortName "'/WBT_synchronizer/rpc:o'" @@ -5057,7 +5255,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" period "Config.tStep" } } @@ -5074,7 +5271,6 @@ Model { VariableName "yarp_time" MaxDataPoints "inf" SaveFormat "Structure With Time" - Save2DSignal "3-D array (concatenate along third dimension)" FixptAsFi on SampleTime "-1" } @@ -5092,7 +5288,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Yarp Clock" SourceType "YARP Clock" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" } Line { ZOrder 1 @@ -5133,15 +5328,15 @@ Model { SrcPort 1 Points [24, 0] Branch { - ZOrder 413 - Points [0, 20] - DstBlock "Goto" + ZOrder 431 + Points [0, -25] + DstBlock "emergency stop: joint limits" DstPort 1 } Branch { - ZOrder 412 - Points [0, -30] - DstBlock "emergency stop: joint limits" + ZOrder 413 + Points [0, 20] + DstBlock "Goto" DstPort 1 } Branch { @@ -5175,17 +5370,17 @@ Model { } #Finite State Machines # -# Stateflow 80000014 +# Stateflow 80000019 # # Stateflow { machine { id 1 name "jointsControl" - created "12-Mar-2015 18:23:09" + created "19-Nov-2014 19:38:03" isLibrary 0 sfVersion 80000012 - firstTarget 20 + firstTarget 22 } chart { id 2 @@ -5196,18 +5391,18 @@ Stateflow { screen [1 1 1366 768 1.25] treeNode [0 3 0 0] viewObj 2 - ssIdHighWaterMark 8 + ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART chartFileNumber 14 disableImplicitCasting 1 eml { - name "checkSpikes" + name "checkInputSpikesFCN" } supportVariableSizing 0 firstData 4 - firstTransition 8 - firstJunction 7 + firstTransition 9 + firstJunction 8 } state { id 3 @@ -5223,8 +5418,8 @@ Stateflow { decomposition CLUSTER_STATE eml { isEML 1 - script "function noSpikes = checkSpikes(u, delta_u_max)\n\n noSpikes = wbc.checkSpikes(u, delta_u_max" - ");\nend" + 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)" @@ -5300,10 +5495,35 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [2 5 0] + linkNode [2 5 7] } - junction { + data { id 7 + 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 [2 6 0] + } + junction { + id 8 position [23.5747 49.5747 7] chart 2 subviewer 2 @@ -5312,7 +5532,7 @@ Stateflow { linkNode [2 0 0] } transition { - id 8 + id 9 labelString "{eML_blk_kernel();}" labelPosition [40.125 31.875 102.544 14.964] fontSize 12 @@ -5320,7 +5540,7 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 7 + id 8 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] @@ -5336,49 +5556,49 @@ Stateflow { linkNode [2 0 0] } instance { - id 9 + id 10 machine 1 name "emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/MATLAB Function" chart 2 } chart { - id 10 + id 11 machine 1 name "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 11 0 0] - viewObj 10 - ssIdHighWaterMark 8 + treeNode [0 12 0 0] + viewObj 11 + ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART chartFileNumber 18 disableImplicitCasting 1 eml { - name "checkRangeFCN" + name "checkInputRangeFCN" } supportVariableSizing 0 - firstData 12 - firstTransition 18 - firstJunction 17 + firstData 13 + firstTransition 20 + firstJunction 19 } state { - id 11 + id 12 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 10 - treeNode [10 0 0 0] + chart 11 + treeNode [11 0 0 0] superState SUBCHART - subviewer 10 + subviewer 11 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE eml { isEML 1 - script "function inRange = checkRangeFCN(umin, umax, u, tol)\n\n inRange = wbc.checkRange(umin, umax," - " u, tol);\nend" + 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)" @@ -5387,7 +5607,7 @@ Stateflow { } } data { - id 12 + id 13 ssIdNumber 4 name "umin" scope INPUT_DATA @@ -5409,10 +5629,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [10 0 13] + linkNode [11 0 14] } data { - id 13 + id 14 ssIdNumber 5 name "umax" scope INPUT_DATA @@ -5434,10 +5654,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [10 12 14] + linkNode [11 13 15] } data { - id 14 + id 15 ssIdNumber 6 name "u" scope INPUT_DATA @@ -5456,10 +5676,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [10 13 15] + linkNode [11 14 16] } data { - id 15 + id 16 ssIdNumber 7 name "inRange" scope OUTPUT_DATA @@ -5479,10 +5699,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [10 14 16] + linkNode [11 15 17] } data { - id 16 + id 17 ssIdNumber 8 name "tol" scope INPUT_DATA @@ -5504,19 +5724,44 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [10 15 0] + linkNode [11 16 18] + } + data { + id 18 + 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 [11 17 0] } junction { - id 17 + id 19 position [23.5747 49.5747 7] - chart 10 - subviewer 10 + chart 11 + subviewer 11 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [10 0 0] + linkNode [11 0 0] } transition { - id 18 + id 20 labelString "{eML_blk_kernel();}" labelPosition [40.125 31.875 102.544 14.964] fontSize 12 @@ -5524,29 +5769,29 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 17 + id 19 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 10 + chart 11 dataLimits [21.175 25.975 14.625 42.575] - subviewer 10 + subviewer 11 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [10 0 0] + linkNode [11 0 0] } instance { - id 19 + id 21 machine 1 name "emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" - chart 10 + chart 11 } target { - id 20 + id 22 machine 1 name "sfun" description "Default Simulink S-Function Target." diff --git a/controllers/fixed-base-joints-control/src-static-gui/closeModel.m b/controllers/fixed-base-joints-torque-control/src-static-gui/closeModel.m similarity index 100% rename from controllers/fixed-base-joints-control/src-static-gui/closeModel.m rename to controllers/fixed-base-joints-torque-control/src-static-gui/closeModel.m diff --git a/controllers/fixed-base-joints-control/src-static-gui/compileModel.m b/controllers/fixed-base-joints-torque-control/src-static-gui/compileModel.m similarity index 100% rename from controllers/fixed-base-joints-control/src-static-gui/compileModel.m rename to controllers/fixed-base-joints-torque-control/src-static-gui/compileModel.m diff --git a/controllers/fixed-base-joints-control/startModelWithStaticGui.m b/controllers/fixed-base-joints-torque-control/startModelWithStaticGui.m similarity index 100% rename from controllers/fixed-base-joints-control/startModelWithStaticGui.m rename to controllers/fixed-base-joints-torque-control/startModelWithStaticGui.m diff --git a/controllers/fixed-base-joints-control/stopJointsControl.m b/controllers/fixed-base-joints-torque-control/stopJointsControl.m similarity index 66% rename from controllers/fixed-base-joints-control/stopJointsControl.m rename to controllers/fixed-base-joints-torque-control/stopJointsControl.m index e4b9935..d033f72 100644 --- a/controllers/fixed-base-joints-control/stopJointsControl.m +++ b/controllers/fixed-base-joints-torque-control/stopJointsControl.m @@ -56,4 +56,37 @@ warning(['The time step tolerance of ', num2str(Config.tStep), '[s] has been violated more than 50 times.']) end -end \ No newline at end of file +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-position-control/README.md b/controllers/floating-base-balancing-position-control/README.md index 52aef16..720998c 100644 --- a/controllers/floating-base-balancing-position-control/README.md +++ b/controllers/floating-base-balancing-position-control/README.md @@ -2,6 +2,8 @@ This module implements a position control balancing strategy. The robot is assumed to balance on one or two feet. The control input is a desired reference trajectory for the robot joints obtained by means of an integration-based, stack of tasks inverse kinematics. + + ### Compatibility The folder contains the Simulink model `positionControlBalancing.mdl`, which is generated by using Matlab R2017b. @@ -12,10 +14,14 @@ Currently, supported robots are: `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 `initPositionControlBalancing.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`. \ No newline at end of file +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-position-control/positionControlBalancing.mdl b/controllers/floating-base-balancing-position-control/positionControlBalancing.mdl index 13b6ef0..a890421 100644 --- a/controllers/floating-base-balancing-position-control/positionControlBalancing.mdl +++ b/controllers/floating-base-balancing-position-control/positionControlBalancing.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.3504" + ComputedModelVersion "1.3513" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -180,7 +180,7 @@ Model { ExternalFileReference { Reference "WBToolboxLibrary/States/GetLimits" Path "positionControlBalancing/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/GetLimits" - SID "2432" + SID "5196" Type "LIBRARY_BLOCK" } ExternalFileReference { @@ -203,13 +203,9 @@ Model { } OrderedModelArguments 1 } - DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" - LogicAnalyzerGraphicalSettings "" - LogicAnalyzerPlugin "on" - LogicAnalyzerSignalOrdering "" - CustomCodeFunctionData "" SLCCPlugin "on" + LogicAnalyzerPlugin "on" + DiagnosticSuppressor "on" PostLoadFcn "%% Try to open the static GUI\ntry\n\n staticGUI = simulinkStaticGUI;\n\nend" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on @@ -230,10 +226,12 @@ Model { "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" StopFcn "%% stop the simulation\ncd(fileparts(which(bdroot)));\nstopPositionControlBalancing;\n\n%% Try to edit t" - "he GUI (the user may have already closed it)\ntry\n\n %% Update Compile Button\n set(sl_synch_handles.compileB" - "utton,'Backgroundcolor',[1.0,0.6,0.0]);\n set(sl_synch_handles.compileButton,'Enable','on');\n\n %% Update Exi" - "t 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" + "he GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n if get(sl_synch_handles.checkbo" + "x_recompile, 'Value') \n set(sl_synch_handles.startButton,'Backgroundcolor',[0.0,1.0,0.0]);\n set(sl_" + "synch_handles.startButton,'Enable','on');\n end\n\n %% Update Compile Button\n set(sl_synch_handles.compile" + "Button,'Backgroundcolor',[1.0,0.6,0.0]);\n set(sl_synch_handles.compileButton,'Enable','on');\n\n %% Update Ex" + "it Button\n set(sl_synch_handles.exitButton,'Backgroundcolor',[0.0,1.0,1.0]);\n set(sl_synch_handles.exitButto" + "n,'Enable','on');\n\nend\n" LastSavedArchitecture "glnxa64" Object { $PropName "BdWindowsInfo" @@ -268,7 +266,7 @@ Model { IsActive [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [1815.0, 876.0] + Extents [1815.0, 821.0] ZoomFactor [3.028813559322034] Offset [850.37772803581424, -288.61108002238387] } @@ -290,11 +288,7 @@ Model { "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABiAP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAALAD///8AAAc9AAADqgAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACA/////wAAAAAAAAAA/////wEAAADo/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAFo/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAANR/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" + "QBjAHQAbwByAAAAAAD/////AAAAOQD///8AAAc9AAADcgAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAA" } } HideAutomaticNames on @@ -304,9 +298,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Mar 08 16:25:39 2019" - RTWModifiedTimeStamp 473963138 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Mar 23 00:04:24 2020" + RTWModifiedTimeStamp 506822663 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -341,7 +335,6 @@ Model { FunctionConnectors off BrowserLookUnderMasks on SimulationMode "normal" - VisualizeLoggedSignalsWhenLoggingToFile off PauseTimes "5" NumberOfSteps 1 SnapshotBufferSize 10 @@ -414,6 +407,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] StartTime "0.0" StopTime "Config.SIMULATION_TIME " AbsTol "auto" @@ -453,6 +447,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] Decimation "1" ExternalInput "[t, u]" FinalStateName "xFinal" @@ -494,7 +489,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 8 + Dimension 9 Cell "BooleansAsBitfields" Cell "PassReuseOutputArgsAs" Cell "PassReuseOutputArgsThreshold" @@ -503,9 +498,11 @@ Model { Cell "OptimizeModelRefInitCode" Cell "NoFixptDivByZeroProtection" Cell "UseSpecifiedMinMax" + Cell "EfficientTunableParamExpr" PropName "DisabledProps" } Description "" + Components [] BlockReduction on BooleanDataType on ConditionallyExecuteInputs on @@ -561,6 +558,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] RTPrefix "error" ConsistencyChecking "none" ArrayBoundsChecking "none" @@ -671,6 +669,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] ProdBitPerChar 8 ProdBitPerShort 16 ProdBitPerInt 32 @@ -719,6 +718,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" EnableRefExpFcnMdlSchedulingChecks on CheckModelReferenceTargetMessage "error" @@ -738,6 +738,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] SimCustomSourceCode "" SimCustomHeaderCode "" SimCustomInitializer "" @@ -787,6 +788,7 @@ Model { Cell "GenerateMissedCodeReplacementReport" PropName "DisabledProps" } + Description "" SystemTargetFile "grt.tlc" HardwareBoard "None" TLCOptions "" @@ -797,9 +799,7 @@ Model { PackageName "" TemplateMakefile "grt_default_tmf" PostCodeGenCommand "" - Description "" GenerateReport off - SaveLog off RTWVerbose on RetainRTWFile off RTWBuildHooks [] @@ -887,6 +887,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] Comment "" ForceParamTrailComments off GenerateComments on @@ -942,7 +943,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 17 + Dimension 18 Cell "GeneratePreprocessorConditionals" Cell "IncludeMdlTerminateFcn" Cell "GenerateAllocFcn" @@ -960,6 +961,7 @@ Model { Cell "RemoveResetFunc" Cell "ExistingSharedCode" Cell "RemoveDisableFunc" + Cell "PreserveStateflowLocalDataDimensions" PropName "DisabledProps" } Description "" @@ -968,7 +970,7 @@ Model { Dimension 1 Simulink.CPPComponent { $ObjectID 19 - Version "1.18.0" + Version "19.1.1" Array { Type "Cell" Dimension 10 @@ -985,6 +987,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] Name "CPPClassGenComp" GenerateDestructor on GenerateExternalIOAccessMethods "None" @@ -1074,6 +1077,7 @@ Model { Version "1.17.1" DisabledProps [] Description "Simulink Coverage Configuration Component" + Components [] Name "Simulink Coverage" CovEnable off CovScope "EntireSystem" @@ -1112,9 +1116,9 @@ Model { PropName "Components" } Name "Configuration" - ExtraOptions "-aBlockCommentType=\"BlockPathComment\" -aIgnoreTestpoints=0 " CurrentDlgPage "Solver" ConfigPrmDlgPosition [ 730, 330, 2252, 1200 ] + ExtraOptions "-aBlockCommentType=\"BlockPathComment\" -aIgnoreTestpoints=0 " } PropName "ConfigurationSets" } @@ -1130,7 +1134,6 @@ Model { DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" DefaultExtrapolationMethodBetweenContTasks "None" - AutoInsertRateTranBlk [0] } ExplicitPartitioning off BlockDefaults { @@ -1351,7 +1354,7 @@ Model { MustResolveToSignalObject off OutputWhenUnConnected off OutputWhenUnconnectedValue "0" - VectorParamsAs1DForOutWhenUnconnected off + VectorParamsAs1DForOutWhenUnconnected on } Block { BlockType Product @@ -1423,7 +1426,6 @@ Model { TreatAsGroupedWhenPropagatingVariantConditions on ContentPreviewEnabled off IsWebBlock off - IsObserver off } Block { BlockType Sum @@ -1465,11 +1467,20 @@ Model { MaxDataPoints "1000" Decimation "1" SaveFormat "Array" - Save2DSignal "Inherit from input (this choice will be removed - see release notes)" + 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 "positionControlBalancing" @@ -1488,7 +1499,7 @@ Model { ShowPageBoundaries off ZoomFactor "303" ReportName "simulink-default.rpt" - SIDHighWatermark "5149" + SIDHighWatermark "5208" Block { BlockType Reference Name "Configuration" @@ -1501,6 +1512,11 @@ Model { 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'" @@ -2031,7 +2047,7 @@ Model { " 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'),'TimeRangeSamples','14'" ",'TimeRangeFrames','14','DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop'," "false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','201" - "8a','Location',[1170 722 2470 1321])" + "9b','Location',[1170 722 2470 1321])" NumInputPorts "1" Floating off } @@ -2111,7 +2127,7 @@ Model { "none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" "t',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[5 1]),extmgr.Configuration('Tools'," "'Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',tru" - "e,'Version','2018a')),'Version','2018a','Location',[135 169 3841 2159])" + "e,'Version','2018a')),'Version','2019b','Location',[135 169 3841 2159])" NumInputPorts "5" Floating off } @@ -2191,7 +2207,7 @@ Model { "ne','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" ",1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[5 1]),extmgr.Configuration('Tools','P" "lot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true," - "'Version','2018a')),'Version','2018a','Location',[135 207 3841 2105])" + "'Version','2018a')),'Version','2019b','Location',[135 207 3841 2105])" NumInputPorts "5" Floating off } @@ -2271,7 +2287,7 @@ Model { "ible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'Tim" "eRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[5 1]),extmgr.Configuration('Tools','Plot Navi" "gation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version" - "','2018a')),'Version','2018a','Location',[135 169 3841 2159])" + "','2018a')),'Version','2019b','Location',[135 169 3841 2159])" NumInputPorts "5" Floating off } @@ -2351,7 +2367,7 @@ Model { "e','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRa" "ngeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[5 1]),extmgr.Configuration('Tools','Plot Navigat" "ion',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','" - "2018a')),'Version','2018a','Location',[1170 722 2460 1367])" + "2018a')),'Version','2019b','Location',[1170 722 2460 1367])" NumInputPorts "5" Floating off } @@ -2431,7 +2447,7 @@ Model { "isible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'T" "imeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[5 1]),extmgr.Configuration('Tools','Plot Na" "vigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Versi" - "on','2018a')),'Version','2018a','Location',[135 223 3841 2121])" + "on','2018a')),'Version','2019b','Location',[135 223 3841 2121])" NumInputPorts "5" Floating off } @@ -2511,7 +2527,7 @@ Model { ",'none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placem" "ent',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[5 1]),extmgr.Configuration('Tools" "','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',t" - "rue,'Version','2018a')),'Version','2018a','Location',[353 460 1633 847])" + "rue,'Version','2018a')),'Version','2019b','Location',[353 460 1633 847])" NumInputPorts "5" Floating off } @@ -2591,7 +2607,7 @@ Model { ",'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1)" ",'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[5 1]),extmgr.Configuration('Tools','Plot" " Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Ve" - "rsion','2018a')),'Version','2018a','Location',[135 169 3841 2159])" + "rsion','2018a')),'Version','2019b','Location',[135 169 3841 2159])" NumInputPorts "5" Floating off } @@ -3125,6 +3141,11 @@ Model { 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" } @@ -3140,6 +3161,11 @@ Model { 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" } @@ -3598,7 +3624,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "1554" + SIDHighWatermark "1569" Block { BlockType Inport Name "pos_l_sole_error" @@ -3772,20 +3798,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4935::1553" + SID "4935::1568" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 126 + ZOrder 141 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4935::1552" + SID "4935::1567" Tag "Stateflow S-Function positionControlBalancing 29" Ports [19, 2] Position [180, 75, 230, 475] - ZOrder 125 + ZOrder 140 FunctionName "sf_sfun" Parameters "Config" PortCounts "[19 2]" @@ -3800,9 +3826,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4935::1554" + SID "4935::1569" Position [460, 241, 480, 259] - ZOrder 127 + ZOrder 142 } Block { BlockType Outport @@ -3811,135 +3837,136 @@ Model { Position [460, 101, 480, 119] ZOrder 31 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 322 + ZOrder 432 SrcBlock "pos_l_sole_error" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 323 + ZOrder 433 SrcBlock "pos_r_sole_error" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 324 + ZOrder 434 SrcBlock "rot_r_sole_error" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 325 + ZOrder 435 SrcBlock "rot_l_sole_error" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 326 + ZOrder 436 SrcBlock "posCoM" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 327 + ZOrder 437 SrcBlock "jointPos" SrcPort 1 DstBlock " SFunction " DstPort 6 } Line { - ZOrder 328 + ZOrder 438 SrcBlock "desired_pos_vel_acc_CoM" SrcPort 1 DstBlock " SFunction " DstPort 7 } Line { - ZOrder 329 + ZOrder 439 SrcBlock "desired_pos_vel_acc_joints" SrcPort 1 DstBlock " SFunction " DstPort 8 } Line { - ZOrder 330 + ZOrder 440 SrcBlock "vel_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 9 } Line { - ZOrder 331 + ZOrder 441 SrcBlock "vel_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 10 } Line { - ZOrder 332 + ZOrder 442 SrcBlock "velCoM" SrcPort 1 DstBlock " SFunction " DstPort 11 } Line { - ZOrder 333 + ZOrder 443 SrcBlock "jointVel" SrcPort 1 DstBlock " SFunction " DstPort 12 } Line { - ZOrder 334 + ZOrder 444 SrcBlock "J_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 13 } Line { - ZOrder 335 + ZOrder 445 SrcBlock "J_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 14 } Line { - ZOrder 336 + ZOrder 446 SrcBlock "J_CoM" SrcPort 1 DstBlock " SFunction " DstPort 15 } Line { - ZOrder 337 + ZOrder 447 SrcBlock "JDot_l_sole_nu" SrcPort 1 DstBlock " SFunction " DstPort 16 } Line { - ZOrder 338 + ZOrder 448 SrcBlock "JDot_r_sole_nu" SrcPort 1 DstBlock " SFunction " DstPort 17 } Line { - ZOrder 339 + ZOrder 449 SrcBlock "JDot_CoM_nu" SrcPort 1 DstBlock " SFunction " DstPort 18 } Line { - ZOrder 340 + ZOrder 450 SrcBlock "feetcontactStatus" SrcPort 1 DstBlock " SFunction " @@ -3947,7 +3974,7 @@ Model { } Line { Name "nuDot_ikin" - ZOrder 341 + ZOrder 451 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -3955,14 +3982,14 @@ Model { DstPort 1 } Line { - ZOrder 342 + ZOrder 452 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 343 + ZOrder 453 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -3977,6 +4004,7 @@ Model { Position [1345, 168, 1375, 182] ZOrder 769 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 3 @@ -4418,7 +4446,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "48" + SIDHighWatermark "63" Block { BlockType Inport Name "w_H_l_sole" @@ -4466,20 +4494,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4946::47" + SID "4946::62" Ports [1, 1] Position [270, 315, 320, 355] - ZOrder 33 + ZOrder 48 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4946::46" + SID "4946::61" Tag "Stateflow S-Function positionControlBalancing 1" Ports [5, 6] Position [180, 102, 230, 243] - ZOrder 32 + ZOrder 47 FunctionName "sf_sfun" PortCounts "[5 6]" SFunctionDeploymentMode off @@ -4509,9 +4537,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4946::48" + SID "4946::63" Position [460, 326, 480, 344] - ZOrder 34 + ZOrder 49 } Block { BlockType Outport @@ -4520,6 +4548,7 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4529,6 +4558,7 @@ Model { ZOrder 13 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4538,6 +4568,7 @@ Model { ZOrder 14 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4547,6 +4578,7 @@ Model { ZOrder 15 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4556,37 +4588,38 @@ Model { ZOrder 16 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 37 + ZOrder 97 SrcBlock "w_H_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 38 + ZOrder 98 SrcBlock "w_H_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 39 + ZOrder 99 SrcBlock "w_H_CoM" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 40 + ZOrder 100 SrcBlock "w_H_r_sole_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 41 + ZOrder 101 SrcBlock "w_H_l_sole_0" SrcPort 1 DstBlock " SFunction " @@ -4594,7 +4627,7 @@ Model { } Line { Name "pos_l_sole_error" - ZOrder 42 + ZOrder 102 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -4603,7 +4636,7 @@ Model { } Line { Name "rot_l_sole_error" - ZOrder 43 + ZOrder 103 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -4612,7 +4645,7 @@ Model { } Line { Name "pos_r_sole_error" - ZOrder 44 + ZOrder 104 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -4621,7 +4654,7 @@ Model { } Line { Name "rot_r_sole_error" - ZOrder 45 + ZOrder 105 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -4630,7 +4663,7 @@ Model { } Line { Name "posCoM" - ZOrder 46 + ZOrder 106 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -4638,14 +4671,14 @@ Model { DstPort 1 } Line { - ZOrder 47 + ZOrder 107 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 48 + ZOrder 108 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -4694,6 +4727,11 @@ Model { 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" } @@ -4708,6 +4746,11 @@ Model { 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" } @@ -4722,6 +4765,11 @@ Model { 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" } @@ -4736,6 +4784,11 @@ Model { 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" } @@ -4750,6 +4803,11 @@ Model { 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" } @@ -4794,6 +4852,7 @@ Model { Position [1235, 633, 1265, 647] ZOrder 676 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4803,6 +4862,7 @@ Model { ZOrder 681 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4812,6 +4872,7 @@ Model { ZOrder 732 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4821,6 +4882,7 @@ Model { ZOrder 733 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4830,6 +4892,7 @@ Model { ZOrder 734 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -5121,6 +5184,11 @@ Model { 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.COM" } @@ -5135,6 +5203,11 @@ Model { 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" } @@ -5149,6 +5222,11 @@ Model { 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" } @@ -5163,6 +5241,11 @@ Model { 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" } @@ -5177,6 +5260,11 @@ Model { 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" } @@ -5191,6 +5279,11 @@ Model { 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" } @@ -5218,6 +5311,7 @@ Model { Position [775, -162, 805, -148] ZOrder 652 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -5227,6 +5321,7 @@ Model { ZOrder 653 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -5236,6 +5331,7 @@ Model { ZOrder 654 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -5245,6 +5341,7 @@ Model { ZOrder 721 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -5254,6 +5351,7 @@ Model { ZOrder 722 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -5263,6 +5361,7 @@ Model { ZOrder 723 Port "6" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -5492,6 +5591,7 @@ Model { Position [135, 233, 165, 247] ZOrder 715 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -5501,6 +5601,7 @@ Model { ZOrder 732 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 19 @@ -5728,7 +5829,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "45" + SIDHighWatermark "60" Block { BlockType Inport Name "nu_base_ikin" @@ -5749,20 +5850,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4999::44" + SID "4999::59" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 30 + ZOrder 45 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4999::43" + SID "4999::58" Tag "Stateflow S-Function positionControlBalancing 24" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 29 + ZOrder 44 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off @@ -5776,9 +5877,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4999::45" + SID "4999::60" Position [460, 241, 480, 259] - ZOrder 31 + ZOrder 46 } Block { BlockType Outport @@ -5787,9 +5888,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 16 + ZOrder 41 SrcBlock "nu_base_ikin" SrcPort 1 Points [120, 0] @@ -5797,7 +5899,7 @@ Model { DstPort 1 } Line { - ZOrder 17 + ZOrder 42 SrcBlock "pose_base" SrcPort 1 DstBlock " SFunction " @@ -5805,7 +5907,7 @@ Model { } Line { Name "pose_base_dot" - ZOrder 18 + ZOrder 43 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -5813,14 +5915,14 @@ Model { DstPort 1 } Line { - ZOrder 19 + ZOrder 44 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 20 + ZOrder 45 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -5890,7 +5992,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "43" + SIDHighWatermark "58" Block { BlockType Inport Name "q_ikin_quat" @@ -5902,20 +6004,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "5003::42" + SID "5003::57" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 28 + ZOrder 43 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "5003::41" + SID "5003::56" Tag "Stateflow S-Function positionControlBalancing 31" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 27 + ZOrder 42 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -5929,9 +6031,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "5003::43" + SID "5003::58" Position [460, 241, 480, 259] - ZOrder 29 + ZOrder 44 } Block { BlockType Outport @@ -5940,9 +6042,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 13 + ZOrder 33 SrcBlock "q_ikin_quat" SrcPort 1 DstBlock " SFunction " @@ -5950,7 +6053,7 @@ Model { } Line { Name "q_ikin" - ZOrder 14 + ZOrder 34 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -5958,14 +6061,14 @@ Model { DstPort 1 } Line { - ZOrder 15 + ZOrder 35 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 16 + ZOrder 36 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -6002,7 +6105,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "43" + SIDHighWatermark "58" Block { BlockType Inport Name "q_0" @@ -6014,20 +6117,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "5004::42" + SID "5004::57" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 28 + ZOrder 43 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "5004::41" + SID "5004::56" Tag "Stateflow S-Function positionControlBalancing 30" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 27 + ZOrder 42 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -6041,9 +6144,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "5004::43" + SID "5004::58" Position [460, 241, 480, 259] - ZOrder 29 + ZOrder 44 } Block { BlockType Outport @@ -6052,9 +6155,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 13 + ZOrder 33 SrcBlock "q_0" SrcPort 1 DstBlock " SFunction " @@ -6062,7 +6166,7 @@ Model { } Line { Name "q_0_quat" - ZOrder 14 + ZOrder 34 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -6070,14 +6174,14 @@ Model { DstPort 1 } Line { - ZOrder 15 + ZOrder 35 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 16 + ZOrder 36 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -6125,6 +6229,7 @@ Model { Position [350, -18, 385, -2] ZOrder 505 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -6134,6 +6239,7 @@ Model { ZOrder 502 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -6257,6 +6363,7 @@ Model { ZOrder 710 BlockMirror on IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -6267,6 +6374,7 @@ Model { BlockMirror on Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -6276,6 +6384,7 @@ Model { ZOrder 768 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 23 @@ -6549,7 +6658,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3740" + SIDHighWatermark "3755" Block { BlockType Inport Name "posCoM_0" @@ -6570,20 +6679,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4872::3739" + SID "4872::3754" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 142 + ZOrder 157 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4872::3738" + SID "4872::3753" Tag "Stateflow S-Function positionControlBalancing 10" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 141 + ZOrder 156 FunctionName "sf_sfun" Parameters "Config" PortCounts "[2 2]" @@ -6598,9 +6707,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4872::3740" + SID "4872::3755" Position [460, 241, 480, 259] - ZOrder 143 + ZOrder 158 } Block { BlockType Outport @@ -6609,9 +6718,10 @@ Model { Position [460, 101, 480, 119] ZOrder 31 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 16 + ZOrder 41 SrcBlock "posCoM_0" SrcPort 1 Points [120, 0] @@ -6619,7 +6729,7 @@ Model { DstPort 1 } Line { - ZOrder 17 + ZOrder 42 SrcBlock "t" SrcPort 1 DstBlock " SFunction " @@ -6627,7 +6737,7 @@ Model { } Line { Name "references_CoM" - ZOrder 18 + ZOrder 43 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -6635,14 +6745,14 @@ Model { DstPort 1 } Line { - ZOrder 19 + ZOrder 44 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 20 + ZOrder 45 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -6668,6 +6778,7 @@ Model { Position [630, 83, 660, 97] ZOrder 891 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -6818,7 +6929,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3765" + SIDHighWatermark "3780" Block { BlockType Inport Name "J_l_sole" @@ -6866,20 +6977,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4879::3764" + SID "4879::3779" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 133 + ZOrder 148 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4879::3763" + SID "4879::3778" Tag "Stateflow S-Function positionControlBalancing 5" Ports [5, 2] Position [180, 100, 230, 220] - ZOrder 132 + ZOrder 147 FunctionName "sf_sfun" PortCounts "[5 2]" SFunctionDeploymentMode off @@ -6893,9 +7004,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4879::3765" + SID "4879::3780" Position [460, 241, 480, 259] - ZOrder 134 + ZOrder 149 } Block { BlockType Outport @@ -6904,37 +7015,38 @@ Model { Position [460, 101, 480, 119] ZOrder -6 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 32 + ZOrder 72 SrcBlock "J_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 33 + ZOrder 73 SrcBlock "J_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 34 + ZOrder 74 SrcBlock "feetContactStatus" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 35 + ZOrder 75 SrcBlock "jointVel" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 36 + ZOrder 76 SrcBlock "pinvDampTolerance" SrcPort 1 DstBlock " SFunction " @@ -6942,7 +7054,7 @@ Model { } Line { Name "nu_b" - ZOrder 37 + ZOrder 77 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -6950,14 +7062,14 @@ Model { DstPort 1 } Line { - ZOrder 38 + ZOrder 78 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 39 + ZOrder 79 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -7025,6 +7137,11 @@ Model { 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" } @@ -7039,6 +7156,11 @@ Model { 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" } @@ -7049,6 +7171,7 @@ Model { Position [935, 498, 965, 512] ZOrder 676 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -7058,6 +7181,7 @@ Model { ZOrder 677 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -7127,6 +7251,7 @@ Model { Position [725, -67, 755, -53] ZOrder -15 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -7274,6 +7399,11 @@ Model { 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" } @@ -7298,6 +7428,7 @@ Model { Position [340, 253, 370, 267] ZOrder 583 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -7358,7 +7489,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "31" + SIDHighWatermark "46" Block { BlockType Inport Name "w_H_b" @@ -7379,20 +7510,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "5020::30" + SID "5020::45" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 21 + ZOrder 36 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "5020::29" + SID "5020::44" Tag "Stateflow S-Function positionControlBalancing 25" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 20 + ZOrder 35 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off @@ -7406,9 +7537,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "5020::31" + SID "5020::46" Position [460, 241, 480, 259] - ZOrder 22 + ZOrder 37 } Block { BlockType Outport @@ -7417,9 +7548,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 16 + ZOrder 41 SrcBlock "w_H_b" SrcPort 1 Points [120, 0] @@ -7427,7 +7559,7 @@ Model { DstPort 1 } Line { - ZOrder 17 + ZOrder 42 SrcBlock "jointPos" SrcPort 1 DstBlock " SFunction " @@ -7435,7 +7567,7 @@ Model { } Line { Name "q" - ZOrder 18 + ZOrder 43 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -7443,14 +7575,14 @@ Model { DstPort 1 } Line { - ZOrder 19 + ZOrder 44 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 20 + ZOrder 45 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -7567,6 +7699,11 @@ Model { 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" } @@ -7581,6 +7718,11 @@ Model { 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" } @@ -7613,6 +7755,7 @@ Model { Position [1450, -192, 1480, -178] ZOrder 904 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 3 @@ -7704,6 +7847,11 @@ Model { 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 on } Block { @@ -7719,6 +7867,11 @@ Model { 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 { @@ -7734,6 +7887,11 @@ Model { 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 { @@ -7748,6 +7906,11 @@ Model { 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 on } Block { @@ -7757,6 +7920,7 @@ Model { Position [340, 28, 370, 42] ZOrder 996 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -7766,6 +7930,7 @@ Model { ZOrder 991 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -7775,6 +7940,7 @@ Model { ZOrder 992 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -7784,6 +7950,7 @@ Model { ZOrder 989 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 306 @@ -7978,6 +8145,7 @@ Model { Position [175, -142, 205, -128] ZOrder 1005 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 334 @@ -8127,6 +8295,11 @@ Model { 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 on controlType "Position Direct" refSpeed "10*(pi/180)" @@ -8135,10 +8308,10 @@ Model { Block { BlockType SubSystem Name "emergency stop: joint limits" - SID "2416" + SID "5188" Ports [1] - Position [990, -197, 1095, -173] - ZOrder 960 + Position [990, -199, 1115, -171] + ZOrder 1001 BackgroundColor "red" ShowName off RequestExecContextInheritance off @@ -8151,7 +8324,7 @@ Model { } System { Name "emergency stop: joint limits" - Location [134, 55, 3840, 2160] + Location [67, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -8164,11 +8337,11 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "904" + ZoomFactor "401" Block { BlockType Inport Name "jointPos" - SID "2419" + SID "5189" Position [150, 238, 180, 252] ZOrder -1 IconDisplay "Port number" @@ -8176,7 +8349,7 @@ Model { Block { BlockType Constant Name "ON_GAZEBO\n1" - SID "4660" + SID "5190" Position [232, 190, 488, 210] ZOrder 553 BlockRotation 270 @@ -8188,7 +8361,7 @@ Model { Block { BlockType Constant Name "ON_GAZEBO\n2" - SID "4663" + SID "5191" Position [224, 295, 496, 315] ZOrder 555 BlockRotation 270 @@ -8200,14 +8373,14 @@ Model { Block { BlockType SubSystem Name "STOP IF JOINTS HIT THE LIMITS" - SID "4661" + SID "5192" Ports [1, 0, 1] Position [285, 229, 440, 261] ZOrder 554 RequestExecContextInheritance off System { Name "STOP IF JOINTS HIT THE LIMITS" - Location [134, 55, 3840, 2160] + Location [67, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -8220,11 +8393,11 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "899" + ZoomFactor "433" Block { BlockType Inport Name "jointPos" - SID "4662" + SID "5193" Position [60, 103, 90, 117] ZOrder 552 IconDisplay "Port number" @@ -8232,9 +8405,9 @@ Model { Block { BlockType EnablePort Name "Enable" - SID "3207" + SID "5194" Ports [] - Position [227, -35, 247, -15] + Position [227, -20, 247, 0] ZOrder 553 ShowName off HideAutomaticName off @@ -8242,24 +8415,29 @@ Model { Block { BlockType Assertion Name "Assertion" - SID "2420" - Position [340, 72, 400, 118] + SID "5195" + Position [360, 37, 420, 83] ZOrder 207 HideAutomaticName off } Block { BlockType Reference Name "GetLimits" - SID "2432" + SID "5196" Ports [0, 2] Position [15, 23, 135, 92] - ZOrder 551 + 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" @@ -8267,8 +8445,8 @@ Model { Block { BlockType SubSystem Name "MATLAB Function" - SID "2421" - Ports [4, 1] + SID "5197" + Ports [4, 2] Position [180, 22, 300, 163] ZOrder 205 ShowName off @@ -8294,11 +8472,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3782" + SIDHighWatermark "3792" Block { BlockType Inport Name "umin" - SID "2421::18" + SID "5197::18" Position [20, 101, 40, 119] ZOrder -1 IconDisplay "Port number" @@ -8306,7 +8484,7 @@ Model { Block { BlockType Inport Name "umax" - SID "2421::19" + SID "5197::19" Position [20, 136, 40, 154] ZOrder -2 Port "2" @@ -8315,7 +8493,7 @@ Model { Block { BlockType Inport Name "u" - SID "2421::1" + SID "5197::1" Position [20, 171, 40, 189] ZOrder -3 Port "3" @@ -8324,7 +8502,7 @@ Model { Block { BlockType Inport Name "tol" - SID "2421::20" + SID "5197::20" Position [20, 206, 40, 224] ZOrder -4 Port "4" @@ -8333,22 +8511,22 @@ Model { Block { BlockType Demux Name " Demux " - SID "2421::3781" + SID "5197::3791" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 131 + ZOrder 141 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2421::3780" + SID "5197::3790" Tag "Stateflow S-Function positionControlBalancing 18" - Ports [4, 2] + Ports [4, 3] Position [180, 102, 230, 203] - ZOrder 130 + ZOrder 140 FunctionName "sf_sfun" - PortCounts "[4 2]" + PortCounts "[4 3]" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -8356,45 +8534,59 @@ Model { PortNumber 2 Name "inRange" } + Port { + PortNumber 3 + Name "res_check_range" + } } Block { BlockType Terminator Name " Terminator " - SID "2421::3782" + SID "5197::3792" Position [460, 241, 480, 259] - ZOrder 132 + ZOrder 142 } Block { BlockType Outport Name "inRange" - SID "2421::5" + SID "5197::5" Position [460, 101, 480, 119] ZOrder -8 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "res_check_range" + SID "5197::3789" + Position [460, 136, 480, 154] + ZOrder 139 + Port "2" + IconDisplay "Port number" } Line { - ZOrder 22 + ZOrder 9 SrcBlock "umin" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 23 + ZOrder 10 SrcBlock "umax" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 24 + ZOrder 11 SrcBlock "u" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 25 + ZOrder 12 SrcBlock "tol" SrcPort 1 DstBlock " SFunction " @@ -8402,7 +8594,7 @@ Model { } Line { Name "inRange" - ZOrder 26 + ZOrder 13 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -8410,26 +8602,63 @@ Model { DstPort 1 } Line { - ZOrder 27 + Name "res_check_range" + ZOrder 14 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "res_check_range" + DstPort 1 + } + Line { + ZOrder 15 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 28 + ZOrder 16 SrcBlock " SFunction " SrcPort 1 + Points [20, 0] DstBlock " Demux " DstPort 1 } } } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "5198" + 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 "5199" + Position [320, 48, 340, 72] + ZOrder 559 + ShowName off + HideAutomaticName off + InitialCondition "1" + SampleTime "Config.tStep" + HasFrameUpgradeWarning on + } Block { BlockType Constant Name "index1" - SID "2422" - Position [65, 140, 90, 150] + SID "5200" + Position [55, 137, 95, 153] ZOrder 204 ShowName off Value "0.01" @@ -8446,43 +8675,57 @@ Model { ZOrder 2 SrcBlock "MATLAB Function" SrcPort 1 - DstBlock "Assertion" + DstBlock "Unit Delay" DstPort 1 } Line { ZOrder 3 - SrcBlock "GetLimits" + SrcBlock "index1" SrcPort 1 DstBlock "MATLAB Function" - DstPort 1 + DstPort 4 } Line { ZOrder 4 - SrcBlock "GetLimits" + SrcBlock "MATLAB Function" SrcPort 2 - DstBlock "MATLAB Function" - DstPort 2 + DstBlock "To Workspace" + DstPort 1 } Line { ZOrder 5 - SrcBlock "index1" + SrcBlock "Unit Delay" + SrcPort 1 + DstBlock "Assertion" + DstPort 1 + } + Line { + ZOrder 6 + SrcBlock "GetLimits" SrcPort 1 DstBlock "MATLAB Function" - DstPort 4 + DstPort 1 + } + Line { + ZOrder 7 + SrcBlock "GetLimits" + SrcPort 2 + DstBlock "MATLAB Function" + DstPort 2 } } } Block { BlockType SubSystem Name "STOP IF THERE ARE SPIKES IN THE ENCODERS" - SID "4664" + SID "5201" Ports [1, 0, 1] Position [285, 339, 440, 371] ZOrder 556 RequestExecContextInheritance off System { Name "STOP IF THERE ARE SPIKES IN THE ENCODERS" - Location [134, 55, 3840, 2160] + Location [67, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -8495,21 +8738,21 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "827" + ZoomFactor "380" Block { BlockType Inport Name "jointPos" - SID "4665" - Position [15, 53, 45, 67] + SID "5202" + Position [-10, 53, 20, 67] ZOrder 552 IconDisplay "Port number" } Block { BlockType EnablePort Name "Enable" - SID "4666" + SID "5203" Ports [] - Position [217, -20, 237, 0] + Position [172, -15, 192, 5] ZOrder 553 ShowName off HideAutomaticName off @@ -8517,17 +8760,17 @@ Model { Block { BlockType Assertion Name "Assertion" - SID "4667" - Position [340, 72, 400, 118] - ZOrder 207 + SID "5204" + Position [335, 37, 400, 83] + ZOrder 556 HideAutomaticName off } Block { BlockType SubSystem Name "MATLAB Function" - SID "4669" - Ports [2, 1] - Position [165, 24, 285, 166] + SID "5205" + Ports [2, 2] + Position [105, 26, 260, 164] ZOrder 205 ShowName off LibraryVersion "1.32" @@ -8552,11 +8795,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3782" + SIDHighWatermark "3792" Block { BlockType Inport Name "u" - SID "4669::1" + SID "5205::1" Position [20, 101, 40, 119] ZOrder -3 IconDisplay "Port number" @@ -8564,7 +8807,7 @@ Model { Block { BlockType Inport Name "delta_u_max" - SID "4669::18" + SID "5205::18" Position [20, 136, 40, 154] ZOrder -1 Port "2" @@ -8573,22 +8816,22 @@ Model { Block { BlockType Demux Name " Demux " - SID "4669::3781" + SID "5205::3791" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 131 + ZOrder 141 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4669::3780" + SID "5205::3790" Tag "Stateflow S-Function positionControlBalancing 14" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 130 + Ports [2, 3] + Position [180, 100, 230, 180] + ZOrder 140 FunctionName "sf_sfun" - PortCounts "[2 2]" + PortCounts "[2 3]" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -8596,24 +8839,38 @@ Model { PortNumber 2 Name "noSpikes" } + Port { + PortNumber 3 + Name "res_check_spikes" + } } Block { BlockType Terminator Name " Terminator " - SID "4669::3782" + SID "5205::3792" Position [460, 241, 480, 259] - ZOrder 132 + ZOrder 142 } Block { BlockType Outport Name "noSpikes" - SID "4669::5" + SID "5205::5" Position [460, 101, 480, 119] ZOrder -8 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "res_check_spikes" + SID "5205::3789" + Position [460, 136, 480, 154] + ZOrder 139 + Port "2" + IconDisplay "Port number" } Line { - ZOrder 16 + ZOrder 7 SrcBlock "u" SrcPort 1 Points [120, 0] @@ -8621,7 +8878,7 @@ Model { DstPort 1 } Line { - ZOrder 17 + ZOrder 8 SrcBlock "delta_u_max" SrcPort 1 DstBlock " SFunction " @@ -8629,7 +8886,7 @@ Model { } Line { Name "noSpikes" - ZOrder 18 + ZOrder 9 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -8637,14 +8894,23 @@ Model { DstPort 1 } Line { - ZOrder 19 + Name "res_check_spikes" + ZOrder 10 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "res_check_spikes" + DstPort 1 + } + Line { + ZOrder 11 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 20 + ZOrder 12 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -8652,11 +8918,38 @@ Model { } } } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "5206" + 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 "5207" + Position [290, 48, 310, 72] + ZOrder 558 + ShowName off + HideAutomaticName off + InitialCondition "1" + SampleTime "Config.tStep" + HasFrameUpgradeWarning on + } Block { BlockType Constant Name "index1" - SID "4671" - Position [-40, 124, 100, 136] + SID "5208" + Position [-65, 121, 75, 139] ZOrder 554 ShowName off Value "Sat.maxJointsPositionDelta" @@ -8673,7 +8966,7 @@ Model { ZOrder 2 SrcBlock "MATLAB Function" SrcPort 1 - DstBlock "Assertion" + DstBlock "Unit Delay" DstPort 1 } Line { @@ -8683,6 +8976,20 @@ Model { DstBlock "MATLAB Function" DstPort 2 } + Line { + ZOrder 4 + SrcBlock "MATLAB Function" + SrcPort 2 + DstBlock "To Workspace" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "Unit Delay" + SrcPort 1 + DstBlock "Assertion" + DstPort 1 + } } } Line { @@ -8796,7 +9103,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" SourceType "Simulator Synchronizer" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" period "Config.tStep" serverPortName "'/clock/rpc'" clientPortName "'/WBT_synchronizer/rpc:o'" @@ -8874,7 +9180,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" period "Config.tStep" } } @@ -8891,7 +9196,6 @@ Model { VariableName "yarp_time" MaxDataPoints "inf" SaveFormat "Structure With Time" - Save2DSignal "3-D array (concatenate along third dimension)" FixptAsFi on SampleTime "-1" } @@ -8909,7 +9213,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Yarp Clock" SourceType "YARP Clock" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" } Line { ZOrder 1 @@ -8957,14 +9260,14 @@ Model { SrcPort 1 Points [30, 0] Branch { - ZOrder 346 - DstBlock "POSITION CONTROLLER (IK)" + ZOrder 355 + Points [0, -35] + DstBlock "emergency stop: joint limits" DstPort 1 } Branch { - ZOrder 343 - Points [0, -35] - DstBlock "emergency stop: joint limits" + ZOrder 346 + DstBlock "POSITION CONTROLLER (IK)" DstPort 1 } } @@ -8979,17 +9282,17 @@ Model { } #Finite State Machines # -# Stateflow 80000014 +# Stateflow 80000019 # # Stateflow { machine { id 1 name "positionControlBalancing" - created "18-Feb-2014 10:14:40" + created "18-Feb-2014 10:13:36" isLibrary 0 sfVersion 80000012 - firstTarget 111 + firstTarget 113 } chart { id 2 @@ -9739,18 +10042,18 @@ Stateflow { screen [1 1 1366 768 1.25] treeNode [0 38 0 0] viewObj 37 - ssIdHighWaterMark 8 + ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART chartFileNumber 14 disableImplicitCasting 1 eml { - name "checkSpikes" + name "checkInputSpikesFCN" } supportVariableSizing 0 firstData 39 - firstTransition 43 - firstJunction 42 + firstTransition 44 + firstJunction 43 } state { id 38 @@ -9766,8 +10069,8 @@ Stateflow { decomposition CLUSTER_STATE eml { isEML 1 - script "function noSpikes = checkSpikes(u, delta_u_max)\n\n noSpikes = wbc.checkSpikes(u, delta_u_max" - ");\nend" + 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)" @@ -9843,10 +10146,35 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [37 40 0] + linkNode [37 40 42] } - junction { + data { id 42 + 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 [37 41 0] + } + junction { + id 43 position [23.5747 49.5747 7] chart 37 subviewer 37 @@ -9855,7 +10183,7 @@ Stateflow { linkNode [37 0 0] } transition { - id 43 + id 44 labelString "{eML_blk_kernel();}" labelPosition [40.125 31.875 102.544 14.964] fontSize 12 @@ -9863,7 +10191,7 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 42 + id 43 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] @@ -9879,49 +10207,49 @@ Stateflow { linkNode [37 0 0] } instance { - id 44 + id 45 machine 1 name "emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/MATLAB Function" chart 37 } chart { - id 45 + id 46 machine 1 name "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 46 0 0] - viewObj 45 - ssIdHighWaterMark 8 + treeNode [0 47 0 0] + viewObj 46 + ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART chartFileNumber 18 disableImplicitCasting 1 eml { - name "checkRangeFCN" + name "checkInputRangeFCN" } supportVariableSizing 0 - firstData 47 - firstTransition 53 - firstJunction 52 + firstData 48 + firstTransition 55 + firstJunction 54 } state { - id 46 + id 47 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 45 - treeNode [45 0 0 0] + chart 46 + treeNode [46 0 0 0] superState SUBCHART - subviewer 45 + subviewer 46 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE eml { isEML 1 - script "function inRange = checkRangeFCN(umin, umax, u, tol)\n\n inRange = wbc.checkRange(umin, umax," - " u, tol);\nend" + 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)" @@ -9930,7 +10258,7 @@ Stateflow { } } data { - id 47 + id 48 ssIdNumber 4 name "umin" scope INPUT_DATA @@ -9952,10 +10280,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [45 0 48] + linkNode [46 0 49] } data { - id 48 + id 49 ssIdNumber 5 name "umax" scope INPUT_DATA @@ -9977,10 +10305,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [45 47 49] + linkNode [46 48 50] } data { - id 49 + id 50 ssIdNumber 6 name "u" scope INPUT_DATA @@ -9999,10 +10327,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [45 48 50] + linkNode [46 49 51] } data { - id 50 + id 51 ssIdNumber 7 name "inRange" scope OUTPUT_DATA @@ -10022,10 +10350,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [45 49 51] + linkNode [46 50 52] } data { - id 51 + id 52 ssIdNumber 8 name "tol" scope INPUT_DATA @@ -10047,19 +10375,44 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [45 50 0] + linkNode [46 51 53] + } + data { + id 53 + 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 [46 52 0] } junction { - id 52 + id 54 position [23.5747 49.5747 7] - chart 45 - subviewer 45 + chart 46 + subviewer 46 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [45 0 0] + linkNode [46 0 0] } transition { - id 53 + id 55 labelString "{eML_blk_kernel();}" labelPosition [40.125 31.875 102.544 14.964] fontSize 12 @@ -10067,37 +10420,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 52 + id 54 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 45 + chart 46 dataLimits [21.175 25.975 14.625 42.575] - subviewer 45 + subviewer 46 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [45 0 0] + linkNode [46 0 0] } instance { - id 54 + id 56 machine 1 name "emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" - chart 45 + chart 46 } chart { - id 55 + id 57 machine 1 name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/InverseKinematics Integration/Compute the " "Base Pose Derivative" windowPosition [357.12 483.496 167 391] viewLimits [0 156.75 0 153.75] screen [1 1 1280 1024 1.041666666666667] - treeNode [0 56 0 0] - viewObj 55 + treeNode [0 58 0 0] + viewObj 57 ssIdHighWaterMark 7 decomposition CLUSTER_CHART type EML_CHART @@ -10106,19 +10459,19 @@ Stateflow { eml { name "computeBasePoseDerivativeFCN" } - firstData 57 - firstTransition 61 - firstJunction 60 + firstData 59 + firstTransition 63 + firstJunction 62 } state { - id 56 + id 58 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 55 - treeNode [55 0 0 0] + chart 57 + treeNode [57 0 0 0] superState SUBCHART - subviewer 55 + subviewer 57 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -10130,7 +10483,7 @@ Stateflow { } } data { - id 57 + id 59 ssIdNumber 5 name "pose_base_dot" scope OUTPUT_DATA @@ -10150,10 +10503,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [55 0 58] + linkNode [57 0 60] } data { - id 58 + id 60 ssIdNumber 6 name "nu_base_ikin" scope INPUT_DATA @@ -10175,10 +10528,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [55 57 59] + linkNode [57 59 61] } data { - id 59 + id 61 ssIdNumber 7 name "pose_base" scope INPUT_DATA @@ -10200,19 +10553,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [55 58 0] + linkNode [57 60 0] } junction { - id 60 + id 62 position [23.5747 49.5747 7] - chart 55 - subviewer 55 + chart 57 + subviewer 57 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [55 0 0] + linkNode [57 0 0] } transition { - id 61 + id 63 labelString "{eML_blk_kernel();}" labelPosition [32.125 19.875 102.544 14.964] fontSize 12 @@ -10220,37 +10573,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 60 + id 62 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 55 + chart 57 dataLimits [21.175 25.975 14.625 42.575] - subviewer 55 + subviewer 57 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [55 0 0] + linkNode [57 0 0] } instance { - id 62 + id 64 machine 1 name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/InverseKinematics Integration/Compute the " "Base Pose Derivative" - chart 55 + chart 57 } chart { - id 63 + id 65 machine 1 name "POSITION CONTROLLER (IK)/References and Initial Conditions/MATLAB Function" windowPosition [422 539.941 189 413] viewLimits [0 156.75 0 153.75] screen [1 1 3600 1200 1.180555555555556] - treeNode [0 64 0 0] - viewObj 63 + treeNode [0 66 0 0] + viewObj 65 ssIdHighWaterMark 6 decomposition CLUSTER_CHART type EML_CHART @@ -10259,19 +10612,19 @@ Stateflow { eml { name "getSystemConfiguration" } - firstData 65 - firstTransition 69 - firstJunction 68 + firstData 67 + firstTransition 71 + firstJunction 70 } state { - id 64 + id 66 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 63 - treeNode [63 0 0 0] + chart 65 + treeNode [65 0 0 0] superState SUBCHART - subviewer 63 + subviewer 65 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -10282,7 +10635,7 @@ Stateflow { } } data { - id 65 + id 67 ssIdNumber 4 name "w_H_b" scope INPUT_DATA @@ -10301,10 +10654,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [63 0 66] + linkNode [65 0 68] } data { - id 66 + id 68 ssIdNumber 5 name "q" scope OUTPUT_DATA @@ -10324,10 +10677,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [63 65 67] + linkNode [65 67 69] } data { - id 67 + id 69 ssIdNumber 6 name "jointPos" scope INPUT_DATA @@ -10349,19 +10702,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [63 66 0] + linkNode [65 68 0] } junction { - id 68 + id 70 position [23.5747 49.5747 7] - chart 63 - subviewer 63 + chart 65 + subviewer 65 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [63 0 0] + linkNode [65 0 0] } transition { - id 69 + id 71 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -10369,37 +10722,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 68 + id 70 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 63 + chart 65 dataLimits [21.175 25.975 14.625 42.575] - subviewer 63 + subviewer 65 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [63 0 0] + linkNode [65 0 0] } instance { - id 70 + id 72 machine 1 name "POSITION CONTROLLER (IK)/References and Initial Conditions/MATLAB Function" - chart 63 + chart 65 } chart { - id 71 + id 73 machine 1 name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Desired State Accelerations/trajec" "toryGeneratorCoM" windowPosition [395 -299.6 71 854] viewLimits [0 112 0 601] screen [1 1 2726 768 1.25] - treeNode [0 72 0 0] - viewObj 71 + treeNode [0 74 0 0] + viewObj 73 ssIdHighWaterMark 36 decomposition CLUSTER_CHART type EML_CHART @@ -10409,19 +10762,19 @@ Stateflow { name "computeStateAccelerationsFCN" } supportVariableSizing 0 - firstData 73 - firstTransition 95 - firstJunction 94 + firstData 75 + firstTransition 97 + firstJunction 96 } state { - id 72 + id 74 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 71 - treeNode [71 0 0 0] + chart 73 + treeNode [73 0 0 0] superState SUBCHART - subviewer 71 + subviewer 73 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -10444,7 +10797,7 @@ Stateflow { } } data { - id 73 + id 75 ssIdNumber 22 name "pos_l_sole_error" scope INPUT_DATA @@ -10466,10 +10819,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 0 74] + linkNode [73 0 76] } data { - id 74 + id 76 ssIdNumber 24 name "pos_r_sole_error" scope INPUT_DATA @@ -10491,10 +10844,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 73 75] + linkNode [73 75 77] } data { - id 75 + id 77 ssIdNumber 26 name "rot_r_sole_error" scope INPUT_DATA @@ -10516,10 +10869,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 74 76] + linkNode [73 76 78] } data { - id 76 + id 78 ssIdNumber 27 name "rot_l_sole_error" scope INPUT_DATA @@ -10541,10 +10894,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 75 77] + linkNode [73 77 79] } data { - id 77 + id 79 ssIdNumber 17 name "posCoM" scope INPUT_DATA @@ -10566,10 +10919,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 76 78] + linkNode [73 78 80] } data { - id 78 + id 80 ssIdNumber 19 name "jointPos" scope INPUT_DATA @@ -10591,10 +10944,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 77 79] + linkNode [73 79 81] } data { - id 79 + id 81 ssIdNumber 6 name "desired_pos_vel_acc_CoM" scope INPUT_DATA @@ -10616,10 +10969,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 78 80] + linkNode [73 80 82] } data { - id 80 + id 82 ssIdNumber 23 name "desired_pos_vel_acc_joints" scope INPUT_DATA @@ -10641,10 +10994,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 79 81] + linkNode [73 81 83] } data { - id 81 + id 83 ssIdNumber 29 name "vel_l_sole" scope INPUT_DATA @@ -10666,10 +11019,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 80 82] + linkNode [73 82 84] } data { - id 82 + id 84 ssIdNumber 25 name "vel_r_sole" scope INPUT_DATA @@ -10691,10 +11044,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 81 83] + linkNode [73 83 85] } data { - id 83 + id 85 ssIdNumber 18 name "velCoM" scope INPUT_DATA @@ -10716,10 +11069,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 82 84] + linkNode [73 84 86] } data { - id 84 + id 86 ssIdNumber 20 name "jointVel" scope INPUT_DATA @@ -10741,10 +11094,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 83 85] + linkNode [73 85 87] } data { - id 85 + id 87 ssIdNumber 30 name "J_l_sole" scope INPUT_DATA @@ -10766,10 +11119,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 84 86] + linkNode [73 86 88] } data { - id 86 + id 88 ssIdNumber 31 name "J_r_sole" scope INPUT_DATA @@ -10791,10 +11144,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 85 87] + linkNode [73 87 89] } data { - id 87 + id 89 ssIdNumber 21 name "J_CoM" scope INPUT_DATA @@ -10817,10 +11170,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 86 88] + linkNode [73 88 90] } data { - id 88 + id 90 ssIdNumber 32 name "JDot_l_sole_nu" scope INPUT_DATA @@ -10842,10 +11195,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 87 89] + linkNode [73 89 91] } data { - id 89 + id 91 ssIdNumber 33 name "JDot_r_sole_nu" scope INPUT_DATA @@ -10867,10 +11220,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 88 90] + linkNode [73 90 92] } data { - id 90 + id 92 ssIdNumber 34 name "JDot_CoM_nu" scope INPUT_DATA @@ -10892,10 +11245,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 89 91] + linkNode [73 91 93] } data { - id 91 + id 93 ssIdNumber 35 name "feetcontactStatus" scope INPUT_DATA @@ -10917,10 +11270,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 90 92] + linkNode [73 92 94] } data { - id 92 + id 94 ssIdNumber 4 name "nuDot_ikin" scope OUTPUT_DATA @@ -10942,10 +11295,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 91 93] + linkNode [73 93 95] } data { - id 93 + id 95 ssIdNumber 36 name "Config" scope PARAMETER_DATA @@ -10967,19 +11320,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [71 92 0] + linkNode [73 94 0] } junction { - id 94 + id 96 position [23.5747 49.5747 7] - chart 71 - subviewer 71 + chart 73 + subviewer 73 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [71 0 0] + linkNode [73 0 0] } transition { - id 95 + id 97 labelString "{eML_blk_kernel();}" labelPosition [48.125 43.875 104 16.042] fontSize 12 @@ -10987,38 +11340,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 94 + id 96 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 71 + chart 73 dataLimits [21.175 25.975 14.625 42.575] - subviewer 71 + subviewer 73 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [71 0 0] + linkNode [73 0 0] } instance { - id 96 + id 98 machine 1 name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Desired State Accelerations/trajec" "toryGeneratorCoM" - chart 71 + chart 73 } chart { - id 97 + id 99 machine 1 name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/InverseKinematics Integration/Rewrite the " "Base Pose as Pos+Quat" windowPosition [357.12 483.496 167 391] viewLimits [0 156.75 0 153.75] screen [1 1 1280 1024 1.041666666666667] - treeNode [0 98 0 0] - viewObj 97 + treeNode [0 100 0 0] + viewObj 99 ssIdHighWaterMark 5 decomposition CLUSTER_CHART type EML_CHART @@ -11027,19 +11380,19 @@ Stateflow { eml { name "fromTransfMatrixToQuaternions" } - firstData 99 - firstTransition 102 - firstJunction 101 + firstData 101 + firstTransition 104 + firstJunction 103 } state { - id 98 + id 100 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 97 - treeNode [97 0 0 0] + chart 99 + treeNode [99 0 0 0] superState SUBCHART - subviewer 97 + subviewer 99 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -11053,7 +11406,7 @@ Stateflow { } } data { - id 99 + id 101 ssIdNumber 4 name "q_0" scope INPUT_DATA @@ -11072,10 +11425,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [97 0 100] + linkNode [99 0 102] } data { - id 100 + id 102 ssIdNumber 5 name "q_0_quat" scope OUTPUT_DATA @@ -11095,19 +11448,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [97 99 0] + linkNode [99 101 0] } junction { - id 101 + id 103 position [23.5747 49.5747 7] - chart 97 - subviewer 97 + chart 99 + subviewer 99 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [97 0 0] + linkNode [99 0 0] } transition { - id 102 + id 104 labelString "{eML_blk_kernel();}" labelPosition [32.125 19.875 102.544 14.964] fontSize 12 @@ -11115,38 +11468,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 101 + id 103 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 97 + chart 99 dataLimits [21.175 25.975 14.625 42.575] - subviewer 97 + subviewer 99 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [97 0 0] + linkNode [99 0 0] } instance { - id 103 + id 105 machine 1 name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/InverseKinematics Integration/Rewrite the " "Base Pose as Pos+Quat" - chart 97 + chart 99 } chart { - id 104 + id 106 machine 1 name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/InverseKinematics Integration/Rewirte the " "Base Pose as Transf Matrix" windowPosition [357.12 483.496 167 391] viewLimits [0 156.75 0 153.75] screen [1 1 1280 1024 1.041666666666667] - treeNode [0 105 0 0] - viewObj 104 + treeNode [0 107 0 0] + viewObj 106 ssIdHighWaterMark 5 decomposition CLUSTER_CHART type EML_CHART @@ -11155,19 +11508,19 @@ Stateflow { eml { name "fromQuaternionsToTransfMatr" } - firstData 106 - firstTransition 109 - firstJunction 108 + firstData 108 + firstTransition 111 + firstJunction 110 } state { - id 105 + id 107 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 104 - treeNode [104 0 0 0] + chart 106 + treeNode [106 0 0 0] superState SUBCHART - subviewer 104 + subviewer 106 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -11181,7 +11534,7 @@ Stateflow { } } data { - id 106 + id 108 ssIdNumber 4 name "q_ikin_quat" scope INPUT_DATA @@ -11200,10 +11553,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [104 0 107] + linkNode [106 0 109] } data { - id 107 + id 109 ssIdNumber 5 name "q_ikin" scope OUTPUT_DATA @@ -11223,19 +11576,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [104 106 0] + linkNode [106 108 0] } junction { - id 108 + id 110 position [23.5747 49.5747 7] - chart 104 - subviewer 104 + chart 106 + subviewer 106 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [104 0 0] + linkNode [106 0 0] } transition { - id 109 + id 111 labelString "{eML_blk_kernel();}" labelPosition [32.125 19.875 102.544 14.964] fontSize 12 @@ -11243,39 +11596,39 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 108 + id 110 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 104 + chart 106 dataLimits [21.175 25.975 14.625 42.575] - subviewer 104 + subviewer 106 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [104 0 0] + linkNode [106 0 0] } instance { - id 110 + id 112 machine 1 name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/InverseKinematics Integration/Rewirte the " "Base Pose as Transf Matrix" - chart 104 + chart 106 } target { - id 111 + id 113 machine 1 name "sfun" codeFlags "" - linkNode [1 0 112] + linkNode [1 0 114] } target { - id 112 + id 114 machine 1 name "rtw" - linkNode [1 111 0] + linkNode [1 113 0] } } diff --git a/controllers/floating-base-balancing-position-control/stopPositionControlBalancing.m b/controllers/floating-base-balancing-position-control/stopPositionControlBalancing.m index 3a5a225..17d8c53 100644 --- a/controllers/floating-base-balancing-position-control/stopPositionControlBalancing.m +++ b/controllers/floating-base-balancing-position-control/stopPositionControlBalancing.m @@ -57,4 +57,37 @@ warning(['The time step tolerance of ', num2str(Config.tStep), '[s] has been violated more than 50 times.']) end -end \ No newline at end of file +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/README.md b/controllers/floating-base-balancing-torque-control/README.md index b4ee29f..b5214cf 100644 --- a/controllers/floating-base-balancing-torque-control/README.md +++ b/controllers/floating-base-balancing-torque-control/README.md @@ -2,6 +2,8 @@ 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 @@ -14,10 +16,14 @@ Currently, supported robots are: `iCubGenova04`, `iCubGenova02`, `icubGazeboSim` ## 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`. \ No newline at end of file +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/stopTorqueControlBalancing.m b/controllers/floating-base-balancing-torque-control/stopTorqueControlBalancing.m index 3a5a225..17d8c53 100644 --- a/controllers/floating-base-balancing-torque-control/stopTorqueControlBalancing.m +++ b/controllers/floating-base-balancing-torque-control/stopTorqueControlBalancing.m @@ -57,4 +57,37 @@ warning(['The time step tolerance of ', num2str(Config.tStep), '[s] has been violated more than 50 times.']) end -end \ No newline at end of file +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/torqueControlBalancing.mdl b/controllers/floating-base-balancing-torque-control/torqueControlBalancing.mdl index c0acb05..6e9b3ff 100644 --- a/controllers/floating-base-balancing-torque-control/torqueControlBalancing.mdl +++ b/controllers/floating-base-balancing-torque-control/torqueControlBalancing.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.3486" + ComputedModelVersion "1.3510" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -144,14 +144,14 @@ Model { ExternalFileReference { Reference "WBToolboxLibrary/Model/Jacobians/DotJNu" Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/DotJNu l_so" - "le " + "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 " + "le" SID "2376" Type "LIBRARY_BLOCK" } @@ -344,7 +344,7 @@ Model { Reference "WBToolboxLibrary/States/GetLimits" Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF JOINTS HI" "T THE LIMITS/GetLimits" - SID "2432" + SID "4921" Type "LIBRARY_BLOCK" } ExternalFileReference { @@ -385,13 +385,9 @@ Model { } OrderedModelArguments 1 } - DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" - LogicAnalyzerGraphicalSettings "" - LogicAnalyzerPlugin "on" - LogicAnalyzerSignalOrdering "" - CustomCodeFunctionData "" SLCCPlugin "on" + LogicAnalyzerPlugin "on" + DiagnosticSuppressor "on" PostLoadFcn "%% Try to open the static GUI and try to adjust it \ntry\n\n staticGUI = simulinkStaticGUI;\n\nend" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on @@ -412,10 +408,12 @@ Model { "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 Compile Button\n set(sl_synch_handles.compileBut" - "ton,'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" + " 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" @@ -443,20 +441,122 @@ Model { $ClassName "Simulink.ExplorerBarInfo" Visible [1] } - Object { - $PropName "EditorsInfo" - $ObjectID 5 - $ClassName "Simulink.EditorInfo" - IsActive [1] - ViewObjType "SimulinkTopLevel" - LoadSaveID "0" - Extents [1815.0, 876.0] - ZoomFactor [3.61010101010101] - Offset [776.12199216564068, 363.67375489647452] + Array { + Type "Simulink.EditorInfo" + Dimension 12 + Object { + $ObjectID 5 + IsActive [1] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [1815.0, 821.0] + ZoomFactor [3.61010101010101] + Offset [776.12199216564068, 371.79127028539449] + } + Object { + $ObjectID 6 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4917" + Extents [1815.0, 821.0] + ZoomFactor [4.3275706438106187] + Offset [10.41989407940855, -23.235071395084702] + } + Object { + $ObjectID 7 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4913" + Extents [1815.0, 821.0] + ZoomFactor [4.0050505050505052] + Offset [94.721007009531633, 186.50441361916774] + } + Object { + $ObjectID 8 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4925" + Extents [1815.0, 821.0] + ZoomFactor [3.8000000393627378] + Offset [-68.684210488152473, -33.394738158739756] + } + Object { + $ObjectID 9 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4836" + Extents [1815.0, 821.0] + ZoomFactor [1.2332814930015552] + Offset [-60.865177726986076, -5.3518284993695033] + } + Object { + $ObjectID 10 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2341" + Extents [1815.0, 821.0] + ZoomFactor [1.3863636363636365] + Offset [-1148.4886014344261, -206.09836065573768] + } + Object { + $ObjectID 11 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4321" + Extents [1815.0, 821.0] + ZoomFactor [3.0] + Offset [-211.69612560424537, 280.84982585720286] + } + Object { + $ObjectID 12 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4886" + Extents [1815.0, 821.0] + ZoomFactor [4.0050505050505052] + Offset [94.661097099621685, 186.50441361916774] + } + Object { + $ObjectID 13 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4890" + Extents [1815.0, 821.0] + ZoomFactor [4.3274215552523874] + Offset [5.415825977301381, -23.235182849936947] + } + Object { + $ObjectID 14 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4664" + Extents [1815.0, 821.0] + ZoomFactor [2.5] + Offset [-221.43409915356727, -143.9621523579201] + } + Object { + $ObjectID 15 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2416" + Extents [1815.0, 821.0] + ZoomFactor [2.5] + Offset [-41.475000000000023, 125.00000000000006] + } + Object { + $ObjectID 16 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4661" + Extents [1815.0, 821.0] + ZoomFactor [4.0] + Offset [-19.375, -38.5] + } + PropName "EditorsInfo" } Object { $PropName "DockComponentsInfo" - $ObjectID 6 + $ObjectID 17 $ClassName "Simulink.DockComponentInfo" Type "GLUE2:PropertyInspector" ID "Property Inspector" @@ -472,11 +572,7 @@ Model { "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABiAP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAALAD///8AAAc9AAADqgAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACA/////wAAAAAAAAAA/////wEAAADo/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAFo/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAANR/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" + "QBjAHQAbwByAAAAAAD/////AAAAOQD///8AAAc9AAADcgAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAA" } } HideAutomaticNames on @@ -486,9 +582,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Mar 08 16:25:41 2019" - RTWModifiedTimeStamp 473963141 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Mar 23 00:04:27 2020" + RTWModifiedTimeStamp 506822667 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -523,7 +619,6 @@ Model { FunctionConnectors off BrowserLookUnderMasks on SimulationMode "normal" - VisualizeLoggedSignalsWhenLoggingToFile off PauseTimes "5" NumberOfSteps 1 SnapshotBufferSize 10 @@ -538,7 +633,7 @@ Model { TryForcingSFcnDF off Object { $PropName "DataLoggingOverride" - $ObjectID 7 + $ObjectID 18 $ClassName "Simulink.SimulationData.ModelLoggingInfo" model_ "torqueControlBalancing" signals_ [] @@ -584,7 +679,7 @@ Model { Type "Handle" Dimension 1 Simulink.ConfigSet { - $ObjectID 8 + $ObjectID 19 Version "1.17.1" DisabledProps [] Description "" @@ -592,10 +687,11 @@ Model { Type "Handle" Dimension 9 Simulink.SolverCC { - $ObjectID 9 + $ObjectID 20 Version "1.17.1" DisabledProps [] Description "" + Components [] StartTime "0.0" StopTime "Config.SIMULATION_TIME " AbsTol "auto" @@ -631,10 +727,11 @@ Model { DecoupledContinuousIntegration off } Simulink.DataIOCC { - $ObjectID 10 + $ObjectID 21 Version "1.17.1" DisabledProps [] Description "" + Components [] Decimation "1" ExternalInput "[t, u]" FinalStateName "xFinal" @@ -672,11 +769,11 @@ Model { LoggingIntervals "[-inf, inf]" } Simulink.OptimizationCC { - $ObjectID 11 + $ObjectID 22 Version "1.17.1" Array { Type "Cell" - Dimension 8 + Dimension 9 Cell "BooleansAsBitfields" Cell "PassReuseOutputArgsAs" Cell "PassReuseOutputArgsThreshold" @@ -685,9 +782,11 @@ Model { Cell "OptimizeModelRefInitCode" Cell "NoFixptDivByZeroProtection" Cell "UseSpecifiedMinMax" + Cell "EfficientTunableParamExpr" PropName "DisabledProps" } Description "" + Components [] BlockReduction on BooleanDataType on ConditionallyExecuteInputs on @@ -734,7 +833,7 @@ Model { DifferentSizesBufferReuse off } Simulink.DebuggingCC { - $ObjectID 12 + $ObjectID 23 Version "1.17.1" Array { Type "Cell" @@ -743,6 +842,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] RTPrefix "error" ConsistencyChecking "none" ArrayBoundsChecking "none" @@ -849,10 +949,11 @@ Model { UnitDatabase "" } Simulink.HardwareCC { - $ObjectID 13 + $ObjectID 24 Version "1.17.1" DisabledProps [] Description "" + Components [] ProdBitPerChar 8 ProdBitPerShort 16 ProdBitPerInt 32 @@ -897,10 +998,11 @@ Model { UseSimulinkCoderFeatures on } Simulink.ModelReferenceCC { - $ObjectID 14 + $ObjectID 25 Version "1.17.1" DisabledProps [] Description "" + Components [] UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" EnableRefExpFcnMdlSchedulingChecks on CheckModelReferenceTargetMessage "error" @@ -916,10 +1018,11 @@ Model { SupportModelReferenceSimTargetCustomCode off } Simulink.SFSimCC { - $ObjectID 15 + $ObjectID 26 Version "1.17.1" DisabledProps [] Description "" + Components [] SimCustomSourceCode "" SimCustomHeaderCode "" SimCustomInitializer "" @@ -946,7 +1049,7 @@ Model { } Simulink.RTWCC { $BackupClass "Simulink.RTWCC" - $ObjectID 16 + $ObjectID 27 Version "1.17.1" Array { Type "Cell" @@ -969,6 +1072,7 @@ Model { Cell "GenerateMissedCodeReplacementReport" PropName "DisabledProps" } + Description "" SystemTargetFile "grt.tlc" HardwareBoard "None" TLCOptions "" @@ -979,9 +1083,7 @@ Model { PackageName "" TemplateMakefile "grt_default_tmf" PostCodeGenCommand "" - Description "" GenerateReport off - SaveLog off RTWVerbose on RetainRTWFile off RTWBuildHooks [] @@ -1033,7 +1135,7 @@ Model { Type "Handle" Dimension 2 Simulink.CodeAppCC { - $ObjectID 17 + $ObjectID 28 Version "1.17.1" Array { Type "Cell" @@ -1069,6 +1171,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] Comment "" ForceParamTrailComments off GenerateComments on @@ -1120,11 +1223,11 @@ Model { } Simulink.GRTTargetCC { $BackupClass "Simulink.TargetCC" - $ObjectID 18 + $ObjectID 29 Version "1.17.1" Array { Type "Cell" - Dimension 17 + Dimension 18 Cell "GeneratePreprocessorConditionals" Cell "IncludeMdlTerminateFcn" Cell "GenerateAllocFcn" @@ -1142,6 +1245,7 @@ Model { Cell "RemoveResetFunc" Cell "ExistingSharedCode" Cell "RemoveDisableFunc" + Cell "PreserveStateflowLocalDataDimensions" PropName "DisabledProps" } Description "" @@ -1149,8 +1253,8 @@ Model { Type "Handle" Dimension 1 Simulink.CPPComponent { - $ObjectID 19 - Version "1.18.0" + $ObjectID 30 + Version "19.1.1" Array { Type "Cell" Dimension 10 @@ -1167,6 +1271,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] Name "CPPClassGenComp" GenerateDestructor on GenerateExternalIOAccessMethods "None" @@ -1252,10 +1357,11 @@ Model { } } SlCovCC.ConfigComp { - $ObjectID 20 + $ObjectID 31 Version "1.17.1" DisabledProps [] Description "Simulink Coverage Configuration Component" + Components [] Name "Simulink Coverage" CovEnable off CovScope "EntireSystem" @@ -1294,25 +1400,24 @@ Model { PropName "Components" } Name "Configuration" + CurrentDlgPage "Code Generation" + ConfigPrmDlgPosition [ 328, 140, 1850, 1010 ] ExtraOptions "-aBlockCommentType=\"BlockPathComment\" -aIgnoreTestpoints=0 " - CurrentDlgPage "Solver" - ConfigPrmDlgPosition [ 730, 390, 2252, 1260 ] } PropName "ConfigurationSets" } Simulink.ConfigSet { $PropName "ActiveConfigurationSet" - $ObjectID 8 + $ObjectID 19 } Object { $PropName "DataTransfer" - $ObjectID 21 + $ObjectID 32 $ClassName "Simulink.GlobalDataTransfer" DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" DefaultExtrapolationMethodBetweenContTasks "None" - AutoInsertRateTranBlk [0] } ExplicitPartitioning off BlockDefaults { @@ -1503,7 +1608,7 @@ Model { MustResolveToSignalObject off OutputWhenUnConnected off OutputWhenUnconnectedValue "0" - VectorParamsAs1DForOutWhenUnconnected off + VectorParamsAs1DForOutWhenUnconnected on } Block { BlockType Product @@ -1590,7 +1695,6 @@ Model { TreatAsGroupedWhenPropagatingVariantConditions on ContentPreviewEnabled off IsWebBlock off - IsObserver off } Block { BlockType Sum @@ -1632,11 +1736,20 @@ Model { MaxDataPoints "1000" Decimation "1" SaveFormat "Array" - Save2DSignal "Inherit from input (this choice will be removed - see release notes)" + 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" @@ -1655,19 +1768,24 @@ Model { ShowPageBoundaries off ZoomFactor "361" ReportName "simulink-default.rpt" - SIDHighWatermark "4885" + SIDHighWatermark "4936" Block { BlockType Reference Name "Configuration" SID "4537" Ports [] - Position [1180, 545, 1245, 565] + 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'" @@ -1691,7 +1809,7 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 22 + $ObjectID 33 $ClassName "Simulink.Mask" Display "disp('VISUALIZER')" } @@ -1954,6 +2072,7 @@ Model { Position [-365, -132, -335, -118] ZOrder 700 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -1963,6 +2082,7 @@ Model { ZOrder 702 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -1972,6 +2092,7 @@ Model { ZOrder 703 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -1981,6 +2102,7 @@ Model { ZOrder 706 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -1990,6 +2112,7 @@ Model { ZOrder 701 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -1999,6 +2122,7 @@ Model { ZOrder 704 Port "6" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -2008,6 +2132,7 @@ Model { ZOrder 705 Port "7" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -2017,6 +2142,7 @@ Model { ZOrder 707 Port "8" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 48 @@ -2269,7 +2395,7 @@ Model { "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','2018a','Location',[135 169 3841 2159])" + "rsion','2019b','Location',[135 169 3841 2159])" NumInputPorts "6" Floating off } @@ -2341,7 +2467,7 @@ Model { "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','2018a','Location',[135 169 3841 2159])" + "'Version','2019b','Location',[135 169 3841 2159])" NumInputPorts "4" Floating off } @@ -2431,7 +2557,7 @@ Model { ",'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" - "','2018a','Location',[135 179 3841 2127])" + "','2019b','Location',[135 179 3841 2127])" NumInputPorts "6" Floating off } @@ -2531,7 +2657,7 @@ Model { "),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','2018a','Location',[401 474 1711 10" + "mgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2019b','Location',[401 474 1711 10" "60])" NumInputPorts "4" Floating off @@ -2954,7 +3080,7 @@ Model { "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','2018a','Location',[995 541 1565 " + "xtmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2019b','Location',[995 541 1565 " "915])" NumInputPorts "2" Floating off @@ -3047,7 +3173,7 @@ Model { "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','2018a','Location',[898 828 2209 1" + "tmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2019b','Location',[898 828 2209 1" "517])" NumInputPorts "6" Floating off @@ -3420,7 +3546,7 @@ Model { " 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','2018a','Location',[313 476 1506 1203])" + "urements',true,'Version','2018a')),'Version','2019b','Location',[313 476 1506 1203])" NumInputPorts "2" Floating off } @@ -3619,7 +3745,7 @@ Model { "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','2018a','Location',[630 446 1920 1048])" + ",true,'Version','2018a')),'Version','2019b','Location',[630 446 1920 1048])" NumInputPorts "3" Floating off } @@ -3679,7 +3805,7 @@ Model { "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','2018a','Location',[135 478 2" + "e),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2019b','Location',[135 478 2" "065 1412])" NumInputPorts "3" Floating off @@ -3751,7 +3877,7 @@ Model { "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','2018a','Location',[135 239 265" + ",extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2019b','Location',[135 239 265" "0 1508])" NumInputPorts "4" Floating off @@ -4144,7 +4270,7 @@ Model { "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','2018a','Location',[811 296 1749 1223])" + "easurements',true,'Version','2018a')),'Version','2019b','Location',[811 296 1749 1223])" NumInputPorts "6" Floating off } @@ -4237,7 +4363,7 @@ Model { ".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','2018a','Location',[701 359 2556 1241])" + "Tools','Measurements',true,'Version','2018a')),'Version','2019b','Location',[701 359 2556 1241])" NumInputPorts "6" Floating off } @@ -4319,6 +4445,11 @@ Model { 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" } @@ -4412,7 +4543,7 @@ Model { "',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','2018a','Location',[135 204 1853 1049])" + "n('Tools','Measurements',true,'Version','2018a')),'Version','2019b','Location',[135 204 1853 1049])" NumInputPorts "6" Floating off } @@ -4506,7 +4637,7 @@ Model { "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','2018a','Location',[135 184 3841 2132])" + "'Tools','Measurements',true,'Version','2018a')),'Version','2019b','Location',[135 184 3841 2132])" NumInputPorts "6" Floating off } @@ -4599,7 +4730,7 @@ Model { "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','2018a','Location',[135 169 2055 1097])" + "'Tools','Measurements',true,'Version','2018a')),'Version','2019b','Location',[135 169 2055 1097])" NumInputPorts "6" Floating off } @@ -4693,7 +4824,7 @@ Model { "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','2018a','Location',[379 354 2309 1288])" + ",'Version','2019b','Location',[379 354 2309 1288])" NumInputPorts "6" Floating off } @@ -4786,7 +4917,7 @@ Model { "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','2018a','Location',[1266 568 2566 1252])" + "easurements',true,'Version','2018a')),'Version','2019b','Location',[1266 568 2566 1252])" NumInputPorts "6" Floating off } @@ -4951,7 +5082,7 @@ Model { ",'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','2018a','Location',[135 169 1990 1170])" + "surements',true,'Version','2018a')),'Version','2019b','Location',[135 169 1990 1170])" NumInputPorts "6" Floating off } @@ -5856,6 +5987,11 @@ Model { 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" } @@ -5873,6 +6009,11 @@ Model { 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" } @@ -5890,6 +6031,11 @@ Model { 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" } @@ -5915,7 +6061,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "Ports.IMU" signalSize "Ports.IMU_PORT_SIZE" timeout "0.5" @@ -5934,7 +6079,7 @@ Model { RequestExecContextInheritance off System { Name "MOMENTUM BASED TORQUE CONTROL" - Location [134, 55, 3840, 2160] + Location [67, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -5947,7 +6092,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "278" + ZoomFactor "123" Block { BlockType Inport Name "jointPos" @@ -6546,7 +6691,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "66" + SIDHighWatermark "87" Block { BlockType Inport Name "feetContactStatus" @@ -6558,20 +6703,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4590::65" + SID "4590::86" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 56 + ZOrder 77 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4590::64" + SID "4590::85" Tag "Stateflow S-Function torqueControlBalancing 15" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 55 + ZOrder 76 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -6585,9 +6730,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4590::66" + SID "4590::87" Position [460, 241, 480, 259] - ZOrder 57 + ZOrder 78 } Block { BlockType Outport @@ -6596,9 +6741,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 53 + ZOrder 81 SrcBlock "feetContactStatus" SrcPort 1 DstBlock " SFunction " @@ -6606,7 +6752,7 @@ Model { } Line { Name "onOneFoot" - ZOrder 54 + ZOrder 82 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -6614,14 +6760,14 @@ Model { DstPort 1 } Line { - ZOrder 55 + ZOrder 83 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 56 + ZOrder 84 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -6737,7 +6883,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "67" + SIDHighWatermark "88" Block { BlockType Inport Name "H" @@ -6758,20 +6904,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4690::66" + SID "4690::87" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 57 + ZOrder 78 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4690::65" + SID "4690::86" Tag "Stateflow S-Function torqueControlBalancing 26" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 56 + ZOrder 77 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off @@ -6785,9 +6931,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4690::67" + SID "4690::88" Position [460, 241, 480, 259] - ZOrder 58 + ZOrder 79 } Block { BlockType Outport @@ -6796,9 +6942,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 16 + ZOrder 51 SrcBlock "H" SrcPort 1 Points [120, 0] @@ -6806,7 +6953,7 @@ Model { DstPort 1 } Line { - ZOrder 17 + ZOrder 52 SrcBlock "g" SrcPort 1 DstBlock " SFunction " @@ -6814,7 +6961,7 @@ Model { } Line { Name "analyticalSolution" - ZOrder 18 + ZOrder 53 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -6822,14 +6969,14 @@ Model { DstPort 1 } Line { - ZOrder 19 + ZOrder 54 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 20 + ZOrder 55 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -6850,6 +6997,11 @@ Model { 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 { @@ -6880,7 +7032,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "75" + SIDHighWatermark "96" Block { BlockType Inport Name "analyticalSolution" @@ -6919,20 +7071,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4692::74" + SID "4692::95" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 59 + ZOrder 80 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4692::73" + SID "4692::94" Tag "Stateflow S-Function torqueControlBalancing 27" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 58 + ZOrder 79 FunctionName "sf_sfun" Parameters "Config" PortCounts "[4 2]" @@ -6947,9 +7099,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4692::75" + SID "4692::96" Position [460, 241, 480, 259] - ZOrder 60 + ZOrder 81 } Block { BlockType Outport @@ -6958,30 +7110,31 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 28 + ZOrder 77 SrcBlock "analyticalSolution" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 29 + ZOrder 78 SrcBlock "primalSolution" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 30 + ZOrder 79 SrcBlock "QPStatus" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 31 + ZOrder 80 SrcBlock "feetContactStatus" SrcPort 1 DstBlock " SFunction " @@ -6989,7 +7142,7 @@ Model { } Line { Name "f_star" - ZOrder 32 + ZOrder 81 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -6997,14 +7150,14 @@ Model { DstPort 1 } Line { - ZOrder 33 + ZOrder 82 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 34 + ZOrder 83 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -7023,7 +7176,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/QP" SourceType "QP" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" lbA off ubA on lb off @@ -7038,6 +7190,7 @@ Model { Position [1790, -172, 1820, -158] ZOrder 760 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -7047,6 +7200,7 @@ Model { ZOrder 755 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -7257,7 +7411,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "67" + SIDHighWatermark "88" Block { BlockType Inport Name "H" @@ -7278,20 +7432,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4678::66" + SID "4678::87" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 57 + ZOrder 78 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4678::65" + SID "4678::86" Tag "Stateflow S-Function torqueControlBalancing 24" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 56 + ZOrder 77 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off @@ -7305,9 +7459,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4678::67" + SID "4678::88" Position [460, 241, 480, 259] - ZOrder 58 + ZOrder 79 } Block { BlockType Outport @@ -7316,9 +7470,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 16 + ZOrder 51 SrcBlock "H" SrcPort 1 Points [120, 0] @@ -7326,7 +7481,7 @@ Model { DstPort 1 } Line { - ZOrder 17 + ZOrder 52 SrcBlock "g" SrcPort 1 DstBlock " SFunction " @@ -7334,7 +7489,7 @@ Model { } Line { Name "analyticalSolution" - ZOrder 18 + ZOrder 53 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -7342,14 +7497,14 @@ Model { DstPort 1 } Line { - ZOrder 19 + ZOrder 54 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 20 + ZOrder 55 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -7370,6 +7525,11 @@ Model { 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 { @@ -7400,7 +7560,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "75" + SIDHighWatermark "96" Block { BlockType Inport Name "analyticalSolution" @@ -7430,20 +7590,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4680::74" + SID "4680::95" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 58 + ZOrder 79 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4680::73" + SID "4680::94" Tag "Stateflow S-Function torqueControlBalancing 25" Ports [3, 2] Position [180, 100, 230, 180] - ZOrder 57 + ZOrder 78 FunctionName "sf_sfun" Parameters "Config" PortCounts "[3 2]" @@ -7458,9 +7618,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4680::75" + SID "4680::96" Position [460, 241, 480, 259] - ZOrder 59 + ZOrder 80 } Block { BlockType Outport @@ -7469,23 +7629,24 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 38 + ZOrder 80 SrcBlock "analyticalSolution" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 39 + ZOrder 81 SrcBlock "primalSolution" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 40 + ZOrder 82 SrcBlock "QPStatus" SrcPort 1 DstBlock " SFunction " @@ -7493,7 +7654,7 @@ Model { } Line { Name "f_star" - ZOrder 41 + ZOrder 83 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -7501,14 +7662,14 @@ Model { DstPort 1 } Line { - ZOrder 42 + ZOrder 84 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 43 + ZOrder 85 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -7528,7 +7689,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/QP" SourceType "QP" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" lbA off ubA on lb off @@ -7543,6 +7703,7 @@ Model { Position [1765, 1098, 1795, 1112] ZOrder 756 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -7552,6 +7713,7 @@ Model { ZOrder 760 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 37 @@ -7676,6 +7838,7 @@ Model { Position [245, -297, 275, -283] ZOrder 401 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -7685,6 +7848,7 @@ Model { ZOrder 738 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -7694,6 +7858,7 @@ Model { ZOrder 728 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 3 @@ -7863,6 +8028,7 @@ Model { Position [1630, 608, 1660, 622] ZOrder 425 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -7872,6 +8038,7 @@ Model { ZOrder 639 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 293 @@ -8118,6 +8285,11 @@ Model { 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 { @@ -8204,6 +8376,11 @@ Model { 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" } @@ -8218,6 +8395,11 @@ Model { 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" } @@ -8228,6 +8410,7 @@ Model { Position [495, -22, 525, -8] ZOrder 910 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -8237,6 +8420,7 @@ Model { ZOrder 912 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -8331,7 +8515,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3783" + SIDHighWatermark "3804" Block { BlockType Inport Name "J_l_sole" @@ -8370,20 +8554,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "3721::3782" + SID "3721::3803" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 107 + ZOrder 128 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "3721::3781" + SID "3721::3802" Tag "Stateflow S-Function torqueControlBalancing 20" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 106 + ZOrder 127 FunctionName "sf_sfun" Parameters "Reg" PortCounts "[4 2]" @@ -8398,9 +8582,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "3721::3783" + SID "3721::3804" Position [460, 241, 480, 259] - ZOrder 108 + ZOrder 129 } Block { BlockType Outport @@ -8409,30 +8593,31 @@ Model { Position [460, 101, 480, 119] ZOrder 10 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 169 + ZOrder 218 SrcBlock "J_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 170 + ZOrder 219 SrcBlock "J_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 171 + ZOrder 220 SrcBlock "feetContactStatus" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 172 + ZOrder 221 SrcBlock "jointPos_err" SrcPort 1 DstBlock " SFunction " @@ -8440,7 +8625,7 @@ Model { } Line { Name "baseVel_equivalent" - ZOrder 173 + ZOrder 222 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -8448,14 +8633,14 @@ Model { DstPort 1 } Line { - ZOrder 174 + ZOrder 223 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 175 + ZOrder 224 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -8476,6 +8661,11 @@ Model { 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" } @@ -8492,6 +8682,11 @@ Model { 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" } @@ -8523,7 +8718,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "96" + SIDHighWatermark "117" Block { BlockType Inport Name "state" @@ -8535,20 +8730,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4027::95" + SID "4027::116" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 86 + ZOrder 107 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4027::94" + SID "4027::115" Tag "Stateflow S-Function torqueControlBalancing 19" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 85 + ZOrder 106 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -8562,9 +8757,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4027::96" + SID "4027::117" Position [460, 241, 480, 259] - ZOrder 87 + ZOrder 108 } Block { BlockType Outport @@ -8573,9 +8768,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 97 + ZOrder 125 SrcBlock "state" SrcPort 1 DstBlock " SFunction " @@ -8583,7 +8779,7 @@ Model { } Line { Name "booleanState" - ZOrder 98 + ZOrder 126 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -8591,14 +8787,14 @@ Model { DstPort 1 } Line { - ZOrder 99 + ZOrder 127 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 100 + ZOrder 128 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -8639,6 +8835,7 @@ Model { Position [1450, 28, 1480, 42] ZOrder -2 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -8903,31 +9100,31 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "102" + SIDHighWatermark "130" Block { BlockType Demux Name " Demux " - SID "4551::100" + SID "4551::128" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 91 + ZOrder 119 Outputs "1" } Block { BlockType Ground Name " Ground " - SID "4551::102" + SID "4551::130" Position [20, 121, 40, 139] - ZOrder 93 + ZOrder 121 } Block { BlockType S-Function Name " SFunction " - SID "4551::99" + SID "4551::127" Tag "Stateflow S-Function torqueControlBalancing 9" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 90 + ZOrder 118 FunctionName "sf_sfun" Parameters "Config" PortCounts "[1 2]" @@ -8942,9 +9139,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4551::101" + SID "4551::129" Position [460, 241, 480, 259] - ZOrder 92 + ZOrder 120 } Block { BlockType Outport @@ -8953,10 +9150,11 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { Name "reflectedInertia" - ZOrder 81 + ZOrder 109 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -8964,21 +9162,21 @@ Model { DstPort 1 } Line { - ZOrder 82 + ZOrder 110 SrcBlock " Ground " SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 83 + ZOrder 111 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 84 + ZOrder 112 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -9081,6 +9279,7 @@ Model { Position [485, -57, 515, -43] ZOrder 592 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 6 @@ -9222,7 +9421,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3782" + SIDHighWatermark "3803" Block { BlockType Inport Name "feetContactStatus" @@ -9423,20 +9622,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "2407::3781" + SID "2407::3802" Ports [1, 1] Position [270, 570, 320, 610] - ZOrder 184 + ZOrder 205 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2407::3780" + SID "2407::3801" Tag "Stateflow S-Function torqueControlBalancing 17" Ports [22, 13] Position [180, 70, 230, 530] - ZOrder 183 + ZOrder 204 FunctionName "sf_sfun" Parameters "Config,Gain,Reg" PortCounts "[22 13]" @@ -9495,9 +9694,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2407::3782" + SID "2407::3803" Position [460, 581, 480, 599] - ZOrder 185 + ZOrder 206 } Block { BlockType Outport @@ -9506,6 +9705,7 @@ Model { Position [460, 101, 480, 119] ZOrder 72 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -9515,6 +9715,7 @@ Model { ZOrder 70 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -9524,6 +9725,7 @@ Model { ZOrder -5 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -9533,6 +9735,7 @@ Model { ZOrder 58 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -9542,6 +9745,7 @@ Model { ZOrder 75 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -9551,6 +9755,7 @@ Model { ZOrder 76 Port "6" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -9560,6 +9765,7 @@ Model { ZOrder 59 Port "7" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -9569,6 +9775,7 @@ Model { ZOrder 60 Port "8" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -9578,6 +9785,7 @@ Model { ZOrder 63 Port "9" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -9587,6 +9795,7 @@ Model { ZOrder 64 Port "10" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -9596,6 +9805,7 @@ Model { ZOrder 68 Port "11" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -9605,156 +9815,157 @@ Model { ZOrder 69 Port "12" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 2078 + ZOrder 2330 SrcBlock "feetContactStatus" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 2079 + ZOrder 2331 SrcBlock "ConstraintsMatrix" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 2080 + ZOrder 2332 SrcBlock "bVectorConstraints" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 2081 + ZOrder 2333 SrcBlock "jointPos" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 2082 + ZOrder 2334 SrcBlock "jointPos_des" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 2083 + ZOrder 2335 SrcBlock "nu" SrcPort 1 DstBlock " SFunction " DstPort 6 } Line { - ZOrder 2084 + ZOrder 2336 SrcBlock "M" SrcPort 1 DstBlock " SFunction " DstPort 7 } Line { - ZOrder 2085 + ZOrder 2337 SrcBlock "h" SrcPort 1 DstBlock " SFunction " DstPort 8 } Line { - ZOrder 2086 + ZOrder 2338 SrcBlock "L" SrcPort 1 DstBlock " SFunction " DstPort 9 } Line { - ZOrder 2087 + ZOrder 2339 SrcBlock "intL_angMomError" SrcPort 1 DstBlock " SFunction " DstPort 10 } Line { - ZOrder 2088 + ZOrder 2340 SrcBlock "w_H_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 11 } Line { - ZOrder 2089 + ZOrder 2341 SrcBlock "w_H_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 12 } Line { - ZOrder 2090 + ZOrder 2342 SrcBlock "J_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 13 } Line { - ZOrder 2091 + ZOrder 2343 SrcBlock "J_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 14 } Line { - ZOrder 2092 + ZOrder 2344 SrcBlock "JDot_l_sole_nu" SrcPort 1 DstBlock " SFunction " DstPort 15 } Line { - ZOrder 2093 + ZOrder 2345 SrcBlock "JDot_r_sole_nu" SrcPort 1 DstBlock " SFunction " DstPort 16 } Line { - ZOrder 2094 + ZOrder 2346 SrcBlock "pos_CoM" SrcPort 1 DstBlock " SFunction " DstPort 17 } Line { - ZOrder 2095 + ZOrder 2347 SrcBlock "J_CoM" SrcPort 1 DstBlock " SFunction " DstPort 18 } Line { - ZOrder 2096 + ZOrder 2348 SrcBlock "desired_pos_vel_acc_CoM" SrcPort 1 DstBlock " SFunction " DstPort 19 } Line { - ZOrder 2097 + ZOrder 2349 SrcBlock "KP_CoM" SrcPort 1 DstBlock " SFunction " DstPort 20 } Line { - ZOrder 2098 + ZOrder 2350 SrcBlock "KD_CoM" SrcPort 1 DstBlock " SFunction " DstPort 21 } Line { - ZOrder 2099 + ZOrder 2351 SrcBlock "KP_postural" SrcPort 1 DstBlock " SFunction " @@ -9762,7 +9973,7 @@ Model { } Line { Name "HessianMatrixOneFoot" - ZOrder 2100 + ZOrder 2352 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -9771,7 +9982,7 @@ Model { } Line { Name "gradientOneFoot" - ZOrder 2101 + ZOrder 2353 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -9780,7 +9991,7 @@ Model { } Line { Name "ConstraintsMatrixOneFoot" - ZOrder 2102 + ZOrder 2354 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -9789,7 +10000,7 @@ Model { } Line { Name "bVectorConstraintsOneFoot" - ZOrder 2103 + ZOrder 2355 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -9798,7 +10009,7 @@ Model { } Line { Name "HessianMatrixTwoFeet" - ZOrder 2104 + ZOrder 2356 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -9807,7 +10018,7 @@ Model { } Line { Name "gradientTwoFeet" - ZOrder 2105 + ZOrder 2357 Labels [0, 0] SrcBlock " SFunction " SrcPort 7 @@ -9816,7 +10027,7 @@ Model { } Line { Name "ConstraintsMatrixTwoFeet" - ZOrder 2106 + ZOrder 2358 Labels [0, 0] SrcBlock " SFunction " SrcPort 8 @@ -9825,7 +10036,7 @@ Model { } Line { Name "bVectorConstraintsTwoFeet" - ZOrder 2107 + ZOrder 2359 Labels [0, 0] SrcBlock " SFunction " SrcPort 9 @@ -9834,7 +10045,7 @@ Model { } Line { Name "tauModel" - ZOrder 2108 + ZOrder 2360 Labels [0, 0] SrcBlock " SFunction " SrcPort 10 @@ -9843,7 +10054,7 @@ Model { } Line { Name "Sigma" - ZOrder 2109 + ZOrder 2361 Labels [0, 0] SrcBlock " SFunction " SrcPort 11 @@ -9852,7 +10063,7 @@ Model { } Line { Name "Na" - ZOrder 2110 + ZOrder 2362 Labels [0, 0] SrcBlock " SFunction " SrcPort 12 @@ -9861,7 +10072,7 @@ Model { } Line { Name "f_LDot" - ZOrder 2111 + ZOrder 2363 Labels [0, 0] SrcBlock " SFunction " SrcPort 13 @@ -9869,14 +10080,14 @@ Model { DstPort 1 } Line { - ZOrder 2112 + ZOrder 2364 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 2113 + ZOrder 2365 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -9891,6 +10102,7 @@ Model { Position [2340, 73, 2370, 87] ZOrder 279 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 303 @@ -10222,7 +10434,7 @@ Model { RequestExecContextInheritance off System { Name "Dynamics and Kinematics" - Location [134, 55, 3840, 2160] + Location [67, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -10235,7 +10447,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "312" + ZoomFactor "139" Block { BlockType Inport Name "w_H_b" @@ -10372,7 +10584,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "82" + SIDHighWatermark "103" Block { BlockType Inport Name "M" @@ -10384,20 +10596,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4518::81" + SID "4518::102" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 71 + ZOrder 92 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4518::80" + SID "4518::101" Tag "Stateflow S-Function torqueControlBalancing 6" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 70 + ZOrder 91 FunctionName "sf_sfun" Parameters "Config" PortCounts "[1 2]" @@ -10412,9 +10624,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4518::82" + SID "4518::103" Position [460, 241, 480, 259] - ZOrder 72 + ZOrder 93 } Block { BlockType Outport @@ -10423,9 +10635,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 94 + ZOrder 122 SrcBlock "M" SrcPort 1 DstBlock " SFunction " @@ -10433,7 +10646,7 @@ Model { } Line { Name "M_with_inertia" - ZOrder 95 + ZOrder 123 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -10441,14 +10654,14 @@ Model { DstPort 1 } Line { - ZOrder 96 + ZOrder 124 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 97 + ZOrder 125 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -10483,6 +10696,7 @@ Model { Position [455, 143, 485, 157] ZOrder 592 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 109 @@ -10536,6 +10750,11 @@ Model { 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 { @@ -10576,6 +10795,11 @@ Model { 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 { @@ -10590,6 +10814,11 @@ Model { 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 { @@ -10599,6 +10828,7 @@ Model { Position [750, 48, 780, 62] ZOrder 587 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -10608,6 +10838,7 @@ Model { ZOrder 588 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -10617,6 +10848,7 @@ Model { ZOrder 589 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 83 @@ -10761,7 +10993,7 @@ Model { RequestExecContextInheritance off System { Name "Kinematics" - Location [134, 55, 3840, 2160] + Location [67, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -10774,7 +11006,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "365" + ZoomFactor "300" Block { BlockType Inport Name "nu" @@ -10814,6 +11046,11 @@ Model { 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" } @@ -10859,22 +11096,28 @@ Model { } Block { BlockType Reference - Name "DotJNu l_sole\n" + 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\n " + Name "DotJNu r_sole" SID "2376" Ports [4, 1] Position [65, 464, 235, 521] @@ -10885,6 +11128,11 @@ Model { 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" } @@ -10910,6 +11158,11 @@ Model { 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" } @@ -10925,6 +11178,11 @@ Model { 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" } @@ -10940,6 +11198,11 @@ Model { 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" } @@ -10955,6 +11218,11 @@ Model { 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" } @@ -10970,6 +11238,11 @@ Model { 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" } @@ -10980,6 +11253,7 @@ Model { Position [380, 48, 410, 62] ZOrder 865 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -10989,6 +11263,7 @@ Model { ZOrder 874 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -10998,6 +11273,7 @@ Model { ZOrder 875 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11007,6 +11283,7 @@ Model { ZOrder 876 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11016,6 +11293,7 @@ Model { ZOrder 877 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11025,6 +11303,7 @@ Model { ZOrder 878 Port "6" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11034,6 +11313,7 @@ Model { ZOrder 879 Port "7" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11043,6 +11323,7 @@ Model { ZOrder 881 Port "8" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 37 @@ -11051,13 +11332,13 @@ Model { Points [57, 0] Branch { ZOrder 203 - DstBlock "DotJNu r_sole\n " + DstBlock "DotJNu r_sole" DstPort 4 } Branch { ZOrder 193 Points [0, -80] - DstBlock "DotJNu l_sole\n" + DstBlock "DotJNu l_sole" DstPort 4 } } @@ -11068,13 +11349,13 @@ Model { Points [80, 0] Branch { ZOrder 206 - DstBlock "DotJNu r_sole\n " + DstBlock "DotJNu r_sole" DstPort 3 } Branch { ZOrder 204 Points [0, -80] - DstBlock "DotJNu l_sole\n" + DstBlock "DotJNu l_sole" DstPort 3 } } @@ -11139,14 +11420,14 @@ Model { } Line { ZOrder 217 - SrcBlock "DotJNu l_sole\n" + SrcBlock "DotJNu l_sole" SrcPort 1 DstBlock "JDot_l_sole_nu" DstPort 1 } Line { ZOrder 218 - SrcBlock "DotJNu r_sole\n " + SrcBlock "DotJNu r_sole" SrcPort 1 DstBlock "JDot_r_sole_nu" DstPort 1 @@ -11182,12 +11463,12 @@ Model { Branch { ZOrder 274 Points [0, 80] - DstBlock "DotJNu r_sole\n " + DstBlock "DotJNu r_sole" DstPort 2 } Branch { ZOrder 214 - DstBlock "DotJNu l_sole\n" + DstBlock "DotJNu l_sole" DstPort 2 } } @@ -11247,12 +11528,12 @@ Model { Branch { ZOrder 273 Points [0, 80] - DstBlock "DotJNu r_sole\n " + DstBlock "DotJNu r_sole" DstPort 1 } Branch { ZOrder 220 - DstBlock "DotJNu l_sole\n" + DstBlock "DotJNu l_sole" DstPort 1 } } @@ -11316,6 +11597,7 @@ Model { Position [-475, -167, -445, -153] ZOrder -2 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11325,6 +11607,7 @@ Model { ZOrder 217 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11334,6 +11617,7 @@ Model { ZOrder 216 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11343,6 +11627,7 @@ Model { ZOrder 882 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11352,6 +11637,7 @@ Model { ZOrder 883 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11361,6 +11647,7 @@ Model { ZOrder 884 Port "6" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11370,6 +11657,7 @@ Model { ZOrder 885 Port "7" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11379,6 +11667,7 @@ Model { ZOrder 886 Port "8" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11388,6 +11677,7 @@ Model { ZOrder 887 Port "9" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11397,6 +11687,7 @@ Model { ZOrder 888 Port "10" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -11406,6 +11697,7 @@ Model { ZOrder 889 Port "11" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 109 @@ -11639,7 +11931,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "77" + SIDHighWatermark "98" Block { BlockType Inport Name "u" @@ -11678,20 +11970,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4556::76" + SID "4556::97" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 65 + ZOrder 86 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4556::75" + SID "4556::96" Tag "Stateflow S-Function torqueControlBalancing 11" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 64 + ZOrder 85 FunctionName "sf_sfun" PortCounts "[4 2]" SFunctionDeploymentMode off @@ -11705,9 +11997,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4556::77" + SID "4556::98" Position [460, 241, 480, 259] - ZOrder 66 + ZOrder 87 } Block { BlockType Outport @@ -11716,30 +12008,31 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 133 + ZOrder 182 SrcBlock "u" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 134 + ZOrder 183 SrcBlock "u_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 135 + ZOrder 184 SrcBlock "tStep" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 136 + ZOrder 185 SrcBlock "uDotMax" SrcPort 1 DstBlock " SFunction " @@ -11747,7 +12040,7 @@ Model { } Line { Name "uSat" - ZOrder 137 + ZOrder 186 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -11755,14 +12048,14 @@ Model { DstPort 1 } Line { - ZOrder 138 + ZOrder 187 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 139 + ZOrder 188 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -11808,6 +12101,11 @@ Model { 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 { @@ -11817,6 +12115,7 @@ Model { Position [545, 88, 575, 102] ZOrder -2 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 2 @@ -12064,7 +12363,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3806" + SIDHighWatermark "3827" Block { BlockType Inport Name "J_l_sole" @@ -12103,20 +12402,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4215::3805" + SID "4215::3826" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 174 + ZOrder 195 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4215::3804" + SID "4215::3825" Tag "Stateflow S-Function torqueControlBalancing 7" Ports [4, 2] Position [180, 102, 230, 203] - ZOrder 173 + ZOrder 194 FunctionName "sf_sfun" Parameters "Reg" PortCounts "[4 2]" @@ -12131,9 +12430,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4215::3806" + SID "4215::3827" Position [460, 241, 480, 259] - ZOrder 175 + ZOrder 196 } Block { BlockType Outport @@ -12142,30 +12441,31 @@ Model { Position [460, 101, 480, 119] ZOrder -6 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 22 + ZOrder 71 SrcBlock "J_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 23 + ZOrder 72 SrcBlock "J_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 24 + ZOrder 73 SrcBlock "feetContactStatus" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 25 + ZOrder 74 SrcBlock "jointVel" SrcPort 1 DstBlock " SFunction " @@ -12173,7 +12473,7 @@ Model { } Line { Name "nu_b" - ZOrder 26 + ZOrder 75 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -12181,14 +12481,14 @@ Model { DstPort 1 } Line { - ZOrder 27 + ZOrder 76 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 28 + ZOrder 77 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -12248,6 +12548,11 @@ Model { 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" } @@ -12262,6 +12567,11 @@ Model { 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" } @@ -12272,6 +12582,7 @@ Model { Position [935, 498, 965, 512] ZOrder 676 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -12281,6 +12592,7 @@ Model { ZOrder 677 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -12350,6 +12662,7 @@ Model { Position [660, -122, 690, -108] ZOrder -15 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 20 @@ -12549,6 +12862,11 @@ Model { 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" } @@ -12563,6 +12881,11 @@ Model { 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" } @@ -12596,7 +12919,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3797" + SIDHighWatermark "3818" SIDPrevWatermark "9" Block { BlockType Inport @@ -12654,20 +12977,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4257::3796" + SID "4257::3817" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 196 + ZOrder 217 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4257::3795" + SID "4257::3816" Tag "Stateflow S-Function torqueControlBalancing 8" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 195 + ZOrder 216 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" @@ -12682,9 +13005,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4257::3797" + SID "4257::3818" Position [460, 241, 480, 259] - ZOrder 197 + ZOrder 218 } Block { BlockType Outport @@ -12693,44 +13016,45 @@ Model { Position [460, 101, 480, 119] ZOrder 80 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 28 + ZOrder 91 SrcBlock "imu_H_fixedLink" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 29 + ZOrder 92 SrcBlock "imu_H_fixedLink_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 30 + ZOrder 93 SrcBlock "fixedLink_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 31 + ZOrder 94 SrcBlock "rpyFromIMU_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 32 + ZOrder 95 SrcBlock "rpyFromIMU" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 33 + ZOrder 96 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -12738,7 +13062,7 @@ Model { } Line { Name "w_H_b" - ZOrder 34 + ZOrder 97 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -12746,14 +13070,14 @@ Model { DstPort 1 } Line { - ZOrder 35 + ZOrder 98 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 36 + ZOrder 99 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -12813,7 +13137,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -12881,6 +13204,11 @@ Model { 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 { @@ -12896,6 +13224,11 @@ Model { 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 { @@ -12996,6 +13329,7 @@ Model { Position [425, 53, 455, 67] ZOrder 726 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -13050,6 +13384,7 @@ Model { Position [1875, 278, 1905, 292] ZOrder 732 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -13308,6 +13643,11 @@ Model { 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" } @@ -13322,6 +13662,11 @@ Model { 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" } @@ -13355,7 +13700,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3797" + SIDHighWatermark "3818" SIDPrevWatermark "9" Block { BlockType Inport @@ -13413,20 +13758,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4857::3796" + SID "4857::3817" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 196 + ZOrder 217 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4857::3795" + SID "4857::3816" Tag "Stateflow S-Function torqueControlBalancing 3" Ports [6, 2] Position [180, 102, 230, 243] - ZOrder 195 + ZOrder 216 FunctionName "sf_sfun" Parameters "Config" PortCounts "[6 2]" @@ -13441,9 +13786,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4857::3797" + SID "4857::3818" Position [460, 241, 480, 259] - ZOrder 197 + ZOrder 218 } Block { BlockType Outport @@ -13452,44 +13797,45 @@ Model { Position [460, 101, 480, 119] ZOrder 80 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 28 + ZOrder 91 SrcBlock "imu_H_fixedLink" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 29 + ZOrder 92 SrcBlock "imu_H_fixedLink_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 30 + ZOrder 93 SrcBlock "fixedLink_H_base" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 31 + ZOrder 94 SrcBlock "rpyFromIMU_0" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 32 + ZOrder 95 SrcBlock "rpyFromIMU" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 33 + ZOrder 96 SrcBlock "neck_pos" SrcPort 1 DstBlock " SFunction " @@ -13497,7 +13843,7 @@ Model { } Line { Name "w_H_b" - ZOrder 34 + ZOrder 97 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -13505,14 +13851,14 @@ Model { DstPort 1 } Line { - ZOrder 35 + ZOrder 98 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 36 + ZOrder 99 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -13572,7 +13918,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "Ports.NECK_POS" signalSize "Ports.NECK_POS_PORT_SIZE" timeout "0.5" @@ -13640,6 +13985,11 @@ Model { 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 { @@ -13655,6 +14005,11 @@ Model { 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 { @@ -13755,6 +14110,7 @@ Model { Position [425, 53, 455, 67] ZOrder 726 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -13809,6 +14165,7 @@ Model { Position [1875, 278, 1905, 292] ZOrder 732 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -14008,6 +14365,11 @@ Model { 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" } @@ -14022,6 +14384,11 @@ Model { 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" } @@ -14032,6 +14399,7 @@ Model { Position [650, 28, 680, 42] ZOrder 910 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -14041,6 +14409,7 @@ Model { ZOrder 912 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -14339,7 +14708,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" SourceType "MinimumJerkTrajectoryGenerator" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" externalSettlingTime off settlingTime "Config.SmoothingTimeGainScheduling" sampleTime "Config.tStep" @@ -14389,7 +14757,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3785" + SIDHighWatermark "3806" SIDPrevWatermark "9" Block { BlockType Inport @@ -14483,20 +14851,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "2220::3784" + SID "2220::3805" Ports [1, 1] Position [270, 495, 320, 535] - ZOrder 182 + ZOrder 203 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2220::3783" + SID "2220::3804" Tag "Stateflow S-Function torqueControlBalancing 13" Ports [10, 11] Position [180, 100, 230, 340] - ZOrder 181 + ZOrder 202 FunctionName "sf_sfun" Parameters "Config,Gain,StateMachine" PortCounts "[10 11]" @@ -14547,9 +14915,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "2220::3785" + SID "2220::3806" Position [460, 506, 480, 524] - ZOrder 183 + ZOrder 204 } Block { BlockType Outport @@ -14558,6 +14926,7 @@ Model { Position [460, 101, 480, 119] ZOrder 80 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -14567,6 +14936,7 @@ Model { ZOrder 53 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -14576,6 +14946,7 @@ Model { ZOrder 43 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -14585,6 +14956,7 @@ Model { ZOrder 40 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -14594,6 +14966,7 @@ Model { ZOrder 70 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -14603,6 +14976,7 @@ Model { ZOrder 84 Port "6" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -14612,6 +14986,7 @@ Model { ZOrder 85 Port "7" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -14621,6 +14996,7 @@ Model { ZOrder 71 Port "8" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -14630,6 +15006,7 @@ Model { ZOrder 83 Port "9" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -14639,72 +15016,73 @@ Model { ZOrder 174 Port "10" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 860 + ZOrder 1014 SrcBlock "pos_CoM_0" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 861 + ZOrder 1015 SrcBlock "jointPos_0" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 862 + ZOrder 1016 SrcBlock "pos_CoM_fixed_l_sole" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 863 + ZOrder 1017 SrcBlock "pos_CoM_fixed_r_sole" SrcPort 1 DstBlock " SFunction " DstPort 4 } Line { - ZOrder 864 + ZOrder 1018 SrcBlock "jointPos" SrcPort 1 DstBlock " SFunction " DstPort 5 } Line { - ZOrder 865 + ZOrder 1019 SrcBlock "time" SrcPort 1 DstBlock " SFunction " DstPort 6 } Line { - ZOrder 866 + ZOrder 1020 SrcBlock "wrench_rightFoot" SrcPort 1 DstBlock " SFunction " DstPort 7 } Line { - ZOrder 867 + ZOrder 1021 SrcBlock "wrench_leftFoot" SrcPort 1 DstBlock " SFunction " DstPort 8 } Line { - ZOrder 868 + ZOrder 1022 SrcBlock "l_sole_H_b" SrcPort 1 DstBlock " SFunction " DstPort 9 } Line { - ZOrder 869 + ZOrder 1023 SrcBlock "r_sole_H_b" SrcPort 1 DstBlock " SFunction " @@ -14712,7 +15090,7 @@ Model { } Line { Name "w_H_b" - ZOrder 870 + ZOrder 1024 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -14721,7 +15099,7 @@ Model { } Line { Name "pos_CoM_des" - ZOrder 871 + ZOrder 1025 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -14730,7 +15108,7 @@ Model { } Line { Name "jointPos_des" - ZOrder 872 + ZOrder 1026 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -14739,7 +15117,7 @@ Model { } Line { Name "feetContactStatus" - ZOrder 873 + ZOrder 1027 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -14748,7 +15126,7 @@ Model { } Line { Name "KP_postural_diag" - ZOrder 874 + ZOrder 1028 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -14757,7 +15135,7 @@ Model { } Line { Name "KP_CoM_diag" - ZOrder 875 + ZOrder 1029 Labels [0, 0] SrcBlock " SFunction " SrcPort 7 @@ -14766,7 +15144,7 @@ Model { } Line { Name "KD_CoM_diag" - ZOrder 876 + ZOrder 1030 Labels [0, 0] SrcBlock " SFunction " SrcPort 8 @@ -14775,7 +15153,7 @@ Model { } Line { Name "state" - ZOrder 877 + ZOrder 1031 Labels [0, 0] SrcBlock " SFunction " SrcPort 9 @@ -14784,7 +15162,7 @@ Model { } Line { Name "smoothingTimeJoints" - ZOrder 878 + ZOrder 1032 Labels [0, 0] SrcBlock " SFunction " SrcPort 10 @@ -14793,7 +15171,7 @@ Model { } Line { Name "smoothingTimeCoM" - ZOrder 879 + ZOrder 1033 Labels [0, 0] SrcBlock " SFunction " SrcPort 11 @@ -14801,14 +15179,14 @@ Model { DstPort 1 } Line { - ZOrder 880 + ZOrder 1034 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 881 + ZOrder 1035 SrcBlock " SFunction " SrcPort 1 Points [20, 0] @@ -14830,6 +15208,11 @@ Model { 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 { @@ -14845,6 +15228,11 @@ Model { 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 { @@ -14901,6 +15289,11 @@ Model { 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" } @@ -14925,6 +15318,7 @@ Model { Position [400, 253, 430, 267] ZOrder 583 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -15010,6 +15404,11 @@ Model { 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" } @@ -15034,6 +15433,7 @@ Model { Position [400, 253, 430, 267] ZOrder 583 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -15072,6 +15472,7 @@ Model { Position [365, 124, 395, 136] ZOrder 578 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -15081,6 +15482,7 @@ Model { ZOrder 480 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -15090,6 +15492,7 @@ Model { ZOrder 478 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -15099,6 +15502,7 @@ Model { ZOrder 479 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -15108,6 +15512,7 @@ Model { ZOrder 545 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -15117,6 +15522,7 @@ Model { ZOrder 680 Port "6" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -15126,6 +15532,7 @@ Model { ZOrder 689 Port "7" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -15135,6 +15542,7 @@ Model { ZOrder 691 Port "8" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -15144,6 +15552,7 @@ Model { ZOrder 905 Port "9" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -15153,6 +15562,7 @@ Model { ZOrder 550 Port "10" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 173 @@ -15628,7 +16038,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "30" + SIDHighWatermark "51" Block { BlockType Inport Name "d" @@ -15640,20 +16050,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4782::29" + SID "4782::50" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 20 + ZOrder 41 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4782::28" + SID "4782::49" Tag "Stateflow S-Function torqueControlBalancing 2" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 19 + ZOrder 40 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -15667,9 +16077,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4782::30" + SID "4782::51" Position [460, 241, 480, 259] - ZOrder 21 + ZOrder 42 } Block { BlockType Outport @@ -15678,9 +16088,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 17 + ZOrder 45 SrcBlock "d" SrcPort 1 DstBlock " SFunction " @@ -15688,7 +16099,7 @@ Model { } Line { Name "D" - ZOrder 18 + ZOrder 46 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -15696,14 +16107,14 @@ Model { DstPort 1 } Line { - ZOrder 19 + ZOrder 47 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 20 + ZOrder 48 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -15742,7 +16153,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "30" + SIDHighWatermark "51" Block { BlockType Inport Name "d" @@ -15754,20 +16165,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4783::29" + SID "4783::50" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 20 + ZOrder 41 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4783::28" + SID "4783::49" Tag "Stateflow S-Function torqueControlBalancing 1" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 19 + ZOrder 40 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -15781,9 +16192,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4783::30" + SID "4783::51" Position [460, 241, 480, 259] - ZOrder 21 + ZOrder 42 } Block { BlockType Outport @@ -15792,9 +16203,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 13 + ZOrder 41 SrcBlock "d" SrcPort 1 DstBlock " SFunction " @@ -15802,7 +16214,7 @@ Model { } Line { Name "D" - ZOrder 14 + ZOrder 42 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -15810,14 +16222,14 @@ Model { DstPort 1 } Line { - ZOrder 15 + ZOrder 43 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 16 + ZOrder 44 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -15856,7 +16268,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "30" + SIDHighWatermark "51" Block { BlockType Inport Name "d" @@ -15868,20 +16280,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "4784::29" + SID "4784::50" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 20 + ZOrder 41 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4784::28" + SID "4784::49" Tag "Stateflow S-Function torqueControlBalancing 4" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 19 + ZOrder 40 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -15895,9 +16307,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "4784::30" + SID "4784::51" Position [460, 241, 480, 259] - ZOrder 21 + ZOrder 42 } Block { BlockType Outport @@ -15906,9 +16318,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 13 + ZOrder 41 SrcBlock "d" SrcPort 1 DstBlock " SFunction " @@ -15916,7 +16329,7 @@ Model { } Line { Name "D" - ZOrder 14 + ZOrder 42 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -15924,14 +16337,14 @@ Model { DstPort 1 } Line { - ZOrder 15 + ZOrder 43 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 16 + ZOrder 44 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -15946,6 +16359,7 @@ Model { Position [420, -82, 450, -68] ZOrder 918 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -15955,6 +16369,7 @@ Model { ZOrder 919 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -15964,6 +16379,7 @@ Model { ZOrder 920 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -16085,7 +16501,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" SourceType "MinimumJerkTrajectoryGenerator" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" externalSettlingTime on settlingTime "0.01" sampleTime "Config.tStep" @@ -16154,6 +16569,7 @@ Model { Position [965, -162, 995, -148] ZOrder 915 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 615 @@ -16308,7 +16724,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" SourceType "MinimumJerkTrajectoryGenerator" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" externalSettlingTime on settlingTime "0.01" sampleTime "Config.tStep" @@ -16353,6 +16768,7 @@ Model { Position [685, -77, 715, -63] ZOrder 909 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 13 @@ -16422,6 +16838,7 @@ Model { Position [745, -17, 775, -3] ZOrder 705 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -16431,6 +16848,7 @@ Model { ZOrder 706 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -16440,6 +16858,7 @@ Model { ZOrder 707 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -16449,6 +16868,7 @@ Model { ZOrder 712 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -16458,6 +16878,7 @@ Model { ZOrder 713 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 604 @@ -16579,6 +17000,7 @@ Model { Position [25, 2524, 55, 2536] ZOrder 578 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -16588,6 +17010,7 @@ Model { ZOrder 577 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -16597,6 +17020,7 @@ Model { ZOrder 647 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -16606,6 +17030,7 @@ Model { ZOrder 410 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -16615,6 +17040,7 @@ Model { ZOrder 517 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -16624,6 +17050,7 @@ Model { ZOrder 692 Port "6" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -16633,6 +17060,7 @@ Model { ZOrder 693 Port "7" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -16642,6 +17070,7 @@ Model { ZOrder 323 Port "8" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -16651,6 +17080,7 @@ Model { ZOrder 418 Port "9" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 545 @@ -16877,23 +17307,23 @@ Model { Block { BlockType SubSystem Name "emergency stop: joint limits" - SID "2416" + SID "4913" Ports [1] - Position [300, 246, 425, 274] - ZOrder 908 + Position [305, 241, 430, 269] + ZOrder 966 BackgroundColor "red" ShowName off RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 23 + $ObjectID 34 $ClassName "Simulink.Mask" Display "disp('EMERGENCY STOP')" RunInitForIconRedraw "off" } System { Name "emergency stop: joint limits" - Location [134, 55, 3840, 2160] + Location [67, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -16906,11 +17336,11 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "904" + ZoomFactor "401" Block { BlockType Inport Name "jointPos" - SID "2419" + SID "4914" Position [150, 238, 180, 252] ZOrder -1 IconDisplay "Port number" @@ -16918,7 +17348,7 @@ Model { Block { BlockType Constant Name "ON_GAZEBO\n1" - SID "4660" + SID "4915" Position [232, 190, 488, 210] ZOrder 553 BlockRotation 270 @@ -16930,7 +17360,7 @@ Model { Block { BlockType Constant Name "ON_GAZEBO\n2" - SID "4663" + SID "4916" Position [224, 295, 496, 315] ZOrder 555 BlockRotation 270 @@ -16942,14 +17372,14 @@ Model { Block { BlockType SubSystem Name "STOP IF JOINTS HIT THE LIMITS" - SID "4661" + SID "4917" Ports [1, 0, 1] Position [285, 229, 440, 261] ZOrder 554 RequestExecContextInheritance off System { Name "STOP IF JOINTS HIT THE LIMITS" - Location [134, 55, 3840, 2160] + Location [67, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -16962,11 +17392,11 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "898" + ZoomFactor "433" Block { BlockType Inport Name "jointPos" - SID "4662" + SID "4918" Position [60, 103, 90, 117] ZOrder 552 IconDisplay "Port number" @@ -16974,9 +17404,9 @@ Model { Block { BlockType EnablePort Name "Enable" - SID "3207" + SID "4919" Ports [] - Position [227, -35, 247, -15] + Position [227, -20, 247, 0] ZOrder 553 ShowName off HideAutomaticName off @@ -16984,24 +17414,29 @@ Model { Block { BlockType Assertion Name "Assertion" - SID "2420" - Position [340, 72, 400, 118] + SID "4920" + Position [360, 37, 420, 83] ZOrder 207 HideAutomaticName off } Block { BlockType Reference Name "GetLimits" - SID "2432" + SID "4921" Ports [0, 2] Position [15, 23, 135, 92] - ZOrder 551 + 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" @@ -17009,8 +17444,8 @@ Model { Block { BlockType SubSystem Name "MATLAB Function" - SID "2421" - Ports [4, 1] + SID "4922" + Ports [4, 2] Position [180, 22, 300, 163] ZOrder 205 ShowName off @@ -17036,11 +17471,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3782" + SIDHighWatermark "3795" Block { BlockType Inport Name "umin" - SID "2421::18" + SID "4922::18" Position [20, 101, 40, 119] ZOrder -1 IconDisplay "Port number" @@ -17048,7 +17483,7 @@ Model { Block { BlockType Inport Name "umax" - SID "2421::19" + SID "4922::19" Position [20, 136, 40, 154] ZOrder -2 Port "2" @@ -17057,7 +17492,7 @@ Model { Block { BlockType Inport Name "u" - SID "2421::1" + SID "4922::1" Position [20, 171, 40, 189] ZOrder -3 Port "3" @@ -17066,7 +17501,7 @@ Model { Block { BlockType Inport Name "tol" - SID "2421::20" + SID "4922::20" Position [20, 206, 40, 224] ZOrder -4 Port "4" @@ -17075,22 +17510,22 @@ Model { Block { BlockType Demux Name " Demux " - SID "2421::3781" + SID "4922::3794" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 131 + ZOrder 144 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "2421::3780" + SID "4922::3793" Tag "Stateflow S-Function torqueControlBalancing 18" - Ports [4, 2] + Ports [4, 3] Position [180, 102, 230, 203] - ZOrder 130 + ZOrder 143 FunctionName "sf_sfun" - PortCounts "[4 2]" + PortCounts "[4 3]" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -17098,45 +17533,59 @@ Model { PortNumber 2 Name "inRange" } + Port { + PortNumber 3 + Name "res_check_range" + } } Block { BlockType Terminator Name " Terminator " - SID "2421::3782" + SID "4922::3795" Position [460, 241, 480, 259] - ZOrder 132 + ZOrder 145 } Block { BlockType Outport Name "inRange" - SID "2421::5" + SID "4922::5" Position [460, 101, 480, 119] ZOrder -8 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "res_check_range" + SID "4922::3789" + Position [460, 136, 480, 154] + ZOrder 139 + Port "2" + IconDisplay "Port number" } Line { - ZOrder 22 + ZOrder 17 SrcBlock "umin" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - ZOrder 23 + ZOrder 18 SrcBlock "umax" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 24 + ZOrder 19 SrcBlock "u" SrcPort 1 DstBlock " SFunction " DstPort 3 } Line { - ZOrder 25 + ZOrder 20 SrcBlock "tol" SrcPort 1 DstBlock " SFunction " @@ -17144,7 +17593,7 @@ Model { } Line { Name "inRange" - ZOrder 26 + ZOrder 21 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17152,26 +17601,63 @@ Model { DstPort 1 } Line { - ZOrder 27 + Name "res_check_range" + ZOrder 22 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "res_check_range" + DstPort 1 + } + Line { + ZOrder 23 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 28 + ZOrder 24 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 "2422" - Position [65, 140, 90, 150] + SID "4924" + Position [55, 137, 95, 153] ZOrder 204 ShowName off Value "0.01" @@ -17188,43 +17674,57 @@ Model { 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 3 + ZOrder 13 SrcBlock "GetLimits" SrcPort 1 DstBlock "MATLAB Function" DstPort 1 } Line { - ZOrder 4 + ZOrder 14 SrcBlock "GetLimits" SrcPort 2 DstBlock "MATLAB Function" DstPort 2 } - Line { - ZOrder 5 - SrcBlock "index1" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 4 - } } } Block { BlockType SubSystem Name "STOP IF THERE ARE SPIKES IN THE ENCODERS" - SID "4664" + 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 [134, 55, 3840, 2160] + Location [67, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -17237,21 +17737,21 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "827" + ZoomFactor "380" Block { BlockType Inport Name "jointPos" - SID "4665" - Position [15, 53, 45, 67] + SID "4926" + Position [-10, 53, 20, 67] ZOrder 552 IconDisplay "Port number" } Block { BlockType EnablePort Name "Enable" - SID "4666" + SID "4927" Ports [] - Position [217, -20, 237, 0] + Position [172, -15, 192, 5] ZOrder 553 ShowName off HideAutomaticName off @@ -17259,17 +17759,17 @@ Model { Block { BlockType Assertion Name "Assertion" - SID "4667" - Position [340, 72, 400, 118] - ZOrder 207 + SID "4928" + Position [335, 37, 400, 83] + ZOrder 556 HideAutomaticName off } Block { BlockType SubSystem Name "MATLAB Function" - SID "4669" - Ports [2, 1] - Position [165, 24, 285, 166] + SID "4929" + Ports [2, 2] + Position [105, 26, 260, 164] ZOrder 205 ShowName off LibraryVersion "1.32" @@ -17294,11 +17794,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "3782" + SIDHighWatermark "3795" Block { BlockType Inport Name "u" - SID "4669::1" + SID "4929::1" Position [20, 101, 40, 119] ZOrder -3 IconDisplay "Port number" @@ -17306,7 +17806,7 @@ Model { Block { BlockType Inport Name "delta_u_max" - SID "4669::18" + SID "4929::18" Position [20, 136, 40, 154] ZOrder -1 Port "2" @@ -17315,22 +17815,22 @@ Model { Block { BlockType Demux Name " Demux " - SID "4669::3781" + SID "4929::3794" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 131 + ZOrder 144 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "4669::3780" + SID "4929::3793" Tag "Stateflow S-Function torqueControlBalancing 14" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 130 + Ports [2, 3] + Position [180, 100, 230, 180] + ZOrder 143 FunctionName "sf_sfun" - PortCounts "[2 2]" + PortCounts "[2 3]" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -17338,24 +17838,38 @@ Model { PortNumber 2 Name "noSpikes" } + Port { + PortNumber 3 + Name "res_check_spikes" + } } Block { BlockType Terminator Name " Terminator " - SID "4669::3782" + SID "4929::3795" Position [460, 241, 480, 259] - ZOrder 132 + ZOrder 145 } Block { BlockType Outport Name "noSpikes" - SID "4669::5" + SID "4929::5" Position [460, 101, 480, 119] ZOrder -8 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "res_check_spikes" + SID "4929::3789" + Position [460, 136, 480, 154] + ZOrder 139 + Port "2" + IconDisplay "Port number" } Line { - ZOrder 16 + ZOrder 13 SrcBlock "u" SrcPort 1 Points [120, 0] @@ -17363,7 +17877,7 @@ Model { DstPort 1 } Line { - ZOrder 17 + ZOrder 14 SrcBlock "delta_u_max" SrcPort 1 DstBlock " SFunction " @@ -17371,7 +17885,7 @@ Model { } Line { Name "noSpikes" - ZOrder 18 + ZOrder 15 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -17379,14 +17893,23 @@ Model { DstPort 1 } Line { - ZOrder 19 + Name "res_check_spikes" + ZOrder 16 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "res_check_spikes" + DstPort 1 + } + Line { + ZOrder 17 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 20 + ZOrder 18 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -17394,11 +17917,38 @@ Model { } } } + 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 "4671" - Position [-40, 124, 100, 136] + SID "4931" + Position [-65, 121, 75, 139] ZOrder 554 ShowName off Value "Sat.maxJointsPositionDelta" @@ -17412,10 +17962,10 @@ Model { DstPort 1 } Line { - ZOrder 2 + ZOrder 6 SrcBlock "MATLAB Function" SrcPort 1 - DstBlock "Assertion" + DstBlock "Unit Delay" DstPort 1 } Line { @@ -17425,6 +17975,20 @@ Model { 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 { @@ -17467,6 +18031,7 @@ Model { Position [1160, 313, 1190, 327] ZOrder 560 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 94 @@ -17653,15 +18218,15 @@ Model { SrcPort 1 Points [36, 0] Branch { - ZOrder 294 - Points [0, 70] - DstBlock "Robot State and References" + ZOrder 322 + Points [0, -40] + DstBlock "emergency stop: joint limits" DstPort 1 } Branch { - ZOrder 297 - Points [0, -35] - DstBlock "emergency stop: joint limits" + ZOrder 294 + Points [0, 70] + DstBlock "Robot State and References" DstPort 1 } Branch { @@ -17709,6 +18274,11 @@ Model { 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" @@ -17727,7 +18297,7 @@ Model { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 24 + $ObjectID 35 $ClassName "Simulink.Mask" Display "disp('SYNCHRONIZER')" } @@ -17792,7 +18362,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" SourceType "Simulator Synchronizer" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" period "Config.tStep" serverPortName "'/clock/rpc'" clientPortName "'/WBT_synchronizer/rpc:o'" @@ -17870,7 +18439,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" period "Config.tStep" } } @@ -17887,7 +18455,6 @@ Model { VariableName "yarp_time" MaxDataPoints "inf" SaveFormat "Structure With Time" - Save2DSignal "3-D array (concatenate along third dimension)" FixptAsFi on SampleTime "-1" } @@ -17905,7 +18472,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Yarp Clock" SourceType "YARP Clock" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" } Line { ZOrder 1 @@ -17953,7 +18519,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "Ports.WRENCH_LEFT_FOOT" signalSize "Ports.WRENCH_PORT_SIZE" timeout "0.5" @@ -17975,7 +18540,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "Ports.WRENCH_RIGHT_FOOT" signalSize "Ports.WRENCH_PORT_SIZE" timeout "0.5" @@ -18037,17 +18601,17 @@ Model { } #Finite State Machines # -# Stateflow 80000014 +# Stateflow 80000019 # # Stateflow { machine { id 1 name "torqueControlBalancing" - created "18-Feb-2014 10:14:40" + created "18-Feb-2014 10:13:36" isLibrary 0 sfVersion 80000012 - firstTarget 235 + firstTarget 237 } chart { id 2 @@ -20371,18 +20935,18 @@ Stateflow { screen [1 1 1366 768 1.25] treeNode [0 114 0 0] viewObj 113 - ssIdHighWaterMark 8 + ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART chartFileNumber 14 disableImplicitCasting 1 eml { - name "checkSpikes" + name "checkInputSpikesFCN" } supportVariableSizing 0 firstData 115 - firstTransition 119 - firstJunction 118 + firstTransition 120 + firstJunction 119 } state { id 114 @@ -20398,8 +20962,8 @@ Stateflow { decomposition CLUSTER_STATE eml { isEML 1 - script "function noSpikes = checkSpikes(u, delta_u_max)\n\n noSpikes = wbc.checkSpikes(u, delta_u_max" - ");\nend" + 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)" @@ -20475,10 +21039,35 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [113 116 0] + linkNode [113 116 118] } - junction { + 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 @@ -20487,7 +21076,7 @@ Stateflow { linkNode [113 0 0] } transition { - id 119 + id 120 labelString "{eML_blk_kernel();}" labelPosition [40.125 31.875 102.544 14.964] fontSize 12 @@ -20495,7 +21084,7 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 118 + id 119 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] @@ -20511,22 +21100,22 @@ Stateflow { linkNode [113 0 0] } instance { - id 120 + 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 121 + 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 122 0 0] - viewObj 121 + treeNode [0 123 0 0] + viewObj 122 ssIdHighWaterMark 5 decomposition CLUSTER_CHART type EML_CHART @@ -20535,19 +21124,19 @@ Stateflow { eml { name "robotIsOnSingleSupportQP_FCN" } - firstData 123 - firstTransition 126 - firstJunction 125 + firstData 124 + firstTransition 127 + firstJunction 126 } state { - id 122 + id 123 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 121 - treeNode [121 0 0 0] + chart 122 + treeNode [122 0 0 0] superState SUBCHART - subviewer 121 + subviewer 122 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -20559,7 +21148,7 @@ Stateflow { } } data { - id 123 + id 124 ssIdNumber 4 name "feetContactStatus" scope INPUT_DATA @@ -20578,10 +21167,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [121 0 124] + linkNode [122 0 125] } data { - id 124 + id 125 ssIdNumber 5 name "onOneFoot" scope OUTPUT_DATA @@ -20601,19 +21190,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [121 123 0] + linkNode [122 124 0] } junction { - id 125 + id 126 position [23.5747 49.5747 7] - chart 121 - subviewer 121 + chart 122 + subviewer 122 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [121 0 0] + linkNode [122 0 0] } transition { - id 126 + id 127 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -20621,37 +21210,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 125 + id 126 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 121 + chart 122 dataLimits [21.175 25.975 14.625 42.575] - subviewer 121 + subviewer 122 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [121 0 0] + linkNode [122 0 0] } instance { - id 127 + id 128 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/One Foot Two Fee" "t QP Selector" - chart 121 + chart 122 } chart { - id 128 + 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 129 0 0] - viewObj 128 + treeNode [0 130 0 0] + viewObj 129 ssIdHighWaterMark 82 decomposition CLUSTER_CHART type EML_CHART @@ -20660,19 +21249,19 @@ Stateflow { eml { name "momentumBasedControllerFCN" } - firstData 130 - firstTransition 168 - firstJunction 167 + firstData 131 + firstTransition 169 + firstJunction 168 } state { - id 129 + id 130 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 128 - treeNode [128 0 0 0] + chart 129 + treeNode [129 0 0 0] superState SUBCHART - subviewer 128 + subviewer 129 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -20694,7 +21283,7 @@ Stateflow { } } data { - id 130 + id 131 ssIdNumber 66 name "HessianMatrixOneFoot" scope OUTPUT_DATA @@ -20716,10 +21305,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 0 131] + linkNode [129 0 132] } data { - id 131 + id 132 ssIdNumber 64 name "gradientOneFoot" scope OUTPUT_DATA @@ -20741,10 +21330,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 130 132] + linkNode [129 131 133] } data { - id 132 + id 133 ssIdNumber 5 name "ConstraintsMatrixOneFoot" scope OUTPUT_DATA @@ -20764,10 +21353,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 131 133] + linkNode [129 132 134] } data { - id 133 + id 134 ssIdNumber 52 name "bVectorConstraintsOneFoot" scope OUTPUT_DATA @@ -20789,10 +21378,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 132 134] + linkNode [129 133 135] } data { - id 134 + id 135 ssIdNumber 69 name "HessianMatrixTwoFeet" scope OUTPUT_DATA @@ -20814,10 +21403,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 133 135] + linkNode [129 134 136] } data { - id 135 + id 136 ssIdNumber 70 name "gradientTwoFeet" scope OUTPUT_DATA @@ -20839,10 +21428,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 134 136] + linkNode [129 135 137] } data { - id 136 + id 137 ssIdNumber 53 name "ConstraintsMatrixTwoFeet" scope OUTPUT_DATA @@ -20864,10 +21453,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 135 137] + linkNode [129 136 138] } data { - id 137 + id 138 ssIdNumber 54 name "bVectorConstraintsTwoFeet" scope OUTPUT_DATA @@ -20889,10 +21478,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 136 138] + linkNode [129 137 139] } data { - id 138 + id 139 ssIdNumber 57 name "tauModel" scope OUTPUT_DATA @@ -20914,10 +21503,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 137 139] + linkNode [129 138 140] } data { - id 139 + id 140 ssIdNumber 58 name "Sigma" scope OUTPUT_DATA @@ -20939,10 +21528,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 138 140] + linkNode [129 139 141] } data { - id 140 + id 141 ssIdNumber 62 name "Na" scope OUTPUT_DATA @@ -20964,10 +21553,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 139 141] + linkNode [129 140 142] } data { - id 141 + id 142 ssIdNumber 63 name "f_LDot" scope OUTPUT_DATA @@ -20989,10 +21578,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 140 142] + linkNode [129 141 143] } data { - id 142 + id 143 ssIdNumber 13 name "feetContactStatus" scope INPUT_DATA @@ -21014,10 +21603,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 141 143] + linkNode [129 142 144] } data { - id 143 + id 144 ssIdNumber 50 name "ConstraintsMatrix" scope INPUT_DATA @@ -21039,10 +21628,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 142 144] + linkNode [129 143 145] } data { - id 144 + id 145 ssIdNumber 51 name "bVectorConstraints" scope INPUT_DATA @@ -21064,10 +21653,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 143 145] + linkNode [129 144 146] } data { - id 145 + id 146 ssIdNumber 14 name "jointPos" scope INPUT_DATA @@ -21089,10 +21678,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 144 146] + linkNode [129 145 147] } data { - id 146 + id 147 ssIdNumber 4 name "jointPos_des" scope INPUT_DATA @@ -21111,10 +21700,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 145 147] + linkNode [129 146 148] } data { - id 147 + id 148 ssIdNumber 7 name "nu" scope INPUT_DATA @@ -21136,10 +21725,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 146 148] + linkNode [129 147 149] } data { - id 148 + id 149 ssIdNumber 8 name "M" scope INPUT_DATA @@ -21161,10 +21750,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 147 149] + linkNode [129 148 150] } data { - id 149 + id 150 ssIdNumber 9 name "h" scope INPUT_DATA @@ -21186,10 +21775,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 148 150] + linkNode [129 149 151] } data { - id 150 + id 151 ssIdNumber 11 name "L" scope INPUT_DATA @@ -21211,10 +21800,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 149 151] + linkNode [129 150 152] } data { - id 151 + id 152 ssIdNumber 6 name "intL_angMomError" scope INPUT_DATA @@ -21236,10 +21825,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 150 152] + linkNode [129 151 153] } data { - id 152 + id 153 ssIdNumber 42 name "w_H_l_sole" scope INPUT_DATA @@ -21261,10 +21850,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 151 153] + linkNode [129 152 154] } data { - id 153 + id 154 ssIdNumber 12 name "w_H_r_sole" scope INPUT_DATA @@ -21286,10 +21875,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 152 154] + linkNode [129 153 155] } data { - id 154 + id 155 ssIdNumber 77 name "J_l_sole" scope INPUT_DATA @@ -21311,10 +21900,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 153 155] + linkNode [129 154 156] } data { - id 155 + id 156 ssIdNumber 38 name "J_r_sole" scope INPUT_DATA @@ -21336,10 +21925,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 154 156] + linkNode [129 155 157] } data { - id 156 + id 157 ssIdNumber 32 name "JDot_l_sole_nu" scope INPUT_DATA @@ -21361,10 +21950,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 155 157] + linkNode [129 156 158] } data { - id 157 + id 158 ssIdNumber 33 name "JDot_r_sole_nu" scope INPUT_DATA @@ -21386,10 +21975,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 156 158] + linkNode [129 157 159] } data { - id 158 + id 159 ssIdNumber 40 name "pos_CoM" scope INPUT_DATA @@ -21411,10 +22000,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 157 159] + linkNode [129 158 160] } data { - id 159 + id 160 ssIdNumber 16 name "J_CoM" scope INPUT_DATA @@ -21436,10 +22025,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 158 160] + linkNode [129 159 161] } data { - id 160 + id 161 ssIdNumber 15 name "desired_pos_vel_acc_CoM" scope INPUT_DATA @@ -21461,10 +22050,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 159 161] + linkNode [129 160 162] } data { - id 161 + id 162 ssIdNumber 17 name "KP_CoM" scope INPUT_DATA @@ -21486,10 +22075,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 160 162] + linkNode [129 161 163] } data { - id 162 + id 163 ssIdNumber 81 name "KD_CoM" scope INPUT_DATA @@ -21511,10 +22100,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 161 163] + linkNode [129 162 164] } data { - id 163 + id 164 ssIdNumber 82 name "KP_postural" scope INPUT_DATA @@ -21536,10 +22125,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 162 164] + linkNode [129 163 165] } data { - id 164 + id 165 ssIdNumber 20 name "Reg" scope PARAMETER_DATA @@ -21562,10 +22151,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 163 165] + linkNode [129 164 166] } data { - id 165 + id 166 ssIdNumber 47 name "Gain" scope PARAMETER_DATA @@ -21588,10 +22177,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 164 166] + linkNode [129 165 167] } data { - id 166 + id 167 ssIdNumber 19 name "Config" scope PARAMETER_DATA @@ -21613,19 +22202,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [128 165 0] + linkNode [129 166 0] } junction { - id 167 + id 168 position [23.5747 49.5747 7] - chart 128 - subviewer 128 + chart 129 + subviewer 129 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [128 0 0] + linkNode [129 0 0] } transition { - id 168 + id 169 labelString "{eML_blk_kernel();}" labelPosition [36.125 25.875 102.544 14.964] fontSize 12 @@ -21633,65 +22222,65 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 167 + id 168 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 128 + chart 129 dataLimits [21.175 25.975 14.625 42.575] - subviewer 128 + subviewer 129 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [128 0 0] + linkNode [129 0 0] } instance { - id 169 + id 170 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Momentum Based Balancing Controller\n" - chart 128 + chart 129 } chart { - id 170 + 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 171 0 0] - viewObj 170 - ssIdHighWaterMark 8 + treeNode [0 172 0 0] + viewObj 171 + ssIdHighWaterMark 9 decomposition CLUSTER_CHART type EML_CHART chartFileNumber 18 disableImplicitCasting 1 eml { - name "checkRangeFCN" + name "checkInputRangeFCN" } supportVariableSizing 0 - firstData 172 - firstTransition 178 - firstJunction 177 + firstData 173 + firstTransition 180 + firstJunction 179 } state { - id 171 + id 172 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 170 - treeNode [170 0 0 0] + chart 171 + treeNode [171 0 0 0] superState SUBCHART - subviewer 170 + subviewer 171 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE eml { isEML 1 - script "function inRange = checkRangeFCN(umin, umax, u, tol)\n\n inRange = wbc.checkRange(umin, umax," - " u, tol);\nend" + 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)" @@ -21700,7 +22289,7 @@ Stateflow { } } data { - id 172 + id 173 ssIdNumber 4 name "umin" scope INPUT_DATA @@ -21722,10 +22311,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [170 0 173] + linkNode [171 0 174] } data { - id 173 + id 174 ssIdNumber 5 name "umax" scope INPUT_DATA @@ -21747,10 +22336,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [170 172 174] + linkNode [171 173 175] } data { - id 174 + id 175 ssIdNumber 6 name "u" scope INPUT_DATA @@ -21769,10 +22358,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [170 173 175] + linkNode [171 174 176] } data { - id 175 + id 176 ssIdNumber 7 name "inRange" scope OUTPUT_DATA @@ -21792,10 +22381,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [170 174 176] + linkNode [171 175 177] } data { - id 176 + id 177 ssIdNumber 8 name "tol" scope INPUT_DATA @@ -21817,19 +22406,44 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [170 175 0] + 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 177 + id 179 position [23.5747 49.5747 7] - chart 170 - subviewer 170 + chart 171 + subviewer 171 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [170 0 0] + linkNode [171 0 0] } transition { - id 178 + id 180 labelString "{eML_blk_kernel();}" labelPosition [40.125 31.875 102.544 14.964] fontSize 12 @@ -21837,37 +22451,37 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 177 + id 179 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 170 + chart 171 dataLimits [21.175 25.975 14.625 42.575] - subviewer 170 + subviewer 171 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [170 0 0] + linkNode [171 0 0] } instance { - id 179 + id 181 machine 1 name "MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" - chart 170 + chart 171 } chart { - id 180 + 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 181 0 0] - viewObj 180 + treeNode [0 183 0 0] + viewObj 182 ssIdHighWaterMark 5 decomposition CLUSTER_CHART type EML_CHART @@ -21876,19 +22490,19 @@ Stateflow { eml { name "footOnGround" } - firstData 182 - firstTransition 185 - firstJunction 184 + firstData 184 + firstTransition 187 + firstJunction 186 } state { - id 181 + id 183 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 180 - treeNode [180 0 0 0] + chart 182 + treeNode [182 0 0 0] superState SUBCHART - subviewer 180 + subviewer 182 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -21903,7 +22517,7 @@ Stateflow { } } data { - id 182 + id 184 ssIdNumber 4 name "state" scope INPUT_DATA @@ -21922,10 +22536,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [180 0 183] + linkNode [182 0 185] } data { - id 183 + id 185 ssIdNumber 5 name "booleanState" scope OUTPUT_DATA @@ -21945,19 +22559,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [180 182 0] + linkNode [182 184 0] } junction { - id 184 + id 186 position [23.5747 49.5747 7] - chart 180 - subviewer 180 + chart 182 + subviewer 182 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [180 0 0] + linkNode [182 0 0] } transition { - id 185 + id 187 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -21965,38 +22579,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 184 + id 186 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 180 + chart 182 dataLimits [21.175 25.975 14.625 42.575] - subviewer 180 + subviewer 182 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [180 0 0] + linkNode [182 0 0] } instance { - id 186 + id 188 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Select ba" "se to world transform" - chart 180 + chart 182 } chart { - id 187 + 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 188 0 0] - viewObj 187 + treeNode [0 190 0 0] + viewObj 189 ssIdHighWaterMark 10 decomposition CLUSTER_CHART type EML_CHART @@ -22005,19 +22619,19 @@ Stateflow { eml { name "getEquivalentBaseVel_FCN" } - firstData 189 - firstTransition 196 - firstJunction 195 + firstData 191 + firstTransition 198 + firstJunction 197 } state { - id 188 + id 190 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 187 - treeNode [187 0 0 0] + chart 189 + treeNode [189 0 0 0] superState SUBCHART - subviewer 187 + subviewer 189 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -22033,7 +22647,7 @@ Stateflow { } } data { - id 189 + id 191 ssIdNumber 4 name "J_l_sole" scope INPUT_DATA @@ -22052,10 +22666,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [187 0 190] + linkNode [189 0 192] } data { - id 190 + id 192 ssIdNumber 6 name "J_r_sole" scope INPUT_DATA @@ -22077,10 +22691,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [187 189 191] + linkNode [189 191 193] } data { - id 191 + id 193 ssIdNumber 9 name "feetContactStatus" scope INPUT_DATA @@ -22102,10 +22716,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [187 190 192] + linkNode [189 192 194] } data { - id 192 + id 194 ssIdNumber 10 name "jointPos_err" scope INPUT_DATA @@ -22127,10 +22741,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [187 191 193] + linkNode [189 193 195] } data { - id 193 + id 195 ssIdNumber 7 name "baseVel_equivalent" scope OUTPUT_DATA @@ -22152,10 +22766,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [187 192 194] + linkNode [189 194 196] } data { - id 194 + id 196 ssIdNumber 8 name "Reg" scope PARAMETER_DATA @@ -22177,19 +22791,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [187 193 0] + linkNode [189 195 0] } junction { - id 195 + id 197 position [23.5747 49.5747 7] - chart 187 - subviewer 187 + chart 189 + subviewer 189 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [187 0 0] + linkNode [189 0 0] } transition { - id 196 + id 198 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -22197,38 +22811,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 195 + id 197 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 187 + chart 189 dataLimits [21.175 25.975 14.625 42.575] - subviewer 187 + subviewer 189 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [187 0 0] + linkNode [189 0 0] } instance { - id 197 + id 199 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Get Equiv" "alent Base Velocity" - chart 187 + chart 189 } chart { - id 198 + 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 199 0 0] - viewObj 198 + treeNode [0 201 0 0] + viewObj 200 ssIdHighWaterMark 6 decomposition CLUSTER_CHART type EML_CHART @@ -22237,19 +22851,19 @@ Stateflow { eml { name "analyticalSolutionQP_FCN" } - firstData 200 - firstTransition 204 - firstJunction 203 + firstData 202 + firstTransition 206 + firstJunction 205 } state { - id 199 + id 201 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 198 - treeNode [198 0 0 0] + chart 200 + treeNode [200 0 0 0] superState SUBCHART - subviewer 198 + subviewer 200 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -22261,7 +22875,7 @@ Stateflow { } } data { - id 200 + id 202 ssIdNumber 4 name "H" scope INPUT_DATA @@ -22280,10 +22894,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [198 0 201] + linkNode [200 0 203] } data { - id 201 + id 203 ssIdNumber 5 name "analyticalSolution" scope OUTPUT_DATA @@ -22303,10 +22917,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [198 200 202] + linkNode [200 202 204] } data { - id 202 + id 204 ssIdNumber 6 name "g" scope INPUT_DATA @@ -22328,19 +22942,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [198 201 0] + linkNode [200 203 0] } junction { - id 203 + id 205 position [23.5747 49.5747 7] - chart 198 - subviewer 198 + chart 200 + subviewer 200 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [198 0 0] + linkNode [200 0 0] } transition { - id 204 + id 206 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -22348,38 +22962,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 203 + id 205 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 198 + chart 200 dataLimits [21.175 25.975 14.625 42.575] - subviewer 198 + subviewer 200 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [198 0 0] + linkNode [200 0 0] } instance { - id 205 + 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 198 + chart 200 } chart { - id 206 + 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 207 0 0] - viewObj 206 + treeNode [0 209 0 0] + viewObj 208 ssIdHighWaterMark 14 decomposition CLUSTER_CHART type EML_CHART @@ -22388,19 +23002,19 @@ Stateflow { eml { name "processOutputQP_twoFeetFCN" } - firstData 208 - firstTransition 214 - firstJunction 213 + firstData 210 + firstTransition 216 + firstJunction 215 } state { - id 207 + id 209 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 206 - treeNode [206 0 0 0] + chart 208 + treeNode [208 0 0 0] superState SUBCHART - subviewer 206 + subviewer 208 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -22412,7 +23026,7 @@ Stateflow { } } data { - id 208 + id 210 ssIdNumber 7 name "analyticalSolution" scope INPUT_DATA @@ -22434,10 +23048,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [206 0 209] + linkNode [208 0 211] } data { - id 209 + id 211 ssIdNumber 4 name "primalSolution" scope INPUT_DATA @@ -22456,10 +23070,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [206 208 210] + linkNode [208 210 212] } data { - id 210 + id 212 ssIdNumber 6 name "QPStatus" scope INPUT_DATA @@ -22481,10 +23095,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [206 209 211] + linkNode [208 211 213] } data { - id 211 + id 213 ssIdNumber 5 name "f_star" scope OUTPUT_DATA @@ -22504,10 +23118,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [206 210 212] + linkNode [208 212 214] } data { - id 212 + id 214 ssIdNumber 10 name "Config" scope PARAMETER_DATA @@ -22529,19 +23143,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [206 211 0] + linkNode [208 213 0] } junction { - id 213 + id 215 position [23.5747 49.5747 7] - chart 206 - subviewer 206 + chart 208 + subviewer 208 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [206 0 0] + linkNode [208 0 0] } transition { - id 214 + id 216 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -22549,38 +23163,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 213 + id 215 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 206 + chart 208 dataLimits [21.175 25.975 14.625 42.575] - subviewer 206 + subviewer 208 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [206 0 0] + linkNode [208 0 0] } instance { - id 215 + id 217 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Proc" "ess QP output" - chart 206 + chart 208 } chart { - id 216 + 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 217 0 0] - viewObj 216 + treeNode [0 219 0 0] + viewObj 218 ssIdHighWaterMark 6 decomposition CLUSTER_CHART type EML_CHART @@ -22589,19 +23203,19 @@ Stateflow { eml { name "analyticalSolutionQP_FCN" } - firstData 218 - firstTransition 222 - firstJunction 221 + firstData 220 + firstTransition 224 + firstJunction 223 } state { - id 217 + id 219 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 216 - treeNode [216 0 0 0] + chart 218 + treeNode [218 0 0 0] superState SUBCHART - subviewer 216 + subviewer 218 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -22613,7 +23227,7 @@ Stateflow { } } data { - id 218 + id 220 ssIdNumber 4 name "H" scope INPUT_DATA @@ -22632,10 +23246,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [216 0 219] + linkNode [218 0 221] } data { - id 219 + id 221 ssIdNumber 5 name "analyticalSolution" scope OUTPUT_DATA @@ -22655,10 +23269,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [216 218 220] + linkNode [218 220 222] } data { - id 220 + id 222 ssIdNumber 6 name "g" scope INPUT_DATA @@ -22680,19 +23294,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [216 219 0] + linkNode [218 221 0] } junction { - id 221 + id 223 position [23.5747 49.5747 7] - chart 216 - subviewer 216 + chart 218 + subviewer 218 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [216 0 0] + linkNode [218 0 0] } transition { - id 222 + id 224 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -22700,38 +23314,38 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 221 + id 223 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 216 + chart 218 dataLimits [21.175 25.975 14.625 42.575] - subviewer 216 + subviewer 218 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [216 0 0] + linkNode [218 0 0] } instance { - id 223 + 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 216 + chart 218 } chart { - id 224 + 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 225 0 0] - viewObj 224 + treeNode [0 227 0 0] + viewObj 226 ssIdHighWaterMark 14 decomposition CLUSTER_CHART type EML_CHART @@ -22740,19 +23354,19 @@ Stateflow { eml { name "processOutputQP_oneFoot" } - firstData 226 - firstTransition 233 - firstJunction 232 + firstData 228 + firstTransition 235 + firstJunction 234 } state { - id 225 + id 227 labelString "eML_blk_kernel()" position [18 64.5 118 66] fontSize 12 - chart 224 - treeNode [224 0 0 0] + chart 226 + treeNode [226 0 0 0] superState SUBCHART - subviewer 224 + subviewer 226 ssIdNumber 1 type FUNC_STATE decomposition CLUSTER_STATE @@ -22765,7 +23379,7 @@ Stateflow { } } data { - id 226 + id 228 ssIdNumber 7 name "analyticalSolution" scope INPUT_DATA @@ -22787,10 +23401,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [224 0 227] + linkNode [226 0 229] } data { - id 227 + id 229 ssIdNumber 4 name "primalSolution" scope INPUT_DATA @@ -22809,10 +23423,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [224 226 228] + linkNode [226 228 230] } data { - id 228 + id 230 ssIdNumber 6 name "QPStatus" scope INPUT_DATA @@ -22834,10 +23448,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [224 227 229] + linkNode [226 229 231] } data { - id 229 + id 231 ssIdNumber 14 name "feetContactStatus" scope INPUT_DATA @@ -22859,10 +23473,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [224 228 230] + linkNode [226 230 232] } data { - id 230 + id 232 ssIdNumber 5 name "f_star" scope OUTPUT_DATA @@ -22882,10 +23496,10 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [224 229 231] + linkNode [226 231 233] } data { - id 231 + id 233 ssIdNumber 10 name "Config" scope PARAMETER_DATA @@ -22907,19 +23521,19 @@ Stateflow { } } dataType "Inherit: Same as Simulink" - linkNode [224 230 0] + linkNode [226 232 0] } junction { - id 232 + id 234 position [23.5747 49.5747 7] - chart 224 - subviewer 224 + chart 226 + subviewer 226 ssIdNumber 3 type CONNECTIVE_JUNCTION - linkNode [224 0 0] + linkNode [226 0 0] } transition { - id 233 + id 235 labelString "{eML_blk_kernel();}" labelPosition [28.125 13.875 102.544 14.964] fontSize 12 @@ -22927,39 +23541,39 @@ Stateflow { intersection [0 0 1 0 23.5747 14.625 0 0] } dst { - id 232 + id 234 intersection [1 0 -1 0 23.5747 42.5747 0 0] } midPoint [23.5747 24.9468] - chart 224 + chart 226 dataLimits [21.175 25.975 14.625 42.575] - subviewer 224 + subviewer 226 drawStyle SMART slide { sticky BOTH_STICK } executionOrder 1 ssIdNumber 2 - linkNode [224 0 0] + linkNode [226 0 0] } instance { - id 234 + id 236 machine 1 name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Proc" "ess QP output" - chart 224 + chart 226 } target { - id 235 + id 237 machine 1 name "sfun" codeFlags "" - linkNode [1 0 236] + linkNode [1 0 238] } target { - id 236 + id 238 machine 1 name "rtw" - linkNode [1 235 0] + linkNode [1 237 0] } } diff --git a/controllers/simulink-balancing-simulator/README.md b/controllers/simulink-balancing-simulator/README.md new file mode 100644 index 0000000..14d37df --- /dev/null +++ b/controllers/simulink-balancing-simulator/README.md @@ -0,0 +1,35 @@ +## Module description + +This Simulink model implements both control and dynamics simulation of a torque controlled humanoid robot. The joint torques references are calculated with a momentum-based controller, and used to estimate the system accelerations with forward dynamics. Then, forward dynamics is integrated inside Simulink. + + + +### Compatibility + +The folder contains the Simulink model `torqueControlBalancingSim.mdl`, which is generated by using Matlab R2017b. + +### Supported robots + +Currently, supported robots are: `iCubGazeboV2_5`. + +## Module details + +### How to run the demo + +In the current configuration, `WBToolbox` blocks require to run `yarpserver` on terminal, and to open `Gazebo` and insert the `iCubGazeboV2_5` model. This actions could be avoided by substituting all the `WBToolbox` blocks with proper Matlab-Simulink functions. After opening Yarp and Gazebo, just run the Simulink controller to start. + +#### How to visualize the robot movements + +At the moment, no visualization is provided. A way to visualize the robot is to use Gazebo, but the current YARP-Gazebo workflow does not allow to remove physics from the simulator, so the robot may fall down and fail in Gazebo even if according to Simulink the robot succeed to balance. + +#### Limits of the current implementation + +The `contact switching` demo (`YOGA`) can only perform switching from two feet to one foot balancing, as switching from one foot to two feet requires to trigger the instant the robot is touching the ground, but no contact model is simulated at the moment. + +### Configuration file + +At start, the module calls the initialization file initTorqueControlBalancingSim.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/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/configRobot.m b/controllers/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/configRobot.m new file mode 100644 index 0000000..b85a469 --- /dev/null +++ b/controllers/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/configRobot.m @@ -0,0 +1,108 @@ +% CONFIGROBOT initializes parameters specific of a particular robot +% (e.g., icubGazeboSim) + +%% --- Initialization --- + +Config.ON_GAZEBO = true; +ROBOT_DOF = 23; +Config.GRAV_ACC = 9.81; + +%%%% INITIAL CONDITIONS %%%% +Config.init_jointPos = [ 0.0,0.0,0.0, ... + -30.0,30.0,0.0,45.0, ... + -30.0,30.0,0.0,45.0, ... % + 0.0,0.0,0.0,0.0,0.0,0.0, ... + 0.0,0.0,0.0,0.0,0.0,0.0]*pi/180; +%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% 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 = false; + +% Config.USE_MOTOR_REFLECTED_INERTIA: if set to true, motors reflected +% inertias are included in the system mass matrix. If +% Config.INCLUDE_COUPLING is true, then the coupling effect (some joints +% motion is the result of more than one motor motion) is taken into account. +% Config.INCLUDE_HARMONIC_DRIVE_INERTIA is true, then the harmonic drive +% reflected inertia is also considered +Config.USE_MOTOR_REFLECTED_INERTIA = false; +Config.INCLUDE_COUPLING = false; +Config.INCLUDE_HARMONIC_DRIVE_INERTIA = false; + +% Config.USE_IMU4EST_BASE: if set to false, the base frame is estimated by +% assuming that either the left or the right foot stay stuck on the ground. +% Which foot the controller uses depends on the contact forces acting on it. +% If set to true, the base orientation is estimated by using the IMU, while +% the base position by assuming that the origin of either the right or the +% left foot do not move. +Config.USE_IMU4EST_BASE = false; + +% Config.YAW_IMU_FILTER when the flag Config.USE_IMU4EST_BASE = true, then +% the orientation of the floating base is estimated as explained above. However, +% the foot is usually perpendicular to gravity when the robot stands on flat +% surfaces, and rotation about the gravity axis may be affected by the IMU drift +% in estimating this angle. Hence, when either of the flags Config.YAW_IMU_FILTER +% is set to true, then the yaw angle of the contact foot is ignored and kept +% equal to the initial value. +Config.FILTER_IMU_YAW = true; + +% Config.CORRECT_NECK_IMU: when set equal to true, the kinematics from the +% IMU and the contact foot is corrected by using the neck angles. If it set +% equal to false, recall that the neck is assumed to be in (0,0,0). Valid +% ONLY while using the ICUB HEAD IMU! +Config.CORRECT_NECK_IMU = false; + +% Config.USE_QP_SOLVER: if set to true, a QP solver is used to account for +% inequality constraints of contact wrenches. +Config.USE_QP_SOLVER = true; + +% Ports name list +Ports.WRENCH_LEFT_FOOT = '/wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o'; +Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o'; +Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; +Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; + +% Ports dimensions +Ports.NECK_POS_PORT_SIZE = 3; +Ports.IMU_PORT_SIZE = 12; +Ports.IMU_PORT_ORIENTATION_INDEX = [1,2,3]; +Ports.WRENCH_PORT_SIZE = 6; \ No newline at end of file diff --git a/controllers/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m b/controllers/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m new file mode 100644 index 0000000..fb21333 --- /dev/null +++ b/controllers/simulink-balancing-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 = 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/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m b/controllers/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m new file mode 100644 index 0000000..fd01596 --- /dev/null +++ b/controllers/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m @@ -0,0 +1,347 @@ +% 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 50 % state == 1 TWO FEET BALANCING + 50 50 50 % state == 2 COM TRANSITION TO LEFT + 50 50 50 % state == 3 LEFT FOOT BALANCING + 50 50 50 % state == 4 YOGA LEFT FOOT + 50 50 50 % state == 5 PREPARING FOR SWITCHING + 50 50 50 % state == 6 LOOKING FOR CONTACT + 50 50 50 % state == 7 TRANSITION TO INITIAL POSITION + 50 50 50 % state == 8 COM TRANSITION TO RIGHT FOOT + 50 50 50 % state == 9 RIGHT FOOT BALANCING + 50 50 50 % state == 10 YOGA RIGHT FOOT + 50 50 50 % state == 11 PREPARING FOR SWITCHING + 50 50 50 % state == 12 LOOKING FOR CONTACT + 50 50 50]; % state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_CoM = 2*sqrt(Gain.KP_CoM); + +% Angular momentum gains +Gain.KI_AngularMomentum = 5 ; +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 + +for k = 1:size(Gain.KP_postural,1) + + Gain.KP_postural(k,12:end) = Gain.KP_postural(1,12:end); +end + +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.08; %% state == 4 YOGA LEFT FOOT + 0.0, 0.00, 0.0; %% state == 5 PREPARING FOR SWITCHING + 0.0, -0.02, 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/simulink-balancing-simulator/initTorqueControlBalancingSim.m b/controllers/simulink-balancing-simulator/initTorqueControlBalancingSim.m new file mode 100644 index 0000000..305aac0 --- /dev/null +++ b/controllers/simulink-balancing-simulator/initTorqueControlBalancingSim.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 = 70; + +% 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/simulink-balancing-simulator/src-static-gui/closeModel.m b/controllers/simulink-balancing-simulator/src-static-gui/closeModel.m new file mode 100644 index 0000000..5399362 --- /dev/null +++ b/controllers/simulink-balancing-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('torqueControlBalancingSim.mdl'); +close_system('torqueControlBalancingSim.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/simulink-balancing-simulator/src-static-gui/compileModel.m b/controllers/simulink-balancing-simulator/src-static-gui/compileModel.m new file mode 100644 index 0000000..583b4c0 --- /dev/null +++ b/controllers/simulink-balancing-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 + torqueControlBalancingSim([], [], [], 'compile') + torqueControlBalancingSim([], [], [], '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/simulink-balancing-simulator/src/computeBasePoseDerivative.m b/controllers/simulink-balancing-simulator/src/computeBasePoseDerivative.m new file mode 100644 index 0000000..57197c4 --- /dev/null +++ b/controllers/simulink-balancing-simulator/src/computeBasePoseDerivative.m @@ -0,0 +1,20 @@ +function pose_base_dot = computeBasePoseDerivative(nu_base_ikin, pose_base) + + % COMPUTEBASEPOSEDERIVATIVE computes the base position and orientation + % time derivative in terms of linear velocity + % + quaternion derivative. + + %% --- Initialization --- + + % angular velocity in body coordinates + w_omega_base = nu_base_ikin(4:end); + w_R_base = wbc.rotationFromQuaternion(pose_base(4:7)); + b_omega_base = transpose(w_R_base)*w_omega_base; + + % calculate the base quaternion derivative + quat_base = pose_base(4:end); + dquat_base = wbc.quaternionDerivative(quat_base,b_omega_base,1); + + % compute the base pose derivative + pose_base_dot = [nu_base_ikin(1:3); dquat_base]; +end \ No newline at end of file diff --git a/controllers/simulink-balancing-simulator/src/estimateContactForcesFromDynamics.m b/controllers/simulink-balancing-simulator/src/estimateContactForcesFromDynamics.m new file mode 100644 index 0000000..c3cdfe6 --- /dev/null +++ b/controllers/simulink-balancing-simulator/src/estimateContactForcesFromDynamics.m @@ -0,0 +1,36 @@ +function f_dynamics = estimateContactForcesFromDynamics(tau, JL, JR, JDotL_nu, JDotR_nu, M, h, LEFT_RIGHT_FOOT_IN_CONTACT) + + % try to estimate the contact forces using the joint torques, the + % dynamics model, the robot state and the contact constraints. + ndof = size(M,1)-6; + B = [zeros(6,ndof); eye(ndof)]; + f_dynamics = zeros(12,1); + + if sum(LEFT_RIGHT_FOOT_IN_CONTACT) > 1.5 + + % two feet balancing + Jc = [JL; JR]; + JcDot_nu = [JDotL_nu; JDotR_nu]; + + pinvDampJMJ = eye(12)/(Jc/M*Jc'); + f_dynamics = pinvDampJMJ*(Jc/M*(h-B*tau)-JcDot_nu); + + elseif LEFT_RIGHT_FOOT_IN_CONTACT(1) > 0.5 && LEFT_RIGHT_FOOT_IN_CONTACT(2) < 0.5 + + % left foot balancing + Jc_left = JL; + Jc_leftDot_nu = JDotL_nu; + + pinvDampJMJ_left = eye(6)/(Jc_left/M*Jc_left'); + f_dynamics = [pinvDampJMJ_left*(Jc_left/M*(h-B*tau)-Jc_leftDot_nu); zeros(6,1)]; + + elseif LEFT_RIGHT_FOOT_IN_CONTACT(2) > 0.5 && LEFT_RIGHT_FOOT_IN_CONTACT(1) < 0.5 + + % right foot balancing + Jc_right = JR; + Jc_rightDot_nu = JDotR_nu; + + pinvDampJMJ_right = eye(6)/(Jc_right/M*Jc_right'); + f_dynamics = [zeros(6,1); pinvDampJMJ_right*(Jc_right/M*(h-B*tau)-Jc_rightDot_nu)]; + end +end \ No newline at end of file diff --git a/controllers/simulink-balancing-simulator/src/forwardDynamics.m b/controllers/simulink-balancing-simulator/src/forwardDynamics.m new file mode 100644 index 0000000..6cf9cf6 --- /dev/null +++ b/controllers/simulink-balancing-simulator/src/forwardDynamics.m @@ -0,0 +1,10 @@ +function nuDot = forwardDynamics(tau, JL, JR, M, h, f, LEFT_RIGHT_FOOT_IN_CONTACT) + + % get the robot accelerations using the joint torques, the + % dynamics model, the robot state and the contact constraints. + ndof = size(M,1)-6; + B = [zeros(6,ndof); eye(ndof)]; + + nuDot = M\(B*tau+LEFT_RIGHT_FOOT_IN_CONTACT(1)*JL'*f(1:6)+LEFT_RIGHT_FOOT_IN_CONTACT(2)*JR'*f(7:end)-h); + +end \ No newline at end of file diff --git a/controllers/simulink-balancing-simulator/src/momentumBasedController.m b/controllers/simulink-balancing-simulator/src/momentumBasedController.m new file mode 100644 index 0000000..3456004 --- /dev/null +++ b/controllers/simulink-balancing-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 \ No newline at end of file diff --git a/controllers/simulink-balancing-simulator/src/processOutputQP_oneFoot.m b/controllers/simulink-balancing-simulator/src/processOutputQP_oneFoot.m new file mode 100644 index 0000000..27a00f8 --- /dev/null +++ b/controllers/simulink-balancing-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/simulink-balancing-simulator/src/processOutputQP_twoFeet.m b/controllers/simulink-balancing-simulator/src/processOutputQP_twoFeet.m new file mode 100644 index 0000000..7b32870 --- /dev/null +++ b/controllers/simulink-balancing-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/simulink-balancing-simulator/src/referenceGeneratorCoM.m b/controllers/simulink-balancing-simulator/src/referenceGeneratorCoM.m new file mode 100644 index 0000000..513e203 --- /dev/null +++ b/controllers/simulink-balancing-simulator/src/referenceGeneratorCoM.m @@ -0,0 +1,18 @@ +function references_CoM = referenceGeneratorCoM(posCoM_0, t, Config) + + % REFERENCEGENERATORCOM generates sinusoidal references for the robot + % CoM position, velocity and acceleration. + + %% --- Initialization --- + + ampl = Config.referencesCoM.amplitude; + freq = Config.referencesCoM.frequency; + dir = Config.referencesCoM.direction; + + % CoM references + posCoM_des = posCoM_0 + ampl * sin(2*pi*freq*t) * dir; + velCoM_des = ampl * 2 * pi * freq * cos(2*pi*freq*t) * dir; + accCoM_des = -ampl * 4 * pi^2 * freq^2 * sin(2*pi*freq*t) * dir; + + references_CoM = [posCoM_des; velCoM_des; accCoM_des]; +end \ No newline at end of file diff --git a/controllers/simulink-balancing-simulator/src/stateMachineMomentumControl.m b/controllers/simulink-balancing-simulator/src/stateMachineMomentumControl.m new file mode 100644 index 0000000..7e3078d --- /dev/null +++ b/controllers/simulink-balancing-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/simulink-balancing-simulator/startModelWithStaticGui.m b/controllers/simulink-balancing-simulator/startModelWithStaticGui.m new file mode 100644 index 0000000..bf9d5d3 --- /dev/null +++ b/controllers/simulink-balancing-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('torqueControlBalancingSim.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/simulink-balancing-simulator/stopTorqueControlBalancingSim.m b/controllers/simulink-balancing-simulator/stopTorqueControlBalancingSim.m new file mode 100644 index 0000000..17d8c53 --- /dev/null +++ b/controllers/simulink-balancing-simulator/stopTorqueControlBalancingSim.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/simulink-balancing-simulator/torqueControlBalancingSim.mdl b/controllers/simulink-balancing-simulator/torqueControlBalancingSim.mdl new file mode 100644 index 0000000..0ace9bb --- /dev/null +++ b/controllers/simulink-balancing-simulator/torqueControlBalancingSim.mdl @@ -0,0 +1,26374 @@ +Model { + Name "torqueControlBalancingSim" + Version 9.0 + SavedCharacterEncoding "UTF-8" + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.3540" + NumModelReferences 0 + NumTestPointedSignals 0 + NumProvidedFunctions 0 + NumRequiredFunctions 0 + NumResetEvents 0 + HasInitializeEvent 0 + HasTerminateEvent 0 + IsExportFunctionModel 0 + NumParameterArguments 0 + NumExternalFileReferences 47 + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Configuration" + Path "torqueControlBalancingSim/Configuration" + SID "4537" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "torqueControlBalancingSim/Dump and visualize/Visualizer/GetMeasurement1" + SID "4543" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingSim/Init. Cond./Transformation Matrix From FixedLink to Base/RelativeTransfor" + "m Between l_sole and Base" + SID "4976" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingSim/Init. Cond./Transformation Matrix From FixedLink to Base/RelativeTransfor" + "m Between r_sole and Base" + SID "4977" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MatchSignalSizes" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Balancing Controller QP/Compute Desired To" + "rques/QPSolver/QP One Foot/MatchSignalSizes" + SID "4691" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/QP" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Balancing Controller QP/Compute Desired To" + "rques/QPSolver/QP One Foot/QP One Foot" + SID "4693" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MatchSignalSizes" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Balancing Controller QP/Compute Desired To" + "rques/QPSolver/QP Two Feet/MatchSignalSizes" + SID "4679" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/QP" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Balancing Controller QP/Compute Desired To" + "rques/QPSolver/QP Two Feet/QP Two Feet" + SID "4681" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Balancing Controller QP/Compute angular mo" + "mentum integral error/CentroidalMomentum" + SID "3718" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Balancing Controller QP/Compute angular mo" + "mentum 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 "torqueControlBalancingSim/MBC control + forward dynamics/Balancing Controller QP/Compute angular mo" + "mentum 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 "torqueControlBalancingSim/MBC control + forward dynamics/Balancing Controller QP/Compute angular mo" + "mentum integral error/Jacobian" + SID "3719" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Balancing Controller QP/Compute angular mo" + "mentum integral error/Jacobian1" + SID "3720" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Dynamics and Kinematics/Dynamics/Centroida" + "lMomentum" + SID "2345" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/GetBiasForces" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Dynamics and Kinematics/Dynamics/GetBiasFo" + "rces" + SID "2348" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/MassMatrix" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Dynamics and Kinematics/Dynamics/MassMatrix" + SID "2349" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Dynamics and Kinematics/Kinematics/CoM" + SID "4363" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/DotJNu" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Dynamics and Kinematics/Kinematics/DotJNu " + "l_sole" + SID "2375" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/DotJNu" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Dynamics and Kinematics/Kinematics/DotJNu " + "r_sole" + SID "2376" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Dynamics and Kinematics/Kinematics/Jacobia" + "n com" + SID "2378" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Dynamics and Kinematics/Kinematics/Jacobia" + "n l_sole" + SID "2379" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Dynamics and Kinematics/Kinematics/Jacobia" + "n r_sole" + SID "2380" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Dynamics and Kinematics/Kinematics/l_sole" + SID "2405" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Dynamics and Kinematics/Kinematics/r_sole" + SID "2406" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Joint Torque Saturation/holder " + SID "4559" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute State V" + "elocity/Feet Jacobians/Jacobian l_sole" + SID "4219" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute State V" + "elocity/Feet Jacobians/Jacobian r_sole" + SID "4220" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute base 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 "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute base 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 "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute base to" + " fixed link transform/LFoot to base link transform /Neck Position" + SID "4254" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute base to" + " fixed link transform/LFoot to base link transform /holder 1" + SID "4258" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute base to" + " fixed link transform/LFoot to base link transform /holder 2" + SID "4259" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute base 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 "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute base 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 "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute base to" + " fixed link transform/RFoot to base link transform/Neck Position" + SID "4860" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute base to" + " fixed link transform/RFoot to base link transform/holder 1" + SID "4865" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute base to" + " fixed link transform/RFoot to base link transform/holder 2" + SID "4866" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute base to" + " fixed link transform/Relative transform l_sole_H_base" + SID "4269" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Compute base to" + " fixed link transform/Relative transform r_sole_H_base" + SID "4306" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/State Machine/M" + "inimumJerkTrajectoryGenerator" + SID "2176" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/State Machine/h" + "older 1" + SID "2187" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/State Machine/h" + "older 2" + SID "2188" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/State Machine/x" + "Com/CoM" + SID "4229" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/State Machine/x" + "Com2/CoM" + SID "4735" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Update Gains an" + "d References/Smooth reference CoM/MinimumJerkTrajectoryGenerator2" + SID "2153" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + Path "torqueControlBalancingSim/MBC control + forward dynamics/Robot State and References/Update Gains an" + "d References/Smooth reference joint position/MinimumJerkTrajectoryGenerator1" + SID "2152" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetLimits" + Path "torqueControlBalancingSim/MBC control + forward dynamics/emergency stop: joint limits/STOP IF JOINT" + "S HIT THE LIMITS/GetLimits" + SID "5028" + Type "LIBRARY_BLOCK" + } + OrderedModelArguments 1 + } + SLCCPlugin "on" + LogicAnalyzerPlugin "on" + DiagnosticSuppressor "on" + PostLoadFcn "%% Try to open the static GUI and try to adjust it \ntry\n\n staticGUI = simulinkStaticGUI;\n\nend" + 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)));\ninitTorqueControlBalancingSim;\n\n%% Try to" + " edit the GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n set(sl_synch_handles.sta" + "rtButton,'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)));\nstopTorqueControlBalancingSim;\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.checkb" + "ox_recompile, 'Value') \n set(sl_synch_handles.startButton,'Backgroundcolor',[0.0,1.0,0.0]);\n set(sl" + "_synch_handles.startButton,'Enable','on');\n end\n\n %% Update Compile Button\n set(sl_synch_handles.compil" + "eButton,'Backgroundcolor',[1.0,0.6,0.0]);\n set(sl_synch_handles.compileButton,'Enable','on');\n\n %% Update E" + "xit Button\n set(sl_synch_handles.exitButton,'Backgroundcolor',[0.0,1.0,1.0]);\n set(sl_synch_handles.exitButt" + "on,'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] + } + Object { + $PropName "ExplorerBarInfo" + $ObjectID 4 + $ClassName "Simulink.ExplorerBarInfo" + Visible [1] + } + Array { + Type "Simulink.EditorInfo" + Dimension 9 + Object { + $ObjectID 5 + IsActive [1] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [1815.0, 821.0] + ZoomFactor [2.44] + Offset [546.23392674180332, 421.26229508196718] + } + Object { + $ObjectID 6 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2341" + Extents [1815.0, 821.0] + ZoomFactor [1.49] + Offset [-1102.6232697147652, -185.16778523489933] + } + Object { + $ObjectID 7 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4321" + Extents [1815.0, 821.0] + ZoomFactor [1.5368217054263564] + Offset [-501.90285111916774, 25.890290037830994] + } + Object { + $ObjectID 8 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4836" + Extents [1815.0, 821.0] + ZoomFactor [0.73402501421262079] + Offset [-41.655084160680417, -55.559009512766579] + } + Object { + $ObjectID 9 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4504" + Extents [1815.0, 821.0] + ZoomFactor [0.82091097308488614] + Offset [-430.49872418820928, -525.05422446406055] + } + Object { + $ObjectID 10 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2275" + Extents [1815.0, 821.0] + ZoomFactor [1.844186046511628] + Offset [-92.087011349306408, -33.591424968474143] + } + Object { + $ObjectID 11 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4661" + Extents [1815.0, 821.0] + ZoomFactor [4.0027992625236077] + Offset [-19.21634036123433, -38.49755235818985] + } + Object { + $ObjectID 12 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "2416" + Extents [1815.0, 821.0] + ZoomFactor [4.0] + Offset [94.284029784954612, 186.406368221942] + } + Object { + $ObjectID 13 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "4664" + Extents [1815.0, 821.0] + ZoomFactor [3.9253157605711149] + Offset [-43.566592053721308, -29.952574146614438] + } + PropName "EditorsInfo" + } + Array { + Type "Simulink.DockComponentInfo" + Dimension 2 + Object { + $ObjectID 14 + Type "GLUE2:PropertyInspector" + ID "Property Inspector" + Visible [0] + CreateCallback "" + UserData "" + Floating [0] + DockPosition "Right" + Width [640] + Height [480] + } + Object { + $ObjectID 15 + Type "GLUE2:SpreadSheet" + ID "ModelData" + Visible [0] + CreateCallback "DataView.createSpreadSheetComponent" + UserData "" + Floating [0] + DockPosition "Bottom" + Width [640] + Height [480] + } + PropName "DockComponentsInfo" + } + WindowState "AAAA/wAAAAD9AAAAAwAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABiAP///wAAAAEAAAAAAAAAAPwCA" + "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" + "QBjAHQAbwByAAAAAAD/////AAAAOQD///8AAAADAAAHPQAAAPb8AQAAAAH7AAAANgBHAEwAVQBFADIAOgBTAHAAcgBlAGEAZABTAGgAZQBlAHQAL" + "wBNAG8AZABlAGwARABhAHQAYQAAAAAAAAAHPQAAARYA////AAAHPQAAA3IAAAABAAAAAgAAAAEAAAAC/AAAAAEAAAACAAAAAA==" + } + } + HideAutomaticNames on + Created "Tue Feb 18 10:13:36 2014" + Creator "daniele" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "gnava" + ModifiedDateFormat "%" + LastModifiedDate "Mon Mar 23 00:04:31 2020" + RTWModifiedTimeStamp 506822671 + ModelVersionFormat "1.%" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "disabled" + WideLines off + ShowLineDimensions on + ShowPortDataTypes 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 + ExecutionContextIcon 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 + SimulationMode "normal" + PauseTimes "5" + NumberOfSteps 1 + SnapshotBufferSize 10 + SnapshotInterval 10 + NumberOfLastSnapshots 0 + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + Object { + $PropName "DataLoggingOverride" + $ObjectID 16 + $ClassName "Simulink.SimulationData.ModelLoggingInfo" + model_ "torqueControlBalancingSim" + signals_ [] + overrideMode_ [0.0] + Array { + Type "Cell" + Dimension 1 + Cell "torqueControlBalancingSim" + 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 17 + Version "1.17.1" + DisabledProps [] + Description "" + Array { + Type "Handle" + Dimension 9 + Simulink.SolverCC { + $ObjectID 18 + Version "1.17.1" + DisabledProps [] + Description "" + Components [] + StartTime "0.0" + StopTime "Config.SIMULATION_TIME " + AbsTol "auto" + 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 + } + Simulink.DataIOCC { + $ObjectID 19 + Version "1.17.1" + DisabledProps [] + Description "" + Components [] + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState 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 20 + Version "1.17.1" + 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" + 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 on + 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 + } + Simulink.DebuggingCC { + $ObjectID 21 + Version "1.17.1" + Array { + Type "Cell" + Dimension 1 + Cell "UseOnlyExistingSharedCode" + PropName "DisabledProps" + } + Description "" + Components [] + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + 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" + InvalidFcnCallConnMsg "error" + 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" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + ChecksumConsistencyForSSReuse "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 "" + } + Simulink.HardwareCC { + $ObjectID 22 + Version "1.17.1" + 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 + } + Simulink.ModelReferenceCC { + $ObjectID 23 + Version "1.17.1" + 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 24 + Version "1.17.1" + DisabledProps [] + Description "" + Components [] + SimCustomSourceCode "" + SimCustomHeaderCode "" + SimCustomInitializer "" + SimCustomTerminator "" + SimReservedNameArray [] + SimUserSources "" + SimUserIncludeDirs "" + SimUserLibraries "" + SimUserDefines "" + SimCustomCompilerFlags "" + SimCustomLinkerFlags "" + SFSimEcho on + SimCtrlC on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode off + SimBuildMode "sf_incremental_build" + SimGenImportedTypeDefs off + ModelFunctionsGlobalVisibility "on" + CompileTimeRecursionLimit 50 + EnableRuntimeRecursion on + MATLABDynamicMemAlloc on + MATLABDynamicMemAllocThreshold 65536 + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 25 + Version "1.17.1" + 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" + 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 "" + 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 26 + Version "1.17.1" + 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 + StateflowObjectComments on + MATLABSourceComments off + EnableCustomComments off + InternalIdentifierFile "" + InternalIdentifier "Shortened" + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + ReservedNameArray [] + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 27 + Version "1.17.1" + 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 28 + Version "19.1.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 + GenerateFullHeader on + InferredTypesCompatibility off + ExistingSharedCode "" + GenerateSampleERTMain off + GenerateTestInterfaces off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + ConcurrentExecutionCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns on + CombineSignalStateStructs off + SuppressErrorStatus off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging off + MultiInstanceERTCode on + CodeInterfacePackaging "C++ class" + SupportNonFinite off + SupportComplex on + PurelyIntegerCode off + 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" + 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 29 + Version "1.17.1" + 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 "Solver" + ConfigPrmDlgPosition [ 328, 144, 1850, 1010 ] + ExtraOptions "-aBlockCommentType=\"BlockPathComment\" -aIgnoreTestpoints=0 " + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 17 + } + Object { + $PropName "DataTransfer" + $ObjectID 30 + $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 "none" + BusSelectionMode off + } + Block { + BlockType DiscreteIntegrator + IntegratorMethod "Integration: Forward Euler" + gainval "1.0" + ExternalReset "none" + InitialConditionSource "internal" + InitialCondition "0" + InitialConditionSetting "Output" + SampleTime "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit via internal rule" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow off + LimitOutput off + UpperSaturationLimit "inf" + LowerSaturationLimit "-inf" + ShowSaturationPort off + ShowStatePort off + IgnoreLimit off + StateMustResolveToSignalObject off + RTWStateStorageClass "Auto" + } + 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: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Goto + GotoTag "A" + IconDisplay "Tag" + TagVisibility "local" + } + Block { + BlockType Ground + } + Block { + BlockType Inport + Port "1" + 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" + 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 on + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Zero" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Reshape + OutputDimensionality "1-D array" + OutputDimensions "[1,1]" + } + Block { + BlockType S-Function + FunctionName "system" + SFunctionModules "''" + PortCounts "[]" + } + Block { + BlockType Saturate + UpperLimitSource "Dialog" + UpperLimit "0.5" + LowerLimitSource "Dialog" + 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 + PropExecContextOutsideSubsystem off + 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 + GeneratePreprocessorConditionals off + AllowZeroVariantControls off + PropagateVariantConditions off + TreatAsGroupedWhenPropagatingVariantConditions on + ContentPreviewEnabled off + IsWebBlock off + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + 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 "torqueControlBalancingSim" + Location [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "244" + ReportName "simulink-default.rpt" + SIDHighWatermark "5040" + Block { + BlockType Reference + Name "Configuration" + SID "4537" + Ports [] + Position [1140, 705, 1205, 720] + 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 Constant + Name "Constant" + SID "4947" + Position [780, 493, 940, 507] + ZOrder 1228 + Value "zeros(Ports.IMU_PORT_SIZE,1)" + } + Block { + BlockType Demux + Name "Demux" + SID "4949" + Ports [1, 2] + Position [890, 516, 895, 574] + ZOrder 1229 + ShowName off + Outputs "[6;6]" + DisplayOption "bar" + } + Block { + BlockType SubSystem + Name "Dump and visualize" + SID "2275" + Ports [] + Position [1140, 671, 1205, 689] + ZOrder 547 + ForegroundColor "blue" + BackgroundColor "[0.333333, 1.000000, 1.000000]" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 31 + $ClassName "Simulink.Mask" + Display "disp('VISUALIZER')" + } + System { + Name "Dump and visualize" + Location [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "184" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "114" + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "3245" + Position [-580, 237, -550, 253] + ZOrder 648 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "3251" + Position [-580, 337, -550, 353] + ZOrder 682 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "f_star" + SID "4510" + Position [-580, 438, -550, 452] + ZOrder 688 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "state" + SID "4785" + Position [-345, 68, -315, 82] + ZOrder 690 + Port "4" + IconDisplay "Port number" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "229" + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "4807" + Position [-620, -98, -590, -82] + ZOrder 697 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "4808" + Position [-620, 67, -590, 83] + ZOrder 698 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "f_star" + SID "4809" + Position [-200, -22, -170, -8] + ZOrder 699 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name "Demux1" + SID "4801" + Ports [1, 2] + Position [-500, -159, -495, -26] + ZOrder 692 + ShowName off + Outputs "[3;3]" + DisplayOption "bar" + } + Block { + BlockType Demux + Name "Demux2" + SID "4802" + Ports [1, 2] + Position [-90, -144, -85, 114] + ZOrder 693 + ShowName off + Outputs "[6;6]" + DisplayOption "bar" + } + Block { + BlockType Demux + Name "Demux5" + SID "4803" + Ports [1, 2] + Position [-10, -127, -5, -38] + ZOrder 694 + ShowName off + Outputs "[3;3]" + DisplayOption "bar" + } + Block { + BlockType Demux + Name "Demux6" + SID "4804" + Ports [1, 2] + Position [-10, 3, -5, 92] + ZOrder 695 + ShowName off + Outputs "[3;3]" + DisplayOption "bar" + } + Block { + BlockType Demux + Name "Demux7" + SID "4805" + Ports [1, 2] + Position [-500, 6, -495, 139] + ZOrder 696 + ShowName off + Outputs "[3;3]" + DisplayOption "bar" + } + Block { + BlockType Outport + Name "measured forces l_sole" + SID "4810" + Position [-365, -132, -335, -118] + ZOrder 700 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "des forces l_sole" + SID "4812" + Position [75, -112, 105, -98] + ZOrder 702 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "measured forces r_sole" + SID "4813" + Position [-365, 33, -335, 47] + ZOrder 703 + Port "3" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "des forces r_sole" + SID "4816" + Position [75, 18, 105, 32] + ZOrder 706 + Port "4" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "measured moments l_sole" + SID "4811" + Position [-365, -67, -335, -53] + ZOrder 701 + Port "5" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "des moments l_sole" + SID "4814" + Position [75, -67, 105, -53] + ZOrder 704 + Port "6" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "measured moments r_sole" + SID "4815" + Position [-365, 98, -335, 112] + ZOrder 705 + Port "7" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "des moments r_sole" + SID "4817" + Position [75, 63, 105, 77] + ZOrder 707 + Port "8" + IconDisplay "Port number" + 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 + IconShape "round" + Inputs "-+|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + 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 + IconShape "round" + Inputs "-+|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + 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 + IconShape "round" + Inputs "-+|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + 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 + IconShape "round" + Inputs "-+|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + 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','2019b','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','2019b','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" + "','2019b','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','2019b','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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "225" + Block { + BlockType Inport + Name "feetContactStatus" + SID "4106" + Position [5, -71, 35, -59] + ZOrder 646 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "KP_postural_diag" + SID "4107" + Position [5, 127, 35, 143] + ZOrder 647 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "state" + SID "4108" + Position [5, -6, 35, 6] + ZOrder 648 + Port "3" + IconDisplay "Port number" + } + 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" + DisplayOption "bar" + 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','2019b','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','2019b','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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "874" + Block { + BlockType Inport + Name "QPStatus" + SID "4621" + Position [100, 164, 125, 176] + ZOrder 409 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "state" + SID "4622" + Position [100, 198, 125, 212] + ZOrder 554 + Port "2" + IconDisplay "Port number" + } + 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','2019b','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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "82" + Block { + BlockType Inport + Name "tau_star" + SID "4505" + Position [65, -192, 95, -178] + ZOrder 591 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_b" + SID "4506" + Position [740, -457, 770, -443] + ZOrder 592 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos_des" + SID "4508" + Position [740, -157, 770, -143] + ZOrder 594 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "pos_CoM" + SID "4509" + Position [65, 253, 95, 267] + ZOrder 595 + Port "4" + IconDisplay "Port number" + Port { + PortNumber 1 + Name "CoM_Measured" + } + } + Block { + BlockType Inport + Name "pos_CoM_des" + SID "4511" + Position [65, 288, 95, 302] + ZOrder 597 + Port "5" + IconDisplay "Port number" + Port { + PortNumber 1 + Name "CoM_Desired" + } + } + Block { + BlockType Inport + Name "state" + SID "4507" + Position [65, -277, 95, -263] + ZOrder 593 + Port "6" + IconDisplay "Port number" + 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" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "nu" + SID "4109" + Position [740, 192, 770, 208] + ZOrder 887 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "4826" + Position [740, -307, 770, -293] + ZOrder 895 + Port "9" + IconDisplay "Port number" + } + 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.62264','MaxYLimReal','0.77059','YLabelReal','','MinYLimMag','0.00000','MaxYLimMa" + "g','0.77059','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',3,'LineNames',{{'basePosition:1','basePosition:2'," + "'basePosition:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-1.25','MaxYLimReal','1.25','YLabelR" + "eal','','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.392156862745" + "098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07450" + "98039215686 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," + "'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',9,'LineNames',{{'bas" + "eOrientation:1','baseOrientation:2','baseOrientation:3','baseOrientation:4','baseOrientation:5','baseOrientatio" + "n:6','baseOrientation:7','baseOrientation:8','baseOrientation:9'}},'ShowContent',true,'Placement',2),struct('Mi" + "nYLimReal','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.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',{{}},'UserDefinedChannelNames',{{}},'NumLines',1,'LineNames',{{'state'}},'ShowContent',true,'Pl" + "acement',3)},'DisplayPropertyDefaults',struct('YLabelReal','','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',1),'TimeRangeSamples" + "','400','TimeRangeFrames','400','DisplayLayoutDimensions',[3 1],'DisplayContentCache',[]),extmgr.Configuration(" + "'Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measu" + "rements',true,'Version','2018a')),'Version','2019b','Location',[630 446 1910 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','2019b','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.15906','MaxYLimReal','0.73025','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag'" + ",'0.73025','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',3,'LineNames',{{'CoM_Measured:1','CoM_Measured:2','C" + "oM_Measured:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.15798','MaxYLimReal','0.72054','YLa" + "belReal','','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.39215686" + "2745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0" + "745098039215686 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','Vis" + "ible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','" + "-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{" + "'CoM_Desired:1','CoM_Desired:2','CoM_Desired:3'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-0.03" + "677','MaxYLimReal','0.01367','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',3,'LineNames',{{'CoM_Error:1','CoM_Error:2','CoM_Error:3'}},'ShowContent',true,'Placement',3)," + "struct('MinYLimReal','0.375','MaxYLimReal','6.625','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',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',4)},'DisplayPro" + "pertyDefaults',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.411764705882" + "353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569" + " 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache" + "',{{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','non" + "e','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),'TimeRangeSamples','400','TimeRangeFrames" + "','400','DisplayLayoutDimensions',[4 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false" + ",'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2019" + "b','Location',[73 111 1896 1079])" + 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" + DisplayOption "bar" + 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" + DisplayOption "bar" + 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" + DisplayOption "bar" + 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" + DisplayOption "bar" + 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" + DisplayOption "bar" + 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" + DisplayOption "bar" + 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" + DisplayOption "bar" + 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" + DisplayOption "bar" + 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" + DisplayOption "bar" + 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','2019b','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','2019b','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" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain1" + SID "4114" + Position [945, 187, 995, 213] + ZOrder 892 + ShowName off + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain2" + SID "4115" + Position [985, 366, 1030, 394] + ZOrder 893 + ShowName off + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + 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" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain4" + SID "2291" + Position [910, -8, 960, 28] + ZOrder 503 + ShowName off + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + 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','-30.77742','MaxYLimReal','32.17188','YLabelReal','','MinYLimMag','0.00000','" + "MaxYLimMag','32.17188','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.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',3,'LineNames',{{'torso:1','torso:2','tor" + "so:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-85.30234','MaxYLimReal','102.32903','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',4,'LineNames',{{'left_" + "arm:1','left_arm:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-81.068" + "26','MaxYLimReal','115.95971','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',4,'LineNames',{{'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true," + "'Placement',3),struct('MinYLimReal','-15.85804','MaxYLimReal','42.9704','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',6,'LineNames',{{'left_leg:1','left_leg:2','left_leg" + ":3','left_leg:4','left_leg:5','left_leg:6'}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-117.72592" + "','MaxYLimReal','109.21243','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.41176470588" + "2353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450980392156" + "9 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','-','Lin" + "eWidth',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'),stru" + "ct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames'," + "{{}},'NumLines',6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right_le" + "g:6'}},'ShowContent',true,'Placement',5),struct('MinYLimReal','0.375','MaxYLimReal','6.625','YLabelReal','','Mi" + "nYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'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','-','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',1,'LineNames',{{'state'}},'ShowContent',true," + "'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','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',1),'TimeRangeSamp" + "les','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation" + "',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version'," + "'2018a')),'Version','2019b','Location',[68 109 1921 1079])" + 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','-32.90319','MaxYLimReal','31.46514','YLabelReal','','MinYLimMag','0.00000','MaxYLimM" + "ag','32.90319','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',3,'LineNames',{{'torso:1','torso:2','torso:3'}}," + "'ShowContent',true,'Placement',1),struct('MinYLimReal','-92.5889','MaxYLimReal','107.15185','YLabelReal','','Mi" + "nYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'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',4,'LineNames',{{'left_arm:1','l" + "eft_arm:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-85.35003','MaxY" + "LimReal','120.17321','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'Y" + "Grid',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." + "16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0" + "588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{st" + "ruct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'Li" + "neStyle','-','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','Vi" + "sible','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',{{}},'N" + "umLines',4,'LineNames',{{'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placemen" + "t',3),struct('MinYLimReal','-12.83612','MaxYLimReal','25.58472','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.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',6,'LineNames',{{'left_leg:1','left_leg:2','left_leg:3','lef" + "t_leg:4','left_leg:5','left_leg:6'}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-116.82956','MaxYL" + "imReal','98.641','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid" + "',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450" + "9803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607" + "8431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882" + "35294117647 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],'LineSt" + "yle','-','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',{{}},'NumLi" + "nes',6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right_leg:6'}},'Sho" + "wContent',true,'Placement',5),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.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.65098039" + "2156863],'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','Visib" + "le','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',1,'LineNames',{{'state'}},'ShowContent',tr" + "ue,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','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',1),'TimeRangeS" + "amples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigat" + "ion',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Versio" + "n','2018a')),'Version','2019b','Location',[684 598 2537 1566])" + 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','-4.81045','MaxYLimReal','8.45739','YLabelReal','','MinYLimMag','0.00000','MaxY" + "LimMag','8.45739','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',3,'LineNames',{{'torso:1','torso:2','torso:3'" + "}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.01242','MaxYLimReal','11.24663','YLabelReal','','" + "MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'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',4,'LineNames',{{'left_arm:1'," + "'left_arm:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-8.1897','MaxY" + "LimReal','6.45288','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGr" + "id',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.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',4,'LineNames',{{'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement'" + ",3),struct('MinYLimReal','-34.91801','MaxYLimReal','28.99514','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',6,'LineNames',{{'left_leg:1','left_leg:2','left_leg:3','left_" + "leg:4','left_leg:5','left_leg:6'}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-5.62493','MaxYLimRe" + "al','4.5474','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'" + ",6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right_leg:6'}},'ShowCon" + "tent',true,'Placement',5),struct('MinYLimReal','0.375','MaxYLimReal','6.625','YLabelReal','','MinYLimMag','0','" + "MaxYLimMag','10','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,'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',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',6)}" + ",'DisplayPropertyDefaults',struct('YLabelReal','','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',1),'TimeRangeSamples','40','Time" + "RangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtS" + "top',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Vers" + "ion','2019b','Location',[73 146 1916 1074])" + 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','2019b','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',3,'LineNames',{{'torso:1','torso:2','torso:3'" + "}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-4.19072','MaxYLimReal','4.87188','YLabelReal','','M" + "inYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesCo" + "lor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.066666" + "6666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.8313" + "72549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509803921568" + "6 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','L" + "ineWidth',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],'Li" + "neStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth'" + ",0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'left_arm:1','" + "left_arm:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-4.21765','MaxY" + "LimReal','4.8762','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGri" + "d',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.160" + "78431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588" + "235294117647 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],'LineS" + "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0." + "5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" + "le','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',{{}},'NumL" + "ines',4,'LineNames',{{'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement'," + "3),struct('MinYLimReal','-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','%<" + "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',6,'LineNames',{{'left_leg:1','left_leg:2','left_leg:3','left_l" + "eg:4','left_leg:5','left_leg:6'}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-44.89002','MaxYLimRe" + "al','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.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',6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right_leg:6'}},'ShowC" + "ontent',true,'Placement',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],'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,'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',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',6)}" + ",'DisplayPropertyDefaults',struct('YLabelReal','','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',1),'TimeRangeSamples','14','Time" + "RangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtS" + "top',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Vers" + "ion','2019b','Location',[611 395 1921 1079])" + 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 + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum1" + SID "4827" + Ports [2, 1] + Position [210, 320, 230, 340] + ZOrder 896 + ShowName off + IconShape "round" + Inputs "+-|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + 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 + IconShape "round" + Inputs "-+|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + 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','-95666336154330198274832096957265561747801901517537520066992001990832736421616212186" + "787250344171865405217544478464286372890138546621274335557866403149565308442860058507668305178663551593362288657" + "397855551488.00000','MaxYLimReal','8609970253889719448717918358956907110839063765587045713804927258746796872192" + "828670774778502776111579954551731825168508239049428662698510364205729892013299020558650480531073023421597348183" + "25249267626521133056.00000','YLabelReal','','MinYLimMag',' " + " " + " 0.00000','MaxYLimMag','860997025388971944871791835895690711083906376558" + "704571380492725874679687219282867077477850277611157995455173182516850823904942866269851036420572989201329902055" + "865048053107302342159734818325249267626521133056.00000','LegendVisibility','On','XGrid',true,'YGrid',true,'Plot" + "MagPhase',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" + ".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','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" + "uct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'Lin" + "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',3,'Line" + "Names',{{'torso:1','torso:2','torso:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-5881497403942" + "495020131109909310191587071272453563035133745093479420576334991499142822005615335455167803550190415185967608251" + "72806527860162596146426966962778073508250148382564248016233335293674525674931486720.00000','MaxYLimReal','65349" + "971154916597473208372536013183220843461332106955369476928096027447969372722552655023130141469945477953842460487" + "118174946711506198362327986300206370001483588920999343150470281872921866549521898193027072.00000','YLabelReal'," + "'','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.0" + "666666666666667;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.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,'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" + "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineW" + "idth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'left_arm" + ":1','left_arm:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-810895422" + "186449176438881230178946918452806887524368628364549114244792301420158962712290735111860329513682259508447772202" + "138486816228674695483947517260051020334330997449984971265023234237081554386822331629568.00000','MaxYLimReal','9" + "009949135404988671072343796947179082944049500999730687318185482313697603838671769197898823488931978752679424612" + "7270884844638591760442840480962946739911059212809990626232674423569674479938553390369098170368.00000','YLabelRe" + "al','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',fals" + "e,'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.074509" + "8039215686 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,'" + "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','-','L" + "ineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'righ" + "t_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-3" + "633113806428513008755545295795450015692654171373963797033360001358664396206405339621391946991932588961674162481" + "08642905530313795916548986084744357393736498605621100222634903308108969774668161169802429202432.00000','MaxYLim" + "Real','27308481346587658801047912543650573129158191162982238406061876365015329948707963593612645383077765734067" + "9099301176756060320680786542955999826034294839935427421265798533676687223797477754430145016152455118848.00000'," + "'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],'ColorOr" + "der',[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]," + "'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',6,'LineNames" + "',{{'left_leg:1','left_leg:2','left_leg:3','left_leg:4','left_leg:5','left_leg:6'}},'ShowContent',true,'Placeme" + "nt',4),struct('MinYLimReal','-351897732030540317135432030318462559504717234592146282235311658339537645066397618" + "436784613589610180608333255651794868866515036752366467495869338112007729467492592916216339820925353024739560992" + "472985088557056.00000','MaxYLimReal','2974737429819443451111679282622781258357715161482278632194659214455657436" + "299831249597014977339824651563126486648795259656326850785687804046421236396722295397081737827834494816157517585" + "51134796748676420075520.00000','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',6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right" + "_leg:6'}},'ShowContent',true,'Placement',5),struct('MinYLimReal','0.875','MaxYLimReal','2.125','YLabelReal',''," + "'MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'Axes" + "Color',[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.83" + "1372549019608 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','-','LineWidth',0." + "5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" + "le','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',1,'LineNames',{{'state'}},'ShowContent',tr" + "ue,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','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',1),'TimeRangeS" + "amples','90','TimeRangeFrames','90','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigat" + "ion',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Versio" + "n','2018a')),'Version','2019b','Location',[72 111 1917 1079])" + 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 SubSystem + Name "Init. Cond." + SID "4985" + Ports [0, 2] + Position [1075, 655, 1115, 735] + ZOrder 1241 + BlockMirror on + RequestExecContextInheritance off + System { + Name "Init. Cond." + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + 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 "404" + Block { + BlockType Constant + Name "Constant" + SID "4992" + Position [510, -20, 645, 0] + ZOrder 1247 + Value "zeros(ROBOT_DOF+6,1)" + } + Block { + BlockType Constant + Name "Constant1" + SID "4993" + Position [-50, 205, 45, 225] + ZOrder 1248 + ShowName off + HideAutomaticName off + Value "Config.init_jointPos" + } + Block { + BlockType Constant + Name "Feet in Contact" + SID "5000" + Position [140, 122, 180, 138] + ZOrder 1250 + Value "[1, 1]" + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "4970" + Ports [2, 1] + Position [500, 63, 660, 267] + ZOrder 1238 + 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 + 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 "43" + Block { + BlockType Inport + Name "w_H_b" + SID "4970::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "4970::22" + Position [20, 136, 40, 154] + ZOrder 13 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4970::42" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 33 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4970::41" + Tag "Stateflow S-Function torqueControlBalancingSim 16" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 32 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "q" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4970::43" + Position [460, 241, 480, 259] + ZOrder 34 + } + Block { + BlockType Outport + Name "q" + SID "4970::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 16 + SrcBlock "w_H_b" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 17 + SrcBlock "jointPos" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "q" + ZOrder 18 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "q" + DstPort 1 + } + Line { + ZOrder 19 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 20 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Transformation Matrix From FixedLink to Base" + SID "4971" + Ports [2, 1] + Position [260, 75, 390, 150] + ZOrder 1234 + RequestExecContextInheritance off + System { + Name "Transformation Matrix From FixedLink to Base" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + 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 "534" + Block { + BlockType Inport + Name "jointPos" + SID "4972" + Position [820, -107, 850, -93] + ZOrder 890 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "4994" + Position [1030, -192, 1060, -178] + ZOrder 971 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "Constant7" + SID "4974" + Position [820, -285, 850, -255] + ZOrder 885 + ShowName off + Value "eye(4)" + } + Block { + BlockType Product + Name "Inverse " + SID "4975" + Ports [1, 1] + Position [1350, -203, 1385, -167] + ZOrder 907 + Inputs "/" + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + } + Block { + BlockType Reference + Name "RelativeTransform Between l_sole and Base" + SID "4976" + Ports [2, 1] + Position [965, -278, 1130, -242] + ZOrder 725 + NamePlacement "alternate" + 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 "RelativeTransform Between r_sole and Base" + SID "4977" + Ports [2, 1] + Position [960, -128, 1130, -92] + ZOrder 896 + 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 Selector + Name "Selector" + SID "4978" + Ports [1, 1] + Position [1140, -204, 1180, -166] + ZOrder 970 + InputPortWidth "2" + IndexOptions "Index vector (dialog)" + Indices "1" + OutputSizes "1" + } + Block { + BlockType Switch + Name "Switch" + SID "4979" + Position [1230, -296, 1305, -74] + ZOrder 895 + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "fixedLink_H_b" + SID "4980" + Position [1450, -192, 1480, -178] + ZOrder 904 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "jointPos" + SrcPort 1 + Points [42, 0] + Branch { + ZOrder 2 + Points [0, -150] + DstBlock "RelativeTransform Between l_sole and Base" + DstPort 2 + } + Branch { + ZOrder 3 + DstBlock "RelativeTransform Between r_sole and Base" + DstPort 2 + } + } + Line { + ZOrder 4 + SrcBlock "Constant7" + SrcPort 1 + Points [24, 0] + Branch { + ZOrder 5 + Points [0, 150] + DstBlock "RelativeTransform Between r_sole and Base" + DstPort 1 + } + Branch { + ZOrder 6 + DstBlock "RelativeTransform Between l_sole and Base" + DstPort 1 + } + } + Line { + ZOrder 7 + SrcBlock "RelativeTransform Between l_sole and Base" + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + Line { + ZOrder 8 + SrcBlock "RelativeTransform Between r_sole and Base" + SrcPort 1 + DstBlock "Switch" + DstPort 3 + } + Line { + ZOrder 13 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock "Selector" + DstPort 1 + } + Line { + ZOrder 10 + SrcBlock "Switch" + SrcPort 1 + DstBlock "Inverse " + DstPort 1 + } + Line { + ZOrder 11 + SrcBlock "Inverse " + SrcPort 1 + DstBlock "fixedLink_H_b" + DstPort 1 + } + Line { + ZOrder 12 + SrcBlock "Selector" + SrcPort 1 + DstBlock "Switch" + DstPort 2 + } + } + } + Block { + BlockType Outport + Name "nu_0" + SID "4989" + Position [715, -17, 745, -3] + ZOrder 1244 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "q_0" + SID "4990" + Position [795, 158, 825, 172] + ZOrder 1245 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 344 + SrcBlock "Constant" + SrcPort 1 + DstBlock "nu_0" + DstPort 1 + } + Line { + ZOrder 348 + SrcBlock "MATLAB Function" + SrcPort 1 + DstBlock "q_0" + DstPort 1 + } + Line { + ZOrder 314 + SrcBlock "Transformation Matrix From FixedLink to Base" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 347 + SrcBlock "Constant1" + SrcPort 1 + Points [28, 0] + Branch { + ZOrder 346 + Points [0, -120] + DstBlock "Transformation Matrix From FixedLink to Base" + DstPort 1 + } + Branch { + ZOrder 340 + DstBlock "MATLAB Function" + DstPort 2 + } + } + Line { + ZOrder 355 + SrcBlock "Feet in Contact" + SrcPort 1 + DstBlock "Transformation Matrix From FixedLink to Base" + DstPort 2 + } + } + } + Block { + BlockType SubSystem + Name "InverseKinematics Integration" + SID "4886" + Ports [3, 2] + Position [950, 615, 1020, 735] + ZOrder 964 + BlockMirror on + RequestExecContextInheritance off + System { + Name "InverseKinematics Integration" + Location [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "131" + Block { + BlockType Inport + Name "nuDot" + SID "4887" + Position [85, -7, 115, 7] + ZOrder 498 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "nu_0" + SID "4888" + Position [85, 43, 115, 57] + ZOrder 500 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "q_0" + SID "4889" + Position [505, 103, 535, 117] + ZOrder 501 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "Compute the Base Pose Derivative" + SID "4890" + Ports [2, 1] + Position [525, -22, 790, 27] + ZOrder 516 + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Compute the Base Pose Derivative" + Location [223, 338, 826, 833] + Open off + PortBlocksUseCompactNotation off + 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 "57" + Block { + BlockType Inport + Name "nu_base" + SID "4890::23" + Position [20, 101, 40, 119] + ZOrder 9 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "pose_base" + SID "4890::33" + Position [20, 136, 40, 154] + ZOrder 19 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4890::56" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 42 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4890::55" + Tag "Stateflow S-Function torqueControlBalancingSim 5" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 41 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "pose_base_dot" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4890::57" + Position [460, 241, 480, 259] + ZOrder 43 + } + Block { + BlockType Outport + Name "pose_base_dot" + SID "4890::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 16 + SrcBlock "nu_base" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 17 + SrcBlock "pose_base" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "pose_base_dot" + ZOrder 18 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "pose_base_dot" + DstPort 1 + } + Line { + ZOrder 19 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 20 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Demux + Name "Demux" + SID "4891" + Ports [1, 2] + Position [1090, 111, 1095, 149] + ZOrder 507 + ShowName off + Outputs "[7;ROBOT_DOF]" + DisplayOption "bar" + } + Block { + BlockType Demux + Name "Demux1" + SID "4892" + Ports [1, 2] + Position [445, -44, 450, 94] + ZOrder 509 + ShowName off + Outputs "[6;ROBOT_DOF]" + DisplayOption "bar" + } + Block { + BlockType Mux + Name "Mux" + SID "4893" + Ports [2, 1] + Position [855, -22, 860, 87] + ZOrder 511 + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType SubSystem + Name "Rewirte the Base Pose as Transf Matrix" + SID "4894" + Ports [1, 1] + Position [1200, 43, 1365, 107] + ZOrder 514 + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Rewirte the Base Pose as Transf Matrix" + Location [223, 338, 826, 833] + Open off + PortBlocksUseCompactNotation off + 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 "55" + Block { + BlockType Inport + Name "q_quat" + SID "4894::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4894::54" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 40 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4894::53" + Tag "Stateflow S-Function torqueControlBalancingSim 31" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 39 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "q" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4894::55" + Position [460, 241, 480, 259] + ZOrder 41 + } + Block { + BlockType Outport + Name "q" + SID "4894::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 13 + SrcBlock "q_quat" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "q" + ZOrder 14 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "q" + DstPort 1 + } + Line { + ZOrder 15 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 16 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Rewrite the Base Pose as Pos+Quat" + SID "4895" + Ports [1, 1] + Position [565, 87, 755, 133] + ZOrder 515 + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Rewrite the Base Pose as Pos+Quat" + Location [223, 338, 826, 833] + Open off + PortBlocksUseCompactNotation off + 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 "55" + Block { + BlockType Inport + Name "q_0" + SID "4895::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4895::54" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 40 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4895::53" + Tag "Stateflow S-Function torqueControlBalancingSim 30" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 39 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "q_0_quat" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4895::55" + Position [460, 241, 480, 259] + ZOrder 41 + } + Block { + BlockType Outport + Name "q_0_quat" + SID "4895::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 13 + SrcBlock "q_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "q_0_quat" + ZOrder 14 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "q_0_quat" + DstPort 1 + } + Line { + ZOrder 15 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 16 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType DiscreteIntegrator + Name "State Acceleration Integration" + SID "4896" + Ports [2, 1] + Position [210, -26, 255, 76] + ZOrder 496 + InitialConditionSource "external" + SampleTime "Config.tStep" + ICPrevOutput "DiscIntNeverNeededParam" + ICPrevScaledInput "DiscIntNeverNeededParam" + } + Block { + BlockType Terminator + Name "Terminator" + SID "4897" + Position [1130, 132, 1145, 148] + ZOrder 508 + ShowName off + HideAutomaticName off + } + Block { + BlockType DiscreteIntegrator + Name "Velocity Integration" + SID "4898" + Ports [2, 1] + Position [935, -2, 985, 147] + ZOrder 497 + InitialConditionSource "external" + SampleTime "Config.tStep" + ICPrevOutput "DiscIntNeverNeededParam" + ICPrevScaledInput "DiscIntNeverNeededParam" + } + Block { + BlockType Outport + Name "nu" + SID "4899" + Position [350, -18, 385, -2] + ZOrder 505 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "q" + SID "4900" + Position [1410, 67, 1445, 83] + ZOrder 502 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "nu_0" + SrcPort 1 + DstBlock "State Acceleration Integration" + DstPort 2 + } + Line { + ZOrder 2 + SrcBlock "q_0" + SrcPort 1 + DstBlock "Rewrite the Base Pose as Pos+Quat" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "nuDot" + SrcPort 1 + DstBlock "State Acceleration Integration" + DstPort 1 + } + Line { + ZOrder 4 + SrcBlock "Demux" + SrcPort 2 + DstBlock "Terminator" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "Velocity Integration" + SrcPort 1 + Points [67, 0] + Branch { + ZOrder 6 + Points [0, 55] + DstBlock "Demux" + DstPort 1 + } + Branch { + ZOrder 7 + DstBlock "Rewirte the Base Pose as Transf Matrix" + DstPort 1 + } + } + Line { + ZOrder 8 + SrcBlock "State Acceleration Integration" + SrcPort 1 + Points [39, 0] + Branch { + ZOrder 9 + DstBlock "Demux1" + DstPort 1 + } + Branch { + ZOrder 10 + Points [0, -35] + DstBlock "nu" + DstPort 1 + } + } + Line { + ZOrder 11 + SrcBlock "Demux1" + SrcPort 1 + DstBlock "Compute the Base Pose Derivative" + DstPort 1 + } + Line { + ZOrder 12 + SrcBlock "Compute the Base Pose Derivative" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + ZOrder 13 + SrcBlock "Demux1" + SrcPort 2 + DstBlock "Mux" + DstPort 2 + } + Line { + ZOrder 14 + SrcBlock "Mux" + SrcPort 1 + DstBlock "Velocity Integration" + DstPort 1 + } + Line { + ZOrder 15 + SrcBlock "Rewirte the Base Pose as Transf Matrix" + SrcPort 1 + DstBlock "q" + DstPort 1 + } + Line { + ZOrder 16 + SrcBlock "Rewrite the Base Pose as Pos+Quat" + SrcPort 1 + DstBlock "Velocity Integration" + DstPort 2 + } + Line { + ZOrder 17 + SrcBlock "Demux" + SrcPort 1 + Points [73, 0; 0, 73; -698, 0; 0, -178] + DstBlock "Compute the Base Pose Derivative" + DstPort 2 + } + } + } + Block { + BlockType SubSystem + Name "MBC control + forward dynamics" + SID "4836" + Ports [5, 2] + Position [965, 427, 1115, 573] + ZOrder 961 + RequestExecContextInheritance off + System { + Name "MBC control + forward dynamics" + Location [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "73" + Block { + BlockType Inport + Name "jointPos" + SID "4837" + Position [160, 288, 190, 302] + ZOrder 556 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointVel" + SID "4838" + Position [160, 418, 190, 432] + ZOrder 557 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "IMU_meas" + SID "4840" + Position [160, 478, 190, 492] + ZOrder 559 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "4881" + Position [160, 598, 190, 612] + ZOrder 964 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "4880" + Position [160, 538, 190, 552] + ZOrder 963 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "Balancing Controller QP" + SID "2355" + Ports [20, 1] + Position [945, 31, 1100, 609] + ZOrder 542 + BackgroundColor "lightBlue" + RequestExecContextInheritance off + System { + Name "Balancing Controller QP" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + 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" + Block { + BlockType Inport + Name "M" + SID "2360" + Position [1065, 168, 1095, 182] + ZOrder 326 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "h" + SID "2361" + Position [1065, 213, 1095, 227] + ZOrder 327 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "L " + SID "3219" + Position [1065, 258, 1095, 272] + ZOrder 639 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_l_sole" + SID "4384" + Position [1065, 348, 1095, 362] + ZOrder 837 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_r_sole" + SID "4391" + Position [1065, 393, 1095, 407] + ZOrder 844 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "J_l_sole" + SID "2357" + Position [1065, 438, 1095, 452] + ZOrder 550 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "J_r_sole" + SID "4392" + Position [1065, 483, 1095, 497] + ZOrder 845 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "J_CoM" + SID "4390" + Position [1065, 663, 1095, 677] + ZOrder 843 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "pos_CoM" + SID "4383" + Position [1065, 618, 1095, 632] + ZOrder 836 + Port "9" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JDot_l_sole_nu" + SID "4386" + Position [1065, 528, 1095, 542] + ZOrder 839 + Port "10" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JDot_r_sole_nu" + SID "4388" + Position [1065, 573, 1095, 587] + ZOrder 841 + Port "11" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "2358" + Position [1065, 33, 1095, 47] + ZOrder 323 + Port "12" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "nu" + SID "2359" + Position [1065, 123, 1095, 137] + ZOrder 325 + Port "13" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "state" + SID "4387" + Position [1065, 318, 1095, 332] + ZOrder 840 + Port "14" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "2363" + Position [1065, -102, 1095, -88] + ZOrder 386 + Port "15" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "KP_postural" + SID "2356" + Position [1065, 843, 1095, 857] + ZOrder 336 + Port "16" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "KP_CoM" + SID "3328" + Position [1065, 753, 1095, 767] + ZOrder 644 + Port "17" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "KD_CoM" + SID "3329" + Position [1065, 798, 1095, 812] + ZOrder 645 + Port "18" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "desired_pos_vel_acc_CoM" + SID "2365" + Position [1065, 708, 1095, 722] + ZOrder 335 + Port "19" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos_des" + SID "2364" + Position [1065, 78, 1095, 92] + ZOrder 324 + Port "20" + IconDisplay "Port number" + } + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "140" + Block { + BlockType Inport + Name "feetContactStatus" + SID "4410" + Position [430, 464, 460, 476] + ZOrder 640 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "HessianMatrixOneFoot" + SID "4411" + Position [430, 508, 460, 522] + ZOrder 641 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "gradientOneFoot" + SID "4412" + Position [430, 553, 460, 567] + ZOrder 642 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ConstraintsMatrixOneFoot" + SID "4413" + Position [430, 598, 460, 612] + ZOrder 647 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "bVectorConstraintsOneFoot" + SID "4414" + Position [430, 643, 460, 657] + ZOrder 648 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "HessianMatrixTwoFeet" + SID "4415" + Position [430, 688, 460, 702] + ZOrder 643 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "gradientTwoFeet" + SID "4416" + Position [430, 733, 460, 747] + ZOrder 644 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ConstraintsMatrixTwoFeet" + SID "4417" + Position [430, 778, 460, 792] + ZOrder 645 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "bVectorConstraintsTwoFeet" + SID "4418" + Position [430, 823, 460, 837] + ZOrder 646 + Port "9" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tauModel" + SID "4395" + Position [1235, 553, 1265, 567] + ZOrder 412 + Port "10" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Sigma" + SID "4396" + Position [1235, 643, 1265, 657] + ZOrder 413 + Port "11" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Na" + SID "4397" + Position [835, 743, 865, 757] + ZOrder 414 + Port "12" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "f_LDot" + SID "4420" + Position [835, 653, 865, 667] + ZOrder 650 + Port "13" + IconDisplay "Port number" + } + Block { + BlockType Sum + Name "Add" + SID "2367" + Ports [2, 1] + Position [1525, 506, 1580, 719] + ZOrder 400 + ShowName off + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Add1" + SID "2368" + Ports [2, 1] + Position [1055, 500, 1090, 855] + ZOrder 409 + ShowName off + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Product + Name "Product1" + SID "2381" + Ports [2, 1] + Position [1420, 634, 1485, 696] + ZOrder 397 + ShowName off + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + } + Block { + BlockType Product + Name "Product2" + SID "2382" + Ports [2, 1] + Position [915, 734, 975, 796] + ZOrder 410 + ShowName off + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + } + Block { + BlockType SubSystem + Name "QPSolver" + SID "4579" + Ports [9, 3] + Position [565, 452, 760, 848] + ZOrder 652 + RequestExecContextInheritance off + System { + Name "QPSolver" + Location [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "162" + Block { + BlockType Inport + Name "feetContactStatus" + SID "4580" + Position [-480, -106, -450, -94] + ZOrder 394 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "HessianMatrixOneFoot" + SID "4581" + Position [-230, -322, -200, -308] + ZOrder 395 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "gradientOneFoot" + SID "4582" + Position [-230, -292, -200, -278] + ZOrder 396 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ConstraintsMatrixOneFoot" + SID "4583" + Position [-230, -262, -200, -248] + ZOrder 403 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "bVectorConstraintsOneFoot" + SID "4584" + Position [-230, -232, -200, -218] + ZOrder 404 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "HessianMatrixTwoFeet" + SID "4585" + Position [-230, 8, -200, 22] + ZOrder 397 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "gradientTwoFeet" + SID "4586" + Position [-230, 48, -200, 62] + ZOrder 398 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ConstraintsMatrixTwoFeet" + SID "4587" + Position [-230, 88, -200, 102] + ZOrder 399 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "bVectorConstraintsTwoFeet" + SID "4588" + Position [-230, 128, -200, 142] + ZOrder 400 + Port "9" + IconDisplay "Port number" + } + 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 + 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 "78" + Block { + BlockType Inport + Name "feetContactStatus" + SID "4590::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4590::77" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 68 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4590::76" + Tag "Stateflow S-Function torqueControlBalancingSim 15" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 67 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "onOneFoot" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4590::78" + Position [460, 241, 480, 259] + ZOrder 69 + } + Block { + BlockType Outport + Name "onOneFoot" + SID "4590::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 69 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "onOneFoot" + ZOrder 70 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "onOneFoot" + 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 "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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "161" + Block { + BlockType Inport + Name "H" + SID "4685" + Position [970, -237, 1000, -223] + ZOrder 750 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "g" + SID "4686" + Position [970, -207, 1000, -193] + ZOrder 752 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "A" + SID "4687" + Position [970, -177, 1000, -163] + ZOrder 753 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ubA" + SID "4688" + Position [970, -147, 1000, -133] + ZOrder 754 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "4708" + Position [1360, 13, 1390, 27] + ZOrder 762 + Port "5" + IconDisplay "Port number" + } + 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 + 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 "79" + Block { + BlockType Inport + Name "H" + SID "4690::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "g" + SID "4690::22" + Position [20, 136, 40, 154] + ZOrder 13 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4690::78" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 69 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4690::77" + Tag "Stateflow S-Function torqueControlBalancingSim 26" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 68 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "analyticalSolution" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4690::79" + Position [460, 241, 480, 259] + ZOrder 70 + } + Block { + BlockType Outport + Name "analyticalSolution" + SID "4690::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 36 + SrcBlock "H" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 37 + SrcBlock "g" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "analyticalSolution" + ZOrder 38 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "analyticalSolution" + DstPort 1 + } + Line { + ZOrder 39 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 40 + 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 + 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 "87" + Block { + BlockType Inport + Name "analyticalSolution" + SID "4692::29" + Position [20, 101, 40, 119] + ZOrder 20 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "primalSolution" + SID "4692::1" + Position [20, 136, 40, 154] + ZOrder -1 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "QPStatus" + SID "4692::22" + Position [20, 171, 40, 189] + ZOrder 13 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "4692::66" + Position [20, 206, 40, 224] + ZOrder 51 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4692::86" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 71 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4692::85" + Tag "Stateflow S-Function torqueControlBalancingSim 27" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 70 + 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::87" + Position [460, 241, 480, 259] + ZOrder 72 + } + Block { + BlockType Outport + Name "f_star" + SID "4692::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 56 + SrcBlock "analyticalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 57 + SrcBlock "primalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 58 + SrcBlock "QPStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 59 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "f_star" + ZOrder 60 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "f_star" + DstPort 1 + } + Line { + ZOrder 61 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 62 + 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" + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "QPStatus" + SID "4694" + Position [1360, -162, 1390, -148] + ZOrder 755 + Port "2" + IconDisplay "Port number" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "201" + Block { + BlockType Inport + Name "H" + SID "4673" + Position [1460, 1023, 1490, 1037] + ZOrder 751 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "g" + SID "4674" + Position [1460, 1053, 1490, 1067] + ZOrder 753 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "A" + SID "4675" + Position [1460, 1083, 1490, 1097] + ZOrder 754 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ubA" + SID "4676" + Position [1460, 1113, 1490, 1127] + ZOrder 755 + Port "4" + IconDisplay "Port number" + } + 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 + 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 "79" + Block { + BlockType Inport + Name "H" + SID "4678::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "g" + SID "4678::22" + Position [20, 136, 40, 154] + ZOrder 13 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4678::78" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 69 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4678::77" + Tag "Stateflow S-Function torqueControlBalancingSim 24" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 68 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "analyticalSolution" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4678::79" + Position [460, 241, 480, 259] + ZOrder 70 + } + Block { + BlockType Outport + Name "analyticalSolution" + SID "4678::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 36 + SrcBlock "H" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 37 + SrcBlock "g" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "analyticalSolution" + ZOrder 38 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "analyticalSolution" + DstPort 1 + } + Line { + ZOrder 39 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 40 + 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 + 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 "87" + Block { + BlockType Inport + Name "analyticalSolution" + SID "4680::29" + Position [20, 101, 40, 119] + ZOrder 20 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "primalSolution" + SID "4680::1" + Position [20, 136, 40, 154] + ZOrder -1 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "QPStatus" + SID "4680::22" + Position [20, 171, 40, 189] + ZOrder 13 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4680::86" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 70 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4680::85" + Tag "Stateflow S-Function torqueControlBalancingSim 25" + Ports [3, 2] + Position [180, 100, 230, 180] + ZOrder 69 + 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::87" + Position [460, 241, 480, 259] + ZOrder 71 + } + Block { + BlockType Outport + Name "f_star" + SID "4680::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 62 + SrcBlock "analyticalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 63 + SrcBlock "primalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 64 + SrcBlock "QPStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + Name "f_star" + ZOrder 65 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "f_star" + DstPort 1 + } + Line { + ZOrder 66 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 67 + 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" + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "f0_twoFeet" + SID "4682" + Position [2155, 1018, 2185, 1032] + ZOrder 760 + Port "2" + IconDisplay "Port number" + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "onOneFoot" + SID "4717" + Position [-10, -137, 20, -123] + ZOrder 738 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "f_0_twoFeet" + SID "4627" + Position [240, 108, 270, 122] + ZOrder 728 + Port "3" + IconDisplay "Port number" + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "f_star" + SID "4409" + Position [1440, 733, 1470, 747] + ZOrder 639 + Port "2" + IconDisplay "Port number" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "102" + Block { + BlockType Inport + Name "jointPos_des" + SID "3715" + Position [-230, -147, -200, -133] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "3716" + Position [-230, 143, -200, 157] + ZOrder 1 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "3717" + Position [620, 98, 650, 112] + ZOrder 80 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "state" + SID "4028" + Position [50, -147, 80, -133] + ZOrder 732 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Sum + Name "Add" + SID "4724" + Ports [2, 1] + Position [-40, 114, -5, 161] + ZOrder 902 + ShowName off + HideAutomaticName off + Inputs "-+" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "355" + Block { + BlockType Inport + Name "jointPos" + SID "4423" + Position [50, 108, 80, 122] + ZOrder 902 + IconDisplay "Port number" + } + 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(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + } + 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(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + } + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "fixed_r_sole_H_b" + SID "4477" + Position [495, 93, 525, 107] + ZOrder 912 + Port "2" + IconDisplay "Port number" + 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 + 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 "3795" + Block { + BlockType Inport + Name "J_l_sole" + SID "3721::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "J_r_sole" + SID "3721::23" + Position [20, 136, 40, 154] + ZOrder 9 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "3721::3701" + Position [20, 171, 40, 189] + ZOrder 26 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos_err" + SID "3721::3702" + Position [20, 206, 40, 224] + ZOrder 27 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "3721::3794" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 119 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "3721::3793" + Tag "Stateflow S-Function torqueControlBalancingSim 20" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 118 + 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::3795" + Position [460, 241, 480, 259] + ZOrder 120 + } + Block { + BlockType Outport + Name "baseVel_equivalent" + SID "3721::24" + Position [460, 101, 480, 119] + ZOrder 10 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 197 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 198 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 199 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 200 + SrcBlock "jointPos_err" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "baseVel_equivalent" + ZOrder 201 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "baseVel_equivalent" + DstPort 1 + } + Line { + ZOrder 202 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 203 + 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 + 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 "108" + Block { + BlockType Inport + Name "state" + SID "4027::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4027::107" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 98 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4027::106" + Tag "Stateflow S-Function torqueControlBalancingSim 19" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 97 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "booleanState" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4027::108" + Position [460, 241, 480, 259] + ZOrder 99 + } + Block { + BlockType Outport + Name "booleanState" + SID "4027::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 113 + SrcBlock "state" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "booleanState" + ZOrder 114 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "booleanState" + DstPort 1 + } + Line { + ZOrder 115 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 116 + 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 + IconDisplay "Port number" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "166" + Block { + BlockType Inport + Name "tau_QP" + SID "4520" + Position [-30, 3, 0, 17] + ZOrder 591 + IconDisplay "Port number" + } + 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 + 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 "118" + Block { + BlockType Demux + Name " Demux " + SID "4551::116" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 107 + Outputs "1" + } + Block { + BlockType Ground + Name " Ground " + SID "4551::118" + Position [20, 121, 40, 139] + ZOrder 109 + } + Block { + BlockType S-Function + Name " SFunction " + SID "4551::115" + Tag "Stateflow S-Function torqueControlBalancingSim 9" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 106 + 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::117" + Position [460, 241, 480, 259] + ZOrder 108 + } + Block { + BlockType Outport + Name "reflectedInertia" + SID "4551::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + Name "reflectedInertia" + ZOrder 97 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "reflectedInertia" + DstPort 1 + } + Line { + ZOrder 98 + SrcBlock " Ground " + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 99 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 100 + 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 + Inputs "-+" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Constant + Name "Constant1" + SID "4952" + Position [-475, -108, -375, -92] + ZOrder 1233 + Value "zeros(ROBOT_DOF,1)" + } + 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" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Product + Name "Product" + SID "4526" + Ports [2, 1] + Position [-170, -239, -135, -86] + ZOrder 597 + ShowName off + HideAutomaticName off + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + } + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 6 + SrcBlock "Product" + SrcPort 1 + DstBlock "Gain" + DstPort 1 + } + Line { + ZOrder 16 + SrcBlock "Gain" + SrcPort 1 + DstBlock "Add" + DstPort 1 + } + Line { + ZOrder 23 + SrcBlock "Add" + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + Line { + ZOrder 24 + SrcBlock " " + SrcPort 1 + DstBlock "Switch" + DstPort 2 + } + Line { + ZOrder 41 + SrcBlock "Switch" + SrcPort 1 + DstBlock "tau_joints" + DstPort 1 + } + Line { + ZOrder 20 + SrcBlock "tau_QP" + SrcPort 1 + Points [24, 0] + Branch { + ZOrder 62 + Points [0, -75] + DstBlock "Add" + DstPort 2 + } + Branch { + ZOrder 93 + DstBlock "Switch" + DstPort 3 + } + } + Line { + ZOrder 52 + SrcBlock "(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" + SrcPort 1 + DstBlock "Product" + DstPort 1 + } + Line { + ZOrder 108 + SrcBlock " 1" + SrcPort 1 + DstBlock "Switch1" + DstPort 2 + } + Line { + ZOrder 109 + SrcBlock "Switch1" + SrcPort 1 + DstBlock "Product" + DstPort 2 + } + Line { + ZOrder 114 + SrcBlock "From" + SrcPort 1 + DstBlock "Switch1" + DstPort 1 + } + Line { + ZOrder 116 + SrcBlock "Constant1" + 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 + 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 "3794" + Block { + BlockType Inport + Name "feetContactStatus" + SID "2407::28" + Position [20, 101, 40, 119] + ZOrder 14 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ConstraintsMatrix" + SID "2407::808" + Position [20, 136, 40, 154] + ZOrder 56 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "bVectorConstraints" + SID "2407::809" + Position [20, 171, 40, 189] + ZOrder 57 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "2407::29" + Position [20, 206, 40, 224] + ZOrder 15 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos_des" + SID "2407::1" + Position [20, 246, 40, 264] + ZOrder -1 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "nu" + SID "2407::22" + Position [20, 281, 40, 299] + ZOrder 8 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "M" + SID "2407::23" + Position [20, 316, 40, 334] + ZOrder 9 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "h" + SID "2407::24" + Position [20, 351, 40, 369] + ZOrder 10 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "L" + SID "2407::26" + Position [20, 386, 40, 404] + ZOrder 12 + Port "9" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "intL_angMomError" + SID "2407::21" + Position [20, 426, 40, 444] + ZOrder 7 + Port "10" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_l_sole" + SID "2407::800" + Position [20, 461, 40, 479] + ZOrder 50 + Port "11" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_r_sole" + SID "2407::27" + Position [20, 496, 40, 514] + ZOrder 13 + Port "12" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "J_l_sole" + SID "2407::841" + Position [20, 531, 40, 549] + ZOrder 84 + Port "13" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "J_r_sole" + SID "2407::790" + Position [20, 566, 40, 584] + ZOrder 41 + Port "14" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JDot_l_sole_nu" + SID "2407::784" + Position [20, 606, 40, 624] + ZOrder 36 + Port "15" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JDot_r_sole_nu" + SID "2407::785" + Position [20, 641, 40, 659] + ZOrder 37 + Port "16" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "pos_CoM" + SID "2407::798" + Position [20, 676, 40, 694] + ZOrder 48 + Port "17" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "J_CoM" + SID "2407::31" + Position [20, 711, 40, 729] + ZOrder 17 + Port "18" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "desired_pos_vel_acc_CoM" + SID "2407::30" + Position [20, 746, 40, 764] + ZOrder 16 + Port "19" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "KP_CoM" + SID "2407::32" + Position [20, 786, 40, 804] + ZOrder 18 + Port "20" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "KD_CoM" + SID "2407::845" + Position [20, 821, 40, 839] + ZOrder 88 + Port "21" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "KP_postural" + SID "2407::846" + Position [20, 856, 40, 874] + ZOrder 89 + Port "22" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "2407::3793" + Ports [1, 1] + Position [270, 570, 320, 610] + ZOrder 196 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "2407::3792" + Tag "Stateflow S-Function torqueControlBalancingSim 17" + Ports [22, 13] + Position [180, 70, 230, 530] + ZOrder 195 + 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::3794" + Position [460, 581, 480, 599] + ZOrder 197 + } + Block { + BlockType Outport + Name "HessianMatrixOneFoot" + SID "2407::824" + Position [460, 101, 480, 119] + ZOrder 72 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "gradientOneFoot" + SID "2407::822" + Position [460, 136, 480, 154] + ZOrder 70 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "ConstraintsMatrixOneFoot" + SID "2407::5" + Position [460, 171, 480, 189] + ZOrder -5 + Port "3" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "bVectorConstraintsOneFoot" + SID "2407::810" + Position [460, 206, 480, 224] + ZOrder 58 + Port "4" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "HessianMatrixTwoFeet" + SID "2407::827" + Position [460, 246, 480, 264] + ZOrder 75 + Port "5" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "gradientTwoFeet" + SID "2407::828" + Position [460, 281, 480, 299] + ZOrder 76 + Port "6" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "ConstraintsMatrixTwoFeet" + SID "2407::811" + Position [460, 316, 480, 334] + ZOrder 59 + Port "7" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "bVectorConstraintsTwoFeet" + SID "2407::812" + Position [460, 351, 480, 369] + ZOrder 60 + Port "8" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "tauModel" + SID "2407::815" + Position [460, 386, 480, 404] + ZOrder 63 + Port "9" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "Sigma" + SID "2407::816" + Position [460, 426, 480, 444] + ZOrder 64 + Port "10" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "Na" + SID "2407::820" + Position [460, 461, 480, 479] + ZOrder 68 + Port "11" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "f_LDot" + SID "2407::821" + Position [460, 496, 480, 514] + ZOrder 69 + Port "12" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 2222 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 2223 + SrcBlock "ConstraintsMatrix" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 2224 + SrcBlock "bVectorConstraints" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 2225 + SrcBlock "jointPos" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 2226 + SrcBlock "jointPos_des" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 2227 + SrcBlock "nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + ZOrder 2228 + SrcBlock "M" + SrcPort 1 + DstBlock " SFunction " + DstPort 7 + } + Line { + ZOrder 2229 + SrcBlock "h" + SrcPort 1 + DstBlock " SFunction " + DstPort 8 + } + Line { + ZOrder 2230 + SrcBlock "L" + SrcPort 1 + DstBlock " SFunction " + DstPort 9 + } + Line { + ZOrder 2231 + SrcBlock "intL_angMomError" + SrcPort 1 + DstBlock " SFunction " + DstPort 10 + } + Line { + ZOrder 2232 + SrcBlock "w_H_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 11 + } + Line { + ZOrder 2233 + SrcBlock "w_H_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 12 + } + Line { + ZOrder 2234 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 13 + } + Line { + ZOrder 2235 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 14 + } + Line { + ZOrder 2236 + SrcBlock "JDot_l_sole_nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 15 + } + Line { + ZOrder 2237 + SrcBlock "JDot_r_sole_nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 16 + } + Line { + ZOrder 2238 + SrcBlock "pos_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 17 + } + Line { + ZOrder 2239 + SrcBlock "J_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 18 + } + Line { + ZOrder 2240 + SrcBlock "desired_pos_vel_acc_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 19 + } + Line { + ZOrder 2241 + SrcBlock "KP_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 20 + } + Line { + ZOrder 2242 + SrcBlock "KD_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 21 + } + Line { + ZOrder 2243 + SrcBlock "KP_postural" + SrcPort 1 + DstBlock " SFunction " + DstPort 22 + } + Line { + Name "HessianMatrixOneFoot" + ZOrder 2244 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "HessianMatrixOneFoot" + DstPort 1 + } + Line { + Name "gradientOneFoot" + ZOrder 2245 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "gradientOneFoot" + DstPort 1 + } + Line { + Name "ConstraintsMatrixOneFoot" + ZOrder 2246 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "ConstraintsMatrixOneFoot" + DstPort 1 + } + Line { + Name "bVectorConstraintsOneFoot" + ZOrder 2247 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "bVectorConstraintsOneFoot" + DstPort 1 + } + Line { + Name "HessianMatrixTwoFeet" + ZOrder 2248 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "HessianMatrixTwoFeet" + DstPort 1 + } + Line { + Name "gradientTwoFeet" + ZOrder 2249 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 7 + DstBlock "gradientTwoFeet" + DstPort 1 + } + Line { + Name "ConstraintsMatrixTwoFeet" + ZOrder 2250 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 8 + DstBlock "ConstraintsMatrixTwoFeet" + DstPort 1 + } + Line { + Name "bVectorConstraintsTwoFeet" + ZOrder 2251 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 9 + DstBlock "bVectorConstraintsTwoFeet" + DstPort 1 + } + Line { + Name "tauModel" + ZOrder 2252 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 10 + DstBlock "tauModel" + DstPort 1 + } + Line { + Name "Sigma" + ZOrder 2253 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 11 + DstBlock "Sigma" + DstPort 1 + } + Line { + Name "Na" + ZOrder 2254 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 12 + DstBlock "Na" + DstPort 1 + } + Line { + Name "f_LDot" + ZOrder 2255 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 13 + DstBlock "f_LDot" + DstPort 1 + } + Line { + ZOrder 2256 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 2257 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "tau_star " + SID "2414" + Position [2340, 73, 2370, 87] + ZOrder 279 + IconDisplay "Port number" + 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 + 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 "149" + Block { + BlockType Inport + Name "w_H_b" + SID "2342" + Position [-850, -167, -820, -153] + ZOrder 209 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "nu" + SID "2344" + Position [-850, -27, -820, -13] + ZOrder 582 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "2343" + Position [-850, -97, -820, -83] + ZOrder 224 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "Dynamics" + SID "4353" + Ports [3, 3] + Position [-645, -196, -540, 16] + ZOrder 838 + RequestExecContextInheritance off + System { + Name "Dynamics" + Location [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "249" + Block { + BlockType Inport + Name "w_H_b" + SID "4354" + Position [120, 28, 150, 42] + ZOrder 584 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "4355" + Position [120, 63, 150, 77] + ZOrder 585 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "nu" + SID "4356" + Position [120, 188, 150, 202] + ZOrder 586 + Port "3" + IconDisplay "Port number" + } + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "311" + Block { + BlockType Inport + Name "M" + SID "4516" + Position [-55, 88, -25, 102] + ZOrder 591 + IconDisplay "Port number" + } + 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 + 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 "94" + Block { + BlockType Inport + Name "M" + SID "4518::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4518::93" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 83 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4518::92" + Tag "Stateflow S-Function torqueControlBalancingSim 6" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 82 + 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::94" + Position [460, 241, 480, 259] + ZOrder 84 + } + Block { + BlockType Outport + Name "M_with_inertia" + SID "4518::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 110 + SrcBlock "M" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "M_with_inertia" + ZOrder 111 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "M_with_inertia" + DstPort 1 + } + Line { + ZOrder 112 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 113 + 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 + IconDisplay "Port number" + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "h" + SID "4358" + Position [625, 178, 655, 192] + ZOrder 588 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "L" + SID "4359" + Position [625, 308, 655, 322] + ZOrder 589 + Port "3" + IconDisplay "Port number" + 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 + 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 "154" + Block { + BlockType Inport + Name "nu" + SID "4342" + Position [-250, 493, -220, 507] + ZOrder 857 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "4339" + Position [-250, 58, -220, 72] + ZOrder 868 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_b" + SID "4336" + Position [-250, 163, -220, 177] + ZOrder 870 + Port "3" + IconDisplay "Port number" + } + 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 + 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 + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "w_H_r_sole" + SID "4367" + Position [380, 108, 410, 122] + ZOrder 874 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_l_sole" + SID "4368" + Position [380, 173, 410, 187] + ZOrder 875 + Port "3" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_r_sole" + SID "4369" + Position [380, 223, 410, 237] + ZOrder 876 + Port "4" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_CoM" + SID "4370" + Position [380, 273, 410, 287] + ZOrder 877 + Port "5" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "pos_CoM" + SID "4371" + Position [380, 333, 410, 347] + ZOrder 878 + Port "6" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "JDot_l_sole_nu" + SID "4372" + Position [380, 408, 410, 422] + ZOrder 879 + Port "7" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "JDot_r_sole_nu" + SID "4374" + Position [380, 488, 410, 502] + ZOrder 881 + Port "8" + IconDisplay "Port number" + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "h" + SID "2351" + Position [-475, -97, -445, -83] + ZOrder 217 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "L" + SID "2352" + Position [-475, -27, -445, -13] + ZOrder 216 + Port "3" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "w_H_l_sole" + SID "4375" + Position [-185, 43, -155, 57] + ZOrder 882 + Port "4" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "w_H_r_sole" + SID "4376" + Position [-185, 83, -155, 97] + ZOrder 883 + Port "5" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_l_sole" + SID "4377" + Position [-185, 123, -155, 137] + ZOrder 884 + Port "6" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_r_sole" + SID "4378" + Position [-185, 163, -155, 177] + ZOrder 885 + Port "7" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_CoM" + SID "4379" + Position [-185, 203, -155, 217] + ZOrder 886 + Port "8" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "pos_CoM" + SID "4380" + Position [-185, 243, -155, 257] + ZOrder 887 + Port "9" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "JDot_l_sole_nu" + SID "4381" + Position [-185, 283, -155, 297] + ZOrder 888 + Port "10" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "JDot_r_sole_nu" + SID "4382" + Position [-185, 323, -155, 337] + ZOrder 889 + Port "11" + IconDisplay "Port number" + 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 "Estimate Contact Forces and Forward Dynamics" + SID "4901" + Ports [8, 2] + Position [1315, 163, 1520, 337] + ZOrder 1228 + BackgroundColor "[1.000000, 0.333333, 1.000000]" + RequestExecContextInheritance off + System { + Name "Estimate Contact Forces and Forward Dynamics" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + 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 "317" + Block { + BlockType Inport + Name "JL" + SID "4902" + Position [185, 143, 215, 157] + ZOrder 1223 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JR" + SID "4903" + Position [185, 183, 215, 197] + ZOrder 1224 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "M" + SID "4906" + Position [185, 303, 215, 317] + ZOrder 1225 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "h" + SID "4907" + Position [185, 343, 215, 357] + ZOrder 1226 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JDotL_nu" + SID "4908" + Position [185, 223, 215, 237] + ZOrder 1227 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JDorR_nu" + SID "4909" + Position [185, 263, 215, 277] + ZOrder 1228 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "LEFT_RIGHT_FOOT_IN_CONTACT" + SID "4910" + Position [185, 383, 215, 397] + ZOrder 1229 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tau_star" + SID "4944" + Position [185, 103, 215, 117] + ZOrder 1250 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "Get Contact Forces From Dynamics" + SID "4913" + Ports [8, 1] + Position [505, 90, 800, 410] + ZOrder 1222 + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + ContentPreviewEnabled on + System { + Name "Get Contact Forces From Dynamics" + Location [223, 338, 826, 833] + Open off + PortBlocksUseCompactNotation off + 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 "47" + Block { + BlockType Inport + Name "tau" + SID "4913::25" + Position [20, 101, 40, 119] + ZOrder 16 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JL" + SID "4913::26" + Position [20, 136, 40, 154] + ZOrder 17 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JR" + SID "4913::27" + Position [20, 171, 40, 189] + ZOrder 18 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JDotL_nu" + SID "4913::1" + Position [20, 206, 40, 224] + ZOrder -1 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JDotR_nu" + SID "4913::29" + Position [20, 246, 40, 264] + ZOrder 20 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "M" + SID "4913::22" + Position [20, 281, 40, 299] + ZOrder 13 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "h" + SID "4913::35" + Position [20, 316, 40, 334] + ZOrder 26 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "LEFT_RIGHT_FOOT_IN_CONTACT" + SID "4913::37" + Position [20, 351, 40, 369] + ZOrder 27 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4913::46" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 35 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4913::45" + Tag "Stateflow S-Function torqueControlBalancingSim 34" + Ports [8, 2] + Position [180, 100, 230, 280] + ZOrder 34 + FunctionName "sf_sfun" + PortCounts "[8 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "f_dynamics" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4913::47" + Position [460, 241, 480, 259] + ZOrder 36 + } + Block { + BlockType Outport + Name "f_dynamics" + SID "4913::33" + Position [460, 101, 480, 119] + ZOrder 24 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 34 + SrcBlock "tau" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 35 + SrcBlock "JL" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 36 + SrcBlock "JR" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 37 + SrcBlock "JDotL_nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 38 + SrcBlock "JDotR_nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 39 + SrcBlock "M" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + ZOrder 40 + SrcBlock "h" + SrcPort 1 + DstBlock " SFunction " + DstPort 7 + } + Line { + ZOrder 41 + SrcBlock "LEFT_RIGHT_FOOT_IN_CONTACT" + SrcPort 1 + DstBlock " SFunction " + DstPort 8 + } + Line { + Name "f_dynamics" + ZOrder 42 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "f_dynamics" + DstPort 1 + } + Line { + ZOrder 43 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 44 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Get State Accelerations From Dynamics" + SID "4945" + Ports [7, 1] + Position [890, 424, 1050, 636] + ZOrder 1251 + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + ContentPreviewEnabled on + System { + Name "Get State Accelerations From Dynamics" + Location [223, 338, 826, 833] + Open off + PortBlocksUseCompactNotation off + 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 "47" + Block { + BlockType Inport + Name "tau" + SID "4945::25" + Position [20, 101, 40, 119] + ZOrder 16 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JL" + SID "4945::26" + Position [20, 136, 40, 154] + ZOrder 17 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JR" + SID "4945::27" + Position [20, 171, 40, 189] + ZOrder 18 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "M" + SID "4945::22" + Position [20, 206, 40, 224] + ZOrder 13 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "h" + SID "4945::35" + Position [20, 246, 40, 264] + ZOrder 26 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "f" + SID "4945::1" + Position [20, 281, 40, 299] + ZOrder -1 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "LEFT_RIGHT_FOOT_IN_CONTACT" + SID "4945::37" + Position [20, 316, 40, 334] + ZOrder 27 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4945::46" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 35 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4945::45" + Tag "Stateflow S-Function torqueControlBalancingSim 10" + Ports [7, 2] + Position [180, 100, 230, 260] + ZOrder 34 + FunctionName "sf_sfun" + PortCounts "[7 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "nuDot" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4945::47" + Position [460, 241, 480, 259] + ZOrder 36 + } + Block { + BlockType Outport + Name "nuDot" + SID "4945::33" + Position [460, 101, 480, 119] + ZOrder 24 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 31 + SrcBlock "tau" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 32 + SrcBlock "JL" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 33 + SrcBlock "JR" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 34 + SrcBlock "M" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 35 + SrcBlock "h" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 36 + SrcBlock "f" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + ZOrder 37 + SrcBlock "LEFT_RIGHT_FOOT_IN_CONTACT" + SrcPort 1 + DstBlock " SFunction " + DstPort 7 + } + Line { + Name "nuDot" + ZOrder 38 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "nuDot" + DstPort 1 + } + Line { + ZOrder 39 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 40 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "contactForces_est" + SID "4942" + Position [1090, 243, 1120, 257] + ZOrder 1231 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "nuDot" + SID "4946" + Position [1090, 523, 1120, 537] + ZOrder 1252 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 1 + SrcBlock "JDorR_nu" + SrcPort 1 + DstBlock "Get Contact Forces From Dynamics" + DstPort 5 + } + Line { + ZOrder 2 + SrcBlock "JR" + SrcPort 1 + Points [185, 0] + Branch { + ZOrder 3 + Points [0, 310] + DstBlock "Get State Accelerations From Dynamics" + DstPort 3 + } + Branch { + ZOrder 4 + DstBlock "Get Contact Forces From Dynamics" + DstPort 3 + } + } + Line { + ZOrder 5 + SrcBlock "JDotL_nu" + SrcPort 1 + DstBlock "Get Contact Forces From Dynamics" + DstPort 4 + } + Line { + ZOrder 6 + SrcBlock "LEFT_RIGHT_FOOT_IN_CONTACT" + SrcPort 1 + Points [105, 0] + Branch { + ZOrder 7 + Points [0, 230] + DstBlock "Get State Accelerations From Dynamics" + DstPort 7 + } + Branch { + ZOrder 8 + DstBlock "Get Contact Forces From Dynamics" + DstPort 8 + } + } + Line { + ZOrder 9 + SrcBlock "h" + SrcPort 1 + Points [135, 0] + Branch { + ZOrder 10 + Points [0, 210] + DstBlock "Get State Accelerations From Dynamics" + DstPort 5 + } + Branch { + ZOrder 11 + DstBlock "Get Contact Forces From Dynamics" + DstPort 7 + } + } + Line { + ZOrder 12 + SrcBlock "M" + SrcPort 1 + Points [159, 0] + Branch { + ZOrder 13 + Points [0, 220] + DstBlock "Get State Accelerations From Dynamics" + DstPort 4 + } + Branch { + ZOrder 14 + DstBlock "Get Contact Forces From Dynamics" + DstPort 6 + } + } + Line { + ZOrder 15 + SrcBlock "tau_star" + SrcPort 1 + Points [240, 0] + Branch { + ZOrder 16 + Points [0, 330] + DstBlock "Get State Accelerations From Dynamics" + DstPort 1 + } + Branch { + ZOrder 17 + DstBlock "Get Contact Forces From Dynamics" + DstPort 1 + } + } + Line { + ZOrder 18 + SrcBlock "JL" + SrcPort 1 + Points [216, 0] + Branch { + ZOrder 19 + Points [0, 320] + DstBlock "Get State Accelerations From Dynamics" + DstPort 2 + } + Branch { + ZOrder 20 + DstBlock "Get Contact Forces From Dynamics" + DstPort 2 + } + } + Line { + ZOrder 21 + SrcBlock "Get Contact Forces From Dynamics" + SrcPort 1 + Points [28, 0] + Branch { + ZOrder 25 + DstBlock "contactForces_est" + DstPort 1 + } + Branch { + ZOrder 22 + Points [0, 340] + DstBlock "Get State Accelerations From Dynamics" + DstPort 6 + } + } + Line { + ZOrder 24 + SrcBlock "Get State Accelerations From Dynamics" + SrcPort 1 + DstBlock "nuDot" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Joint Torque Saturation" + SID "4553" + Ports [1, 1] + Position [1125, 301, 1235, 339] + ZOrder 555 + BackgroundColor "orange" + RequestExecContextInheritance off + System { + Name "Joint Torque Saturation" + Location [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "247" + Block { + BlockType Inport + Name "tau_star" + SID "4554" + Position [-115, 153, -85, 167] + ZOrder -1 + IconDisplay "Port number" + } + 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 + 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 "89" + Block { + BlockType Inport + Name "u" + SID "4556::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "u_0" + SID "4556::22" + Position [20, 136, 40, 154] + ZOrder 13 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tStep" + SID "4556::67" + Position [20, 171, 40, 189] + ZOrder 56 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "uDotMax" + SID "4556::68" + Position [20, 206, 40, 224] + ZOrder 57 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4556::88" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 77 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4556::87" + Tag "Stateflow S-Function torqueControlBalancingSim 11" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 76 + FunctionName "sf_sfun" + PortCounts "[4 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "uSat" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4556::89" + Position [460, 241, 480, 259] + ZOrder 78 + } + Block { + BlockType Outport + Name "uSat" + SID "4556::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 161 + SrcBlock "u" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 162 + SrcBlock "u_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 163 + SrcBlock "tStep" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 164 + SrcBlock "uDotMax" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "uSat" + ZOrder 165 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "uSat" + DstPort 1 + } + Line { + ZOrder 166 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 167 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Saturate + Name "Saturation" + SID "2353" + Ports [1, 1] + Position [405, 77, 445, 113] + ZOrder 546 + ShowName off + InputPortMap "u0" + 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 + IconDisplay "Port number" + 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 + 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" + Block { + BlockType Inport + Name "jointPos" + SID "4834" + Position [-580, 2678, -550, 2692] + ZOrder 959 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointVel" + SID "4833" + Position [80, 2523, 110, 2537] + ZOrder 958 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "IMU_meas" + SID "4835" + Position [-580, 2733, -550, 2747] + ZOrder 960 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "4878" + Position [-580, 2788, -550, 2802] + ZOrder 961 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "4879" + Position [-580, 2843, -550, 2857] + ZOrder 962 + Port "5" + IconDisplay "Port number" + } + 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 + 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" + Block { + BlockType Inport + Name "jointPos" + SID "4212" + Position [-10, -187, 20, -173] + ZOrder 562 + NamePlacement "alternate" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointVel" + SID "4832" + Position [200, -92, 230, -78] + ZOrder 870 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_b" + SID "4213" + Position [-10, -167, 20, -153] + ZOrder 561 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "feetContactStatus\n" + SID "4214" + Position [200, -147, 230, -133] + ZOrder 244 + Port "4" + IconDisplay "Port number" + } + 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 + 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 "3818" + Block { + BlockType Inport + Name "J_l_sole" + SID "4215::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "J_r_sole" + SID "4215::1472" + Position [20, 136, 40, 154] + ZOrder 42 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "4215::1473" + Position [20, 171, 40, 189] + ZOrder 43 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointVel" + SID "4215::23" + Position [20, 206, 40, 224] + ZOrder 9 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4215::3817" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 186 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4215::3816" + Tag "Stateflow S-Function torqueControlBalancingSim 7" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 185 + 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::3818" + Position [460, 241, 480, 259] + ZOrder 187 + } + Block { + BlockType Outport + Name "nu_b" + SID "4215::5" + Position [460, 101, 480, 119] + ZOrder -6 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 50 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 51 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 52 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 53 + SrcBlock "jointVel" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "nu_b" + ZOrder 54 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "nu_b" + DstPort 1 + } + Line { + ZOrder 55 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 56 + 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 + 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" + Block { + BlockType Inport + Name "jointPos" + SID "4217" + Position [585, 598, 615, 612] + ZOrder 673 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_b" + SID "4218" + Position [585, 483, 615, 497] + ZOrder 680 + Port "2" + IconDisplay "Port number" + } + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "J_r_sole" + SID "4222" + Position [935, 588, 965, 602] + ZOrder 677 + Port "2" + IconDisplay "Port number" + 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 + IconDisplay "Port number" + 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 + 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" + Block { + BlockType Inport + Name "jointPos" + SID "4310" + Position [50, 38, 80, 52] + ZOrder 902 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "IMU_meas" + SID "4311" + Position [245, 183, 275, 197] + ZOrder 903 + Port "2" + IconDisplay "Port number" + } + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "122" + Block { + BlockType Inport + Name "jointPos" + SID "4246" + Position [470, 227, 495, 243] + ZOrder 736 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "inertial" + SID "4247" + Position [1000, 212, 1030, 228] + ZOrder 741 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_link" + SID "4248" + Position [655, 132, 680, 148] + ZOrder 744 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_base_fixed" + SID "4249" + Position [470, 32, 495, 48] + ZOrder 745 + Port "4" + IconDisplay "Port number" + } + 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 + 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 "3809" + SIDPrevWatermark "9" + Block { + BlockType Inport + Name "imu_H_fixedLink" + SID "4257::112" + Position [20, 101, 40, 119] + ZOrder 82 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "imu_H_fixedLink_0" + SID "4257::113" + Position [20, 136, 40, 154] + ZOrder 83 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "fixedLink_H_base" + SID "4257::111" + Position [20, 171, 40, 189] + ZOrder 81 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "rpyFromIMU_0" + SID "4257::115" + Position [20, 206, 40, 224] + ZOrder 85 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "rpyFromIMU" + SID "4257::93" + Position [20, 246, 40, 264] + ZOrder 69 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "neck_pos" + SID "4257::118" + Position [20, 281, 40, 299] + ZOrder 86 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4257::3808" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 208 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4257::3807" + Tag "Stateflow S-Function torqueControlBalancingSim 8" + Ports [6, 2] + Position [180, 102, 230, 243] + ZOrder 207 + 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::3809" + Position [460, 241, 480, 259] + ZOrder 209 + } + Block { + BlockType Outport + Name "w_H_b" + SID "4257::106" + Position [460, 101, 480, 119] + ZOrder 80 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 64 + SrcBlock "imu_H_fixedLink" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 65 + SrcBlock "imu_H_fixedLink_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 66 + SrcBlock "fixedLink_H_base" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 67 + SrcBlock "rpyFromIMU_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 68 + SrcBlock "rpyFromIMU" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 69 + SrcBlock "neck_pos" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + Name "w_H_b" + ZOrder 70 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "w_H_b" + 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 Product + Name "Inv" + SID "4252" + Ports [2, 1] + Position [825, 40, 895, 95] + ZOrder 731 + ShowName off + HideAutomaticName off + Inputs "/*" + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + 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(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + 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" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "414" + Block { + BlockType Inport + Name "neck port" + SID "4261" + Position [45, 18, 75, 32] + ZOrder 725 + NamePlacement "alternate" + IconDisplay "Port number" + } + 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" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + 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 + IconDisplay "Port number" + 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 + IconDisplay "Port number" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "122" + Block { + BlockType Inport + Name "jointPos" + SID "4851" + Position [470, 227, 495, 243] + ZOrder 736 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "inertial" + SID "4852" + Position [1000, 212, 1030, 228] + ZOrder 741 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_link" + SID "4853" + Position [655, 132, 680, 148] + ZOrder 744 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_base_fixed" + SID "4854" + Position [470, 32, 495, 48] + ZOrder 745 + Port "4" + IconDisplay "Port number" + } + 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 + 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 "3809" + SIDPrevWatermark "9" + Block { + BlockType Inport + Name "imu_H_fixedLink" + SID "4857::112" + Position [20, 101, 40, 119] + ZOrder 82 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "imu_H_fixedLink_0" + SID "4857::113" + Position [20, 136, 40, 154] + ZOrder 83 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "fixedLink_H_base" + SID "4857::111" + Position [20, 171, 40, 189] + ZOrder 81 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "rpyFromIMU_0" + SID "4857::115" + Position [20, 206, 40, 224] + ZOrder 85 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "rpyFromIMU" + SID "4857::93" + Position [20, 246, 40, 264] + ZOrder 69 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "neck_pos" + SID "4857::118" + Position [20, 281, 40, 299] + ZOrder 86 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4857::3808" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 208 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4857::3807" + Tag "Stateflow S-Function torqueControlBalancingSim 3" + Ports [6, 2] + Position [180, 102, 230, 243] + ZOrder 207 + 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::3809" + Position [460, 241, 480, 259] + ZOrder 209 + } + Block { + BlockType Outport + Name "w_H_b" + SID "4857::106" + Position [460, 101, 480, 119] + ZOrder 80 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 64 + SrcBlock "imu_H_fixedLink" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 65 + SrcBlock "imu_H_fixedLink_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 66 + SrcBlock "fixedLink_H_base" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 67 + SrcBlock "rpyFromIMU_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 68 + SrcBlock "rpyFromIMU" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 69 + SrcBlock "neck_pos" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + Name "w_H_b" + ZOrder 70 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "w_H_b" + 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 Product + Name "Inv" + SID "4858" + Ports [2, 1] + Position [825, 40, 895, 95] + ZOrder 731 + ShowName off + HideAutomaticName off + Inputs "/*" + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + 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(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + 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" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "414" + Block { + BlockType Inport + Name "neck port" + SID "4868" + Position [45, 18, 75, 32] + ZOrder 725 + NamePlacement "alternate" + IconDisplay "Port number" + } + 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" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + 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 + IconDisplay "Port number" + 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 + IconDisplay "Port number" + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "fixed_r_sole_H_b" + SID "4320" + Position [650, 113, 680, 127] + ZOrder 912 + Port "2" + IconDisplay "Port number" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "144" + Block { + BlockType Inport + Name "jointPos" + SID "3166" + Position [-575, 263, -545, 277] + ZOrder 676 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "fixed_l_sole_H_b" + SID "4730" + Position [-575, 403, -545, 417] + ZOrder 902 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "fixed_r_sole_H_b" + SID "4731" + Position [-575, 438, -545, 452] + ZOrder 903 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "4876" + Position [-375, 333, -345, 347] + ZOrder 955 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "4877" + Position [-375, 368, -345, 382] + ZOrder 956 + Port "5" + IconDisplay "Port number" + } + 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]" + DisplayOption "bar" + } + 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" + 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 + 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 "3797" + SIDPrevWatermark "9" + Block { + BlockType Inport + Name "pos_CoM_0" + SID "2220::58" + Position [20, 101, 40, 119] + ZOrder 34 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos_0" + SID "2220::82" + Position [20, 136, 40, 154] + ZOrder 58 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "pos_CoM_fixed_l_sole" + SID "2220::92" + Position [20, 171, 40, 189] + ZOrder 68 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "pos_CoM_fixed_r_sole" + SID "2220::107" + Position [20, 206, 40, 224] + ZOrder 81 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "2220::97" + Position [20, 246, 40, 264] + ZOrder 72 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "time" + SID "2220::84" + Position [20, 281, 40, 299] + ZOrder 60 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "wrench_rightFoot" + SID "2220::93" + Position [20, 316, 40, 334] + ZOrder 69 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "2220::103" + Position [20, 351, 40, 369] + ZOrder 77 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "l_sole_H_b" + SID "2220::101" + Position [20, 386, 40, 404] + ZOrder 75 + Port "9" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "r_sole_H_b" + SID "2220::108" + Position [20, 426, 40, 444] + ZOrder 82 + Port "10" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "2220::3796" + Ports [1, 1] + Position [270, 495, 320, 535] + ZOrder 194 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "2220::3795" + Tag "Stateflow S-Function torqueControlBalancingSim 13" + Ports [10, 11] + Position [180, 100, 230, 340] + ZOrder 193 + 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::3797" + Position [460, 506, 480, 524] + ZOrder 195 + } + Block { + BlockType Outport + Name "w_H_b" + SID "2220::106" + Position [460, 101, 480, 119] + ZOrder 80 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "pos_CoM_des" + SID "2220::77" + Position [460, 136, 480, 154] + ZOrder 53 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "jointPos_des" + SID "2220::67" + Position [460, 171, 480, 189] + ZOrder 43 + Port "3" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "feetContactStatus" + SID "2220::64" + Position [460, 206, 480, 224] + ZOrder 40 + Port "4" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_postural_diag" + SID "2220::94" + Position [460, 246, 480, 264] + ZOrder 70 + Port "5" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_CoM_diag" + SID "2220::112" + Position [460, 281, 480, 299] + ZOrder 84 + Port "6" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KD_CoM_diag" + SID "2220::113" + Position [460, 316, 480, 334] + ZOrder 85 + Port "7" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "state" + SID "2220::96" + Position [460, 351, 480, 369] + ZOrder 71 + Port "8" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "smoothingTimeJoints" + SID "2220::111" + Position [460, 386, 480, 404] + ZOrder 83 + Port "9" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "smoothingTimeCoM" + SID "2220::3776" + Position [460, 426, 480, 444] + ZOrder 174 + Port "10" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 948 + SrcBlock "pos_CoM_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 949 + SrcBlock "jointPos_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 950 + SrcBlock "pos_CoM_fixed_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 951 + SrcBlock "pos_CoM_fixed_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 952 + SrcBlock "jointPos" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 953 + SrcBlock "time" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + ZOrder 954 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock " SFunction " + DstPort 7 + } + Line { + ZOrder 955 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock " SFunction " + DstPort 8 + } + Line { + ZOrder 956 + SrcBlock "l_sole_H_b" + SrcPort 1 + DstBlock " SFunction " + DstPort 9 + } + Line { + ZOrder 957 + SrcBlock "r_sole_H_b" + SrcPort 1 + DstBlock " SFunction " + DstPort 10 + } + Line { + Name "w_H_b" + ZOrder 958 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "w_H_b" + DstPort 1 + } + Line { + Name "pos_CoM_des" + ZOrder 959 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "pos_CoM_des" + DstPort 1 + } + Line { + Name "jointPos_des" + ZOrder 960 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "jointPos_des" + DstPort 1 + } + Line { + Name "feetContactStatus" + ZOrder 961 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "feetContactStatus" + DstPort 1 + } + Line { + Name "KP_postural_diag" + ZOrder 962 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "KP_postural_diag" + DstPort 1 + } + Line { + Name "KP_CoM_diag" + ZOrder 963 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 7 + DstBlock "KP_CoM_diag" + DstPort 1 + } + Line { + Name "KD_CoM_diag" + ZOrder 964 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 8 + DstBlock "KD_CoM_diag" + DstPort 1 + } + Line { + Name "state" + ZOrder 965 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 9 + DstBlock "state" + DstPort 1 + } + Line { + Name "smoothingTimeJoints" + ZOrder 966 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 10 + DstBlock "smoothingTimeJoints" + DstPort 1 + } + Line { + Name "smoothingTimeCoM" + ZOrder 967 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 11 + DstBlock "smoothingTimeCoM" + DstPort 1 + } + Line { + ZOrder 968 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 969 + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "405" + Block { + BlockType Inport + Name "w_H_b" + SID "4227" + Position [30, 237, 60, 253] + ZOrder 581 + NamePlacement "alternate" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "4228" + Position [30, 267, 60, 283] + ZOrder 582 + Port "2" + IconDisplay "Port number" + } + 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 + IconDisplay "Port number" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "404" + Block { + BlockType Inport + Name "w_H_b" + SID "4733" + Position [30, 237, 60, 253] + ZOrder 581 + NamePlacement "alternate" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "4734" + Position [30, 267, 60, 283] + ZOrder 582 + Port "2" + IconDisplay "Port number" + } + 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 + IconDisplay "Port number" + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "feetContactStatus" + SID "2236" + Position [580, 229, 610, 241] + ZOrder 480 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "pos_CoM_des" + SID "2234" + Position [365, 158, 395, 172] + ZOrder 478 + Port "3" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "jointPos_des" + SID "2235" + Position [365, 193, 395, 207] + ZOrder 479 + Port "4" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_postural_diag" + SID "2237" + Position [580, 257, 615, 273] + ZOrder 545 + Port "5" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "smoothingTime_Joints" + SID "3181" + Position [365, 403, 395, 417] + ZOrder 680 + Port "6" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_CoM_diag" + SID "3316" + Position [580, 297, 615, 313] + ZOrder 689 + Port "7" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KD_CoM_diag" + SID "3318" + Position [580, 335, 615, 355] + ZOrder 691 + Port "8" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "smoothingTime_CoM" + SID "4738" + Position [365, 438, 395, 452] + ZOrder 905 + Port "9" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "state" + SID "2238" + Position [365, 367, 395, 383] + ZOrder 550 + Port "10" + IconDisplay "Port number" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "239" + Block { + BlockType Inport + Name "pos_CoM_des" + SID "4743" + Position [315, -137, 345, -123] + ZOrder 909 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos_des" + SID "4742" + Position [315, -252, 345, -238] + ZOrder 908 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "KP_postural_diag" + SID "4748" + Position [315, -17, 345, -3] + ZOrder 914 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "smoothingTime_Joints" + SID "4741" + Position [315, -222, 345, -208] + ZOrder 907 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "KP_CoM_diag" + SID "4739" + Position [315, 13, 345, 27] + ZOrder 905 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "KD_CoM_diag" + SID "4740" + Position [315, 43, 345, 57] + ZOrder 906 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "smoothingTime_CoM" + SID "4033" + Position [315, -102, 345, -88] + ZOrder 704 + Port "7" + IconDisplay "Port number" + } + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "381" + Block { + BlockType Inport + Name "KP_postural_diag" + SID "4775" + Position [20, -82, 50, -68] + ZOrder 915 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "KP_CoM_diag" + SID "4776" + Position [20, -12, 50, 2] + ZOrder 916 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "KD_CoM_diag" + SID "4777" + Position [20, 63, 50, 77] + ZOrder 917 + Port "3" + IconDisplay "Port number" + } + 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 + 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 "42" + Block { + BlockType Inport + Name "d" + SID "4782::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4782::41" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 32 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4782::40" + Tag "Stateflow S-Function torqueControlBalancingSim 2" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 31 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "D" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4782::42" + Position [460, 241, 480, 259] + ZOrder 33 + } + Block { + BlockType Outport + Name "D" + SID "4782::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 33 + SrcBlock "d" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "D" + ZOrder 34 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "D" + DstPort 1 + } + Line { + ZOrder 35 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 36 + 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 + 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 "42" + Block { + BlockType Inport + Name "d" + SID "4783::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4783::41" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 32 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4783::40" + Tag "Stateflow S-Function torqueControlBalancingSim 1" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 31 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "D" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4783::42" + Position [460, 241, 480, 259] + ZOrder 33 + } + Block { + BlockType Outport + Name "D" + SID "4783::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 29 + SrcBlock "d" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "D" + ZOrder 30 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "D" + DstPort 1 + } + Line { + ZOrder 31 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 32 + 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 + 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 "42" + Block { + BlockType Inport + Name "d" + SID "4784::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4784::41" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 32 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4784::40" + Tag "Stateflow S-Function torqueControlBalancingSim 4" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 31 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "D" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4784::42" + Position [460, 241, 480, 259] + ZOrder 33 + } + Block { + BlockType Outport + Name "D" + SID "4784::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Line { + ZOrder 29 + SrcBlock "d" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "D" + ZOrder 30 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "D" + DstPort 1 + } + Line { + ZOrder 31 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 32 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "KP_postural" + SID "4778" + Position [420, -82, 450, -68] + ZOrder 918 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_CoM" + SID "4779" + Position [420, -12, 450, 2] + ZOrder 919 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KD_CoM" + SID "4780" + Position [420, 63, 450, 77] + ZOrder 920 + Port "3" + IconDisplay "Port number" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "175" + Block { + BlockType Inport + Name "pos_CoM_des" + SID "4768" + Position [55, -257, 85, -243] + ZOrder 910 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "smoothingTime_CoM" + SID "4771" + Position [55, -202, 85, -188] + ZOrder 913 + Port "2" + IconDisplay "Port number" + } + 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" + 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 + IconDisplay "Port number" + 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 [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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 "209" + Block { + BlockType Inport + Name "jointPos_des" + SID "4762" + Position [-65, -97, -35, -83] + ZOrder 905 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "smoothingTime_Joints" + SID "4763" + Position [-65, -42, -35, -28] + ZOrder 906 + Port "2" + IconDisplay "Port number" + } + 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" + 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 + IconDisplay "Port number" + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_CoM" + SID "4035" + Position [745, 13, 775, 27] + ZOrder 706 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KD_CoM" + SID "4036" + Position [745, 43, 775, 57] + ZOrder 707 + Port "3" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "desired_pos_vel_acc_CoM" + SID "4041" + Position [745, -117, 775, -103] + ZOrder 712 + Port "4" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "jointPos_des_smoothed" + SID "4042" + Position [745, -237, 775, -223] + ZOrder 713 + Port "5" + IconDisplay "Port number" + 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 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "nu" + SID "2271" + Position [340, 2539, 370, 2551] + ZOrder 577 + Port "2" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "state" + SID "3201" + Position [25, 2870, 55, 2880] + ZOrder 647 + Port "3" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "feetContactStatus" + SID "2272" + Position [80, 2610, 110, 2620] + ZOrder 410 + Port "4" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_postural" + SID "2268" + Position [340, 2670, 370, 2680] + ZOrder 517 + Port "5" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KP_CoM" + SID "3330" + Position [340, 2710, 370, 2720] + ZOrder 692 + Port "6" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "KD_CoM" + SID "3331" + Position [340, 2750, 370, 2760] + ZOrder 693 + Port "7" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "desired_pos_vel_acc_CoM" + SID "2274" + Position [340, 2790, 370, 2800] + ZOrder 323 + Port "8" + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "jointPos_des" + SID "2273" + Position [340, 2830, 370, 2840] + ZOrder 418 + Port "9" + IconDisplay "Port number" + 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, -20] + Branch { + ZOrder 721 + Points [0, -25] + DstBlock "Compute State Velocity" + DstPort 4 + } + Branch { + ZOrder 685 + 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 "5020" + Ports [1] + Position [305, 241, 430, 269] + ZOrder 1230 + BackgroundColor "red" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 32 + $ClassName "Simulink.Mask" + Display "disp('EMERGENCY STOP')" + RunInitForIconRedraw "off" + } + System { + Name "emergency stop: joint limits" + Location [67, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + 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" + Block { + BlockType Inport + Name "jointPos" + SID "5021" + Position [150, 238, 180, 252] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n1" + SID "5022" + 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 "5023" + 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 "5024" + 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 + 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" + Block { + BlockType Inport + Name "jointPos" + SID "5025" + Position [60, 103, 90, 117] + ZOrder 552 + IconDisplay "Port number" + } + Block { + BlockType EnablePort + Name "Enable" + SID "5026" + Ports [] + Position [227, -20, 247, 0] + ZOrder 553 + ShowName off + HideAutomaticName off + } + Block { + BlockType Assertion + Name "Assertion" + SID "5027" + Position [360, 37, 420, 83] + ZOrder 207 + HideAutomaticName off + } + Block { + BlockType Reference + Name "GetLimits" + SID "5028" + 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 "5029" + 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 + 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 "3795" + Block { + BlockType Inport + Name "umin" + SID "5029::18" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "umax" + SID "5029::19" + Position [20, 136, 40, 154] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "u" + SID "5029::1" + Position [20, 171, 40, 189] + ZOrder -3 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tol" + SID "5029::20" + Position [20, 206, 40, 224] + ZOrder -4 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "5029::3794" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 144 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "5029::3793" + Tag "Stateflow S-Function torqueControlBalancingSim 18" + Ports [4, 3] + Position [180, 102, 230, 203] + ZOrder 143 + 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 "5029::3795" + Position [460, 241, 480, 259] + ZOrder 145 + } + Block { + BlockType Outport + Name "inRange" + SID "5029::5" + Position [460, 101, 480, 119] + ZOrder -8 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "res_check_range" + SID "5029::3789" + Position [460, 136, 480, 154] + ZOrder 139 + Port "2" + IconDisplay "Port number" + } + Line { + ZOrder 17 + SrcBlock "umin" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 18 + SrcBlock "umax" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 19 + SrcBlock "u" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 20 + SrcBlock "tol" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "inRange" + ZOrder 21 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "inRange" + DstPort 1 + } + Line { + Name "res_check_range" + ZOrder 22 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "res_check_range" + DstPort 1 + } + Line { + ZOrder 23 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 24 + SrcBlock " SFunction " + SrcPort 1 + Points [20, 0] + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "5030" + 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 "5031" + Position [320, 48, 340, 72] + ZOrder 559 + ShowName off + HideAutomaticName off + InitialCondition "1" + SampleTime "Config.tStep" + HasFrameUpgradeWarning on + } + Block { + BlockType Constant + Name "index1" + SID "5032" + 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 3 + SrcBlock "index1" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 4 + } + Line { + ZOrder 4 + SrcBlock "MATLAB Function" + SrcPort 2 + DstBlock "To Workspace" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "Unit Delay" + SrcPort 1 + DstBlock "Assertion" + DstPort 1 + } + Line { + ZOrder 6 + SrcBlock "GetLimits" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 7 + SrcBlock "GetLimits" + SrcPort 2 + DstBlock "MATLAB Function" + DstPort 2 + } + } + } + Block { + BlockType SubSystem + Name "STOP IF THERE ARE SPIKES IN THE ENCODERS" + SID "5033" + 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 + 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" + Block { + BlockType Inport + Name "jointPos" + SID "5034" + Position [-10, 53, 20, 67] + ZOrder 552 + IconDisplay "Port number" + } + Block { + BlockType EnablePort + Name "Enable" + SID "5035" + Ports [] + Position [172, -15, 192, 5] + ZOrder 553 + ShowName off + HideAutomaticName off + } + Block { + BlockType Assertion + Name "Assertion" + SID "5036" + Position [335, 37, 400, 83] + ZOrder 556 + HideAutomaticName off + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "5037" + 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 + 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 "3795" + Block { + BlockType Inport + Name "u" + SID "5037::1" + Position [20, 101, 40, 119] + ZOrder -3 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "delta_u_max" + SID "5037::18" + Position [20, 136, 40, 154] + ZOrder -1 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "5037::3794" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 144 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "5037::3793" + Tag "Stateflow S-Function torqueControlBalancingSim 14" + Ports [2, 3] + Position [180, 100, 230, 180] + ZOrder 143 + 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 "5037::3795" + Position [460, 241, 480, 259] + ZOrder 145 + } + Block { + BlockType Outport + Name "noSpikes" + SID "5037::5" + Position [460, 101, 480, 119] + ZOrder -8 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "res_check_spikes" + SID "5037::3789" + Position [460, 136, 480, 154] + ZOrder 139 + Port "2" + IconDisplay "Port number" + } + Line { + ZOrder 13 + SrcBlock "u" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 14 + SrcBlock "delta_u_max" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "noSpikes" + ZOrder 15 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "noSpikes" + DstPort 1 + } + Line { + Name "res_check_spikes" + ZOrder 16 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "res_check_spikes" + DstPort 1 + } + Line { + ZOrder 17 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 18 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "5038" + 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 "5039" + Position [290, 48, 310, 72] + ZOrder 558 + ShowName off + HideAutomaticName off + InitialCondition "1" + SampleTime "Config.tStep" + HasFrameUpgradeWarning on + } + Block { + BlockType Constant + Name "index1" + SID "5040" + 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 2 + 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 5 + 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 "nuDot" + SID "4841" + Position [1565, 288, 1595, 302] + ZOrder 560 + IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off + } + Block { + BlockType Outport + Name "contactForces_est" + SID "4948" + Position [1565, 203, 1595, 217] + ZOrder 1229 + Port "2" + IconDisplay "Port number" + 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 + Points [107, 0] + Branch { + ZOrder 390 + DstBlock "Balancing Controller QP" + DstPort 11 + } + Branch { + ZOrder 329 + Points [0, 327; 420, 0; 0, -382] + DstBlock "Estimate Contact Forces and Forward Dynamics" + DstPort 6 + } + } + Line { + ZOrder 100 + SrcBlock "Dynamics and Kinematics" + SrcPort 9 + DstBlock "Balancing Controller QP" + DstPort 9 + } + Line { + ZOrder 91 + SrcBlock "Dynamics and Kinematics" + SrcPort 2 + Points [134, 0] + Branch { + ZOrder 388 + DstBlock "Balancing Controller QP" + DstPort 2 + } + Branch { + ZOrder 325 + Points [0, -61; 373, 0; 0, 236] + DstBlock "Estimate Contact Forces and Forward Dynamics" + DstPort 4 + } + } + 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 98 + SrcBlock "Dynamics and Kinematics" + SrcPort 7 + Points [88, 0] + Branch { + ZOrder 386 + DstBlock "Balancing Controller QP" + DstPort 7 + } + Branch { + ZOrder 321 + Points [0, -240; 442, 0; 0, 225] + DstBlock "Estimate Contact Forces and Forward Dynamics" + DstPort 2 + } + } + 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 + Points [69, 0] + Branch { + ZOrder 384 + DstBlock "Balancing Controller QP" + DstPort 6 + } + Branch { + ZOrder 319 + Points [0, -222; 477, 0; 0, 217] + DstBlock "Estimate Contact Forces and Forward Dynamics" + DstPort 1 + } + } + Line { + ZOrder 90 + SrcBlock "Dynamics and Kinematics" + SrcPort 1 + Points [112, 0] + Branch { + ZOrder 387 + DstBlock "Balancing Controller QP" + DstPort 1 + } + Branch { + ZOrder 323 + Points [0, -46; 408, 0; 0, 231] + DstBlock "Estimate Contact Forces and Forward Dynamics" + DstPort 3 + } + } + 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 + Points [379, 0] + Branch { + ZOrder 391 + DstBlock "Balancing Controller QP" + DstPort 15 + } + Branch { + ZOrder 331 + Points [0, 226; 456, 0; 0, -381] + DstBlock "Estimate Contact Forces and Forward Dynamics" + DstPort 7 + } + } + 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 + Points [127, 0] + Branch { + ZOrder 389 + DstBlock "Balancing Controller QP" + DstPort 10 + } + Branch { + ZOrder 327 + Points [0, 341; 382, 0; 0, -386] + DstBlock "Estimate Contact Forces and Forward Dynamics" + DstPort 5 + } + } + 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 438 + 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 + } + Line { + ZOrder 315 + SrcBlock "Joint Torque Saturation" + SrcPort 1 + DstBlock "Estimate Contact Forces and Forward Dynamics" + DstPort 8 + } + Line { + ZOrder 316 + SrcBlock "Estimate Contact Forces and Forward Dynamics" + SrcPort 2 + DstBlock "nuDot" + DstPort 1 + } + Line { + ZOrder 317 + SrcBlock "Estimate Contact Forces and Forward Dynamics" + SrcPort 1 + DstBlock "contactForces_est" + DstPort 1 + } + } + } + Block { + BlockType Selector + Name "Selector" + SID "4950" + Ports [1, 1] + Position [855, 636, 880, 654] + ZOrder 1230 + BlockMirror on + InputPortWidth "ROBOT_DOF+6" + IndexOptions "Index vector (dialog)" + Indices "[7:ROBOT_DOF+6]" + OutputSizes "1" + } + Block { + BlockType Selector + Name "Selector1" + SID "4951" + Ports [1, 1] + Position [855, 696, 880, 714] + ZOrder 1231 + BlockMirror on + InputPortWidth "ROBOT_DOF+16" + IndexOptions "Index vector (dialog)" + Indices "[17:ROBOT_DOF+16]" + OutputSizes "1" + } + Block { + BlockType SubSystem + Name "Set Position for Gazebo" + SID "5016" + Ports [1] + Position [655, 694, 695, 716] + ZOrder 1252 + BlockMirror on + Commented "on" + RequestExecContextInheritance off + System { + Name "Set Position for Gazebo" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + 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" + Block { + BlockType Inport + Name "jointPos" + SID "5017" + Position [160, 68, 190, 82] + ZOrder 1252 + BlockMirror on + IconDisplay "Port number" + } + Block { + BlockType Reference + Name "SetReferences" + SID "5014" + Ports [1] + Position [35, 62, 110, 88] + ZOrder 1250 + BlockMirror on + BackgroundColor "[0.392200, 0.788200, 0.345100]" + 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 on + controlType "Position Direct" + refSpeed "10*(pi/180)" + refAcceleration "1000000*(pi/180)" + } + Block { + BlockType Reference + Name "Simulator Synchronizer" + SID "5015" + Ports [] + Position [20, 24, 125, 51] + ZOrder 1251 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" + SourceType "Simulator Synchronizer" + SourceProductName "WholeBodyToolbox" + period "Config.tStep" + serverPortName "'/clock/rpc'" + clientPortName "'/WBT_synchronizer/rpc:o'" + } + Line { + ZOrder 351 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "SetReferences" + DstPort 1 + } + } + } + Block { + BlockType UnitDelay + Name "Unit Delay" + SID "5004" + Position [835, 530, 855, 560] + ZOrder 1249 + HideAutomaticName off + InitialCondition "[0,0,150,0,0,0,0,0,150,0,0,0]" + SampleTime "Config.tStep" + HasFrameUpgradeWarning on + } + Line { + ZOrder 289 + SrcBlock "MBC control + forward dynamics" + SrcPort 1 + Points [52, 0; 0, 170] + DstBlock "InverseKinematics Integration" + DstPort 1 + } + Line { + ZOrder 296 + SrcBlock "Selector" + SrcPort 1 + Points [-85, 0; 0, -175] + DstBlock "MBC control + forward dynamics" + DstPort 2 + } + Line { + ZOrder 298 + SrcBlock "Selector1" + SrcPort 1 + Points [-104, 0] + Branch { + ZOrder 366 + DstBlock "Set Position for Gazebo" + DstPort 1 + } + Branch { + ZOrder 354 + Points [0, -265] + DstBlock "MBC control + forward dynamics" + DstPort 1 + } + } + Line { + ZOrder 288 + SrcBlock "Constant" + SrcPort 1 + DstBlock "MBC control + forward dynamics" + DstPort 3 + } + Line { + ZOrder 292 + SrcBlock "Demux" + SrcPort 2 + DstBlock "MBC control + forward dynamics" + DstPort 5 + } + Line { + ZOrder 291 + SrcBlock "Demux" + SrcPort 1 + DstBlock "MBC control + forward dynamics" + DstPort 4 + } + Line { + ZOrder 293 + SrcBlock "InverseKinematics Integration" + SrcPort 1 + DstBlock "Selector" + DstPort 1 + } + Line { + ZOrder 295 + SrcBlock "InverseKinematics Integration" + SrcPort 2 + DstBlock "Selector1" + DstPort 1 + } + Line { + ZOrder 335 + SrcBlock "Init. Cond." + SrcPort 1 + DstBlock "InverseKinematics Integration" + DstPort 2 + } + Line { + ZOrder 336 + SrcBlock "Init. Cond." + SrcPort 2 + DstBlock "InverseKinematics Integration" + DstPort 3 + } + Line { + ZOrder 343 + SrcBlock "Unit Delay" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + ZOrder 348 + SrcBlock "MBC control + forward dynamics" + SrcPort 2 + Points [29, 0; 0, 62; -343, 0; 0, -57] + DstBlock "Unit Delay" + DstPort 1 + } + } +} +#Finite State Machines +# +# Stateflow 80000019 +# +# +Stateflow { + machine { + id 1 + name "torqueControlBalancingSim" + created "18-Feb-2014 10:13:36" + isLibrary 0 + sfVersion 80000012 + firstTarget 294 + } + chart { + id 2 + machine 1 + name "MBC control + forward dynamics/Robot State and References/Update Gains and References/Reshape Gains Mat" + "rices/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 "MBC control + forward dynamics/Robot State and References/Update Gains and References/Reshape Gains Mat" + "rices/MATLAB Function1" + chart 2 + } + chart { + id 9 + machine 1 + name "MBC control + forward dynamics/Robot State and References/Update Gains and References/Reshape Gains Mat" + "rices/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 "MBC control + forward dynamics/Robot State and References/Update Gains and References/Reshape Gains Mat" + "rices/MATLAB Function" + chart 9 + } + chart { + id 16 + machine 1 + name "MBC control + forward dynamics/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 "MBC control + forward dynamics/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 "MBC control + forward dynamics/Robot State and References/Update Gains and References/Reshape Gains Mat" + "rices/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 "MBC control + forward dynamics/Robot State and References/Update Gains and References/Reshape Gains Mat" + "rices/MATLAB Function2" + chart 29 + } + chart { + id 36 + machine 1 + name "InverseKinematics Integration/Compute the Base Pose Derivative" + windowPosition [357.12 483.496 167 391] + viewLimits [0 156.75 0 153.75] + screen [1 1 1280 1024 1.041666666666667] + treeNode [0 37 0 0] + viewObj 36 + ssIdHighWaterMark 7 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 5 + disableImplicitCasting 1 + eml { + name "computeBasePoseDerivativeFCN" + } + 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 pose_base_dot = computeBasePoseDerivativeFCN(nu_base, pose_base)\n\n pose_base_dot = " + "computeBasePoseDerivative(nu_base, pose_base);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 38 + ssIdNumber 5 + name "pose_base_dot" + 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 0 39] + } + data { + id 39 + ssIdNumber 6 + name "nu_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 [36 38 40] + } + data { + id 40 + ssIdNumber 7 + name "pose_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 [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 [32.125 19.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 "InverseKinematics Integration/Compute the Base Pose Derivative" + chart 36 + } + chart { + id 44 + machine 1 + name "MBC control + forward dynamics/Dynamics and Kinematics/Dynamics/Add motors reflected inertia/Add motor " + "reflected inertias" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 45 0 0] + viewObj 44 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 6 + disableImplicitCasting 1 + eml { + name "addMotorsInertiaFCN" + } + firstData 46 + firstTransition 50 + firstJunction 49 + } + 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 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 46 + 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 [44 0 47] + } + data { + id 47 + 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 [44 46 48] + } + data { + id 48 + 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 [44 47 0] + } + junction { + id 49 + position [23.5747 49.5747 7] + chart 44 + subviewer 44 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [44 0 0] + } + transition { + id 50 + 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 49 + 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 51 + machine 1 + name "MBC control + forward dynamics/Dynamics and Kinematics/Dynamics/Add motors reflected inertia/Add motor " + "reflected inertias" + chart 44 + } + chart { + id 52 + machine 1 + name "MBC control + forward dynamics/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 53 0 0] + viewObj 52 + ssIdHighWaterMark 10 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 7 + disableImplicitCasting 1 + eml { + name "computeBaseVelocityFCN" + } + firstData 54 + firstTransition 61 + firstJunction 60 + } + state { + id 53 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 52 + treeNode [52 0 0 0] + superState SUBCHART + subviewer 52 + 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 54 + 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 [52 0 55] + } + data { + id 55 + 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 [52 54 56] + } + data { + id 56 + 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 [52 55 57] + } + data { + id 57 + 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 [52 56 58] + } + data { + id 58 + 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 [52 57 59] + } + data { + id 59 + 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 [52 58 0] + } + junction { + id 60 + position [23.5747 49.5747 7] + chart 52 + subviewer 52 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [52 0 0] + } + transition { + id 61 + 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 60 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 52 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 52 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [52 0 0] + } + instance { + id 62 + machine 1 + name "MBC control + forward dynamics/Robot State and References/Compute State Velocity/Compute Base Velocity" + chart 52 + } + chart { + id 63 + machine 1 + name "MBC control + forward dynamics/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 64 0 0] + viewObj 63 + ssIdHighWaterMark 88 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 8 + disableImplicitCasting 1 + eml { + name "getWorldToBaseTransformFCN" + } + firstData 65 + firstTransition 74 + firstJunction 73 + } + state { + id 64 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 63 + treeNode [63 0 0 0] + superState SUBCHART + subviewer 63 + 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 65 + 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 [63 0 66] + } + data { + id 66 + 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 [63 65 67] + } + data { + id 67 + 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 [63 66 68] + } + data { + id 68 + 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 [63 67 69] + } + data { + id 69 + 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 [63 68 70] + } + data { + id 70 + 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 [63 69 71] + } + data { + id 71 + 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 [63 70 72] + } + data { + id 72 + 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 [63 71 0] + } + junction { + id 73 + position [23.5747 49.5747 7] + chart 63 + subviewer 63 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [63 0 0] + } + transition { + id 74 + 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 73 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 63 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 63 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [63 0 0] + } + instance { + id 75 + machine 1 + name "MBC control + forward dynamics/Robot State and References/Compute base to fixed link transform/LFoot to" + " base link transform /Get Base Rotation From IMU" + chart 63 + } + chart { + id 76 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/From tau_QP to Joint Torques (motor reflected in" + "ertia)/(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 77 0 0] + viewObj 76 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 9 + disableImplicitCasting 1 + eml { + name "computeMotorsInertiaFCN" + } + firstData 78 + firstTransition 81 + firstJunction 80 + } + state { + id 77 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 76 + treeNode [76 0 0 0] + superState SUBCHART + subviewer 76 + 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 78 + 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 [76 0 79] + } + data { + id 79 + 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 [76 78 0] + } + junction { + id 80 + position [23.5747 49.5747 7] + chart 76 + subviewer 76 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [76 0 0] + } + transition { + id 81 + 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 80 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 76 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 76 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [76 0 0] + } + instance { + id 82 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/From tau_QP to Joint Torques (motor reflected in" + "ertia)/(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" + chart 76 + } + chart { + id 83 + machine 1 + name "MBC control + forward dynamics/Estimate Contact Forces and Forward Dynamics/Get State Accelerations Fro" + "m Dynamics" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 84 0 0] + viewObj 83 + ssIdHighWaterMark 22 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 10 + disableImplicitCasting 1 + eml { + name "forwardDynamicsFCN" + } + firstData 85 + firstTransition 94 + firstJunction 93 + } + state { + id 84 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 83 + treeNode [83 0 0 0] + superState SUBCHART + subviewer 83 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function nuDot = forwardDynamicsFCN(tau, JL, JR, M, h, f, LEFT_RIGHT_FOOT_IN_CONTACT)\n \n nuD" + "ot = forwardDynamics(tau, JL, JR, M, h, f, LEFT_RIGHT_FOOT_IN_CONTACT); \nend " + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 85 + ssIdNumber 9 + name "tau" + 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 [83 0 86] + } + data { + id 86 + ssIdNumber 10 + name "JL" + 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 [83 85 87] + } + data { + id 87 + ssIdNumber 11 + name "JR" + 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 [83 86 88] + } + data { + id 88 + ssIdNumber 6 + 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 [83 87 89] + } + data { + id 89 + ssIdNumber 19 + 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 [83 88 90] + } + data { + id 90 + ssIdNumber 4 + name "f" + 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 [83 89 91] + } + data { + id 91 + ssIdNumber 21 + name "LEFT_RIGHT_FOOT_IN_CONTACT" + 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 [83 90 92] + } + data { + id 92 + ssIdNumber 17 + name "nuDot" + 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 [83 91 0] + } + junction { + id 93 + position [23.5747 49.5747 7] + chart 83 + subviewer 83 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [83 0 0] + } + transition { + id 94 + 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 93 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 83 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 83 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [83 0 0] + } + instance { + id 95 + machine 1 + name "MBC control + forward dynamics/Estimate Contact Forces and Forward Dynamics/Get State Accelerations Fro" + "m Dynamics" + chart 83 + } + chart { + id 96 + machine 1 + name "MBC control + forward dynamics/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 97 0 0] + viewObj 96 + ssIdHighWaterMark 8 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 11 + disableImplicitCasting 1 + eml { + name "saturateInputDerivativeFCN" + } + firstData 98 + firstTransition 104 + firstJunction 103 + } + state { + id 97 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 96 + treeNode [96 0 0 0] + superState SUBCHART + subviewer 96 + 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 98 + 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 [96 0 99] + } + data { + id 99 + 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 [96 98 100] + } + data { + id 100 + 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 [96 99 101] + } + data { + id 101 + 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 [96 100 102] + } + data { + id 102 + 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 [96 101 0] + } + junction { + id 103 + position [23.5747 49.5747 7] + chart 96 + subviewer 96 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [96 0 0] + } + transition { + id 104 + 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 103 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 96 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 96 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [96 0 0] + } + instance { + id 105 + machine 1 + name "MBC control + forward dynamics/Joint Torque Saturation/Saturate Torque Derivative" + chart 96 + } + chart { + id 106 + machine 1 + name "MBC control + forward dynamics/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 107 0 0] + viewObj 106 + ssIdHighWaterMark 86 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 13 + disableImplicitCasting 1 + eml { + name "stateMachineMomentumControlFCN" + } + firstData 108 + firstTransition 132 + firstJunction 131 + } + state { + id 107 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 106 + treeNode [106 0 0 0] + superState SUBCHART + subviewer 106 + 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 108 + 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 [106 0 109] + } + data { + id 109 + 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 [106 108 110] + } + data { + id 110 + 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 [106 109 111] + } + data { + id 111 + 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 [106 110 112] + } + data { + id 112 + 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 [106 111 113] + } + data { + id 113 + 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 [106 112 114] + } + data { + id 114 + 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 [106 113 115] + } + data { + id 115 + 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 [106 114 116] + } + data { + id 116 + 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 [106 115 117] + } + data { + id 117 + 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 [106 116 118] + } + data { + id 118 + 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 [106 117 119] + } + data { + id 119 + 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 [106 118 120] + } + data { + id 120 + 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 [106 119 121] + } + data { + id 121 + 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 [106 120 122] + } + data { + id 122 + 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 [106 121 123] + } + data { + id 123 + 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 [106 122 124] + } + data { + id 124 + 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 [106 123 125] + } + data { + id 125 + 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 [106 124 126] + } + data { + id 126 + 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 [106 125 127] + } + data { + id 127 + 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 [106 126 128] + } + data { + id 128 + 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 [106 127 129] + } + data { + id 129 + 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 [106 128 130] + } + data { + id 130 + 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 [106 129 0] + } + junction { + id 131 + position [23.5747 49.5747 7] + chart 106 + subviewer 106 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [106 0 0] + } + transition { + id 132 + 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 131 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 106 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 106 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [106 0 0] + } + instance { + id 133 + machine 1 + name "MBC control + forward dynamics/Robot State and References/State Machine/STATE MACHINE" + chart 106 + } + chart { + id 134 + machine 1 + name "MBC control + forward dynamics/emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/MA" + "TLAB 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 135 0 0] + viewObj 134 + ssIdHighWaterMark 9 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 14 + disableImplicitCasting 1 + eml { + name "checkInputSpikesFCN" + } + supportVariableSizing 0 + firstData 136 + firstTransition 141 + firstJunction 140 + } + state { + id 135 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 134 + treeNode [134 0 0 0] + superState SUBCHART + subviewer 134 + 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 136 + 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 [134 0 137] + } + data { + id 137 + 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 [134 136 138] + } + data { + id 138 + 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 [134 137 139] + } + data { + id 139 + 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 [134 138 0] + } + junction { + id 140 + position [23.5747 49.5747 7] + chart 134 + subviewer 134 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [134 0 0] + } + transition { + id 141 + 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 140 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 134 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 134 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [134 0 0] + } + instance { + id 142 + machine 1 + name "MBC control + forward dynamics/emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/MA" + "TLAB Function" + chart 134 + } + chart { + id 143 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute Desired Torques/QPSolver/One Foot Two Fe" + "et QP Selector" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 144 0 0] + viewObj 143 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 15 + disableImplicitCasting 1 + eml { + name "robotIsOnSingleSupportQP_FCN" + } + firstData 145 + firstTransition 148 + firstJunction 147 + } + state { + id 144 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 143 + treeNode [143 0 0 0] + superState SUBCHART + subviewer 143 + 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 145 + 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 [143 0 146] + } + data { + id 146 + 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 [143 145 0] + } + junction { + id 147 + position [23.5747 49.5747 7] + chart 143 + subviewer 143 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [143 0 0] + } + transition { + id 148 + 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 147 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 143 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 143 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [143 0 0] + } + instance { + id 149 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute Desired Torques/QPSolver/One Foot Two Fe" + "et QP Selector" + chart 143 + } + chart { + id 150 + machine 1 + name "Init. Cond./MATLAB Function" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 151 0 0] + viewObj 150 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 16 + disableImplicitCasting 1 + eml { + name "getSystemConfiguration" + } + firstData 152 + firstTransition 156 + firstJunction 155 + } + state { + id 151 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 150 + treeNode [150 0 0 0] + superState SUBCHART + subviewer 150 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function q = getSystemConfiguration(w_H_b,jointPos)\n\n q = [w_H_b(:);jointPos];\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 152 + ssIdNumber 4 + name "w_H_b" + 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 [150 0 153] + } + data { + id 153 + ssIdNumber 5 + name "q" + 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 [150 152 154] + } + data { + id 154 + ssIdNumber 6 + 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 [150 153 0] + } + junction { + id 155 + position [23.5747 49.5747 7] + chart 150 + subviewer 150 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [150 0 0] + } + transition { + id 156 + 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 155 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 150 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 150 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [150 0 0] + } + instance { + id 157 + machine 1 + name "Init. Cond./MATLAB Function" + chart 150 + } + chart { + id 158 + machine 1 + name "MBC control + forward dynamics/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 159 0 0] + viewObj 158 + ssIdHighWaterMark 82 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 17 + disableImplicitCasting 1 + eml { + name "momentumBasedControllerFCN" + } + firstData 160 + firstTransition 198 + firstJunction 197 + } + state { + id 159 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 158 + treeNode [158 0 0 0] + superState SUBCHART + subviewer 158 + 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 160 + 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 [158 0 161] + } + data { + id 161 + 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 [158 160 162] + } + data { + id 162 + 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 [158 161 163] + } + data { + id 163 + 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 [158 162 164] + } + data { + id 164 + 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 [158 163 165] + } + data { + id 165 + 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 [158 164 166] + } + data { + id 166 + 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 [158 165 167] + } + data { + id 167 + 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 [158 166 168] + } + data { + id 168 + 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 [158 167 169] + } + data { + id 169 + 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 [158 168 170] + } + data { + id 170 + 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 [158 169 171] + } + data { + id 171 + 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 [158 170 172] + } + data { + id 172 + 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 [158 171 173] + } + data { + id 173 + 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 [158 172 174] + } + data { + id 174 + 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 [158 173 175] + } + data { + id 175 + 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 [158 174 176] + } + data { + id 176 + 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 [158 175 177] + } + data { + id 177 + 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 [158 176 178] + } + data { + id 178 + 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 [158 177 179] + } + data { + id 179 + 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 [158 178 180] + } + data { + id 180 + 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 [158 179 181] + } + data { + id 181 + 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 [158 180 182] + } + data { + id 182 + 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 [158 181 183] + } + data { + id 183 + 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 [158 182 184] + } + data { + id 184 + 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 [158 183 185] + } + data { + id 185 + 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 [158 184 186] + } + data { + id 186 + 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 [158 185 187] + } + data { + id 187 + 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 [158 186 188] + } + data { + id 188 + 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 [158 187 189] + } + data { + id 189 + 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 [158 188 190] + } + data { + id 190 + 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 [158 189 191] + } + data { + id 191 + 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 [158 190 192] + } + data { + id 192 + 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 [158 191 193] + } + data { + id 193 + 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 [158 192 194] + } + data { + id 194 + 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 [158 193 195] + } + data { + id 195 + 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 [158 194 196] + } + data { + id 196 + 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 [158 195 0] + } + junction { + id 197 + position [23.5747 49.5747 7] + chart 158 + subviewer 158 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [158 0 0] + } + transition { + id 198 + 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 197 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 158 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 158 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [158 0 0] + } + instance { + id 199 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Momentum Based Balancing Controller\n" + chart 158 + } + chart { + id 200 + machine 1 + name "MBC control + forward dynamics/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Functi" + "on" + windowPosition [369.958 -65.92 200 534.4] + viewLimits [0 156.75 0 153.75] + screen [1 1 1366 768 1.25] + treeNode [0 201 0 0] + viewObj 200 + ssIdHighWaterMark 9 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 18 + disableImplicitCasting 1 + eml { + name "checkInputRangeFCN" + } + supportVariableSizing 0 + firstData 202 + firstTransition 209 + firstJunction 208 + } + 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 [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 202 + 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 [200 0 203] + } + data { + id 203 + 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 [200 202 204] + } + data { + id 204 + 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 [200 203 205] + } + data { + id 205 + 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 [200 204 206] + } + data { + id 206 + 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 [200 205 207] + } + data { + id 207 + 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 [200 206 0] + } + junction { + id 208 + position [23.5747 49.5747 7] + chart 200 + subviewer 200 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [200 0 0] + } + transition { + id 209 + 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 208 + 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 210 + machine 1 + name "MBC control + forward dynamics/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Functi" + "on" + chart 200 + } + chart { + id 211 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute angular momentum integral error/Select b" + "ase 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 212 0 0] + viewObj 211 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 19 + disableImplicitCasting 1 + eml { + name "footOnGround" + } + firstData 213 + firstTransition 216 + firstJunction 215 + } + state { + id 212 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 211 + treeNode [211 0 0 0] + superState SUBCHART + subviewer 211 + 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 213 + 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 [211 0 214] + } + data { + id 214 + 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 [211 213 0] + } + junction { + id 215 + position [23.5747 49.5747 7] + chart 211 + subviewer 211 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [211 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 211 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 211 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [211 0 0] + } + instance { + id 217 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute angular momentum integral error/Select b" + "ase to world transform" + chart 211 + } + chart { + id 218 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute angular momentum integral error/Get Equi" + "valent 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 219 0 0] + viewObj 218 + ssIdHighWaterMark 10 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 20 + disableImplicitCasting 1 + eml { + name "getEquivalentBaseVel_FCN" + } + firstData 220 + firstTransition 227 + firstJunction 226 + } + 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 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 220 + 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 [218 0 221] + } + data { + id 221 + 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 [218 220 222] + } + data { + id 222 + 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 [218 221 223] + } + data { + id 223 + 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 [218 222 224] + } + data { + id 224 + 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 [218 223 225] + } + data { + id 225 + 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 [218 224 0] + } + junction { + id 226 + position [23.5747 49.5747 7] + chart 218 + subviewer 218 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [218 0 0] + } + transition { + id 227 + 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 226 + 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 228 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute angular momentum integral error/Get Equi" + "valent Base Velocity" + chart 218 + } + chart { + id 229 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Ana" + "lytical 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 230 0 0] + viewObj 229 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 24 + disableImplicitCasting 1 + eml { + name "analyticalSolutionQP_FCN" + } + firstData 231 + firstTransition 235 + firstJunction 234 + } + state { + id 230 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 229 + treeNode [229 0 0 0] + superState SUBCHART + subviewer 229 + 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 231 + 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 [229 0 232] + } + data { + id 232 + 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 [229 231 233] + } + data { + id 233 + 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 [229 232 0] + } + junction { + id 234 + position [23.5747 49.5747 7] + chart 229 + subviewer 229 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [229 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 229 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 229 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [229 0 0] + } + instance { + id 236 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Ana" + "lytical Solution Two Feet (unconstrained)" + chart 229 + } + chart { + id 237 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Pro" + "cess QP output" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 238 0 0] + viewObj 237 + ssIdHighWaterMark 14 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 25 + disableImplicitCasting 1 + eml { + name "processOutputQP_twoFeetFCN" + } + firstData 239 + firstTransition 245 + firstJunction 244 + } + state { + id 238 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 237 + treeNode [237 0 0 0] + superState SUBCHART + subviewer 237 + 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 239 + 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 [237 0 240] + } + data { + id 240 + 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 [237 239 241] + } + data { + id 241 + 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 [237 240 242] + } + data { + id 242 + 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 [237 241 243] + } + data { + id 243 + 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 [237 242 0] + } + junction { + id 244 + position [23.5747 49.5747 7] + chart 237 + subviewer 237 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [237 0 0] + } + transition { + id 245 + 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 244 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 237 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 237 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [237 0 0] + } + instance { + id 246 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Pro" + "cess QP output" + chart 237 + } + chart { + id 247 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Ana" + "lytical 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 248 0 0] + viewObj 247 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 26 + disableImplicitCasting 1 + eml { + name "analyticalSolutionQP_FCN" + } + firstData 249 + firstTransition 253 + firstJunction 252 + } + state { + id 248 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 247 + treeNode [247 0 0 0] + superState SUBCHART + subviewer 247 + 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 249 + 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 [247 0 250] + } + data { + id 250 + 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 [247 249 251] + } + data { + id 251 + 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 [247 250 0] + } + junction { + id 252 + position [23.5747 49.5747 7] + chart 247 + subviewer 247 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [247 0 0] + } + transition { + id 253 + 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 252 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 247 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 247 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [247 0 0] + } + instance { + id 254 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Ana" + "lytical Solution QP One Foot (unconstrained)" + chart 247 + } + chart { + id 255 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Pro" + "cess QP output" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 256 0 0] + viewObj 255 + ssIdHighWaterMark 14 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 27 + disableImplicitCasting 1 + eml { + name "processOutputQP_oneFoot" + } + firstData 257 + firstTransition 264 + firstJunction 263 + } + state { + id 256 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 255 + treeNode [255 0 0 0] + superState SUBCHART + subviewer 255 + 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 257 + 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 [255 0 258] + } + data { + id 258 + 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 [255 257 259] + } + data { + id 259 + 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 [255 258 260] + } + data { + id 260 + 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 [255 259 261] + } + data { + id 261 + 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 [255 260 262] + } + data { + id 262 + 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 [255 261 0] + } + junction { + id 263 + position [23.5747 49.5747 7] + chart 255 + subviewer 255 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [255 0 0] + } + transition { + id 264 + 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 263 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 255 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 255 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [255 0 0] + } + instance { + id 265 + machine 1 + name "MBC control + forward dynamics/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Pro" + "cess QP output" + chart 255 + } + chart { + id 266 + machine 1 + name "InverseKinematics Integration/Rewrite the Base Pose as Pos+Quat" + windowPosition [357.12 483.496 167 391] + viewLimits [0 156.75 0 153.75] + screen [1 1 1280 1024 1.041666666666667] + treeNode [0 267 0 0] + viewObj 266 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 30 + disableImplicitCasting 1 + eml { + name "fromTransfMatrixToQuaternions" + } + firstData 268 + firstTransition 271 + firstJunction 270 + } + state { + id 267 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 266 + treeNode [266 0 0 0] + superState SUBCHART + subviewer 266 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function q_0_quat = fromTransfMatrixToQuaternions(q_0)\n\n % Reshape the base pose as a matrix" + "\n w_H_base = [q_0(1:4) q_0(5:8) q_0(9:12) q_0(13:16)];\n\n % Get the base pos + quaternion\n qt_base " + "= wbc.fromTransfMatrixToPosQuat(w_H_base);\n\n % Final state converted as a 7+n element vector\n q_0_quat " + "= [qt_base; q_0(17:end)];\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 268 + ssIdNumber 4 + name "q_0" + 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 [266 0 269] + } + data { + id 269 + ssIdNumber 5 + name "q_0_quat" + 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 [266 268 0] + } + junction { + id 270 + position [23.5747 49.5747 7] + chart 266 + subviewer 266 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [266 0 0] + } + transition { + id 271 + labelString "{eML_blk_kernel();}" + labelPosition [32.125 19.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 270 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 266 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 266 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [266 0 0] + } + instance { + id 272 + machine 1 + name "InverseKinematics Integration/Rewrite the Base Pose as Pos+Quat" + chart 266 + } + chart { + id 273 + machine 1 + name "InverseKinematics Integration/Rewirte the Base Pose as Transf Matrix" + windowPosition [357.12 483.496 167 391] + viewLimits [0 156.75 0 153.75] + screen [1 1 1280 1024 1.041666666666667] + treeNode [0 274 0 0] + viewObj 273 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 31 + disableImplicitCasting 1 + eml { + name "fromQuaternionsToTransfMatr" + } + firstData 275 + firstTransition 278 + firstJunction 277 + } + state { + id 274 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 273 + treeNode [273 0 0 0] + superState SUBCHART + subviewer 273 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function q = fromQuaternionsToTransfMatr(q_quat)\n\n % define the base pose in quaternions\n " + " q_base = q_quat(1:7);\n\n % define the transformation matrix\n w_H_b = wbc.fromPosQ" + "uatToTransfMatr(q_base);\n\n % column-major serialization\n w_H_b_vectorized = w_H_b(:);\n\n % new stat" + "e vector\n q = [w_H_b_vectorized; q_quat(8:end)];\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 275 + ssIdNumber 4 + name "q_quat" + 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 [273 0 276] + } + data { + id 276 + ssIdNumber 5 + name "q" + 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 [273 275 0] + } + junction { + id 277 + position [23.5747 49.5747 7] + chart 273 + subviewer 273 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [273 0 0] + } + transition { + id 278 + labelString "{eML_blk_kernel();}" + labelPosition [32.125 19.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 277 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 273 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 273 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [273 0 0] + } + instance { + id 279 + machine 1 + name "InverseKinematics Integration/Rewirte the Base Pose as Transf Matrix" + chart 273 + } + chart { + id 280 + machine 1 + name "MBC control + forward dynamics/Estimate Contact Forces and Forward Dynamics/Get Contact Forces From Dyn" + "amics" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 281 0 0] + viewObj 280 + ssIdHighWaterMark 22 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 34 + disableImplicitCasting 1 + eml { + name "estimateContactForcesFromDynamicsFCN" + } + firstData 282 + firstTransition 292 + firstJunction 291 + } + state { + id 281 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 280 + treeNode [280 0 0 0] + superState SUBCHART + subviewer 280 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function f_dynamics = estimateContactForcesFromDynamicsFCN(tau, JL, JR, JDotL_nu, JDotR_nu, M, h," + " LEFT_RIGHT_FOOT_IN_CONTACT)\n \n f_dynamics = estimateContactForcesFromDynamics(tau, JL, JR, JDotL_nu, JDotR" + "_nu, M, h, LEFT_RIGHT_FOOT_IN_CONTACT);\nend " + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 282 + ssIdNumber 9 + name "tau" + 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 [280 0 283] + } + data { + id 283 + ssIdNumber 10 + name "JL" + 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 [280 282 284] + } + data { + id 284 + ssIdNumber 11 + name "JR" + 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 [280 283 285] + } + data { + id 285 + ssIdNumber 4 + name "JDotL_nu" + 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 [280 284 286] + } + data { + id 286 + ssIdNumber 13 + name "JDotR_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 [280 285 287] + } + data { + id 287 + ssIdNumber 6 + 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 [280 286 288] + } + data { + id 288 + ssIdNumber 19 + 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 [280 287 289] + } + data { + id 289 + ssIdNumber 21 + name "LEFT_RIGHT_FOOT_IN_CONTACT" + 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 [280 288 290] + } + data { + id 290 + ssIdNumber 17 + name "f_dynamics" + 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 [280 289 0] + } + junction { + id 291 + position [23.5747 49.5747 7] + chart 280 + subviewer 280 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [280 0 0] + } + transition { + id 292 + 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 291 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 280 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 280 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [280 0 0] + } + instance { + id 293 + machine 1 + name "MBC control + forward dynamics/Estimate Contact Forces and Forward Dynamics/Get Contact Forces From Dyn" + "amics" + chart 280 + } + target { + id 294 + machine 1 + name "sfun" + codeFlags "" + linkNode [1 0 295] + } + target { + id 295 + machine 1 + name "rtw" + linkNode [1 294 0] + } +} diff --git a/doc/How-to-run-controllers-on-real-iCub.md b/doc/How-to-run-controllers-on-real-iCub.md new file mode 100644 index 0000000..a5ee5f3 --- /dev/null +++ b/doc/How-to-run-controllers-on-real-iCub.md @@ -0,0 +1,19 @@ +## HOW TO RUN BALANCING WITH SIMULINK CONTROLLERS ON ICUB + +#### Preliminary procedures: + +- Set the environmental variable `YARP_ROBOT_NAME` in the `.bashrc` according to the robot one wants to use (e.g. `iCubGenova04`, etc. for experiments). + +- Start the robot. Please refer to [How to setup iCub for whole-body control experiments](How-to-setup-iCub-for-wbc-experiments.md) for more information on the startup procedure. + +#### Before putting the robot feet on the ground: + +- Bring the robot in a suitable home position. Run on a terminal the command `yarpmotorgui --from homePoseBalancing.ini` and then select a custom position by clicking on the window `Global Joints Commands/Custom postions`. + +- Type on a terminal the command `yarp rpc /wholeBodyDynamics/rpc` and then execute the command `calib all 300`. It will remove offsets from FT sensors measurements. + +- Then, put the robot on the ground. + +#### After putting the robot on the ground: + +- Open the simulink model and run the controller. diff --git a/doc/How-to-run-torqueBalancing-simulations-with-iCub.md b/doc/How-to-run-controllers-with-gazebo-simulator.md similarity index 56% rename from doc/How-to-run-torqueBalancing-simulations-with-iCub.md rename to doc/How-to-run-controllers-with-gazebo-simulator.md index 3a7ca63..f34dbc7 100644 --- a/doc/How-to-run-torqueBalancing-simulations-with-iCub.md +++ b/doc/How-to-run-controllers-with-gazebo-simulator.md @@ -1,15 +1,14 @@ -## HOW TO RUN A SIMULATION WITH TORQUE CONTROL ON ICUB +## HOW TO RUN SIMULINK CONTROLLERS IN SIMULATION WITH GAZEBO -The procedure to run the torque balancing module is still quite elaborate. Users willing to use the module should follow this list. +The procedure to run the simulink controllers in simulation is still quite elaborate. Users willing to use the module should follow this list. - Set the environmental variable YARP_ROBOT_NAME in the `.bashrc` according to the robot one wants to use (e.g. `iCubGazeboV2_5` or `icubGazeboSim` for simulations). - Verify that Gazebo and the robot model for simulations are available and installed. You can check if the controller is targeting the correct robot model by typing on a terminal: -``` -yarp resource --find model.urdf - -``` + ``` + yarp resource --find model.urdf + ``` then, check that the path and the model name are correct. @@ -17,10 +16,10 @@ yarp resource --find model.urdf - Launch gazebo. It is in general required to use the synchronization between the controller and the simulator to avoid real-time factor related problems. Therefore launch gazebo as follows: `gazebo -slibgazebo_yarp_clock.so`. -- Bring the robot in a suitable home position (e.g. use the `$ yarpmotorgui --from homePoseBalancing.ini` and then select a custom position by clicking on `Global Joints Commands/Custom postions`). +- Bring the robot in a suitable home position (e.g. use the command `yarpmotorgui --from homePoseBalancing.ini` and then select a custom position by clicking on `Global Joints Commands/Custom postions`). - For `icubGazeboSim` robot, launch `wholeBodyDynamics` as follows: `YARP_ROBOT_NAME=icubGazeboSim yarprobotinterface --config launch-wholebodydynamics.xml`. Same holds for `iCubGazeboV2_5` (just change the robot name). For further details see [here](https://github.com/robotology/codyco-modules/blob/master/doc/force_control_on_icub.md#run-wholebodydynamics-on-an-external-pc). -- (OPTIONAL) type on a terminal `yarp rpc /wholeBodyDynamics/rpc` and execute the command `resetOffset all 300`. It will reset offsets of the fake FT measurements, that might be affected by the results of a previous simulation. Fake FT measurements are used e.g. for defining the threshold for switching from single to double support balancing. +- Type on a terminal `yarp rpc /wholeBodyDynamics/rpc` and execute the command `resetOffset all 300`. It will reset offsets of the fake FT measurements, that might be affected by the results of a previous simulation. Fake FT measurements are used e.g. for defining the threshold for switching from single to double support balancing. -- Open the simulink model and run the module. \ No newline at end of file +- Open the simulink model and run the controller. diff --git a/doc/How-to-run-torqueBalancing-on-real-iCub.md b/doc/How-to-run-torqueBalancing-on-real-iCub.md deleted file mode 100644 index ea96fcd..0000000 --- a/doc/How-to-run-torqueBalancing-on-real-iCub.md +++ /dev/null @@ -1,19 +0,0 @@ -## HOW TO RUN BALANCING WITH TORQUE CONTROL ON ICUB - -#### Preliminary procedures: - -- Set the environmental variable `YARP_ROBOT_NAME` in the `.bashrc` according to the robot one wants to use (e.g. `iCubGenova04`, etc. for experiments). - -- Start the robot. Please refer to [How to setup the iCub robot for whole-body torque control experiments](How-to-setup-the-robot-for-wbc-experiments.md) for more information on the startup procedure. - -#### Before putting the robot feet on the ground: - -- Bring the robot in a suitable home position (e.g. open the `$ yarpmotorgui --from homePoseBalancing.ini` and then select a custom position by clicking on `Global Joints Commands/Custom postions`). - -- Type on a terminal `yarp rpc /wholeBodyDynamics/rpc` and execute the command `calib all 300`. It will remove offsets from FT sensors measurements. - -- Then, put the robot on the ground. - -#### After putting the robot on the ground: - -- Open the simulink model and run the module. \ No newline at end of file diff --git a/doc/How-to-setup-the-robot-for-wbc-experiments.md b/doc/How-to-setup-iCub-for-wbc-experiments.md similarity index 97% rename from doc/How-to-setup-the-robot-for-wbc-experiments.md rename to doc/How-to-setup-iCub-for-wbc-experiments.md index 3e8ab64..ee1b86d 100644 --- a/doc/How-to-setup-the-robot-for-wbc-experiments.md +++ b/doc/How-to-setup-iCub-for-wbc-experiments.md @@ -59,10 +59,9 @@ On iCub it is sometimes required to re-calibrate the “zero position” associa - This value will be added in the specific file associated to the joint. On the `on-board PC`, go to the folder containing the "\.xml" calibration files for your robot and open the corresponding file (example: `.../robots/$ROBOT_NAME/calibrators/left\_leg\_calib.xml`). Add the encoder value that you have previously noted to the current `calibrationDelta` of the joint (each number of the parameter `calibrationDelta` corresponds to a joint). For example, we add some values to the line: -``` - -5.0 8.7 -11.4 -0.6 0.0 - -``` + ``` + -5.0 8.7 -11.4 -0.6 0.0 + ``` Restart the robot to apply the modifications. Check that the joints were successfully calibrated. #### IMU calibration @@ -85,4 +84,4 @@ Once everything is correctly updated and calibrated: - Be careful with the head and hands of the robot. - - How to hide DEBUG information: in the cmake information, put the variable DEBUG from DEBUG to RELEASE. \ No newline at end of file + - How to hide DEBUG information: in the cmake information, put the variable DEBUG from DEBUG to RELEASE. diff --git a/doc/README.md b/doc/README.md index db304bf..148529e 100644 --- a/doc/README.md +++ b/doc/README.md @@ -4,8 +4,8 @@ Guidelines on how to create/use Simulink models for control. ### Available documentation -- [How-to-run-torqueBalancing-simulations-with-iCub](How-to-run-torqueBalancing-simulations-with-iCub.md) +- [How-to-run-controllers-with-gazebo-simulator](How-to-run-controllers-with-gazebo-simulator.md) -- [How-to-run-torqueBalancing-on-real-iCub](How-to-run-torqueBalancing-on-real-iCub.md) +- [How-to-run-controllers-on-real-iCub](How-to-run-controllers-on-real-iCub.md) -- [How-to-setup-the-robot-for-wbc-experiments](How-to-setup-the-robot-for-wbc-experiments.md) \ No newline at end of file +- [How-to-setup-iCub-for-wbc-experiments](How-to-setup-iCub-for-wbc-experiments.md) diff --git a/doc/pics/jointControl.png b/doc/pics/jointControl.png new file mode 100644 index 0000000000000000000000000000000000000000..5169a2fd5ab73c2f762b0ec401f4c1d716cf929a GIT binary patch literal 87086 zcmeFZhg;9>|3CcJAS9uZN+m^0At`M|G?0eUq+}HBt)w)NG&HC*kai(yPbvzfG&B@S zduY-0dz`Mz_5B_9KXBj2aeq5}J{OJm`}I0s&-3|wtg{;$M-Q!8#kPtz zSwTagP>s+n$7i;2*2m&MH1>)r+I0AEr#o{U|K8+qP|rcj`mBTVX*+YuIV}j8T`$Q7GILl>_^=U1EkiU0vSxE&cw{qQ3iHdW4Ujnt0ftvRcs6`5Yh4 zRHuZ*2+pH9z1q5JI*wX5wNf*(%u<>;X>xK@>u%)aKxdoj1IqqFJFL>nM~k?!OziOev45e0ca`{)tr+Gcyq{zi>0r zP~z$T+oCc)LX2-^#p+F;zOJrLdZb28q3~V*yS2v|GsS5VWA1se1RVVI=@XwJGj96A z-@Sg--{u|T)WpQ@=5=r1zsF1S4>V9H3M+a3ZhsjcOTYL^(~u^M@VGdW*lFy2g->Th z2L5)DJbz2SQ*LeryT!!Jvp@Z54PM;;?g$E{`_2D0i@%HU|G#cKNy;Q%@>2VYBlh3l zhBOX@@$S2S)U0+*>u?$V#tvYABvb$__cDVw^_ z{(G3iChP`_rc|BA>+80jd8*elGO|NdRMhqIu?(B`-%Tt-`SV>GRryrq9b(iOC(<|=4yi;E)Ip(i{9iw5c{aYu zLg&N@7O{WE>^S3}y{fROZxxM)yZf0O{ocO5?S-93{~5ej{dfM?KW zsU0|fKDpxM%ce{tY1xU_TPJ@0jLgl=B@b|iv&R3xAf_bKH8l5+CN2Fe{e9ur_!n2j zGQTzKsmC8Gr|RdVrs||U#{X(sT5=`T1X%EF``+JgyeD>!Y2(JE4cm{NM^WkL*(9)t z7$)2G)?BEM-_N$^d~i*qsAxbX5B^wNTW{s%W&IB`DcB0WP#Euv^K3@RKDX+WQqc@x z+3_*QQrB^~_4w)2rz;(E`9#FUb-J$1XPn5;PwcIYw4CU9qyOZTmmmG62eGo3YwGHb zok-J>lao`H>|eF~e}=2G{pD4?Hj+~x4soU6`t@_I?njARKJM>7f9cb+gQCS#p@;mv zLYWvD!n@=#4Zvfu)WRHOU-l&mH>W&*jwD*|A4aQE`v! z#F@ppp{&7<{OpHJFhr&}k18$ceUk~P7XjDc+_^|l1 zE6b?RDaEoSGd*Z~iqlBj&6_tH3dc&)8k3F<-8&ciVQzXTJSHYREQ~2JF;QUaR;80C zPd;lO@%bO5YQvsO8^A=9+LJ^WFFM zSbXFkY0D8Znb#=ne6Rk`7$+B(7id6L9Cbg{ZNRM?U@2!V!Ypbi@($v(P@Wu9j_(+ZR@2y>3kvFPfv$QZ9wdX=8DkI)^^~M_Xn9Yu^ z&lxsL_2W);4Gm*HyL?-7U*6L=B_&09NF(~4GXMV+Xl`Dn!cKq`(UM;eyuH1rnsiN4 z+-E07Q)|BkvhGrpb(^a9qF(XbVL+3Qk52&4ZPTVr|2#LA45j0bg^dcd>*RkInVOlY z6)^q1E}0rfTJLYwK`pbBed6xFY?{(^>x{(nk6gT%j-^c^mxlM*F<>{b^PYOtZu6T9 z3k%O(ewbEPSF>j*-Q&Cdj9*Ml3r|B=Pmi^n`|rzIIlS}gzOiBZ9>+m#ofOT3Fa6fo zpF3i;%lflSr(=uJ9BM7cX+-~0XF<**wVEWO&JCf(0ujm?wh^pg*L zd}z#e?5Elv=D0Wd$nT$?zmp0ahd$SzvLTzh#6ADYY;UCIKI8oy4E_ZU0}sZ>ZQkA8 z_3&^D1Bavrri0tJK#9!RrL~)-9$~WzNlDE`RsX#trZ~%0G@BXxA3S)_Gc=TTWpOUQ z@Uf-SM2`wt4^*|Lp5EOTm)akr=R`Q1jMOP{cby*XG`M%~-W+CNcxdQ@=g*(JXVZ8+ z8&qC#je0qKJ=q3m4B<6D9>s{RzqMJq`PZk5d(%|I1>2rqxKkC#?%b5&x0X{T8TW13 z`1nY{`tCE>t zv6fR%?ijg>*B+rgVdK^l6qawoUkyhWqdF9t>on(#u^FPOQftz>l-W?VavcFv;p2F9p`uuro_PL;odksn!1KGqI zx|U`nR6J=tBF(Zt?eSc4SJ{`%iV=Ew_1_&B#@50_cV%(9CG-*6NPf=Up{Dr#)Uwkb zwfKybLrw~_ke-81C*{x|45%9T?rioOtK1EARFV9dr^dT}FXb+0;7HCYnM*DEMp|;L z>-SCK7a9)ac3mlq+IuP0bKx5o*1b_DsLw%4{Cqw48WaU)gchK$+`_n0&c#of(?cyS z9zQ#iHX6I^Fa5n#XnO>=MP3%2MK$!w{D=hkA?g(jAIq<;;9C?|vzZ=jx)~HyI{eM> zsi`lT_4Ye&9-WKx*jx7$?V0TQ9s18T7U!m$@@&myXM67GKQ*mnj+Om)eHAWf_YjaW zRG*;qOg|$P)nfZmds|bA)*;QKh4Q+L{cY&zS#C#m#;3p8OwYh6qqqH7Y*FTq<_v=* zQZ-|ri)TmsQj%gd+h1Iop6!bp!t-54PyZ3SF#*7fUq;4oasFpr?)P*1zX8zf(0ihg zeXg$g!^1;O&&<`Pw-!y@fA|iz zWM!?~u05Y-(XW(U&N_#O|z<7 zA08YGU}9xWMspl`drz#b&^haU%)W=o$^5f(bAw=M^`*ZT$I{s9OrIVO#RlGHVq&sc z)=k^i)>c(r{Uf$wAh%V+BbCrc*qj3U_MOZsT`J%mC||*)ZD;o^^v<28$4A&zLgV7% z=4M8O1h$V04Lfe#wyo;!hyExXeiefk7e9#A$!v{&_gLbyi_*Ee*rw0Lmzg(gc!)Wh zrk`V>U+9!!e0e0t(&ax8z?b;is~bI?PX!n4e0i7oJuUl-EpkJp0mvF@9t85hoT_vj+7~bcWqBn&)ROV1b$?hk65|lq`pZ#M_*PalWYBcB% zN2lU`FC?woR#jPfVW>GH^iibAmHe`&?YY)jU>E~<{`$pkIm_3uX+LWF+sF7GzV?c@ zta=^ixim0iTC=-l^1|Ne=}+h41bBH5l$V#|z4uzJSiU^MCS2jiJ$cWP*ei1vaszc! zH2I1rYJ^hto|?wKxZCnIDeg)rFCN&j{rmUR#%^F?*(NV9pO}<11e{nhsPF`1gLc$; z@ptUKdnbF`&|=ep>2{cu7^1z`ZjdDH?Pg$Lcy6w9tSDEH@wf421M4Orja3|lr(gO> z*?+(FBj)cltCsTQ(yv*$GKBZOoJZS8WtjQ7_c0;7v-9(6p)p63o-XN*wo^p1AcM?x zUt8<2@XHqchUZv&t)(LtS4H#u`SS^Vch$FTbrDjHZa`r(OMT86Y|BYgR8k5DBy^qp z-n;L+#r&rft?dtWo=&xxh1q%6-DDPG$;)r%R6l$;@B4pqK;lHmIXXW-KZ0m~{P51ybhg=Pemu~L^#@p4Rx@I?Q$;>4EIQ0 zU@W5Y`yyRc5$LQAxX+g`bc-*@KU*0QH~Y|Qg_A@3%!BQ_oljg6R|%6JB3vO`s(wW2K9U9hbGYG{@Xj2&v{ zJzsHlBv#t_XkT4ir1r0Zs}h>`r5r!LxMZNxfG)+`o8HLZo4(6sAd-GG?;FFn8H^Sy zjqrcp?<)J3D9y*WH%qIAf+T%uHmyko{T0};;|K`HGdSTW=Fs)Kz50v zyLRo85T zUD=d;Tq)`3{e~x}U-GJ~Tept%3Fpi=XBtYErbSg62%s^mzU|)U4)8bFo|_dVVV!wr z`!VA&rN1<&gZrSdl13;P<;Q&ca~=yns^^BY>j;-{ZgD&U#PeA8@(h#9#jX+$SN*Rl z4XDjz0jpFRI;J6Yhwex#Q%s11N|VBj?UUj728P%|AQrgC4~wJUJM^ zlB$(-8~k^5%yeIGZz3qd;7D#)GlmkM3fA2)!K`$uL5)DhlBGFI*@f|!gP)5F(fuER zR9*U9bTV4PIs(h#vvPwr8n>yd`@fHl^0V-97?hGBlizcZRMgbsrz>d8I)#}!2{3*B ze2=(gv-+}S%O-yOc$2-3?m>Eu>{gqe*CBjt%T*gL<#s+EEB$SZ^}ZPp(D>@chH3Ca z%ekq6nwpvhz?B@wp%dWnnR-L#TQcvv%?t~uw3T=if_KG(Y;1H}ir;^&r?;0+rQql1 z%g_k273-!aCK_~2JQ9C)d75n5vPHkh<*D*5)&zY07;fPNT4auCVnTww=zn9qlY**>8R9i>!b5!>H{M z4}PpGHUtwB(<7BT+km8pZfuXuyVx$(GhHK;bsi6clqGJD7ZVQsj@>IhE{+TD95yr3 z9(l7l9=kbIdpJY>?~CRg&vZR4)goLDl4OB0LfL5fGi(lvjU9bIC)0omn8KbGl9bd# zKh!*RDxQjpYT{z%Chv4EI;PS`k9dzn?NK2bz|k1VEh+`Sm*&;-1ph8v$G0ebKUGg! zZ)hoOGA>UTxa88&<NVJync_H0S%`X3Qpz+XJ*(t^yyZ_!0=a(qz{krWM(1x%C zfn4&&Af@`HSBlvrtkS_0hDHm=oX%xPhy(U*QSv+g{p~$SSGtiRM%oXR*Wl3+@-Jvo zQnX8}zDeX`dwaA~LfINdHnvnkKA}bsW`fr!2^CrS_dav;GgHpxpIf<{mi7V)gKhVj z>+l>B&%TQyz60p8gKmaI1{9Zp1b@y3KpkehT#3g#X-?#=$nCCT7ECtGw*!6NeRf4b z^V%~`+!_m*zNGhI+6Clk+1W(r-ZkyWd=$f@b-G9j|Xwj)uzDun4Xv9LVGG<^NV zCNHr&`VHX=53xSuWz=o?_DMt^A@7Ro0yKKyeeD`E2S+-_ix59Qf8DS`KxM_S!ryzM z80K19Z5sR<#djXcr-Kv`zFuuOmJH#GwBhF#O@h-{{H@Bj2pwxmRNE*!Rj-6$H=11f zao?4Nyc1c*8G!Oc8iTBtkdOeCR~MYBcWf-{>eZ_<8H%e7q=R=d+K4vO{FSQ04z_Hj zD>%DS>i*G_87IsJ3gV2C^C);>-IaMvZuUT&`@{`&MxD|t9y(dZM!6k! z0T#{a0`l^uLpyr?>?Jv*F3+SxkeROL^6WuX7BQffg)LvhY*ITas`=HIe z+Nt&rWNAE!-&tQ>eTaNB{|Pi@G7F*Cz`Cl6kY+8qcUS$fOwfEo^=QdcKP)+{0n-Yl^pq&Vup}dv{g~GAAJAIyz%1wI|P4T+5`? zG1C=rdhYi+HTvml=3*vyusHSKJxtNy=-|W)`FdkhwT(ELMZ~{5EB2zpD;_?Xjo+yY zYiR9Pxa6OsGj7}cvAVjtRXFU;hi??hKUn}4wb)HwG+t$aTKXnxfA5n2Xytmk0wyN< z0>%A>og#&und#}mK~|gG2EMz=(NX>P&Nf?_EV=tb3j-fF4!jlHQ2)h-b$|T!9C}CE zYnk0ajDI&FjJ_{^G~W5W3@b!+^nU5;E0tGN$STUmPvvhq$N3ilcQZ)HFcxu8{bOQ~ zODQYs;3mPr=6TpF5<+x$pw6Qq++5j^W2JLtcB+qOkGY$`kQ5_%SJ z;p_}_7+ljO^zMXw`#!;K+mtbk$Z)hk4L6uG_ctDsZ-Q#k^x)v?;_-^rRZDUnbNtfM z2G%9BkJx4qe|F{u{+TS%<(~bhb{Qq8d)0En>|j5vr-vEqy?(<6W@hHZ z*RKy>>c~6(N5iGEV#7~1rf4Oh+9xmmF{iygeoW?$91j#Sy zp}q0vh&5zZb>!QJM?^e^2Qg49TB;2atOGg{{dGDwo=wfZaG=96yqRrT_t`@`=_m?6 z)Kpa)fTx)@ZAw8SczrfoB^2G3sH6uE9Ow@@#2Jz+aeTRrxS)12xBtt8o}M#kcf^MI zV)J^yp~moIx!lq(vz+rE6yf1HT82QXI8JN~JciBt>|SMg1R4sx}|&I zfKLG|jDAPjrV?NbQg{FrWO3WOOsEx3Iz99*hoDI?r0ddVT1xlXQ0z<8xgZuCwV?xH zm{)+G|1gXj4ZIlen7vVH5Y_8CxS}Xo&7-X6)3@q<^a3e*h;EiwTn25yWU{Z$c|I?c zG+9D=gJfs)Vilf1wm@6E`#x4S4RSt<0Aq}}Wd!dVIOo@vud%!Q4$X7Y)EC$XmjlW!*HMH>wK^M^NtLpsMay5T>>CdMEO;{H&ya!MWV!kDy zYv?0Eoz!G#m4G-OV8QLNY!-%YuL?!5Mqk;tW>AxSXZ&Ucr2wSl=YbLm2AUYf1*9PD{RXk94Emt+LE zuKe1s79sorHV3$qMa-Ck)HJXmTb(b2L$I`?u`($TZv+JeRgvy#f!ZdLz0bOD3B(Mb z<^!G1HE(Zt4+-SGfN^mf;osli7rOJtp|YTKamE1Uat!z*O7`;J)7%uM4Z1s96O>j2 zZkB#gCfaWp1Sw2FSeRt#Y_wF49czKJmIJ1kaWY#Om_PIpfEF<#20vvdwY|7BG_&#$ zW$A0wmb=pQQ`6Hs=GeR?9&VO)V#@0RgrKAIobBCssjFl$wJIPesO66sg9fSbE=s&p z3&WXTt15^q3mO=!yr{tob;xO?(LA-rpd?Qj?L7yJxCm}21@KFB?4`x|+AlVwL08Rr z@SR2>!gJ7#-g}8xrA}t@*4J-XST5_gQFmorpAjPm#^<|eiMsPjo?lNy92lK}GA%HehajFI%n-o| ze{Zw=6+_Y`;C?V~-kgC4?}>|PH0Nr8q)S-O;#A`?Dci3;N8{uOWJj?Kg6-Ei#|3N@ zZAKBD*?o_g-UBceNEJJdcI<<`1uCji8?mD?luvEK5bjJW#NgD-xsR?6+84f-= z#D&jAuAsq<&&+G#SN8}log9XMTzvWG30&P{^?MJlU%xJ7Oijs?Bje`Iwj+dP5VL~M zhyjh=5IPmmD7+FkRd5K>4AD~HDSrg3KX~w<0KB!;EW#Sqw>JaWvUfL#(_}w;c9WAX z78n_1R|@ZGUKbD;2)orI5k{>cxhG6OXsIXrXHh5EImh8Hqokj94LGpP8+C09euTBu zIez?jl-Rl3#KFXU^sxs+(bf*T&9To;$Bdd^E;GXBB0dt$p+2~t&B3?_2YB7m(jwUh^<7XmGlrxO{5joT zw{pP}fmSL6jT);)wRNdZeIUK$7wD&gkyQdb4;K!MzqlsHC{Q5!sHX<$m z0adqd5p#F7(b_6=G>yk$5sdNK_csCq zbs!5nEG@Va8x!haE+F>( zp!qH69FEunI+=zi*UF4vwS8+o-d#QijUo&s1-`*Dw?8~=GNA)jq=jMR<;{_>bI&8S z1g=hAoB$~BtA1!eByuBKy6^y55F$d&Rm(3Oe@^VxFjqK6ABYugwv+c!*5w>}eokM^Sob6Z(wbLlt3Kzy1MmAnl43efb`;mg-ar z7o57wmUUv&Td|9xUy-N`hR>a`)vqG6PrqEgkK?w?{QEfhtozc=g0Lg%mOuzF)!*)z zrlQ#J0e6z`M$69uk$X59i-;cTFpzO4KYxE}75mOpR|ydL{w})j-dp=(!kP{$D-&G| z0STM-JcZL&mS)dAyw-z2;nBgN-usfI58yP8LI)En-$qa5E?24C?OQ@lthe2`; zpm^ypfyrhC@>CBQHN)RXUX56RGF`E^fwqXc11TSax|5D(9-$t9=M)f z2j%NgOt8q$`Ii;Eyu3iwY%K_F3}yCUcvmeJTv0(6HcbfHAt{+x=4J#ir`eywEU2vk z6B`~;a9<2KIvMVG=7q!d`t_6c`TDEx#KvZ#ClUc3!>0iz!LOeT6xFLjbnYWLEkws1 zfw^(GbY}l)xwj$#7N_YuayBg;GP6b5sy6gXRq38S!z*W23QyGTT};N z<6vrzYFl31Oi?$>|(9E;)BrzDdAV=WELNwmu~)A0;%3OE6Qz_!C6cG?&} zm}3xB!^eE9EpjZ-!tuZ&U^q?>B*h_>p`vTF72xqQ$u>EUWunrc=mG1hk?sNAmml%- zfXW9D_ALKkoPBa=*H%RT{Pi$&mb%ub&@AnF5mMfDa$wwBNH9*3CHR`g`!}T(Xv@gC$36F1#Cssod03@P5bncb1VA`oRT;qip~vD#RG!C1jltyGJoOzm$VG z?~g?Rud_&=$hYftfM`pc?H92XMUc^mLBQ$Ozhx1dpI`;RV7nt`%P5(*3%wO6W$%d1 z03odD*N<=FrmwC??`Fjr0Rmdl_vGY^HQ#UK=E4M@9{!|-jaJiW)ayntk`=p9E*LHA zed^9jSmG#v4`?aD+ZnNt{0;;D1mNu5OZ>lPPzdAhePD`$5pG@;d06y}A9j!*pmA?> zGahw12J+9Z$$fX;pexeOgLSJ4gxrG+G-Z9(7%w55ZY8_IGX`m$YZ=(o#|&b|1FN1$)y zhRt&Bd1X%x$Ep!M^UCsscqKY@rE|+qmrPMBl6t9m|Gs4Sa%G2I*6*LV_(44I;=%c> z;}88vzUw|HRDHEm&+By$o4w!fgM8sN-`Lpb2$Z3s2@?UwwsIhcm_6=VEn1|}?U+wp z-cdaSmWx#$KvSbNK9}1*jn_q%&W`kXa*|{cHF@@}Y7Am51c(yR0Z0#cz@YpX1Eo7^ z9Ss#l1af>zv>>a4BnK!ap(Z)*vv&9Q*(+-vrE5j}2zp;KQa`I!t`zv9suuKzZVnE_=~TCVZTWO!|ZRoc2Xua&u4zr@|MR?4FRbpIXj`ghG@^h>!tF^VYwz(MNea&X6JzYxx zhirAZY3~Bl0(?GQ6d4CFP`~woVYER$4fzf|6OZX|q@FapH&&4(F}jFK&B|RyJ9PMZ zFO+r_I9d{CkjrB##E)TfDj?y|i*~ERVKW+v;byX3!h0B&Ln6t9HW`<=jXVkx?L?F{ z1!^clmRg~9S3EB?rH1qH@E{1@!aiZ$G0>QVQ!}u9vg+&W73-vJx~~#*5`Zfe_8)ZbSmg>IJw0ybJEVTExXR{%-Bk!1qa({C+nr@+Wy=agoqw z)CUR985KCZ`T0{6syrm{6Ua0YM;%ayJ#I5Vgz`oa!+M~)Oh~z9dEarLP;WF96ciFX zS2Ec25WK4u?t_Fci$Q*tUSuE(LQH?S)$a*SWBA;+ND$^J5tGPM3~R8djEs{(du zUm~iBxiS>4YrLyKG_TptDIdXFzTW41RgNBgZ$FI9+3eiq(O0sOwuC4&08WIuV z2XxH={B`ifcAf_haGc}U+}u}W8u31!X{Y18y!PLayU4@ypk~Y7y)2M8#5p)P7+G2M zPLI7|UPX&~`~h|6<@@&xnVFe-MJ~HQh3a8H3!9X1phP6EEJ#+}+BDg`6gbpbC^0-T z0wvQVxwF)~mIZ3eS}0{Nu{Jvq(4lABLwEG((PYHJ5=zIy5JFvZ`SN8+DXG+p^0r@T z@S~@#t%DG)*(xkdkKKI$s#y2PNZ`7y%JJZLPjYgYBWShQcAlc(TYVYpf#)Tghdh6V zH$(T@i9G$?=;()tKoa!43;&!NY+B92!on(P6K}KR4fN&vlta>nf^Fk>?HaX=jLc5R zpr@%eQh2v*tLW_9JTo)Hy5~G2?DhIWXX}sYCpW;F4X*8F6+26hps61Ka7qkOub$-Q z>R!%H*F1dq`p$^)@o~j@EEKHg)8^*BdoQ(}g2FQXo$=bIPzHSL72IxCUtU8dOB4$tIC|2;#(JYMyUOWtN*C76UhD!hkq{ znf}Ci?6bs7)~%L|@J-(9Z_v28xxKEbaWi|ZYg78Tuv6&fVcrUecilrn0l3O9Xo@xt z4hPV9#^oyzWZw4ZyDIJ2SB6!qDlpx;2J>$8zpHu3!MQHv1s=jzz~Z}d9#UwueA+^+ zGB~MBMHL(ov8^LVfF)oZ8{19j@ntP78zIsC0+nH0zaAnWOFUMkzaw7^d-n&VTpke- zQYSX{_Se+6DV;gPixVkNFz>&teNt2;ld*JL`=_)sBEJ2fvbT>SM7nX~MtTlOMhwjT zrlvf|_Fn}UX5F;uG|*MW=4i^!D+?|Q=#G+3!>r(dzMxZIu;mWu>$AaK@JIexk$3$U zys=+kU|@Y+WhJf0+*CR83=kvayWyhP*x4y+YOcpCQ;dv^TwGmG1AzKgrP1?lSHF7| zrNu2N$@>2Ndn{xH^hsMl$^OB?uRzD^=3o%9;r1#as*rl3XWvUNZt<}Ug9}f>7-5H1 z(4*M_RsA7#smDmJ!=00b1K9V2%zTS=7ZtmN)hmp~4@g2hN=o`R{^h01%&Z-lFK?9rhv>X^y>b1pbXa*e7W;GCt^K}= zUI2A#Azi!#@%hnRPEDDOyX@RwzYM}KO1+!idQUn(&hwNbMn&n#+K*(Qr{2kvUyx63 zExs)A;Ninv(E8w9CbfF@F%_-bgUtp=>gVfAs~pHi3$}gW;K45_kn2G~)#y3vHf<76 zXM-I^8uq6;STMt`Gg<`Z{fwK=F}&=~dy}w*eu%;x}0RA5e(DpfXw!v>UG~ z!LDn4X1*G-;8mQ!SS{8FfJKW}xWy^UhENCj8fM~7INGP~A?cH6} z09Vye(kVTq)}Nl7rY3sFmj(P(uW?loKnQ8kMDhcFQ%WwO^mG|Eqrc$?AJ%uqm_B7S z$hE43T>k`tz2T1h4Tusj%>at8M_2WWjy6cJ&(zOZrS(|7JYt6d+4YEket9E_30r{KZbAc7 zFfrjm^L6EX;cD!*@ObRH^yAw`uk>BxABRI3Z$vEOIBcYeEj`a z;MB@f=z-AoH&UM1d{KOqn7GnY)N^UJ@2-@i#Ys_}o!DeZ>Qd(B<|G~Z8Oaf*XTMPo zKmyXR)S}=uyB4}=M?p+i)sX|H-C0z{?=d}Tc=-}Ywp|+fq3*4y)VyoavRQNoW5{j<%$(6z63r%fk3~z z398Z$AyfMl#00;jr0rH+C4mS<=;`SJP%N+DVh|7z&@aB--QCUevnBaBHSi^QaN62T zyr&~Gc}StAsJL~#i%ZaL7Jt8cDD;>jL;Uh?AnDYW;y>cIizj!Q(N0tL_%H0yO(jw zU%3U1`ek|fesiOhSd3q@vnz*|Gkt_H86#;+bu3P<5``VF!4J`w8Zc#}RAg3`D3)8? zxG*|8y1BIUzyY02*A#eo1g&dMk)w;GjP7}pb`R+d($7|mDR@s6B>)&@wY96U_yAJ3 zU-+4fVoq~M)}n?mXa^OCW(NibdHDJ1(8~@0W5U1DPgi(yif${u0ZA3b>~5Tb;!ig* z*;&eeOv3u}D>#75C|&N8M-mVxB-jMj!ms)HZ}5)toJQD)hD1sAI(7a$Kde9_etH^i zA5DOOyL&I`CoEz|;cVF2z1E-8ebY1nSIMzM%4}^E#O$Pcw&1;c_tMeR`$R{xqa)wO zMEipD5G}>9#C?;((z%sc{-%^ur%!t&szsV2vRzr?^+FOn#fLqlv$IpDLV^4DISZ^t zYmVhcQ&ZDb(^+p@GIA8^8yh#kJ~({rST!O$fPUkOmoP?rxP-TF-+tQdlB-rpE0A=h zg}haVb)-XL$N37B<6$s!?H1}e{AEr<2sK~0Uc+` zNyGZ=mzO8Gg;o1KtGnz%Lk~Ja>x)b5aF1>0XD*#C3)Si|2jzd;fA|0KbzC+qzsTG(QPjRjrvHxOvU8Mv6 ze6(pG3yMk>qvz#$kShXciYi|Ndh-NBCYHEgG|8(dlz7)S4`sFFI-G|_;72{ za$0*nXJ==iC~gUfjTl?iKnVa}Cr2gTeJ*fRbNLMN7dS#rL^xeGFR{va#~5~)x3$1g z2!|d>{p984#beak*w`poSO_A$=Y>yFX2rFzlX4sfCF~p>D`6c3-@QvGEiK*J*7g9E z(+ZiWN)CcWC1ge4s9NNq4t`EKZDw{2se_jpD9+Zn_7Cd;P+Z1>Z|>8_R%DNGpTkaSE{> z!foEZd-nsLYy$c*(41$8T5y=e?zXV5_z-Wn3!7_OCK@KUdy4uRj!p^xC(Xc`T-u` zm+ygn1K`D2knX-de;haMhCV7d-wx6Mhio}OyLka8kKjIE#qvkkbpcvXjn6 z5W>e7jtmL1bo|C#vSZhB)iSoXZ4S|X@xh@uswSQ##xXTzO|bkYHQx0X7PA|@zrCxi z6?&F&fXC|qkHGWccF@RroFyY53+GT2FJ2TzSkre}3@qyha(1Rju2+Z7N^0CY z_1DmNEmR8VX6XAuUi*SsO0WgyIAQ8Jmp%>vJGXI3-Z-F{&h-igDhoN)+vZVKRVC>* z#SgucjAcE2{ferp$FIL_VrO677^YV7;lny4R0y8N*;Kf7XYNKt;X9T;e*BnizS!6_ zoGi{I1of+-e5p3Iyn1z@@Y)t0!9=vSoFNx}R84t9!+K~iuYef9#dVV3fiV)4$J{2e z{>G_`7wN%55_r_1l;lWVsfi2@z6xx8VrnOPvVdKF(F6bS&#rS*DQ!R3o1eC{3_yBk ztAM~t&&BD;Cmt|6uHU@r4P+v>IHkqP&Tctf#GKKso+RPxPW#jH^4Zq*M;w=J6_1!l zhZ$q+R~r=;?>2G1LA}3MUc@>|Q7ByX1IhGqmKdHiFnHP4 zHkT2jhI6sW=hV>&wLjgvdv|B+)(6hUhK3wC5WH07g!lGG!I0($pnlxE3nKnaM|y70 zBat{)Ih!GJh|0oPkuS&dIkVelJ-nD6Xjrj^h3Y%I9?^y$-zNr~`{8E?Z{7Vyl^;NWqL zz}?u`+fW@T{I(RMCmrVHyCbp+J%2B;2kB^^O6KSqT6MI>4H9+AwWr=3n>T;y@4rrJ z2(cR*XJ-O>$Dsq7!i?3Qy~i*46p8zDY_*ee?!PofW>B1bu}Ucm<-2z7gNEN|kgZ@> zh;XdluwjE?q0<`d=2x({Q-^;a18E~rM7vph3%2?QV5A5FN;e?P69%W5qTQO5mKFdh z=rrv4JdgRk$mIs1nv{fD%wRQwUwA_pl982t3THC>S@al=2@lc@wHo2qoVUo_ydXxbP z85;=D9CGfgl;@!q>v0MA2=@mEtoZclWbX%b2mBfY7O1ZMO=%k`rvRA+hkTem0^o)( zA#nW?XZ#qMn0%19IgL0Rmy&97}y z)VQb&DtgnXW$TkI`p?fVqv#=cSoO05druK9a3}1ejv`m+tNV^(dJ$<8x<`X#iFX&5 z5;Xe})d!G@6hGl~`jWw=XpFL9sOux7IaaQs$wKlK+ZM+%X%WW(1y=HBndki%riAexLZU&JqF`*y2}#HoUm{9Hp2NU;%u(t?hpH91ZWp=y zpekLO6M}!uaOlt>)ohtz%5L;p&M?xY2 z*Zlg;n{ucmUq(mC!L(C=pnxLQA(rtv+b~yUZ{50u8eNUJ7{+(`HM znLExfSeqLn;znmkQn)$OQkG4rzL>>S6uhQC^tz1}*?@!T2DuCr^5igDB7Vu>ml@Df zxY56Hn(xHQq-W2b5qYtr)YHW5O#^l>jta=QT>U*?`djPM&*4vEd4+{(VXf|+5YXu< z*KgeL!ddTxi3b2J5ZbsAjJQfb5^$-)(&Z6s^^QX4V|fWzm{fZ!(5*`?2t24VdL0S!f7t4)q z2023s<4tEySpPX4_Kq^Xi3pUW6^M&719-o}xF)j)4D|t?w_+SnEEuLC>paQTR)cSEi14Ut$Z&)xN#e}Ot zL2FSkTYWCm)&|5%Ij({^c?eqUBTWH3avD%uD9YWS&|vLl=!GOP99sGdH#~N|rG-v- zN62BC&6_tL=3S4?ThZFeN@Op*3q)x@#2I`ro>0}T&o68MC-#A0mH=jfPO6B8l8}~` zRw>lT38YK5f|4Koe(+a7|9TvKLp!3zAR@XLxMqA-RuC$AHHy;1(#*1}L>9Er4-f(n zx#HC;8i-7U>fXC8jXa29u^Tf&P8Ik)Qt(KwBP*~4XdTl-@m?JvV&JNYQq^dEPf&g_ zGOp`!NTVEuS&w8Nj?iw|vxf3H~VdElJK$TxeZ{so0p_3EE5$HeJ zxL67-?kYGLg}lXG5*vO`i>kRQ&p#95L*b)t`XYeN z$T>-((Oz9fP3eg=70kp7f5Vtw9$b0?+KeDr<;HWeKl_Mp~QVNnQS0!2T*FLL5qHCUQih0jX87Z*1n@xi!pBM$P>K;2*V zdM$sPj4Ktlr}C+C`?2=iwW1~^=$qSpF4e4Mf;oB}C1ZwWO1E}xHS(dWC@vE{HyD=} z6c&C1lD>ZH)+>+|yxs~42??~Gvd+%pdZR|vq>fic#tuZ3jz8YymF0gGy57z`dwgJW zAh&bc)RYQ9@?)ybYSIMJn90EoGWp?mxl9i-FU<{!x+7JajQAI@6A}A*dV0cMGDij$ z7D{9P`J)?u0jq^$u>!wm={9^J?(N%G;IzO&)GY*opC_tu@`va2${ffHI9{?@N~-ea ziJQDo4iRu5I%IQm^Dn6FWUehB4~1Ltxqp8%GB4Lr1ux6W;@ZDyTv(2BA>G25Kw&<`Y3TlA=ZRMM*0Iqy@ZumP*5Pq6?+ z&YH&OoE#J~5_ z72Z%&T_v%-dzCdb7%{qTk=W zdpB(0E^V#-HeTL%?1F;Wl;efko^&;}vd<=zJlOuZMfWYh~j%+ce`B@x ztTm+GxG_?(9i~3^xK8>HDh+94Xim<)yXmHW{1Au~pS!tgg}uE!5M$ad4q?e_^{-#k zfszvY1%rmjdN3S2N<7#>e5)|7;6B{Ifw_wR!`_?6^}K#<-(Pc*af>n!MS~$^$h1kJ zOi3wrMaa03282+^98xlbqEsTHkg3d6q)Et3laN%B49|Oo&2`_;>v`^fp8uX+*R_9p z(D(Zp&huPr9qU-f0kX_GwuS@bSTchAs)dq8f@pIUTV03ulIiq%iDZAdI z!Z-zagozWrXTu#*eE9o_?w!4HRo(GN29cnVc2r?QpE-MWN8c7B$BY@qIsENg#r&XV9!{eMwwudrrtZDE zOLphw=li+!n(T0W(9hHIZ~z;=iOx7}KT~_?0i`{id_(%fe6xBrv5hH$fC|$lm$fU- z=dbFM9h04t)1>0QL&&_52}M=4PQ2Oabl%O)%}lGYgK~%Uz$6_wY=D3-#I!Nno9ead zn19orY;&$I<>ZVpFe{WDj0-_;fQ9TIoNzdP;>2aJm}mL<>)B9#d4~>@6+=i-p0x4W z6czRE!Ts$TElD=$+qa#jW=Q&1TK3X8?(7}rWZ!1EQDF^WvYyrX+I3Ib6YuV3W_B$r zCUc%l98;{Muht}Di_`eYq`0gMIwBh$cO*TdgiE!aF6x>g?#9}SmuK%xAN!8?*X#-a z=8{m~+`dagf;Kzq>FU;D^T{RB2Jj@I2s)A{9920JCY-3w7aH~fn9u6{tvO4nOVU|n z1G))>0^;|6?)gp@BRLRm-o9;l?p^V@t2C*}>O$Tf3lCUfIW~6roF55N!wW-}f6nhn ziU)JS9&XUJ*J&Ic&&vr(cS+0*=K_Z@v08=I%MJ@-Qs?qT(w!@}ZkfD|;!%#vYoW?q zc~u%w!-o%#Cqfn>#1%HzYQpsq)CzvS8ZA0m4=pJTyi3YVGBcEuvtjGjTcjT7-@gSM zl50Os_jV+>%FhGoE9eY}6^B&eqX08$y3nAvx^7>{A1#p3pj}%Vu7S;?FY*@u?wQ4w$a%QGI>hg-gBL zvB!)o#=|=8c56@lUt9pvW)(0EFLI=63I^v^LcLL~P@_dDnJ`VPD6%do1Xr(@RGxFY ziekU}M|H&@I@^DB7#R;b290<{d|KK#hw-)`CmluyD6KK9LQ(9(lTXUj**08CjHINI z^n(Mhe(g9qU>(yClF}&Et4CuqG~pX?Xk8|CSUK9FrmbKG)xMzC3oL8^HI_rL3 z;x-mc00Lzo;%bWlB=wx%|DbTKjf?x6JFdk!AO|{E->L3si`se(G!B>!4l;Ycxr)3x zOC8<|3B%<~=n*<|e88rcF>*}dWj4#Ih4l8xfE=0Ni=La+gfeYyOiWLZBw}V&g<9`< zEf}TmR8mx0dQJWqc^#eVD_L31iPpoWOlc=)4oAQxiY1WIrcC~hYe1$uog7<0MrlP* zOS}OCws!BwBW6Oh3X%fEP`y+09&eyzZ{M$9b27&pbhg_xS}(~n{kvHXax7Z(e|>rI zfB_fUT^a0}ZM=EYrpZ5XSlT(XBKVGB|bm?fwqm@I+rda`1@juL10;l}bk zB-59qrS9HXj`9N=e0J|X{2^wzh9LvWG?7~oWvOxS&JAW!@}?)&shJ4_rFHAp zBlq=JN4#UAns&p6V>8Ub8Bh>vNhWVOp=0nfXP)bZRvpIr?byD3l{DwUMsjj;W`ajw zow8t?JNboVYMkPR9kvJY`Fg#2b@4Bv8`q92VvkT%hJ4%WSomZBNzLF+O@t=M(@gy^ zTz3mReQ8Nilfye&WL$sklfU4<#Z$`^5jO=yd>dDQ4xQ?Nkvg^d_U%i`raD07uxDfC zHD=}(UHl{J)~Zv}^L4_;>11_K6Tv@*EneJ*UVEn&Pg7FHYUw7_u4?p<+RKwnf(q#L z-KACT$htS~;y)D*IY;%L@vAPqK(sQR@$I{?FH1OuQd@FqkVIOATWsLqK5pcGbxP4c z=OXL?tqEJY#S4Bg5s$5ros*Jf=9m)*m|>x_26~sCvmOD3R;l42YrX0}^9slefR*u7 zGOK&mn*ZpG2W#IRT!caTU3$)d0bZ)^`9_wFWF`74*IY$C0p7QN{D{`GoFEei40NA3 zr)}`5EmpoHsGArxf4O|Ukte(QpfqPeR{^~f-37|8{3&_2bU+J>J1x?QevRv0960BF zY0p|MTeP6W@}Pcx0R|_9U;}ma^;9215TXiYS4b0tj_B777hMkMtRnHXob43bg4ceX z9hlfcrAesa2O*hWbYF_>S)}#bryq+HlR0N8bg$C2vOE>Hq*OU{%>FL-x}goWQn+}? zVNAZICi_W#bWXkST4n6l@b;ZMUQtoqkQP#)3@-rNPJ_ke9~`9XPgEJg%Sj0x(|Xyb z=WD;HYz$K)O`SY>GC5OA4yNZ-ko9@p0{@@jUUP+ zcXxMEqrp0U-Q^1tvk3vx=GJb7~L(TP1#rBmWvOT9$$MxRh6)~4PX!=Mj}4OXg~@`U?K zjxu@LwCW_+67a~R8?9e0CCMJsW-3jVZ8!6!yCKvRP_kN3cqYvJ4oVTVv}_)nsA|k9 zvmw0*!03Lgq@-?AuvmFbRrWjSne;nVR8)>nzul}wzlAD-JJOE}*t@qIcxd|3f0`<` z?%P)nwWF(7uPPK!AJ@{6ucBZ^Wiue4>e-72A{X%`)+eUbCYM@(8oJ@h+S}>u-Lv`K=T)Ygn3eK0D~el*zCxWBkyUtULqf&4sLei*D^M; zT*Wi%+O<9jr{SQmd8Z~{ikFmyTB70SS2;P#H^#Q|U-#|C-MbA)tMs~e_e8{>B+zp0 zsTnGq0xejHUVeUlrfqg|dPy^t1GTC`z&%)+)M70_!8~c^5SsrY8R#cR+6{WS_hX$q z!ryuhwV15^C2iv4(a}a|{Nw$x;O)bnayFw#0^it}e+jdtQ;XgV*nIj7FwnWE=ZFw< z;ZyR&NWaV;EaHe3rYCnxfTjDs(l98VpuiI|kz8FO;>1(`hNsSlvIP~UvG;s<+w?A; z%RYtL+K?s*x!>hSkIoxS)%>xMsAMt=o(KLY@m8-Kqh9ZDotl32v)>pxw9)cUw4AQe zXzV~B92QOd-Md>ghpZ2qZC-PgutM3v-KJhwIl7}!DUDH@(rvcil0FRAOX0g_5e!a_ zn0hqqpfNQ>zTo@ymnI?RY@W->$@O_deE@Y?{8bb@-zHUK$1+)$xonj>;G4>3{3F$~05Y*wvEnfb9BL7@+G}G}@er`wZ&C1q~ zGze3v?l-s{$7-o>l|YvL`)4W~u|9;Yv6;E7O?74fv? z-*Y1o(Ud$*FCxVFg;^r&i^~jH_?oNjUwhYj6<1zmC5ZlNFE1cqqx(-GatVE%@G*>u zjN^}4l#SLK<;~|GoZe(6Tc(%pgXcxQuTadGF(c?L&tj{OPgNiQX`usUTpkEOD1Xn% zMFOWy8R5n0UPZx}Ykh@4OY}D^BQtN^s?G4gCq_8TBF1;VXwtsj;nknc?W#Owp7a6{ zUvE^E>~g(PyVc8T%&4oAxZ%V!t+7Rr+yKdyzgrIMGh}PnCKneMoh3@a9^KW_hq@&I z-nF5&F%NJ2?%liDSt@FJ3L!3&m@J?hX*lP@tQS<0fBm)HFw1Z7bnwaiT9IC?VrbNm z7HTOZA0TtrdGq(OPkJlA6r^NibWVRtK~3wpnW8o8u}^j zd&S>J?dtqEjX94ijM_Ei|F}GTuT?chY~tFtO^3-oC92Pgq0ioSqxR^}s;W5STCn5zQMTqz zv_*uZ3Ggl%q0o}e_z3gO`zzlAe|hpZS!wpjK@N(rWp@T`4bvlb`K|r%y(9k%WMdJd zwJM$mFnG+E`ZQmvz0Ya1`Mx-l*Nz9fGfaMl5{ey*AWs>G+KN8g+} z`*qpp;!N5Y&pfVAefZ#kv@|3E-?Vx2n&mZa*rvzm%+PQ0?|dLk(8Rxl&-CtTTz!sl zD2GARqQb?e>9UHAuN-3jD?Y8;a6qM26{X(Me~r!WMzGU)UfRZNkCU@=l`qFS9qN$a zTCMoQhv+5WQsuaZsd?A*FY$h8-sii40v zb%vWR!Kx4@Hf2p80|V(Dg#j*3=vtX*{di=iD%Mu9_<+UZ{f3NC?hN?Yz1TP;R%0+yN8b+-Q;0JKmS8>=g*Fq z^~l&yrSswa9RNM$a1(f{Wy{Sxx5IOr{`FV2LB*~*`9PR#NYbag)Q~!C&;^8GU~~9w z2aKlv;5o?!97s(I&#Tg)p`XlhNHqz(GKKa;b}!xIfsFDi6hDBtL#J*t(_2NsDa&|= zW$c#l^G!x9bakzvfJ0&Nc6qFgH{^?23JX&o>6dXH(qr*?xEwfJ)ia&saBjUHC&7>7 zpz@La@bvi=~h3OQx7Cw8~!u)T_c5uOCi z#sFni6cWnGp>(5HFz6**Rw$2pE_r75OF?R-suG%Z7yqWOPc!nM)mRxuE=m6Af|nhB zKl$5&pX&5vZ^@*#^p9O!T}QaL7!_i$F>5(3F)4#281rxsz`Y`czCpbmHUH^A+L)5$ zN`q~^=_;MF3$r?zviEClZLL&~V@l>iXkbFW4eV}v%xB}qIt;a}B_s(8o>1dKFWbq< z$q|f*qgntJX04A$MK!$hoFN!q=@deu1T+d*6#x~WqGucteO7$yub51z5di75y7zx6 ziIIg`Ws)APT7C9L`aMi}ZxnJca~llEuQ{H~p$AOzzO-}=i!|1Ii2BfX^q4r`cq4<5 ze~Mn>jgbSvwF+M@%xamKu%Cn~oD8G{@B(r zQ|Hb-KbCW~#Or$O(ni9aP4w?dM|GocQl@JB_u>zn-Xc|mGUCHmnLkMAsnluou7pRA z9`!!eTTidDj(#z9&Nz;PHCvDNPo6t#U!^%H(H zKz5>s@IHnn8*$QI%WGR7@ ze%z5f`=2nFgfl;CQ=^h6lWUv({_-WQ^DRmgzki><3;V*i{QeUl$d_faJgK9+#*?h= zzdxII;i@`F{~te=vJ(IJIiRzNUjWGR3Htq~N(R0lqWK&gRLun{qLu+{r~nkWFS9o5a8{< zU;aP3!T;^j!FsvlABgXdkIHlZKYdWrfrpf5C1k#JI(r#Eqqw{|t-BOjKtfgzGUL1C*M@8GYpMf&t<}`3mA?nleHqMz#1RJV-A|J$f0eXf zrEXnH!`14ZTxP(p=WVe@maNVgKpt`?{>x2Bo6(~tO^Wjz#w)Q{5NsZPd|}{+IFfQM z)8@bC=!VHsH|=9A3+{yw$YW?BS?H(^{K67gGRXvn1$8iu)$BOd`i~N{aw&{ejb{fO z?oXOyrWJVbV6`agPDv_Bw{8sv05`6`jRk&LYkt7kC!N|eY?b@_N_+meQuV4EDCnz= zrtT%S@e1s7Se+>q!$C(EueS$$NaG@{3$gHfQT5tl(cinUS!f$KNf3v>3@F4|H)q9QvUq; zv0V==dIZM_I(;n{Ch~+){}CpzeFdeGuH$OW;z;ibcgyrL(aC`ER1M*uv4ao(mq#eJ zRvsT=ACy&8u7b4cziYbQNGg2LSi0FlN#H$&&!uSmK85mXb{XsD64?{=?n+`Kg*a`j z2{}cKr8R>RqWMlF{1`X!FaI(>k%%kli(;ism!_w3sjssjElz4XRd@4ggLllf+r!Nk zY7Y;h0qp|jiPzGUbuO7w?4xw#2m&Ak1iE;@ z^VdmP^q6P?M~xjh^>j?kHOM3zA3wQzHF#w1!GuC6B$q*nRv%l+a4PlE0Rc(7cNfC6 z3{Fw2yddM_vIYT82*)NGmQdAUcK2yv9G`rtDlO(e0z`})!DFK~HS4L&7H(DsQA`G! zA!1#YehYvbjUUusDX$Om#%7mgzj`$pFzpkxy=_~!DxmyAGO~x99n$KYpP%KfA;yP= zLDXiohs0>OTPZ|!!?(;$bQ2;GQT`Ld-P?BWM!2g!5BUJy-uX-KH`|K_NIYqvGs zZ_{O%kx_N10b6(Ns!hvKpkd0DYp==nJvcer1Lwmu;{2RR$`*CcDiMMpGimSNKOe%p z)&bgJqDvYc`4U`I2(&*L2sA%u+f+maQW(6XA2r;>!~@P6qz~z(wOjSVQ>bu&hwTgu z)H`(OAk(QwkIoH`tfHW}Tm2&_fQFs6b{9qxdVp0F@aMjLEt{l2?~(P?rLd${u2euQ zn3|s&vbZF#A#QMb7_G&~_{RdUpZ@Nj8*||TIZ^3BZ907FE8q7X{-C0~#{^dw7skFC zv})zeV6)I&c>i*TL7C+sc`|iOeyLDUoyb#L`6VzdK3)MCRc1KIqJ<$K=k+4?ed=Y^3sPMkV*9rW?}n>Q-Z^#m;OVe7aa+wGJZ7}nl9D9p50L)EQ1Eyo61H!!xkR8x!iH_3Q{ zvA^Y5v*7T?x7+Eg3pBbKILL5&8xO6mgB1B&w+?h(zT(yH;>4}3%qK67i<}cV;EqAE z!G7oICmPKknCzU8|CME5a58zIZt4K*PEAC@LR_YR!}fj@x4iEtn3lv>tN+SiA-I-U zIdF6b*;I(E26N?h$pb5nkkWo=fj~oUTZ5W)p5`F#rvZJxHn#uQ8 z%5y`Z4t4+gYsq$C-O2mfBTOY%EPh8WHVCN7d~aR#?pK%12O$}qTynej)AT*_FKU~J zH5JEWsCtj-dE(v2`-moqFD-of^ozlgyr8vW*5`79F@ zt`k!k84~XXm?GQk)katF4i8b!rLV7_ zS&RxHKWCnZ!LsQn1fpZz-QHU^f@_q9jgS}vRO*Q?e6#aGw#7b<_)vzdOnm5L9YjP~ z450fWw{r2*i#lw^(Lf9-JXxk-`h`ZZrmcu*V#Q*T+H%pOAkJjdgniRXK@~H^3Wx_S zo_n!}4hWFPPSiqG4AX{LV8MqO=oFptR;?ocnSa3&tClfAWzxbF*=6x8gvlxDcw!;O zU8k@c^#RDy!kl?^UBl5Yp|ww4m@d9G#54L}ns;|F8eR~0;TuMXl=)l;WH1I`BSab8V`%Hq z6FOfq3j`-Np9#7hxMj$NkPyRdF=aERgKi}i$>+*4C!MhKNoUd>g)mbebZ*|ceB(A` zfsh4$URHk9@yb--0Sd94qW_Rg%ZTRx$!%10v z*cS3Idd4l~HCbW7CW_<$8nfsq5W!EAA-vJ=mFrxAG@3p^2_-bKN=ZvA-Z1CKqH;8- zxv~+(dqiHNmEBA44ymv1qhd02_Uzd~H@?G}%_(B&-tl1>zrVWz-ROT_H520y=SR%6 zZ?IeeLfiJ1qz6&gvt|7ke}Gq2YAalKKZ_RvgB4nFv$?wQX z_0&)wACX~wVTo&GF#${s5XDR5CG8B?hyJ^E&4P*_zg0%RvY2BEOZz60r|1y%I5K|D zzWr&erKB=kf;YiWR-?VWsP7GE&YO8FyD~{W^vVdcPw2txX{f`I>PG+hmHx-nkP$-> zeXDEYMf6{Q>|*FwcBweKCKW*)aC}GyZwW8?%GPOg$MllVKE-V*v>T-P3kEq+H(Di@ z_OC-4GsXKYL*P-@n*^`;{>@8+Zhh7#o{-bqEt@v!1M%_%*;4@@k4}nvaz2wNp$=-) z6Yw^`yLSEhy|?VbStIQt_v8Gu`TXPk zC7(mZAk>}M*`o{%;Rp0Pts)C?&Q#Ub(MjMYAq5>6amn?SN3B*oC|DW2iGwp3!f(#v zl>AHtAYPT{4mwlwTSa3kck&~G5@rZrQWtbbxxZ{0}PTYvxQ=StsbXZ%On3Ir4y*_Du;(OR4WT(Y?Q{+4&lT)UjoJB*}CzzyQ z&;D7XIcxO34cp-VWptS%1IJ|OCyLL*Jqk}gCL;&QsVJ@)q<@OI`8*)Uh*o?jWr*0Rq{w_nZ^nlX=o$QbLX#V2Vy zIWHv|mACSD*0~35Cf6(RzxZK@j_=C$gNnz+=56xuB=*E`&J zL_#;$Ic8>C%fBXXoZ9yh*|aWWZ^={NA=GFyj&`T~#H-IC>TU>T>Bd%7yP_wCjCcko z>ZzEB&$U=n`9E|;`tI1L7W}rc@@%1QZMfxl|inz1+oA z1MKupPm6j#a$MD^$wo|CpzAq!&1Y=FM+0670=|DgEg}xyqmiY2{um(lt8>Rl(h)j7 z@!;R-nd9w<79#adZjW-|l`k`D4J+Gwg{Q*Ww8{x34JI_t^N8t)C_bE-zV4r+>02CR z5#=_&(?ny2yFtIC$ki{4p;pidJNjWjBrS!U838AUy#Fxu!^^_LuK5YeTZuJ9ZiPYI zN&jOWyKoD$l>24OAZKZP!wrMOL=sX%F+L zn3EnBM2RNk<=8_S=NlQEyJO1mNSO4soR{)qYb-?EBg7STsBz1dbDzu@OL1xH z`ulF>&F8fbpTv|}y$7z)Io~H(p!Q;mIsB3^*Jy7Xa?Bm?`QNZ8L-7>0I z-u22alA3LpM#t|1wZoF++qXy2G8Jbq{L0KbWZr(TUIsxL?LEk*{mh!#=<@HKJ5eM8 z&RGZx_qp@z(0d<|$hlTJ8(gmL9> zAPb7S$(q%xwe3 zx0${k?S?0|FLpa&aI`Jhrr=xlwFM*Z6dJoF6fS*nrdIg-3a!mcZg#JZV zc3&FPQO63yVvo;i2gjk#04w`k$JJX5&ZXO}`0Cn2MIeTr9I-Z+UNB4YL8Uq~_1Qo% z%uOs?6>#7td8D}PQPjT{QE+sg52IH!7V=o9kFj)LB)LLOTHGjjI#!Qvzs{SdYdR>W z&!Ir?GaI&^+;{oiLH&1^c*5Wk*xbMfJy=4x-toz%@}?A8yHh6&s-;-j{Ww(!$%2WG z3qx@1i;3AwzLJaW%qkycW$jzUY?4m&6}_(XkaIo-jO%FErES}wyUvUtlsW+xLZyh zhnwo0H)6DMqt#NeJIGn;9c~R3WWwaUC6ttms}A-__`1|)XF%GmCLbpt8Ety@!&1jD z|N6el#g9gQt_?$7^^A^4X*1Cir@s2arxzQu?S|0jx&4;K)o2S`Mk|RxH+s)~Xd%aV z5WUVGPhXNfo9D%%H?yzsqDs_dWPcOje6aR!q?rw>$@av8+rCk$i;0wCu_W zs&26$6Q%eM=WpzpclK6*lM4|>ypdI|+y(uZ?=|jP5#}ko(>7m3hSgYkoaQVdx(F%f z+}Rr;0+KluBm`T1zn;fY{6owA#l?&I`$w#K*(%sGZUk#uv^Ilv<7(YV9%^A>5%f6X z$BmwGl4B;~jwm5oxL99_%$LJ9g|B|5CN`>W`SlSb36kQ(7z(Uui6 z>EYy5FtyMlvv&n$)}BIFF&|>3&f<`q-gK8nsJ6LPq|L6}`()#)+UfZJEE#w`a299;U|2|n08WbCJYJ2}<1&szFXBNFT?=2E%4?Gu8Ac>6p-hTAZ-umsW~ zYql+ft-TP^Xcy@$%yl5iDMdQjG~wbgN`}{vOKNzRgWjP)t3){~1;*yyPxQ)|wM|k{ zYud?rYoP&|Pdn|U7tWF9G>ypdWW;M%mOo^aSGt_D<3yDOO65h;C>R0?r@ z%4{3^M9QH^p4m7mEDMS{yi}&WD?q>z(-$oh@tu!`3>q3G1sFak0f)+yZl#oV6qDAo zFhfgN)csoiMKMGO7?JR<`uU>k&2PJVcZmcH#7yh@EF0(%BKb~2CI<38)0R({t=Icp zi;kmqPm3Iu7dDqyBkM2t&E(Dup(R(`3}?7lumMdICI&Z-J8d@>;qY-hBVP@17diXf zt2xs-`jQ=eSw;dFgZv`hz|n+xOKL8ZhNytwS)doyvvkXfFGGB=KRD3;%277-uC(pn zUbg;zv#eOBHavnJ!ylvg?=dd1$tzShw`pJkF^g7q?B&NKA9E@p4nqF!amRL~Gvt+2 zn2G0P2}8ZveLFCcPCh%<$mpu-sD5>RY=T=C4)Aw2E9g&8%In565`;w`0Byq2dro0G z^XZwUim>{MuLYCD{xKSQnUv;4*$pt-4gvn+$&AzW2=Vu0e|Q( ztzG{8>mtMx#8F)wQ)3{#OLmKIhu|8RBWbhA#7P)8Jct}E5AJ_DCx{rx%90@D_~Nqz00W-6 zci$w+UkC{~`kRkjp++lZM+sEYtlz>TRJQ}PJ0HKS2>uic$235BEKGy!H;kcSVH+!SL)vGA1Y7hy?9>T~E zmjF05#=w`yWi?YR@4XnCvY6$-+M{_u2BJ0nn7@b~eF>@(a$O)x76i0XnI&WpcXVbeObNuJbSJV-G{io>Fp{Y;#{Mi+EGH7ji>s$b2RG$g%hj0-&ntrmcwY$U{P zIf?Gs44!Q-fJgCe^tSgi6l`~-2oAWoXdz(a3i7mP?_Sm(!>?QMWJ7LDmMiahCgTZi zULNKP{m~aH;P6w)06k&UBvaNC+MZ&#Gy86!2RMC*U_@a3*AAc5Lb&xPg_Mao8axmr zbT@{4T5)v`9#~Ar21%!j>*E~12j(L6E)FavFwlsMzLIF_xb~i%KAfA9kSvz9M8g7R zAu^e;6q#da3bWYYpTVsfW96Zf`6GQjV;8G#ko#9PGpdo0t7#{zoau#b$9QH*%|!zl z33BeC$=Q##qk&g52ol)ts^UAr(%dtrR5gdl#cl7DgAkk5`1trnkFS8wh_#m(&H-Fi zmLoT^{_Vv%H-`M%lCq1pX2p_VeHHaD+II@d90UpFO1^vyo$KJHI;I+}m?ly|?#xzOW~VAVuck1BsVhr%>98 zemG~X6lD0f4kz4NvaoKyeeva;=bT#K-IWz)jYq00D+YDzvplFvI3FD*im%PGm_g$F z3A>uP5HNNFb=9O-FOQCo;0rDD7DzGZ7>6YyEDGCK1v{9k>Mb5FRI+g|oc-?QTC^A+ z5tH7xOe384ckSK7^NoNgiCH}}11jHNydUG91vEwlppZ;*PRy7@DiBJU!&u1iobe`( z+t8)h`{gkTf61m(rL{ z`r_=@w!x2eLlin=NDU^AWa8(8_IB}CBL89T8vXULX!zo@v}}}@11MWM7|Tzm6FHW| zv_tZLxzqXsKLv_G4u*I?M5_~~MD487R;P(an;(Hrp%_$~jEMJ{FC~B+rzX80yKEPI zc2W>giSMGm4n=!w(LN%kgz$Ls39*2Q?!f}Gg9S4}>`!wp``x>FUaKNMwTM*Rv7yVa z&sRK~Nakulbu?Ux8*rF|=+|lixxnV!10->XM|AdIrF--b1{7WWOAbRYCrNLR!Z#Pw z5qh}ck~vd^P-VzsWtwu@=`7u|ZwmgrlWr6%uMh2+mkNUAk~Q}Iayf0*tuQzBv)qqk{8m-4gNH2=B{GZlJunWK6? z4kmieL@EELagWZ~Bg#$Lo8mBG8P!0CJBfHxEwS>AA2?Ofkh9rEsynbp&b7C)A$kx zMo(C8_yh?tZ?%WW(CiV>RIq$;N+&`(?H;JFtGjXZB`2dlZe|_zF?#kEz`L3BP|_h@ zZRnDJIFtISv+Y?6YA$hqU{EMPw{djVTUFZ}R}bpbpFcKf)01`vfgt(k_r#^GE>?54 z;sLUcnBV=v4qbKqnOo&gP?6^YT4u`A_3=3Xr+_PMpuFQRAhe%9j-x67!|5lDcnlus z&`6~fq+EDs=Ij)~7EZjg?Ka^1mts5$;Np-ac69B~Z3nD|Iyfk6JQ zpf=~6|dJLA=*^DaMhEHt|T|8~a-zvDbUl-oBPTV#K*~PVGiYJL>;;?`OP%gM(*6Bs`13 zi-VdMSlhhYyLt>`)2P}ubF@60I#~p#c4+Y4qdO(&ZevnZjw41#EJ(K`M1R?HaWEXJ(y()?JY~kHSrk{8ke5I z1_3@gN_35Y1(0p#kRm*KX!q$*=ElO4%je1e&x^Rsd@et{DL%@P~V z&e+$X$`>9HL^Vx7?JS9cvbGy-yo>bqIqHquSPxYq4Xpfm zrgPfy`Q>H(s&!q&R6G`@$2aL#wORXkUM7WGI{$BT=~>W_wHhY8iNj$ZY`kAifI^#~ zQnh8*J-5f_OBV$*6#H6H~pc2bH%pi1Mizqit*Nt_bYx_ai8RF!QlEOV62Z!%ozm< z-}`+@?z?x9WCYTIny$37*m&*a2W@`ECL59Zkyd~>%yZphG7|f?px~dB?B~jlc(c$l zx_&64M)pT8Cb9bwr88$~CyMh!mzd%jwQ9`~9lq$9+e8J4luHLNGX2hq0X%@xMKf?}kTfDmaft4ym?uiiauKj%wko;;u|fUc#E3BHd;HLP;_ z0$YUgC=dh1Z!EXmFu(n;OJ5oB2*u1W@|?!`_t6&^@afqq2b+e^CU&V`_3M`}yoWiN zJ2YDUz5IN;L}zX98R}|kTH5EP-tCq>=gwT;F26Th0-2R~>8y#YGc(FMdU4T|L+fg6 z(28qwH;B{3C-~E+PaCw7H8nLfhF7*G8-Ht9eMjftzh4tkEn~W$f4*2n;7rkICRdjZ z4+f_PIRFmAG1=4>oKUUfCUZ-jYsX&Yx}f;o_KwPlN(F?f~F13mk?x{$$=pw zCA2Yn&VORlWTR$sQ`6QSZGL`@%@7g&MTvyZ2s$vLN~AjBCe2rSoc@9rSKHO_s0T_) z&7L71RkUj#yJp%tScepBq$k1V$RM&4kP=CDu#cY|vpdU0X-D1vo`vF>k7={&ePKCB zjvy&GBSyz|4-$JIz@^cJWV*Y)L%}VGJ|?S;WHdIze?MG)P@z33j~@?6Zrak*;m;%6 zSGR7}mZaPy*M}(kXVU`VcJ(q;I5D=#L(P`$rxYtMd7sQW~ep{CT} z4|PT>8aHG#U1%z}End7ctGcJd+R&e1Akct}-%8_GQX@9wg#&tK3H7QE=-vA%o)lkiPXP|8`R z^t#6$(v6ia6P}<5dA+95ET+xbFJHE&d)T7^e0~IdYgKxt|Mw*narR&T?V5b$A1?Vh@c0ra&tpx>~k9`vZ~lS zXkXFOIr~UCiVF3yOWLSr22R24{jdC^P_$8)7!9^yU?ZIoRjKQtNcixLmLaC9I6OX3 zsnznahj(eU{Cb`K`Q!Xh#AkGx;UCEA0XE?O#70r_8-cAZe*d;~vsyPpVZ#7F;KJAj ze8|!z-$4iaSlD9sxF9iwAMUfe*WAMp0T`T|n4=%$JxGr#lR@&)k3CTQcB%Deinh|Q z&)Mquz)Vx~QdgD}PN+(s%L_6_wWNgCp+hsh|U5aAAIw zwF?DU-u~}jLPh54E9Ah3@H$Yjh#?VCK63V7{2_;NBAKzAv4U&|sKamG!nPJbLCy!> zXgO}c+ociy(4)!5HG_`-&MOp(#va7wN00X&RNZ*39Wo)KwO@mMajCP#WfMfJl%Yf| z9;6iQlr$O!M|&(jdclQ z%g{Np`S3-P%m2K|1BpZUBa`n;PC}T6v5S zaKVhYe4w-P1ldp(R^?kCay2lZ9>u!I1 zhG~4m4cTuRB_$+uKUcj+_wGrVrFstR+t6~SBca0^EOz}bU!j(+>Q-Zww;56?Bft&0 zVwZgFAzfD(?5j2SS;ilQO>MK^SwCec#NL^{Z zqeqMLHp`zs&1j~a)AU`#&$j*kxUz(XRhlR42@wSO*N@r4KmNO->CoS$nBt7y?+;eQ zuaS1=ub(dB2mb5F(f*Gka%G)APL1-_AZEkZx@&zbPBiL0wrf<*lcyio;rzXzM?R*mgr*I5yQVhh> zhf^fhSI5w8#PH#Iaf`6R@{G{_`}Z}qnM9hB0R=(Bgf?EtLOV80@p+Cdt(fAz%kcP= z*b;H<;_Qq56lFj&HYsjtsi~3C`hkyr-}K650`5{APYB{kKG52A1Enj&HZ$kMo&1{4wz>tlaX2)CDN4yfT;$ zP&&O0rZa;eT%1TMsGd;Aqm9CU$JVWZSQc5hsN-9^5vcI7K#Xkz7Aj2q1fl7ZCe-gmlmYB5l-5S$vmXXs0&cf~x8t07<2!HTwj2M|BXG#?E~ z(r{Sbp@9jq^c2iLHa^CYUE|&hf9RR+P;AWk7Uw1*dGdS^ts1=hgHYq$e=sy;m*}64#nl|%6(tv9{R`8dNb2QZGdXg^@UV_MSsxF#BZ*Hu?U>H@cVX-vhJO4U|5vb_{(ZEvZOn~PaP`~LkO zK~UEdf(P@QSJ8PjH}+S0*6bsk#9eWVPv3J~QnKh&O%Q-_$kW4Uw<0}vHP~7Y!r@jsg;X{kwvr;vN_#kmwr+w$nx6hCr++I+BK*`p$D1azEGV$W` zx#S`*!4A4V?Z0OQx%P zQKjyiy6D}bSumH|Cr6RZT?fjV#S;jQpJ8KOKHxOTZFAuq33a#l0?*Yyw(#=Zwo1>= zcC){{dPA2gwBw^79ns|0>Cs~xF92f2R-#K<;*=;0mtC^l1#(ZLJt^Ev6SNKYrlVnp zyK){B0r61;p_jIZJGy?f7p=G5DR}<8tqFM7Jv5q1;O9q>mA-x$6G28n^BH-bUSCN< z#Vb^xcUoSwS%iKhL>SOgEkKxl@6HSnI_F9A*xXFJrK7rDDDlw1Y#*CmW(FNE#K z|5y^J<4S0JzXrph6d%8@Oh6vGmb!TFLUfe8XI$vpWd>qXSk! z6`4t$I-}M8Ntx44-r_JMJuNthvB^?0bC4W6UG^qPn~H*#Wf(9Sl#|wc|L!IuyP?Iet$Al z6U$<4Bg$+ozVx(bmBt%0RjOrRFhRI>g6*WG0fK0rdu*2yBDIy{U=VZ)yB!*k^CnQF z-F6*`GG@-Y2F>U#cH=W_Pnjz&_LdmxKMqi9uQf$f`c|ti8SJfl+PsIZ* z;mx88e-aHTI{IXG6BZ_$D`~0|O%{KRaC$&)k*yz4H3-QiLx=#>@fXrx-zk4|Xw3dd z{#3uWW!PxG|M3_qgYU|Xj@3PXBja?@Ygbm5zUv8Qc?doF1hWsam)fAiTM6-W4{g%f z8_rvH`EqbSgWEYYCpWr3c>w|`W}})W?1ejv5^k{B@*$@CYe+NI%l?(yBT&bH+=@zK zVl5T5t-0Ok1z2*{Y_wUrK!mw`f>~?C)b3ATwiq`b<0D&CgL5hn&GHF`>1#{2=;kf% zzNN`4zKsJg=zR3V-hKa2j1~9qfUy-9oaFp8X310IN-JZ<8Ps4^fFBKNc#@X@hnIh= zT3LZ>X6LQ*3FgqvUK;R&J*Qy;Mey3i(ZacV3~9Hc3HWf=k8T?|Exu6|`2>3wTKUX7 zUo_MtmGI+={$c+ux4PclxOuam2Fa}8ljU{?xuG@pY#Px`^tX8L>>mtrxhKmYu8v?? z!EY*foCjr&0eXA1ijHf};(62pH3pszaLsi&`ef1ASSy#xqTY1v!6A1mv5thawC&nM zHXlAhH2`iKb`%W9RfO(v8-hbH)0O`|fhc!~i~ByLvTm+pt#KGzi6_&6z@j zNq+a_xs%a-_WfgKX6BI$$j~Rb>{v!c7o8v2L(>K2zCk~_6M>yVf`fNvQ83qNRd9d* z?%fj)_4Etzd34P8`+%iOmgJOQoO<@wmyb<*eKVcr;Gj+1@S%RA3-iGp$W-bgxDNRb zz;H2pIkI;(X_zQX-M$TmA)cnx@xDc*Ip$NPW zx-fTM?&;C{hR`b32&blXm^f~N&aJPeYsPe463f1TR(T*gFz%Tfyd1-1%ezZH(5Ajw zER?q4PW$Q=o1Q^>4<^@VQ4*ni9sxlYdZn+u+7hmDM0PXtj}}4Z&yzco5dM56zq3}e zeMFdPU*#s8-7 zBbsND%Eug`OPu4D#HrdC8^-WyWm!-~oGVs*ub8y&#OTV{I0XLXnIC}i}@%}gJtLeqR*@1?O547lW zHa8d!5M&&r(sB2|o*EiPXdqiqUku%(7R0;DZgvjo3HOqeVLriR9Ji)u?+rT)#eVkv z<3Sq2XS>U3wfCcFZGx}$h->bDZhYanut$lA2mSnscTj0P(1shh$L$>@$q~w+w0nf6 zL1`iE6JjP+Od1V=X)zNMLn0Yf*queI)PfM_0yh}I^V5deNLzR?19ODXBD+kvZe2@> zFLYSRZ)QUQBz@Y!))bE!8qsn0GmlR^x|_K>hQk71y_)uTNx@MPvhD1(qf{r-K_uZw z$A|kM{W|Sr=l%>{eWrQnN8!b5Dxj@v&ULvmjEXyB1ow?j+tz( zop_<}z=0wE74N?H$!nu=s+7xO1WV`+awx)e6JECTTKP$bljam5qZ~^{KfuPVa==2U9FUX5~oh*Q{MzKriDiQv^o=b_8bxF3X^~Y`SbSS?OzLD8@lExUsVsQA2OJUbSD6Gzf(g#*M;@-XV}OQ(5wTq(T{_){OxxO~?vj4_VHIFvJy> zB=txZeugB&G7QFt>d?q1sY)f8lXF$8O`G^xZ?Kn%ab&GxJ-7gv*l(Zdah@w`S0;MJfa9*HC&M}VPn535=1vW1(ZoGUOgZCO{ z(_qtB#=}Q4cDw`E5$o6FyU-)JDJ~m zh~>ld-1S6JrCQ>yrGY((GZjVvigWo)MlAIHcdB zwR0@51tkHE4V{n;Hh+1v`E}JdL#PO)O^#VxzUDC?@|+EK#Md14b-NIY zZFByBE@RVynMp;tN-xt;nA{|rPg9v$=bx;i*-c<@Fyso-1t-hSG~>+gR-a;P zqD%~)t={;VFl3yQ2Uw*z*hFy|vk;wEwe?CoY4IcL`*Dp4^?qNi;@Rc@4{<+*joSbJ zRX-QT|2c5#d@Qf3BJjML*Vq(VSY{nQEoxEu8Cpt}$>@*I`yco!&v~3z)iYhxye_%? z3Z*Lx8vQ@~@&EB!tcNs+cU{$!qAYU53S_kSS?LKN^W&TN>;HlPcXQBMS>S1}HZruP z!g0cve(%0VEqZD{)@D+*8*wGJ8&|L+lvHhZquOqsc% z=e?_8M#lrKYp>HB`tQ2V9~(8lU*oE$r^~{pe=lBm*|ec!!-KiS{VS%Or1;-8c=}oc z=VVnwrM9aZ@E4!dzG>N<(W7EM1aQz;$096n zNTXlBR{6X9rRZTJPt-bR5M$7Ei^A3bkZ4lop-Iir^J3s_OGJb&ZLvdSXvo`4g-rg1 zY0|L3D~d*AGYeKhfq@{=1WPZ4Swx0te`1r066gXPenZT&7irZDO__<6 z-3t(J#9H?gE1al{&~3g%gD!kxj1EHAfuf7IdpFRjn1G^_Zt7{KRP=ecLC+s|{qyn* z4=fv@G;m4DI!dn-ziAzk5uGbC5j=Ed?Pt|3p{M>jS~Qb$|;d& zp@TSm`gFcWr?#Pqit_*XXv1}1rUkU#%b?}BAj29c&<={4at&SES7bFJQ7O>){JGs5 z1oCXjGobq0!!#6O6iSp3k^v0dHzDc>l#OyHK2Yo7OzSrrHF$03U(f#M8+>A3vt`?` zjM?Rk;Oy-0)?LSM13^|SyMx9@)Q#Qs9fgkB&k762V9qRJ;3T~-ffSJY^z81CAbz&= zp1aRB8(K$s+luIkM}1XT97Xqh3j6$bL(^_dZeN|6LZs2i2a!YGg(et%sV5JnO*9q`s)^FadIdI@U^eB0#CZk6A?J@XY-uUG}6`uy_vtyQ#^^hld z!Ojp9x7YWM&RG}vv>D==fZ!bR$)Iq;i<>iI>SJ&^}U1T#8)+*ai z_W8Fd_kQYs+;MRfAoW|tZ7Ulru_RaYSvR#T8`V-dBTa+q)tI8Geu&UED(ft0aE2OS%y;FRKW&p z3gZ_wTFh<6SqFi02#P$zyjzUTY2>>)u)!?5MN{-})Ml6i%l7@fPsF`JOl2jc>1pNo zGrK5m-wWYkJ^tTcmAIKx7$)@8&kW^ z$~dfwIH{Mn_888Yo!uRj6&!Xsbp1K*hflD>uMNW1V6Kt(^NiI0`s2r6H}Bkkj6m`Q z&~WwY)pwD2$MuHFHXhp$8c!ofjq29xPVwZ?_LE_Q%X5b@dN5;lKBCandeg|B*py9N z7SHc&JH?Umiqo%$)2o{{tqTJ*GAl|8hS7PKw#;CkBCmJ#CxcJ(=exUon06ug<%<_s zLQV4i{0WYzxTA>1HC0T_^&^yurr&z~<*(+=69L>Qoq9n2Ni6GgqVJ5_SA1&SwCnP< z=OYdhcy3$g^l5u_M1G@H-ERa|F5OSZ62}=jQqC%5a2 z^yyHhI+FJPjijm)0OzP(-5u-}g{g}HLCnP4ge-9wJ39WLdAP6t(1U{`GA=f}mAJ@u z_;4k9Ij=Zv6J`*nrh}-6Uk`v61QnZ=4KIFG*`}5>8tAI-J*(v<{yTTxL{TA}T=Z1$ zOzafJ3aE782vgG+Y-;hW2b6%ZITDpjfn#TGGF;bt$oSm-ao@k~p_@yjSwj@fDGAUt zTAui(Sv|Q`Bkx~}^Gjy-tosiDg%o*|;DcAWse;NshbD>UxCe;hj0cgd?1Tzro-!Le zNSrWnB0huv5JNxl<|PQxLrn0xXixEL$UEx(Xn1NY>c>Bi|6>Lx7L6NxyPNf0jMqn* zo2QP*vOHqFO-LFS2tY<0#t(2L1+uEvMo(`m?;^)Jo$J5FCZCjb%4x}+M(zH-)ia>M z<*jxzv{QhTTlHVOb^g~na{8=WG$L}C(ro=^T;^KXVRlYO10Q5%r4cv$Pn~nlt*1aP z$+wGb-?%FYQAE@#jxVp*rSKhv;R6+bhNk8=AD?!$FI8>Qw5iyAaY8>hl{Ql!QHrgc z5F_%#GmZS!0#MQgS|U2=q)k1RG26LhDU)aI(8e0c@IpRyl~*=JB~br10v#epLwMke z3on$3>^*BcY3z!LM|VI256qao%fPtgXJe zgu|uYeVP*X3%fopMSg?2>?+ZuQnUHgDdcU7V0yj84%;*&At$d+Q4Tkb&!jgr6O?$j zAh`r^JDKVgw+?X{4&uW5y6T`ItH6rf>e*0ZcJDjsi}pP?Dh4%3eii% z$+>z013nic0F|JCHxL+U4nhfwQ7Bi}n>@(tsb$Nf;}Zb~)@|7m%JD1h#Vh5xc#%jB z0yD4LaeQQvYMBQsJ#{Z@3A!M5KKsN)|-#_Eqq(0h)@84q(A)UrM~y)Jz2-n6w5u`(lt zVDn>;?e3&s)i_eMN5@P@lEn{whpH@RCJG1T&0Ko3=t-4Pw`g91tKoZ6284Xa9WYw| z{n7b_AW32qxso$shTF-AHhJ?p3|#zvkV*B|LY$nb-;B&%xlh8POu&K@%MO1rKHGc` z&*@HXn9-8@t@`5s7H=T6ZAM7GX>9jC=L7ArDa_x z)~E1F8H_EWsq*@341?F65~9KlE)yUECr zR{<~PuWy)a6zEEJd%L!*3?N8m%>zI3lEKm$}Y-|1V<_nrm5Edt{~TYG8AEUQ15lE7z_WcWoP%GE+P}WfN@uH~@Xas{{}O zw+|ZZziAB4j)aAIEDppjEpEjMSS#In^*RKnonZq<&Kc$ec5zrjYz1TaJMm@b6CEm7 zafcA2%B1}23ggHgOL#I>zm1yw)qmtpJr7Zto-*Y!5PqG$Ee4!@S&rr$9Xe#LI~W-3 zqPS6S`>CcF%3nOy)a&2~l^wcP-aN-Y&a+=c`JBR$>TkFXt~wP-P~r1!t~x02a$D$gQK z*!MA?O`up(e;8+h)~$GA%wN2AfMRhxs9m>i2tCXGREY!KKZ{bH!v$cvN}a`#BSnIo zJ=LaDr%f<$!~+i`=?>F zMw)Oc@s`0HRt090+W;d)JBF}xg%OT3j?b&Y!wp?l{sGW#H%CKW>0EpJD#4oHsUnOy z7v%T{-aTlvzR{qEd2c{DgUrM4pLNw`a;H8^FPm3G%VbE`=jAVyuAXPevlVohc#1F% zQ`uRt#Q!h$-aM-3_5J^Sn};3SwuOw@iWEgMWXxQdk_J*4Qpu1ZX`-;nOwu5dsAx_q zlm@djQj&yBl}d&NA?ZBsYVXhI`}?hR&iegx);epQ)?RzCd)I5Y@9Vy<=ks}8&v?ez zS6xb_4?UZ7*uxe*nkcK-TA~1kk+mUFru@3kRD~I!3=UQAI$@AIO7rlizv5m0oSvwx z;?;czb>qL)fuC_vD`HOD9P65&-Wk5?CwT0wN5a2CJS@KPM_KNXlj@E&3Y_Rluh!f- zwB@uLJ>=Cc)@>^bXM6XKy?WJfN_u7XqWmcnCY*v9X{R~SI~H+TlnP^@M644e6lu83 zn6-Kr#--&(0cx}j^h`z;)SX;9tqLymA!1w{)gOo!k?e@@a1Up!ps;GoshBuUegQJ+y}?Qv+R+WEJAfK{B4`JQ|#DURHoRSrb#?=%XF$@Z?cvK!O}-rx+88MUe{ z*-rMu5*8I1{(e#wX6cPCw!!*A6j>S?687vW-YHwI{Z2N*XvWJSjx9)X8pnciEM!LD zN-|{)qpFcHMWZ4N4&j``{!06PA2LGy*qS&aioL_+DWuOO5Ev+X7DpXp`hY?}Icnd= zoT6gyGcj9#x8z>BK)fWq732|N@*cHSD(|`W&7=N8DkVZYTcLKg#X7Y<#g@!^)tvJ4 z^NgV7p4xYLUE`VFr8@Kk!tTpI?^dtbmUGt9Y}G0zJa&~VcWD?STGyAQFcy+H2A}_ol`peU#2iwRGQB5C`k_6DdNTta0SKg$kHjSXdJ@qKX2|As&|neQ z^5=W^e*D6vxzK$x|9Hh7|Bv^wc$8fR%yVXnr*Os-b{U$dKUq*kx9g`rJKB@;Ys-03A*`{64IJ!PA8vBp3#wcud@4sE&uJk768r@0 zgOZ`K#4uR4YR$(N6LZHx$*pG09~N?M5PCvg)UWmhHBnMv&;k;$RvvwyxSr2jTfk7g zE!9cX>5LXxuyjDTZj!*tW7a(uI1)u!&I1tP11$u}wY4qkLV_ueXBCSesg|6<71jT= zx>@^wis`b<%G(ud^Zn4lo+GqTWb$g;y?J5;zv+;HJ8d zCdD@RhElIHc;1|~BGY|03+gR<0g#Gv;A_>H3?VLepxIj;FJ092_+c4t{LTt|1RMl6 zMPJ>R?7~(AyHeyf?Nh6s7cqzp5HOyxqp)_m-FGLv4S%!-SZI^0J22z&QowqNP%O?! z!GEWwve28ilaezfQ1pASmX3ymSb0sYCovO)Y!Uy=B=eS5)YHj2L&`4Bypv`RpFj5% zqCZ71ps0s}fL@nc^U3!9CzC;95qQUwjkxYxVd@D;$7{&QD;igpQg6%Qc;Mi{v`gVS z=caFb+(V%WlX{afDF0YWxwUbHwO7&uzbQMkiI*O$U$L(|zDK-NLm8m)%y@dABSUh% z6~>F&A^+>GP1pwZCY=ZAT8ZYhy-&)FarX`Oc9vF^OV|oMw$CV{{VR?-^^e!VQ+=Ex z3y)gsZL{7%$D^44`UygF!@(ZIrlYJmaQJW_y+%}a)8c(FHL#)DZ*{rWrP6Kcojj6v=;`&40E-oTUAb}N zLh5u7v8;#6VU;LF#?LV?GyVE&R$1vlr$r!2WdIniIve_?($xtKSRv zgW9akWl2jGOt@J3A^hvYt5Z3vbL;Dldy*C@RNw%1oPJqR8K?pw62SguOgQbEB%^zo z5~Ag~-%z$G(%$3`z{(ckVl%1koYP2^Bn}5vro}icnb!0+d(Vvgb2rakk5>WQ$tZk$ zH5NPHsny2qRvUyxEPebhz}E+=vHp3bPe^Td#pFML;y$5Dtnt{z1%jP;8#09 z%Bt|n2wv+Ahym=Oy7n8d!8Re_nUPa68}N4~&k?eg+t%$#9hXq_cdvAv3*JlBU({SS zG7Nb1kOGB|_|+0jvt@J9pY5f_D~RB zzFQJbFGYnV)I0)UL-M86Y1#{gJ1mRU>beugZQ0`cy0|ni<4Lwwk*o9gX?7cQSW}dp zKXbm8MV$pTNGBcWL{`eRQyhtmkHemR*P(>X`|0V2V0-(hmDueyzTIh174-8NVR~|5 z@RMf9-9Go-gh3r2UYOq(T|_EUk(bvluG#n^VHU-Tc>HX^w(Z;N($BNItI{%>5@?y~ z;HfF@=UNoS!{?x_bOL0Z?)B{rFlrm-*8q92Pu$0V!}Ws?tR*W^aXE zBUWsEuoLYGnI&E>BtM))4&_c5`;7l=F z?^~h$*{2T?jpzP`vJ{0mBi&P3m6F>YzwLF&9)}8(QrPml|8&!=F!k>;<+FE}co~Fc z>SRg(iukN^W&Kj7n!8mvA31VFEEyn(b~&>TtS?GF(_9cN;9Mn^1*)#neRotmaH;v- zceR`%3JdLy3Okq^;2|p?t}xZdo{zKhEpLl~x`%*PQ@%XV*a;isH_2f-lW&Dx&C{pj zIaTbb4%ld;aRU~9YPm?cXiU!nejksZ>A9w37ob{!9ubkvId0;$+m~zKFt1AsPqOw) zp}K}`x==w#&>XhdFhn9-q@!IV{~CztrzV7nLW!06r^{*`^ zHT+_|-!(%9Xq!FwCRRKz75-2NY?l z8)8UsB%ELjke!x%-(&eWXWRM&3gCI(B+g`ycte$BdS=&E7Tzasw*UEU^MwkbOi@3^ z!~&%GgQUutjXh_eT zlE^&yz5@Y;t@<9fDT{hd4*#ytf{w?{xTJSD9k_RFW;hzH9JD*0I&~^1WbsfYQA36v zIcJu3g0tsMadEpFNTuw5{Zyw*D(kCfzWZ|58ftnf)qU~t>Wu$ElB1UEeYjhXPoXsj zG!`z*&Og=hG0XqpmKZxq^0fIo&Hx>V6)vIju8Lb7jH{%CkxS{`fv+G1cgD_4<)>kv zcWu)k7Fq!>o*_K)~v9+^9ErdI_GxZeB1MuPG@PwnV|ZT zZ!DeK?oWa8?CtFnW4CWI*xL9NQt<5SKJRFLz)Z>yY8nHtKaN`sWP9EKSs;eXhaesE z3LC$*^mxsIkY2SP3jQRa2dlqD&W-!hIUa8OvjsmkB-v+SYGwiGD;aK*pf*G1KEaW~ zcBesD5U(x%)WAp3cici7tI{Da-fuX=>8TVjX1OTTgfZ4FgyXed)XXP?8J~{zS+QFX_Lgj@W z5T|R<+_o?(XAtP5e63_i{IuC4n;`#=e?Q3Ww;SqSAm&IC#Ir+InA_Ur@)No-;~%L} z(sMr7=GZdbD}z8+Ju!AY*Ln=roTKhA2?l!xS90MRpXj~E-@ca*ivom+SFp-WZpDK1 zkg{iqPo0_BvK@IPqfQTU6}fPe{rdI8Uau$bsF+j;uh28xv!#M%-Q~)Z;gL1zw&V7I ziKkPtYI3A8gIk_XE+4N)=@HzQuq=h|7wqEG%jUmPi4O)@g$LRM$IqG-MJj<>_sS@` z^96=YKvTz6oP+4zqesWlEHWTrE}*Dn`Hos?MNe(*!UN;zE6By+Ly)^5ZfBV}*bU-| zlZ?{i4gI*U>0oGAdYfYYF6v6p4(d5|!m(pf?3zXJLP94&jeiav z^Ldh!H%ICyM1MrP+pr%VIsOw8J{=%;rkSY?tA94zSGZw7VC->qUA19DCJ8-&=IpRM zRVGq#Itvd}fg*FXF3yUeGSD7#lxOTl{IziUS)8cILd|6YFaK>It97GhYd~3ZCNy%- zq@is&xw0XZNIKh*ky;=Ja6LN0aO*;&MsFZERod3puU{X$JY@=Y)iS2;gqA)thPLh! z1lxv9pf}x#-Fo*<<(fRrsm}3d-OV4ZFf%(5wVCSuC@9-w5#6fYN5pCdt+G2twhxCLL{6K}NCTfM# z$EN(y)arP_(#q-p?(OKv-?7lh@7q`fbR~fR5Z^5)re~?(1OjGfS7$u14N)pSpSdTz{Mj6PohS5YqGkq3 zWoe$?cmDwU0d)p>$VAGGZf0g?@ShE{UnIKZg2@RXG%9NmA|PAQHtymzFD4l;PJ(U@|ZlWB`; zPm_V@#BquRPOx4`clHOR?iYZ$Hp2BH&0FQ{$|Uf9iuPf8!QJ@C%m{;0`_rUu#@DwU z;d=1?9cGeHGq-Li><2lk_~Ag3OjZg2`cqH-RY=lDa8|R**~4gya+8G@TvV!cFS7fy zV(-Ltd-K8uDFjL;DZh?C$vO~swgS+&mz|xxqM`HgGmLY1Q(2h_jK}_UzL4L#oyqID zq7H30G<$H{`Sa%s+KV}&E7|m*3EjGPcaO5MOq!2w2^8&jhE1Jgi}8}l(ALHoIc!93 zg(8RNx21Y^Vl9w-?d-z?a*vG7`e>I0FwXW%iF8vf{_x>&OpHzP#*6cOR2jH3#IQo( zwKST%TRe06yA&D}z|S$1mS$()&B|J^@w?!x#M`n}?UoZ6U^;f7@$tWAZF#ZL+RVOS zyI(?81*o}0@xB43+p<*221p^IVvmIDLD56@l{hA`Z7feIWOTBzsW#63Zn<*hxp9t& zTJBV^Tf49^ew!fGyzHw?{62o{Eh8hm?70;luJN@^!m{aANeNEz%Uv%%-MOF3(IfjOj!+4)#TKnyC(Tm@vJ(N)0nZqWp zZ-KP5^j*NNU9sQyzI;-BhkqqX-D3yAL?qhQTkTgj{Eq;s@9muPx zd9-j>j7M1%qUUiNHN+XMmQr-uwsQN! zdV1n*(E%?MU*tX036Nzku8*TaEC#9+p^r?1RH5_sU#n*nMw@j2@(`{BBKPO{L<2O2 z{E}^!ECC&V={Zt!`a|BaNCX+*Mp)n^G$(adwbYvc3dtE%4ZLb*ZM}dxVHofa$T}RP zf{Jiv9x;O3z#eVxbe;YAvO|WrLQ5((7SZw~2379-qtl?y^b~jsrQ+?|f73Yfg_NLd z#6}QiX3P4+(X{Ka^4I6;YBNf24I`^}EEq&Tx;GYe8Z=XkRkjms6MH0w0I|F4oF=fP z7L9$nQx@#EGe-LV zG;UolAxDK*W36)*&Hya(jQ{-dO0^h6)nxez00Os#Bq7pwM#y?un}=y`czuj$m;x3@vIq_SHJ5M;Ae;yzgBrD$kw*3e^Q_BEm zr|lcGC)JXJ<~$jf8vPNUsySju)1gk@TBZHq7726=Dc+z!d6+HMtkMkTMB;jpx)?@z=No@CwLTgwO-F9{gZ8M-mD2 zMXCTKSBmD-H*DB|_UZnUCp}5Wk{<1Xg;1T)taR%*s46uxb3|@7mHAN)FAnP+L3@PF zRsoodYc{Xl(>)!rsF+eE^Nct%D!Z^nb-X{(K4_2Z&xzU(!MqsTDc259liQ>+1r$%|s4j81Uco2dNk~f=+7JNJ>j>fZ}6G77h zDGXR{=&@8`I3C={DCz1!neHlI{Fry181Zi6g8jat$9US0M-54JqL-(52cw--HwYX5^13mJSrfz!Ul}mg4dgoBtu1w&$S1yyGxQI`G&4SfkKc{yOfoIN8K9)LmXbL z4RXh}JL$`aVICC%a1B=NHvnP<)5P(j>#geA`vLZ2#*MQ?D}rk5-S^(;7h|?n8Z~G6 z@`DDoqNy77*B!#*XphYc=Fd0Bs46X5R9C_d9PyCo_7l20?LwZUh?$5bW^LJ`3L+Ii z$ttKEiU4U;6HFUQ1IFGJyXXkvt=$UNg*X~#By#_~`i@=iPd?3YDSx|)T=g<5pJpA( zqg#YrSS`ZZLeW7|Hj3>5sOkxeQCu{gLsG#ovL6DJ^BkpBcF`?r)~d8l(=_M)@;Oy* zhfzE^MYOIi%}u8~RF?bkel~*}k(bW~M(3pkUbukV^YH9|R=<`l9KSBk2qNcFOiTb{ z%g0Zgs8l?KL}3)IhjTlY&+ceMOf)wS^5`^w>PG*=U0a0kJv_MR9Dp5);VwJbh-B$^ zi;4!q)A925z^DTKJ9ZN0(M{v3`!IiBXYU?nw@;L;Pfs<-kmbfIwRr4_AMkpVCWQ65r>SeMhksx!bNg7lCjE4GH9QhA|b ze7&5c6)>0ZZ-jjzz3m8@PQ+=AYvZ0AYmPfYvL`qM`vwf=HHSTdBM`7XAtA#^<8x1^ z31_*2MxoyJ8PZk-sXP}>ZJqActp;%pBncuvg6<4bKZl>+MzJ1Npq^wv?s2fRGX3_< zp%E2@c7Z;r2%Clpx~nOTp>2I(1z(kweb>Hk)VwE;-;?b$E%juN$49!&JKKA4h~AyM zcZoa0mR^=4^TY)lDg1O4!n)REGv)H_iXOLJ`6KX*AfvO({ydkkB~*kA`5Cr@TiYJc zc#*21)xPubsiu>nntuHL3`TK!5a<^t+HVFW6>S6(n#hEH9&k)z_4}?17j;7<^f>&* zb{BO3wM9kTW;?r5Vh)GRCHiK`-`d?^F)$cqlAtf2U)zio;4ZQ_Gb<}C@_t0cxAODH zrSB0Q;;r7pk-A>nD&+Jm3xdX(%TtMy;HyNq`Ly}QZ~v|e36_=`UqcRXLGBMOHUt^Nk`}u;$An93(OkMhu1+@UZN#dq$4b7t4HVCNr2$j7hX963;~-nA7bmW@9(-T zFVm*|+>Wg%2 zH*~PER=-mxPim_E6RTGU_zPI06|$cL?ow?xZQgtn*|<<*CqycD0s@&Ls$IrLoIIDw zQ@9itXU>b!rp<#r2@u~F4Y$CPLGy1?Fzt+$JL~P;i$E**{J&-$T>ZY9r7wY3luh3; zdfd4Dxjjj|NDMq12aa9en>@ghH7^w2GTkjuUvU$!c+!j%^DW+HYC+`<2fqU4Ifn!X zp7K3*J~hd79i47KJp!gp+k(af${O#^_^|-lix7;1XExoFJ&}+uq&4VIF40qzq~CQE&@+w%qjNY!NF zxYBdYr_QaBQhwd`xSSBF+=akEg#giXZr|d{akD316qOwmrDhpk| zspt0loo0w+Qi5dU*7N6+asf(WAE*rqHnX&xLoi&P5P@b`py_mG5x$@}_kuCXMr^$x zx?(8be%9nJq+y|4o=_0s9^4n!X6{v={v_vMXNW<5CoXXEaL82Cb=)e-K)Hub$wFkr z)?ORdJ_+v>m>FZ&@WYl)j`LR&VaI(UFV=rIAJgGHg*Z8tZufmrLVA@}k4mz8|NdvJ zu2bjziO1!Ki5g(UPZJ$R^GI;rJP0S0gNr6Rd#&5kmPG7FmHeRK6H8a=c=veRaMRn@ z_osok)Q&Gf69@c*_fJo>5=V*Kx#5?>)V}NEcIwvV-EWOZ@ z%k#IZj~}n_3FzHprGvqJs*(FLPqHIF83q;hlAUvZ*0@GF4I*Ydc!)@Gc=%%OB#Z7d zJLq0!<`DvL(TeEicCjy!>G^y77yM8?MXSw5Ise%Ki@NXHxcGsN2{-)hgj_SV)a}iD zZ+G|BZX2}by)fRj^ubyaqB4t3Ft~Ko2xgr~C}1UXqrGhTO^A@BDh$F5jsP%hMjS`e zKn7hMyIj)uI+JTTRx@v!CUJ)6cSdY-6T%mqyG^6lZ~Xy8l{OAmSC`Q|t)!ww*?5{3gwE31!czC!3 zj>f@jL(Z|z=|?!ghXpB8!D)%EhuEb+U0xM+9G4KiZz?L%xidP<%t?W6gn9oqede)Y?*iQkVhf)-x(oqQ80U4XSJ&^Mx(b| zSXy38T+R%U%MH(QVYa|iMrc^sH~1+WBV=L1sZ)QDITJYoOIX2hfIZry_(OR!T{dbI*10bd9S#UY<~G7>;; zRg+{Y1)gq))lA2wP;Aa8}q*K3bzD<27*Nw!>meeS3cI-fV|!`gNly9%TJ(^K;uSLvZu-i{{q5prG=&&?0m;747dbqs4rOZoJ*XsCB-=@SL zO`>45Mb}S;U59S{`(q%pgC+t^zaNpHlM*P(Or33*=n3JZ4ji=Xh(SI+aGtF$;T_8v zvh%s;Z)3)cnH8kls^Hl9zQ`>l0wH0K2o4EusMvmD0X4n~T<~k)9z?gJW~{2>xDrXx zPSE|5&`&(QHbO=xlAWu=0eX-uPRdo6&CJN~IDUv5d(bVHTN^!M^V-#`??AM4y+Mkv zjzH3NEXDH$cHPgS#)g3u<_1#+(osP_>ksw*wFj3$5; z*avSR@)5d0@hj|rW2!%GfR4qpfUu+E;^bvzccL*C-9Q{xTcqcJ>8Njh(-z{erQ=Fe zuSsLUT!frZTV~#jBVG5zs<)avd9n~oQEC+PEXi|Qk;Djk2zna%;(5ivILa&5ONRPO*MQX>;qkbojP{(M{Pux4>&|RpSiF|_M)E1mv^e3Ql>qh$|N59au?2L`?YDqdw%O9%5k#J=J z0H;q23cf&kkvNE$NL4^f;NVXkmNloQ=iK(R#P{hP??UaOMitsl->EKsfZiaB`2W2H zlr}8cn!dp&i*8-|0`9|KgWoUiKAD1WxKXTFi=Pqr(cqAYAV5^}y)jHOovst4OCRp< zFdZRxPKjYYqEti1-Vt3A^{MKHacE?4UpYovp{n{wN0fY>JFns;3&x04Qz>2BYxn~i zUObN5#WMqXFI5xdgs2i>+$9eYl<-R5-n9wZvKZq{v9h<47pdvNamN0Jn?%#XG{}1%I~h|--c()5p$Ph zT9ZPqdTYOb5(2qFEjeMg=?gh%%a|Y}@_BjrMZr3Q%zYw#Eq`RoMkE%e&+d8!`iv{s z0>&@&G03Sx*vO#o8qZRr>E`A@r+vsLYuq)awBQKzC1*$K+Q{3y)6MKa3VgA)k%^|p znrz*wCgI?_Kuxdm!tdyrGbTqB7h_}QL6!TbwDc^xNXQjHsLW*+{y2F#z#!~95TYP^ zQ5w+AyO->Tsz}PM?D=jtucxQ!&}4rIIj6XCIxV4Mx&&tSI+^Fp3mHX6h?vYSFoxBu z<-m;uu~KMO%62<_x-Sv zY;N+~J%gBjcnpSwG;KOWkcWERR*0%D{u%1t%V61s>0{wocrS0e zAC=~VpZ-%L0B&#tF(Q+FH+uN*VEMosJCUp1rUS^)g!90>xe+ZLXmV&C@Bc2FA<-&!H`BFUu3rZ>1#ve3` zju-X9?>}4C{+krJ!jxCU#=R^8397jEg~4?1=WOJsc6kz&1(kDcNBvk9R_dh>93wKbA-q94oRFzeVCdKB)a}85tQd7fX%>w~%`4?$_&^ zUhfuubsQ$z4Ce>772aS7Y-(54GsRG8*f1@3F-8xxAf}QCDR4^$cs5Pe!x!ceS(T@+ z?{LcYz5DhljUWH?wJ$Ma0|0<=GwTm6xyfMZ7!h7`S5Vt>s#$c zKXj^yxT?k)iHYFs7S2qRQMEL6<8-Rka6&}&7sG2QA;o}OOrxJMGM-=- z1pl)JOofTaizq(Qx0=@9t7LQ^_vtI96R`mlnQtkE`JpP{m4x$wVD{G!033N?I$}VA z8SB#MOvhpx0jkw&oNkO>7JA4rcv;lUv!-89Oq?|N^NlvbA1iRTBnru|7^g)#9Jl@Z z7BRzCOtl5c$=syxF5crGf1IOT?ZK+(;C1=x_A=l8(E>y=tW8XlM+O^9dL%}#(ZCw@ ztG=l5^8GeCt(MI0B6$`5t$&B6x2wJQqlTy=#p-|^k9BTve8CK)u9e*GVv6>y(DMyt z3te(xLZixma>l|{da-~mFwpn*cG_4L^QiQhjSE%gPdFD7^wE`Ehf50}+O8wdVRnI- zHg?%G++~JMKIRCWQ;?a8706BV;_DmbjVq~Ma z2f_gOd@3z2w&Hf&JA26`O_K8H{8cu#j{N2@4PdLUFxPO7?#+yQ>9;ksLK!{BNV!Jg zAI91+zWd_e5pfc0#o|+gme`k=Fx2)y1Jh+fp;s5{-gMpr3#wQ>hKH}V>dyXEg>=tZ z54d>Yg4cInLsFv|LSEzN^2@zuv3R;`iLZ-Ai{D*D}BQL3VajYCTI`d)_>~1q+VK zg^;}QmnW^FnNz-IYq(n@+vy&)GN#}$IlvBLY1gwiL50;&GND{7kG%ddXLRwk+y(;l2O8 z_HhAQ+U;g1o{(dLveMYGSME47!gJ&?hGC}LgiIY?Zz-!qg=Kc2foLTVb1lO<8Bpsr zL*pdxEl~YGV7cqKzh}ISxo5dVSX*MBY7Gm%2 zS;GSL+;)Z}S1efFn>$L1i=_)U(t}>&eh&G|aXBO?R_`C4!%OiItjOF%*9KELX4r8 zlm(`yej@H@Cp{rp4A&C22W-4^zP|nTePt(>GhM+`P7M7gI(zC}@?J50`ff1cK&Fze zf&ZG7wFAJb`lmQ+`A?-!v;TdfuVRhUCVm!^KE;{LBzgm^pC5_$4*kv65l+&=##XKj zsY4#VyNgz~TdsvbJ>xBLDum!m2Z6812rw5rmrpOlCAz)>8R@W=8rmquISEJvcY_c# zs_(`4NN$CS6$`h@O$`Jz1;%9p95YuifFKmfXH%gGD0ZGP))e?CW$%qbmG1DV1i%6#(Ht?FyrNBP_V}OQRi7+aE3sbfy7H1Q55Vd_k>IJu$T4H|VbGeh5(dF?6A6)f|NRk?YuoO& z-%U4(0Ru3?0X)IT|TH(L1)KzBpzn|Lq=YKh& zNPPdtM8rE!8NE0E?*>naBHx5LvR@iyE{an}95?|?mkIVn3?`N{#a534>=;zVqrxCQ$RrcT8d3EQ%2SyIP z=YCFN-A8c%n_ ze+}$QTe|p!D0{O}~57Xdd*d z$0#sssi(-8Ik-$z8<%|>Px2g^-d6JJ6uCeJ7Bs`UcT7k~=-zB(v*|?B$Izcg(3?cq zm3F`xy@L3Pm1|hJYAeY#3pfMeM8YXZ?^iK{z+rfCB`H{cB~zRxp(o4K7ZChJ|k_&r{a*w zj<#(3?AQzN!WQ?oU9yr=%d=hZ0L<9*5G!AvT3UMgVm_(w%}2(>H2&SIKJ0UL%Zqyc zoL@A4vQx)3jT^assoOgI{d;T4tJ#*Q_>)>!-ru{t>8f9qFarNZ887Ug2l3BD)twbb zT@;%t*QRg$UzX&5oOGx?|A&3h|6D8oH=E{vEI{E@6pXXCo-ODXhEE(|2FO6-ko1Kr zcL*NIRGl;5*Ecc_C9Hf)(*;R%4j6|f5cZE$P#zmyTCq#Gk&DUFf-VKR#%m1IkYIq# zT8cDr8ehNer0=~9xg-!ow-1nL(g~G7< zU{&+O+H^+vdq!$wUqSm~7`3f-Q^5fuD-uxbG=wz^C!!cVVugqT)A?Mo$69=u)?oQ4 z>{q0VZsyT@A$%sZ*Ed!yZwzH9m7bRDHO4B_+I8yOhYeR>T!2y)z9ZoI+Vkh@lXHl| zROBPT3Zifq{^+D2#&wRH8zM}u8#MkboytM9T&|lGcAm8b&5v|vwXoGP_Pjs9DMPBw zU>teT^^mGWX{JC)y^nHf2osL=!ZD?wpumx5mzTW5Y44sr#-A=L&EmHrFArU<{#Ycc zoZNb3rfj(y*3UB73AekehUL_uX%ki}wW))TDe!!Su`Hf5V0WPsTUw@PXD{Sw zup}RY^KB&^=ZQx=y|K9{dB?L9mlV7PRfvPa9}tvv*4I=l4sywa7)(9X569a`GbgK$K);p28l1Ce$a8=5o>>Ufa>_qvg(9WL;%UE+C;|u=?CYQ?5){ ztZU&Vhv}e8NNoY`n$xgeb`NBoi)qS1UymI=?17CO-ifP}zyILIJeMq{lm>@5A09p& zM;>9~h(Vm7K{X$#pz)fNF~pM3F&vg zUUezTY20xZA9*rQk-q@RKRtoqaXNd=iWUCEbe{J)gkf5`pkvxUKPZ+v z&>L!KZ0t)ed67U-ku~wt>JZ(gg|V~ZE|A+{(%to%su+Pr11_l^;{}e!ad0E%IlT(8 zNW9wD{qWSr@7c3imq^T3YpTl0fc)CMzYa4om{e}P%1_LF624$plAD^HJ+7Vy)rUG~ zl$;DTSe8)9+lGSE|{MbSOT}t z=26kumHt<06mzhdsCBFRr^!>MynJ7HHELz_l7olEVEj=6^su~dP2FaZ0;}E!2>Qe$ z65PK^86o2p4Y9f^ig;$t)<6Gr-SunNdLvL)6;8SfBTXN$8 zJbWMb!ys*jw!^L+n1nbKD==8Nd#HmCqehKHB z&_g?>s;Dd!SOgW6tX4TSj$_UHL4dv}+jZcT^8rC0Ge$tb36ySX_n)Oq4OwCFjCNin zDxb;pE#Ov>BcdF!NZ9!ZaEw|{%_5_>@PQMyZp<18Va%6(RbJTcP-z=v35##4U z-v5Cb5rXGXyfN;7(!S`upEgqB_L;K%8c0+KXUaTMK~XR>QcyU?Bjc+;G$k;M%u$g2 zhNPSxUS3167G#mnr6@~#@Zb@xIl>oS7_TDn$%pOGkFl6<^MSaK$DBE=S<`Ocp1~el zj9~+1Y7OGkb!^KYL`Z37r*Y_>idlp-EF3w~=@^ZTNwo%|F$MU^-N~YgD9y@Q;G_PIX3it-MKmRtkcUoASZcs?-G*Vijwi3yh6G z!K-`y@uM|tZ7Ro!&3l`V&(&|^bLh63b|>lY`4NQw*&r_&OBd4}Ke7_0@`m(p^@;8Oi<=rx($+jx?ymJ>{@0cR}=p&bq-z2xs*7+5{S9W|au&eXu^A7bv3 zQXk>uQ9Y)ViEo9~%Tj?01U7&v&xQ)l^s(RjiR!bkU*F!n*U(q~6n@&hz!{!YIPAHt zMhDfKcFB84{3sdI?iY37n0-q(?A?Gt*7`8k&?r{lU?rvWbJ>VhGpF#9ZPxT1ZE;xr z@6YH4W<@5UrMO*GROGh$XG&vTW4~vxAFDJ~QOX|5NXsM~qiwV>wFA}ZS_xRAR%nX; z`>*^F8(de+n*2M5`54m4bvVxFto%v#BgK(7!(DioNvluQDWFdCzf3iyze`k;{%Z0Y z*S3{t41xquJ%*gc6`jSNi#>G$|W1uYdEj81h=7o#j+(!PJ#r#aY_HZ)dO zS9cG@FD5XfbiurN9(tcAd&GWvk;n27Lrvwh2%lFDsu`a>E>$}Z^ewq@3aQ> zc@re9F&am9C|~kzAQ)CSFZ_X@k`FG4TCR zEi8Dh-L6Z)8MMixoEe3$M>=I&1Ad73QgHFotvbNe2vDC?%AIrB@RGvETPufH)9=}b zxcn_H}swg`vOZdKs-c$4lJ$yXZMK6IISF@S@5>e41-|LeU zs~ivjKLJkHM>(_a3wXj4nr7vsGhV7m1x#Pt3>;~E5p{_9maiX{R*#pJ_0Nf{#`Lde zy)!^Z9DSRhLROe~-TfM0?6oZ8El1Ua*dv@m?lp+(6o(B9Nqs7!86nkMZZfAwacD!8 zBPE}_7KH4GE5~-k3-7Ejns&^mTusV4_+tQxlew~I9~qgYt5P6r(u#_fyJrN+&8=9x zxTUdH&LecV9DRTQv0c>Xwom56f@t_3Zm^!&UTKF}9Fo>_FO@EVu1hHxyr@W`Om&3UkKt6j1Z|9KQTnQw26-l=@m?vtso z*a=GRoIQ4UICeq8OWCI!h%*<`1&d>6&{ad|upGGbwO0x)Ulw58rVF<3x^fM@JF~K` zVk@Sl%U;g(S({(J(B;R8z}-8`h{l`OOoU$)4aQpX`~2mjiwcWl6w<>!A5J7jB;9ca zH$u1LCdV6+F1#4HBIfhqEOZo}&q}L4MPA}5dp>;v{AF`*HR#;;U%UnR2fp$~?#NY7 z-j~nR9bVb*>#I-PYS&MNZ60!-1ylO`;N>IXkJeo`Oq}n4dbJGZSjQTNGsIUtIWTAH z)v!g!vu7-wq7w4nvZY!%Sge~z;cP7hUoiSu<>>J6Or<09%c9B?(Hl*mQ7mM1S>$XOM~Cg-bB=n)xXkg+-W_v& z`!K`nw+3Clc(GKv-Xk#f2ib@ic_!!?s_fB43k6$Dik~@XPlE5{B_{$QFQg0a1Bc%E zBA+*P^P8E%*Niy6nB-2T2Qz4eu~{D`{~=K_eNz<$NS}iqD~@QW+xS7>>%hM2adqRjhC1 zKeS=_j;{~ZVU&Hgjnfu|5Q)&=Qo3`9NWwCr<_=RMB84 z$}XyQBC3V@a3@7AkpKz%SFLs~Q$8kCC{v+gU8JF`tjzuVAQMy^-p|WAytD=TG@k(u zqHsb`nv<{#kE-=f`5E-o8FnGZ?AT<3cMWvC8jtEWJd<=SbL8ici+(xcm|cCLfSg{t zOWO`e;$f+c5dat^Yz=|zQ7l7b3u_x7GSn^kOMHnH6{56djlg4HB=Z$Qg^~ewhS!aQ z<%BD;h$uX_Ci^-Qxk5QOAS9&p?_Z9yb%$ zl#=ebBTksXkPsbf>qs$LY6Al{m^rqWF%Mw~m)60n2yU8sVu;oc625h=yNE&zm^p-E zG3l?;1{S`eTDQdGel5VhM;E82)dMvz(nq;6ShgHEiESoPDa^-fLQiTa$$DJ>0KzbI z?&N(0zV==FDQMxpTNolcoY@qKfxA~c zi{#P_`#DXHzjiH{=jDh)Wt?U9fDJDbml6)B*c?lIEHj*GiXB<+-J`v>q4US6Cr4@~ zHHTFH^-TxRSMqRe#i#4YS~hoeiZnT}vUhQ^H33Ix0YX;vT&@L&YIE17>FkMR(^1K4 z(v+Y}c`veJmNsvFh79vv`}nmfOn9hNjIKm+JLc>IQi+qF3jh9qs9kkx_F}4Z!lu`1y=KPzqXn40_leEm$;}w6M4>N8sfJG% zEaHS9K(35@@e!F=)U`bUdfndiFkSU{RkU%QEsdJ-buX!14ps`c5<%0KWPjc9Rym&< zMAR|vLM0&5areh5E-v~}rVXLxmy6~AiiW&?&t#l;+$@eL1Gvxo; zCncjB`7A0=sA5Pt*B8?5m+|(NlQEQ+vAeM861?3Vn-qc3VcI9Ccs-rCcG=F4xTZ+&pO96^ z;qWk%emudT2+C)lGgYKf^7eRPd&kjNMjC}|M0kD)!*=6Z+sc?kapIFX@4G_ZIjthW zC_5~I?bwj3vdv-&ApJXPnEd7G#bFxQDVRKgju4+KK`rSu3WIv|wyL3}B z$NN%u4~)=<`q`h?ycP>ZXX7fAE&aVlB>C|;FQ8=|CNF>Ybpz1hsa=~WifjuvT=m(o z<44V?vuB}W4FJ)Uw!EZ7U&qXmXN;PX_I{Aqhk8wP_6Gp^t)UE>z|nPl3ME}W+gTw* zHZ|7hw_l}8$-a?ZtgB~2>-fW3pswP~0raUQc)9l&IPg=qd30ujf=C36o=`q$Yr_5 ziqLh?;3piOqc0X_f3zM9-!|KGM}dyhCEb_B(|>bff7e3Kl8MbF)dI&6D^XAvnATF) zzSxpJ!m-%Xa4YI4K`0_L$(~$ieB4GD{893`%Lzo8%gRIVSi>?Cr71Onpbne4+7YKS zw`JCP--jTNfGQ2+co$5spZ%Lo0&o|VC@G5qeYBZ=lM7cK#_Agoyhp!&;lSdxWw~?6 zZADGb{N8X_$O`UT54}SIM}99;`*XL-BNksgO1&^hiFq}nI zY6hsxB05!%+u-dOLi$va%~dNJ`afcr7|J7?%S(ofLXOD^0;TL#c$MRFZQ+#&gfM2_ z*$%1#xf-ydDHb5se!IF7gGq!x7p`hH=H`k;FC> ztGFzJzZFLkGCv*WZR-FSBFe6Lwl0eGJ|Fd1#KTi(Sr&kcPo&)J;LTN%zfI>mMQH?; z7DQpWmOM0}_Q|r$&6HF593&Z*V_;)|DIZf?MFF-b@t-Wf$LPZ!$E`Ee%7Nn#qmRD2 zADxD}ELk}%I>z#uoR=Z9-*C&||LoU8D|^q@?*#)GE#rEQ!0?_#AuTJ*fXil^u2BwI zdYtes^jabz7Dj~tIBb@QiO(U#&b{5bZ>;i&LhArJ)}F}Qi&PUbZ3!k`NsDU z7UdZ}>v#lxk7OZVu}beNWH=LG>&>5#1<%pygaoENdIF0}5-@ME#DY63!^Ko+uo z>}degAaDxPQEpSXLW<`hY`k_II`q0;RLl|T!N}5g*(OvVL@R-C^Mn;;WnYC>{Nh~J zn1ySqVccP5iE#vv_+(h?9xqFK2hS6-4+1N*e#f%}7KkUKp|O(o8#8lDKBTxaB5WaX`~>tb_{4InI5Df6gf_9H z8riVo$dQ*?D~2xF+t~jEG(~hwtEf9`g>XI-9kk!Z-rWOw!m+&92c6J{&;&B5kWOY(f4$hCsolH%o);R;_6Au>dg zB-TRBF_)hKp>tby|E3cw;khM>(}2mu`BZJm%LLZ=kf4lfQS(>#(c;JT5O+>Y7Ptj* zcpe&8ymf=iFI#K!SYEz-DM;tjrD>ZT6#m2^lD!2Uq zYb*buSM_#-;8(Cg!^>4SoxqI3PwoNav_%YmKc&J&kV%QJo!VmRh`w#7YY*%($>v$n zLWvsJn`h19Xac(_RYSY&k`>=B(P)S6^*~Q$<6t10YSzQ+&qcIogew2J;2wlDA5H-7 zcvo3j*{vg4zTgfworviz_}M?i=QkxSi7?hh=H%vjdU+YV8KrZaG^_aS z+rgk<7>)LF`cP70kZ0anvQzOEDhu(ov$I7^!Mitagbyye|G}?M(Xa@j8^l8AwfJ;d z8Ktd?gQMeR^ca#&8q`GMOXp@YasIBnToJ-d>*z}Vix(}S%q>Y@;A@rn!|G!GiBYt8 zF8_Y+(T83B@M7>Y{D2v~*|jEl^0yxJ=;~Y97&kCxpDbx=i&!Ig#aGFtO}!a=6NWO3 zs~a|M+||&zdrm|fMU?2qQMZ*uXIhvECB?rVSz{2E_cQ9yp+i!)-B$Ka%g#0=ZI8bw ztFwsT_?bHoScb;cfWEeqPVenvLWh=#5v>NLZ zB1Q1pdAjv3*``~(F>ogJ|5)qi*uX19m?jxP((9Xa*Rdim;4mE;mS^ zDb=KdK@CKZyC|R-@wMp;N~JOcP6og}qPY!vO3R&W>4I=yr;E%0@G|V0_NyP4vb$wu zXWMmb)8%$>c@CeP&?3i?cn(KPPEPoPDu1icx4NG_dv*YjtVkX0o^S3jSMJRP0U44S zkCwz1tVgOYPYm$yE<&&P(%Vfcq%dKEk@zV$8gpyJD-?!I}S`V|!Y6e?P!c%^U zJEvNN$P5H?OlD1^g;CtBU%SLWvQramL*>d&ES&Oa#MS4zcZI0&x$!>%_uz3qzevHP z0PN>cVwxqp@R4Q}}byk8dkAbpisNCqXnED|s!8gpJw<{Hbn%zfZQjK}2+ug-%^D;*7KcONIXArVt zWYs<7NHYYC?k)u6`^MfLS##?*6~PYjE1IJ~wc11ElBK3k%OO z_1;lmQ}#W3^Xf~-u%}m;30^@F=qV>INe~uatE;Q8mrUT-{?crDdK!C2OWl@gj}j_4 zh5JpYyiypNFua4nDdV>U3Cs7Q#$Sf6D>dC&N`ce^LQpT)JE5zPICq{bW*ahe*LZy) z`z}L+X)MXGx%PWa`;vqnJJ_AGz~&7;bsJFETfs@BzaRb}0yQyX{Nw(}2k@JXv&x;# zq>jz+&z?9D00bm2FE79We%rWuIzr1}RXpXo$f({)j6%iKf3l;V?#=Guz&=4wRv|mI z3?46(3J{e@tB8m}$Si3FJDqJPL`s;(TRQ)Oz+Ibr;RyNcw!6{1nF*?_0m-W&d_FXx z?TwJiqLd;xy*KHrqMOtlV8Tu&6%&|>CZ@{!&upwEK-)4*sth6axtSC{Z)u{*`$ zEex#`t9(EW?u%LSG}&*jZeSqT$+u`&RwSojrEJFhE>|>v-(RAg%1kDB+yjr5W-ry@z`S zJrGp1=={))`VMQ+$6bmHbFjR8&>d&=hCvQTAOO4_vzXH;LuGRJi6Nwp887PYQ75RL zE?Tn@_#H+^^_Coc%>Bu5OkH;b1Oyy`)sR+iL^Nl?yeApf>bdWNTF=}JGB*ZyriQw? ziTmgudgiwJ>G~_H3g5ncdxY|~PtQX^nq4@9)fk6l#dhvBaT_a{(O@&9%}k7q4}(Xm zPqN!fEt283nlTbLzcKRch!C^Tdo+rtz78?Vx-Lv%_#Q^IN zstzSiP|x5XK z@m9<$fw{gF9WPLi0wAP(qzj{8ZE^zwm?-}aeq|1wKUml9T-r29UC;_qs-9@&=y&Z~^fP@osgs&B@JmLO=?pOvNtxuNe9w3PQ^1c|tk^ zloPVF|nbTTHXX?yTYjvK$ z&_}s->x2F)=B1k3WmnJfo4io@v}EO=OUcVePaIJ9f3_5FSh@AvEUc~aE*v}k06|C5fVUBjmDm8~~q*H?XK z)%;}Puzh*qo>Y@QRX$mJ!^2nHWP{Zj*Kk8uaQafYk&)58Uo7*T?Cd^v>VqA87+`3U zzZFuBFTeb95=z2VRwZPh7q;^3>f1S*#5CnplX3c6shWjWo&Pq#qw%$CLq7xa>nNnIebjt$v|J(@LdT)ud5G7<^hg+@~>P|AB4Kt{>~E$iOcQR zIi*(S_Z;#cGvDRyMDn(H%_d@CR1FK!n{jeh<@YY{xZP^+bb1x!r@{;7>b+q0#uMIy zQDiju)~z+yRm_<;@9WRN-(Ab^UQa9Fq(D={p}`#mZ>u8XZK!H@rQQu|kp)yoXYj#4 zg9nC^rg@cF7=GYDdCX%n6W)go&Pd6r7tnhyRjG2-szk&O@u0T1`S^iNiShgP?W_HL z?7iL8mFxSR#XM|eWtCsiDg4&N;Lcq+!OQ0>+uJS`6_vpw9_660(qZkS2*_j-ea6Uu z+P$s~;POdOlD6u7(b2wlUpD$B*@@#;vvXS9=Rb$H)p5{|Wu2!zn=#d?4aMroEbuED zr@Y%Mpk%&TWP1d~M8r*>EE1kGIN{0l@tYN^r{&$hX!4xRAhE3?pg31fSuRD#I|ET^ zy~`dJh=Y&E6v3sGjr@u<>#LM3mo-lAOCWo(QIAnz^iBrP^4@*OIyirE+T!%S>%h>| zaY%$ku$q|yYn?08TBK>HrO)hf2E-q>wWnS%J*D}v26!9k>*n*`u*+>_OMFFnc8Uv| zN)0BYH;aH+7yQ~9?Qj*`Cha1nO!KF`D8X7cLCw2eFgMW0o)!70n|PKGIhL;{pV^Mu zR1&_y%~Fb*XP{$KjU=XPDb&+<;tC7Mdl>!fNxb20SYBsJ)S~RNDTgRO|M1T>T*X$F zS#)6keih50+Bi5`^P`t-c9l;MOusq~zPtQJ(@EJar<8esq&Vj{FaXk z!U9How{Kq@fNAuDisJ3@=T!7LIL8(>WV49PRa3i&If)8Bxfcq-TUO&D#@Ae9vr7<&~DON4cRrI%ZoM)he`KINp; z84{id^pY9%tJq*2hZc3(@EH(1J~sf?ZLmm+l-&^?UZl_r_dBN>Dswlc?%cW4P=NtG zX=Acy%cbbxuAB;AC*`u1*qE4^_H|a@Ixq86My|)~Rrj^G&zES&6ycjcEQ_b0uTP6N zUZnQPy)#V_Uc5IVBAMN?S#%~kCg?iyKUwn!f5=y9?Q4D5l$L=Penhc#4JIoQ@zOzw zT#Xzo6yT}iQG5C2m)GSi^~@`R zx^70CdZe2(uRfjQ>Q@OZIxcZ??fu?6@+Tpe6&jP3>!8@%WT9)Yyko<@kcySDHL9Df z-a`g%^N1x6Hd-+|%gYY=%W=uG{7r?BvXZ&{i{%A?W_6Wou|@^ zN#={YlrrKmmj}bWR>y7I*1Kv$$^34PMsL`LU!3dzrgP7GU1yi|3b_+rF|BLHy;AdA z;xJvOHJ7gA(G8+jmtcZ_q!ewmTr)Uy>(-_bgWr7fv-6v=*jN+~95hH1{H&|KI>dSD zXWK=+37H^%bvjvLw{C=`A16Y?h}=y!0|!vW@|kOqr$akh{%%q1g!Pi;EKY`%NPK(! z7#sU{=yE&t29#lsK21@p-)g;HqRE(`EQW`yXg+wVdV51qd3#Dx{pYskkqgrAmrVNF zRe4})Kva5@Z+{3nXZIi_Zjnb=(o=YFi3tfG^3Ek_f-atNacKJwu__NSuL+VIz_b$T*Y9ldr8d3C!0xUD6J`A28kBJv`FP^KpJK zE7|%}zVoXePRnpT(N|yJiP?xx+Fw{*z5Hi%_oA-})-Fl_636 zNMc}EHhwiWO8Ba9PymRQi<5L`bku}e9RjnDc>6!AWx~+jfm?TJQt)XDe+um7n(Kdp zj_C#ICuK8wGzlp{-ahT8{-*PUj8-t%{8DlmfYDnXtqN`6T;ekUH`!e+E{N0TK{YX_ zS5J50EZS>0CGH5P3%q>8EQLKlG#qgkM{&gH@1k8Z>`Ymf;MnPTDZRxV-Cv#uY}@F> z$xEk~u)Ck3pj}x~pw0853grpuw*T z$Z|VX2{?%orL|?o=0a_4;#&wQ`(a(4 zCIiU6pkjNVW&mV-^9VE6L4`eAtB7S;{u5Zbo2v63zgN6C8k2|O+L>ohefa>z%?Cjx zOlYvx(hf(q8t_lEJ#-dpQa#W?xO--ekeM+#qTn+<^j9$h2|WbNvxNnmXd52 z+59>Dxb*pO@zGv9e2eZTfFHX(CLz8!Vt2!u18 z^!vhIit#J3&5Jg~25F>r#KxOPp-ta2&|h=oWSx5TxWM0cn>A^YQq+c`Nm_kwoPx0{ z)lwJ6DC9?5^rC*xI`i~I&c-U#Cv9vTM;ew+I3NHXGpz97y9H13V`9!W=fSC^ax9Ys z9k>!g5_eIPj#TPVL^<|>e<*nDN$KZ~IS!2#YqL+*5~&@#8$0uc&Z1fXCeGZzL?DqT!b2}~v{8dl&)eTPG78+39M*MGn=c;IamBXZD%z{e^T^}opxnpa8<_bpQD(2u%D@;fX$xRU z6aDT`zYuRQS#gcTh5 zX{ljTq-9)Rf7uSvP2IX2;y#V_hD@4?jZXT&VpZ@DyTjq$s&qt|JY+0%Zk-Z zAX$QcDhxI8%Z}vf#GA+A;;u-<6^5fDicbAdX7$nrjCWG(z=W^8Ca8u79 zupPN-1WT;Ok_02~%Dv;{Q6qT}2=7?J%YdeR`s6hH^huf*#y#``kLW}<6zG{H^Z?&< zI$FZWWe6fDSQy!UulHWo0wzwbPxh~U1cRo*a$L9WBqo&0&E9vX;4{s@)yLgP%FcEn z4b1^Z1dEQL{9o}c4huX+3hQ zk;TNBj)x49&h=feVIeIqCe5znsyDJ}=z*w?&uS+l4};!btt!0A#~CJ`oTiO~?qmZ~U0K*->(hG4`ysUnFK4-Ep(peJz+cj1t)O7Jt-2`uv>R{Gr% zTr+$)?x-MTv1RML%;oW;mCkgQaRMVln>W*ExpIzMbwI-iL<2oCQb%MJl9EX2Ec}Eh z7*KGlsxFmGbJVQ|N7h?f!LK%9v;g%A;Cx(_~sP!i67ToO5au+FJw=|q1X3TfNs3cowAbR^d706dt@zhry%*7iaMjo~RCAf)=t*s!|b+xrpN9uL$mZ#Wz z$kb>3M|V%mkUr!&NnxZ4^3AH83aZBroBHGE7{|r8YhN>C-C~77YXowXa{vCi@bGZ$ zW4K-pJKC`i?4MMg^bw!34$;Mq-BECq2X^>z^wj-Sb}YFz&&6+3&ji=*{h;mglwu>K z0D!etsghxpM1jO8L@bdo&?;x?1Sac7sfhmY-yGUtv4k+SGGOHa{rfw3o@*rH$9)-j z0qAci(bJHNZu+rY+6m@CqiE*Y9HbFNuG_KU8yQ&${@ioXb4p;dKvn9MA8_ zc=seqcs}Tl2gv?9;yl15!Aa2VQ{sIS|% zuNt_z$|rS@zLTELrHdCGU0qcpcWmE24%CY3Q_SLcU?o9JGNg!U7D<|c{tijb!o~Ke z#@T!`HHao5VG+SVnWL_NzB;S-9XzoZrA%z?}% zjP0pz&Uzx5z)9dYEA;GP#4b|0@d1X0jZV@UA1VCjflJ4B?(EBqn=&Lh>4X>BMzqyV z(kjUtib0PGq#1ii8B*+I^g&*rq3g^IPElhAjhq8kDfwig%@hGjVZ?h5vKKP4ct{U8 za_%7gq>|EEmm z!4rKlI`c9IJaE(x+qP}fiDAjE@47JQSh?`w$OT<2EYu>SJdVCn#aRp@9_8|v|1jXe zipI|JGhIsDo=S!cJM(cd8?bQtWK|k)jIjexWLcZj@!dz+8(6)P9xmhJMS^EH0?Ksl zH$9wBi2MCA`_lAVF=GvqQ&rzHKe=KpyBv`Aqi#_x4buY-Ao9kmDi-eU*26$wT5E9h zhRkZYcMxM0*dqlP{^=(hp(T(P0lZ04@7u|oELJ~=w(e+h&3dB<9r zqM0@}!Kh6J3>x%L#kLsk)9K!*SWF-Oy?eRRnb4*y0RWMOViYM zEICM4yc*OJ^SgQEacn)}9549w3ZLMEO}$Dz{`aYBYhyEP^C2Cbj)BI{^Bc~l&MDj< zJK?}wpRC;K{jpQ6@iSrOH(j!moB(5H8t9#twi6BMSV}`|?VZjJg_PVObBBL1s{2|y zlzrJ3nHnDt@zioM$hqY*@Yuf7k9koLOA;C*TSD*2R?E-dA!v znZ+S_NL%mFUVU^IQQ!GtwNF3RNO-AKZ8ATo$ijJ6EZZy6Zw!09mkC}I|ANYGvU8T# zs=48)7e<~TrgE$alJz6XI1u|rNQDxdCh-(+sKQm^R^#b-SPP%^ME8qP{6X@>Voim8 z=^cNtfCSe`E>jFRGV8>^0lZ=oi_glrSA{HwH4MaE4gE1Ok=>hRKw=RPeD=K9%n-zh z9uuFF{59)V3<(1zz3HV6GdiV-6P)O|~;2_ic#xLpIo9Ngh9f;VWQHD-kvJ?G+kh8ZSk=F``TIjyb(`{$e)vycaQ|g1~qf$Opl3rj3C?X&p&#|9!%g)`lpso z$XS}A8b7<(v3qs-?YB>;K<2X4ud|3`OsyOf+t78kPU>=AuZaT>M1uO|m#<=bjjOoe zlV$dhV3vC(J>ybm@V8m}W0eK7d>`vN>AgnBSNM_b!tBPJv1{MMF4MPPKQn7EbP?A* zbUsp8cAYesv#D1m59odzHGh-kW@Y*zWhigPMwXEN$@iyE-_(6G@vUvv-}CdYy=I3= zxbx2pT|HghxD`g=>N;OM|3GJF|MuD|q#7mNF>}d0=2bYO&x~8sBNwp9|83UNhx7-) zHidb;LPJAWWK}YvNc_2t}a|iv;opP zkBR-cbuZTEwO`%q9TZhl>@cvglS}V9>oA)0dkURSDXpW7T^Xg*U)+rYLl$0l&J6s% z>D@0zP4J8gf|uykY6=`~Jlhzi0m~CnLU@W*-fOLfI^F=??`8WgWg> zxww8Uep2lJXb=9k+Ez?LaXr3V*X#drx*@y9tu$I$QXPESJZ!$yC~DmhI0GOT3Ydh78Y2ezuMQ={9uLBD8g zK>n?lo}PF9)Y5%g)N`kll=|%ndDRF*1X=n;_;u)1tlCB z7H+q?l{2r{E|4&i+B6g@5S$q2UY+Z^V=&acZ(7JpGDcwoE)idHSMD`1E`Li={rRWZ z^0)El#11ZhSAQUC>+*M|kL1=bf5&r)bv5-n&wb2vE2-a|-MpP<1@*hgwHv&aUnY0^ z{~qrDuHFBuKp9LDrcf$mV-(7DmS&sgzpN8|e>|g zPEAfuCL|?EX=!QQyRh_*NzTf^k7Un7zro(Nz z-E};q^CNGoxnIyJquxHzC8v?1ahb=|5cl+{^cM4Ed`|XYX;R1b*Vns!J%Kt^i*ut9 z$F9Wc`zt9YOc_(}cH#uC!*yj{oNRS{!j-@|CvtShu7}@qJKgt+y1QbpN3af>F2mV} zmWxouAFQi?t){*PThVNuSs9>l!raWPBD+aDL#5mqxXx8a2F69&Z^^1arpK( zi{U?iZF8DxHcA>>JmNH7$#+Wc-Gd8^RwXlrEiEkt6PnbkS?@*B!fV6(H{Ho?zo8MB ztd^k2?KV5%I#Z!jb#oo#)2nap^nAU`RE0|lDRG{OkB5DPd{0#iow~ht^PuZun#Y0}qsRQuHs_&~W4rF>52Y4z z-}wHyM?^0{@k8GAF7B(Z|J<~foB90REtQWBp6Awm>Dl9KNmj#iXG}@UD$2w~S)reQ z+~{Vefp;$$>7)3UN;>pGehtj$Bk0E?kQ9K>E-%!`F^1Z^&cC(X1 zf#+_oHSm+>Ehk}PlRy8{V+XZ8IzXRYyb2eVLoD-RH=_br{vh%QLx5EiTNlI4#vCGxT-n`3Tz#Cd3(bJ>;vJ zY|@F?Z<%*+zeSe3jOfBd6>IbL?+DQ+*j~X_t5UZr!OLeo8Vsb zrpEuoWlmw57b}Hly8W7Q)Qs|@A>D|FN(6q0o;UwdM2j@pN9t{j6pogN89C#u)U$WL zf1E#BGFRKy7RpSPD5E#ntT#T|`-7TMn7?fmzft<=Xx_&@Er)i0bPnzvM=$rgrs$Nq zlc2Mm8_qSF=>J4c%5B^G@p6~|Pgj|bPCug;Q;(0wyq>o6K+Glvfz#M?8Q(UW3J#Jp z8Py_l-Lpe!qe3^ZR+lw2G%OKrZBBh*&G>Kpw2*R(!N5tHZ`}&29;mzsC_QG z*i|1bOm~)^+J58A7td8T6Q9KUrdq9ZuCac9@qsO=dkDQztlIuNqbur-=#&snVPa-B zG(Xw7IF(C|#lQHg>q>&6&G+=O8znbT7?>rFN(X1X7g)z&Cr7<_1Sz?AK?kFfAFnqC z@mXo_+rMAXrx+n7I^{|&h{axVpYJQO`J4Jsc>4aWQuU+zc}x|L%7%wBIJBL-%Wy;0 zvT!)Zb#XkR8dW|VKfKO=xj-K)X6~#1@#)F?MZBmg$?5O*&5SNBj^c2%3*s9Ni+gK! zDI~d);OdvlM8?|YE_3A>uS}T0xQ3gN>fl!@T8)r4Q+&UZn6S~C>I}AtfPNq zTP>VBJ{DZj`n@^RZ+>ZRX+d^jb`tHosx9BPF4wB_NNQbuef^PfdeqFlCv?yDrAfso zkU0GQ;>{K{y@x!z1g72?jy9#~_;#Z*47rYY3y!>ha_ovtB~O$7hbPB^xD6zOLqakU zpS=i+kBLdCEk`1#8z0F6grV%Yw0HedT)JLaFQU4N*8RtA(%tgMs|0PodhPB_(pea^ zDb2DjvnASEHKL)zxI`4F)<5!Z$eJ=0w69)NTgkQJU(L_A)dUPkJs7k@_6uFzF;>pc}wj;ibjkRY;- z-umU;i}6VHzHUF+sw07#z9Me3`VnqZ<-dM*jgM7uaW(C2LVhtXEo~2Ds;{l}Rnc+d zL9LlO6u5JEi-`HuKwTWlM78d9k%a--zMAv2mA7d{o+1f(e1xY|!`){GdidDMZk^?; zTW}xTShLY?_3OTH7fzkImL`$YTX)@md;0Y0=lS{k*pdom4GVvAC(Tx+3pU6(eZ+b7 zh22_DrlzKPxQBqNe6OWHMZY}OW8)Ona=#z6fhEeB~| zO^=1i`$ad<^;snPzL)-b%au{d+uB|2u{eIH>_*$}biLh&sMp0|Js|JNIT2Ltdeo0a zwn}udOVmACN);CdsE19cZ)iZVCw~$Wj)^lm>F1!c&G(Coet0a-7&MetDx#w8WWf7b z_cmvk695PQV0`pg7t_)iQl`E6d!KHiKXYQb)5F7Cbn#bmPKzo3&wq9les^V<-dluT z!ma=Q5j9N;OXms=)XmUyE6E+-=PY0)9cXE>b4k#`!XkgBH^dfT&;V&S^x*Hq7azQT zLn|73z3Uxy|55e>*KFL?{Yjju(qBb8NIv=^$2dK?L5lEWx%D+`0(iH&%K@$?C`tl3 z4-O4o(b0*xbLS4wq#+im+8|IXAjGPKUljY8N$N6*2nm+3C-2_9;|y6sKdSul>b7l% zQ(q5orlc>H#h{GA;p3X!nvbH+-@VbC|L)=bKr}P~a<3z$ON*h_uQycC_v2JZ!0GOg z&UwSCGhGitZ?u?5bd+*^eJd@ij)csgI1O2fFovEr(!>YKx@9BxFVyPvwgF>Vsp>*taP62*J zjM(;8h7j;_3Mg*rp9qudgc%@XH|;qQSyA-~i9kv`aA3r2eOzr#&5M8ZuyO4lL-|jE zS?uLC|9t4?3q)p&!m+M*4=euMu!HO~=`iQ-Y$^wSE6~>_d*f-EX}|Kz4ppEPa%uL> z&R${%3AWsIKP>Rj@Mo>h_4TMTvE5WmO)%d^onK;G1i!xsz5Z^m)Xp@4kvF+{41eQF zkd-$A-NKImmg4-XfvL^tl=e+p(|Qbo)?Z#C3SUT7(~^_c)X@>Pb$>9=;i|2z?d9bP3h+(Oy;1#jorL_ zd=4X=n)VG!nFK=s{#Cx+trM!GtagNu&rEXP_=eIl9(bt$DqZxuOn&1MK9!g5Hvef##+qJvG0 zc9xEJc#)(3^Y;#0T&NMjl0VCsBC6d+Y@GlOlZ0)26j|vM*mqsQA6B|u`Ju)6;>P3u={f&}bM2|hP37>Fg<;uJQYSpu|UY@%JGjPoO2 zHZj!1RaseSVr+aVy}z@G9G6mj7ggfTPHNSw(p6>i7r6ZuTs15ur-e;L^ZEs3Q;zj5cacc&gXfM>@uxt~|QX-CPi{<A0}(81EM zIy|g7)5r)}X<}kh4lZ2V*cf1w|Gjj-lpLFXZK=o7TM@LhBw?x=bvE!t^w!AT1D~~2 zRaA0oMqc;!_GV>evD4ACwzhh`KEmoRiOZ0Q5OE`jFgEt6;BO)?PKZ2>ml(nmt0#E- zs-B+S?9_0`%y=Ic9gSKMfC4%W`^0bJ-|;R+++{-`y&9+Ik=y031T`qXt1j>-?}t$@CQH70FSHES;u zS)65gX0X{Pg2emym)o>JAv2dtrn1uvY9h}yFeZr`gVyGjOs0TKag7#F6I5FF`SVk; zr~}LTV(}e>X9f8!Gz)4ULF4LucAxV@)Llk4YYs1Oxz8y3s7qYfLuwH%;iYzsjyJMG zE1Vy%V{$#QgGJ^ecuELl8U8?RXOla$OxnZg&dJ^kUMQfb)jc-(Er+Rf$8K+{6vg?b zTBc#oUx&R3QTNO3a6Zo0;izX>x^Uz1j~#=b1m;2BA1i8*sFU?m!U=Yk{iKgB{&F#Di z)|V_b$g&MspuXpx~j1`l> z`0@3PZC{uJX9IsHz*NyhtyGYx$D%^@Msm&?4I&dIAHVe#Eh}U*U)u??Bqd}1X;j4y znB|FBR836g7Ib)`YsCa=tNTy0loN=Eh$|d#x=U|!$+sI_YrbZ>7`Pv8LnDKby5l-C zAGjl^c@n9NwX7)5SpBASaWaj+Kk_z_Z6G%q4%R2+qwAdli0Ba-_RY@C_4vl~p=az` zM4BSo%rw@)D}3(t?vh8k^W)WA*DJwdIZ=Pfp=v*W{*_)$75!h%K`!n2 z-nIG-sx(2<(c(Uk2gXJFNUW0#`Po&*)znOCK2g^I-Fn5UOQ%}Qg3esOrmW*O321zp zhsS_OPXtl>xGqhXOcx{MkEWF_Ql*CE;;9z2?7aeZ#z!wdt#J~4MbIVSvO5tH+itX4 zkWNW3F>^VPpf9run(20zJI{_bQUkC}OS}hALf`fxeP04OlKi)apm>V$5c!z8W zCHpMs-#LL>UkPh~RsfNPfHL4?jVW}lO^iZkXhnYMau)q~4cMleU!8?YB?Q{;1hrEj z!Ud=|6PwW6VwP6fgjz%u9k6l1C~icGLq$lO;inOevHcmici41H-97G6$r*^q8y+Yoc_W1RNiC`;3 zC>j95iUWk!ps*YUEwZ#EQdvd?Xa1L48#CH(v_dP+9`aa4O=#yAv!=9XN1Ga^6>o=+ z5J@tybiRiGkY>9&X*M1s5FQ4g) zh#=7qrZl3d_tJAkvqPtw?c1Fy-2l{;1R>Y(3ykPE_D9`4DnS%6dbf#BBnB22I+ui3 zt=sZF)amn!t3g6eHk<|h&q&YEnkb|elA7^m`S&#P35r5O&OKewv~+q)cc+!inl28N zE+&mPK|8p7{``5BdH=aTh?If&>oEh16sw*dZso(7RS0G$0FEiY3X9XRcZ1*asHlS^ z`otapegF8&T1@=4zW}gFplDKc%2yXgigF;{nK> z=LrcWkDJ7O7{i#I!1P6IQlRh|J??{A;8)~PUh9*Av^KiWk@KDu7SY9@K5DoXKW5Ts z2x5X@#S1(w;Y*Zl2}q%7-G~ki{S^0w*&axIGyh#Akriij4}Aa3ugV#iIS?CyE;1wS z4(YS6Omy)&A*ik}1*+x685;J!R(=93#v%diU;4Re4ss>46FLws@2%In82k4J?}98b zmN%&AgfT(QlkiPSEnvfS1c5O1U1t!aODh~Hn4j+x)%mWDW($6rkT%`0j-=%y(sgWd z8`aHvYNfa;Nr~=zbbgE4_nr$-RfvjA^g|MZM6f6Dk|cz3e=>k65si-;8ynw>Kot2yj9h>x zEm~!O-`6*HJ@gOEM*@qGDVveDYj`<@W%iU`+G0FnrKqt*|Q#*{}hWpK6#azW9V6RXb6 z&Fw?qlmG>Wuv&klk8M}y(!SXjF1r|Q-XE0kC3TeO8QiAz@o4uqqV#Sc^nkZ|AUmn+ zr;t(takCmIr}@Y!Dr%e9C3f)aZbSz$D@3`s9v!`#p^)$dSfuLG8OI-Ah%7;LYZ?BY zCxo^`A#@mf%jJHOG)5vW_RR7Vhho#d)2!r2xc9)N*|en(MC?g(?K$p*o^L}Om+S`R zAz$0gXoEmhb+OVFm3*C{E+hFykM$7uVXgyGIv>cpe`67l)r}0NTz~x_l^{sO&6$WR zq&dv^EdBBU;@I4bB0}^7yXKE%b1B7MAv`Nw#opYdttRS(m2202rsG={Wf6q{a0t!loC(F7Cm@AoWNHZjz4^GN1{t=>iWtr5Glil7TV4Q9ihA>qF zxr8R&r$629FzV_~1Qsjz@euQpnO-ekK5`#tt>ohE=2t&uOp#-KzOR`Q{O>G4;iCMF zzUD7}!NM*#fPw>$ZRM1SQIJ6V6L}b+PAU~N?DAx_%rU5my+raM8Y~d)F|VuP^i-!}pI4LMna9626VxO{5^$+Ls;uouwXusZeJ~ z!-i5vL|Mt0s&+4FY3XJN#@x;m{Y0Xj3G|o=yd1*IVHDwZV~fbIFI96391kISGE3;` zbzRfZF^TcdsFghK_N;p1IvROiMI##>%}jlqNQ1!;wHHn(kI1jD*=*ld_=0GM@Y-J9 zTqQWFm4~`x0FcW+-DXQ_1U*t_=G*SlrzG_fs(>bCyP;{wj4k$Dq#h`q17_sEvc>)H zoaP|KSFT(E=pgw;wE}<>w=Tk$1Znw5fsCT&uO4_B^5%z0?BJJ6jlng{M9r-mT+${l@33cSAiE zXPR|yZ?=Jos^nr$y@G;+9G*Ud zh#{2|eX_2m=FzoJ7pdk%!AUk=L}^-dSA=&j}C;<+rT``-3ZJe@ASa$4|`D5FHIFa-TaoIO#agL6#*_+{A!iZ?EBR>)oHcFBat?HPtfe zzs>8djwqF5)&B`PLxjf~3@(sOa^649`u#}3xErN#++1!Wmg5R%U|mN?NzEH!n!scn zMU|b?j=#xUJW4Kp%8|OST4LlawhxA?GO4*hsb_1FqwW>u|KPzMs>VURf77yKaiP9s zsH36>VTHe|mX^+;lMrQ&Okp5I(O2z7ND#-~?TC?^iSQWje@x@``q0w0Va|f_<@`ui zmD(}W2=ve+O?j_ne44~B5%gsuQu5ejA(o03xbj;>z} zz9{uY$tR>T_rK8OG-^y%ljxv*uzB-lv4eI8e~)1G2MCB=g^q}5Lx6gT^^5&hqG~um z3CVkire|xLDdi)!+t!B)hbWO6Y=IT1mKE>;sOO;k|2@7HPi6*4OPg#eUF1RfrA+Oi zBvUYqAX)0ZkSM&C!oHJw2LXNZ3<>fO1!$_C`qudvw#CwZHJ>qFpJuEs?E~}uT61W_(F{!Dk%PP=I?rk(t z`<-V8;>N9(rmP6^O+=_IRg=+x6|2_jlbA6#|L(eci4%&5w)2?y0NeTq?x8#^Px<9R z)j0?sfPspV63$HKudw*bh27KOFr64*_x5N?`xSVqfF)=E8}>uV6{2DTcEPBv1yMHn zn#0>Dst-V}F;F20kAs9NJoRjG(xp5Nl#|KrkyaNDb&GHx;^8dL&x8dYSVLYBPKFMD zJ;ivE0CER~p1OOK_zWQZI5}3=XW|??evcrkj-dY8k})t02n}WM=%525!42H&1%79e zXHP0-Bwlai`uP6wVd^VkIHyfr;#oc@g`(?aVqsC~wOjis4j~8ugm29owtq()Rlllh zXxP45vwQs=rGEz?<#?Dj-60W2#*|Eb6i@v-`xJm&m9@?0ga%KH87?$ zElYwFPfL@O?8XnImin!5-5oxGlk*Gr2|o2DIJ& zm0#~2{yhRJXODkCo1@-|9M8(DlmEUf*&6@&_ui9Fth_pX@b}AJw83&9R9tJ{`nzCI z5QXf@)wBPdFV)RIzUzK7xO9PCC2ZChsiq{Jx29CN5@$(v{9XPizlT%-$ozE_|Pl#ci=j{$5N9JwEJx=9&2F^PToMQyDoEPY-6IKB$z=HURVz z;7L?Zn|FJS^nto%$h4m$WrBhz3DmTaKxxn!e!S~j;T8(zR~~iz@#Q_DIvfGhcgE0K z1aYv#h`JVmsfzD@g!Tc|IP7Fu9mxV&?&22F`5*H??uved(E4=4j-wDeq7qNdq@olD z++pydoXsV?&>;FD?*|NdWlT0|kYSQ)aJxV#uiZe1nu*y&-vOtfzy8m*jl!i(+AeVF zl*>V?Yy6{E$sMLnLt?6htOA)gis`u8Pp&wA7Z9PRvi zG)QJ@&^ZCasTjSa|p2$au>PH67K+}8WZbGL!Gv)-GL zQ78Ag*3MW>POj_i0H%Uu%0a=MhT@z7&B839vu@iaqBs!0lgZiKVU8@A>3wn@Dfo#x z)Y(5s-#Y#7vSz9IhB;8-KvgUfnnK}Xq3!OTQ!k@c?5he3Ay9kj>8so8j4%VTms|f5 z0n@)zP)c|mMYs&`v-smE1E@9jI|6DQ*L}BQLH8!Wt?aj=2Uk-HV#q`V?t^_H0t!OO zWi#H>r}Z%%bKfnc%UuD0y&{X3Vz-CSQj^iSKZ=Za5IPa)7&E-@j0IpBGiMKj7knJ&(yK6J~@| z`)5iQhOLP59B5Jr^%w=YyI?3)qkpDzVZ?!iD;cvh2;c45l9_z-#qIxb32X=m%zKPs z;Gp-<9Ww8kmVT9yVJ7N?*t>mZ>~+DAu`L&WGA*4Xl$^gW)Q?OVlGV(_35~$(t6r^R zYI_^8(zB!AZ|&*kP9dva!J|WDi+pxE}t$ zrv<9FiPNWlG{ayF3+c%=fGeb2?-g>ihEAQ$8M@E2<2mp<=rsSrL|q~K7WYTg#Rw)_ z*{VTYY1qj(27(Ex?f(>AXeUi^j=<=UA>LB4AhdjMDE*#{Nf=hZl<1ObnfIQPht@GV zez6H#=h+d5?w{BaS!wRkx8-ml08x zFj8jXG$aXarq~HnYumjq5&M^GI#cUT70t&uPh^W8MGNvfwbF~^bS^DUgAiN5tx$cn z)%nbFkJa5kWLMej&tIRH~~loxx@5Q!u<26i3` z3gv_W;>=fT_ba-v2fVJ66%8YCL?l!)fZ2OrXZ{*i^$Og(Fq67i0|;|7wSgs1h)6#J zA4G2Wv8PY@as%^x>xh0jj^p;ubuPIBS#WHhIxyZ_S&?4GNcD2aR1Z~-#)wEWG$&i) z4s{ecH87^D1j8-s+wunU*rhF96sFtD82ZH?MC=V;{W@vBlrd)Zlu?YXkSPQ<{|{o- zDs28l@*?Z$e>7<`QNb|aczll-?Qb)){Wa1!5MyebWEy1ce)C6&O%`HSkdz`*lnITA zifmNri)iacqH}75UXbz4uryf=FOtc)eP+pz0ey@>CaR--L|SHM$^DQOknr6hbLODV z{I}iXaiR+eW+JQ=4P*&1NaEUmk|?2MFl*;&`QgF!cgSSj>ht}v)Rstz93(0$aq>u2 zC8L#fGJ+iB7RbSuDmvWC__T&S=sj4ZgL6!z)w}hQC@(y>KQgd>2G)8O% zpSk`_CR`{1riB9Zu1CzmFc+OMG(A?vWtg0o!k7+ zvVkcEew#mdL!<%WlvPz#y=T3K{`ljM_qr$Acx$-#Y`XW?U+)W_jp&7khYvPq#z8lG zcId*ryaIX(-+|v%g!08EAt9kTSbIW7NLaW|IOnc7miY>9!^&Ca>{-vcp|c$uHg2qg z5_{Uwn{ZN}eWOS`{uFa@FPXhHf-4N*v_I}Es#?y z)Y+n&3A%V7vosQ@{k<4M4Bg8VRW+4uI9wdZ(HawCeQ^K&6}**GtKdnKmeH5KzPLw^ zwmLaER?bhwaOn1Fq4OSV}e60!0v&8 z1UwOO5#2^*ZfbY~&FI0aSFh4++;|0}H50J)TN`@X{MN6(S*Ve^#tTN4lA_`hk;Pwn z&E~xR$5mufG;)c!64n23*qIc^Nu zm}bkCfM)Z6a(&-&{X-{~dkDowB1t*rv`zOVNPrpuI(L~RHt*+E#JL3Q2fOP_U_~EG zYDh$_=Z}rdQcoc(2KNoW@pRd_;-IyawX>XYX7dDW$N_Y3zgN07HBsjq()GlZ;^kg7 zYw`P68rDU4EGA%F@X~0}xc4p&_2;ObNoGx{8LI5~!yQs-V{3c(zYBT=<1qE$`8)mS z!8DsTDSmzJH3^PrH0ON%`gOi`OYWf4PGkDgPfwgfI{IKE==SYnyZ_dAB#+mkC0;s& zw|RsqPvyeJi-SN2%m-GFthi{T{e7?@$+R^mh13Wr(&yIEGS*`_aLnxztCq$;yzK0k z(JLFEA~xh**L%3%fEQcJGe+^Uu{sSXkmT=65acwC6bTJxL)(l1($uI(52d zWF!^oopd>fd#{)nCATqN{;-(X7sjty+q{ql&Y0kzz~c`6m>T`~V(X9g=SXGsJnJaa zhQt^M0sR=Pn*~Gc9vV8H)W9~-q9uMY`iiac^0_FvKVdTrfGY~DX*M_9 zhi53s*MZ#_08bS2q8a=7^V8IDE7sT}zkr!S8QCkdfd))J0duYW1W2=py&lRo{v?JZ zZo;&NQk2*lEH6%VCK!>#{>R$goSKi|o?Si~$F*u%PP5iAsfoOKm_Au0E&P7c3d)xb z->&>qmA`>cTNqKXF=p6 z>*`b+h4lYQ^Uy_FCykRS@Ku0`9oZ-3(JdnlwbfW^Ko1stMlaJ%L=7#V{=OR1~x zknV>Xkm=en(q5RY`)cLevNFMx-`h{2SUY@>m)nELK!7Al1#+^fWjyzjEGT~doEZk3 z;)Q#3^6A&!jy;!^NL0_pAJdo^slY*KI0ddV892A5NTT)hNP94kSpc&MbfA`Em)r`| zeE&Hf)~t$$FMr?>Iys=+4A%}^L_>+2%iDMFdh^yw>=Sm*3g)v4Plcsv>)hk7G6D3> zdhp<@V+;SSMqh=zT%KtI+Yx5we#}zZDGTRjyUpEDO4HVYa`)KBM>c%IsW``?br8p% zg@JAUk#1p0uJw$!z2&(?0CkH?tLB?#`1nI{M8#PQtPBbZXp|M}-1p|k;I z$w1EG!R=#fjMm;^a{X~rIZZpPXB-w&lvd%51mjQ7)I_Got3_2vd|I@=xOzK)LnFXa zLrm-zZZ_Joy&%DLW^9JLi24X2xpMwZG=c1D&pbUTa76T~68mys0Uboy8E(xzj-f%> z*&=O-n%&4W6em_r&Z?L?A3$$00&r{6P+>5uiz4=3D5dYl$n-WZKAVfWsu}tc{#-gD z6@5F|6fdE+qR^;`IN+iLJQnArr#Z9=>OUzbt487ZoGTT9Tq@{31@=P$JthAx=GhUl zm=>+P4e|2lkNTCXO|~_H=2Ah1VuFHGZRf+H;$kIqCAh(tRZ=xSH6*F{KYA3|-Q6v; znZ{wXBPHQVT*uT?GDsZqSn~mEQGV#^JDA2$1h1{NXf1NeBw6auDlc~K+&O1FJ8>F$ zXE$0YE4rYFE+ybJ_MF+IDOC%7ye3vUWCCL#NzRjlsN~9(!92*xLuF1_k$NDTERQAk z3xE7!47ti7IUVEaiC5yhc`e)Gacf2iG7~Oj>=evN%S|PS)PA!7$r9_ zI`YKPqZbbzJXn?3<4xwujiS0!=s46B`>G>U5EvwJo<4orcbEnj8Y?crn_s;d1hcMpT`cP&NDPn!dmzJ3;!G&%{&(-*rJ-YuBzTU@-_+ZVPfq&V zSy+5ZuAc!8NWep1_wL`XgWhr)rNEyt*Tg2ge!#I5S+mg-_6f{|7XEp%-#|;p*y|<-XuN zGczOMtDa>XbzCLY7*uHA8S5|7=iP3(=hxU-e6aFGtf2fBJL}S?{nj*Tq(>SV68o3y=N?ZN7Z?&>{U3 zdo1@~+Pi5j; z%)>9aE>R^d_2GU#HdKGL$!;^F>hK1n9)Uu)UwtvY82u@|Q9~OK^%kL)KaJ7>=0=K` z0CPL3R}a&lv3}Wbq+4pr^M~ltj1QyB_jGfevJ#nonCjf$1?>l)JU5GSw;UTEx6N9) zhUUzz($K~k$VdY~RP`w4Sq`Jxq^NQgPbhE-EgP3ut`Sjw8X8o#M5IRB^@U8AxA05i zva(M5b84w$L%e(MMb=zPHy-OQZ^^ff1GG%Q^lE1?zisS|k+uX0zoX^X(z{KBbKm^+ zdLne^)|NHv)+K4?+aQhn_3FdhG5K{sA`v~tdoI7uF4w`=UjWD>9@Q*XF!tKpd!~)a zL4=q7`t{44D1FOcspV7qynGdNEwzAm2C?g(Fjd%<96XkSr)&Q}-9@H8y?oR!I;gT& zh?Zvf_3PJTm2}a$um5^&HrSMwhGDXJMyCO;Zx-nw;*j*jlRP0xn} z?PBLK6vA?Sn@wkI-$rJBd@nE8-_sKlF6?58MzD`t|F1;Fgm1|O+@G^d`~uk3zB!sl zBwF{^M0r~v^Bag6b$nq7TwxYvwLVelI4J)6Yw2f#o6W0!?z5WCm=k9GSm{}l>NYop z0(*sUkcEjW5=|Gwuw`n{=^P}Pie09a3vUchV8$a}CQP7tpG0W003wn@0;oF>F@8i# zLp&XpJ<)WkB7k72UtdynGn=10dGZ{i+-6W-XMsTd0|Jf?sLLqjS$_@Tyz3w6>+5Sb zSbq$7P{OyZy?qi|B?$n)gU5%ZGHB0M$lko_eM>C*Q+!9Uiy2HSl;cQ(N6cp;{il@;~Gp_k%ayu;1WRrF)_u zNV@jvNj)2mq7wNu?c(}qaau6DOXcO|Z1T_kBpjImQzDlh((~8;%tJHgE3SCm+A8>F z3RaUgX~w{?gq00=T)87qC<7D&r9u_vtP%i*!j&sKk)yP=Bk?6I*i-@w4YWaXr;jloPMPv48~d;*5aMil3Y(qFnZu*ca#g$?(rW zTo7quIW%%Wbs0bSFAox86Rcwp%Sot_vq%OWy?2`|+VbQv(^{&jr5v#5;l*sTCK;^Q zmFjR|p6lQK0J@Qd3{i)@X1E&Vwogf=Tt4{fi9PzfAw6HeM!}011ZNv;$xfOY?M%Z3 zwl~yWKYHW{Y5u`b)1YyC90Thl9TW?LCFtquIxH(In_pO1=sKd&CQ5Q2kG0AW4@x@j z8*yUE*B8qG08BJ!Gn}3jMwF=0L0_AHAU6FMScC}N$8r(UoQtRnp2ODuprfM79(;avyXl_ z7duZXXljO%#PerUk_1$K@=DkGv|o9*?~4;<=b}CtgOvuv#lZf7_F_phm7`Lsgg{#Gyj-zM~+}=u`DTe>^6&%)-zBWm0P=3CqDH z8iN!js%6Ha`?5!WVukouR_auF>{uYw6JSx?CnU}KA}8C1WVI_1?z0I2oLY$&?(agH zNTK7?Y}g=gZA~DfChr4-?(V<} zeraiG;6n!4>IUhmsv*BcOoZ$b1MtSSJ%q4<&{f;%=7CZ#fXk@o+Zcpzs)bssfW|^( zr9JBdcVi8BTz^?_W)jiFjwzvl;dtPFoPOm``kMXfdMmSJ&#`H3{6a)H?jXVnp>iD; z64C?(lLyNLOQB<8N~^C|EW1B??@Nt{eSEPtwlJvD5OIMHcjngXp;cnCvekBd|G|VW z9$VU(9*?OxE_)N6O81zDIZL-y?6#6y`vb6=r5u zettE?&cH=F$q#k%UQMO%r*y~PZS$JC;db`DUG%>pM+tI>d-(&(l_IVf%^;Lzl6)Wr zT;Iz9P4afJ*e+uPjsUC!P_r-ZtI}5Y0{XK zj3Y$+t^W(em-$;5h2oL@0vGo%3Og&Ji$LPCvo~=%%F24l*A88W2u~+Z%1cNPid9k9 z%rS0$x#wZ@)PIX&*+=SnV@gO;#aYhub$8HYh&()!)YFL~enbKalg-E1K^SB0yFK{8 zsb&Aa?>u{yzz#Ijl4sG;lc-`tI1uW99lOcJ3MLDLPxI`o2%d+Y#i%P$A+_?X&jIzx zxUcKcfhzEr*~!Jl=;`BAT-%mMIIK5{d(&S}4kYybB|jciie~bdmmg|Oxq@ieNk_*V zQW=Y$R#=cFWINU^A%O<0Q&7W`*3(=cP06U*$wXD^76A%`32-LSjq!wd2pg%KPDyXyIf)B!MGTKp$+IIbdiIXM1^?m-@Yh#7(3HS}~jZZ@m>^5O|u* z;f0?x2~bHCMxuA`xk~&-eR+Ucba+vd;g)R3rE~5`mbHszr|Jw{G9wxn~cl zx9C>X8}0VOu!tlCkt|@L$bCGf{eX!o$nvC$m6UGV>1(>b5TmTz+zOBf6;)NA5*-R< zHot%!=W9EhO1mBu)6yvv0%f@4w+(3h?A1;EwqDQ(jq&6VsGA?~^Xbgy(9+wl(YO=! z%Y9oijbw8K?n8AsPb>T+#ny_2msh#Wo1voSnx5Whk{8zs%F8gKm=jGIki5WQG_{P; z0y<8^;Rf8r|tUX#0eo|j%m#SPuEPj3S7^Td!vbwkO8@2 zMc%f$t)h4qcH-dx9$3jlpT&cZ*5+wSNdb5`;{v%JXIz+;>OlaIYqXn05vPSVo8N%% zCqm99I$}>x&k+ger!W&6AEGX10UMd6=}74fLg1jd|<@i3C< zr)OTk^tLhY07FgiK;qsUol*{DA{kpro$!E^Asr#2McgQ3<9J*peWgBkkCWWfm<~MI z7762oi15vW=I9VD1@>k+RsPQ*@+M(A1j1Rj^mHlwbON@bA2Nw*KitPti%&>M!v}SV zVSwj+&fMCu;7Y^jPsUj;ISpw#IzZ?R$e<`Z=X#M$^eDvKd*{uN1W~u_y0FWXVw_AE zcc>_=2fSFcV&S5=`0;}W&jByl6bu93DuBUcTYr765x$Y~GNoJp&#%yvp$Bq7yiLSh zE0N@(yd;4j4PemRy9-8Y^k09iM^EQ$pXu~iDn4}RP!?Pf_J$%zc~}x@tpdC2fBxyK zh!(_S{#gzdrh=)dX+SEn({oM*USP)QTR$#hvu$9JLYOBeBB3QS7Zyvw06TtKU0R0v zn5%Edw>5&oLXLyVOwfLVsr>`ik3$LKnW9CbyMSnH@Cgy-IUyxm_w!j64Lc!0vF^rq zhBxl!RBX+$;DPT4_R$sFhsRewR914htU=JQH-lJW7kLam(Akf6Xc4OoPckIHhWha4 z&3Sy1MgX%#dw~+Xo4r*_D0`DohK=$&k%p)m{U{JJx$VZM6D9rGS~3izrovIJ3ue}z zFVtwa8-pWZf=@HR<|c)^j!8&|o{=XQ4q`7Mh4Atne4j&2CF&t6>1Br$JRWC?e-v1M z6$`L5p8>sPmO%VeSa)Fl^%;O-KQ;vcsu`QMfiivEl6^HLfy5A_{!y*D)(2=##FZgb z5?Myvu=-RjK^El!XWU04^!f=5F0!6EqbedI0@vj6@)DY|63P`L8Q+NkKG-0oDH_ZyW&*#n0 z&(}&{wcJflpA5yfHT8<))OR)5t-3ky!iU6PWjdvW#6X^WQA7!O-gKqhzh^|5$(c+C9z^}ep|3aB9G z-kN|I<1u`C&Me}KSV(0#5NEEyz^?-}OoaT*df>p>(#6@NSlNhFASe@rG@I&+NM;gY z0=B&mx-`)+i5CFI*@RCsnSfG5kh8$`Z-2&K4kEJz$WATjDZVgaQgO2b;0~rOSqWGw zxOX7~EpD!^c37n{E;Y!eWIs?)8^DLi+yY^Xuz*mC1&LdL0TdYIC7*x%um&b2036X- z4~2NLoFyV2S_vf0$AsVc2X05UYlAw`?b_ArXr8XB2xEgdLh2C+%~X0b4B71h6ic^H?&qc9nAOb@0&4lTvQipE9~Fdf#b&;rr7f6(GJnHE(*il^A_4)n#OGpn1f^Oi;({0@v z37#|n8`pd5r&Jg>rciL>*2g3KUlExSdBUC~wxsLbg)kk9#6Bh>qD}ltqF;S`d%vQ(S{8M4 zY5QI=hY@pJmg*0>CR*TY0!1U!mR(`%+rqMp> zaVmG8fiY$d2oRHV{Q2#|+kgIHWoQ2$dtPaHqh(9eHS>lH&;&q8613Zaso}Q#D`fU8Cv_tw&!dO=z(<<7(Rx02 zosT2af6Pr6!NH=Ug(-AmDP$eArs~FQ5LVCcgjl|)SO^AV0k>l^?ChI*Sc0n3*R z8rX&EaR+IJH!x#vVNR{RsYwMViX^=)!=U2-A?@BVlm8KRO#w-6@pE5sk|W37&>g#h zGy?^y0|V2~R=09uFseK*i1~H9p0*E{ANdTutvNMGv84bvf?T_+93IiVb+he4Jjuyt;FvJU<>`VSl0Dl{je;Xo&Vz6}XK zz|73m=*}zMOS{cGR5Ke){h=%$K41rNLr7KyvlGS;zGig@IYdk4p`y?roK2k2?ItG&;cZX z=bJ9WgH=VD1_WN8MiCQ|moCY{cmN$n`#XMdr#90>^KI#6ISRV8OQFaCyB@pfCd?Xq zMva@qhtyyKGX~8ylyj>lbbb^K6hA-u-6lq*?#hiEMgGVdQ;f;N^HoOcpl=1f&vyKI z4O|f3`dQ!8&J)HK%^4TKHKnG##;2x~2{*x5kYMVY_4?}^p0CJLX%=7A@r!>N=3`9j z2oy(93S;N-!|GcN^ zvU=l}knf`h#2O%LX$nt-phJPac4)#Lp-%^{j^Ae&2bAy7p+kzf`yTY~=VYBwl-3nO zW7@TASJa`0*cU29F2YKfi4`x=PY_%J79y|=Ohl4cOAI)zXzZ{m%B(zFynpt^h>;^l zZd`;qXQI7o)oC&HJH%MqYj^-kM6tyj0%$$|!sk;V+YqW8>>#@`m|;c$3Iw1cd1!m! zRPnj~1~<`6;q(GSWT8cqd6vcQt?|2fhbk`<2kI_LCqZ)m`t@t%lAw9_7VIabtj#mK zUhJ2=-BiAn#5*%cu@`IYn@TE_#D>h9bF)`3GSxBBnx|R;uH-Si<~^qf4u;aE*gPM0C*=pFsZMkoS?A=T~+Gc%O5X; zeRI=oRIQOW| z>+-r6p9GYod}pt`WUsv3s!h})5GdV3WP$n@QC_GIbeKfEt$w?XZf#*&6fY>oqGbMA z&!(_{S!;<;qcG5!mwy-DFoD~fD9QSL6NUkJx!u#Ea?C_cg-Ux}D8JM`KPc3>?=yi? zwO2`o$<%aEG>4!S#_#SjbMT4d;gS<2ie+qh!Qv;+w`u$EHirh!&5C~YU62(eRwyVK zoHsgD@eRvBc3*MY{&p)c8flyLzAam>AQs$QamzFBQ%pg~J?Fk-xuZcDg98#EL&Z<= z6u+k=Hprmp7WbQi7+(dPT7G9M%h;M>U&8Z9c;#P0uMuw)S$n(9FUlMtkb!ysKz5rV2GOXr$R z6s}JpZXoj+*f;H_-nZjnh_AtGE9A*fr^*r$#era=J%Y&UA>6!25ba|EhrhXN83z6g zf~u>eYfTrpC%O<4O^ubM1Xnu0e$n5pCt{WCH`9ZpULL9+A{>+~YfH23Z|_8sp>L6Y zudfVO$CGof{QI}f6PX94E5v#VcMn;R*)pnRUF_ik|B+)h5l00a1XBt*PvO;fNO4UM zT8UaDMzN18a6enqH!T)M;HdNjfGwF2z1Lfpjp|Fp#ad%v*8a5 z`RWsESLoSy9bRo&##&VMhc6HB3^|J9NOW4t&7U1{|I^Cb6Eef#&uQ+am~P=KNO{_{%W+_4-F-C0U`$NZkBbtm21ch|D|lq{t}g_v-$Bi7H)56mmP)1pYv`Q&~6U%5#l z#{zGyriVy@cHbtnE-EbiwD+vvk==T|;Tyzw$LMX?C6QwQ7~2dJiBXTh3}l3-#M>?FJh#zWs zqp10k;th4we_H{IJ7b_{E(+E7IJJVWy9?5uZVA%~bm%kI08(Ri6TNAIc;wSAvDCyK z69dJgOm8omxn*P5kv3l|f<@y>;~mR~Qh$C+qgTKrKZ9MM3J>8&D%rfiw>$R+dsabB zA|$#S&3htOEA$Z$5mEm+uTM@B;BL9a?%Rk3RlAG(98~{(#TM?rxp?I_s(BLc_GMku z8?X72{p)X7Kz%2x2^VC(%3dQ-F~hV^z9zyao{%yhjm$9^>j>ubH7v&p)4}4G%!Vvu|Um<5Vw(FRIM*$a?XhunF8uxYhKaULXUP9OTbK2;Y*$U*m z3dX$XQqzcz10W~AXj1i`eYp{lJGVP!GD9-Z8jM}>T(zNMQ{|3%zI~#=K zmkRhRNw$J2P*}$zElRXh>8Muubo6|;q{Qe^o&UM?6X!62ip8QM3K+wOt%WB2GBUDB zNMXWNw`o^?Yi~h?I*+|~nMvbGP*{Z9ey=ssuKn*&v7q?S!Gp@XE0%t(E+{bnD6pmA zh=p&C!GJ~C22o~ucJv5Vz^SGCXD`5SE22o2Fh&zsRieCcGmFL-yi*o=M;~bPIH12_ zoMgM$o0oAuAgmpSS1YNY38A?fwjxs?cD@akQh4z|wM(Eb_18#A43aU0Sz$!dB!M47 z$NVL6Lc5EM+BMeM2-H53np;6n_yzJvRYmRR=7#?3e;FzHdLg}r7_%lq$>G6`_E+b* z56S7`aAGF}->bhX{UeV;m7=Qs=jZ;`ruz8iSMW;$Jdih{ z@0_vy?Fgsw>`M(01#9%&G0S&t{O|4`^vq<>8y!v2@$s7}bjM!vV$-fSUd~CUi7uNF zNQ7X7S6Y9~!h4zY7m%lhW+Ya>y}N>~lJ@x6Cl?mjC9{eB>n^sMq4sd2lEk244Qefa zqtyd>Cr_UqNj4{D3A3Qt#JhKb8eda8vOh+^(5bDsct5u9HBdb2wXd35*y~BL1OM6O z+|`MVBFX?NCchyn4Q#ekr4S@Dhrn#QoqtZ{yh-`a<=V#8?AT@ZY^_#!yNBVqj=6JK zVd#IKi`7LaBHq~wxaq>e!U4^8MQ^qtg4BNdb_%|Me?2yGPo90P#pIsNbN`bX_D?Iq z7Y7SL3l**x>W);9CR$Fp8;ou3=g;%9=F9gq?bUMO?Ej|xT9>~qKSRfT0aaUASU5z! z{o5_NAu+!7k^ObudbRv6>1_Cq%_ur68k-dq07z~_M6hWVaNKP%oWK<6p1oS8&-&wo zI~mHHu;*`!SA8PYXLM_cPit(Sw37I=L1RVVf8Ia;HK_U@!Tj+0{{NJyrcY6v2mVz z8;S32|KrPu&KmchjzoUxfB($?zFCQPYJWU8iM*rpL&qS6@~~HUi%-Abz9C2Y-+TPW zC-D1gKh65@>ivJ*g6Gwa5<5&49R7)YgW`oD3FmWZrfKaYok83Gd&Bz`s8TQMhEdp^ zu~wC^)cE_+&lppbzE>jJvZh~GiD7iM;fm%o<4pR zERl5dpGL>%BZ97C$67O{u^r@pv@;~g-8-$logflWheVywa+X9tu_$~*MjKECkoy5R z+l{SAKjs$+5dFVa-(DKhO#HcU3wy_3{2qg(QvrubZaS)O;5;4{$SxQ}q?%e*@~W|J zVP}aQ&2QMN{%ssqN146AcyaQ~nKPrZ)}j-U^Snl7-OGpeLV{L@sF{}4c*jq84up8i z9=WP*juM`}33Ugf$Kq4Zo?fjVc0#YFrbhUTMHdV^lD<$AUAn3D*Bnd*RX^`8$png{ ztsNv}d8+r<7*j)vE>fWHsZm)_3wOlmMZVX7lxO|zV+hZs#Cok|giz+85rvi<%P`fzB($2!)`wGL;&bY9< zdI07-d#bK*>Ctz3UCN=EMEoNzTa)j3vDWr2ea(^jjvPHI-U%H|&LoJB6&{~7L22aY zh)mJ(d3u62|G39m2BY|%9}}vV3w20*DGXsx$(ARQU!iT)9%TYb@tc8_``?n0mYtt* zK;fLXXMLII5+J!V{6{xuij2g5KRiD~-B-NTuiOKkm*_j!=I7qptQ&Q2SSw%A474>r zR*%bhS{tKcW$$qGs41!pMcmA&^tRhXZ;U8d5S~OUQALHtClyLe;ryl6IQ6NtKM;U? zD@@VR;D0fq(%SzZjg_j!nLv(F? zVJe4E1Iyi@+v4b93PYzA)sApV36qvW)i|!sO){FSiKA^#syQ%QPL<7~W`gZTGXtVs`=gm9crJtLAj&A;UrKK8> zrYm`#;Z1=h+wxHVZMn?@-c#2cG`TGZ@D)cPXDOF95H)I2nu%Okaopgi-+lVD5F(e6 z&y=33~t99G9 z8Ih(dfUygS>>u*urh;B+cLAOY*;ztz>K+BcsqEy*lV7FZJ>@mpN#&@#ZeAmNaOW7i z#;C{NZ~~v_pUzC#JyJ8#?sa1>{NSd%XvHHP`(DKR#RyghsCK=CCeXZudBIGniDnH6 zR>gAVR0EHp-~3l?K+F!A*eS$8+AeBtPY0}Jiytm7o)EAvVaSlNK2WIM^w+$vs7OI{ z+OcQP23jGQ_dT{Ig#dOIT_yt#gf4yc88alPwv1speqvcMNj>EXAx1-n`JO$c#OVi* z9}838IGb-~w23|BrM6&B!mT+6KafUCK%E2BY<)nQInR(B#QJw3;st zn%AAok;c_dwkU2#In%b#q3h-s7yd%;aFJ9zNVct`5_(C-GRb<%6(@4{4DcN!8YFnv z>k$r^*S{YYLn`s!?#ez~br}^thy5Mq8_lt;((RNCGeHN!cm^L1n6j%KB|5&Gg%r=> z5fR4hM%v1EHh$ZRB-?Y~YQ$=>_Eq>2^d-gDb7#*UXZcAV`^mwGh@BTI$I20M6L|U* zyRCrqLzR`iupc_0Ack)*P@~7H)qSAErZ=}nh%L=h;H{T3IU!KPH9KbKFrw#BS=qyE z4(2hnqEej6Lu=rB)n*}pfTuhJ110)pLPZ|}ZTS)~-! zZ41|)Bj1;>LXkr@8@}{%H;S(kWO@-c@LRqA@WJOw*&4e2 z%#U}~{GkBZ+bTalKlA!(F(FG*^Bm7@rB}z!or76JtY8Ke@1P7%f!BY`4p6ae7{}Ct zmW2*xX51dRU>!Yp**Q5i;vM0)*;a5*#4CyHh>cw->Uf!!b#-+^anXK!d)FHQ{v1_r zOY=Gto4tEaOt;_1Gn<1Q4>ERbLqYSeMBW~Qw5CfiV4y4T;nAyCCn3IzK7LHYhk7_H zZ2QfM7f+rz;JN7DzkjOu?VN~VY%e1oD$AuMVG7<$BK!7s8HkGjDp0YAz#}(F|GZum z`(M+r3A8MHGY}wf5nB-v_Xx1$1`UAtM^jj=A?%3@MmsGm9;TE9VG|P?ZY+4y@lfa94%0F-BLE+08HfWFp zHZK&QXM6_{O}G@Fe3zNLW01V36PaI^MEvV#-J+t?nl9|FZy$4Jk$(?F%We|nV$|0D z^n?e{_0+me8#;k{P28TJMjo(YfRjXOWzWLWso2#O_#1)z&U~M72e>~a=oIR7x`l$V zqIr#4;Ra*)nOLVo^d2y9pl(&Hc8YB20QJ1s%~;n4j~tm|)QH+4z8eFghg;*V`rAaf zcU4ucM~}9}(>zQ`$pdNk^QIj68+$&A$T4CA~&1cS-Xde`#TGJ5Y`F z29H-JO5q@Ux;i`yi$dla7Osii>rT|gske3<-CP1W4Az`uU}(MDIrjdpxo;{rBe>7j z(5Qub=-89>y|v0U?`v9YMC>M6h5DWPecQD_+9Z}F{7I*eLc zK(J&s&$xRHlgzTa_3hi2$rmXQ;aYITIm93mpaSrSuWR|Pq+||@Cp;=@;SyWGj}h3j z;m40!I)nOG-Zsm5$eI(BgGWX63*%vt{@lVWR<952b$BRtapf(Uj#T3{7}q2|Ge6Ir z0ouG4$H2g$619%m%f`K*wc3G~3`t}}dSw9|poPQ= z1H(s{6KhMmWU0gzwOR34$jSD63#KfpZ=J601j5ldzob`k~ zd-h0ZJi9cPUGGfI5<$Z^HtAL~2%1S=BYLu$H}E8|+N3s|Sq)=aqmqTxqUAN*P}Ze~ z5yznMh=1>120w)FPJ*e=1pe6K3u3 z(W6W|FL;hQxt0-I;#CukMr%wG`GI7vZBZ-6Gw>H&c^EG#Sd5!cuLuF+k2SBv7c*={ zcH+cS>`w^SO^fdXYuZcT#KpsVG2~>>RuGwSU4Z&zisB)ZCd)POoDvO|v0i{hs zWIR*09+OoUE?wFSfS35_(IKSz=kV|(`W8#R#%kBfl@4Ia=%wWFp4RphcG5hG3+(wP zPa`D2U>8BBwhTKEH%dv>qj&GeC{v6@XiuEBuMpx*GWVTPY!#c%v($z%&6w(v<)LC2 zjW3^!JH2JgmM^EucGqkrWxaHE@n51F9VK$~r(F?IQ4{ZdIE}U9ulo(%A6}ykve)p% z`>}IMNLo(CmCK;QPm(24%A4*(-JY8~BAOS~RYolceAK7X9ww8lY%Cj-hVav7-GF*< zzeJ7o8Q)YnZzh$p^KP!pBBTupJ$go^&59LSdoA1gRX(3~EjVTy0`xlb#4RYBt|qID zV(#3jrM>PZtJB%g2(o+@t-UU?%Zxw;q~-*SeVfA5^M*1vQhP?;zLXO zPD$w?E?4$F9D7Y&-6MYHS*Tj)QJ8)E^tR*vbLV960dtFmZ?=xu+6WPu&kXMrhwV+S93j0zu>E=(m z*H$WGaXmLBZsrz$OU;<#GmpHqJ~UPFY6oEm&#zyzapOjMKc9gzixzBmmt<3K;3G*X zN2$3j0HnK`2iUmxm616&`_)x6s4D};syrGVKQfgd>}jjoih_f|?JWhtjxKag``VD& zpCNMt9FscYAwI#Ld9u2>Vcc{&quZS471Vs{(zShi#nMMtmxlDflPN$wTkQy8Y01^) zqp?J9lRfUAz?5Q^*yE|&Xm)mQus)*_3c6__`AA=_+)8#CDZS|J?z6n@LE9!vIr{Z~ zG3`uRAdLPIP6RP;t4|95TvumI0^&qRPtJ}!N|W<*rj{U)x)*0-CZENPwf`ZTd}l~O>z`HyA67d~yXm@`dWa=QCnu}I8wI6)2cQRfvLaK#qUpr#y&~AH2$ME2 zFepy*tqTdC{>v8va6L>jV8_W)zixrb@hB0B>(k2Imx6O#Zl~+7rAmA9Go$^<@wcZo zH8r)Ma+RXD9g7)MoQ);0ZcGi_HFuwdW zD08hAfcy+l$%57pFqq@9V}9#}r6pSJYPZ+XwA#Nt`4{Q5YKI1obp%pxGZ`zqoFh?DzsI@Yp4J`5GJxocd z2uU71YSh)elitV+C1t`Q!yz#2TzOm&0D@ zwg?Gjp(uv@es7MlHYgj!kctAL~mUc$V8A zkGUM{!c^z?20b5TWO(x6A^>&FZygAgH3NbA@tO#y>+!`(1tVxC+6`h4Ss6ZL;lhQ} zY#UAhz5AD>?Oyk*d%32Ok&(Zid`y~V>f!X0-_NL3wLMg9usXypACJeNeFw)Wrky+D zw6$@4vWMIQ!)mtcb{CEJlo2EV#>PnCT`}QKIcUR&OnH-WKF$FjWa>W*Z&hlnuHR!! zKaMhhp~{mihpCyxk>SW;Gae!^~mdD zT5wMl75hCZaUgK#+N#K7EL_R0p{xoE(WjCbw)PwgQ4#Apf6L|2p;n}cTbs)KD$6%l z-AVA$Y+v^a2Y1ogthu=#7J1}Vj*5JCS9a~H*txQ=J@X6jw5@y5^M_7|`ylU2qaY~= zZ{pbaMGE6LJwLzH_guFdKJ^AYF18Lxs?-eI)7`!Ae7Kd-4(%p%?9}O2LvEgPC?wCb zGmUKurEE&go2v6-#NMWg>Ok^1&lDFYx3P=-r-5t42X0jB>Y1FtT`JsLr}}G}2Q8SZ zYxX%RWvw8lv`FG;mPtU)+gmqn8czDEvDUmtcU~<27I68Ds?8}-=nIUauz=@eAD4do zI3Ebx0MEm;P$fcCFkjebmYZDEFAq}^8OKaG(06zYUi$gU=7zTwrBmiz_Lm-8e@!X* zBw2pglWqeBm<^bIJxFj}?pdoXdK}(RI`Jh?r$}|fr>&b#5#w-Gh=Z;!d|MN&)tkco zA-&s^cb0Y{6ny#g)=&6Xr>O0;$$0v-$~Td0WO~7W?h@a*E_=OVs=SBJ?Gk9YME%i8 zzuOaTs|*UQU%~|o7}IUi8GD*z1#u(#*gTA`>IAmI`?!uzpy>Eawf^$rg8oGt?us`( z8striDx4M^l{u~2BPy*r@c6K1 zQk(CUM8$YDjNCIZg)P}#;Gs;}*1E9tqurO-)p~l0BvIP%PHaikO4<0X@*?UcIPe%S zW5mqiwNft!92qk9@)zm5`r1E66CZ)P{nfospHsz>f^%EExEueq9zF&NaE#v9`k){7uZ?~g4r6LqpUS7x_xm(+ z3zKk@i{!%?E2+v^>gqdo?-oaxXsj`au{c7By82@np+nHxdS%ZCh>@DqR~t}JYEne! zdo&2{N&oG# zR0Y}N%mSbK1gORYhXfNP9oS58Z~We^*@7XcSh?uON7Y>)y3qy`xiX$!E~Sm>E`EOf zc}EwCIs#bJbmDALs!|QR^MvZqm&G3Y!!-NGV5p{dkY&lo!l($%-rhM0xHGTLS4DI> zaqWlA>&V)m$9dnNm(GQc0efv~^v#`tE&#e4Y0zH`oe-=ZJgmpCMbd!o9Ez*hZ-y{a z;)J7PZ}yd1O`;+-(kUehpHZ_r%`zJHpvwRCH=EeJBKu2^t+G6R$|;t8eo@>USR&o5 zcIIU;IVL+hJHciF{Fk6B=vnpeXQTdXmPh)72M0iV%!p}koURTn!i0tg@Jp_nr!6^T z-RsE;{Gfft=qJJV&1D&R$@a2ZFhXbeklKU%t z9&Ut^{xK^&#`e1s_1B}^TtCXkmgb$`ixfng1FI+z)Hd1r%WwebgXCl{plxs`q>=ayq9`Fs64a59n%Z$c?>~dqXNo)x z02CMm+V2pg=Mv-_)JBg1YY8&wH(fcZzMs6Ve&INkuipBl@2?^hwiO&teZ4p@K}1im zVGU@uj`b!1T{3USj&UIOGDtv};Id?6`!gj1<;fmCT z=Vs8HaE`KiNza@hLWm)cDB@HsKMiQrrqesBL@|47#GB~}U$^a#_CH0O8X_wzyCD6p zqhqX^$LT?~90t=L(D;ab@|{-7m|f*P&@zHaJWuAe-@wXEK+Mv3EGO^zD9Iz( z$frIzV@V|vNM;~wwzjvo-=_J>Zfh)f`F*HF-1_68=HK!68yXuUkbSZGE}%EkqxV`! zgmxV}8uJ{*t_NBUyZX!HD&?vKM}YXzlq!CeAmAFXiCFWHQhvnZ^Agl$<2CM0hQH(Q z8$lazy^kcN=;RC_K-N&@?S#i>z?LSPVQ<6d!NQ@ZbBz@dNM97eMZ{DKBnYVV`8+&9 zE!a2z;{E(wo4cJqAb5K?Iyx$=RD0}63k#q{Kp5nR@=0K2|3df8(%u9TsJJu=cS%HS z`Wpws(6c(9gOB#s_0f7GkOoCzi^6-8F1_9ncznt6hs}Yl101JmRZ9pp0alaKoHh$) zg$rwO8RSjc3PLh{k|)g>C;|r|D!xD%Hnv0c6q1np2YNtNS?!$g<_&T3X7WO4DYT8h zRGtN82<7v19Xw{P-FNZGcBBehX|VR?AdW5?YIqTk_5&NudeomhAKpf7h-KX1Po~k7 zFmgGj##&2Ekbjhu<4et7hq{60;cHp`a!x@<1Dy(;%Y!xM$Qmw}|9ae8dBghk!G@9^ zv{QQ$&}#6r23rpI5aXoUwQG0(@#DYx_VvDWDh|4hh%^*pq?pw9K(i3ymnJ`f=Rtv4K-Tt`G~TsKnQ-xtR7*cy(1*P!&&523Cud(kDm>;$IF8LMa>_+d zK=#xk)VWS|k?GsJ_Z)~5A_zd~V#Ra$MV>w^cnTCy&m!ok(f*c5u3frx>9e*SQC3u= z`~$zNR+IP)Q}~dfLkVd!ZrW$2fYoS3XhDdNS}D`L`vd4n^fvh3{CS_H7KS3|(qRLm z={INWu^|6?2o@iN9o}sxi?VbqoK{J!#nb7Rm?BbMEz%cKa8CP&WcT-FWr<*e`b>*S z+w+Ur4aLvS$!y$McIi~-4jq)fqo3x$KO)#{1cpN0^`cwQbv$Melvhw18zR zBtMM$FG)AfiDbR#eN9s^Mm~AqBq@dQGsMY>$SKALIDnvuq* z`#VF;(_Z|+w}gn>m{zuxvsY+q)0MjyG;e;BoHuF%xYzZzT7Zl+hcES9<_-O=tuw!? z^t3_p^70R4stDjG*2RWAc=Kk;gY1mTSZBwi)L>vr$jgj!J^xc*Z1lOUq?4nvbW`Cy zgDT3-oILqTcYC_h&OLkfZMdtf!bbOxS!-%)8o03}!-VL#EccR=yNJZFZi)Rv`^@`X zdOsmE7%ofEcC&@=(MJm|S9ueMhUF)CFX6Nw*2nbxA73U{d)PuIZ2s2t&YU25XJYY5 zZ}6^YFUvQCf75bOckk=-$=`j&Tc8?br}okus+WGv_6A|DK~KD2N_#H4iMj!8*K--; zk^(mOc;qQNF^VNF)jW3Dl}zpLzrHCwQ5Z6$cF1>8C*iffkivFUat(Bp7%81O;7X(Xr8qojYDASV9l<&R&jB|!rt321R;stf*kZ0$t zdtsm!KS7F3<3)+q;`#+Pe zj(ytW2=p7l9T!0=Bok=ar^d(CwKF@p{Pce27v;s<7tCAUaw+3#Ny#rmbcey)9*B!m zW$z+{8JU`1J^asemGw>bmI-(R1`N0n5%K8B6OHz> z{&+y$IfdT(!e{)-n6%DNsSDT*ayfqYuFV!1`DN6KC)kWUqFJ;l_M5V6INqbNSm8uj za&2wAKafM`#=4mZ*XHCl>wY8khX47}#zriydVL$_K-9)mJc6u-hqE&DJiKVhh1u;H zgOPL3ZYmv&LewMEnAd+=!#2m?(HgDjmgLQ)e=MTe#p^w{sCGCmt*S-77g8MHU?{X$ zNMz+Mj+9vQ>qq5Sm?I6`_A8@z42F_bDm}Gh*RG|{)HQcX6&cJfejHG5@}K*+dKIK~ z{2?PTpz9YBJ*lrNwa>{akDq=H`;d_RL~epZ<4tfC-59bc)A`R;T^#IHI6ENO(^ad9MF=vDTc@cbRGBr ze~nqL3ItZEgG&`Ct=s`{pAp8CKxLu_Eq^eMGB(zfmz$JXX=DCbX^&MqK-(+~P(Okn z%4SPK-BJpk3)C<)fF3}EeZheQq|3QD9%7(BPD|Us?1&JB5-Azi0U(fhb+Nw?;{=Ba z6%(|j>myz@(!wd(?VRl1d~?H7DYB;S=u??{D2QO+9+7@Crv9;2-B+wS*5imM3#k-w z{upoHer}bboH(`s7H5k{5fEbSU2DxhQ;FZ z3@Q&_yPO)>h}8YN&;!T?@0?veET;V|x?SH1^_bzk8N<|m1+6EE60MIivO(S|cdj=% zof&575S0F_AZ8o{pd?++u5M(b@QZDyPmk~Q_%a!GbV%I4VIg+Q5-%}-@UQa(?~op8 z8S~k>!XBisat8k3;aoC0nIY&J+;kLLN61pjt^xcu*DF|xPX+OdxB(Wz?74F*+a}D# za-jsX$h)1(b7}D*_x4z}p{TI~Sf^m5<#{;^d(qPY6JTbp|_?<0WeDj~g}^<)ZT4ZmMh zUcNazf7&9$`SUx0v=(JZVl$sd%l&tFvi4oOmJwOmAD=(JpC#8pMiGsJQJdYv79#2p zQ8qH)w!Bx4^U&b-j{v=kFE0*|-CkB$sLsePLXviE6azEi3ncCB30K5wjxqX`u$2*_ z$IyYrbfDUrKVpFF*5KhfV)6(kKL1tW(_Z15xQ|t?$AH2wEK|Ygz~cvR?!;|-Dd>3+ zp@F}w{6VzIsCJMsr!RxctI1U2+U&0%9c;JMqNf8DaWI>>PbFNS1{uPt^7`$)I0$I? zFUC-ED|#aM+!gtOx>$n#u;~<_=EX_fiC-A!&hw384+8G`=fV&i;D7AoU?xn(4Tzfv zyHjw;ZNQP-zDv9fTL5!_76#4X@C*{Sw-<0diZs_9LS|e7M@k8+$(b`A6$}2j?^__T^BL0Fph1-WP|UOj-$L985Wb;ZUD>BCM|&`e6^D)MX{>Sie>yG!A&51Pa zi-GzA`UIEyJ!Zgo5+RG{GZ1LHcTwCGObn)JYa2!?JuC2CT6)-!upWN%rq-7|v?>e} z3ij&TZq#M1golD|%KJ3uVP0~~2bA$B2ER#A)noDKD^ygXUhFGwVUVZx6; z6tvh!GmajEag^~aQZ3lb(cj{5OVpM+MgLXeD%L^a`cXI|6m>8L$}_v_Blm^TRQ z`OImP%{(RUFe!Cb;B#Quoxd?qgh~By*1=HF0dH~Ygd)vbl-tbunk6O^M{O@yh^@gt z)tnYvVb6nQmgYdMj*JI+N69g5?`H|mNYm5n=NuWi+rz=h_TC!0t!%+&3a-->79*W;2j)o~E@UBu+ zeF4kX049)BwN_?FEUGI~{X{m>kF!3>ES!#jUt^sM&tEcezXdVXVNPvIN{V0z6%8LA z{?ei9h`UsK_3?FihX{i;(jB{WX)_2`a>SG;5dvxoTPO2OfIhw1S~Ez#g^&A5OAAkL zhH{^OUw`-RnrBR&>9nJ{bi9P<`Cr<;bTb=D*ms5D)O(Dkx}%N1h~Fc0+i(h~F${(7 zh0opLG=u~W5_(ruG>eq_xu)hlmkMLtyKUQ}U%Dv1^(h-PZQswyEEKJimGWF$G)8i= zRR6e+PccGtzK`?jY+=!~t@Cnt>4k{TK=TH;AVL070Ynbhy}oJ!75@_gmEQQgXWYh)-7st=$L`DT1gyITHuEF~v{E*`F zp`BnU9;{@I+=yDJlm7Lju+Dc@{Lw6TyY2gk(sT6jKQze_EHZ&E5fS5IqBatf^6pw5XAr@({9iU- zK0E*vf54T?k_kW+0SX*}zcX+6YzQvbU^s!lRG{NL>z6iyk^cQ7z)TJnFOizqTs2Uy z7|-i7kB#!DiArJ|^53vi<~w(4N6dZ?J@%B9NlgBW<7aQIq<%U%+^cnPX}7>dMc65U zG{UJ411%v6V&quNbbccfg$MCNR+tPVm}q%mN4_bnK{5^BRlWqAyBSh8whDiI68DjY zzA##vvAfDpv||5-^tdE0G|741q%(g6(Oz;Lwwjq;*1gn)Fd|Kkx{qfLQf+9YQN#|O zta8qFBay#Fe+3Iw2T_aUxdUTyPEO7Toy!4pOLXvSY~;H)`wy}eb52FFBm0*iR6$w% z?Z(Cs9shX10Tuvk)%5&(u%t!Tx8o`%#mXxvr1|u%(LrchxMS^cI$c`|US$Ra{nNg^7cNZ&TLkrFNbj|G?(Qt>Lf^ zjnegN*F;VD^r(+4uu8WjDkt{eUzH&Jk6^1#01mj;1dnv3j!J=`9;gge|%4E4{h@mlL;%-pPU;aRbX4!nT0J#WV5_z zcM{hn4VUslfW$3!3>Ir>vjt3A;cd5Ny9^VV=fd;!^o4ajwN6{vx-=q|M$n@oT!k29 zl-u0s`+~rn zqzpVD_t|H{7ep(-21q=dvCimr`$6LF)9rt?E?cfW7SUQnTX5|STs9C{n7u}`YB=m^ z3i5(@%-Ca-@RYK@6-(upZvP)tidXo1()N7R!kPXGI4Hn%e1)_AE=#%ESfVnl3ZyPI&qG#*CjU$3~EH=y!Muc6vG0 zOW7PcKmy7jd1JZfzh3o>uAT?bg; zW3f$q;(UQh&1w)eB)XWCq5!APrKMHhj7xIye>2&zCBL`H!5vXN!2;su=qvm-=q{#-Tu42&AwnIV5Z!b(ak z#4v^2;%%GOuNN`+_3Px+2^V+S>_^1%CY;kc``3LElOESDTDuq|YFiNnv9%F1n{+qegeN5Uvv(of-L?qf0%(h*MrE7Vz3 zLQkrtz&y%4QOz*{{~Z(-N#HZ{y69f-3aV(~GtRK3Xrd7`mca5Nj)ANOP zC5B(&W1!0K-lK;)k|CqCyE6#yetv%bHT% z?c0-}(;|o4h~_|=v=|LTnrOwCwEKiiyzYVXv7!kf{qLrzW4H@mGRg`3da5`ZP@+yU z08))+OS;rY)4EHuA`JXFsB;0960ncx<7g5&1AE|oKn?FN;c7LOYSBq3Jj$@$XlblGyN8(m1I;noSeJeLsR>B!AIHl z+Ld)@4JdU*Awxe@8=k@Ky<4I3guhA11kBImb*g)O+ZuWaaXbQ?i_P}z-u3^;6C=o} zY_hPWZ&RG-WI+2pl86&+TRZ7-cg+a518ydRz@@lO7ND~;oYXUq&dXtA?Wf3 z$ATj=%%nPA((4c!%2W)NGW!|^<9S`ra7rA}BIMDJBpG&%N6sv%3zX1xw0;Sp=hoKTKEsAVkv16L-1sb=dff-T7r-~k;rtIw1ks3e;ngT@7KCJ|GY1Ah zxvzsemBy>Uw(!J4At1JJ*zwp@D9s4vt)b#Pc=}?I(W|$gW@E8;dA!M?e&WNR&V*~` z4-m`-lpi$c{$v*W?aleIDxfGXx-07f*z}lMTo~ms{G+RI|J~KFg3{Hl1(g#X8TlBQ zmrhDX6{+(Fbd%n*{m#hD_H3n#1mY(@Xtc)JHjl6gELym$DL4UMisCW=+nz1wCW=~0 z*lsux4(1Lortl;ZSw~RRiJe9}9gZVV-eb{d1Th+8Q$GfJPVi$tnZrGlHA_n5H0rtH zdx)St24=AjMMoxM;rr0gSlTmH)qlPk{jJP5IFEf465LZxyAR)&BDNG>E%H?qB(klQ z=x74(7s+)X4r}739r}JDlEN#eS>Pwmd5l&h%W36G5otvfoEK7te|2fGwIjm1mUAu~rHV?D;XWPlGWufe zfpZ)r!Xyp426sPlm$+G+VI_L@>zB+BftjpMQ6jqdIl?4kwO`LJz};oMapPP*r5v0Q zJMm!#%-4QyYze0)X}Cxl*7ziw6f)#~h~=j|K@7~ZZIWAoh668#-f|Xh3wSMdC#!1+ z*`UvQYMfM(=QF}E6A%JevON3#lZNi}G04giIhcP+O=jCHS`%=ol*oXy7vtvO!N#9x+vM`|Y$J+9G7}Ds_e8K^BS#)11UzW4V2bTfI*|-@@T1t7W#2e} zw{jdRm$4nvfFOa{nRzKv)y3 z>pyM%(glOH3Mi6F#DSox%R(x}lm*qzIDlf@8Eu5-BN8L`NDYK`O|-ahd+X#5&<2gppV!+rjc<_DuK(F-p~=Vyqmi{5V~+Vx(|*D7}O%NKOaV zKdGiVV|PdS%bSA*U`L&OZHIw#VQhesVXTRv-Uay3bX1{*dme zjobD$mK1 zF*zye`IZ7!5z5uD6*V0OAKrVLFgkWc)!O-z7lVRgZ#BX7*VNXMQu_#q!3;dXSeG=e zG%^~(Zwh_KMOyVVgjP36$s~TQf!OaA89Y`$S_E&px*ud@^kjIY2*v_gI6E8D?Kz{= zGLg5{(zkgzN%a)1?&gpTgb=TOMePTIX^>~Ak(ZK@;TJTy-P&lNrD%k5n zW+ZNwRQ!x{V?CN9LeYj9vMO>Um{~?(L;}DAChW(=hoc9`v(CwD^yP>xNKiHZ_0vie z{QmyZgueUq(ZhPZFE4k}3KqTclc!GI`T5;USDSHR)KZ!Z@*=tHIwc!$+Rd#CTQf5| z3Y#Ir&KHt;eCao?U45AnN2qyNakt#EWbgw=_!<$ciWmCO5lX)!h`LjoHf<#P_XoKQg^c}@lEv0>=YuKhdeSbf z1rrTde;(!r{bX*8(`}d{F|k2iy=8p!uixd5Fik@ix46N54KQAdIe5yHDO#)d6Dw+Q zgd3Wfx!k*_2BEr<i?6UhFZOx(KI=W{O%;O-3C zXuDV==M&RIi^hv%nQ0kT%#|H%^2>3Nw)Ug9Z&g$G!DQWZ`BX)3@BW(wJSqtjP1%s# zl9QpK+fMbXVCA${+L$!6RB9Teh>pkMEfqWWA_Kl_PJfCj!Ye^;ky_Sj=Jp_QxZZ8nd08^Je1U`c=^GD53pXysLl$L&ft>Pwzd`x9eM^? z>|uuMDQ{cO+1b@5CMNGHDz1F}g=Zp}OBG$v>>SxCQ|>&^vhq4Qt$1)(t6m?6hA~6b zm#8sHd-Yg@SV^EfukbKz4mThs+h9)Ea*JfwkCs9sU{WW%Ly)RxaDtC#LZS#1_}E*} zg|&Z)+dYD}V9R(hN{rfeadqA4^638k{nML&dQ#+yw6H!Z7u7&<+_?KdE3!~V=$ffQ z3nK$SHM-SSf2R?0o~EWDJq+mgke&}QF&mhEzQIx!Wf`e}IrS18b0PYq5vln_-Oy(c z{^Q1vf5e9pFka#rXC$YKvvWs;E@ux}hSQG47HUL|BW%R{N?~QCXjwj4^jsWVviHk} z?o=u-vhTqVGM~qjy+OjxCvV(9{2 zd=J4(dk8N>%%I6RHHFnebfFviZHKO##IV0K+SRJUIM!=D=c1;xgm`sJ|B=62{XLhc5BQ7p3c*J8pesT&} z2kdk+O8csdLqz&Cd9U5)6ceMut4ZBcTuSy9e;HL_X{dg`q%%)xz?;t%L9?&pS5h2r zA@O<=qq8z(6HG^Z^czyUcd9fj0Qq`-PN;N?&|eW#-qWiti5j6S#mA#Ly^9h)D3{*Qt)0*7 zKP!9Y3-A-~m!PcGNP@xqS*aiwWAJTXPhRgW!*%QK|09s#-CFKeB~)jAO;j;N+>~!{ z;2ZTC>v?c1SFg_C5LMSIq)!R0hgzy9L=Ur6yc2ZB&zF$_yLas(Qnmy?E{@{l${kGq z5+?6pmn&=$EYi!`e4v}X3nXiaBtYdbZOL~hvZ?}$mCgC%ii#`74gWpYW06ZUT#%dTKbcSloG`W=Afim zEN6PS(qDZ~9fj>k%&-7`2ZXP&OUEU>KdoO77xdIZ6?iij^bMerm6x|50V3>7XycZX zo4b?h4_4r={^L_VKJCu$Zf}sJKlUf2KLn0m;Dw_B0fSlTr7Xvs^ez;SzYF?UE8UzZ z0N}@}st~qGXkj0o@!^K&#ir#i@X@fk9uRqy_w5(DA0s#fYY0BF_~~&uFi^OX!f)JI zhdP$>O>9>XYPe1PHqnp+U#K`?!sEomqm*%*ugx(b>{Bd?AvoC#Tsq}D7AVkq@$2!9 zuCD8GKk=@1ayKPMkANDWhQ%BUsE18nH9gfGCu!zdBvs7TKqGfhF>f>TdQw61pVh#*G_E78zP`Yro!6?$@s$6nU}D=CY3( zAy?}Iq*c!N`~P~Gbv2hGHEZ#e!Nn$lmKykPNw*$7N_mIqIY?7VbjZ|YA&k;)xco^|Yf=RaNm-pF1RMl*x$pHiNL zy|UiMhW%@Cn}w>2nyQ0VapY`ydAW*wKBlk(XJ zkL|9s?Qs@LEp^{3%|T!H;h55<3XJ&T>$l`&ju>TqYHLNEm0x_~NT#)j_} zgg&TXuVY!iI}POVMTD5*YsmNU_uD; zWG|g7#+T!#hSb*;+B`)@(JNRoFwA;JVV%K%mBF3An(_#PzM5tpl8LzMUl9%4@72O+ z3tQKg9(Pgr5SbY@^P3Y?TCWva^zTx=u!9r`)H2OH+*YJJ9x3d?4zOQ|_RfWwB~LSn zYS(4EM=H|KBDxsFh;;;$pACux8BtGC@3!<^pNJj*pttvmQ!kEki|d&{RX2|Ft;CwQwVehP`5u2$LQamJv24Rtf_x2d z4?4nch4fLa;31qI6_{D%vO=kK+ z#@q->iygWm>gx7~mqVV-+*4Nmcu&}tz|q>;+Bo%eDK3zbSGsAwpl}FBr$9w^X0oE2 z`IW1^V)v@PB7GmF1Yin^ysGM1;;LvvEJ+O)5#mP4#F>{bE-c&>(dVZ14xK%%ygBS6 zwnU$2YX>TKKp$_n_kz=DF~di>6k zQfswmnVG#-tXP5l>}2EQ6T9`=3_c8)a9?&$u0zirJs50ofr>aMeX>s2`dwIMS9}xm zTQ~pITd|_=tXZ?J$6sTE&#bNal*>`r2;_LyaFOBa9tuS#6s&goJuAJ1u z$Ioxn?ypC>T^J{5YY3W+nm6L2_nfs>%|h9j1dMoCo;P7s3dwZHssblfZU$9_j z@3nN<5Vt4syo)G%_1gGvviVgzQeM7I(v;I{;~boS$h)8|3VNA$MM_)WW(!zBbj`%X zh;A3=_9apLu$Ub*`iWZd00Av}7x0oq&CLKK6CSPJ>w&R$jT1o=bAp#BB4i!w zHR2$Ema=7-s_L4e(tsbA}Sdn%<7NJJWD% z7q7^us8Vn^k8Rb}dq_57n3iBugy&kY_y`?K_O&vdrD}Q=9w;ntJ4!jNhlZ!Ba!xv> zb^dDUTNd(69ECb=(xkRGWo$3*yKa5HyNN*rcQVp+nGF1qXyYfxMo`A-KmHcq+|-p~ zVPA8Tr8wT3E%3XwH^(VP#oj6sv@7l!xcAcu}W(Kl5W4>S79N8 zD4hJw>Z`ZqR;`7xW(Rg}0$>W8}eP()AhV~_)OhQ0FB?`U(XXD95YTsvT_Ub~t!yX4E%|yu{oEF1) z!Zhl_=&nB%gTBV=eVCQyP5h#q6J5GAyXb1|DHVt4-MD$vh#V;)z)_Kv$l^e#RyQ_h z$djws??(TNwKoCFvF+Zzugo$fLu4#dGL+0Rq%u^Z$xMSXCNsBE$q*SzA_+;8WXe3; zRAeX_8^{=?6d@X=tLEuChaX>&>oZwrywSwdh7&oS$r|Z+ZUy~=>=Nv-8wmde}8(vXSo#7#nO#X&-2VIYaE{N)BrWf<1IF(o``tq#*TY6y=#s+qOl61p`Xv zX`chr`t)$3-D2dzz;n?@eSdrxalCMSWGIxk!h6#X-g@v#U9)G1;w+d&z_}XqVIanv zZAIV$#g+%pq@Jl6n?JE_aq?XwNzEqaArJ5nK_%JT4d1z!X4{`}W-- z0Dx()KUJjma?6vr{v5^kxdoD+Bj)0iPlrfhi%59kU>+4^C8iCY_&&59<~fG$5V83r zT^BkYOlkhY)^kdr{yLK}<$^LtGWAlHF-^RrXDbLZ5 zqNqBV>uc8GOwfOkO#@Wf)3?Wt1g#_zhy;s`Bkk8PzWLx^;lPZCY_x@Y5N2uKJZ+(y zQd74BG|1kgd2wg-Qkif^1Ex2p4rTlSy97`1(7_hz0YtIbUca~FX5%Q!N%*Fr4_!S26NpU3$TiibT`Dw8p4Vdxvg*h*&(Z4quSz$zws_B|^=7@R;2tWCt~&jcEkkh5_grm|}Q`1zWda!q#F)I8tzsK^pIN^* zS>bc>dqV7dm6U|&A`M8=xGwB#^X`)rPRET3va{E~y|d_mboXFgAW|J38On%Uv+_(N z14Ni@fq~=w@@%W%-t`CFZ+ix6S*`_qZ>+9<*z(kv9vwT@@ZvMQ$?R#Da{c;xl5&A+ zKq+Nnn%AAOdZE!h_a5{cS4SgjBynH5_Kio3fMcpV^Fti>W$u}7zh#&b8S!#fTe@^9 zU2><6s!9L76c#+tZS98LOiR(HUmebk%*>e&O@xZ0_5|Ndf+9+ttz!ssItWYov zBPbg3qm(PYnHBXX#MlfNI4~8m6`*e&fH4xg$xoTp?vJSwQPkYZN|mXalOG*qilXc# zp$)+-aXMPViDETWJ68JIA=_!0-&-<~o!UKf>QveNEK>O>AW?Z2;5X%jl(+Rg2NU)H zH35)$k4u&d%SSR-Z@JU;On^$WzGaR7hHRZf zqc)VIVG}xn3 ztV9Jmib^03^ulgEYM&Epy$%ZveE`H*wdA9%b#bC+XB|5_Bxovl47FY=Qe2|mgVa<& zgodL0F>ST{)B<2e*y?KbbLRNKm4Wps^md%$!Ox_;Oc~Q#(Ytr=_@9-|EZCsvuR^nP zITr!GZ|?D$CxELu{<#A3lCNM~M*wZYqDY2+G2=1w$u)4)ll2Yj3~VVK{Adtr9w7LC zXEK{<%jc90pT2%}1p3*?YFB{CJ5UvIhU#X11`|0+F@;1(uzSHgi}!JE^K`*Eaef6Q^`^u#Yduh=}qqeAz=*pq~Z{;tu$vt zQ|m;V*EzNf@&()_&!gM=WLtz%IGDT!a`$et`KvpFPwKBD#DE_Gf!e=at#&;{;uFQI2b=QbMUQm@{dZ+Zohsv@xk-8QAzvqJ#YN9X|2Ip;QQva{)s-&P@9bOcN#_h1K3W^VK9WG`;!0&!1_MpcdG3`W_PTgfqTjVGi*(k4Hop!Oz=1xp(JIce`$N zV-^-L)G7WH(?nhb9Up&m)`A5;Z!Uc9OnrM8rQqb0hlX$5vUJkkOnX;YXbo%UzGd7o zwE2?{yX9Vg{P?Ty_$bvX(9}QAS)|qCKTV&L3H=m1bxKj&iajw9U*5BL;9w}oKCHE9 z;wx^$T4v!YdbbPP3~_79!{Tr{GXL8iz!Cr;A>whI(0e!dA6~Rcfg9i9y6kU|ML^Gj31?@I?&%B+%~Ro)Vg#Wp0$ej<$7vY zn2KT3eWYz#QTxw^*-p(fzvbdyGtXuJrt;tK-?Ver<+V6*L?brk?D!ZP@XDIhrTtfY zT++&JxJ8W3l%!7w%?jkhSw7DF(Q~~@&h%aR#$j#;jmH+Y&b9fXnGpAJPwo5Z1Yb@iwd6eweX8v$>>e5%+!EfutIwpW>;3QF7W#~--884iL8};L z{TjbN&OiTrIeJ+=eZ~L$8c}O+m)^?1(-nF3NUNC0DL!nWq}<-N{OhGLvzq?7E&seu zozF@E#yp~`>-qP$cI~?k(Y`))jOj#v>Y35d9sj(h1jdp7*Dqr7e|opR|8;?#onI*P zie^J!%SON5(Nc>y{rA!RxyJAVqu*No@1Jz2L4a1%|Lv-Cs(0LE`2XP>E4|xzvB|Up z51%mlb4PNkS1iyW1nN|5WjTEAxr2MPFoq?L*1Dd4ectTXODZu?qNF=_>T^EVZ_pt1 zVPH=E|6E$mi%wismf@^fvlKGnB3Ymh&D~D&P!tkJ-{__?_zhPDCM6Y5yzQvovz00#-vS?#9U*>zO zo49Dg83>yB8Dl$TvKvbxK=?F!iY;6JzB(C7SQj=T6TUCIr;SC;(0?3Vv+ zz2ppm0J%ouq0FT}HPCKZf>vuf>D#>0d|{13I%SBf|z@U;|=phmECKho@aE=JhUVlE=yG zQ5>W}bNH&D{CQXzi-7w)P z=i_heWlE$N}_(e2qeI+VZ^^X3kVEvW=DhCmgM zK2)S{$VMPg^9TBU-}Ry_?aL&<-)$iwAvd?JmIX6sT$I%%GZmjUep= zfF^?jr4-9)$x6T=i#i{oX3yx765ltn z`0^=v{Q%gLP+v$I3L<**!2{j<9-9&YW5$rPbAY?ge8%YsY^)6DK;Pf(^q=fTwn_Tq z6~GqUBu4XWJ!L*_TqBPkmwz?+EB{^_nC=71zj1%`7->n~ZDeH?aC0eSX?^uRKD?Tv zy}f__?qZ`AncXfeT@D*bjx7zK(eu+EJ{)0Z*N!{Vzj>_IfN?V-Ot#kV`d*iqrtolV zV(MWNd*T|A;?XF2g@&4Hrej1O47Rp`wzX6Mb6dQyrviZom^ z*v(%)!IB85K$R?-C9gZzuS$0U7uUB|W3UlDfCunQ5T8P`axyUEUq63{)LHOpAT@yV zyb2(Q&$LlCVWv80yTk0PoG;y&1Yv?P&a?h0x6B^6Q%PS}LNCJ^vf$gAiAkZ)R=94;Ue1=D9qduASK)ElH88#J zoqMFY#WpE@5{`RUj+)Km6Ofz@te%fsSRk2IDpm|KIf-C~3UcH*k2v$pZcJ96_~D`Q zyB5Y*gIS++5_OY>yfB8ywC;FM>L!#HVbRf}Ms8?$;K%GZ;_mrzAShs}h%uXcvC2y5 zCKGs*xkOi~^ic+D^z7*eKs&?Hv9E0?1soVYiz3}ka3c{hpn}n(_Msr625F6{uZSVJ zLPWBbqV{4Id6I}BS!(zV2(5=O8bZT}6-nXt$Snco9vYhcf*Yx*7@&aLSyq$bDOdQ( zQ9RV$aPoi=^3X&p#;!=iCx*66gK5*=Y2UN|M+?xJeg^S{0ho4Nu;6&v&zCbPiDSDT zIcyb!KWdP1_Ns!4fJg#26sX#Y(d2(s&+mBf?%fY0JZ)`x4b4&?Rd0fj6Hw82;vr(> z8BR`us5a?5<^Z^?f@)mmViCBj8&JfNCD7T}S4PMH!+Tb~?E)@xgTsTJp3RK1ScKim`wh6PZm(Wp$Xiibe3JPz!Fjq<2}C?j zq{de%X1SG0VsMH2`aWm_($#1b$%4w_B0AF!wHNe5D{5G7v=O*9htf>!VYgzYHm>dw z1n=uulq>@OP9}$_OYZ@Hx5T-WLLqhKNA|nuvMG_lLPv9g&jqFzaqrcu!{<))&n&E1 zDz1H4ON?f3f%A|Q?2!3bMMc}7^(BtU|6yPjuW5G;6h*d^%ovi=FEO#2*Z^;Bv1ZE_ zv*WRW4+1!%2 zg(zQmZX4+Uzy_wafTYxzbQ`EkcTSWGTS@o{qUJ1KQwHmiU5We{cYgXRR=$mAnTc?B zX`9!V?x3q4H|~C7L!+kt&;=O3j^a&(j->YVZWEvqt{5F|QD=J>QdI?GbBv$tLXtr# zC@BN)nFv<^;3|FzY>Ou+9I6YCj4$;Sm=prQQY>eo9E5ENurExHZp77zI1v3ma=06; zETH9%v`)$hI)gX!BMB3Te(!Ojp}(rd9<%l=o%5{G6#@WjkQSA;0+t|s@!z%UCdIfs zei|z<5x%460RYrzZrXaZkusQvZU|vQY1?-IVlv70?`4}<65IKLOh09VG;r$t0x@Nv>M3r37+Fck z5W5c=4^F^MMimLYX+Vtsfu`0zsOyEU)6xtUp)YMzOuV(Gq;u*mcYAe8{SZ46o z$=GnPKRRJxgVG%u8W{f|8K^=&lCYdKB!d-^UHTP22W{y4sKuS+8fO9j0Y0sF2F3# zx?2%lbnCFJUa{Md)yFyJ07mTjmx98=*V)-&TX*?x-O2#NNshH<4{u{cOAkkUVsf$% zl$Sl7Gfa#`S&+nm#pBO+4KdsPc6^6*-YV7fe*LhCv57o*5C_q9;9=TUn`xM9|u<>8_ zgVcj@XhHSl$h{H&tCTNPNLI|8lpujZS5RdlH+-u%1Y>XJ0O!@d1(k$7XA`+Ke4q!T zD?J#IAfhnJW#KTRu4l6xh1lWbvG@|a*Ry?znUC9}w350Z|D`DGWH|s03EBIk1kWMT zY?5{kfh%D64$dHP-kf%Wi3ww9aDhSUOD;9OvNc?U{Dsz{*zScF=+Yxe!Lcdmlnk?~ zP0cF7lXWHDOnEDnq@|8xA>!y>85e~{76rmKt7?5M6Iefbd-Jo_lO#kPJK z9xn9!@Mc|4*G5L;OblNKyiT8_dhhfMF`5Rom6kMqK4VhErxCQVaxne8ia`WNUbkm2 z=*PgNn1?e9r{Ie}_0(ImsN;VmKz|;f8ijI|pUA}6({T>+_}EaZ9>4YV=lW^@MM+rx>e+=(NF9< zY~#j_)q@iv=w=A}JPlpJZ&9)DJneRS=+*rGt8CL!&tas@mzV6Cxxgg+(+7(dbvZP$ zw*#7JPDK!rGfkcfRu8uVv(}K#?AXuWnLb$Z{C)}6V10hVH@bA%bm$GOD{Lqch0oE4)fErD zqksADZIN+0RgBgNM6gm;Pbp;v-F8j6C*o@J&M0D3yvPh@yWXW|cjx*BUMOZc74r$E zrY&y~B&rJ?>HC_W6~VN6-KUpP1W#bUVnZiwDzBgCkLRAdR+e@J{a1h1qkDJp=-^c9 zprJ85qBN>sMh6$WbN6Pg`1zyUPCB+}`t74SUlHnA*%bylPx^mHjdCviac)}d+M%_| zUT%c4@bxq|H?KhcSKR4mqmYi^W=}1;C|g4_RTXu1rrY9cRlILzc)7=ZyVFB1-S1qM z|LgY8?>S%8e(eoG*?@k^kg`_12G4m^PbpF=yYppVPI3c=ugu?eH7jdq(sfIchtsVc z$9`MgZrE!G(_4^llX0Q+b0J;a5nI@#8VwI=&_nJYO3X`t#QWC>Q)$;(V2M-9DG94^ z>(C>x_d=<02dvNG148^>1Cb;zSG@K7AQ}?!MTQogGG))H=8=C-dw<(cO)v96jh&7S8#Jhye9rjB zmb&4W(>nWoUxe&f9P4=_&=R&CqGu(i)tD{{Q1?XC24ct$bu>G;;wutIlh(cX^pHQ}*fFu)agO{3&BWUN&olD<8*+CJNP&_J)&rOY}%WZ^5um+qPGiFSmM>|)g z9^vMj-K{n`;N(D;+x1*>R<@zZW7)e5LfBXf*kk&eWJqKxkl^xwB3A-VHg7^ZhFRD0 z!@hu>Uab?{k7S-$`MK!!vn6o{?6+M_OA8j@=0RU|U2S0TwdbBtuAI z;JV_V%5o#-h28wZo8=x3CF5dMue*5hqScyYUIn!ZDyPsWJ-@#dW28RxNTZ_gk2<~h-1 ztwkNBs`C?%9D~XIEo3K&-k;v`cB~n4mgvFJW0fge)^S?qyC%HzF>HU|AoAP1ixWF}zHCly@P=T9t=2yD(Q`TmgF){4pf$lohCY+;$$;ByMXiB#g$Y)?)!&9#d3y*wXP0`0IptMp!>mZ73-?P<5ZEbOt%a&A* zSet~9m{LiaNu_`xpH$k}NfS?MnduGW1+7?m*6%KEYCwG=+mok)REgj`gNGw$J7L{4 z{L?^MtRmrFC{Jg#9I*eT)5z?-8XQR1OCmjkR1BS2!2;ueECD)3TdJO-p^O$9GiJKrx7TKn$po|&JSvQYYSMO4EX{N7y)=mYg!V$t7d@DU&Sl;%-PeYt3NorzF^DDJ{0cK%fIIGv8%O%o%7At66-N=VmZP#ubl3Y5u z!a&2Qo$4~iWoDe|Sx^OzH!}d`oso76a+Au^9zOKQ8`vY+QMCF`o(#NQrq{oJH2h}A zE?x4SpJ@CJQ$srvdo(!;Znsb8W;Sly)(>B^Q^&~WrcsiycSSo3;&$TfS#Z5vj=MMdQb$M0P1 zV~+ckv+l8U(Z0QcG7MXxWR47Co6OQ-FiS76+4}YC?_5D626?lDx@jLK=K8v|4EpO> z(fJVO;dQLztuinmEyB=e+Ek7N9z>Oi-`ljBAPDVGUw#cR*OFsFU}qT>a~(Xry>)_yU>d zhJu84!uFZH0N{mqpzIeE3xJo}YYCt~ERhK^)I;d_{CW)>Fu?vdkRrA&;^@P2pMmyA z%oc$bmi{*0I=yqJM<99`e3NAcrYKLiu#T`DBqJtWmrkB!vbU-gyRi3^$&)Q{{F{H@ z8r(+;TQQ=PLTl>!+&8z90DM~}4VbDjt#u@Souz!by1XSS#`LP+uf0L)Vz zJ$CF(Mmiy!Q3^htE3u8r>n&|#Xd17gF+g9V>-l{p3>k;u{q%viwiS*?uF5Tu=*;;U zCad*n)7)OtHb5OB&7WvvWaa^nOe%ODZYs*#`DP9OIVeNXhN0lsI`I=S$?i!4-M+_H zADMfbUezjQ=p{KOg^~DO{)ZX!nI`HrK4%2?7Y~tfllAX|oUvtlQCJb0BJb2XMUxn{ z<@Zn58pXb!iVB_G+6(*-J#R9=kFwKGUHNPjLXYNUSbLW32oXCD=qVC$SQ(phtFa_y zp!B5R;emTwh*up^;47zT1-i(cLmSkukBx*Vk<>f&wsV?0_ZCZ9o&t}ohF<10!Bz!{ z)Y9@n+s7Q`&_cZ)T)iS9$o8E-r~Gwn)#+ z6bDLHnLwnznagojqBfKVDcu4yFX3e`SPZA;#lfwDi(k8$Gviz*{%bXII!dF{Azdf? zC*fSLMLH>VD#V7AF9?#Q=a}yJebJr}=st-ZPiK3pXs#A}jw<ddVfgDk8o*$N`-4W$V`Z0RaK1j0>ETXrCQJc3xRo zDRQT2>-K>pHYWU{#W`{2jQfWc8MgUuywUCp7X~1BI@%m19m!bYs7pb?r8ghM%bfH_S$E0@8E*%daUv2(8-_{Cym#QbTqj>X@>6y zbDQ~B4{d6E@~_6LZm2d>sZmdRbmOl2MeX**4SO`bsm=A`ANx*sc~8A{zeo2sL-l*> zO%CfaqTcCk=Nk`eochhDgYu?B76hF4XPcO?CLljkoRf+VKJ69BX{W{LkXF9r?Ztsj zCM>x)ZA$tlSHuRf8M783NOe8l!_wx=t<3Mqh4DLY=Pm4LQYZK5#z{QlY9BDDnW&Xs zaQR-DUhm%B9t^Q=k<@<2EqfYfO{E%;s`x}hBZi%x<%CExHOfp)_#U>Q zN8w5HeFGYU;#T)R{b<9qo=N!CETRv9`&nn+*}pBCc!Q*JJ`DZH!}B&&Iy=P|&ENT5 zRsQYhEK+0b64W~12UuK(jqPhfmQ1aWS`uYdmuCd~iZN7~OUlCU-r@r;z0+8>iWqd= za+*Isf#uO2)GTe#xhZs#&!JHn*CMSO`$PNHdq<@$$1{1}^f~EAEq~5r!cXCrVaCR# zD=r_iT#I(0UA@lA;+Q;oW52h}TDPvx^$`x>fZ9QC_-xyj1b}uD!7=e%qgSub$8PSz zqmjd3%znXm2NgacW2sHiIuZ{(<-Qw8F|#>xrCcI9mp*Dz2WCtrc}W>RcvMKyv-6Fl zp8y3&>o#qA(c6fYt}TED=2WU+V4KLf#E3~W()wzv9Z;kg{`&bt^89rsaqr*HhsVh# zk>`0gr5$!N^_5{nDjLIT;MCWNR-eH#Oiv~py*p7vq1~td-IQc^R`{nj;^$50zZ$U? zRxBCWeBQV7=gy6!uTYe{#H|MKjJ2YTE_`^THfeCo0-9A}%e=Vt;RIY6{fhFqGVu$5 zoC@}Y)ZDUwc)dZ^MF*pzvbhWzS!ydt0?q1nBUIs)-AbX$2)oG0NU^#{9ens|ya;Gn zjA;EQEu6YyML+1Y%k>(#(}d!j0Q z+bP(1i@7J5pXRWu7A;z2K$d|kd0FDpl_*5%n#krT1t(bxg9rDZlza4<8`=rQ-{p<` zdyj{T$%`^xiq^M&)@i-q`XuB5S^3fC$CySzcBu>k;eX5N(3G=>ua>w`xP`3hUjFw; zdbupL1KDm@ScUj(gr86NGPu>>(I_HhoQlSvLF!z$n6<+0I)1vXaGK=MseXD|+9KwB zI?bAu_~OM6-{UdACk>Qpj%f-E)_A+(6*d1zQU$7(6(LI~ftgP> z0|smQ zM9&C%wsAU*h>peCNUW+x&6;)4W&&uLu$IhIlv%sf$_Lfj7O^XJ8EW^|gES^86jZ~K zwmGhU2Hct`$WNU*wd@P_$Ngo3`=N*PLW;>u!bou*UtAhJ_Tr{ZburHAdF9Wo#u9Q9*o4e63U(n~nF07z)uy^=@ZSw}}mDCmZBZ&47!> zYIY0SAaR-`83;HR931RG{YR~I@2~F)`AEo%3qyvn;^Zy?$halRP#YBJwHeXw_KXU8 z9Xt{=Y=g9mmnpr+cBd4&!Fr8yS_u4StG^*P+FT(^O-_gN31nI9@dHbHQeAG}xwD4C zEn-Ex!7II>Mjarh2H+Wo{0-Os1J z@~ke0`KRLIL~{MoFHH;&05U-7Nq_WWDrj0v+xO_vfYN|Yd(-&WwY{K=nJukM-D*T9 z2}%AM80f1hDax#m zv6HA&-D{Z(o>V@h@|;Ppz4N;dbE)P9l1F#g7D_YpCJiY(6A|ahm=^>DuRyb{UT|3l zSiK4_S@dTnUtak+X!^)?l7i5SL_M4H>u1p~8oxGimSWB;DB7S7?PBcZTn2^+Kgx}hi$ z>u>@{*j+Xl2>3BR!~lAqSYVErF9>g}c!dFwjCt(E(MPs2ao%mb+mZPg(&#Ao1L@Hf z+yszsrTJV35jXXq|95#kgR8Oa6a?4m@DnX)@kEZ&IdC=B7I1Ob5K+mGlG}v*=%e0% zkk+BoVkNPulf(>$gvHepC1x+X&(*IiO!#qd;sm@9utTp0EHeqvgfQpfc}+}=0G7gT3f2dUlCfzZ zEASII@@7AoM$iB{J*w+e0{PF82hY(e)d%KYDt+SdDDCs7Pa6Sa z*y4_z|#DF9H$DHC03rX?u%Z+43#=&Z|lic}TUFxMsU& zekF5cElv(Q(?C&P^fC#n>16@G*wK@a3(zdKiG2QTMmxw|1R(3Vx&xd`X|oT9E!}Mw zJudVn8B{RaOeU%varoI z!!U7>)iAzS5RzYx@!&q&RY>V1)YCkFtbemPwg^O(ntrcT`gLi;#ZifKeOnJy)ecl9 zVrK@&_bvaDAGgVQcX03~(nDB6B^t>jafa=U;`C`)GG*Jz_PI+2vMapF>FMLsAb4TQ zbro0%Gag^ouc>ub{9#bF!R}vGSX^?@DGJ)8@0|d#q8RAxKX24Zt8A_0Iz|e{5vv1g z)mDg{Fq`w3(5$#Zg)1dWI4CHH+*NEF*GTm7T?V?I$m>7Aj7;LL1W}p*m&Mme#d}ja zAaFe1KrGq$I262CFbSrCyzNw1NhD+~!0}m?BF2gNj#NpSOT~2rK|?q@7O8MrOG}W~ zS!jJU`}S={emw%r_7y$MH0f$^=MC-!wE`(@Otd56;oD3%E!U`l(Z&|;4kUU5)H2r) zI(`O?d3(bZ8c&+==}D^M_x)OTHTV7w0-Qsns^(vkS09-bG*=yBqT)dRC_Vp@;30?# zNTkGrPiOGPicNrP!FUzd@b6vx9l3;(f|}}TX68iG=FV+~x=&iDUH~Q{K5L}Kk}(S! z>-obi?s3&b8|BT^zbv{s24k0)blx$%&Kf>D%U$o~H|ChuG`Lr~Wdps^YO+jO){%|Z zPTBA%C(Z9r$`AX>@r1bQ23Z~6+s@3LZVO^syZ!9k86TYv?D`URc16Z4#9`oF!*IDV zi)b4fZGP%SyvMUwM&I-2Hd$YD$U-PLfg30z>W?*=ivc&_q-py0d83T04Jk6{wzrQ%{PFNqD@wP%lbf+36zXOGu>*F|UES#?}&u0ju}v}?BwB+H)a zjyCV=k`%puy^b(h@FGb^73VN?LX8<9r+ZOvQOL&R4G@F^9}Z!kyTTatlpeP|B$yW0C%7U4jtGI!DekLE}ULMzMJf!57&R zLcKNYNlKi}coi{kxdwr96fa)9burq-K}F;C4Uoz85iA8!Xm#(=Lqe`33)TtMB4!pg90*X0s+DX zWgI73VvEUy+Xf0z_@K%+BH8Ih-)bF=6Es%KHxL?)s9#W+HsH*Zg8(dYErv@R<28cE zY#`f-Kndk${cXzBsn=*%bX!=^`c40_YHDgy+Ym2-87`Jqv>+uS{>JIpGrFFHVOnKz z5>(6stmH9faiHel?57v<8fEJC-MbH+uca%=rseQIH7%8=1zavux|&O#ioUNTj-*;$ zksCZM!*7n>&OUmmTf6PJ<)rxZv@i~ZZ}sHh=4mQ)ZdHZJ>Mx+I3w#e}aOd~*SsrsP zUZon*>(=QPz-}$~g{P~y1jHVQ_-ec2hu1#ml_hV?#41ubJ+*QaG($5E=E-23wtlR6zd3MD1169QH zaWnWL2WVp$ECY%$G%%kEQaiXak}mn)y`DS2o^{e|DgE#?t=i)fBi=nYDJr+0ZTga& zs^yr+tJ5yD)LdYklO9?w$2+v)*Nm(>FE8wV_wa|m_l6De#qqdzxn>vf?Z^BH=&X-= z2u6vT3jZDMo40Pg4&C}7(xi!EV8pV!ckjv+qHN%wInOWjLVUCVn|w7ne_A#W84DSq zJqHPr-ceF0k`m8d)>{D*t*Y)ja`xt|@9fW7xal=+-MYKSezY;zwzL6U)UPZrYDB{R zwzon-=G5`WcN!choMh)nLQX2Zff}(UN(HO=3K;~6V8K*NSCrikvql(1-UOdk0Vh^@GIOfYXCxF$girfpEYX(>4GDT<2E`M4XGWfeQ~kp!kdm*_BSRxJfs_HY=Q(wwTqP+RK{;FUux zA?eUWAxOZj$Qq}wD%dHvCH3UOm^%Bw( z9%;tBzKmTi9@+NGydCwaeiONnk|EkQt<1nb)~nQVbZwRSi9=!}TbHTA+t^oy*EQB( zp~1Dm%{FRk8)?KC+~mx{wRA?|4d6@gdqq^K$XbYJpGb_!=a2Ls;k7k&-D6@QenPfjv}*0TTk9%~0#x1JB7}?Gw@gu|=NF zAix7b?|B5xgm5Awx`v6JcF}{dbz|c|wUky?jD89vUKlu@9?L7ghF)Uu!D0l-n1GHj zRjp>gxw@FE4(gmMX&-%`4t(V??oZI3B+EZi$wtE9;dFjz8hYlqPk*X&2?9|WNHi#Z zpJ1i^(}qeE@p!)bcdBo~333v#F2X-mnjQiUdi6#HK3@Ha1+XU|duu?5^r`i9_4Qkk zrCP#0YqUX;Bpg0F2H!1&Pgx7%b0NDHDQxYPhl()qX0SkE&N z^-iBURXS3g8n-32SK!FFytdghe7>k3emGC?F|<54h<~EiF37mVbRArQBfE~aOWKl? zd3@?4=8VThKBTNeZF{hHweiO$NIfDwM&dXEN1b=6XkwW8ojkwnnJ)9T-Fo(H>cXkE z&X41cn!6NVvzRe-n8_HP%d)~p4RASKv|NQ|iqRu`pcsOv-pc#lbWauaunC-<7 zvadOP)CuF;_V8jynuD^9z1x9+Za`g} z!>aH1*mF3~4|?z>N*uhXeE^561noHRqRf6)&AiE2fgzlH=s1^zy1&C$T$QV3K;_^CrzB4Ee% zt8brVZJ-t46>)!eU+3nsi3n&-1xeykvh(vda$3F}k{pk^UQ{M1YwZE^E8ayTfe_kO zq9d4@BTW(1CTpe~6SrSEEvYa-j;-7bhLJas!?(wU1#*SX!uH^&8zAH0>Xb-fC{m}Al3;Jk34yM_KVT%`bUWO1OaUA*>fA0v|x$b(;TB2xDX zr6`(F(e+Th$=Pve`lWtoU@g#NYc*AdAk_fM;1jLIl@D+C$e(tP4$?tE?YmZ~dM%P# zWoqZ^EG1nObkg+?OzD;{j=JLX@rkh|uQ8WN%h-rl+FAerP62NM=L~i@G{7hj+QrCu za4oCBs4s@xog2G~q&AV^X`iro1qE*d8%f{E6%vj`m>Z$9zw>J6)zT#txR{I#W0~9Wwu^lM0PMfU>ScIDOK@w~{nx(VtO$QyYe{_l?U%ZjpE-K*HjK$eIRhd4 zFW}`XkR^w)%*oUlvO-gPTI{9c{m7UtXN3Dpgj`19;s}-qP*syhzL9!V406*}xP)u| zb*e_PiSE-UPoz6?kn^pFhb*f^4^y2BT}}diQUNg-v;HC(Wmcnb7+4`3vM_r%;J@-Q)#ck2nJ^@%h{pi0Aa{b2F--fx=!jN z3cnd(y~%g(7;`!-f0R%{qY}?mxPb3Gbhi$f{ERmM-`k@LXU?0qnS{q4Q2a47Y)Frl za<>k0Ca<7%PO7?+I3 z!9L2K^xs0O=~s>#>DQ$UdV6Lf_gGj?%yd#s9v^u)k9HXq%7>-#$J_ka223b%h=Zo` z?5IZSIY1PWmI6^=gy)}v;fsp8`ex46a1u}}G#p=KXviQJu{)D`1Ku_ARDL_BMy>jZ zF;^0jb!0w~=MUPUY9Om@pizS@id19`O8wV_1h>MqZ7ER|#Y23HOO{~RH;6fRg&C92 zF9;bsot=U)Ohe%C2f_Egl5EPrXSxlx^3YnocLY0xeYrvFgJf=RZ-}7DY4+@Esi~jO zzP{tU`oqipY?jd`cCS|FTRBr_(w;aH2d1b~q=r&$!4n#Dm}2``Rw?I#W=f+!`8Z&g zB*sDs3#E*BNPH2&XdBR4gPp*-Z(~A-wt)q`E*aFGJ<7Ff*M|7NVNW{nxwcqZuX~0k z-hxj`M=o7jwg2T~nVYiz88Pz&@|(C~^sLpACqL6kaQVPUDEo^Oc)l_R7-5Dych(XN z0rNN`u6F&p)3g)p^?KUlNKamY?V|*d21fKrrSFc|Xn!X{=$L<$oDWs55%LYx{+sEY zrgf(d&_Tb)J^gpwso${SO}52iH0fkwd~=Wcl@bQNgk}zOpZp!lFpiwLL*gFG9;?K2_91Xo+ z1S^Dnol-LV`8z+5SwR4lW5Xj^qEq2r}lVd&h4F#T1I{E`o2ck1j)HMT7<8{ za!O=z065}V1#(ymbXtq1A5ILRb6|B6;X4Gvz!Hz#d6U$7lmJ%DE>2U$rtT(fSoU-0 zZUTO|!Wtop8l5yqo zJV1b9yDF-+IpXbIv#D}TVN=Kio9fuM21>7{Uf$dQdI(h)^#7{j}_Z>6m;ROCFoX5 z4sKXteuQBi_eSpz+I~Bag2D6kV!Sj=oS)`RBu*`Vbin-?nH{ZtVZ8&ihtcaJX!Ye4 zzm{BV4L_0A^yI>|w7S54znrwn;NIsqeOnr5!-`-(VIH?QZ za#$a$=PzHjgYCG+>9qV{hY3d>&cSGpv-Du^V{@T&+4`Hw`s|4+iX}s5GYn`gD!OK0 zCk2{zqR*g;UZZx9=RyO*CR+9Exy!;CBh3-CfP=)5Q)&_(xN;hSpNZ5>h!XlNPSElXBoi6 zY))z7@On;NBMwiJDZz9Zc)vwBb#P5Dju?^}T|hrsIDi(_6~dY-R8&;jAG@R-bUPIt zU)=TrdFaM9J2ly7yjN4OzBq+|4~toIfPGIK21yST#vN zr^UI6fKyt>CD3tToIGUI(JlYN-~C5j_Z{tCS<;pREb;pF8lg{HWw#32=3JUaJ?}7Y zUS&$%L{Q?_WY)lUtD=5jQ!UC6p;wSmxIQv7F&PfV0YpU8t>dB^OVOU-O=s`uSUq^m z!SlERwBeMdxxjM2%bdA=)Nz4n&-NjkgRPET<~{}KnwM1!@v2kgDIjZ2N>S7&0V8vj z3^*RBU?XEjD#?$KrgDZb@cmfUXI_ZmZjI37x^?)(o@qPO-3Mml8lM2Yikn@}RK2QO zPdtVqw;%})d5lrkW@tyGYjL{4p_mTB=nRL~e)QDF$su|^f{@Cg-)M7ogL)rAKK&&{0OAzUpbmii~JetFcGqfQKy`r*7O5`k8<- z^!BQ&N`Jop_kSwwds*r`4nO;k7NANXRkaTJt+erci$^36kU?9v{CKMVyX((%!!>Bz#*2JKr`#gKxQ)oTg4ZQueM! zlv^^eOB8GnzbgHIgVLqjyH3B>;N!a0h7%_2%Gx4)i=oAFujk=nO-x<@UFM`W*>WS; zi~L&n=YzEiR&6#f@p+gwB0{mYL}B`Nwx(wLE|!O^V(iJpe6=$Cw0{rv4sTTZ_uXjM z^5V3HDDE_BtXE;RS*d#Y)Jd+`2WUP>$4|?Bh@nq-!!_`Xb%(v)eH>E7dC2itejOflDY(=FFB4BQuN zSaqMm8`b@D@fT*)n%y)o?{n{}XD*MTd#+Bb;qUwM=l6E%zq7l4o}_s$95_L!8u7n*ERmUD-UTy-W-of-kaK_imzHJrN|J8tdP ziY%}8@BZ@M`ZBSLX62&Kz4vF$R`;!i7)nYZ;bAF~a_F*KfAONL<;E{rbFeNJ@do4t z=KwNFmc{TGr%LavMwhLtNXOqFWvH~XCkmJVX(h4%fDh?+u-sT?t7_OD*nziUGqJ1G zeGeYGrkSGl@3T?Hs=8bzjDz_5HMQrH2_gFB zh>=FEaQ`e5nxaRMs>;A~8vkoZI!5BRWzf;`{VLUHRbUIH%~opST^euyPry0kPp-#I z2! zDzie!5yd+pD|(@iN;O_@4S51qSzxBOj5Xr=|GPf5JA!9$*^DvSZVTJWf4jbWK-g=* za0ybdi=|da9*$|6rf*bd4OpcEq~ZlYEqd2{pa8GjyLX&R3;(Oee-?o;vEkSRgppOx z)FzQ@=FW_0qfT(u;6X_#p@Y#?%&3@TQPIEjC~rfJS&K7J280%XkXUXUpDUU#>>&iv z0r0*-g0y*1$8gOX|HOMs4NI-2-0W(KkiO1@zAXdv*h|)DLdzKKm!Htua`b(YSAnt` zcpF+C`T+;|r)t3ON(NI3E(e6AHZP&5x<(DS1!f!D6p(T{s18g8Jw*K zPgjs0ec!o_PU=Rl{}FgJKMksTjYCW{MM&IkfCTiyt5@h0;IgjQeM-bgj6ojifb_dD zJf_>m2SVp6sW*OI?G&S(`>SODMatYoiFD+*;$J{*q@-YiTX#HsM3^huILfs|QReYi z@RDr0a}-|KGt>t^tfdg(1hxZSu%gv~CE?|*qT6=*&yCbJtQj0&Q)dl#OgJOqQbfi> zgH!{0jaJ^aETr)%)Sm-cvhJ*`EQOcQOJrYZ$#D}y{A@_g5nn32gh<}Emp^tu#{e_5 z4yl(-)|t815;jD!0Qgf6W&)Z+>=?T33kg9}_37|N)6(JDxV8QJEBvo3w@1pLPDYiq zqY!3h`c-Z`cHUERO$-%6vxy7H4K=%hodohWq>8Z*&yDG9#MC%I`{e=a3ATn1hC{!- z?;X-nLvi8Kr3fS`6fh&^&3oruHm+3T_5XZBpRmRos5Y*Fo`{?UI^TinT1)IcHYzJ* z&K?p!%jmIf;w;y(d1M>|HIUt0rnZ9*K_T?th(?FMkaJ3X<-A)Z?i`JE*{q4@KO_e1Zm8jG7 zVIjjYWJU&uvl0qbwMTI@_7E=|X3s89vA*k}MG_arO66EwvBW~{arrdMHNx!#m0Hf(r*-SqGjezT z-fHu^4-8|KW-yTY$@3bx~x25F3YG@QfieAJ6;1{*ITWc;yJdDc% zf%o#6;~O{D<~zYpBF?KkxzvrK2z(kEyZ?nBL@KltJeW7FJqM#PEB zw-YXvI$()dQ7B+L9r|quV#a`3N>!%f5;88{O*}B+h>}53J-sRP8Z3+q%^%j}to0sn zi5B!yxWL8UrBdZj_D|Nn{dP)I&_fn-GGF6>ne)%%2Pi196Y6 z)8SjF=v+Z7uTnAy+$)_fXxur}5F3{1mmLVoI*wpM@H})INi1)vFXukvAeJfN7?j1; z>Ck;}(Y2zUmX>Lq>*$bV(1pv)h(2fK{7u={)UiWOh>HZ&;?cY3m=l9TLf zx3Y;399=`4Y^7ft`bPAl6i7*kVPvGMpqpq!T(kF>SG2rXu3fz9yZ6J@4&|2q{azY} ze;MzmK|X`B;(~jHo=n<-ZT%2fd2iwOn!deaD=R(}&FpZSvneT%MN6{;jPzpc zA$i5u$lOvl=}_C#AZb9pMBrfE$|MmW?spl8v}(&mOFsf{&W4~>su$`gK|X-~^Ooy0 z;=2i}?R`X}mwG2>planCo6u`;r)Axt2 zn{sReSOxMVX@S=4)ys0W-T&d(RiH5vf`#QTQC*8sS=_LVgxbPRhtTb=2jvn$Hwi!4 z7s?%gz5=uRMua<|`dH15gai7)gKR^XWTC|2Ec#qUdxV?(SuzbmyEl04ABhn2gnNqD zOI*L=R*oN64IMC`1q_=vm&&K_g`uEgU08H_D1A+Q>;lp+fS zp9M@S*q1P|;sqa92Y)At!wVN~V`1bFr3RJ3%*gZLkq1{d3G>X~tMJ zd97}LMsbQphZZ;*UbGSQZvoV6RuMN8(VW6S+Cnf$lGFj__KIYdkS>OAtvl~yW{tTCc_SsVK<2)yb{vpF-Za=ol!Dl7X->(U(ay!of>~(V zW!UrWAt4y3&v<@eaWyY`%)O;8m*KO%88G1+GxEXMM*#WPP~ecKb8u{kHu;+|&LN8S z?(Xi9edSe!A&Y4+L0U#7{zDKuiW^%_UP%wVz637`>k0Tbp)n2kb1k&sU?abrW z$z5_$JgIET;X=T(4MF@(h2}$5V?{xvOs*jBvS`r>oAd26sdLidIz|HsTHV>(f1I#; zGw|mMM;;K)!>{H=kGbdD&(1|>g%6rp5vRlH+)d%b>Mc0CH#oQ}3vC4Hsj-{e-*gpo z4%AhI!x7Cuax;UNFAYm_`@v1OfDgXAo>x$yCCYWcY7JQ5&cKsh`l$QD+czfpXN1lq zhQy4)C)PrmT&j7s80vl@<0ePGG2z&IOy?#j$5q)3O*sNp*(S~Og2y1hHCYZosIBnf zt+BHP1a1Oz*@S=N)w)Cmb4vS@^Icw@@i;Le#~_)*yP`;4@-H!;K%I zP$5ptK;J$C6G?GJk#zMM#igmF2?Z6>#@y9K*-}q7Kn%mth4|yDT#I6e|CPJFss&x#OwT?mrCz&zy5E;p;oHS|o z*a|{WO-1&H4?P0W29I_t2%XAeExodU0@TpwSEs>njyK3qmc7F)v>tUXxn~!6rDf+_ zSFYp**`HRy=*<=FMN*|_bZY}!|82+cGvL~hHFD(mnZdA|L7UVx*272~Q?y62d*k+; zCf+y|Hl=S}vvcQ>Vk-3h+4*s27QVDqJkb9s3n0p>D;>FqtmEl!Q8JK!(Z&dOjiNmd zD#E%yQZ?o~P3qFi+TWRJle0q#qs~HDzMT?3FZ@LN@+j^kre6>QE)pvq0%8L<0e6K}R@6T+0L;yPv=wkv<`AJ=r~%1;J^ez7bw zWAup@aVwQoBfjiEPXGnu-|pm<-p(F`#9bjFxQ6ct(2#u=oguPKOzkM-U)#;M#$g`+a_5ZT)>yggN zC52=FIWXWdBsITxv@VHpk-p!>Kec3~moI}f|5tfu9@TUH{{8HOH!#UlO;>06>UhHBvc6Le!RkD&iUPc-sjx+ zxgB3$jQV^&ulMV9UDtEDu2-cey)^76tm!RRnnkk3-R!}1h-~>o7EI9){XJyE@bNq7 zJd+!=Tsy5uNaJ9WD*v0mj;UT=JI>AmkDTb6CYfRbc0SKdNIj(JscO1%U+w|NFxMfW zM1C*D7XzC|Py!D%*b|+{qY^i5Ml$!(UFv0aec)mq*W;?Ht^^OJ|Ma9Zxz79lnJcvE zpE8Vm0bCZKmMW#jzrW^&T2lx5!R|$^#8ZJ8V+js5DQT~fZ=XNtfFTT&ZV?#=_y%~s z_ADCZcjUtvAR70H7l`-{oazio?pE=oK&}ZC#?1~A+e8^j>eI^s&fAM(i^&eba822As zeHm&)>e8bQS>yq}#q{`#l=KjWWAA$-6%byx&_)cR{1MOUPc7-GAHLfgefz`IPZ}m; z>C7G6?Kg#O0llW_oL8x4z#G^4TgGQ&+G)q{eUEi=?c#!G|svH@!=p=XB2~F z=H0so3TyK@w<-!PDTm<|kdn@+TTC)X5Q9P{qNSicEs&W(7 zTknyy-Zd`6E&H(7#P>;bOx*B>(o!kC&eH}Y&o6co%(tf`+%Dpjujy?71r z;EmPQYQ1N(Z$dgoLtS-x6HgJzEE?06Pt5`T{>qYv^Z|(EfPZdLpw)&$4BZJM)TZA} z^xz5T85-u(RuxU5WDj@S?`PYsm`fU}j_-A#LHo>uRT=K1>b);$n{3(`(VSuCzi}}w z4GH!+vmk*MI5>8yTr^wSL~%~v9>O!3Cf9yEIC z=4Dx-u~cO%;Bpxo0$>XST!a%4-rWskL?pRZ-hE1B&Z0;4rW|qF+$DG|jNrP~-x`@n zLNKH#{Zl>Zso%Piy&k$rE|rUq=TvKz-Cw8S_3?uEjD*EucuAqp&5$1K&OVTqlixk0 zMbogmL)HoY_TgVkw_wwXW!)U$tNaW*U7_B?@%+IpPs;y9fsJ?S%*wH{GbS81=)GHa zL5$gk6szxc%rJ~JLDE@ZX&G$o*=Q!7yGzm99nk4fV(#7wxymx1E=u=^s=iuYU~31^ z`mhBZJ5Jm@z4dt;>3F$LfE`JFb*+Ep4|UF(c0m!ZU%!@CQw(bPrNu_!yz+gNqEiz~ zTxSrg@*#peLR|gE0q8vGYRO2Fm9+F3gTv*_y|P7%7I~GaAHaTll84QB)oo5rPEVaR z^YFhlgOq}}EDvMGlb_-xwNKAA`est|NP5# zC09oIT)(~&^d#NA`>5BJl}>5nt#{(cW&sR&KNmPub$v2RoO2A)vKFiju1Wb|PL#`@ zJ<#(+aPSY}U$Lh|_;9XEeftdhbUwpbcUyHZQhMsr$0Bxtjmy!ikMBVt#L+vmf*g%y zBvfpDY$M_Vu0EcUdvK&RAY?JYhE081^Y?izrR}Bkl(JMiwm!E`Pe<*h;z=8_C(B0j z>|d2#u%Vv5Q3)@Spj=JE^~jf3A2oGWVyBs47w}@arh6Z}oqp|Ok4oFj%uK)QW%JRs zx#9BI>a?a=)D>@Kx`nT^Nm<(0;lY`38sUyIe}r}_3wLbuDhhR}STRiJbePeQ^{*FT zC*#E`+g~qoblE?(yGl7he?Ao?-02VoG_61xb0~-l;eWOLO^UJ{=2PX(1AB?_osj20 zac$Tl==oR@r@VqfID(A)#$k7IisAg+UnEsFusqR3_deqaYywR8WbAJ#_5{y0Prq{U zqE;=3;$p=qn!T86Ze8W_7MrXEE{~(7ay)i`>q;Of>lUlrl-6BG*RJpOQww5(4I`=9 z6qJc&aMK}SI7t~9U$;cMCc2A} zQ&;JBV)eT^HCc6v93>{R&uHf6(E?#A7_{k?d+M{C$h}<9@u<*+x>a8@ucvD=xzV2R zs2iu^y(lAH-RvFL1KR`A{XQO9w_suIwxpW46#qkqsu%w;`pES%E`0Y?PAQI~$2IlF z`U6`TR9OXYocua-ghJ1rYp<<*>z*nC;p(Q_9#nXya0WiF2pJ1_%4g1=wF_Eyx z#xXu$HBHVUD3#u4)4n_AB9G;65n{kUn3xilTy9_f^l8<=mUlZ!55lM{ie130#^aYN z9|=HZEGWSjlVUhM@Z z)Gy#CG;=9lA1D+HIVG)k!DYLTpKwr%9_#5vc3VF{+uby~$4bq)C6H>_``P2KF@sz)O|Ao-u z4I^)DOuOaY;pC*YM^n5BLH9k~6A4g1irbG8)o+Y5D0Z)j4bt9to_@P%9E_@z|NJwr zTL&6z3(eI7^}%%oSY}B}Ct$#dc84!Kmfz zsLC7VU96iD<{cb8jrFlV8nULTUc686yx!coxuE9+%2Wcs0rFusg_OvV9}k;%6pO|m zJqtIr>)^~zi9to@sEe}Vgvrr`^!A-Y1WRRn|HJdY9i~|)H_dY+XIbCea_u%zN&}5V zc<|l1^BzK=LZ3d7JmZBZ#w<~QlH1dswM9uL5TyP~`(}8Js_zg3)3=zv{ zd`v$>ZT;7_ZUj~T!-r!Caocw6h=Dirx=uunp&P}5ip_-L=$z{-+uxv9*~qPb#Nsz3 zH8IfIJqo?=Bq6^F&v+iiRRUDfo`F-XDbdZrF6$ff%=I-J-J%&Zgn1jdGRq!TN94Ng zvWmxm>NV(Yx)z!Fwx?GhRyky!*j3_QGm>xd>7%3ZV7=&T$1+=9a0|PR_;6U0#dpC; z`u<8WiZ^Az3_ssb#RD{)PQXOuLWvoKYAwPw8*a^xlT#cslR83lZNWwO2LyzR2n(b1 zo^%ut9DIs4k=(bdu6%fr690Fcf@=5IDKb{-f$=G$H8m~vYQ5em-0xs%syJ)oRjZyW z9QU-iZup%gB~ zA$#Bu2DY?c{BG;3C;d(Htb>mmp{sV{d5W6~?@#pu#||FM7YtQVITT?ZL>(QWTz_D( zNa^@o8z(BGL*3dpEB8hfCh}576OR0S0}pg1sc$W>1#RG<5Uh2K-7#1c_?!w6&P)0Vw;^?OK;Cx zr9{g`>0X{^*_h(W+-7V1=eCe`xfAotoeqgCc<9eRf5A{&>|#^mQE!kl&R`vk=Q4nE zb45Jkjx@OUb?k@{w^qM+axpMI^-Yjs@7@*t&1uD3M`~C`CCb_%WNO5>_>PEE9esY% zky{>)iG{!K+V!)_A?EaqY)G(USgzZ{?adbHrVR(1QAMf@Qr*2GqfC`f1knsK*Z0q59TCcX2refN#>PUjfb5YuzVOz5H`hW^p^4koyI4Hic3Rj%G8E0_TwX|6L711=L^n(-S(qZhL zFZZBqjidLb?rl=Quh@=DaY|Px)@l2`u>I(NJ1Z~zYVC%@kZ?5P2Q|Un9Tr1XlYT3l zAa;ms_wEzru8!^^+n0veWu23AouVjcD^vn4ZPD=_FckP`l4G&vI+ld5w+Pek;QROJ zJGf2HjTMC&R_sM8~t_CGBP99h{5(WFq|YMMsRDI;`X)Il8|HV5-iG{l9VN^ zYVP9Iso^5rTiw6?g60fA#UC$|BKh zDQ1Wex|?fPdrz)O=$OfI51{1{r1$>Capz}g7m6Lz4M*pUThHy`9O1tUes(&)a`>~2 zY^~!2R0*|hprw=K(8YP)l%8 z0sk`o{#)KPf7sn4OZ$eG7wQD8xio~1}uW6 zcqXHWx3_XkA|Uq>u`)~hZ7{}?=JR8ltvdOubllb&Hy&-K?O--5bDpX%j^|sNSX%Z< z1mlP4-su!%Px|zdlFID9`HJu5n#Ro94I_T~X*SzIbM2F@K&w_54Hf&ISsEVtOY5!W z0gWq7nZ6}(IHgKjEVR3{oSdAq+a-;F)Rw=ywZ5$XWCEc;1VxUC)Cud^jWvh~xpd?w z5I^Mlj^4DdhtwvivkTSE6xN>M*9WWkjnkwbeE04x7++v{9CJ1LjKRYcm-vg;@pE&O zM{Df>CdELHk%o+5n%OcQZ~LltuliD>s@QTwMewCieNnhPv9R&pjPBLmEpNTa>AfyN z@ATRH#}(h7(o@jS&BbTLTg7Me-@pGT9xzqlyoQE`Q%Z5#y&jzawWu`;$r>SdbppEt zfByxqs?HZJD$G|MLeV6^J(Rxw8@qX=wBujOC5EAvkTW_;y4)-f|>@F6rr+oX?Ws{P+>l^l$?O21H0xQ?|8{Ca{ zUZGOb_~DQChjXg^^Bh`N`9J@7^dwg;CAU;-H!Y>~J2j1+lh2L0HCncG>_EZ|Rp{!m zvOnG{Z&x|fsOy&(m^1MVkOG$Mc`~|Fue=w+P+sjEwP1_mP z!&w?7chQ-qIyGGvYBtEJ5vFXy9l;WUCYmc!3pT2T+ z7RMlHWn4GsMdjBx3Sz53dusgf9jpgG6}J!Gz0G>0zh~RRc03AS6b15n$Q?J7Q^&E( zEG9m7=UZgLWBU8~9TSWd)}q($sbj$F{a?vsc6qYu^Bdq%%mH|b?zEM0NwdfU1&AvI ztKrkH)c+bZRDbWWq9NIxzx?Atb5K-fk+2H*&beHbCJs3ggO7TQWxacN`Nc_nOh*g(8{1QXJPxfIk}+A07`KDmYUM@<9htN z*^QMBZoYM*VIzl8|qe}XmDhT73laoXUVtk?~?8rs+L~eWhegj@C1DNa3BJkvw*Aj zBH{UN*(SO6)UkKbvz{XtVIP{1#y*o6Gx#DZx0AnJ+y1irRi&q}XgbgjDZox6^veq9 zO^Y+X?qbl{VrhjVPNd6>Yu6m$#6->jKatZ^h-6ml^UN4G_qu;6g0}!XohooH8F6JU zF2Z%z>qQSWTyCDssEc6?%8KE+660UHYWXSey?z{q$%+TFpY3$@vG^i}m8!-U)EpLf zrMXiBD@z38#V{qqly(6l7N-lz(;`aH`jX@Dn z3=jPcaZ(^Ehx}7f%mR2;+VUyUi&p*hDCj_>*LzB-;Lud8mMHOt_ySIN%f)9e-Ba}> zl7GZXb{XhlB#1W@C(nYGM_al-i;4_2rn`tnV>J-$4AS{O-2DYJs#B^(HJSt59$9U= z0#SP&ixz5bpyT$djB{%_Bu-h!l#iEQLW5?5mh%7>(`Te3ENOG)OVDJOIJQtEY0{Dx zNLjJIqTrlIlaqM-Jn6;(F_uYZJ~hUB+e6o}1?NZQFHNvekx+r>q0q-eKQ;XkyYTdM zDl{|rIbht!D^~&bRstMBfeLU`16PxWJthh$V(L=j7FpUTb$QbtpUFAiUgplR>-(~J z)QF-+5FA=4P}7sEvHFZx*0KWjE4fzC$x z@;46GUB__STeh$2bb+dVC+O!v`?z>y;)nk%3bQ6GPj@y?IUwXOybU^A?I=z1m=b$Cd5zZhut=-dg{)a^^BZ@AktVk6{CysWN}@giiCeMV$CW*602kG zv%F9H%fA06`At%q<~r}pg*;K4P%sW5e!x^OOO}0px>vK%wssr+_Nr(@;csIMOuB0~ z>=V*5C!-1|lZVI%8>?R)%#~F};QR4txY~@b5R}1$)g`AmkkK%&7U^E8uy$6%D?4ZQ zZ;RdS!!EQtnqfZ{yh*0HW9Xc#-3J(5`y5?C0k$ zrE}<)zOxQVAIUt9g-y&$%Z}Q#=IM?7y3$?dEf0#IyoJ8@2S89kR*6fs^!0OyY_Wn+ zg=%^P$4ehB)`=xSN5d4IS=c`P!jcs$PO^fo|76vynFJfXWw)1g*u`)5lYgvL$^v$q z?ylQ*rx!wtiC@n?5Wh(*EO*u%&(K4$%5pX%CGsV$57n=}<~Yr3_+qRV!gMBJgD1>#rK3Nw|`KO#nV@IONW{L=M?>b-mxGiLC0mO*Aj-s&QwuRv8S|1 ze!RVZ!~I|ORx<=*0b~9#IXXvwLdVt#Y}Zl6*5&IH87}odFo*=Y1!Ru^=8#b{i1h9N zbxy+WUFFxVW8b#LQP{~dq{?TzySiS0F^)zJI>{JX_woR9arDZ|%d78Bt`j!b2I6hU ze7LI{jH{RpqRy(63)Knie_KfQ~#PaItu4!N<{)X^zU!ShBJ1=D0gLfpU+A zlqr5jiF^W6qsXZ#_joXm*W9Rf9Sw9mXWHP*0pEPx zzNLN7Sj2i2Z-a%PcPsPGokkhy`IBPuhVQugII*YvF|Eh{KCDe=pD1p-e7Wq^4Iq2j zt9QRy$4|T$9vVpw9{>OU!R3^iQ=={qWBJ>x%Rck6ZWyR+8AD zH1a@7jalBGOvK(k=Ons=L|I5=7xgcgTXFleIXj)U%p3mo{ZFd^n+Ua8a2!G#g{&1d zci64rfpc?AA&{UlI55vQq+GOK9d@g|@77LEUrWQTz33@!tp@*T8vdCr!V2Td&n9Lly8P2v$l}*~2^Lj0*@2H8k4kw5zQckY>9*gM}832Y-YM zsA54pA=dr^^Jg^c+vKFW2>VPvdIK&XO{#R=VldIX*$NCpLI7LtEwH~x`2znP9l_3QERvBEnad`(bbWIAx{?Wbzma=~ z4jAQCuI5|#ouDG$VYhhx<7)POmV zj4{Z0o24Qqogq~(@o9#sGQw6jo}O{oEQ3R1%70%6S+poNfBIfajV)n8nab0Ac1O2J-r~2nk%uyeWj1v*fC?|`}dFG>Hztq11J^nd2Ya{T{y%7I+smg^U-Cz){cu zJnDyv2VcRZ*T3L0ePIDk@1w|JNQ8GnCKn{O5HVU6Pe_SpVk=@|YrG;eY6Us6yXM2Y9rT> zK7(_12lF;yU=lR9pNGDDAFyKXC0_o5K8#@AA?c_IN zUlqh{X%}1~B_-2IGIN@#Rll*zeh>A0v#9hF@GXjn=l*2ZPmCd+n1|~e0DUp7`ORYC z7;%5Pf3_(HwLZFwYRp%nSV}5Pc9Qvy$H+=2C%?Eb1M+|=H3`C{fe={Ir&p-@lqvjQ z#Y)D_Dp$j}jY!?S&y-*yiA+{fsRrg2Kr0S(Bk zaGN=*5^?T-_-;q2>yvKj&9Fkxic?y{0%6O4;YgiDv)VW`yBju>Wr2l~JGi#-;`p5q z3b%+k&o_KftbF&Ow8eVK5qxmURDuD9k6Rez7oi*l1MSz!n)$Jt7gxQbpt5K4Jj7Lx zeM#@!f%MR^SX1%C^SJ%-yo9tbLwiuG&aLmpSzCB4LCUU;S$P!vdh)q7UI+=hn}eV# z2yh~ADcj?k24NLy*kA@WeqJBoPp`-{eBzE)lE~0*suJp=ZGU8Sm#^<|%}?k9=~MAL zV@ar;#7)-IZOVXx;l0DuY^&#gjVOx_McmxrQ`ofg_upYNjyu16G%dBM;xI&>xd>Yb zdT{6gFH@e^O;7#~RceMLo8+S|5%rU@;MRG)^aCH)s#M3fbMaDr02PF&e`IZc4X0ms z6}0!){RSTlZfe*1xY5mqtEtTuu4I%-1q^_6R*aZ1VF83<>Z5g#?PS;9_gffa8&crG z^4xD6z(gU`9yOx^HNET$YT72cK>VNr$jI^{Tlboye{V{NA z9Bye{UEPqiMeKbL)m_bs>0B6E18rdjFF*)aIUMWSY$m)rK&QsrW3vPWLBlm_>wEw> z4G8*C4q;E*suZJ478#!9?ubDjN5mJ5NGy!~Mgjw)k)W7h7Vp58;rj;&_anxv1CPue z#Z#D;oNiG4`jNw*dw;6*&ZT;kk?vYAOs4;}nhF%}4v6#z)e5%~*&+7}{RwpSzhu_& z8cfGIj_HuSdohu5)b&pf34793E{FLeKZgBp%#Cym8j-z#&Z?;|ugD0ioOrV<<5SPi zTzO%k@Ooq7O}J6Ev7zcHvzP*Hb#*ePwV`m0fKsa!2^OBk+dvpR5$MLCDNRqm7fOmy z=7m{?0}wGYx!dh-fkCjs?{~8*Fo`(hgRgjRr6Jh9A1H20ZAPI07gSpcJL{7wh338ohAEAXo{W z_(kg565I9F?M@#22E6NhcCc}Jc-Uq*P(^nKmf-hL;6fM@tuFB5_ZX*%2P{m#bB{r7 zG0$`~Y|LjED}p(%D$<19z$unOhsHw}=l!v4AT6e`knz`b0%ZqDee=>`yfmMPpm&0~ z$Y!w?W`rtH3JH=)xThFvYQ)=zOqi5Z{6Z(tQk`8=x~%VZ*{fZhB|7{24%N>1x|>X0 zt>ewXl!6<$dp94>Q3PL@ei0Zs*Z#^>41bo-*l{!{)YC?Hgy3#+Syw0&IXcjO3t%1u zr$f6amMtc`bxPKH6x{Z)yN+SaUwr#R zXZdbzJk+!sV&^U3o7pV+!Y36KS*C~e*ml+;7J=TYKg`AVZ6%{rT4gWy|ESO6eXN>b u^XPw|A%rxX8vpg_{C}Vt|DVvpTe=%Lk6HJr{$~%dkP= zf!`kR^W5*f&vXC${ddnxJQL@fz4uycul3&Vwe|$RR*)jZqr}6)!XlKBeyNOwg&Tr} zb%o-eYv2=;xZz!Jy6Pk@qxugxy#F!&2;Sdyme6!ou`_jcGjKG)GPAX_F=2Nyax^iq zbuzbe-n`N*iiLF_OXlTsRrjwO({ApnYG)nWil##^@Y3m?`C~nZq2Gf)l)!oS?K{V< zL{$K9R*6fR}@H(*NwVl!wcHJ20 zOBM!?7M}Jwm@d;~>+b9vI6c~}w3`(LpXv}Z$i3FoOrWz^{{4mWsrz>_HqErcLbhqY zb9DFb-y`5kwG2r@Gr)^>HaPjwAiNtbcq8aK|QoI&d zR&r8(k7Yv1*p*+s3VHbG5wGv5`_a)6sb@Qo*gB$mb~H~VXQaWyNj`z=c~eu2FMeswc57uV}RgH_D+ZnHi)`-sT`Hm65@Uj;K(SIwuvM~P1? zqUhE(#>*4M{F<^AlC}S!=>$_&rfQwwgBd}{nuVJ(%HG}$lzi5?c1=f;*NJFGsNJU0 zt86CWkn`jC$2>ePvpxrT7Q@+5nKqME7F|*F-QC?oVf-_>2?-SR3=Hr9a((-TzW3J% z9Y^S7PWBPQUYm_MpFR<^{rZ)wQ*9fS8Aio#3C&mQ?(KyV@=v8DBqXrXK!B3Ch(gQp z!TH(I4I&~K`e3#s0H3_uvJ##iMC15X*x}Cy8m|p*XpxQvt48rijl+U*r8cko)*F$7 zDJSg)_fk5GsTzmlvy)9;)87ftsCCqDo_^`5ICh6tZ@Orbw?|-S2ofwZg2QhLBolsc3xgJaOE%<#$L-6B7yLn%uMtBjq$9R&DFsS_)KFx z#t005{nL01FYpBYqLaal8rb|u+Vd+e$SJMrY1gE)?FheIx6PTdNd%>cX9b-FtXQuO zQ8hV|Aw>lGuG;@}_Y=l0=jPtvUc1J^#wMqvq=aiJWIyNsEh7V3W+IDP9enX3kYI4R zHwgvoW35kkQ#Do8=LwK&Kbac=z#3VK?N+)_f~!L`5-AAz5hW;6Nju zL+^>tu`{)>t6Uf*Zxme^wXnL+@t#s^AR(_CQrBjpLWb9JWB^E_()(x!r!q4;8=mYo zGicY0&W1w^3Vo*EM$ud%r6y+3Xi5)u+cS6f?aG4|ac z+$kj`CHxs5508qX2r7ndt?^)HxV&GqpjfXy-wIZIe7F!1>eINhv*X;uUcCyM1sXID zF0pYq@Iq9=o)~xD3cDw$9QDMk?;Z*<3FBU`212KNZ2XaqnS(>2de*ldh?A6trnq{> zQ+6oev)}G=Z^*}wpXtEpUO{|NvF+j11zTJ8we|G{z|TxH`!#2z@Xvayya>7p%~i^v zlSxlc$0HyZQ4~YdiHV81Y>X-KT8*k!+fFae&nx{Y$Hu~HnffOLoQipXtIXGLtSeM6 zXb+>x!Nn)b#SCGql?dFyCRGob>453$>%US|jHg4)H0l_2gvYiSMnWysY;B7j9Ubr4 zaB{quMy8^{z9%Bn%2`p0ZSaV4cn3(XTW>hm$4iz{L*x~BP$^(nGYs5CAZ-R zBL+TTsjo@mQoyOE?^j1hf!%CVRA&8Vla6dW2eirO#2J_kF#U{QO5K%1Ji5Kmfr(c!F(icd1irscB4t%KpZ^)4U2Yo2XFZk{ z5Am&)J3ygOYU=87M(4oyw^b4T^Upxq^Y?o|F$MBTe3YWzHAZ=1Wq5daL%@@2(9_W| zJ$Ufk>i`MvQ7?V#q?E0gTD-UCY%|lK1p&Q@ly%VUo1Ja$G;;zHm*s5CTwF93+5OFf zQ8g(!xtm1)RWLrM2TTAG)F^xv_0jf2uR~n+R+T!gPXz=7@3P)CM{X01T!-;#&<7harSLA=>pAH zSVCDk%BV3Oc<)arrzHq(o(I!zOGsq#Orw|8VrQg8Qxh0>iv*hpm=3Q4d``DJ^0msd z30gnrNcx^&~3P@;aA#jv-g?W0CZJi8oI;ut5mClnk)oc5c zb}-1<4mt4o`yuP)+C|zm_UXu`6I+q<(_JBtolJ~b@Y+nMo*Zmy^u)2N07z>Rx=_j5fhadB?C_o+PFEaC|30R{WS{|99pJb1fQ;V`(k>- z%frLsXlDsNk{A8(;lq&7XdTHxUI4AJ^(iM)`r7f;t{U()_gi& z;(iJE99O)1|6Vp(Mo-_GF9~2MH|W9d{(cQh%Y38hm8mIR4|n&@-d_2&wY8n?ZQz&K zM>hWm4S^|#8=jMu1+0seiD?y1^AG4-_Vd>SSXlPS$4l{$z1j0K4S*Jh^VJGQvgN7F z;xYE$B2g;I4Nfj_fMJBZ55KDy>&i%jo3PSe{>Ds=izC_ybJ4|-aPRqlz5H5#fjC+= z`D`XKF}_>9KrPUq)3&;Yi=b4Sf+e)qbokR<9@R!q7p)?7;Jte+D*&iC;u^wy92>NM!HxQxTD+wKZlSeJwV!cZL9ou3t3o zk6${8u-xiU4@q%|D3^VELysV&KUJ&*;H0~O4+@|xV4j-AgWmh&Nn#{0Ck}jMxhz&p zTwFE~K6sdir)Sw7z_|W5whUasz&L)HmJ8NjeFmfofa+_2u|YVyh!}GIX%s*}0k=AT zkY;9OU35Bu03!r{e1o+cHSa>LNLCgXnZ(3WFD|@#aTi8bEsOv2%c*3&3sKh8H8ecM zNTs*8=$Gr2>o-*ac=h)7rqet0?+&H17m-y^7+r`EE1tA#)FYym#_|2x*zUf*(WBj!*wj=`8G!YH>>R-w%D>Do z3@8n2PN>SSjep|F`imu#C4ul{(S%HDyTYMu`5Ov_h44BG3J|EZx3{-(<6@aCgM&*n z8gl#;&zr?NV4OOc0sV@31KMx9W5C228BsMcHDzIBlmg(MtsVzU?Vro;CtCqyn5eeS zF!Eb@*ALBvFfyt;`$$Vm4}3Rh4I%i%8GG2u=hOH{gxaumWUNaO=WOnM*3)SYYceJ(C zfUpFtnY~OtNWXfu=MXm>P0R0-cPy`?t{(g>s}taQAQF7cEK;Q|RKmSuyC2ku*#X&fOMm~~?iH)VNpSFeD0IVy~q#hX=iHQ%DZkzgHG$M6lun{;iox|t4 z#V(w_TUt0Lv-N~@JX~uU<05Wh<|`J%E#9(-I~q1aUtTbhjKOMx4zRNVXS zpk3{p&gXlYUPp9K{e~J27FGP77BJxG#H`i}~ZfKIU_y+0TC>b=Gfc+ok7$2-Hzp(JV%l z9t6`1mrQyKl7EnT{*67AQzdM`^ObOtayThyTbBysGqA9jB*01&*|6JJ0Qc(Oi>HPh zP>DWA2P%H1!&NQG^MFvZ#UF#qib<_5MhIT|cU{bL%+3GrB@n_hW-z6VDFVxTz?)pI ztC+Qnzs%ywqG0_87FKRoVgtve$(hbCXR|x~ zK`E46M`@1;(A!L}Z-nGmeE4vckczKR?(KF&(kMK*{&@XtmF)PBn4PL&Ea;!f3ykZ> zN|3gX_N3f}Hjf;g{g~L_d2AN%q*nucj>WJfSl~!hb@lcQ5#x)&M)>2_GvZV87x6%J zdbNVw02YLWrFy6J^)LUvsLZ*Ko)gWEDv`a0MnzmMS-HD4Cn*0*JtpCk~7G z{5dT*H&?vl5t#hsM@A8HI8g@o! zr?kk~;R2`eD}hOV#PcJOI!>Z`m1KL3-$+G0D8c25q@9o{KV%ppRRN1KM=};W%i*n&1*3AsGHprx*ydsO)kd*5juLSgaM99s z(O=avph(qMZ@iJK%;>#YYTR&>RFRTK*{0&}UKlOpCSF*_*G#sz+cQ~I74u&X=w#M% z`pj*I2Zc_CWn_k`YJ2t;>aAEZ`E+cX)WehVa7E4)H)n+;##e?|7XfqkZ>aN+)}y4L z6|hfhsjZ_EX}U&snC|7e=|4x&GE!8rx?(%a`L# zK6Fj>R5%H3_eDqHtiwk4Ve0xv?}0L~avxttbQo)BKx)Ug`>kBBo~9-w2{6+zMA>-aPBagmZW>J(iP zOjeLJ@|g;>^9*{4m+0cJ{Sp-wZL11hozGxLB?{fLe}!|A_=M7OUmxP^=W3W7+8FUY48nkAjf1H}HZemg0`Aa%+l>vwN>A zFMPVM)XyGO^|+U*iPAMm4}=9+2sclK`zrCPQ}=Qqg@#7)%ct>jWq?_f7WE-8&=N!M z^xbdz8Wa&vBM(80*@>NzRnq#&=ApH3uI0tO@FWvZuUC5{;=b2<=Op03I^)Kqemvb$ zrb#zzOZD%{|F9#LF4mF;Wh88>e$fgHgxR@4+uT05+~03JGWSZVHS><{0rR6JB!XGy z!!Ixvf0;`oHE@}b5*p=biU}%l3r6Em1u{9lseah6_4JVSA-L=f-zh6NrQsFK)P(2r z7dy3~Pc3n~O4MKDs$!sa8q1&74RFRB|2JrO=>-4(x*&}Npufjp-C_b-P^I`cI2sy$ z)2_BHY6~SNrnmwi_MHoF@aE1sO=<~Gke`F=T{U%a@pp!XhQ$J0XP#|d6QyUvc*QpX zu$%bnZb|{Z>pn$%E$_+Q;UN69u&^LY_=(tWEIXChO+(oQG^Y`C;SaHj#lfQj38~bV zTQIOXwpO?vdD%cxgp@D?B9sbEk{DP&g37N{oy zS}72}@b78<|C3|=su!n%;Rsk*@&~rT+8RSNJ(B$Nxy|qb*Qt?RdOY29z}&Hd?^=V1 z;@f^oHG}8oM_hXDNcz^)nEL7*Bfp>f9y>b+E`}0nJ4AH-G2cSeU{NCDy z17#rywZQliaQr{O@xROAf5ILd?QeKcrae9J1{6zf@~t)Yy^pJ;20>Z`TIXWLuHP`W zbj7HZ%ebeX6=t=(v>daJ0&-oyP`$lOUpVV{vCAZChLZZ!S@x}w)TnRH=In9iFW*hV zghHbv*LWT34nD-F*A)iHb6OI{OT-dOMnOB#_yTBOZ*FW&g|T98Ts;Q^E_2>xrj-MZgeSZc7G18th8(SX{&xTZ=-hsd0DbdNW_hE{@ zgjjNb@ENMG7zRx4XK|YcN$N_`4v2i0nO9~9t|i?K>;ik zS-3aB?|b@Iy_sJzaGJxe+46YucePrhE49z*e)Aq;B4P>GPjeT{F`sCW&tZs``57QNSQT6p{#omTyb`uCc+EP%?T8ll(w~{->+U;X5|;@DNRj{W^}Xbx5{U} zUzk1B__V@$aoEyRR-=*g+viTbauBI?5Qe^ZGqak&EHFKGgCq$J);_@_q06 zJEEG5GTowX`W?+OOgl8dJ;PKp?gI8)E}BtsG=^CP?_P;oN&1dsS!|@MLOp*8ER|7y z@YrNCeXO7DS?v!L16kxS< zN)BC)7<4?S;gXzmju@?m`0Czgn%PB29^qcKSZlZ&MF*C}h;v_j%mjaef`%`D&iR+LZpS81|r&ncG2-2B>97@K7qvJ=^8kd(~` zUTVE&mFpUI!^a&f>wT5wV7b!Odwe4GxkihvY-RG^NnkGr5ri|KD$M?#)xGJ`POslz z1y#WE8IO=qOi`WhSOLj;qBt zCY#iU=Fyh3<;|kr7Ipi!v$?sp{IQ<169)5?Pv>dF(MH$rben`-#EeH?Zt(gN(cy|g zA5?r-e^*K>XDAeIZd9siT)jlTYF0f?GLcqFSy{uS)2I|$l@$n<>1metF|?h%;5Avm ztx5U_o3{>cg0Y+7^5G4Rw>C4eukpM1xUZ6OK5*DhX0J`1KU9n1xvseS`z@Ws)P%(R zAR@+xLyxg(I%nXfyo8~6? z&uTJTDvnCG1_`g?ni>H@T2Wmx*{;;vGsPXf+Pm}3YQk)~QL5ESnWE?YRB#?+xi)g` z4^ta#MUX0K{)h&RFzqU}umLW_>9VX#vrDe%AyHe}k)k8YSSq^h~6y5_^O0*pLvlwd(nJ_g@r)=xfK zR$AxI?w=K|Y7r;z5U7pEUj0R02_I77*`)hr3INDuEdXu)wu94I_3R6~vt4Wv0WT$q zIBhmXJ6h@qbaE4=XU4l1aQH}Rs`G65?ntKEbsPOgIY17}i29zDjqmXO*;FK1^*X7! z&b$70-g7_Y2bkZHC4OmjdsG`SD7#6eV87U7)^7U$;W+NvK zqX7FE+9T8Zna%_Em{cIeOH>6plTzkR!o&EBE5<`eUi1gHV>yYgks#1lIgagcp;o!3 z=by(Q&jfP)FY>S*0Nc&$u>7zRK2fj}Qva%UT&ugU`?KF`!dktu8CPHZ;+@@{MZ~&> z_3XxP0-3-Dp8_@UtX#b8h`VW9pW&2a%_YF}@x9N#>z8HT04xzV2C=u0ZfBgb7_4u! zfu#+HbAvvN%+k>*$M6K9Q}?`?{vJC9a=TPEAMlxEXIgd?u!e4&1SQPm4>mM4;GB{F z(c@EpT5v?>aq4EfQ*>L?nW|l{^ykl?PaGFJ9h;HMP+8}VmK5=h_m+XV=ed8(yfKkk zc+^LGu~wKtAucDrTK)7S;2gIOSE35>ZC~W1XLwR7WkxihuindDNOYiBfIgrTGgls7 z%$4Vbq&mD$LXh`$^nKG8d>=q%y@_5-N-%vlkWOEqY0`ge+?kPB{m>SnYz&zlNW-I| z{Z!u#TO>kITaiP$dnT)Y`Btv;s&*vVvhRGYuMeWjvpHx2i9c3n(OlE{Qb2D!yMU^? zSrayt5VhvCb{CS78imr8=|&L{oEM>S0W5Tx&y50ByE~Mg*|Xzbg|U>%yNB{==#T*l z;lYJxBmdQ)AaG>?pzfB23#h4kCzAT>TL-o&?!V^*?Yfzi{`6K!xS3$rG= zkfDDl_t;ps6A!`o&Q8x1jfAY6oa*{$QKkE~;qbNgD(|0;fJ-sE)J4bI;KNB?Tb;97 z={zyC^4wT4Ol*)dX7!?JfKoClc8Y7uq^rlF-#qD34^hv{nK--0By-~*df@0#??r4_xx6h z7$-LF>4ohi(Lz7F^vl}XE_wD7)r;mHH%>C4I+>?;=0^-CZG5fM6@2O;9(M)vryq!m ztW2?brN~p!_^PZ#pc%YunZMm03cqG}AV7@FAmBysRcD3>T73d2nd0H7(MO0;|qvOP3x& zMh@Xz<-c_cy(9nbr`=j6gl{u8sqJCcKwi;_YLUKtmpsdwk5GeSxw7xmjWI=GN4Os% zbkm8CTLy7{#)#goGz7n@j z;-$+-6wK5d;BO1qawXo)yaGL)(LB5N?GtsT0Suxj?O>RKpoea1vsizrAH$Jx9%^iN zw{orAz4vI(D}c{j*}6VQ&4O#tPTBC_1v53?M~e}9JB(o3wh4z(Q3D=CR< zE&zC3Ga7R4L$>=vZxUs4iqoszq=uwPNj&M)#Dwm_*7kU9z#G}?{p*1Gjwtu6N*30^ zdL&5_=$oyeI;fB4Y5KytpK5E8aLh5ylGzdq|D(xLyZ-9fI=t@&q zoJ2mpD{KKotC^kijfGLaT4Kd<@ZopeWR z;=W9`U{Y~((mq)YtgXbz$Vk>hNViz4+%)m2M_IPq7v(wkB2U8D_~@cf6;2b49*t*D zEkZ^)y);CwhYgJ{M&nrxHT+tiu;DW?%NE{uDtj!;+1@Mf^7GLvf?)bQpZ+t?^MDSQ z(>Z;YRiDsrLE*#mA2!t*)v2L{Dv+p=Y%d7AchejXV!BCbM$}hZ@LWPh+0`^~A363C z_?c&<7fTl`l62qHs^j^#rTp=_NI2?K6m(cm27*u@(x@llT^m(Ck-E8=H^9+*?Hi&h8ac0ohqLqknMpsnfdt~4mo4Sqcm;Hf@-R4{fW;;?XdAo{L3iQ{?0yoWp7wD zp+=9wbfpi1(R01)I_Y?Jx#yIqI9UO7u-&k!@}_sfa^zbPT^<0Zc!f&! zBPSrvz}kbx!Kvo{``)!AjqlBDDad0Bs5*3gTHD2|Vra0xss{{E5s`N1gbXDbx75gv zXn3%;kq+70P241PRiQ#VjJgL*k4QG#ilu2I9yvSiHsK=(UVyi`;O#va;itl!hCOR2 zd_Wa-QmOGe^tf!&x@|N(3RAhU*?iu#kDM-0%8=|bWIjt(M|e&ZqI$^kTs3Rxz=}%R z=PZ?fBwgeDReqJZmFQ{pDbBkQzmb<{BjojCWC1iER`Vq)I-KPZ;wv5Vc2H5sNBHYB z2v`9oqY*I^s2VMV*M7Vrub+pfhw#E};7|k&F{yKZ*@tCLxTuUt%UNLv3)8o7y0txI zh9zHqeE0qt329>-eK>n*=~7D={bwwU{)7opZ{C+x%h$D@d5zp%_# zRt-HpSN!zUdI@d45*>PWE19FgW@Sb zeoZYtCDG&jby!5wZZn$3b2pV}^~ZQ9OWBArrpgQcohE;hNx4f#%BAC%CcxM5V&;CU ziBXcCg>g*0pGN3rMxdk%r zZ!pV~$z;82`3bd7E0*n2yL&k(9+2AC)wP8OR9cE$!RO%q>?SI!Zl|dAv{1q&n&8EF zPJa*U7{r#W`kl{kIck%VJCWClOjc2sftgu`iqD#f_kJ-bi(=^0%>*l7HGKzIi)oLB zBwR}kpMc~0;s=9jRvr;~U+m{?2fh6&>c<{x_;|&il;{8acbKXjdgOZ*%bxk7y1|~e z5PNQ?OfkpeU_F)0gTrt&EPsZraI8sqg-v6Ip4QP#NqWW9a-hWLOUdTlt!DqjveG|F zy*Zr=%}&v7VgBDHxsV~GWMtWZ=8ijY;V;quW^{d@M2JR}=OtQ7o~>#&eOkD}NN><+ zF+4b$TK0L2t4{$`5Bo^h7Zz$?KA`h{>3iY}B=$R$_rC+p zQXH(W?=NuT-v)y_|HX-ak>5+*|2s~^z55&S;oqlU;5^d*!g<+kL&O*y2aY$6m)d?V zxAEZ;75=_Nh4i<_|DrZ4;YU1FE5vOTnbo9c;9W31}P!4kUnvxK3 zj=mS5TQ$mIq1|FAQwEeZx#!ss@(tToJxwBLWoovfY!9%CD-{We19_(0TqZzy*8IG( z&@i=wPLEscv zSkqr&aDD4V^bn8%6>&4}3xw>N-=7(^e;PE35Mju(H?2OJfssztrTb1-DCV6g<}O{S zW-+v;MR+ao)SkHKuN|7p48b0m7POYTmq;b7@MUE1svc@Z18KI-o6Q~rI$Ap|gg&t?_Ln+k2L)VtyH>2>q+yvXhGvN|ta zCwZV)_(e^YfArGwGQ5|6R`un}4{>Z-5AWjUc$l7hs;@~!LwCi9@?b$kRxl@7`n%nN zPWrVgL)3`JQOd^sJu2hb;bnqm^{ep7Oj@)Wqq<*7Q)-7rOQ0Fa*kd`Zu9XVEnUyk4 z_Bx9;lIaTXvYUP9%4WpAuDZQ(u^IBj`fAlM#{7dQ%>crU+|7j<&iON@f<~_c%U>UF z1CXI?Yik=YYu7{GqY99c$kB`+uTd96R?3-tpn>^J#R=4Ei%H(=L`7ul)Em7%Umuft z;3ab1%?1{~jsFCZ+`rfFrq0T`&@jrWqdJem0ZEy+nEL62n6{t&p{zPGvHJZFmx5+}!hPiA>_2($#-sJ)s^WL^G+vN#64~6_ErX-% za<&qXI|O@bJ8@`Bp{fW18SoVpIit%wOQZTGluKr)_80Nv9UGxLeg;JoQ%-kZ%|vOM zpGt!Y-i=tXuU(baNAa7G@~SjDKUOvGZK6 z#uZaiK9nVkf3Ke6*2RX4vG3{2eaSk;kkB0Hwu^&DxxoV3Spra!{OjkwTt*a8ZHUBT zv+eKs=P<=}7@?Q*pU~uEhhwRbvkiy4NAXiqQB!PXkjD%JNqXwEoN9K|NCEYg?A(W& z+@LxLl;W^Fd7}E@!2`f$#>U6Pc%gdGi{x4U*18M#eHyS=wO#J*e;q+mE-Oa1Ra@P{%l z;23bS?*Q*7@HQwYh(RtP?frG43o`~a?D$`sy&w1k+6Kov24$5`ATw?@WbE!y&gY5Rq0Kp%2bCOanDNS?(kMT zTbOj04v_%T>$v^}p6z6(gZz1CU3Y({x_uBs{%~~|P9iLN+LXk@FQ!-`l4R0Z+^~R>^NemCXEvvs^`N)lX8;ODesP6#W^PD&X#Gd@nQH zcl1Qx*O<3SxZ-B~`u>)>0$H^Z`rS04>o;6MO2wKMSU?aB&**eB754O>e0N(|6mON&kCmzkL z1#D#D_2bH@q@<){!qha%bX0NPwPyeOy+`NT7-Yk6iPfauNFuMbD2@JW@32r0Sz zTv?BQbp6SEa4qh;xm?>u%o>4)di2GQ2QIOWEM29lfMfAbQW`S!0Je}1Dl+5sCw@TJ z>uG$peq67c52Ska&&Mo)AK`06`f7XeiZN@6fxX42zwdb*p2Voc=Kdqug!p+vGN)q; z`z_lOlPL~P{c+Hq7i2_MO4D?$ulv3@d#IN%RQJUF=mg64?i2ugd}tOUM|4aKW)n>u zEQ}N9MR4ooewCyjbZ^j;pq-6fb`_tkU1jc^@QDBsiT6 zd;Ue29P!#%GKr$5{y$&jTfI4N0#k>53*BnTzI<{Ut0n7i-NGjd?r$4mJ%<_I{Gk#w zQ166);D4qH)xj8qW^xu-tJbDwGQ|4dOjFb3OtwJA8y3kT&d}ru&F+aG8 z&^)k+v_U2aXaO_B%hfOM+yz;=c+F7g^!v{M6p&nG&gTp{*o2-Lx{2UhQgO^)3wR;w z2I0K>*ci2|Jbh*py0K1rN7G>8y@Q4z;C%BI7%P1o}2}5lbkVqwiN-%?W zFJKQAi&o{##560Cw)ALbhc+u*#jV!mIaPOk=#!Ts2?HDLJ{ zhyOa|1^K9ZOBSo;FDz=iK$e)VxdcrwN`&t(yZrp$xVsSPjsF956_VorU&+Ka3PW5h zf$RTCo!(U9{u=Nf>?Fs3P^Y8+N7*xA7Xj+~w!@8K6Mu(Z?Bl`o?v3LGz`cBVQB6Ph zP7@33Uk3JqR*ZjX{4aPVz2GJB9~S@RCI3e>EOX6Cc>d}q00IBX)n0h|3`K9UvXNIv;v7O1++1~k^JO=l?%ThcCoOsIxjfQ z57_^5njl>b_{?4FzT$7)7(P=~HHM5+U)S=DbKHu;~23@_@$?XW2LLH(9Hu&(>s}ZBG=t`+qCFp0r0BEHjM*p3JTL zdiN`#M}W!{x3$QTB+kHJ{FT!zGx9@=wg--Q!0%R|-Alv1IdH$j<*u8?vf-Q4#b`wq zObJOKA!}8UT_u)8)jA7pvt^e40d2LL@;a66P)Ud1(4&%e@=4_M7ZsN`D}ZV;WqO$Z z9_edXZ0+HZcs}y}*l!;xESk^aw{W;Jv$l5YemXz%jiIYqkKSqi2#lHg_I*^%uOKZA z$+3?EpCGHZdXDSDp1W@NoK5rfbJbh7Z@u=!&t#1LQrj%Xa5wt%wU?$*9y1<#y;H7D zJWid)DT0D-2UW9M9*y_-cjP4H&#Y<})O}Fg=SMJvz%-3DpK$!tF;|$1_Q+Xy=*N%J z?=#ahU%q@v_mH{FDj`22#0h4b(|VpPH^IK5UJE%Z@ft%R4+zjZ>R7%$@JAu8n`Y~* z5Q8zfa4(c4j)Z6&L1Vz;13mGTZE~`0w2313n|T*^ZN_qwnU%uYHL*R<({FgFt{>Ws zp1gPO-koxtv~h2ofu}BOf0~)8Gaxk6cP3nc@Yi>vU2VrZ%i1=#=6=#*X&(N3WQ>^U zVzldPH7$2q7{+JbCv)gs6?NX^XLF{pdyY+{$MZ#4|M){T`()W+<%+xSK1vtWUmN7Q zBJpaiBj}@aW6tHUaMsYSt!mRwR=7A%MDn~UPil0Y{B#!7beJc8B2s(3w`0FGlH$ek zkW0lE@26yMYBrwGjC}E#wZ7kE<-~S>Wd(87+qbG|pBJm`WVchdu4$+WY|wRBb>n)K zm6O8*AQjk**<0pp}M=L>kc(%g1buLwuNhIznzpIZI0xu!{(65&Yw*2GMv?4K@(fxo!Hk91r=sgB_s> z*>~|mdai9@H`5NAvTE5GB$$TLUK14Ih9Zq7sDJzVFc-x6{;sAyk03ukBC_=`xb53a z)M|=PZI!^_+sx_wb#}93DLh@zvt*!-=Dj4mmHGDV+k4#VSfj&Vyq_@f^q{tR!}uVmK$<2_0VdoY?d?r@(mX9KUQ(5Q`)8a< zBnlyU$FqtsnLk?v@2uhB9k1$)u8$ePh#{OFJ2mb1BW&#KVW0@V9PG{lAk$x~EeK1; zPpg&mk&wy;`?s3h&(k`7Sr$E2LDOgZ^6Q%0YhHU4Pu_L;oimDDyJ-eC(GJY7iWV~8 zePFFcw%&Xm7%oLiEL1EJ#6hyW;;0(Yv`6FlxG3V)qgz`V&`&^1FK>r2k%0jQONVU0 z>&FzBGr!J+(qA!sXD|RieNdCcsI=>(<72!o7j6WFq%Oc1$&JQr4tGP%9S!*t2^7uR z7CR1Ihg#j;Rz8EF%J@u{+pT$GZEV}t&4`AV^dB=i=xKjG+2yO7;j0qYfxGd_$jTal z9n_y_#hSY9YYOA2>m8>$d1{YikZ&ly3YZg!aRIie`rM`oY2%sa!!BR0i!u3qKLcl$wj;MkKHZ4PA)V_*0E^NtEB zxZw$umU+g*)-z=H9&Ew#!y0~a<0{TPN5-p2eMp7pYI;zO$4v8T^;VsI{6m*tsgSGL z!M%vw;oajhe5~(`?%gH*?7x;B-w0U8(wb|z^X%F`32EaBb4)t*knkQjeuddUEH9$; z1MDc)EHjn~!*&4awOZqSC+Btk!l~nL@17F0{#x5t^__dr-QR_CpFy;A>iAv5y{{bn zd|pfFzA`q){Y`q&Qf^6h9J|QGDZVAui5x!D5#~oS^78BlyRD`Le^w%ann*6ybo=Pa zxMBD8LaLR9AhD8Fe{|HbYZ@>h>@8&PQgsfQhG*0A$~`IToRKRyf0XwH+)%i8uk|g~ zXsdFVR9ZqqK7INWuDX%+Z!N$J(V3-SLZ8$|J0kp4znUTRwN%j*ppsF~<`8TLW<_i7 zN)$!lbMXv@N@hJE%5NXu83JR8{Fo_rY?+%#Aj|mLd+zJ^V$WQ_Ym~#!OD%J~3!95C5+-c?pjaMPRz%DgC?@*st`j(mlqdJ7jP- zpx%drvhmfV;M>b1*pX6+4Sag%?UQ1apU=_7w&u$>z*l(@BqShae0=#ZOYY9i%R7~1 zFCTYxNVoLg&jw|kF0KQYVy+haJ5!!f!kFjNd>JuMZv6WsItf?IT`oV$-i1m(QHm$O zop=5O_2KeP@V|7-@W%_u{bQ6Ksda}2Tq~d@mBWJ8+1#)MDF^#d~ zwqXWzyHnYa(cs_4o=9Pae0lt@CuTY%U$iY1kp;dQ;s5)zaE=f3^WQGJS@gG?_uE`9 zE|n(Aeo2M$08&(ZUMhWMx0|(u3ov$o!oKEjvc}Js=LL2cUB$r$iHFc>+YZxmgYxn3 zlc+LGm%yP+yGGq8eI+_IRgqw8Q?LG~UdWR-VmPoXk=Gb*{qv#zYVaN4OJidLOti)juJ>+Uk2A4& zSt?CEm}GO~I!YL{61kd{rkVM6`vg7D@%-Bd6D?yiGo$ixd!c0ar!>96?0*NM$jn4$ z@#e`l&2W;Qvd*rqA4`t6iHV65--*A3hlsjI+P}W+vpS~F6u51UbS5Sy^V{wpBO)|j zvnh9wz+`~N)QW0qDC(#6dzj;%$YNA9B4%2Hf9t{ugPq0BPkF$8(3MyX{XBa_ks9%lwf6L9Q{qphvOasx zf2NE$LiL^hk(19nN)3vE_;pOW3q8rwk~|*}Le<03U}HP*tSLHnjj^D}_%YZ(Em!Vp zWLIT#_=P{UhRVPE-0@1DO+>!yc_nMpVunWQ1E&u4sKo5YdmAzsmo;G|>SaZI=Rb|J?xUShl}W=sLR;dfxM1g>lGfOGejy_ujQI z%Kl+~NLLg;1U%bi*00UXuGeVVP4>ll;WK}?l-ksiS-(bCHw8?eaRmBHO?Ql!H+xSPr-=epXH>W?K|3` z?lZM+daWjIqA~|>8F2;*UA&5Y_6YGD%ds_4ONL}m`S61#-&-We)owpetT4awY;!?z zJ_DALrBX5{y&Vhjl`hB_Iy6}-YX!d!@$lhL3$SruT+%N?{O60c!{?OIPALbirhh!E z+Gm)ur;b0IQQKF_Jv^9{-|$>x9fBO=uW>>Gmx#VQAy?ATiQyt~db{!0?5^V&H`PZ; zKB`w!3BC6TTA&df8TtGGIlb-K801nsh4vb+cA}Cxpw+ka*>z1`wRffs!Q1s%tYbbk z86DO=I853TL+Pe8uTKp4id;+m1uqfJ&gG)@SJd|lf1bf}vw#6J$#4E^*d#Cs@`9h| ztUD&;Z`|*x+4i4VUJ34SyOu+UFa5L$|71o-0xt0a`{UXuj-SGVfFvGf&FbjGGiq+# z{VU(*hc?XsPOWichG+gSoT|A3N(ojC(-)kyZ4#nih1H-#5AX$ z{MiKclV)d%8Jm*=>rL?60omXO9{@Owii#2kNR=ag{T9-swe-J%)Gn}%n&_XoE52gc zy};>ey-8FHJdEDATgZ>eggnl)d%a zMng$8q2FJnWAyMjCd~W;;%S~nmECOL6{4pt;P+l?k#+0g(r+3F4hy4Du5;awCv&iZ zN%uW&aQSbyghx%re!8o6Z+$?Ui*5p#!Ab{g5Ah})THWM z)v<4wwNU4ujcbWmIXG+29&Ekr@{wgO_e(QI%RCFhLtOVic)RpWbl{EvzPXZoFBaM# zn|{_$xeVefgD{J^2g%koFRx3g0$UI;%hBt~Z?)}iQdbBkAql{_dYjC;R z0!!*aO%uX_u48*0VK=(M` zVDUoSe)c8Dt|=;ys8LQ%{Ejb#yMJoO4=gL&^?X?CtIuBf?)w_viPAhk!zwmiaL;b2p0*Cq{&Q_0D+NcAn7~AK);Ek@Yx9~bHcUgH!>NVp8P&}v)b5n4xL>8W zxP^k>*xRtGEUdHynb=O=2y9mIr}C$JH-BW;a--$EyFzK7+nJakE(mLqB~R+*L#>*X z9_Wq~XNp3vxwp2PFNHU`7(uc`xy%)NKE>Q)3gWs3d+vaBx3Y$nutK4w)KhmjI#ZYH zV|#{Ilt3miw?(g(&wlfv#}F||;iOD=%n$mGE1F}()9&Gxk~^y@CZj4cUdqW86;Hrk zl@73bKY`0Yyz&9}v(TG*t6qP;{HB0L-_7vi@tz8JqF|wf;3m$Ql}O^4uHCPo`F&6L zvjt<$LU`DUB5d}BocBZUWgB(0;&OT(vDVg@-dDHjjVpcJdxbK;82E}y*)g$R88{o| zh}diGCQ=mVw}r6$@=Q8j62dIjxUZ)SWqV5z19(w69D>MoF)%a5BuHZg;Z&oM9q6Y%Hlr5+N%X zi%{F>_~Z~+g*x0j9yp+;{X%lwG?<(6gc4e=TMu_rlYRBd5adWPtEhJS8pCDLwPUZl zZcuiLz~Es-9(M#NXOOEq(?5H%VCz`^I9T4CB(V+$KQKiw_(4X0#(mDDgu~2%=;J09e{%m3k-662h*!6mSLi^4D z&Kl%_H1wr)X!!c(4_a))N*L=D3mj4;fj}Cy09x7U- z;m`H_HtJ}OSFMBWXfCylwekm&}kEpe8u-_GU@kl)rdEn_9jX>YF`wZWV+nuOs zsotm`c?%+JSn|V)tM|XCcOJQ6g+8y#SU7a_ao804v3|u?-`6>Q?OyGItd;rZ6WgRY z&1-&b##XcfxsRV=+)NNNz-bixkep*Oz3eUQt&g@WOob$e7MlB5cyMsw=7{prp~3?l z8BrFj-k@_6wy_)G(qB`&ezgU^>3?P9nm(S;GbhOH@#rEiPi;^nDlbOO6<4^l-zwl) z{0p}n=>_HbTQ{?|h={sw5?(`V(tcFS52=)n=gA`K(9za(GH9R#sK^WY`cr$xq`#Ti zENqy(HS~gFR48hu1c#p>VM*^1{c^zkkzkPQ3yQr3dV#U>YtCs#SM#!Et*E#1`!3NM zkz6r<5<4&5F~_Kg(zDgvd1P}WXGB&Q<$-7rvN3xC5h`kVrtB;B%C76u;`LfWKqS3pe zO8k+?8R<>s@>D$po``tig)X7q$npoJ znZ71p?33Ltj;rmD$9hIcM>i5e@7Ce0hBU|R+t*)FdMu9E+XblQ4!A6%4$+Jhlbk4l% z3EXR{sf|Qw2u@hpG@1(FaQ`5^-~R#bOB%=;$t8)LeDH7HQaQOcILtgjC2wnFZ9E|V zfYUa2U1S(pg;{;;u5(IR7nVH!ui%;A#17X&b4qU}v^A>tFGcz*I%H{L>3qyH?6ksc z3~#==BcNQx)cC8L77L>l^NdMT$d+T9#7ErO@U3>ktC@Zcis`zvLvk!JhEBA1b6oWI z1EnR>M&zNEM W8W1D0r1C&GjCN`%@-x03%j(M|H}Zni?v1w%Ub1ytsFt}Yxz8eV zgx}7zjy{b=llpZTIWtj%@zM6QgAH5&!ves@T#sQCPS*J9JGtzPjc=QEGPd&<$g_hkP8 z9eU}KEC+6IEzb}Iegq_=)h&`sm6K>FZ|#GmwVBFTh3FY0K7M>)Y01hSja{wH!9?>$ z)~5U~V!P1stJ*aj!vv@i?z_$SM#jd4URRz)MMaOMgMLRIA3H;G3qwP97#J|h$3p-h z;0(5nX6V`($ab* zS=eKu^7kgdomc)>kFPrl%V$Q6jEpEsuS2aJ5`v5PPZg8aB2@U+<>ck#xmk>ot1;+m zQEiSt!ZZJaV)pZN(%v#rDrxc2v9q(ctsVGP^O`+?&%*OZ%^V15o^6=5{fjy=NrYF! z`uyg1Rpsb!7}+>fl%}|bR=_GtQXC;<8pq(LuNP1Qqk1n?KM;rI`zGf*u1>G>>7LTm zCmr#Q+qlQ4hE!XKm@Ka!b+{jPp$<IuJ>;V6yXjnj_^7hkwnw z2ZgxF_2zb6Wn)Sk5qDvUzB+G}OgY@#>ht8jy?K(Lw|aNZo3w{-Y@clt6WcE7UOpMK zYe)TQ)o`uuc+35Xwg{!S&jf3k&3SBFR_@xWJ^}1o*GCXL5W5U7V(}p2;_zwQlr6WF zZatAy5FN}K(ZBUED7!egr6D2^I@G7Yp#K`*acWl=$#L?8JvZr`aD!Ou$vigZ++;=h zk3EvSiNI>pY(;4;<_uk73N`}7I;iKfX7<#G4=lAZ&6JNEH4Ud+5VY?ztSuThk|PIA z9C^`LOGXQMQKY^w)0?PubE?tVLjGD(?6_q#-D{<__a|P?{-xJAF`j2I^9?6Mw8$IV#w~%-rs9UKOkyEch_sa(kwvr@9x?01w3onU8W$SM zWkFc#uyP-CK(dbh!$`l__W5S#zdJ+0c5-DtT*6XQbH*sm+-~0>%ANKVrH9L!IgLr7 zWKugMCnmO|tu8ugTJCf|#ZE|!n_C$!`>}WCaGh)7khy%>t2Q9!E@h*BREYx-dY2bV z7H+~y*0IC&9qV4Yqli-HP^+NXGS#tRB=?5i}z6Q=<=JDRXo~GH|=asdV!p}6znpXmh`^t1B#{H8#kY%T0up5?b zYC9T!5oXusLm2MogyR%}US?sJO?#E{Su&^*AEWABQf-rEyj{faHI27#Crg{K!+1ME z<4JyHV?U+-VlgX`GG4L7oIEQ3T&to^TM%uxR;}(qK~K@f^AUbKcj_V??T14w)1L(D z6iPwfuX@X*(FdJXyXrGgTa*T$& zN>Pt7J=sv>Bkiq+cSzfhkCqA-&zC#SUa@Ubv32SmYQDPiS*)Mk{FOQ(NYK0ac=Jqr zcCv(|k1NjW&=$n{BC$U_D?TlMS?^Ara6Rn~e|(Op@T>kJHmm9blZ!wOg7+-rD5ycqA!x%B-@N@4o0*s#|s!rHV1~2jeCFNpdfoy zI@%us5r8EkBX`XI@cuJ$k0%M8d*xS=4^7WkK2rZ}uuQ+^`iq-iGcvP$S!;mY>liZX zIq22Exotfnk?j(}HK)yc(cWoEbwm{E*hyqFCAI2d*kF}8BIRHU_jgVy*k z_J#NsLy5aJfv{;!=c!n6LAi8GJxB5aiywiLD?gIfR?b|xLvl;7gZ>F>NrfX?`hbC& z+6aM`fG}U<;CZewUm$n&Z1lQ2ar<5eqkOTIUJ^r6I;-&A|tGzb*S@y?xj~a*Ti)yU2~KPEE3WH5 ze^$A%3xyQt#mL0xiE_#jn);r+$KJEIt(X{D5B(_w-Hq|&02YQrZ^d#U7xgn+7lQ&} zeD}KDq9a1WWVH?Vb{hZHjc|7$-M`&NrU96X~BQEa=sCgH$?Y z&bPL=YWGQ8`Ify}n05TV4qdsWTZ3!n_*c_sa2T%s*UY?@Hk0Ye6(Le_Rwh`p++*B6 z_|-Tv{ubddZPYETrxb(5(_3r!N&`_Fuf~eB2R8!V$DgJEGeh+(n*&m~|6V{qx%sbS zmob-|8$`kMzlP+0T}q|rtp6(o{_keo|JAn(S7!KM59=HBu;4<0)KVDnzr-5RiGufN zKK?C${@3cNxE*C&2qe%c_q}IgCkuq2-~GhV(eah0_N$2sE>OcT03p?23c*h!BWjQl z^Tp#1@N(gNl^;Vy`J3Ui(pi5qVxH@PEc)ui(8il^Sxy99SO3k)YKJB)Mo!R&o|~In z%}Ip5orwtpbh+n2`qgNx#7r((ED>bQXC15Ugc4JJs=bBUNBVW)XKoqN|2HkZr54o+ z>cxUncR?5XCn(NNSo0$)lF7V+zZG~IRl!eh zg8;GBJ!k#Og`E?iX(1KKB%l*0RR}RSB&7qXS&On`p=HOjW%uG$CIrqG}nGZ=jp0=y2YlZB$mubVpH#{y+ zmWhaohiVKfA|A2*s{Q-H&y^Lmnu=Hv3nF%P?m*?f-6)Z?qz^*_-ASVHpg>1u&j!qF z5zs>u*=ZrP1SR0Fu)3>R()b3nwRL`fK#nI9>KH^dGs|$G5hL zRjajiPrvs>f10Zz+HBDcilT*YmG;Q|QXzTg#!Oab(i}h)j9gqhAA5H|_?8F6#Swb% zd^S_=Y6m!!LZA2de`6u({ErLp8Zn&!a?$th-D_AofFOsrRwJchXU7HojMgGnU~|jD zwN}Uy+!Qo`;0!=hDNSWv|8N*AW?f^gr{#EANOoS;_o2634O7|E4QI_|t9`uILK3b9G?8X}u-TCif9OGhio4oV_d)ti=x>oDe`e)1ma|hX zAl+e{ohqs5X*mYJ+)Z$>+qYET5Xd8zz0}w^Onosvdey+F_TWp0)Kol6K)gGJT|`*3 zgR-i4~*N)|im5lgTu=x`Q zUw3o07l59xy@Nw%Utjy$!Q9@->iW4?34+A~mu+K=S-9lC@C45Bb_{US9vX{ zY&b0NtsUnLj;lkrt>66Hc@r3F3+HFDj?bKBiBq;s0;%skDdJKJY3^vQRw_}@P;@hnKhharWIDmPl3SA4FgP^&*^CFjdFErg$$%N z9xV(Ff7BK46+P0@0e$$VO7g7J8>V^CxaBlbL_wr)}{!y=M!MXQ=w$`rvyq zSpTqFJaoW)ZPTn z0lM-?_6=<~&9f&@FkljcsoWYS)Qyac-e=K3Y_|YS>RI*_jc^U!l3Gt3Dv=nN8rT^% zO}3j0U__yRcav@HD1E<-11aOCMwq_diRaZtLlJ91gY5`21^Kau!P`#hH|8`akaj#p523zFrx~#0R*kdNLGHG2XRdl_gt&7+y01IGfD+=_v4|{>4wv z)0IryXvJLVy!^ZMo`_fsLhK(wcfDi?Xjj8u4h5?C43Ca0*DJT_-Pew8I9(Q&3Jv|b z@9Xo}qOL(tijcW7lI0sToK63DHSWIx}okfIDP_@TA$l&GUi-O6y-!g3AgEEsE#6I^~ zMIN3PhVR(dni_w&6K`bv%pLW2nFr@gMga9YK9kan+&TMP2^)fa2}n z{!Nf!HuqXcMow-4RN5DZG;Pws5dx7UC3qZ;dL2tofzCOV$G(zv{e`3J;d&?NxPv_j z7X%luBbeWD2IA3y^(xlQ@K;MmXBJaT!8EmXuT*ajj|%aB?POUt%xGb0iV#mYRU53) zv8heCE-z`m1+Fej;L4C;+!a;zGxM_{DaLr2)jT+M814W01-vI%Nxt}J`Pltm?%iVt zh8BDW{QjRMW-8WoCnm(~S{)#PR7L0CGSYcJ{np_Isx4#IdolIlA>pu;D&~ zy$2`Ign*#_#NM78+!5&L>3yFNS-`cz0?e-u(oE1xTdC=tppqU|xfQ=UK_TVialZQq ztGd);7(p;H=}qW-k45+$AA^;p;(hqEYOq=H@Tgn#gFDBwsb;Gj6zR7QL=-@Ql?BG4 zXZ*ON{Huuq9*|7$p_B7l(=jqK@``@zS_Bv!H&D49ahq)gY^x_*T3c^kLlf9286f4< zLt=TEGXpB&1|a>ju(;?e_ADnz;g%>BKCHN0qEIBXnc)@#Co2=CP-=8I#FE1v+)-qFSg22$ z;+P!RnV%Zit~O{FVV=q4E7WbKNyvXn<$?K;s-xo8ZAsNcHORSn%QXI!IZWc+de*V6^4FYSCtci<{V`nRw$uTgB{ObJ{`N3>-6Uk>pn`Zc{e$mW@m1}pykA&TXTS75zRK6MV?{X z$QBkpajwVyNquS!f3l;Ex-R3}o#e8W_K$WKwZ|T0jNk zv9Y^C?)f#xd#3Up=aLdtYvuiP=TF}wV4*`K6j^S)b-9iDUyqlyoue($;$^;8lBwy4 zyD7L4EH^l|$csh6rd1^)7=AgWqI=Uphk7D2b9Oo=;3$5&po-u0s-#?6hZ9|M5AC{+ zu1v2L^~e2@%9Q!tWKE21H|utSx&8U+r6BoN%x+Ud86i)EwcQ z*KcBP7%q|!jzqet9m)7SMV*JEpE7^gtz$VrMmlxPuKCzyl;FJ1pw4*>-P(DUa2=1) z!KN~SK3}{Ui;#Ujkz}C z@Eg^E3gb=6JIJ`OWK)fI#!F8eAaoW0`W98=*2Gmo4W$+ z<#Vd)CNtoi9JMF!@#M^dG)mOHMRZq_C&sR4{Rv^xTx(==!nv1as}m!ojJ?y;Z|q<*Qmo&GR)UTSPk~Diy#mTt79OL_?_Tm2z=`g#EkG$wb_RC)|;f^{4+Sv1LAlaZ@4B%frl^ zY%j2rxA*vyi&l$|)Hs)kGsI5BQN(n89dXY+z5bMm&$O=`{`@W*_Or6;kXmQH{T`ox z@r2mR$}kBZ6y&R$$x)iCp-KGEi_Sk~VV9O34X<~sru1*V8FzcmHmYB#$G3YR|Ru52<=mI>PpjLj}UN zr0?_v-SoPak~@hOub#&y_}a|OtUFmeIg&*cc*yUWIZVn82Q*AVH{bSYN*0!X?B1w< zA(US*t=c^vHWAvh`NiUj@KwZAJnl3HuGMtGuEki|0mjp&@ww*sRQ+L_`tA8;>W@QP zXl*(?L3tN`g=T}?V`)cA(OW(n8UTd7=rW5KVtK7)UfqirZOPN5uEM2|*~8N=#Iuu3 zWMqH4E-H-N(e=X!K8j+<1JS~3COn`GOa^}}vCCt_B)82<^P}qRChV2zdT|n6ce~ua zi&hfd0_UB%pBo^73vPD;z*q6=H~DNDh6Z*3+?Y=eA-eva9&u{s`M6;7sn9^SC zd_sJfv@dD>^R?UDJ9Xok=hf?{BE6xn8TGM}|>ZEKak0__*c5 zP%_=>C0|H0@)_wzK}Ys7ybvtQUYbJ=)H_o$6VrYXf;*2bH*Vh?NqBrNni(yu+wI%_ z=}jy7dj0XO92)T_KG?%Di5Ba$>GSuylDZ;(v8YL8GQ>PHC_I1rG&AYrcj$!pvTLcn z5YTc|vos&?-M`HjsM)rU-mJGhsv>PZ<^v4n9TK<7g9$_AdxyloD*J9+o^Hp-_=V)P z;~BQ5qE9&OWK|f>VI6w}*X>j5J&Z4z5)op1C&RP+v&y1x6+3T^DMVIo>BN*{IU+i_ zp=6Yax#R12Lxl8}7mY&qF=5!VcN+lkMdBq)9JMKx#a5sRMasZ3yjF-2xu78gKh#X`uwY!rQ z!)_4~fy}C;vhpJBv!{qL0$YI(0RdT#_23&-2#S0#Hc@QSGY8H`OHJ7CRCjv1yBDBJ zr>CWT2S$5edp9LK^x`s>+z7(I5w3`z(bS54qrN{>3Z9xDERyT}pIixwPo)30Ax0~@ z&KV(s>Gdj!q}dohw1G`KS~qyMBvR8!+8r5X)Hf!8svz=2w#eHzQuBTIOkP#qxj{qI z2Ro{lSTC(_`n9XqT_}?4I675F!#0yrQFvZp+(ir^ma@kKoD%EFI6HnZndLf(AGsXN z9S~C}^4_gQ)ERI9i0>+)w~nG-#6$-*FE*JtJ$Fqo zR$nkSCW!xe?Q2~7R|behHV-`$CrGgJ{qqEZz6&X@exe)w9a<+faRwmF|$c{MkW z$@sT&xX5=8g*=5Dnx=|Z@-i}6TnYqP3aOrwJHRwOhPwxBQxqhS1egPV!&Z}GF4z56 z82ETPlc(SU_!T(c`)k8tU=0@!QZ+jh4t)sN{7D#958xBskUIY;GVZE4S&FZOx(Vse zNFlANj*IcPS|}{Iz|zjTa*-IyQONxDfqn_Zm|yw2;?U0E#Cf;AZkgnHYBrRws{qRw zBjUnsHAZpFX#TgK`uKqHueJ!>x zYRp(1-Ls@C-wx13h-*-ha%v7WiCBofQv3ojo?t*Yf4VvC#iGWiY z0=*V=Sc2W$0t!7bJf?V1oC)REUkU+yo}pXsi2$tKVUk+QWifJnvO3xU!ZPSB|4f%9 zfwBaQqv*J}3i}U1{u~B$jxRjHQ?CJhkDO=TV!j&AqvSE|T{zpSHw6QQ1h2~zkN^q~ zAY|Ny=|-N{e-r_X@o!c-VLV0L*`N1{f_R?$?TCG@}Jq1Xna;#GLg6aK+gr8JaOWKCaM9n7V8s4(W zJJC}kCL>|Y;`B>J4kneeH-bV5!Z^sbgD+HfMhwI30KTCv*g_}Kx}kBE?{yJG?RCz@ zd+H25mWs(naq{l&Zh+=yTJL}#Ydh?^E*K1fbPIv=gS!|5Z`=t)aDBk&5H}LwJO>cp z;ep+Sfk)8+zKr=F0fM3nyr&lMfL*xkI!PQ$7fLsu%!5TQAC1gDGd6QUB4}?dc1X77 z3Q=oY;R2(*Y^64PGBc0YY&4XCZTw<(1AagU^)e@;x`i4z#Yy|)Q9ddkFSqG~MbcQQ z0~;G`k1hMkd#tQEuj`Y&E@MGqj|=Q15rpj+E<ODm}I|sY@5u`iZrts6^H{pQ@&J=4w>e{0!nm5Lv2Ul}E z7sRSn@ez!x3c>J+HG9*u8t7p3&=Li6Rvz$ugc6|814d!S+l^lL9MbI3;-iA z4{Tyqy^fDFw*nEB=}%swQYBY<)%;;{08%p!sXUMMtt)0R;BmI8Js^fn*;2&HUb#R3 z>693l1$Pdr>h^#Wa66cW{_QzR;{F@*qaj#i4f38Q*<_}pS-D@t4AzSiuLqtXx@E9Q zxDa5<>y<}vn3Ia>iS^G-BY7o7$ofIWStu`=|>ud=i z6omc3#LAimcn>tyYKPk0M5s;=H)OY_YF*BEe&n-j4FTZP4hD=e({&y)^70JA!tvlH zjF&ke1IW-r zra6fl2M-I4)l)(v;^KbHwFV*DA11}{+a8_O?)S^4Y?XuX&?ZH)$D#V4v;4Z#K&U$a zco%?9SGLu-N%0Qob3wBN`b{%eqt#p6-@fOD%}=jHBt5wEkiZpRTS)pvoR7B;vd-fN zk~M01A9NlTIIRa7q`07?J%5m&dd=Ba_T~+u8$_Iu!j7zA;+F!93Q2UVtT@@FEnZoFcW)p%E;X%kFd2p)b09tD zX=Rc&Y;#DOGwc_C0M(W(zQ=LU0232)Z_Fb97x?EQAn1k82ymIb3pAZ(PFTmM#JWBN zi5?RrW`ujoeVH$FHPnaB&dzl0<4uZV8yXsdgxbtYO=Jj6b#y`8Q+5-P=MtH^3exyK zJWBka6%Pm5!+1WcZ;C08#N2L^>d?c5(5Iz3gWnlwU;BZ3=~BFr2TU4oyVk0T$|R_R zU+;&Yp!vdfD#RqUbM1;@aJ(FY97Du)4vtTi1;UCkaGPHZ<&c5S7}I(leBB2l{a3$~ zrGq|nlp;)r!ErSaEFi-wDk>(NL^PFh9(SF{%ABydoFCgET7+N<+YTc{4vE2=AYM^( zaX4Ocb$J#YNhjvGPMTk{dmTEf^$UF91rh)cJ(%V!!7vjY0|U@Pea1J~e|GLag*$R@ zb10X*Pwb2p_A}eR%YZ9Ey5Y6{^F_SkhIt#aC8XVQal8Rw56xxJ@_~Y|%`aR>DWR~1 zkl+(OKHU9ry ze_e@8`9$YgUy@x?A6Htxf_Gt28D?8 zM>yR+e*P>8`;01#A+v_`_BV;YTQSW$vir3U(fdVRd6V%fV>v3G_rl4S-jne|MMiCB zYF3Wxy+|0MS6s-A`f{Fg)D-ozn^5)&{bwDNOF_`0)r%&7Y zF)>a0E7vi!aL7u$jWMXK@aPpvB6-wuN)ngqq6U3NRq_5-q1b_o>ypiR(aH^spCuXs z`FPfC-|Is;-|S4gJ9}9m=_Hqj|JiPcW1qXaqRs{Cb>r+r`wms$DJ`RpC&zb~c~crKh9lIpcr+EzzG+HftbAbR!lSQqF7FuH9STJdTlLfYlWA zIOT=@2sB*3r5+R9`xifmm<$;aaa=mwVLA&9WTikl~ zp`NWaqU^(53o{st6F#dE&|Ie;i=qwN0uZ?!Vv2V zNXxD4f9CGIX^}sU9oKmI#ch7=n<9gEqCcTK;)AyR=5}0H`Z;O(4&z^7&o6TDbGtIX z{!ZQbRn+3`EJN8FUWb?K#_RR?w%bPBQgab~p^x1+Mf$3d5VaX39X(E7K(M)S`?l-J zaM?BWYDYH60DJ5-<>YYY+ru(p45q|#^b>d;YcEbMveVPIM%Ot0O-!`dqX}9`WDNh# zs1rT(6_w8wd%EdQgw*^qb#XVU_(-X#)38QtpW%n3YKp|V$8(F{w>zrv{p_r{t3<|X z>h`Qe_Gin{BAJ^Lu)?^UnhZzBT&~`V(IpAV$BVr)>o47^v?3iz>~%l2)M~XU>M|`K zC*v|egUs4=t|b6MK)MZQ!*)drMwY|`Y5H~b>&+@t)F%5%$>Pv zbf&qe^54$x7+En67_@fvXA0YzRtH*+xFisVaTqN(I^*PH;?VY~`L(}Go`mS8`nvOX zJ-ru&-hxzpKUW6M)gR@^r?ZzU!<}m~^(Llac5Wi1Pd0~}wku+7O+6SU2EBP?vL<$$ zlvx|TlTfyCG(|3@_OVJ)FqY}Yp`jjW#e_>cH;#fUw{Ub>{=%n9 zuQex%Oshn0-Yf#2R?F@7DV2&zDxX9Xy!K2zXHjQSsv&D}WF4D4D~9wmE|(p=)yJ4z zt5K^WQ`y9_d{ijSL0ua?JW;dHu5>!`r)Bx64B*(=H+vbXAf?sU`Tlum8)Kp8vdpHn zYWNT1bG4IUr3osVvb}EmOL|3ltT`M}*L^*J7+{i|*Z{vTR@99T3dMFyd4)G|atz%j z4pCyVm%RIk(QoOSX=9j7I8|HMpX_AkeooDJwBhHz{|D(*^b&*5|D8-2UPI(3$vx{tjAr?@Zk1JUuLFSC>zThlwBoK$6*cSI|8)yz&wGD@CptlKL^9 z_W8SI#x6-`y9y@xBeJC(4}^v}x+GLQ4bQDgS1wMO4=J9}{hof3xcjg|Ym;@uhlS>n6{ zrQr`nzK$PPrMVB*HCeEB%Iz#7>=T~a?@S$yzf!9Gyd{!c|1~r}@ze9zIF}Cr69l>Z z6Sk;}KO*O$^f&zdO^Jk(mWwMq@D>Y-EhM5qAPXEcH-5^dG|)Xi}!eYYxXO;9F_CNWbFwe=Vq1WtEhYau~0Wd0v0%*R&Z(Way{= zG48WB&mGa5QMRiBkWa>Rn~3D`vhoQd<&_X|;#lI>VMw1G z(7t4AP%7LX;l>bQGhI`2^}y!4E{Tp&`#nJ68dqK~RrZnF)HJF_=*-0b>M6<*k635; zY1V4wv%rf_mH+U|K%l;N*L{rR2Sbm^|wcB{8tYX&-3ItOOt+To3Y@ou%A3@yW3 z8Qm8SZn*M{l}xu@q23wXXtgWz%KW7D{&bwZVv;D@zvVf8I?B4y0{y^*PL1E&0h3Oi zAzw}$f$*3oUz!7(9XBhE=}6?O(ftR5$1U_wSWQ%-_PT$blpZ`c8`+Gb{wi?){;lfC zBpZ5b0|Oe_Xm;ZUZ)8@Dk`DlW=%!Pd0Nc6+mw(~`=q0`S`Q>3$vM3ZV>91G%kI&9X zo=ykSzWARi5Yb(Rh|cH=^lLb58*T~VbQlGr#SCJ@97*ksl$u4QBwAIhR@UT!qXzVx ztRj7C@C?uYwE5HmR15>};f4&$%|HZ!@~^(%C0SA*yYW zG(}+|_nCPf&#Q%ccve`@Q}FSmpZ9q4ciA<{N7!h$ zjg$HDuen6=lAN{`44S6{78Fd*ABvbo17$b>Gwglcq6Zt_VItT~Kni-Es}YGva8XdM z;o!7PPg4UNM>qA2Sw1f@83D@C4BxouiYCICbfRh1e4Xp}nO&6&l9s(D=1}x$Ny})K z2J@eS46N-|#wcv)1YZTrRC&5rDv6a+%Cz#!(eeEj}PnKM**lmw}ZUrz@RSmy+HHJ!`GWwr#>H)5?ZvP z-5xtV)Cy9*WO#bA>{u&nV!Rz`Qm9riz5$zHAXk&$d?*J1WA{hxp_EwHu6=_W9{?f_20qogEKW4Lb{wc96W+ePntcyg8fv*uJ#=ui|7sV|bSSpT zUw^wPe3NPC`nOcIJkhv07M9}CU_iB2UK5a#N~b@KbOtQ75-N6B8C>{AT37@RAL)Gl zT8%62e0?=FQUF&Mpy_i5j$)WigrfZ6!-wyEeQ&}y$`T1D%3Jb{-^#VDBWC)WwE`yO zjU#cD!m6y5&RF{SUR1&#%(Ii6mn?Yx&HN`Nqm}?z^ut&@0Re$DpiFFRxaH#;vc$UV z5uFP`hUMcsdwaB;oF8Xq^nBjED-Kr3F`o;{WMM#HO)aE<5GqvCvx<)k7anaPnHX}% z0v+pt>!%TeLcLNA;E1E4p&b*nQiUkoc!`I;!iEa0yCcdQ#s4_%g0g0s5pb?O( zzr-V$G6e;ysKB|dXGEsYiA-CHG;-@&;o8A?WV`z*75%?Sx^w%hjQTv@-Lut z2#gn?$y)FCJEhTz&`{5B4{0@L0o+523*$T7VzZ{Ya*yfg&g2WYPNxRs65h+r^W96z7uli8pxLZQaxoO1+~XD~a@6iM5gm z!zKgR`VpYluT<^0p>-6?ZG0V}w*?1k^^Bvg=ZW#eW`&N zVADKN_%yUoB-yG#AnLv8?DU9E1=sL(J;OC6Esjh6+TXmRz6ZKiT>MO7POUl<)aTdb zSz_cyl2*O5+_x)HRVFKRjyc-uoNZkSC$tDrgMsikdojozbEMpwk9eEsSS!OxfdN-?0#+Z(8+&2V+y+*dFoyf7+%b2Xp%Rco>UzqPGgh*55ytv^)DVtah=1QJ7dZF`RSnVBTfu%@qVT6Q#WP+idL_Wbfi=xD-TOvsvNOxG@lcv|C*y8W)Q7AVsq4NKhk$fSNs>%l+kWo zMAJM@et5cF*XFXu9aBBra$3jLF+wjp z4=fDmGGXL7aaH)m7VXk?Sgp6S~)2uf#&rwQiItxX#C(U~ZbN71oZS z8*VY_UIiPGv=!1#TA<&d)jvPq>x+nrirO0?j5VH0G84c0qG--%)jn+zJ!@B~cG0se zBJ_rdI6jfAFU+WQSjBAIMgUvfCGugPM;w5pvI#tlp>sIb=2lMPi&{G^8{HJRT*thy zaIlyAZUqV4E{}-tKU^ji{{F;6SJOd<>~V4Y*#K+_ijeWn7ECvu&8}(tXVPw3W;wd& z_YF-ho~juUskGxQ9noXodyz-8BZYr@8tuF)07=W~T{Q}A7F`~RTdkjh3~a*`062nd zK&8x5eQtjK1YrD;S~q?;d`(9R^dY(zm<=$1D$+xsXm+${riCI`M0=?A-%E|=rHB5jk(6h6Lc1AEd{P}qo z4)<2*PHrC^_3hN;(Z?030$!UY&0Rv&(d3fM!}hRj|GEKAjRn4*wJsJX=VSDD$q=QF z9@P;uaPqU^%gZL6#g4yhxoErEuBj%F&(y1|#cS)GR~({NschKy(2z3B5syi~n!0~> zXVllUKi@ec2UVP9>Hu?hGeTd_+U?Mt*{>n%Gew?+^)g17=pkvX>jBWVCZYH(6NISdLJ|) z)d2RnZ~j{kW*u-S1U|xM8Ay{M+^jv)A!e1P-tz=?eD{Mv)jCJ$Gc|#WFH(%K6FbZW zd~rM4EU}qR^1L`Lv>fF|0wFYZXow(q#Ku29rvL+>LMu-sUrD|l4ITX^0l}l^&xv3} zqIuE&7tAJtHz*nJs~g#>`3U(7!KC5RC%66%_C1zc;!V<`3qy`7o?jr-i?NpvLvbFa zYG8;Fj5nQ^M0*^w2L#)u~;V@HC?PaCWffKWb?LobU#hqqt#*y#681BF~;a3!7o; z?5yEUlf$dpn-TwqvA2M#s_oiFw*iWjgoLD|NP~2$fQW>m(t?11NOyyXbV)ZTf`~{< zr*xwtCEe29Z20Fg-tQalIp-f|jc52M?7i|{Gp>2fYYtkQR}2OI6#NIc#j zWiAO#u0<*an;sGyqY}>Fr)avz*Go4;m$&cbXyDjqNT9ruQ-X9XpXIFM)IKjX8Z=L= z;^a*U>|b18=4@=|+vMZ@PSi*@Nc=9r75WGOCQWwTxP!XP_Z(i3v(O{hUTgui9|%ZX zP^C~s8qCu@14jPrxpOv9YJ32c^t%C&+yY5n7iI>bkp9F|XE47OnW)l=ksm=Ne9naf z3WwLa#dayFgl$pd&?PtvN%fQug*h#}pB#_NzrrH}4XDP)4kO;3hV6w_i6KZluAD9h z$u!i4K!0Qs$R3;MQGq!5>gRKt-HRIUr1D!HJte~waad`9ek~YL>$>yI@G;9xc#ib?r3|F@bbt2bN>K&s)<0;1gRw0gEbf}w{K3;^&*fa z_d6$7EN?UZnRyS^)zQz<1(&jPM*aP@FmK&blFbQFx?l$7Nk{~HcZc=zFH%u;CX2Zg zENf;eGmh!|*59~!^QLEGYvR+<5(^rrNyE%0WyZNOnywPJLwjnMCGMtB8hlB}z5yjU zGmbn_=yaTeD)d{Zogw)hv?I(8j(St-7isJtEas96+g=4ln6xwoXoGee+LTKsb!eR9 zAT7fL@kWz_a{(>s&O7TKT#nN-g_!McyZTe63^rU{*sxM-#m;xgbIDph)@R3cmTO z6S~hWCI#T2L#CW|)*FdeQk8KRN@~1g^b)*UrLwfW&a>E1`DcVoS*5Qln_{Dea~^)= zdtUG7P}5{ueD9sHh`SC?io@;UpdN|yxy=f@F3qTmk1GgLf~X~z1|0l9#_hf)bIKPw z@*65hF7fT86BG@fI&`pUjxF)e>E&u3I4m*SqH-Dfpc=b>B&*`KX`x6teAf$4j58B| z#^gu6Ug733T5Js;>)n>!r3(yL9MdtnXA719NCxl7$+Z=50NMp}978CD ztRGD#3R;~GoEa|NK0ZE%ZjZy{6d{|Y-Myp5Sf?6D(%&m2$bg|JY_u3GU|j6aH3AI` zq=$gQywnr%(||r`9VQNO#{v2o3N1kS<|D#jqSec+4WhA7yeVLw{q@3b=9%=>OGROI zdE5>oeA7Rmj&#M*(1h01NPw76zS;&l=1Q1Jd{c7i)7bCNkf2IQ$zM~hbmVa6p70H@fB7mfN3Q#h?Q-KMzW6b2~WQD4KpPFajw zr9;U!|8W1X58rLuMmd6TQ?iQ5e`HMZz2EDcWXF5t`*EDb?3clF7!OS0o67HUke6$Qo>rn#Mw zJ*fstgN5r5gewieGjQ{tgj&|Ua$93z+h4UUVf0Q@!2(xxszsqGj?1EQ9rJbwWh>wP zL<1%Hs{I2gqd5jYr4GIP)?`(<%cbps(a4}>9|)j^dC4Na4a@o#l_fdbtj(Rtz)RuvR-X1j3clRYD{a-dTw_DTl=D%HO?5Tg z$KuIu8EdEx^kVvMcBBqoaBLT&zs*$4JnrX+R>u>xOGrhWd5(XkoZ8CrS?$rMCXA0D zj6fgm)=W?FGCOnnG#YkCzv7FN7d-P)ZDZ9zRzb1DUOv5CA#-FyW^D6lr%>p~6>I58 zFNM>_+F><%+f=)ns~K%hxus-kKZj97ul&0u3yOb6U5Nnqd?|UF;~E*>`n(8%gurvU z`s*chsyt=AoAofW4&X3F$bUaP%p`b<(2o$`tBE}pT%bFNU2cF`8ouS0|w2A zFOC(kpa$iL#<8&!sQ>0!jEU2tT~>i=nhs3VvzVCQ-`+Ymwsr}!tJBb)&&;mzxfalo zZRh>TKu$2P4?n4eK@sR4*m^ulLBLMWe0VIfdxC0TjO91`2|YLM0M$9pB{?{5b_yVR zHqiVCOf4AFA`cmXz^J2s&aw`+ypOhj`=ciaw2f`(S~JE?!#@ElXCcg4gx1S5~{SRs_YeY?kG ziQ(hpvDtv;pC`Lal`Bhy73ZR^-F+stA2_;d&6p_K)OyQG$edND@VE>MJsrO_;pO(> zzIdW$g#!~XKij)QQ{&}D#z6VW`f(aepD~4~a;8);sjLRez6fY?c~4sj_4M>!Y%L6f z=h9b1AxjA3xvXjn_bUeVjswcNUqlMMsq?7gQVvD$v`VN{Srv9{DyY{}I^?Xhp zsRkxDjCi9(-I2QSFx9n4lxc~Z!@Z{c64_HGLU05vyV|*=0L8gbU%Ys4 z;BmxFfJh-rYVPjt-W@b7ycGAA85G;#s%JnT7s}ZOeF^gK?%{NJjR{#gr%RJif%P2)n-P zf=LqL@9X0sY;<>bhdU1P6H$Pqh3Uzk2@WK_O%m9+J4QqvibkWY$_iAc@3w|>i)uGP zZ(+a>GQb{v!IA;E-60y0Eqw<7R!9ycJxg4+e%-fU9Zi}l2KLt_$4Zl6oZNa9%IlJww zp7-QbmP$6=l`Ec5gCeA$xNB?6!CRp%4OwnFq>(rj{GZ?rQa1fdY53t-`DXoMI#+4W zs>pAlyUpM)SLx{Hr)rs)nK!`(Y(U088+SD%I2dVy-7gk`xBF*_h0Boc>(Hk+x$cNCPl=f1$FoaDCxLVL1mr$|)`0&4BA7zgeQO_l8oXL) z6^FhhVW6v}!WRLTU~V?yfnp1Ao97ZDKbiFBz#HS+&>)2Zj`ThV%ncS8U_%+o01y^n z8LWRfM}wR`LO4efVwGHN1t23C|NP8?0WT+j-PdRTdGy)CxwxuOk z@MS(2p~B>I;nFoN`wk*z#kKEuIlqCof{yDuH*cPXbp)yo$!~$Ik4!;;YRNhz4+xS3 zsxT{ei6O@H1`ERl_z=A2Kq)apdIg|O%Ag~b`)Ipg2T@@eJb3U<23Om4T^dN*Lf2G2 zolnExe~qxU!P){%@x1%-z7gVAA>>g23<5%ocd!dE(p(NJTIQAOGCHY_kPRb_w9Lk6 z)cw#bn&Yt?fE~aUEh~n|b%w2f6V_)coWOO9(L`9ERtHd8Hw{I|10t*QmAH-0@K#MjV$nykIAc)<|jt>?yRkD8?G~-}l zO~AW?I7!6WgC+*yrAv)KzsFBI2}B{gXN`%DjA@Y3w3J8FxY_ z(!dN+$jqQ2+*+H;WZ;e?9p2DapbYzigudw|2KF96r{sLkiBUlOoPNHP=> zHY(Ho#e!e`$3Ok2l=jC*5yEO5w-tsu6jbu2Y=s})cg1=Lqda&JeHU;%2g#ajB&qM- z0l!bF*o{|7e9lskV;^_CQV(D$8Ys+XEsc~|=tRrSfC}~=|APCfQx~RFG0}sDG$>2FGvZX zEA(w%s4!7Jt;U8lsPHFr{`SC`{t;4AImO$cL{VXH23ax4r@{^Cea36y-sB;) zrBhBcUc+L^%;`KlhEXYo5Oage0d0;(XeW8G^_EiG18oQI8shW^VEsT`a0O;=z8o?i zDrN^SZvfb!=+1!t{z{5!2NX#G48HabAOeFI8;2q?aBowEeBVPmhfL$T_mTWQMz~U^=Kq%t2biT zk?lh4N2|27vc6TPh}KAb!s~!8F!_51DiZ3Lb_8*|+Qh%f2Es}iwGm(+!^!HBl9D+@ zt3EzHhEB<0U-=R`2b-?o;p5l5aV#ssa?fn*KPRoh|@>hcxbhN;~I)TFj7_?lk&2UGGfwj~svEYa=j2$C`bh*o*>l3h?)5 zplAPnv-5C{>q(zYg)(Ob(knw3qX2Ny>Cecx7Q03JZ&*_m+DGw49#kzhDX}+*HiuW% zorNrya8OvS#>40h(Xox%p)qcob(u=-sN$A%myI6AB|%HN_VV4D*ceSF`wlsWMs zBhlEoov~u7>B0FE3|hv{&W=V3^1%vndKZDIKK`OT%P$&Z_F;FqEe2IWk^!tUpFbEe z9rK`zp+` z5*JM(r?StLTX&ma3pz1*lZqdDPlzQvsgsI`@88!`V>hHev?Nd(8%YKg(rNGR=!v;J zJ^nby)rpmdLFWx-Cxc;s?ApLWOG#omDUY&F_sZ#;3D0_izV$$TP0!=y8Oor2NA5eB z9HW;9qprvJhMXie0lyyDD;KW6A2glx?slJuk=U`^kiS3^x^lzoV;II|Xxm8RGKH8D ziWX}SP;o=)4cccyD0dUKhtZ$sT{!O-T`pJ(#2=_My`qv3{MGNObFec;R_B*jp6pIr zn^B*Rp7R(fQqTNKt`RDC5P?sB-FzDpyZPOVAw{ThGBdX)wSOd*i{R0s*2bV|{#wdz zO6um<=6#oC4|UOPTvI$|roy<Jn`Fs)lX+sBe-!m@oIqjxi9`DogOX`=^dv7 zQf^oltxcUE^I>T66`|hFT+JQc?L*w7g9)xBe(YO1N!_={K23RwcMj&CFre1`GhIs- zR<~YJGkwH$_4R5da(ts?(|2t5BHy8SM$J{Zm@MmL_;%4ox~!a)RezvP;qyr`sTkae zLK=^`uw9SwcB9pC9U64Q&pW%yOxItms-AT2pqU{7XnM#&+Q#XzG!Y%mr&evn2~`ed z$C(r>sVLkofjdXr1?;8+?k&5nb6+C%n{3O(9+q(g!7&Z`!WDv!pAbkWmTqn!lLyky zx5|B-EgLA@ok4b{Z`N0#Xfyo-YbqHF7P zH|g67%_m4*H@hN+wauGV zKLzDN+tpFAez$0g;r3vKk55X}QKgx89+4caa#H{55p8#0rckNVy?e5geUJauR7N4Q zPtP!Q{4k8-ZZ+n}VWn#L!R(xGx60iXb%Czj)Q_`iD#HEro7-9OTkad!E5FEYg{tsn z5|cacGjmAG^&9r8Z47Z-RtcOgU!zU5XC0ahDRdm+DCf(HUR`g^G$#!2u0q+naG3I> zXlnMKesk-coakQH?qIP}LdENOommO_z({#U^scWTF+RjTx^VhI)MCQ@ZuaQvfCYsi z-Tc8*AHID$C1S)kL~WR~}n67@<(DvUS<9jqQI5?tbId$u+zUf$A?!9i3g zjNYY~fRg&oGpU0-L%w+X@rvCP#Z~=;9C>tq()Fbu3BqROgep081}TWhmX-*x(6T1= zjC?pkfQES+S{@6tk&*y(P+bQR+@@vwxj~h4Yn>P%s7!V^@L zOjVm_jX;52BOR02K*Gwdgme{@WVXZop1#e5nX~~CQNR4OuE3g`_N8%0YUQ>o!}~8o z%Bl^8R7?Xl#dNmk#B*}*ue=Q53p?gKjuTo|Nwm-6yn}PE@TRdnjc{Y;t4KEP@CT3O zHS(f;dg;hFJp{Pi?}?CPk67KvPv5(GX~}oQP(ssHf+G{7jXnl%_RxnVDR(fg<%n8 z9HM?n=M;J7ziDzDP^SG_9R7SQqDE6``@?tCQT`^EgT1+B<0RZjL-xUj1}77>l4TT) z*w3keMzFSJ-=FvAEGLw5bWdC2%|!L)STr}Kk8#b@hyjUea=Ub1bPKB>ig@l`xK_?{ zoG~R=AB!=ka6AVzZ$DYriznvC%{{JLlALwzs@i**L)CXc%L?rr=G8|k^5}j8K~CjgqMGH6o?D;O6vs#e*faWuV~6- z%}w~=yXDVpwJw+YBPL61H`^b5|4QUtYszo(>>NQDHgGJ&VeW0T#Y;h9;rN6TzOawM zKll3Fww`d7<51jcbnZm0m406<9T`9$ael-%ZoQ+nY~K-mwCIMD8xxtvyNBTeu(q7S z6?R5~lX2w(O{EJzm_vsZvp-n=4ZTSM8BZ|5+JDVVRZDwu-PFlF^Bd{h2}0!IH+N0j_Wl*NG|?2Tjjdkmw82mO>KjrO1JW?z^nqZXM6 zyrj-l*8VkBk@#`9Vz@)Tg)1yrVn#@A0CzY^4b?nM=@u$=vx)YfV&ku{xLFZs#qDnp zF__4H2j_{6V7^en2`)gPbg4+CDKPV4^hvc?>|&kQg$h>XI*fzCvUG!B$`a}5HkYHx zg!qSFX1cWFE)$U4qjdv~b(S2uYyGp;{$qrc)anC|kwB?60wQJaPwLAX-XjI~8Q+1p<^K-lKmH5#Wv{VBhs*9(`T>zN;BSKvauI}z|MCV5IYtC(S3Uv? z^zTzg{z!ZEKpL5(0}=iyWwGCBhcsBc|M2hO7yq3v{JS{hOOckB-~HKH8m;kbJ(f&c zn6^u7^6w+WTr=&+K}x^<|GEBsz-j6iw@K^;kTc66`*)i+@`3*g^&KEA`4Be2C0Hvc z|Gk9i)21WkQGj#KCU7C~JK^427E0s-et%bv4DxqCez|eHfLA|-z&Cg4=o;l@>Pmrm z4>j-ym;UVT9yBTk?;aE&Pq&GEv0J1*irQetuhTi51Jw$o46{?ul9=Y!;H+!GsoUvb$ zm>**1Zftx@gylVO7nxK6;eLC8IRMrj@qAgYof+Vv8ZZPc_GaDI(9kGe#$v{^*%+tB z6qSfN#VN1f8rhGM6?S_$&70wR*hm8xm4tRg)<)JBjRC5Q1Yss#x{@V^y{A5?R1duTeN_lzqni`fT-TrHoBsVIl9JnE9?lVIkvJ z(CRJF1)WU5PKjG2NNAzH0%dtnJo$RsVUps-prJ^z1)29~k%gmZd7w@k8&wG%ok7a` zWVlD0P0zXY^J8V@I?q1YKM-RarbC+bK%TWVrvJSH#liNX>P>b+0+s{-(f7O-cDZroFIUf3fR6Tj^qQ|hg z7KLQHi{8`6nZC8oxJd|33q|Vd3dL zB@?E5tPY-dY$HJ^z<6yqkzTOJrkF#Ef^j3HxiK{5$sj_m9gO{tN&{ z0FUo4kZ-aC{uxvD``}X3gc*|mm%YQZ(%-vbv5!wj%wikK|JTlWf)Zg+>$fYMbjmyb z4vC!c|Ko&_oBVH!5$wG0df7*JeA^jH&PGtr18Rpa;K%@s0SSk=zCr)(+qXa&i-MqG z63Wrgbn(0h0ecfVW98rL{X#}){n=VFS?+0_ujg8G6h&sh01C?sL$LsEmWIMAz>{o! z%Yfb%f|v|aodO~n!ViNQOd}MUN`^LHHeDb?L^k{)aWS3qVvxUxT_8?r;KBikEf8VQ zhkhYMa~T@Em7yB5S>6fwI3n<2)ydb7B=X-X94r7U7Z(>7q{REAlJ)X`F4J23>OI%f z^;|7oT?h<|#JCXdD#LkgRzC(-t*z~K8n-QiGnhCC4HW^Tfqh63+U%uEwzs!;RqYRo zhTO9`@6(OUaD|n?@%*G+6_3#1iHU8$41F!PGXaqtB`BKs2L(;Td0|+;Y=uUZEKnQx z+}6B8!FceUX_BUoAZ7FM6E`v%jAfVtm0%7I}&?}%= zD9pk>6SBN(gG)AcJZY<9HtO(`=e|Rw%g-6=+!#0C2PiTz<5P>pG}rFU#$jy^R32Ks zU98#K*V8_5T~{ww<1nberxMeTL*G`mblF;sg7;Djcq&prH0!2&Vw%|7{cW$xh%Lf; z8bz(Nqu%#{1|2yJ!`UL;E)dUcz_|b$4Qip6jL~pEd!Sr3>bCnBV68SNEp&tI4QN8M zDZ2seji^LHsSFwX?CdP8uC5N;M7myP(!Tr`R^Su&fzJv(fD3-3*UKii@a2bjW+r+# zbf`)IwL{~gd`H@bdQ_)1tF4SgQ;sTqG@@>H4^W3afbiTocyA+UX2oe+1dm3c1_g$k zplmG!Bk0|`cX7gY&%-cn$b4dQW$*;`%T}9lub`jZydVGDh$~*7Fv|gG8X8A#f|^p` zXLl&nAj5b^7lDlrdNUYM_6r<-pOsr35B!zz`1k-IdqMRHnxKF~9<~^XZ32GW7OEkL zOtkg1Y!0~Eo9!d(-)*T^dV^$&Sl18w+xN{|4VP4~d+WR$7sI*)O;u7?Tz$7AsGlBn zL{ZAi>u=`Nn~u7@f=_Q8j4KXrE;ul}f4WgO?Tu5JPjKI%y1w*WPIEW&oMYTt{d3IG zf%7HJ?c)OOdWXjhTITp}L((>@3c(!HDbj6znVWq)?z7h_wa<8DB!#9dH0Al%6zJX- z6$m{JFs9dc+NpQyII%hBqx@;F_Ll9NV{7XVMh2=S{O2UlxEUip zy?V1Fw=3>tYDwO4Py)zX2k2__CBxR(NS9K*Fa8`jNRH3Ln zbrh<@OK7gE6*#C_wWhe=u)laj2VPoC?M_~#3sPXzhdzH6W=FC3tB)iDw?~dy;e`2Sautl zjwJ93qn~MzNU)#yN?VM0V-dw{dqlwUCm${(&*ER58XU^O950vQ+qXV;URfBvs9Nj` z_qAZb%VksMa<}`oNW*nC2 zw*DNEryy_t((o{Z!6A)K43^MBDCy!3gNgf3Xh1GrZ^+(I>*XIJ-e77-QpH#h><~Ubzh!_pa*N z=W5=_ouEPG>30%3haS9sk#~MJh^BF2RU*BY9ly|Wk^C)uwoG@3YktF}^DDo%_*;{}_nkA!#PtPHa0lohMWOy zY3x-f^BewAfG6goFg0s`ANgw-!57m(m!rOZ2)E2Ds>Su0I2o=mdZqo4$76awlchh^ zRS)RP`!3CMl6rEOf&DAj^0JkxlRGoU5}o3~!H0dncI{dKU;)1a$94`2D>0pr%D@lB zBc+v%+h=b-WYk^VaW&A`aeHlNQ^PpZa%$&fQ-C0*k@$nr+*o_2+M!&uyc_n5ph$-B zsa+ys?U^H6Ie1PSEMN{VU}J+sFw*h308E55VJP-WLsW%`9fD;!=r_}Vo(-TPAOxUS zzP?1%)Q}dQg&&-ho3prsUfUa}bYg610%0F$p2)~_^OD2Qc5p##vs3TBb}~J9i+@&x zL{_}%>hvIiS#A&m3tt4T$Yk7JPm1Vh3FGU{uNCo8H%m7P&ZBZF*%*^4>IVu5^&B;Jhy+zmLJLZb}+#M;~-lCG0saSg?X$W-ddoihvEqIUD;FvP zOEfvr`qJX{+J&jRolO0ZMW~C+nJiVGIv|TuzSW%SCp&1!F^?HZ0+nNGG3PZ<@got= z9f*TUpBIR4eFGj;TNGHlWQQvl9L(E(8pIdaUxZ2n0?++7eOHt@CQg zuo`*9=a;GOX+nd3mD8Si_UJ^`>_l=W8L!io3Vg<%5BAc|I43W&U5LncC2#+f*kM68 zyWTA^s;PbctwtX*3wvp4X@swZ5HH}Byg<{UOR1z%fqy7)(#VYOFq1HcSzvwOd%bdk z|MX3IP@CCveZ~e9+xq7YYrTFIdy{dRnPU@%{G4%}I}#*>R!(QCFIzF6J66Bje6Bff z{9Vl6&i>)vbXZOrjY3NaJ7YSb5*+n?9L}x!b61R{)__3r3-sO~Mlu492V);eX!9+I zIjjUJLHm(*m1GG>TNi<%nwx9kU!+FGfwTYTf3Jb&pO-XRhDDxDY-|CDLuHzb)wHAJRO0i@1i1Y`T zU3$S(O!nOVACR3XJ1>J&DdXK}V&Rv2Qjku}{JcGu=InLkKwe(RV`cDuih`^zAOBIP z2u1zWmxZ-c*6-g~Vq!vxaJ{rJU(M_Z*;9kG$u-=TELPr`tNa-jUDjdBdJ7NGA2hQ? z#ma5jEESFNe|ZK%*6#T}xdq;SAU91!scIv=i@=VxZl-e3TC;Z)rAAa{5m`o%$wU+k zKw&{gM~4#6SNzG{d|+NDx=j#OTb(%WTs|JPf8DY+UMX792+Aiya~3mb%HpYQQhoI@ zTLkFe84e8~3Q=Mc%FQ<9)jic!X@F^FezenV=;t5dMJ!fwcAa%r}4#!-9Wc)B0gyDqVZw`1VjH@oaZZ zMfcr1nV*;x_iDf_B3tCyU@(n;v=$ByzQY@e3dSBAg=o}F$i4_OyoLHb$o#dR99Db9+=b=_f1QN@->RuIrtTFKRo=dfTY=B zdbX3QZ|Pz#P;J5;NJ$MTgZ3|wBB!CDIYOf|fqM*OCj%&`bR>zy5YWrKj*XR-k&($a z?+0!pa9)6VR1f3lq~X><#~CdH!>jS}@k+`3&`^B-gzSNrKa}fm=$ZY`mw)<`)Dy5c zK@wW~ya-6h(9tTXxK;{0JdkDq`Aj((8BCaY2X8|EV!B}RnP)t4kQD<33aDJbIQH=H z09GFs4o-)FM7|8spWGJX@&mRQy5A?CL>I~ol&JwzA^c$Edu-8_zQgO;;s3DT<-((dE3JW)buO63pPU_PKgqpQKFPdcp8e5|tOH=ZlOx-s^0n#DgG~r}8cn%03 zBDo|TMu5_k+M7P}k~s`-CB>7z{3q+Jf2Lul#`1!-@$QtoR7j5tav`v# zrN!MXKb77H1>#!}mUOADAk$_c={I`33Jgm}h_A69?;vWzkjy^FIRwg;4ZM4a+r?=( z+>w!y8{vO0#V=A2;>BLj7k~$qls{~eTP1^JjcPey96@VLSmIH~i;?NUwgqg})9&!ZTd%M~)C~HGa7N4o0uP0h8eh)Q|n*T_A+P zWTJUsegli>Xw~Cn3hB^+?uaX_tc$0&VYKbP&2v)J7oWxuin}P_a(yc{(*$zYZKzZt zWG;lEf!c#Pfk04YL|IhtGa+nkn9chhx~{gC1|z_-1DZ?)5DH+)3|FuEAp)^Tc~%Ft z2bcmf7pZY^`#Zpm;V^7gNf0tCM5j3 zw<6fxoq1h*Z|JGno$t*e1*R9qI3O13z>cllXk*1_8?|bt0x}cEi;_|2KtO7G03Sg? zr_c*{(*RNW0nZ1tHlvYtUzk#lAd{e_i)7mXBtT0k;R_#J3PNh?E})!q-m~~kaY|ET zu}0)d3|^mZ^d6{o6M<#Y=JFWq3p7mJk(IrAj~}pyRR_of3=Iw6+Kqagpb?Tc!ik1C zUeJE72#^KB$%b)Rw8Fx|xu4o+^dK&4?7za|ZMzM@6dAg5>43aUCD!NJBsMix6cXH0I!^zYGs7fKp~v6R&ng!~7AC^A13 znobaUA_(3jZXZA&Cfv!MJl$JxSKNU2k6-)dhSvwxTxyU}R?!}W@;E|A%&FSZKtytY zhYtPJU)9g0|GD$F-}>KJvwY`9utk=8iS1lCKv90pOXmnpt^hm0ZQoE3S_dBgG}_&i zc#RuqgI$O4u6i(wiBSQUw6YTTNqS@$65^|%REwz9B2-UcL93W_nIoOsQ1-Z%j3<|= zM<(;O842lsZ?bppy&)5HT%!fncpA`Z_cyXtOv2_EfhfEGvcZTRB#~u@09g6E?F$fJ z#RMcLPa_-AF=rehE_?dRN9y0NS;XMihywuZOzV5+>%0E&oM0mrNLoZ568SsHd|Cf{ zX=>AgRit33CR2HT8`r=N#u3!Noq#=1zYJw;LY&&KfJY-4lVD*>b9)&$<;r$^(9zbqeZ+i3Jp@d-voDmLDeKz-*aBtNOw zVN?ott9@(Tqf(Uo zto{5S+TKts>hs6RDmv(rH5m^%T1&v=<}gJ1)ICm)5z%-=!T}n?NQCS5rXN&QQK)d+ zbVy#dtMN5-qZXZ8e&4W4|Hd`FvY)~o9qkc~4q_FH`uee`bm4u)`An^M=fzH|I$wae zZw*^?&h7yb-XsOF&!ApoR9G7gFH?cXW(pZK=Zr~Ty7AJPy}fg~j$&zy)J+m>N1wNo zhLK@%WqAOVr%6R_omWgRyd#l4(Ddr$b4LB6-Y%yRi4w}E9g#CHJewEOr+CLMmI#Cq zl<*rZiJ8fEwxljk>4eW)lV+NEjV9JlOb6L$cbhoQZ%*Pn)i^jFm}vB7wM;V$bMGYg zb!e0Y=j-I0#bN3=NJu#uuTEt8uh3r}wLRC5?;du6Z-HlfJ(+Uwl0%oB*Oz!2=0dZXTxku5 zofB$$`u+1y*4h)4Cghaf_IPNgZ+`7Z?X7qpqP)&%9vjZjO?73a$Qf|^`^lJPrRSyBOhLt zJ05n1p5zk>nhNi2kr33{_8P^$;W~-?p$mRVXi}qZsQr`1*)x?w`ndK9IY3C4DeJIqFePtOFF*6o=NXc{SuqK ztzDJD2jTz9{ure)j~3vN+;8-WckTRH!qC^i6(xdLehYiaaJWF-9^ijn>XvnxqsiN> zYKQID^-B;Z*H^+&zJZ+VW#`1*ds2^Pt9fxC38oh4$8`?zYhRqQH{h8|GP*NR zc&UEs?Xp^0U`-I0i zM;bgBO}k%Gv1dGdktdwU60pffB^7?l!=RWimkKNG8rQXTo|_+LD5&K( zf38C0K|`_Xx=X53=ueZp*tGkuf?Kyw81y!S+Gl1Gq)*nQR-29vSd{FU8oIw^FE4!m^@Szq$d=qjb+yhse|%N{ol{bP7S-c8W<83YbYol~M72k_ z#dFQ2CyS0&aMv@lH+Ji`_jkXVmM46`1M_;B#W7TiW61B8+~fWcI;iiQw^oziK+{P( z_M;oDu)#2t0YJZ&BZhUX;M#o!|MDEtZ_E0|-xtMIJa)NJ`JAqbOR?SRd~T<)TuPF7 zYW6$5u&%0x<#bmze_=QA|1d(!DVtal{LAocWNyQ8r-_=t++97L*qck(DO20ddy5&} zD8Wh_Ue6iUZ-@J>*DL20k7I?xxr=eW9K{1!zwy)7ylaOQ&-G0|zPkz{Y z4FxxL&_!@Na-Uf^K3O{0??Lxr$y2oW%6ug6g?`<_uT15HH5S5>@=t|wK3taT9o~P% znMK?X7TIxn5r28ak7ZM!P~u9IL`jp4LFY!<_R3sfMg_|9BzXJOK)D3VSh%lT5lTwOHpB)TbisXkPIO6W!-ttW@9!|Dvhr)Ee>(3#S z8kORX+1^y-e$>18>Fk84=$E}KHVftbxo@27`61^<(dCxMv$wlXd^A4;UA>nTexACh z%n}anQ0oc$*P?U2?iU;q*FDk9+j!y0WcanC+pMJp?aL^sALf$X_aiRs)VNuE6fo`V zyR_-2$rC8FDtfn`!Xl$LJ9LIsm9LzI{5Fx=-g^5}qxS%{S*B+s?O52$OP0y0nc7!- z%%6JBUlH=b#AR|#Vz(>`{CAoql(QpI^t-p^YV;4=rnU*G$bE9@=TbUbm24SW#@ySE z^bh-M-`LMQQ9;|})rya~k6#U&xKUcA4WgT))`hK7#m#}9o+F0hZ#lSvh(f8n_NHxf zoQ_yWUEi>aY4+=VPsB}2tKj0Yoa?iiv=7a766Jg^L&nWlNA^AIN}*b@8n4U05Dk$ViJ0K7Zj6h*(51pR^*_mxc-}g z#r2C?`Ije(Qc>q~FG)Y^Z6El&R&jH)idt#3{OzOXR+fvxmDCQ^^CZBUm{{BHZWYkn z*ic~N)eaR|-Ofkxpk%(-e9QG2lgVaUer8#3#;`QlmO4z(NL^APIas&nEB*D2jmKr% z6(#5Ix}!CP7^?PV>^gCc5hv|oBbvr;L;3?r?w`;zfxLYt(_N>`iZ#m6oiu~n{mR8T ziOs2Za=-3I<(|(`Lx1{-j84hO$Yhq1yU7x(abuqCJJ#XHZ!;5nLZ(_CkL-T(eP}y= zWqUel=fNxth$tA*<{Iy_x7&B>Rjk%5X3wpbn8f?G(rPj2cGWxU`J*TEKIkVeCcLAp z&Xs*s*{=(Y8WxSff^QYCh{*ZXc;$3HS7z);VO-pq*dcpT8&%X+=(w!S^<<}wDk#2^ zGUA4I#ca-~bOm$u72fs;)f(2JTOVrZ$N0I%Lp|`U9Gb5_mp+?sTkj4!$L4WUXq=5A z8(MB$9KrC!)@ZjoPik{LRt)DniS;yfx~7{&!e58CPNW6BL_<){a_hf&D_tJOb{~5J z{eI#ClIdsa${rS+%g9PkRiQJ^vdJ9>LN;WQjn2VqxEH2MLUrm)9Jyb7es_D%uwmKs zu~5od`(mL({l`kP<)N)}6EXb~`=6P{Okg^Kkv{6&g2W~*Ne4w zy@4uwAc-)^~VF6=s2#deT<5_f;XGts(S?^#+? zHW_dJ>QQd}M3NPh{wCMUH@ACMNF_BU-zJvhc)2C0=%c{S7n=2+{>W!qU$h&NdJvU8 zShC+uC}HqWct4g9xFvGHEs=Ybs&5eZrPR}mY^yKVmB-o27iXO}`T+ z$38P!8p)0m-;g=p9JLVL{-$?2QHVYyLv%3g`K2SLQq405RiX*voX4`R%9s$V-|P{d zII7;49Nh^$kzjuOC`&Y-^QeWwE9XnIIpdBu&7E#*n?*ZSu7$!GTN7=s6Fk#;H;XR) zqfn2fwq2}$T!2DWaql~!kChs^bf8{ndzn9pM#jP5c&wsq;ca)OI=A2FAX8KmzIjYV z@CDJ`{Z+>*Ob5FUw^z9(2&auQjI!Eg<=Q7Z<2=3>Xs1`Y<1G;@%uVnPI&sBa^q@)o z=sUUNcz&bMX09})?2|GVR;D_vXg<1|Zr>Qa!NN-aFPaf^ce*Xf<=xVlRqe|STlXC% zTJ3drvBx)}sg&X@ibO0c3idWK6a3wbuZi9EsN=K{|FU-+e4b6mKWJJie%tA_u$lGy zgaFMWdL7Yk6cxD=+M&KGB}wkDY#L^`+2Ta;>P_y|=u5m0EywoNp)DF3mNi_sui_@+ z*(7$3XKLTm6>aLF976d}e~^EH*?Jt-9liYHD{S*y7ol zz9aLu_b=P+n@t9G>{2K^JRD^^wq>K3Q)2%7oc%dpRfx{0V>pLWbyR=1cDGWKX%X*o zrZ^~wU|q-!=$~pLCvIEn_boU$yc5e#Qo!4jIaoSUwu)q>ERn5T7s}>jrMeIhvC~k0 zsM+COS)#`sf!hJJ+(#wI8o`Vp{&@8Om4%CM|QNYI~3tCRmzlNgy4V*#W(6!R5Zn@{qKrldJmm z&B--7t@9kG6Yb@h;DOc|mrUlV1NR!Vz`zAmyE|86!ZDqeh4ZU2e#*eZCr~aU$%ctI z3ou`rr3gW}LHnYMj5bH3=vp?7d(y{k&BWsBr4C!1`r4H(5&i1sq~M9|>4cPlh~>Jd zFqQEy_RVcBBZC93uH7Fka<+(<6g{}&TI3X(7S_J#6;5rJLhU@(I-?NW+mj*@ZcUqJ z3UbVxpodT@aXVT|M*qEVOjA=A%Hq8%DiUOhjY11Cy8V##`Dn5y?_V$IXrSJK*LP?E!LN*wXEKDL9CBrjGtGG*(F%+fOyAoID z@-DS(ax$#OqEa4kio{`^xBa=<7LC2us@ZRzKb_Vq;^s;j*wBc_Go zu#gV%KN6VYWnu)SLJ%-I#@ZG%f=Yx1hE6v84>-f7Oc#`RNAovsgFr)ap1cy=kk0>`a#`3)rGzc&7& zwy=ME46t`Ebjiqk;Ik;QHzj~6YdyTqBncmHqqVssfc)j}CKvs0e{dCaF@g$WR{%*F zG2ygMT(h~-o{ z)%`!<&F|yMCI35Gqg?&(vtPRZ`|Kxg{DpYv(n1Bwe*7Z54Wz*b@GFLAA$nm`L6s+P zAg$|LtxPeO{}dX^(kT9OQ-E%L7qRg#|M3cuhn_0AZcOIm*}upLnhH>k$}{Sw21peo z;NhqDaXhO*_~@fbHqurHLd^BQWtH>Q*kU^k<(VFEBLmO>Zqq-AuX4Q7C4-8RN{$vX zl0FWUe}Jl%3e+o*Cm>n02mQ`LkPU{ad^!Nn2q_MFnV~=deVf2rUC>-vC3Dvih=TLpY7i8FplIMz_jbVeP0s&XygM&}#;-|^ z3sNMK5eIs69&jz-<2jEPnQ%n=LK{7T@&KzH^i#!SFbG9}3ZOEe1WivTX>K%#+We2> z^6${W7MTc%s6qgapwR*vMs1xNiLC^rL?0B;p%>T)A_hN^Jm*H5%t!Q6-stPofz^+8 z3?DfG_=rH^hDP4}Vb{6Ze_fbg@_E}z9*}$2AYvvYiD|g)ya{<%4_K;6P^$IT|I{EE zSs?mfO=pJY(m+aqh)8v3KfrfDkW@1m_R#iZZ8+OAM+oW?FeREG!Wz69c_~|ZH&lyU z*F}(i7HT?S?oh-6xCgslD=D zx7joB?ExH7v*z77gt;PS-c|h;2c+*KpoT!>?5B_rI842gMhry&AK`aceGXOGXyApX zUrHbqi9K#ouT(51z*nr;xv*E6A9j3^V!t)>V4e;6N3tB*hGewh$wup$9sEO6uCnRA zXZ+W`%e@banVhTHizZqM1Njk`38mKK=}7juwyGhgdpqxbS#(WK z@nuDr@r&+}&0Vd8MC9Z!6hx%~DvKVpN-$d;Wp;J&beu+@8Rus!q=m{8_tyySFfvE4 zv_Ej)pZHY%;Iv}W^u!E|-CCW8YK!T*DJi5(e{wXYQ&`Q?9PBFXT7zIof zC?GKR9)K?YMJ7?W95Qlk(IeqR!WhpFm&mN45uRoMnxcyVSAl^#BA_R43kk(%W-eRyn59`yo&~7S)%Tz^K7`d0z@gB(?7;scTo|pdgIZ*LZ}tcbX=UV zGx%g0tYfdfBv4eX(cI?Bcv$KF>{Ld-Fg87)aK6HQxryK?$C!tQEB)QunzhrLjT1(* zmD0aTGLH4aE?HWlbB1q9w+OD8ZIP>+Q|zha_S-Sh#EF0%E!lJ!1_>%2%M3V;^d4lT z9$UtQYeVm`lW8gS8@~&D?oLrUXNSyJJxvdcZR-*)cb`wU^n#lz8C$*11#s zz~#5W3P#60xBrdeL^wKnz7OB4l9e zV(rXrXl?P{G8I6V4(Fd*Rbx9 z`iVX>P;L zX5R}|CmDt6t-^66O{^xn8?z*4Rzplq`{9pWOMd8+W4&NuY}%f1$Te=$;^)p(H5AGy zys1^o8oS|0VBe5N{HI`}=a2l^{b8k?RVD1Z zE2^sWJ#Sw8PG-(b>*$yni=0PC2;Ma@>i*~7GRLB*s0)SE&u6!8D{vkJa51TRT&`m? zbamou9Phy#oo>KH%<@GCdm12331B0Gu-K|m+7CXR9G`>PY-Y>~O1C6`XxQy+SksYA zMmEiabE+^goh*pOOl&SN6diAQP{X#CayL^&dL+c_!IF?K#4={&SWWfTl z${pGBtyws{dY0cMv={Pqm)(RFZ*t{=qli zL6GjD7y?E9qYP)d7cq#@xl3>hAK*ItWj<1WkpDtEDWlqK#f>pP679d-OCS_N@i0*3p+a*JDu?b5q$5lZn~5b($@WUV`I7 z{z`X7IFn6ClTuGEde`_ecyQ=7542Od2@%<5)fM8jq?!5jGF-t;WFBfgFVe(rw!no^ z>_{7}l1x`J75MZx2=62q;;l6Z(bvRM*ffOQlDR0N&3EZj=gp=ip3#Z-scMqfqoqU=G7x~7o zeBtT!k3pFU!f&HGi+YwDSQjuu%XiTz=?2E@uV_p8UT9N)(eK+$rh;ah@N=G8j*`MT zm#`F`UKcn~tY!xDhHM4h%83huT7pyXIID-GVYqT;{PBLtX%C#w$^AXPQ>tJ}fcXHt z=aZJCB?;i_?tywBLb-ukeaBkWsrt;T2H?|y#Q;34Z)SES7JHyD4CpKO-%0)!CMp3f zmkN6}{qdeF700)n8g-Bb6rL{ItG?p%wSLRj##dh-MYd+x-HgrAX7F?han`M%G=8B04k zUQYK3vC&Fhn#XZ+7u#Aa$Vh6|yH3yIO+=yyF}XI$~r95&ncU3bUc<&S)X^_&qBp@r1>w|i>Eu`QwwX@6y}wjLy=$(D7z zP&92?yRu!JAAOL>f>o>*jq&mBo9r3vEbh4eA{*z{7ggH0M_=z!4|P=TF#4_bQ)2~m zNY5_l-yGVe2gO5gF-=Eu=&HDGsJ*CN@qXr^eyu2cR}syZI346N(UrR2Cn?3-{PpMZ zJq^yK%w=Th){Cqppr8wf25tdQ!Q%)k35bnA>@DyQIA~vizl1QC01bC_|EoQ$3u(dJ zf>2mMKLb(@h-sL?q=eZcaP8tkOx3+l6L4%1V2^HCwxwj;6gLS_jWO z)O3UO=$mqJ@?TOV$OkF9={ zT=8N$Sa+JM+~B43sSGJ<*Z$RGuu_B-WybwHHNT9pM^{)_EPU9srn-80N0Wat>uG2U z0Nhh|2h`H#vM-_(I;Y`!Csr@R9vl@|3bS(&+Mib| zO)As`g)(+Qiv0Asx)l8jisPZpxX%J9*_I)Pmjb zR2rFe*Fv_-_2ly@s58!$HMsuXF&l~(RZhKuk1=tfvCB9sTc&*rmTI%o6xfVYgncSHuoHr`$A}+bQ;!fA(bW_j8roN@07gb5FKI&_0)#8lZcM+#aAYtW| zSp_121O*9Pw!pz_(C|1G102Ctd>R>l9Vs0?j`G+JJ_Um$)1m3;#kOiw8_hyKtgYb= z#d9}@3a;0AW){fxUYWjlPvgVpFYTcstvAwq*)?yAxx%uq){P9m&N)1HWo%KNUuc6H zrrS@igInG=p&Qw%S8qrdYYTc~AM*SZ9*-nB2rIBSFMRT--}E@Q~^QaU(i=|COWvFVq70sT;kzQ zcdg~X32o7AO#%&a+`6$tHF%IMC+fv>ir(v?i?#+vjgqIVLlIbM;gRNM%|SZuv;Ozr znvR6F2NZF)o5?UA9PRPEb3@x&$cHB_ve0!N`@C@Z*z4Y7f8U#T*orNj=R7x`(G6?; z{k<28=Dn~orP@c#K3Yj9>?!nfm6NO&Ac0uIy zVH6J9@QRr%h~2kY4moqkK~wL;ROE9*BdUowaWb=~{l{(bYSC zrmkiosL#a?H}ch;JnMy2U{Q68n_A<#eVI?9I#qoz>$nt1`aif0Ux>zg-yAOS%T8Q* zVVQFMF7?QhY4tsvmtW=a+eD~+CU<1_O2?UwG)b+hI&Kvxn*~1p<}qs`s+B8%+4%4l zje_fJ=<`i;kPn~}E_(p#fTy1VFc@tKs=G+5_hAL9k|d!$+IYC7PJrL?tazzZct zb-cXcXxN*4v9hxA|Kqz2Sqk4mg0m2Oa~i?Fq=4A*#RwjYm8$T#1cHZOvVSTr2{f|Z zy3G}E*+XEC_w!?^}a_174Mp@d_6ufPPcp)Pu<`Wf5ok`<1?_@U_?5d?@|@mGgKbfhpS5o zDs=1yU9gvC#QTjdfQ&Zeibw6L>2N^S%&C&e>xHMDG2|8;`D>_aRZ7`Fb78}d^e$+E zCHL-u4GU0&5UzQ&*@m#U$vJc{c%1ChCy58sJ$SI)ewEY?=+bTJ(FN%9h4BM0*Q(pUXE3bUwoOuX{V5E`83WYVXdu%h)akpm;$35=deq$W| zFavI%GukN+tysVdI%eK9I2Z{YO+-tA*-9L+l?&q)0?;Fv_OE7z!kGma8=S#N*bAJ@ zNZ({1fX1nuW{5!uym0B#9bR5u3(;J7E#m=}GTyi;=$(BY z%N?}6ZM(U(m0?>3mOIE=L+;=>M1l{YPaPPVK$-~#AyDJn5S)au9rAjT#_9T{kw~}e z-$(zgXQb2+clNK_*lM#FM7{!(!H;oqH{n&SN>ssZ2QpKM1NKQiSn%|rH*CNk1`Xhk z;BJP-Kwwa_a3OxN7@2_|*!Sr9ppyHlaCM`On24+3_o!E^qXRBc)H|G4R4~${LCd zV3Y>$BjAd`oQ$@gwl8CVHYl!d7+aDC+B^PIh2XY#`zOWihh-PMhkqZfvfqlY(E2Ht zjjc-IQENymNakTMGM2R`7?2G&e{O*MGWgmh;q>i5L#2=u+PlQ$hB}o3#TX;P()5JcR(Hc^CKe* z!9yU{yqVeAjX?m`fxj60)2C0>fb|mtM>f5^~vP`Xvn> zQiR(Kq(|QP&ij69o$v0)Oe-gwvpvZASKdvIi6;T4H$ouT+}s4{X_Cb-tR>00jOD`3 zrUBeD`&-2#1?s45&bRmaH>NM9C!gmKoP zo0`+O{~o}UajF5J+XM|7j(^^sFNT9t0)L=g`>ZD5A2Stm!FYP1x36!pKb?e9z&;rc z0_oMOZ;V!~Qx~m~cDiKgzLbPG6Wh)J-+t$L-EZWYV9Q1tZ2^EB16{-%TD9+Gcj!XX zM|be+zUQnu0<&}D7zM2d&`vc`b?MTjw^OlmknP!p0I0D5;+&V4*W3b0ynNA!F&jh} zI9jnh7IXfLSx9pNB1nS-1uW8%DSyUdq|bV5>#(os(GS1Pzur~kYLfMe3wpSOsyYGbbL2{90q8?#hU)_%q3wb zU_jD<1&2=?5Tf|kANWFJg{&P|$y4A1KtESqktv{E%i%r@e67+}?czW0E@43JhB*xj z!5IV0o6r;Zy4G+-;p6~*7wL(5^avZWWoARfsjzQBT?P7>13=8WAMn5RtWM_nm3|T| z7$^pw30F=q3NyV1_yh)s=SYJiC|xuVuL;l===x{?#Z+03UDXBsD^vNGzjQo*4-ik0 zyfQdDu`$4OManHm_yK4-f(HvpNE;e_8(|h8sC1aaL3*y7$?L!3{Yq8zY!DN$>r2J6 z&8%5^`M?^Dg1)NZA0#au9VuVFcmic3ar>y=BLZ@j@7zgh=Z4D(HrkcMT^~MtfT^jp<8dD{%+NVTf=DKSMX5rQTyUCSr*4WkPUv~kh~ z>5H{W3;%fm^cWSg?lLf>kM2U)&m9()RT(p=OF_GryuK62$>|fpFJj;}w}@;1E8;1F zE&BVA==dtS!N6}lbHUHgZ}vA#ISFH$$@a1dipfz4P*42(7?6fr$^*dtf_S7prX}$+ zbp;r|z7p}ttig!Yn)%;UN?(bquM-qIxei>BOZ+ty_4vu3S`mjNZM**4f0Hgfw?z1V zRSGa<%EwBB$~r%P6Bbais3v4*|if_6*Dq&gyUz3`Visfqs8%N++jE5*i>0CvY-N$dw9OLpzYQOA=Np$ z*B_Q|4(~I@Rz`^j=k85qQm?8cly-2}rH#(tbX&L6XC+AuBPiD{GS7T-5^Q#}2$U&osq?k{4 zsw_SgAKdWA+xl_b_VoU*eE7g<#63OmIP$f1DtRU}s3^C`MUgNvbu=R$ISm#)`QVpH<#c zWZEpE0p4Be8n$L>>8Yl{G;K%0VzqO&`)w0`{%hxOA1n?^7*nIv=7awCL;szwNzwPAwBloPytUYJ8Kq1XT{4rkMf8<`0QaYb1mdkoVaP7 zC*C0{DoS#G=<khF0-7eCMJJu zIIVwkG!NG^zU@>hKsZv%a8(bcKAV{TCV`M2yK_V?i#;4+PHlS|L*KcZ`pMh(?THez zgnS1*M;{Y85Scw{^Yz5*YREjIIa-;+TUJ<=(0N$KW-3pDi;IKu*_id#rPJsp^(1_8 zrlr*(pPm*lb=Hf+Kd1;}7Y)rZL&5~(`)eF?tm^7q=Rr11z78dEPN?>-0_op+xrjwg zw5_sk|2XzT!GZvOK*wsvR&fAkeYFkSvJB7iw?+LI)T!FyuBy|pBHI1xZ|WmAf+wp^ z7d^kM8=AUf>(bNjFQL@e$qL=de4BU2hE9&f@k_~7$Z4Sjc*rKc5ZJm8vJL@7QrQOG3KcI9pxlXNK z$JIrDIJ>W8<$}+oADT@BYAVVk?0SV@stoBRg%tx-jjDMCn>Wtlx!HtM9iP3liJWDa z9>MGIKfcSGX&x_OV*ODKO9fyAvF4ZGeIy7}5%4X1@SEE5mPy`fuFBJ1t-_p~u1>!b zNZcAw%H+<9tGs}fKVV-QJB0oIDHpxdeD(uFDg|Swxf-GOqK%r4f>9q!dJVIGe0)GY-lhdj#|I2UVV$U>{AbG=5%a__yRde{Zc$d<*vz< z-bGT96%-YD;1X4f!i~`3_&BM-xu)?)6}F==)W2T1x(WtdW7dX+3gp@0g|^7;ABue`-2Fx<2j*-_VROe%U>S_5Z*j+ zqr}=*G9sPPy?nE0JJ_=QTFhSQ``K*a$FVhcWQ=2x!&-zenj|Ru*@imHRRp#om6kCJ zompx9KEt}&HRySpc~lj#Ll=?gz}_99G+zN0v&LDqT5QZ3E2J$?m|WP-|FcUjXzA1@ z>-PSvq7lVpvYTAD(OM`>eJ^0acOC-PWsR&cnBd1jIvz31s_R{3n3>9q3m<)VIyjFE zMEHZyR>bfgUD?sNE1kDSKb(92z`Qdhxcs92r3(5NTKg+LnB!P-4x*LRwV5V1)Xfxa zBbyqHp*&seTp^bG^V0^fB;gy3!hcd!aFtI5dfK>SgE}}5`-;^YY+C*LAAB%pEjdTE zbbh3@AIdqSR9;vz-x%fU9^)zU>w}L8p;((d?(F~VAo^=}& zkwsthLMM)EmB5)Nj4O4-RtY0knh z%#^RiSj}GW?=surlwLWxO|ad|z!lsnNgSe#puUa26h!rt+Q;rM5G=Gh6!s#kbvwy7 zYRdz`W(tekqQ+O^L*a}|1hU;g4b}spVhMIPYj_UwkVUr-Pa z!1_N6Eo28;DjDgFylz`mhQR;T=9aFG_{5kW>?3%pzmNW#&tU0A2K-6?#gc#BP_pR1 zgM>Ua=6`qClK=58?AT6i1KITaVz4waR9P%c}*>$AKgKOCn+aQ(p-q;Ym>AOO@9rfZkL zQUn4Qq|oVdylJsd*6#rzoJaT8e=W-g46jrP{}RuTTxdl7fhuYObcd2aI)SF9X1F;$ zGjq`U_h%sqgdAvRuzzC(s}U^zS0GT{DD%CQT2epqdHz0-@uhBJ!v|gqLh*Nm&?02V zhuFWsVI`b|8ikD-Qoe^~No{CKJDO$nr~!`$q!B=kb7&rl#{DZmJCI9*;U88H+p({3|!0TLjB-wJ?=P70R8}^vsvQBi&;=X z>VacSjtene&#nzJI3M2n=kUtr)ME($0cw|IFmnN8hkGSodRV+B5O3hx?7 zC5-|kC5+{X5v|{tIS+_#x=<8JD{zPZA~FIJ!?d`#I5RgV3onU6XXV8%NDdq>Wn?cv zH!?WA2K;V7ar0glO&5jQ#PkOEk~7>y-WWUfd}1UfPCr*@F(C~Uv$e`Y;Pe-j)*&ZH zx-|^{?2dCZ*u9WIVyLjC4Gf0L2)8^T5~>IMopebsz~>+<3fWu&N{&uMggUYQ`R-rzcO?beuPNjX5a7pIAH3p*nh6W6rTPmhy2!gp6+eXz*}2EbK7tpweoU z@M%?ExR1!QyOR<%7YCzH_J!w89E;mGJ($Ov-n|i*c1FRta<#x2SOQfc-+z|i^loOA zCd2eAhqD3`YbEJEn0=AgR^yyi88T=pVfTbvr#IH29iP*Ad!cX1 z018#EkcwBya7Q}4L0b#6K8c7Ox&{WJk&)tBTDPf!=L-(P^K}4>qiR2_ca<(M==$co zt&Y-;L{sQU>xPt|cg_1#)4J*})#%wIJBGluzVC8Izq{9Dnruv~=0_5L`3;-$MFqTl zWL+4*o^3w+jd16}ZV~3=M!ox_YF`reWDWPpZm0CgFUPfuhw%rCR44gImq`w|rqVNX zj0Sl<88t?00XY_)wbzhsbTrY6GehR&+GbDLVlXb_qZIz>&5xfj@t7gI{>G1#Dn_+K zys3BBh8~WLn~^_M*T<7em7ytZ(=_eW92NVpacyn#TAb&#pP*rx7ts4r!6>vQ0izb$3%SF{^VqpgQ6_w?g915MdK?=4Y8ni~3TAUQ|C`y*y#sa*Mgh*C5+S zI&!EQZ*SgWZRP&ert0WeMfX!D(`T)-l|)ypMwkN^t~4&U5pJM`HZ-ei0v|V46vn$5 zeXtF*-#q%&u|NOrY$0{)d@7FvhmIT*%vUVzdPORKHaEM)U4|0XecN;_jeMM+Q(eZc zj-w{r7?`tNxkmD8=jL;2iaNJ|D{jY~6cc$rs5@iO3w+@b)B5dRVDJuT%@kHbMW{+ePUhOX1PY&2H6xr{M5XY=dFew%Ams)36=pXLaTFXX6H z-#fL+Q3(+7S{Y~Z?czyg&y&Kzx|StaTU~hDqaN7F>eXEIqwUPv2|TJ|=3nG_PXL5pCH04F@Y|#Fj1?Ya2E`p}BHRW3YOo_FkPXN1sh^(pm1nGq=jk$gU{3i3PYT zVC77gi9J{yDvBEtlaA=T0SbvU=(+q*{DRmYF!sb-QzLQoNRA-nBfYuu8sudP3W}Ne z`PVOBej#r7H3WbOgv|*-U&5s`!Cm75V&~TzvKKvTHYuY;-w$D|#OXAVFW4Fb=`PK; zPKo9Bi$#f$rnZUk;f73;*p1E?KT94|QU#wnuxfsSvu(Wp%$&4o!>Hd+%$~v_w%4J^ zPzPq5V1JDgU0nR+-X#USoO-tCYFgugfqTmzmJ?H%Mj zQS8WBkKkr1FCo~Hznee%kY#yqF2{0*c)Toy;F5)r*PzMzZ~vLl)gtq2&8-ewk;7G- zP7is>3W?GS+zdTGxc~H0%H_yX5n>&p&+08I|9@cFVW<_Qw?yRa4XONvNOIj0g+rnczJ~uV>o#fpxv_7eRc9<{l z#5ZuQ31GUkOA zO$NW6hOb~&pJ3tFwT=Ih@)P#xmRv>XvHfD#U%7@sa!bM)F|6I$u$xVEvpita;* zTCGH5XVJNnv$ZJu2JO?WH7{iGz54PS=6g;K8!Xwtwgt7lRheuNoko1{ z>>~K*mx4buA(9MX(8dDKB}^NaLrBS+~LSRs$G-ZQ3&_M zo|E$1k=rtn;jy?tS;Nfo1n)r~nyb|`|NTHaJDQ=WBlp?VIy=6us zZp8_(1kUux>}>>}yI3w*xZiU>X}c@w_e$sFXTq;nsj?#oj)%g+0#MV;?l*(^Do>zkY7}3rE`#fid4MU?k&m9+=#i4r1COYZgcl8fPeoD1ZmeXO=k6Ea_*_@~g zn|#sSY_lbIF4!id=}$WAo=$$$!UTVD?MZ1PZttz`)~ajtudMx3=87o}N9-d`5|k#V zEi*GZiKacO^gcz&Dy|Cl!?^r<^Zy7bKep|h_wz;HVvjYBi)%^aE|Upsq!QwkoAbc` z1rKYdSh=p@8GO!DL>E;4xh~%x{f?2ot!T9?Tc|ZPSmXBHJicM^&#rbYD4xr|%b%p9 zxz;%2Doi3i+ZhrOsMy^n3Gi=CWP#gmhLPyTD`52i_lpfecgI_%v)@m*Px$I zOiC&WWsP$ta^_I1%~UIk5T|lF3&m$}k03B+#v+Zr9W*L3{dJmk%{el;-qvc~xo1BV z0v^(JjI~VPiP6^V&JepytX~%fu%5vnad5O&&uwX`b$A&r;p4;+e9ty? zibC71Yva9m`6Wj>L=ArPMdqdrLIGD6AK2jpJwYeWmz0=Uo~`4**2SedK5e;Da&^Tb zRqV-m^!4+Y&Rmj217w4t;6q4>(65GP*4aOHMN>4LJWyHiXxef;>Av@lM%$O);}(?z zG4<(|ELOP~OD)R5p!tbSjcNM!)%@k5Lq!1(I_fbe_Rbs$yu4n+5%lBIj}=7u15XEY z_cjI!mM{AI-kxuEKN#mvl4IgFV7na@`#RgmWcCoI&$FRgaiJ4EEO&T#BK!O008v>3 zYF9M3newj&*qI?PL-1W(B%#Apz8m}g>P~HKtu)_0lfiHBr9ka2{lO#Hd4qi;Gx3Uy zuHo!fZ@jP8L+6CpZmeZVdQr|TA-I2hUwXS(_L21Rl0`~o-!pCF+k+K_URQ~1zuN+` z?QN5Z9RGQ9zqrRlgc-v;WUIMuq)zx}R_0{eqzg6poLGt;nSV`dKk9vXGQDmvy57iA zt3A?py15bK{1`-=OeoLi!wvZvNj(;9z-^?Q1*bln^R}V3j!rbM z)s#|j-SmbfK+!E1x^KfSkOj;fkZy{kjau?DUR*5~qibCFqZO`BANR-5IYe+!I;d zJiu@s<0a3QI!!mtSTAR-s3t}P9eGa0UL5&3n`ps52fc+XzW)AGuqzkNKruB-8!RhW zM5+wJz4Y-nWvqrRqQi6H9@HeEEtz+bHB1~ShzcCB9T3Y6wxNbhrTk(5k||cX3nA-O zsAP*UvYSp+#={psg7=xtX;T-B%@+v>zy)vt99?i^QxiUMnm>%y9Gz|7La}|HI%T2T zHLLgJu33?^gJrs#cI^Yafu|KmZy<$b=G{P$hJXq~Od4K9 z!kVB6AT3M+tHsa``4?$OK(G^ZuMmY+WF+sQ|Mij}eutK&7uNL8w9R{}9|hokBqF*4+UF5wVsMN`7-1-{v-U_Y%@zGJvu<4tK zWU!MV;>#1B_n|f+oldY}knx++0dUnnQStYrsrJCQ2;j(&(9kFD?!y1WJiNd9|1b~1 zF$5lhD>rXGhs^5qeLt?4VwN7%dF0iRM>=1alc0BtZmm9JLTKOL6kS)I5*z;bO3~b= zX@>B7!r||&TAKe%gZT%A2LaTJwo_|s!F7|}hEx2FY=QUpzdTBoW~=T#4>ls%!G)9; zFdoJfaL&B8ek}k|v#~|Rx?vXJvPvgzur!vO0*lZDycnSm4k%=Ba#*Nq(<0e9@KcxE zT%IF;gNE9s#SsmSlaRm*Oj&p??4nWV)bHQF_xn!uy&#PYyvH&eF<`ud2!RdS>9s=O zE&BJt00gmTVRwzuAm%Z+y7H;fwoFY2^jjh30C*&K_BbN=&GaD9NF<;Ac{b3CI{&Xa z2eT-R>{!^ntoqflGK8Q9Gb;a2_qopaHZWdvf#K%9nFAy=_W?8RWFy!^Zp0iQhTst@ zYTtyZjMHkWzR&*+@>~8sI)$K0{)KMKQBYS`H_l!JJb?ND$)-1rjEw~o#iJ5C=%u?Q z6~Tn935l(iF@1qN>c40|`b5{yQmhqW&|rD!UbXXE1#t9)k*d}g!Qsb`{6hHf2viy| zTR^7)0^x&JBZyJegV+EBwF17RtD!CRFL=g8fjNfN+}!*JuB+P&41U6H2bLgpoN3r1 zwE+JVz_R8xr?4zh} z1-?0g0UhzXU?Gtj014brRY7uoS4Og+i0|@*04};6IkQF}zvydjdze+_2WnWa82e*@m z*2?l=_B;~%3(gLtjTVj=k^+o5a0W;p7^Tlb%!MAXH4$a5(ry9<4SYwil|WvINwv5; zvLGlRAPJ;!`q2}J(A@`h8wncs`}i>x3CQ5L_5JpZ2FY&+J&%O$IdW2|TGpJyg4qWb z!i?a9!`_C^Ph8@^0TEsDhY>-9)4wE3g!l^En`l@f!#u|gEW#r|r zA@)4DCTN(L0^thDcihmv#-vyc)~)8&R&D66hZpXK#dU)75(5|`!F=;L*s+V!$0 zWRT}V^EDROZ4hfFm{GsM=}>70d|1Z7F;i|(IjSK$0k0a2W3G09KfDGEU4uW>Z}D&P zenQmS{!$16S?}N!1k)Ho7K2v-9&IX!B?YF#pP_#j5$K_ZQtBrPf+dhp3HT^2u=g_R z|4t0R&3RbnBi08P`0H?X17;Kd;e2QOMIs^t=+yz+9Hr;&C*FVN=N*iRVva7BMkCcG ziFiHFuTnyMWQ*lw#OVXZrr(Oe=a-zC3i%GLkSkn*E<`E9r$pM9Q5IEtAgfju0zvXD z819I(2iw5$>}m(;+{iD*0s^L}==-$^EN~j#y?ggE3CXld8u+&g413-|E+e>04dJbN zS`hw$_0cnuhNec_a_BmiMJ>T%D*Zr593O?Yx3;!!MK_`C^PRR}aRYe=kUnwu)~#wX z&Z7`a;jk&ZSZ&cL7=|b*J3z5_eQnvSq`ffar{{dq?h+>3H8Cf^Io7Rf4-z5~kzmdNGzXr#j% z${&dyCwy37t#gH$967r{vdxD^c|vMo{AwsDNJvZ1!IiZ~TPUM{1GyvoeaIH|FAz77 zso4@EC}VIfKD2tg2UoQLlze6jd15=jcOWndro3{voZtlM&D9Z~*v7kR1d;9ttRIwL zxYd6S4{w0^#F5V451FQsun+`Izp!8k08?fCZ5t=V84ikaF&OU_CaOF>puramNLFrS zK4Y!K(7Nnch=1*J7Q#-jzWHmHvnNR-8Lt$#^P#Z@7>wH$ z%2+WrV+>MHz8@0>tL3%jcYB3!q{T+=X$^bGb!aI*B$^VU7q)u<7Djz#MC#6` zfaD^GYH%{eY!{%P@adoB7}S%eUV|;KaSYn| zT9=7#a0kMj?2VS_e`gHvq&i)vz=8AMLpBo31rl~__G@3}Nwz%~EJY{&Puw)Qc!)c;eTvu^xrJb4BZhqzP-Dq!+>YuQP#VOFkadDl_K z|ubqy@BAn62YSoT>JVD$Je7#C|Q=4X!)0 zl|KXPtZy7MRBxM=90p7zP5EE0rwzsy-S=|J;E;AIA~C?$*M*_)xsEZLOWYdwgs8n`pg=F2&ESF3WkHqEYaX3`QZxo?OKJ z$cQ#jNbCbs%qTjnuxe`7MdHPf>$BxEuE#^mKRc;6O;g7MM&!ef}#9Y_k zl&(C}Pk&eJu;vlb%8!hzeQN5;Uz_f>V3cBJZS9(zS6HRjYdYX0hw@N!I$~B-CZBiY z`!&s$t8jtDi{jypt(*q%<5sdZ>5u z+Eite@TX7US5$radv3G3!M?`NB_0YNmL2`^B$fA=RfJ<-8}BD1R^2i}%jOZS3H50q zV|`_;T{^im?#X`cwjfavD}P{vDGh)Bvk=t!?1lNP_8E9Xip+E|KEsaNy|^=E6r=k+ zTGn_Kx^pfig(hs%r9POa-|da3not>>lCAf&H5utioW0q00!VjL$zIK{7t;v)E|;Fu zAzyEvx?}pnp7^SE!z}xEYR&ZRA&t#mVU{49&8=0}yi4VRLf?qdcW$mov1+%@Gk{Qd zP7K$9@Q2LW+=(flT(gcwvL@C(ivVfJ1xZPrqK6p6LgL@uyT{?^xIt{6QoQas`86P5dL&l8-Yb#v z_TVgSwf@|2NYDkM`==?I7S6<%e(P^c$Fuie%E9X1cD>QDgt8oI*Q{P<e;-@ex*e5laB}&!cc^u$}VjKq(rTwdfNZ-@!x+i-bn3 zuxKR)vpxOa?QZx6ig$OkQHLXNaQ0`>^evLoU}>XC-FvA_Sn2((+*H2OSww7~uh=@@ zkbS{eS&i2_(O^O2U;l+}{o27e zv|V!v-t!Wd*^{D+!CHBWt!ROHi7RE5hn|yNZb5h0NImCpo)VK&1q={-YE}PcozOik zwP05<=SCN9L`M&eYOiODOKL6H_`X9FW6JMUMWj9cDmCc4sAG zP+qrNxVE!=`A%_(#XXf!HP4#H<8w@^kqrXVccMjvKgT_04dB+@Xo$F3>y^nA*BvEe z;ZMUg2+?l3GcL`|dw+HvmCE^i(y|Pjncbd5m2u#(MZu3bG>UDG_}gBQRXvLO)RcW5 z&&fkuC!sR$Hj1!Jntb_p9g8woLB8c<9rGoURuYXAEL@_Ky$cF<_%mp$5J8(aPgXI1 z*u^P6N zK(+j*?#2zyQ}0H>qlr6?<+v7^?YFwMrlwn739@HmXCEoL22vfY5ofJYwKs((k@vEE zkrUKAWJ%Tu^K!<9g_=wKK@V?zt#|U7@l)D>eevnt>$qEk`Ug`~0}P+`+!(kK|njN%E$KhDq!2c9Ekdld)1KnadsnX4@4y*E(^7+;&i>d)!!F zs~+)>_9_~Gv(9~Xv*bDaeMD5%?Tzog{(>c@^iyr-1Ge&+H0s9b`_=2$avf$PZ4(~^ z+1JtK>96Y4?^1MDyQrre2K6a!YzmR9^QcrsNmZo&q;TxYD7eAz*Y1W&Un#9!w$C+y zAH42HMkXCC^HA|Faz>Z21;eHBBwNLRZGD{Y0osY7s$7VYZ764bV*9Wu06)6alRGj{ zfS>uv{;^Nf4$Y(wk!#i3!3hox|90j&Z(O^b{^nYSU)hvCiz9}4L_;__Xiex6x#{5q zYUn5)`SC0s!P>O|ji%{VgV_wC?mcQ&%Z-A)jnV+#ZInOWq4Y#W^g)Nfp9@UKL4{Av z$}V5N_Gl?9XhnVZmDT88)yK)5oGsLc26VZ4%udC-_j=UWxJu_qat_Y@A(O3Y-+xsE zW~P39#3Dt|`K#?a9A%2ZJ7;kWWxJkjvE9JUxzC=6RpzJQai5AIZk|K2BF(dEH=}85 zB878yixjs2?@uB2S55xZCnWX1i+T#&B}LAiJ&fM3(refvJ4Cf-jww7e{^(=Q?jH4A zGe2fwAk#5<&F|y!A8u^&hcLH3-!FG8SdS4p*G@(G5nFUHpIDP8muIZwi#)dAj@@-s zXaM;rou^6Z**8O{POk_z1Iq6vUxUWn;ZSwa#A_!<24YW3n!bwR9A2D8Uz?UOPVk}7 zNDG;C#wP7GtFMg@Ih zB{NQr@^kWsRoAeYhGW_YgaQuD$QEy9RJT>y@(8?EwGG~=Gv^HFJ0;;Ywn z5)`E#No;qb8Ap(Ge?=fuOmMZyP5CR8r=RAI#VvJFrJ*CNu!30nf~@;^ zPY!+U;!q}DNpBT zwa?0AybRHqW>O!#hYB8kKd2(eYBP8v^;6}%1EDGAZHmnJ!?YfUzJjKPtGg4K6IWOE z_iZxWrL_-LKep&*t@Ey&Z1j;d9v;6AH5zroVrDJ&Uco6F#+=w-IST1nRGK)B(%W2c zg*T_u=ToKQD43F(*4)MWu0!NArRNBL+khEx!($rEE+9fS%587AH@y7jqJ-_F9{&=p} zeSh!!%XQV~`h4E+^L?Jjc^=2nwbFR6$eDPpwvik!oQQ(QI?5#-)6HzCsh;NXicY*0 zPqpc8f7OhDl~P?$oL@TfKcG7$5%G* z-+%8sTkczAh~syMpC8#3Ai4E@$xO~F>W>@ii=LIbQx&wwy>C!!*g5n@?IoY#F-Eb1 zVuV7e}UaV=}1^V`>2i4#WnpZI|oR0rSg9? zC>cVYPF%Y}%nuEz#Wli|!MwDdbcc2>~=ftaX5>5v!Ezp#b${P z3!%;2lBcpTZG3osTFhqC6FUd{dm-CvDod#(JQqagzj~ZB>@-Z>*R|qB-Q09s5z*Ma z@m##XbIy##@{IDLnH^7aK}RE{qL%sdrH%)myBplUZI{?JBgu>z21^I6`O?+q@WK2? zule5gIwtD#{s=pp4Q5w0sU7|9IIeMIVse$7d~}j^|7Lx81?TizUXj*jf!2pqgYOi2 z2B~L>aC5ov(q~U4pZJ&&e~By6N z$~iknjf$ozysIy5@c{0fVCjLN68~!B)w-PwVLaac3-$9laVKJFXQ%7Mlrca*En)e(x+Etix`)hgS`;wQJ+f7?hDwPkd* zEm<(M$8{X+UcGa`MUPVWc0M5RVeOI>rKOCIn{XL_pte$p?u74l{_S8@xc+EX z@Z5e>F+FyeE$kp2qq*X0#jW1&u0K@$vEBDG?PJxI1I90_&W=KujtBPw$*;-TN&zb?&S2`{+W@m{*pdosl5VoOE^cPX_v-<*_u!7r&dl>LD(*~C|xSF~v?67QR;3+3rZOhpgzS<7T-L9D{1D>C-ln*UskDbq$xaF4q z=75iN!v-f;(L#?|p(|;HUN-vqf$R<5As*jFm|Z(#v-rHq)2Aw1ZMA($2klNKzyG+Y zS|YOMvTG0XSIQ-t_@&*vrw_4bYKC0+cxloq2iT~&KY0FuN?6VC8YzeYDD)?geOtkUt|3(szz^C)b1Y&N^Xo-IG9 zsC%#K74DmKF&$={wwp@S659PA-F$SZp`D(d{`&8QD{R^}h8N}Yyf?5KNk;~Uyth}* z4&jnJ5Yy-0u*}t+ewO}aA-kLB0h`gQQIiAle@{+b>UV@~Pe3?2LR5mCJ*7JCg} z^^BGj|9&K89jF-9Ds{#9%|hl|`er@bX0=)BYgLAU;3lW+rYvj^$yc}b?f*dFvgB#ip@^)CZeAw~Q<_pVH7xK(y4bM&CoSO0f<%5aw} zYmW$j+(sp*At9Hx{L;p>;a-!nsV?yp?OPsiU+lD1H_wP^W!sMC*gl~`)iBjaTNfD1 zvG<6aX!6kAy_zG)GrDpa`+{QSyx8+ON;~ZV``4#XfTEUZL`^X!>n~twU)2gR}e%kRD}VWxi_lC)XzCy&fP>6nDcGt z@jVsA+&NmWzcnW1PShn2DQj=gTD5bgf8RUnf^XTchs<8QI5~4zCvnm;4Mp{FvsJ&J zv#k3Pb!ErpXjv)$1V8~#hRec6nNpqgOjY(;2iBb&Q@&jL(T8!Q|DDe>rR+w_?EB4) z08M-95@4KUAtE)nU^~%ov07)!C-B)u>U9e(Y^N!+p&MAuHVQoVss1IDp31@~{`Bn3 zj|jgL9(M-1!BUtAPjBMUdh+E-#js-7v!$oK@qy_PuIpaq=ksi2x4o$~(@+IuY=;Kh zKLzAGpUb}pVdC!>W{8#ZiAk#a=NF}FK_q;|w;%fteKjTW#fb-kje-}ZT>cS?cO1p7 z!cQO~#edb5djrY4-ia_V9o)7G3dZ}w{&d9gYpwo!>!I7hSw82EP2GpG;u0=q2m5QD z6FuTrIsP@cjzp86p=d|?y?)GJ$C=K5jU6 zXabymtgVf&9o2Lec*qg^66J=X@Q~OCo4N(=%~m42%k5i2rnrXYM@&RW^BG66-OX z<7!rAuj(kKASN_my~k9-zkh!;WG*JFg$u`^ez9i5hQ;JYh#X&NO5aG@B`_UNw&^V3 z+3)BNc>>0?Mn6MNG>GZ^{@my)S|#6J`^4$DKB{?13U<8s(&lk4x6uK}0Q8U65@kZ- zrA=oXv9(33AC0GhmRCFfr*rIDS@jGp;B!K!SDqaF^B5n?R900@XfQfKv3ZqQ7M(BG zRM%wg9^K(}^`-J*hBG4Way1;Wfn;?*o7(N^YsfhSdsN7e+(*ki=5){u7z21{v+<{& z9KY@c!@8Ji3$(iH?#-9#{BIUOcA5vwC~@c-M||kRh(^Pc@0oDezK+2-qVR*Zua5h7 z(Y4zg-I#~&-z$ulkxP&0vXpkGkN71ayOVZo?%hS9eRron&-d`?B^9>i6$CPDr;Vp0 zRoE=FO^AxG^Y=U3a?#PO-C>ab`-93;hxQFO8!w(SiMsjT)vkR~&9}{ZCf_7ZMp?nx zR3>u3GGa2+GkQ0FkWY)9mQ5>d$kPZ~KSo>UYIwHKQ4jOEc5Cd=$GSe!8SlV$f@RiS z4jpqvG^O#~ilqDNVjHD)?W!p}SK-sMe>9&J?NVq(LHLvbt*G4g5%=pCcDSu&WhHYM zq{aW~p^{RPSXD(~xiHWB?wu}k5BRo*X_UX6phPZUT@7aX??;@mEw$`tgn-#mM_^9Cv# zM~-=qGwQ>aY##l7m2=|N?3bZ)zipNr5pRMgAFL@$MmMdmF1`-EK3Bb2jTk9K80=vZ zq+t6nl*S3E4UU+E{MdbY-(vObxe)=+)O_l0aGLB(m?e8Ojw>s((e`)T&w9MnCn?#= z`}A5Flat52VzI{MnmazK7duLp=t}2h)(=eE+^lxjTO4Z&x1iWpX9=7*r8lxc%aX6{ zQ*q>ELWH=>{AR;9(QB+?KBt;&tfq}$3N1Sy|5#-DtGtEahHn2Qa*ERCQFmiOO_Z`~ z*rH*?;pSiPJN5mOb}N#g&RSLSc<2{0WqrB5^C5#>j{NoJi`%-ETW6T$?|5?hA}dR_y%5Gdr+6 zd$&8SU;Fh+6M>Q6I>rMi)T|RfdI+^9=>)^+lgvl4vE6*%U1Tl%#v%N$2i}orUKdBXW(Rc^4J+vl zR*Q3|CHDu;&gkyn=J|M*bIgMw%Vwp3Kuyh?J32S_9ev&sdo*d# zemZeUr3ySZbIyF@SHUl4%#nNoB(YcXnay@J@5fHD&`UU^3s=6sdGoSnJH1f!9dnzC z!!{VEc_rc1yFW`h*iafe0PxV>SKeWL%id@lZS%W%h$ zw;hivj!1WNU(^kcoE+Z7!P3ALPga*L73fwZtx9hAO*!2(G0u4T>cmN& zB6g{lE?akaFL^)mQLuJY7SxqcIe?USYF~WHLY}%3}WTH%OpCv*kMVvHv5f* zW?6B@Wv|Uo@2<93xlYi_xn)rS3byXvUE1!{RMZhKpkyTeD)nxQ9b zRFpa*BB(aijzx}(r)KZzmYK_0t@@sKMHn4yn9WojmS?eM9eF7qy~=gQ#DQa!Wkp}R z?hRE3Zuji1emffTM54dp-#MGh4?k%rReS5Vs?U1g55tc84W=$vMVyQ|xSFNsXBU{S z=eNuIaQ|$M%-j{L8!dfA%i1MqV2@O2lvr%d&8-7%O%=7Rv^Sjl<`j7KLY1@E-4kWT ze=@Jq>r?K1kUcFu`y<<6lUL2I*)LbWQkv{54jxM23ja_#RJvg39+MY2y__S?Iu;vsuRT%fH?xujdboJWL8rNri@woWePP^^t zVY>R6{N`QJ9|KKU=U|FGSQ#9Ds=@girzRAR6)tS05j@2H)`$+=SbTyNRA!4` z3yW`rA}FufvdP|ivHe4|WdFA!FRf>xlYXOQ*cqVP6`tE+N{oxqm^M@?M#r5oP=eB0dS5av~vZPt+d;4aq53m*~VV*Y|8c9AB# z&wh=|^)0JcR>jtctG11fhet*WNwn46@sGK3Y1+*>HH}e(vtGvO(Ch1loR?o0f7d9S z%DGo_&@kOGKAcLo_JYZ^si7DNrVFmGhw=v;;yagI)(6PY`i@>4-txifquf{D5BEZn z4jyGx^PK$nFzVJ3MS*8GBdEMs_U@L+DpsT9u~fO`j!zXlZy)W7m+Z?|{$wa{PNKn@7)FLQKim0s{W1`ZUTg(B8lJMl3lNP%q_TBa5gQ7-X;hpp4cS>%34t7xgH1vC$>RhKVPfPB-umGxxop zeto9SW00wG{rgEQ03_2xEwB8DNR@*}^oPDx zYiV&fX})#60%u zYRx2Ot0kFena1WAzD8D?2}TsCCfB=MJ^smbBIkj}R>n2g`xg48%zLPv6P=2l?(R4j zB389m@-7#vUi|e&mFvHF0!8SD#jOaZ=IhksCKHk?e)yCGd* z%xbuo``Z=s%;`6`KK~TGhO&2=mQT;v*Dp%+_FjVTgIq5N>p2N?kO<#U1*kg!)N8zYU{{%9r$MCe7VN3aP1Y&Jx@}0+B=zP}+>(#)2r3wRI3P=GF zc?^(UV0dvNzxAbU*8vEgQuXj~Mt$gGjG8<{IqO1OO4l1`U*7m~V)Ur>Gw;4b;}>s7 z&)OPuEUs^r5?gyF<;I;UOD_TKT{o9U%*Zys4=^sE8JPI`g}=Tga9-x}GgH&DW2*)| zr^k+~n8!`sTGVIrD%Y;rve37=;66KRx^?07XVtD7rq20vts$qow~Gm1jULu7>~dOl zchm0A&kT%^3smaJ9pDj=SctoNvE%$BAN%S@4U1p4R~5frRyxi)D0E=6z)+3q zaO7qm4V9~tzr8=uuw3exa1i$u&9W}^u6^~Kvqyd2&fyVNL2Jm-G6h!ILEQnJ&Lhc^ zno{>i9@!u7^0c{lDt)ZMQ20ZN&WxRYcA@OihczZH%==>3n=X7wYhJcr>d!5$@)E-x zVVfv%>7}=lPM5eIrqOwQFaK(y?$Y_`7GHU+T?Fg;^Wmv{OI{!N49-F{Bw1|FZs6xHsLDBrTaEf$TZPPxAcx;A#?@rGuNw)DeFyvL+(>^z=TO?Ch1 z>stvG>zmFO8E`hmD2j7UZBI$+JZ$@_a@~ar2 ztbNo3zI{=nzUR@{eOJh^GsdQg-aFht;!eAHP-e|h(s0Rp$VfypLnIVb5lf*?hfF;d zxUjz?gMW}>^iy{*^U1p6f2(l!*jVY8V@wHG{KF0}Reqa|qo%9nrr97+eudefqOjq~ z`A;^S1h14&e8TO{=XK{&3-sN|zebTjjU+#*R(az8PLR$Nh z)^J;!VKLO5i*Fdo@0IY`HOJCzdY;l)QQ55~`Xr;dW~5tAK#%oW`&s9f{K49x!2H2F z@#kS)pBU@8_R7Z^56HEANw0tT?AC7Ye3=;~`V97(EvB58pPMFX9{&`gA=CD$$V>lm z1AqO2SI5Qjyz0GQ+1oDl5BSG~2ee*VJtSf`942uiHp!^EarO~FrfoO?jW8w+hF)5Q zA`hLXfaJT-$@WIeAIse1EKz}O_m-Ndh@QYLgUiW0+Qr zJ1q@ACXDeOe{kT|q_tn;U?RkriK?SzlgQH6rm&r-F(OD4UVWv*v^w4#!?rxKt}mOY zK)xt4%NlQ&RXjwef97K+)jd(B1F=al1IvzlGf7g!8CJH>WO9c8i`DABU2|J@e6U(= zBJXy$S+C+ZSFYu3!<>K2z5_hTjqz%Erp+yjdX&6*QT^jHaykJiX_vB!J@6Zr|jYMJs(vUP}`G1Y5^PS-@NzdCnvms?8G}rOGFRfxKYX* z=zX1bxjk4@kY(RULrV)Vs<_v_3#=(R-P?PuTBd|k>z3VI9;2He{?g}~wVs&g<^E-J zwX08mvHz*;lC1NAjcZG-mGNP(aEa=t(#~}T62Zc%?k{@pYZq>tkC-*)$mp8Y;8dGf zA3Jbvesg-zOY?&Gg1NoX!zx#i0y2o(T@u^RA-0TdoUtsjh*JCq`LrSCKA1YLiyFa{ zc^A@nVh?ri-hqo31yNAqUl!<6*>=c+2}HO7H*epbtJ6f63~@69TfC7(H0Ep1 zx%7tRLnvBi>f^yJ0s<4CPimaVFe)p2%nTJ)V|ZbanSSslSL2{RbB431s0e&9gPhjA zze=|zTC((wuD(>6XVJzbNY9Z!_$sE5e>qb?)+d2E|2I+LYwm9Pkla^OD{Q`OJpNNh zX5(_){QgJa98mbjBmF88cZrAuK?)foy^{oge7x{cfB57dX4M9e1t5+HXDbduivULT z7+4z7N7PEwi%m#K@cZre#E_HVfl!m4tlSn)ltBqQtaFLt1bP*x1A07N~$eSZ$+$bl9X`~Q9>1>so9<}so zp?y3oNBOpeW5k?_hK9yA?;K&uUEa6%;h95^V@`Y?XUl5#|6o=!a2b_*DWQE$4K(QF zSZ@HsK?YOeF;aFn{QZmj$M)&t8DOUOc$%IpQNhL{1Z{wDw!!xHOlTUoY?ReJwbiZi zxC!AN^=O)8oIw^wJQq0_fkuM1kNRLZ>CVDaFp(VJYydpjZr!M`4HPg{&sUQbi?2HS zt0TiPV2r)X{j)Jm18d+9f@KZ+G`aXdKRZ5!zdjIvUvpYlu2{hjecYg+)xa4iK4?4_ z)*;YzaOQ5w<1ud5UB6r*he>f@3RZ)EMY&PNOr4EG{H62|AJy3h6~?>*AuFRl(8o~j zRllt~Z(xuhj}Ik;b8nZ6zh*ZQ-Wz!x-Cww_{YNbnkD6I)%t^nl%ZWZ%?WP<1;nA2YTuK>dy77 zHBUH0h>z$*WSQ6D-8W&Cmzk9nDeoy~T<&j($E@{C>uFQ|HjOwaROwAgFoQ&ZkgxZ0 z!d;Fpr_G)b_>n2%n7CzA5WZ#u{Cqfs&Z&YpQ~PGfuDkpbCiGFGIUbuOo_1ALG(yMZ zd8KTxVR)aU`qug5%2;dQbQyrw1g6OG9P9H`Ld(0c$C47L93U5M1$l#xk>hyc5%!V7C66VaUg&2P z%>CSnfXpRrzXkePL{S?w7E#-83PkPr;+JRSv)q!_TZDuH2nY}1e^4I9TDEi1jz`j7;uZW3JBZ=yT%Tj+mE>6(4Sr>w=j0_Se(pZ z(Aqe~FC5S4_d&3(iIEP$brYbM@Nz+~B##fl9S1R6eBFD@_s0r%c2a&zLN_F0ztfAt zg%gjI39k(@y0tl-9)lYDp)|05@5LU^4#;MAT%A4-x|DvVq(8P7Z(qH&g3CmQ+bD4? zbEuVs{4OYAE?+OFz5Tr&p|vO0sbfdF6;>%QuEcluVq>q?4*%U{YMr=QRX~D?3k8)^ zhs!oXAPd+(0&(3ne0=%_QYOqk` zIHs&%V0dWXTSd6Upe{BfDAGbbyp)6j zfF7qzI^Evt?Q3r^h3mnC2kqXpD_N7)WdMK{FA!Rw?@=eM-=P-lJ6W$LJPzJa1598DWw$D9 zC*ctWIp}N&ug}%uHLbk>7elt!O!pEVNHNM z6cQWz0oV)}@KN$B=E$**jf0aG+&$)yZ5n-^OY`xbZZE)VyN5%GR{HcQ7EY0Og&(YQ z#H?Dx`L{ip#b>DhXrz@U>vG|@Q)?l>{TQ*AYGHBIvma0Q8*CY=41RokKzW~5FegZ! zFE~CMHg4QV$Y`KY8cRe&DtL0a7azoutP5t%=vjDH5S?}v+<2G1NOK0O5||B-xxQ78 zky>*NbV+bXo3pLBA@l%dW(N={npq&60M#Ej4!6){_ZB!Vflaw^QjM5RKqLUl^jOX2 zpze?-RQ4O)G1U`QM-xad%nZR(3#~A8@g~7jhkSe>VcH-qbU|}0w>w~e1URh##>~+a zBC7jJdLs_m)k%nGYG+!7S41)VM+vPHlyMRP zNwe@sm|*_zmPgZn1W=O295#Tg*$~)msN~{5+x&KkGi!@irO{kx2CQ1gkUw z%)wHg>{JJ@Z{3Q%v#F;@qX?&(-M&~DAM~r8e5A|_{nrwL1ycb4uF4sP2;i=lS2Hp) zW+#%9lI*{|qdsu(;9|cKH8nNKG;j5-Vqxh+K3ev>2vPB=cFO%F7w}-45txFlD%!op zdYQkD;4;t@?%yf4ckio;?4hO%FPyYJ`#l}rwBr)rC^WnDc03=!b<Sot0xg$7H^lRYz zbOrbN)KITn*?9?~{7vj^0eC2&2}71K4yHej3`>;h!l}{Dc*Mn-$>wm zP;n_cKb;TiH4?)b|E^y#iFDKF+zRB8D8j9CPU2l8phKRm6tps&539l38;jR%w#=rP zU56M2!th~LRiYt~k0O2RGfA9@p`oFZ7qhB+1y?_pbsReLz(N;U@Ixg(hUfCmWBf!4 zhzgqu4|6^zF)=Z=Q97{9Xk1?Cxq4Rf(5j)h9SGd$Or=BAZpnV4=P=Z%=4ed&wV!Hk zeUU$LqyRV{P3wJO{bNW?3;1Wy?XE$VG&7V{=RA-YPCRFV88xky*@&(jxpxE!CLB&e zVTPHDL&H(Y49jLw1d!{^mVJn0ge6HTEW)U*sJ@!;qdJL@vkB|aDyN-ZK}#!i=^eEB zKgw?az(|i+B{VL#W!tt%tUIDyfv0@|QJl!}f-MfqBw5I2B3`IqY53f|3&|GZG2uxe zdkCSUXsK=BvZ(Xew@t#SV6oB?X~*TOcpoFqlZOKv*$F~`wuvCL^6!`T)y@72^21O= zaKoR;kh~v|=#0Qg1ILCgTz!+J1m>ugzk`}}fFUOx%}esLF!ylt)~&9|S!eJ${dnx6 zPb0%5@|@%2<6r}xK;n;WrT@_J>ys0-V5kC(>5u14dlLK_oPK%2`a_mQMBq_|{vPeD z184UF%m#39CLS4yU<#u5Rij?_5@mD=PM+Xq2O-F@ zuebD$iDsZ6#+oC2Ahi-`$pY#T??63v`|7A3Clgt?v^e@XRkyKo1T5jPoWjMsl_A?; z7gTrlHYt9#OJ4s@gfw)m^XRT&6O4gM{xMN)9o4DlM|HLzrlMkXI($e`>r4@Tq8jRq zQz>6OD;t`Hrgw~FK5g@2xQ5H19>cbQ<65`AO-`$t__XZ{Z0>K9vp+sR4e@OM^hkxV z?OnolsPO0(&?Y4$@F)CGOM!R1Uv)1ze(ACgTb$G)rO+Xvg7>^`{$Tb}OZV?23~F%N z`NhRUak|%MnyLwEKRX05vmfnmB#{K|?{uHME58KMfKor<0#!VMJxpH6b;lvSB!ZNX z7+-7}yb6_2E@beer-2~F#48hl2sIS^ECjq8cZRdShtb~E38XOsU7^5Ye*tI&XNRLP$WjyNEYLzpN z8`6L)#^MFJ*9?iH(tUPr?rNwc971;aL_39pFz>N1l<4nIA}p<@LIm#t#UAwysx(eX zYaRsZ{g*sQPhZ=brVJrx^VY4mkldflGCNvaTx`29mgp(qGS{-Rmtnz7oG%V>%d3Mp zNjy#O;-LPLVCD)E)9FeOQ8|2dGEoG70n<2}fT!wUPFW=J4d%B!LoE$i@GYyJp zmR^%lCv0eTRoymI4fCqEF9lDJBx<9e!8_*qch*)^7!1@WULI*{<~uHIvB5r=>o)9v z?2$#TnXN|T>1B-RyBbq%A`0k6X-V3v*;43}>Y0kXpX2UH%Fd7sCdU_fc&lW_sL;F- zw`zIWB*WtUNFhi{IJ{c>2ZcR3o!?IHRLZLgmpgv2!R%VKHa-Oj@*d>S@NX-IQ&mFB zL8U`I8rm;t%1EMy1ND{s@wTTYx2;{f)|ZK`|7}|`mM~GBK^+qFi{HE=Xrs7A{mt9V z-1QJ#5)~6$bePk_VON9vh!D(w;4~z$59!|&m~5WPh-=)?+%ndmSf#c76ozIB{oLLK8Okf(m)s{x)#j~^p}6cDdi%#yDB{7iHbc2C^436I8Kkh;jO{YoQ2z3Q!D0dlTb{kEM8Q~$n6`}ljd zZ`w^vO=Bm8%eMyaZ`8tkSg-{Wb|5I*$Eh_9!%y|53ZulAr=z=8*S}d-wq3q4V__Bx z4x;B_PC9oqbNr}mN7v~Ie)WB%R`>=v{3hgp6&0$0sA^rHx^(o(J}{h;DMDe2G+3~c(#kc~Zp?nq)gv5)!t3Z8uU zXI7JmS@7SGBe2$km>|?l*x{>mtik->tl;CR9mjWR3bL(dl|Cv@^eMAiwSWhpBaMUle>07TCeL@rb3SLkmi>84VTeGe3n7#KDI1q1$w4=mQD zo?46}GU|9uoc_p_TcKa`*KTdr9Ref(Lm(ga?%lh3>nPQykOlgS1;V2UBYj|<{=Lju zKir&&by;V2AtfQMYSEC^R5UQ)2mX%l608|Kt?Zb$)M_cw{*mNUc=ZTBR>;8bGRgoA z7YF%_dx5)OMQ??rM8B@OK*-Xc^^2cAd+9>Je>0>2j$&R6vj6?1_-9^?#m|+&YNg0b zbh0fYm-3)o1Ly%-IiWw4U4XZm7TuuxoTk6@@YRmJd&6KzMQ{#Un2V7D_t5dm;C?g$JV{`%A0f7x0m9fCa|oB_q|5hSft-1t?G5*%5@ffrjK&DBB@R*!wU?_wVfL;miaSX>oDG z+O@@SpCH0GL}%kt9+ZrVM3?|R%$SS+cV#huuB->vSq5<0r1TRk3azp4h$4MZ_9^L~ z+ymJUq7H;Zo(SBcwxLWxP{nXzW*mY1W1*)fY@Sy2EDI)zI7I4#z`cOQp*dj!L;}FM zC0bzWxZ=4lu48cy9j> z@Cc`YDc}~&R>W$Zxz1w|H9;yve)p##f)4D#sZG#*_)2!cC&c${#(3xFaJE8T9{mkMpAJ? zfyi&2uo;dH90?7`<59r(Xw0LFRgYBy^b5!A^1_7)M(ijI*WCwxg7c*u&?o6yUXK|N z{99&CF5m2+q@of6F@KT^YHF^ff(#I-_^Ww%&hd|8Wmf{F%|jUvTpzwI-M?HHi~8}S zP(GI=$9q%(FDe0^r-3YzOT>T*sfS>CCk)t<0083s4OkLM$6|lR3;yv0!B{aduEZaD z%yca$2p1L6gCh_pKt-_lzC-#=YLwCenWqi=E+-&YBu;3JdTw`t$KX~JVL`#WY#A!V zlEGg@xdrmwToRU?kc}h4Eq|QQkjnGZMK^&0GfmmvK2zRC zLE)Ob73w;F076wFOeGh-JPX@n(;?&e8dJH=oFWDa*I#SI$yh8P&!hCCStki4Psb7w zCSw%)%FwS{Xhm9Nf-hE1KHZf5U>t7A{ezEivAGJfot{f?Va@qtbkuX9o39*C*ch>k zGv#!e1XGC!wy|*=3NwlfpjlDD(=Uc>4#vw}#ozctjEWmZ3%`>mUEeArMBK&DW;2Wk z(3R#M>~zc)^=nDPFXn{B9f`aJOqBv|`E zkU>n0wIb5&oux#UMS}oZCRktPz4j-tM?B{MR%AMj>Mc#~yYh2}Penz=VeFeC@H^u7 zw=lSygAwQg8Q}($%F!AJEQMG)V$sav=FB4VBpUyqwgSW2sNaH0PXx{0KL($4Ij!e^ zER_Lx9{jT<1Fm z#A{7BZ_9TgMv!3L3?X7rUnbM$VJx{Q$u%|Wj&eODD(b93JYwZRB=HM_*p_dcM(Go< zwr1-&PMe*WM>pVwdn-B(%5y|#0CkzkM&7e$Wv6hh7oxp1c^7u%F61? zqBTyJ6eZPwwOv_Jk$`R?x^lJ<>~g?=8y+e#B6F-l?Y&?T$Rk~hLIBGcd8z!`A4nY8|5RwLlwWC{h8brj5_ca4wuuJe)~$&jU;7D;!nD%} zxgZbPGdQ_PY#@R+Fg5Fg-9hfCc-aJVTF$icEsdDi8Jrh|rT5JPX8r(G<_mo9H7cEE0SK zxh8U`0AATD5I16kSL=o1(9XI%pvk{YH6|#?_NxpPK--)SS3i`1V}MrXTIaBH=UWTx zx|Q{C2-3JX%sT_npt$n6j};y0rxmh@H2>L_Cx#VHYfrO?y?`^ALnp3v*bI9Q;#Zof zXZP|tf1^g{cH(W^zuRO#@DVmLYE6K#oq^&;04GH&t^4hChcP}yAhjW2*2HYp(|8|j zxY62xGFdl9AH^?xyaq=Y<5y>0x^Arlj=_pMOST_cBHlPqsc<|WhH;2Z!K^^zIfuK2 zBu{bxB?jAA8TOE9!eWz=GXW1gM0k|jCSMT&(4skWE4pb9aj2k`5KRI%e!^{Z`v?vQ z>6i%M|FEWhfKwR>EqZ%=I9sk?Ed;v8CF7jFr0j%rjSX%peE^5HNJ-^Z$VH2qGC;1e z3m|B*Q3io-;0t_b?hO_)0W64wIl&1Oz34O0tRdG2D4`jRH=q{VgM=j{?h|uSaAJ<5 z?gc7Sjk^UsP`dj2FKfhP7pEGCU>TmD!@`Um{VHB{(p74jCnHedR`6DiJ(gR_Z>x^0!c>i5^9B0r|vJ=p%Vklf|CEI**#*E{0@D| z7#Wv(dF>mm!{ugKknr@a2^mBq3UTnkA&a_l82p5WYCW7l>{7mPVf(2|dSZd0;qM>o>Sp)(~qh+UReZtQ1sEskh#LC*z zSkTtU#@N`(_N294=d@fY24f9_xqpwE)4jHuGkUj-$0xc(Rz1`I`_SHP!fLN#6=a2E z-UvDt%#c4+a$u$Dmw=M(0ZU~MC4VRxjoS1hK4ZEtdstZH{yP`LGOU}vvc!{Y-93Iu zn(SXX<)4-9eXbIPpp z{IzHI{6GJ?^?XqA4ElGcUG1M!<^U9Sg&+$)X{CLxS$L-sT88uH%?|US7 zD@h99yM%qJ8T|^GmQZi0lH1#iu5X^kIK_7K=uzj6j7C2npZkF#)_j73hgiqA-}&+N z>ycCT3{H*dW%M%)#^X%4ku<9>PuB9TzqeUE+V4606sOz&x|e6~lg@s|#VuH{K<>~X z;Y@A@+qVCH)4SiD9_hENd3r`Q-MUW0#wIDXd=_KLjNcc-U}*7hb2q)YvGS_7x2B59 zqA9|j>tz1@T&7PE{{8zI&;I_`r7#%#{{Al?;{W+?UbC2si_7xMlfx&HtD3NO8NYni z-=~%9!?d$Y8SZb7i3oAOdw&J~M&_y~{bP69f>pbe1@`m>OS=5{`7`|6%S$W8WUb=$ zv%;S|c_QFFZ`tOi{`P7a8$|}gGk5Cmru}G{f`-=C7*9{eqp+|qwq8Cye-Dj}w5Hd&mxg$ZN4@ji7;`9ar&;aO zjG6OSgk`%7=;U=^JB^Qze@?d!!86@}PsLC0OG-*ARVpVQlPS0>EYIpK5UE=-D8}79 z+}Cv4J$*wxjj2vu$1YsFc*1!+ zF1?oJ?d@HWV7O)3vSsB7hK92itorD)cHfWs`cU0;s|GPy1%>NNw;05(7I%o$PClWN zeBuVZnZZG)88c=?8Rz?7_44xP)_-zpkGMmdGPVed#agm(;e*SUFT3hr%gV~)ZZ49Z z5cp+3I*>b2uBs&YsM63w|I&ihEMH&W@AdVm$KNeVJz2chwz05aWF?BtFUFe5Wv9Gp zuI-N3ow*YRJ4_1wE?v5WC;M>z%AF@)Uu74wYw|HT)4(os;DFiZ6w{2tqb32x#i#JE z-90^}UG;hFixxecHfzDjrjHNg@Z7 z;$6#$IV(?l`=Y|%MU%l8y`>(j)s@s`Hr0E-5Kdd$9My-Y5GLjS(Zk zFRQC{YqA_GUtL)|uh%)>>}Q%~^$F3R-@iZ1v~O+s_3fpY&5z3xl9IMTG5ARBou}Tj^Pw5L16cG@3VC`P*V^Nc-j#XFC{q)+kYj3<($26Bl=xW|y z_CxG|*Q%(<$W3vExd!dk>6%yr_#l4!<`S7LIfD=RG~@3jIO7uJ#B%azFMiEA(FQdh&@wZex9wE2LbXm{r6^p=d4poQhxN_piH- z-FnXSnoYy|ch8^ixpL)-oPxp!hwH|nyG=|Q_llYrs7I;Dq?(l{N#UpO)JOju$?!t! zFt3Q!#tzcJ)2{WHaC;OUP6Y$6=-@YlCD^+E{O*)LbPrE2b#xwXj^JUA40IOR2=MYU zX%oFWo@wI37RvrZtiwuNCEMBlP_Tq{=z$w5a&lK4+CQtO$e?8}S^vBCp3x|Ko+4A1 zMbrO$V(1_RL^?esMJ~onHTWe6AZe4j# z)E0|%MnSM-mU5AHj_Vmwaq(8v4`)0{^(+U=zNX@RR#sLsf_F$)9<{bkSSqMF9nw3XK$p>t{VE+!k`r@DN6#9KZxZry~-cYOy z^RmdpI@!*tQ>RViF}84OLJ?Dv8rsjv$$7!OmO271l&U26yZrptoT)9N26<1<^w1Rw z8a&mS7_Q8XEecfF(AQilyW_-%w$ax-<) zEO`A}AEPd~bY@3jmGkf1sevw~f`zRfdSJee3*P-DAPU^5r^4R7dw;aI>svG?_iCi2 zrx*1*U6{*`TMa!HllSf&Q&Cazf|Rv;{LACpqfQjujYgS%P>Ic;%=(Qr-iAGSVVXbp zXJ^icBC8|j7BzKFh2MMiuSE1h`<60hR8$nUXQ}6uY1cQ;!8g3VK6%%kYroXg2s`(l zyv3_}0DZCqJ=fHZiEr3+;?E}>j?oY-2okSs+K#_HvgW^oC)CCN0b=|=4|Z^+jIICw zW&Bq#p4ypSmct>GusgxY>FELJ(+V4W_oz?t+@`hXH|kfuK9h}8KvT0)!JAJ#`YqtI zw`BkAnSU+6|M>Caz2b%dcKw}o2|s@P*c-T*F?SySB;02({`&Yxei_<{97{!2)qbek zcx<>efglB{V5n71Chyj*%7ul61NOnRRwZ5f%Zfu~)gu&xUTj*;m^=Rup!S?HJkXi& z&$L-5&Peg`$tC0rM`MSS#p#Mz*5&A9fARe~)8AHQD{VY=_sxf|XVJ@MJexovxtW`r z>ps#hGB(s0a#!L^5&%Q!&w@bnkN5X{&T_O4??S8a#rL3oBnMglczbh&h?QQB*{|+y zmHqqAqiLp^6mbe_JzkT48!fs8*o&G=Lqo%(!XWY1x{2}h@zEZo*4rpj@9OFd(I0)e z4+V*-1I1Be?N(=FEaCaX9y})&FJ2t_`r0z{Jg?Q%6X+SB3RC~zvuBT4Rnnt}p`mg0 z-dxi0^j-KxRfXw5OEacV|B>Z2m!0oq;XP57AJ2VI7Y)!ZA;%8As_N>Pqc1O_p!;u7 z3Xa_v?6fy`VmvGCdTr^#g$u7gCHHQ_U;8^>ICas1?I=!U!O0zWx6U zL|uL4@3mSy8gN|9_UCoQUA4S`TOttK)rlx*6d!kS?^>1 zmRtd5^x@Ep$BrHQ@$K7M=J3xz>m|5vJoRPrV+@~Vy^~;%!FVFy*HSKQ-=bFa>62DS z4`5u$BYF14i&X`LKUGylDF%ss&h^N}9fy8SH8*c~e=E~{Ec4?0mEov(#w#Y?^CvXD zWRosS$Q_Rc(2HjJZkQoved^R>tcrUL@2eR{6aW?p$9XEQd9)Bl9oD*0P$Ih#RMxOLz!|;sb=uEBv^@7!D2_CbVT~ z2d7`3=E?fgkTzhML>uPjU@s~t&u0v*1txg+?%fRcG2EJ;zdzxxeDnv_kG3`eW~OtW zSz1D(gf+)32G8o>okaj(a>?@LTK{(r=dft;V)kXrBKS4qf0(*DJ7<3X{(Z@^WdWT_ z8Q?L#fq{W&_1ZovQab>`;=PY4rls7>^|D?mQWL$UO7bie zgOK)CfgGacIRegkj4&aorq=*yX}-@K7ywUkAL65Ivk1O+Jn_+X!{ z+Za|b4>KXV|Fc`=)UVG!Mjl?Dk&)5-DbZ-f)+3%EY!N^Vreni>q#wlTq}q%weFLV_ z+n!!0_x_Vp7k(T(rPyO)#E@C?}E4U7PN;XmQZyX&P z;~CrHTKMkH4wEG-RzxQm=f5v1N;uduO}QKU&lJzIySKLtd#@bd-)iyfcq}#WbgV(&z%abLv zr53LzN|M90nSZ?%8owb@>ywi)Mh`H zpen9M`1o5Ng*=CjFHyJnHCyn+-O=9{ulecMaW6wkI)NqQ^{OW?<+ zN?{bW=H_O4d8<~h<}r@F{3Xfw+D@x4k!S|*p>dU#swO<`z8dV-Uo|z`ka4x*Ur^Ay zJW6$Ab#Mv2KB^@kyS$q>D+-&H`GDwfH{<0MZFkT27aULz4GIb(CU7W3N?+D%RWT^Y z4{*1;!saK$bAar`96Roz7(Tkn&aYF=V(ex#6u&CpoRA6*n8oq%3}RVY;2W1ZdS z@9OHB;y&h#8W#x&Dt79tCt&qDF0K%T?7PW<>o;sTaBbO^3e@y6Y_mA+zQBiFslZn=$j9v(pu&u2v_Q3#)M?NY0VsEdVpka<) z$%KYp1ms4?&f1omtn?QzUpD;wxj|M|mWY&~PRd8H;GQvqDVs7ZEG$Se${lOBt~iw$ zS%nf0p|HEV`~9|iX`A#jQ(7t$ly=R(|7z`Ie>hb)H>)oXk0}A`^Zm=0`{>bN38r8p ztQNC7NRNHp`t`yn6MBu0T?O6$XI*Sk3P^KrsCoTbX4|%H*kd=}9cOW854Xf@zoVd& zVPl+TSUxd6(rN$m0|!2`ER)X#&G|E4wwOa(B#5gyHtpOwb4Wr!@p%OChM$kGC8u^H zi7l%Ir5F-I7M2-}Ga0*&MrtJ<`^V75q}3aAjNK`?NNPlZCFC}2C)xdWeMO_CrKPa# zPlXvXXHv`La{cv>@cu&Pf87dwBb?33A|Gtl&)l?YmzJUX&?5uq=0^&7owcjBNlNO^ zTPola7Z*oj4Y;<+U31i=;XLW_2Qt@|g75Gv(WaNUjs_^9R#IAeWAhYTY_P9ysaw(E zC>5S^@QG&$>9HrQs?#j}jUbXl>!!z}sK=s^zb!4@QJJBWYIckFUfPGXysCjdE?oly znrN{80@^Iz`#E;aEZVqWicyBS?yRtVmliamfJJL27^E3y4Rm$g^eY~LYWOwbdc#F3 zuX7LG;!_vGy$4C0DOpkRQxn?JX2To-adB~~`_9Sy1@C;;Ua}uFH*BKMr_ih6wy}j(hk>NNa#~uNb6>e;Uxlup3=VCU&Q9 zZ*IZDN7pqrsvO8#Vr|_CLKfS$GqB)-#w1U?_?svG!+v6`pzg5a4C+MSINN`mDXX)ABBmW?!ergzn+PhY2B4D}oAFCl(JRSTPZ=mH1H0 z9NLbP&&Tf|@{HX%HC6A8O2U{ImMZE>jP;hX!k|x@i2D2a(G!QzUXwgwcQghp)w!?b z5jHoUgoGX{Gt2KxUkbOd#V2J42M5wTtZJWX#|lGBl{olEV$j}m)-<`wwJ5K~$j$9v z5vEv$TY|bf^F#%T8v5?}=ad0XXHz^0o+VEU4MY8j7d@Ooi~1h zx*7mVntS)2T`HjEW9)@;{<(qhoUhacRPTKKrO@X>DwnrJD9S_sM zz(9Md8FR%>i`!6kOlo#*69=xDGy1KfLA|h7mAIOo6w4cUMF4-k2`mVNiJm*~VyPf9G)C6=;x#YHKOIW!RcP2YU}ziM1^PM+OSkj^BV$W^-1YHK)huY3kj^@BI!J z1xuDcKKjxk56JuLx8Lr|v%j${N+3OvOT=KzOcKt-KPJDP87}@qBEbm%2d>QX4Oi%U zuliM4_*Yr%1S)?f`Ad8Lv)R8Z2>rL^XD7Eh{p*q0fA}1h&)R>H3#e1SMeP5_H;e!F z(|-Nh5m;~_0E>T?RLqp+|NKTc^xgk!{?I25eflBKOM(A8E|%fB=noeAU%%4-u)U6& z*iG^zPAgcA|i@Y!v(t+L-T+8#js{(UsH7^;~Du{euaBlwVwY7$rv}a}~q$Ib?v1On6Qx zm+5NJYP@K1a_A=bfSH=4Lo7dsg>&_?ajKn#g#cFK?*FhbVX53HI8pWa^LucNrV@Gz zFY(lNFMJR*i?K!=TMaxR{bH%dh1@MCZD%mpW=<}hLyK?+VN^AyGVV`@>I@+y-S778 z$g(oE7xW>ckFdFRFI;U6V$dm<^cYX5= zwv4baZZs?==){2l6$=Q$c5nfBcz8JU<*wLweU|}fW}>xh_8Srql=@v2KAu~_P<}y= z1G>kJl_Drf$=g>4?lAGHh|>*d7KOQs_MuT8qp68TDtzP_LmiEHmwcL-+i(iNq@KP$ z{lzhHYgB%_wolvUFWL|x)GZ??Zxm=XHAx7i2HC<&RD;0>!H6YvWYO% zmKn2VwZaLkXbie{Pc6YfGS#&70K|`qx{2`tg#?uOz*?vJ`AiijrziH)KH-T)Ll_#0 z4+V|_)=WqWnlQL`@7|K7OZ^jiUgzbt!Wk=vSr!XuCj&u16~x=Qzby(Lp?F@U`$xs} ze`Nvai9pXGU{aQ1sw!;tWtU+ineaYTCwE`j=|1X&Mq)nP+pO&sB;}?96QiIj6=p-R zAFpbPSvjA5$Cr%RF&~0q&_M%8fvhELUHkM?;Yv_RxJ>$3f})(sF!A0x+smi91tilyKVVDa zu8C|Sn1+p+r|U05r}VQFzj5HkikhCj&g_BU&uNx1gaaW842cb#JBbScv)i3mtB0oX zyHDJIHJ6P|Q%mbMl)rND?NxbaoSaT#RaO{=NP8H3`}S=ZB9L8Qi#j<5#l^+ZQ2bB( zjqN_=;7|cl3TfrG(Qdb%pNHIr8Ug?UQ?SJB@e7Z|Z?~R1+~a}2MXTata>U#RuH{ow zQetv^v&VATpm6)X+ep7DY;p20V-j-i8m%TF0t=jnHl~}>h8i${>~T$~QJTpnMHM|o z(pz+^ug6>C$z8pEy$sj|9R6108Ll%wZc1EABbWT_jCtJ5E!n*)UiD|sowJ?)y}iA? zM=!*$YsVoarGU`TQ1t#V{2>3XUAyvI2JS%sIsme(j$46=*!RV@sG)U~?QWt`o*Aqf zsx<~~edQwyq6<#!h*O}2D>%7uQ$f6$tdJ1H_YnrRm6Z|*pNB6Z>w!)k^d}q6H(sk`z_iU@WoVaC%u7^=#d3N64 z{IFJLDoEvOYi&Wkt>&N5n=c7ArnEpC%+!BMe@)kz}gRkz@vV04{^Fvq^51!;{~A@Q36pVBWM{i=5LQ3 z-5HpQdm>RBVwo?51;E&d+H9BBt~{LdgbD#%!-ea#Ue z@1H+^E;fz={yEb==HbJKbZ_7gT?Y+7W@&GrXeC-#_Syff+9TSJ1qE1msN3W+A1Y(vH|7G)n4Zh7exiZDM z9v*0&ig@aP5VryYb=RtECb4TOyCKtp_WQ25c$@0V`{*IGwW0c-`KE*oih|(i%y%}Qz)*zw-TzKDTGrhaw)DuHgIvpODi8Y76h7%T^dDKiqSMU z!z0i29n^+O^W3>}RnYX3?|D=u8PgI4a!AVdN69rs$+;6e4?YFhHi~KACnN8g`kJ>@ zU|{i(>sxT~+Ftm4UAzfZ6&026Mw5cOJX^MGfjYQNe%C#S*>IKRSq^QV9>B+K%IeHX z=Cm#eyMm{C%XC^6uWI=5yP-&gC~n0%7}6r zrkuP9CkfDINa1Pt%0&~( zWT8XH{rhXcoe3*xf@(E1HAU9Bj})vpE4zPxBq|lHIFe$4HwZRiKb3-a^+GO>g2wqd z!!`mDg4munC@qLJkXw#V5kB>GHwunEB48yzQsk+vmU7)CFA0bc${hufbB6O4!O7r@ zc~LFC!yJMKoRd(>hoKZ)y?V7|q%$`G`+EpCIWayaI@6R``tx(M`bg=8x{QJ&<}v;h zbUB_3KBz5nN=lWut{~IYXlSls*Ka+gKoIIr?}r^l6bvA_i@O=2FOo^&@#`LU({<@> zT5DorLNWl9je;d_bTyPlhK8WCu)iC+w$DM@L;eVGD=&CHuToEMTa_wQIAy#plBH2; z8#68f_UVXBdm0ibc;R^x$5+6JUU*<;An_M+RK&y=Z&3KiCOx_a0J-&Q?<~Yj3YKti zME&~pD_3u~<b^@#lLa=x^>)4;DCZY$@~Hc?|twCwOUhl!928A?`ciQjc1b@eCFI%WfG!}#Z6c? zNX%XiE2=b)%i~`AD5cRL2_)%X8E8u`GDDn;wcWWLU{NCfHV{2CF)@+PFy|@C!p-)J zry#A5OKJ`-RU=U+!B844Q<-%ATIW_lCP&~qx31UAu#U8)BW;F^iWWb^o}MGZfNDP z9v5rf2K4PaYbBKu0>vGz!i$o!QZ1@fphxrit>}4aGp*2OaeG4Su5&H8F}-D54AhjB zmE)(_D=Yg1pZUpwhM0b^1yy?YH^f>zXFbDB3z4?Q(Jll(0;>hE$Go?xSo_VBtSs~IudY-DPvAGC#M>A=KC;K3@en0~JR$5({;~dQ zYb1z5V*fb;i9hW6&N-qP;@m=k1=MZAEME3zK}glgljkt(TENCu346;gIC!&*%ZLL1 z>SOycp*F+Cxy$lh>YJKw*K1Z> zbMEV=8VXDYeOqT*%g2w(Fv}>P!uCq8a&&xj0&Y3;y=I^nDk}CAub`k{wZk?RZ2)9q zblo-IO3a&rlGt>oK zt1F!a3KR-`zHum(52C+d~Kntt- z7kdK?1Ln}3zMN*3Tot>`=uIU0oOAsRkrcS8mY*K)L3{KCS|foEIG_api)tv!y)lBw z$OX((CQm~;Y0CB#xYxhu$!nwEU zX8Vn#l_FNeNeGccUQkCysT?(F%hO*k+&p}d(D`ZrwdwwOaYP~M9^ms`vH)K!?<2GQCy2kw@R}GO;MEv0li&;pV^5}kst&<3U7isA<)zn@QT9>Jxt|NXa*sc~k5Goe_c%O$8A zv?9RP)Q}h^kZ-Jj=hLaJYZVNzen(*g$gv{DH8vc&YgZTeq(Kuqz5M>3X$1cWg@Yf) zwo4)as}`HOM9tk}+`mqDXwHPCg~f4m^BAbMt*xE?aOALv-QRbQq9Z>$J4O{Uow^>vaJD?Sn&+^?p1z>&Qs?@PM1TK-r!+(omJhp`Si}Q2tm~ld3Mi&r-u_kOI_8D1OttiYoIj$K}6Qi~$XzQz2Rx>yiQO{Y-dqs_|>Din>{=>bgz?=s{MR*Lqy;P z|NgC%^tcbxgcoKS;X7=_7?7$cRLr5AoQFps0Yc7;0ghpXA{n^`(c-@@WSBSmWg*h4aHBlT6>HH zhFuSQjGxs&2~Rc3Tc8fc$+AQCle!f-_{e_8wwuv936gZtAP>1OP>9tFNlO#g{*mH} zFlukTmG&mVom71uHTXOzS`Xm3L}Q&JfN39|Q@YfD=fVM<`8u!`2_faB%>-k~s948@ zZ2zurm4Cf|mwzT0-DG>1F@sl3Ob5xA(k0pO zoXP=KK($~I7$M|v;2%Y_2=cE&Ud;7 zzt|kn9zvBcxx)Y`%?OvPMX^ZR0IxGZK)a#~?J)gf-hi0No*xa)r1FG?g(=iulM9h@ zgO(bO)oq4WK+K&|Ijxlmt*Mm;rQ}+LHR6iZAwt-V4cnVPyBkulP%w9vV0MtL5r@$a zIf?uNAACF1MLwm3IrHbIj(3jst|n&IiyM)9-{i*0!Ewi1?nZH@y}9|OG0=&Ikv?cH z8RK;mBOa_U6x`N~#$f)k;p+~KjR)L^Z`Vax*X1CL;Oo#?8@G6);wGf~u%CHaPD@h+ z0#6n1SYe9A(Z?yANxg^C2ftt20?{%#(DiB%n)oMlVOg~>F@g~iUX@v`QX8W@hfN0+F*qNJxIhI5u81#O~&l8 zZOIM1efy-ov9n2TU6wjhV3N+gA0ZDCF%=aPBPNez#5EyIX%- z+{M1pK{Df=Qm#I)k&Gh>v@miI6yE@Y2=EiW%sbbAw-`ATT8+0JslU(G*cK5UuH)gY zm6K_8-XPmqd*59se?Nlj7cW{A`wxvE#9)z17&fj%BnR)CkLlb~Akw29w@o1M?oW}( z>xenR3g9-jC=_!a9o(BxaMHgh4DEVpi^vqtndm<)5D&qPQ<0h6TXnL>ipVw?7){k^ zSvwJz2assNYrRsPl7z;fZIO+w(y{Iq$sD27i=A zoXH2E5Rx-Dx;XKz>H^uZv9WRpL#rj7)lhpWL+9$|hUpj9x(w!D{}Sv=yNLVN zSu^n@E%xgNtI~`S%0e(pumh~yW!=(w)OEO9 z!;1mhvwf!)}K8I94pHm7-dx+2_uyWhDE(A)?>+5ecow| zUkpuNKEA#VzDKL(*G{W%H#Pm$!$>KI<~UDC_UPNFSz|%?4RJBsdXh~W@o3{#W+FrN zY1kxk7b+9Ku&{9H+>b9#3@L0k^L@i=Dm>zhCn2*)p{^Mc2MI_3QR|tMMfq@|N3z;Q zOJho1>%gUk6V_INc0bjvMEH7a7MH4+Q$`+fItwo7)+{{hxCi}ItAJ|wkkX@Ni;5DD zJz|axMnRdI4XeI7(8b5&C5r>DskM-aB(-#a)<^n7{IUjJDu~x$g7PlLU zT*Vq;5p?xZOxN%*iP2+&C9ifwk(vCjhuC{UAyie|h(F-X85G5p;FtJ$5bACKH=6mD z(wXY={sP6o8p$;aM?sh>mQHn?wZi{Y+NP;3XovX-uAwQxobUuh^KW+Az1x!}Eila} zuqj`z9pQ@OD>q!aaKRMzScS$q$ILT5;r8l7HJt@$N=Q*`L8c9~I{H~(O%$jxMF^zE z2aWnV>!b}6+JRpnCh_nZmA9d^BwxP*!f|=wdTu79NDZ{Fj;x;}AU=P!rn6CG@xr`i zC+5V(8|KDi&uHt~4UUZHitPE20CwMc$0YbBH-c-~UoX@}fUHHL?@;YaK6N zztmvcTo{O%zo?A(Rq3u9XpT>*v*6{2MG9QIPJIo2o>FCz)3`n&(NVe%WCRY3A}p_) zRTdTCL24LNz@XW>QuB`BJS4wV4p_H+Y1sv}8;~gSOjOny@23Vaj~Xr^UIO^jvx*CB zBq^r@I+zBiA%Z%DOGqmvF${`ZtcSFTW+~W6oMY{oyi8_2QWdxE-bLE^Kr>`DnSKMf zH=#cy8TX$QEBu8e10I0Ytqw`H0_(%}uRxyWOI+@c<86Sn(rCJkm;oXWl|Z8W*4EY- zDm#qbiDcs?V){RR93LC)DVh;&`R&C8>XC}~c0B--iUb`2rw*q-Lcl!$4$=MV*GI^i z$z@1nkE=rQRtB_-eO`>6Op|fMdq6F)zg@#CshrS6+_h_}$B&1l*2o;)1Pp^2LdvxB zBi0Rur4D^fkkAWZe$N0_GR%EsW2sO|g!9|To!{}s2& zaft+Bp0k_u%COCwrwU?8YSKx>Or0Bp94W1c>={K}Xj;hr+z0IS@)c^!$^eVXP>M1X*+;*(wgxuNZ$wEtX?e}4IUrCIg}uPx zk%L1!TZaRJWytV04C%pHD0ZxK3uqYf^kl%x(ejW)2LSD8f-lf{}}WWUAe8uPH{hhJyK7*%-FMVk75B$xGxrsOyBZ%fRZ@ zGF3UJM?bUDrq33BAU9{V%>J93_0?MVWt78apPl<@)4J7RF{2Nc+~ix69aE+pHY4mx zcganIb&?l+GIOPOiEojg+HB$3*?4c2-OoEF9e23rWpuYm(zI6CkG?>qEm)2I$}B`fneDi&f!Wr)A_qRh3@&>=`l=^#dCN#5ntNjZ7dj9@B?aW7^u z%%7i~LBO+gs5;d=f)+7mS5Jw>Q)U$`9%IITZ@G=lO@_A+GLTMeM{I0uONopiPjnK) z9;PdlhSAqisEyy@lr7_j;LOSj#G}bQPsL+h5O4Yjeoz z@_a_`0xO+XjQd!hW`xuiBvKIAzB}GIF}~&bEF)tf((+N;%G0e43r z^6T6hmoDTwcnWy5=)$dWC1@)|sSe%Vx~D$RyArAs(N&{d^krBELPwr?E=jnTLz)K} zqRrUtB_xkw4XPF0(SvxIBe3)uZ9HrO?m;othx_MH2$?)A8?L?-3lZNHUXDw`G)LxF z83}9sD+?eiJqS+0;r1b9L;Qm9PKZR}h|J~94zEV#ZQ6!Gns~}RP&`)}tnR7f>{qW| zMPg){UI|jf{$Y;W9*G1NgjX849LgQ-zJ{3gs)U3Q7#tMSK$kECn`A{qL_E#TW(U3o zLKYu;Xo6Sv=9~^z%lc+3abL({Gy(#ln#{}6Q7=QSnu)ElP^nXnC-sbDI$>L2!8`Y@ z8V7ueTdc|=Z_@LITCpMXJ8#@-gt}2sDJPM1q97V^1LNka&7bzMKfEP$`OPA6_0GL>H`W^(%{bSgGqo*Iz+U8dpujE~t_mY@G`H zNC@!hJ$Vd{PS=B#B~WEMedf$)KwFx4M5|m6emVqy0p47VL7fV6@_9`h);j~sVc9^l z-&)0X!(OiknD)AZ8e0i6+YnaCN+3BpQPMES6@=NVQYv!qg}G#pVyq|%VUpLMI3Xxu zemooWqRV|DiI-%Bg3M8IMn*SYKs%`k&z(waVX$Njo}M|tT-5jqo0KJU7A$c1oMYdr zF)8Z+VN+r`!VRdb6csQ6-dhvRk=qSCzelYGu9no`*J(6N4tbt38XFYvZ9*|8709Xa z1?^g(h-j?ZmfGwr6yC`~4z|BS(m4fc_d{gLThIelVIv}jYiJ5tn^b<1*x5HJ?*=5N z?H#Ug=OJBZXKnU-m`>3hvpn-&(%1*gXI#f5sX&IOv`+I`_V&+3U!7bN5ibkg4fv~u zz*DKEJ96;H2Ml35W);ith?)zAPACQfas^c4`nXl{{NU|&&3wi%BC#S0jT@EB5id{y z)YI2Lkc4S`X?5$PMK{-90!RJcvc2R3N&|`4xV1-Tb_v$FZG440r9jq_3#HznWz$Q& zUy#fMRJSohGm|?swG`pS6ssDsF~H+DIUo-)h>$?q_ITd}K2eX(RaI*EYoz+?8_6 zlp5<*HR8PX`XP`5g{IG!qLAq0$?TT!ZnfUCL#HNQZ_gNa1~4RMLW~o&>_DO;x-I({ zi$u`iDEbATT{@auz`JyA3y6CYTwO)`twHzL%e-{}r3w0> zFz{-|>%GTCJ#|p!C;+&$L%LpDU@6|{vy&XRd^{$`G#iUTG*LzCvv+)lfRR*Xq1d^U z(sX!;ls&-imx1n_f66d`QNqO^d2?usDO2!h|(vz;>7 z%Qw;sjwL=BT`dEtt9d`9uD8GDF54XY;=;VF%)i3US?7*vAd{m|u?^;fbAOddY1G5K zeL$-ixftg5M&`X3l1*vi!;OJJwgw1gXa;nBa?z`M!hw8EZ28d*zPv97zRdERbwRB~ngsX^cs zJ-s=MHS4>2d$I2B0Oaw;l*<(z!*@xP<9^lzDg3bnP!@o9mMmIyY*Yk~vmS5|MF&{Z z0yWDY?FAxK@KyFQhb(=EteSFTe8QS(>tKf2u|#c8Ku|*V>Mayctm90~tOLa9>#txu zn}LuGW|KD{scST&uXNGm=_+h{@m!i;9ZP%*vxRj$Ri^>x%b>+ULC$)zo#Dx^6{mrZ zTh8URlxuQ|+37B2B*hZ71Z*4~1CXT!J%@8m#T&^M&48JVQ&WSJ~Lw}Vb`Gr!Ii zdit*?|KvX=k$L`|A>RD&Lv~KhflUAV%TxZ}QChthEiHJBc-B<_H27akK7+x1w$h9d z|7R=+|LyoO&tpS=bY zyN}7+6>Dq9xY+2t0kDHZ8Zqiiue~1JdPl+gu`~VbLD9*}U6zHo3#jUywt9M)ix!dk zNUssI086j&tmeaH1H-d-@-~0nrehE=u4S<^5H|uuju)#vIdA+m@y~bIOsCbc^qLG# zJCpj(s-@)?h zkI>5qB+zjTlznlQ%BtC%RTDh(79QaewA|0sJ#*BOp@;e;@`ACw}uB(+^hZ<{&~zI&p-hwwn}MwBf?< zVHZT9S8zME=~`#=1Qzr^%Rrzb@MoWt0yGx2q~pv(_jYyN3|@wp`DZxyxb5_E?5=V+ zGP9#Y)&TXu3s%DfbkGY^8c-hC=RF3snDfl+u{C5Oq2R0Ib2J0++&O$Kr5Pj)LU21R zKHXoiT5J=BiD39s00&L*MiTaEHYbVD{R0Q#Q04Qk@BZ1n_&9w{)4ZsJ^LilJp0^~!6Dt@Ot zakJ7K{4$Ce!azo2E=5!uQ=2C!00A_ThG0xv+1X=If$5wQBC$${NrP@j5(a~BN63_H z?QInhnDJ(G9v}IKa5Q*0DfjR13dc~k2jUK8S{3{DA2y7qmg@w|45qc&$NT@_2$xN0!gwzyp(oMX zFNZ&j$}La+j#Vtn3-NMDoGIp&K#_J#pepJKYHEIolEOzdpib$eSw!I*%mDf6rn-N; z3x$z3d4;I&PmK;-UrvhHNn9B2CIIX<9D#^dXmsi@2d~#wL)Y_Om|_Ufj9piP+f>ox zM}v^2s{C~!8SQkw(UT{;(D)*xxt2{1sAxlz3jhjweQcW)<5}XvOlY|fAr3#RH^=ZN zWKdJ&-e`U*(GAsz6^12j4xND1%|vTx51F=&UdRy$G6`F7Q=W*-d3e62v0>T3*_dAl zyL@>ziF*i;du=PitT$ASO?@3Ty0$+*Y)jLP)1cFdq{n)F-6zHz!D0`x=sCk#*hgy= z3K>YS`G;h1G@3y$%JaHaZg#82suDUR4F^#0iTtoqdk>_F=^!g)n)#V@VKkvY@e7EE z>QE+p9bWpb!D^ycjP*gMbFpvM`?Sjh{G2um1w6=Srf`^)+i>K;o9k#u0z~)Nhf>71 z*tZyfOE=hj4pyPOWtvqDg*{GU1-xk#;N~X&P>gf2u%-lwAhJ@BiiDW-7E>cojMSil za@Xiz7=ukn%X7t!6W6Kn08clQn~H@^fd(AC#qjxuM#g^4Mzo_DjLpHG!}L1_)>zzx zFJ8Vp=lOXCW$@5XGBW>?_$LWQON%I#F+3a|v3Q`;Kl;Crzp`>w(7N;ScEZ{eLk>D zkO}vp?)KTT!hnqKQ$7vw4cTl6XhAR=EkZqQ_p%rt`3HSeZvy`<7J$1=!O_$SDe{ z;%*<(pfOZ`hy=;kJ)IE|Vue9x;P{qeH?S;%xI0WcAyTwg?efWmvEg)`aOJPrq8f7O z3j=Izad(Qx8l@+SF(d)mX9ElnBzUyrDxAi&ZjM8;4}(6OUs+P)+?X~6b!}H`=gPs!{IArIFazR;Ic-IP$!2`G{QAtDFd>XNlFwxr)OG0mA*@-+& ztFnmJ%SbAfz@M)!Q}`WN!W@HBW2%+_=A8qGH-hGg8?d#V=N!Gwueq7IU59xOULDLR zP4M&YH6l(UPD6G=4BZOLVuoU>h7y(&%?-?-G;6&9^p*ujNCI2w*>3h8Lktw2uR z3y5QRk9TECmAghMfc0ZF3}#>rJgM_>WBBr%RNypF()tVq%l=o*%jK8R&7 z88}8@9}lS#5|tgCUOG4oTb>+63WdP<;zbGo5^V&9lT!Lm9o*Dp*BijO{rF`>t=X+7 zz^%D!+RwOc0n3Ll@@8e{>}RX916KEJ6zV^^$ocQ@W3~|&@X#qA&VpJ}KRr(m6|tno zoN2jxHO}sX&OfUI={LX0F))@Nw6^JasHwC@iCiO|0@cPI_7;}ItXk6ATA`Q>!(f~P z3j42>B&nDk&h_f_e7Ju&HNXcH4?m@^$Ij5ql)jx4Qe)+DvPlQ&0JLSS!_Pb6mH-_`YO7t&fR?PjmwU||Fuvbs z0V=RMl0&oQ)YQuMB}{vEt}A=$Gj^h4PUnNqdw%XZClCILeg+h>VXE0z-dkE*gVdl; zmD(0bnxs7ZgaHZ4aW)i%7?#vHEC)!!i}mmNwLfp#qQ$;Am4vB+yb`PT8a;Nq7oje)YrD283YgU1FOmX@N4$#i9BadOkIDGW zb(i+Y0hGO{c;NIfuHWFca18_vFasEg?4r+Lv$_AGa=x2q5(_{R$2%2I-{G5&<1d^RJ z5{iPB!b2n-6~fOUyxJgAkAcs|2689p8xbLlg}uQerbMy~4#I=-M)_$zt;hFi_G-V2 zqV!vwYnp1?s7UEOB;ZuA#o4j_L6>m)lrhb#;dhu^M>a9^XHf{9ERt`rGX{8*%okiN zC1atri`SUTUb{_GgEVtrWuXCVtg1GNtA2B(MZKHOn$AKs0wkg#M|I34G&D9grsU!Z zKO?&Wb%4^Q#|tFMP63w23s3<@ywQ%oRY9(&7xHLCbU%C)I*@@<8#EheG0n52d;mlSa4Vd9b=1lXo=Ph0IH!Sv&wkn{64}6 zr0EH>woi#~9MCx^3J1yFzD2klA{Cwn4q0*s^t{uR{}%*U5Is}4js!rr<)E@t=7ew( zHFbCf&9!hlesjk-%3EoWBE;b(!+J^e}Ht#?>z|LXPYW;oGwJPFX0-xk9hA+}ol-&mr=s^JRV0|Nn7_YyPtkO-C~*9BQ! zFsap@%cd|-leIT%7P@++)4}6rG$ewkx?j1OI+W-|`o)~Ee*iadZl@xYO%cwEcYNqm z!3y!mfQSf_yJhnvq5uXT`U~ok5l+5DSLeLE+-;sNT|0J_%>8{5+^#uu=i=x}xl+*` zFEW?onXZ$$y310&HFH(&R=I>B)qKcykm-fSrFr^D^wmbO@9LU^!-*nnetx*~BNGQv z9+wc>me@6L@fSyX&y?druHKAOe-36=ED^hRpqp~xVq#+2d0KH5cGj;!ihU}6Yin!H zX{Va0Cynq7U}}@b2*uQ)7x~aZ(^Q+zMd2(1fL6a)Bg`NMDVF0e!?=FyZ*Pwf=YZat z2}OtDX%~>l$%}%f!UGWr!aYeBdiu!0;p@sVI%P@86OERRu&8qz2pUJ+9qvxXXE?2z z#A?&u;(^eKCl#-6;c8^t-`-qLP|VxAXuWtN2^Tv9L6)VAs^L+Qb<4fF4oM({#Ducn zzjQ-N-TwQlIRx2p250={S&Vysq{UyiY54#>3OTfCVZHvm8I%s(%m-M9BfYt-4G~kK zDHf^TLC_e{Y`J|?X@D0B!AVddKD9_6i0_Pze*{K}aB#|zx4KPVa@=%ra1V(si@t=Q zDbDAT8ogO@{u1TapksN?g5FEdZp|G`|HPp-x4+KqPkLW1+hhd zKsgrfhE5BP1h1p>)FuzJqeu>33{IHk0qg}3R3NqKu_BYKCuDWQ=**`R$x+BLw0y}( z?z(;rUeTWur_15*lp}zGt38M#efC6mn4mTPEDVYP{-uc=h>a8!rNg=K$sbJHP_XH+ zA-owico3!NUwtp;ZD~TlL806JODpO$F?Gs_kk|l_f#;Hm`4ev6cRz&sQ9$TaQ#}4K z91~((6ij1(8u%df$48#4)l6f~U9g~n&ar}M#E0w?0%mGx|Mb8hn@50lBvK%N9u&6o zJF`?irC<7^{oCl zm<5GV!j(cu7r~>8!x3sQ=1Xvk^*7uaZfW)+OZFC>9;H_qFbA0R5k!_2P}MhDCydBA62gUHj&8AL0y>ED2I%kbiO2;xpotV248XmJn5 z_&h|03dnHeK~X;e$0&h@g@Y+q!&d)dI*R@y@*Ckj>=WucY8wWlL_ zfPq>j17md3A$V5__Jy$1uOkhGLAn^CgEe_h2&tLAwyM&mJ(lU!eQ*^CyAC7L6pGBv zodULm0Ah3hPMkU$M zXrL{403wCWgHlQz1FkehkE1SV8`5-K;us1lrI~1C7njHH*ztvRyi5Xu9QmNQgPnzU z%32@m#L=YVMK-Q@)NeB^-UHS@aqu*4CQ^vQ6@#LoTFGUubd{BnG41;Pnh5UUMv9f- zh(fS@gwSP3HrbSgfslMYbFIv};~O%CprCVOaF5lDFQ>|ol7>M&oZu|P*~+KBdp zO-l3UWcSj#f#^)<+S?&$Kyi>Kh3BPY|I^d(*^gay7=8 z`tS6xDXNR81%e*0K1m@=$fQ#Vt}AP@D4_s3B@%=KM~W(_SRGDU1vLuiRosHLtco8e zyB?Q;GbSy;dp>;Xm~BK+TA2Kx{inGb#n6p6WYt)u@SgsK6AIy{b2BkFB9ul)G{9vt zaPbQ;aAwlk7l1{ou3S{@(NJU1D-?15C;0avx&^9!09qWW2TSH{d3xjv8GkX z`F!5S@sM8I#Jc9#3qL4jf4^uW&yOg$KJD3{q1ZZ4?Wm|wC}S`~REmppZV4FRX|`Vn z*Cnj(=7Gh~9@B*@BEbui_^PK?$P_(tSCOPE@tK$!lM1L3PQG`uhGn8#+pHSD+`H6h zr;P)^hsP?ysNRE5&x<)8h|VuelGj3ykfU$Mme0>;7bgms!lBuI>9}`dn(L9RTGeUi z>drKdcNxS{*rn|4xl8H^24s+I76G#9+c>S|=cj0F?QI#6v9YhdvSX}PS?t`i)@8I7 ziU@L&o(nUAB-u_J&^4IC&TPUqsPj@)yv3agP%EN^Poo^3bQnbj`K? z96r9yC*xS5UD3<+=$k3*KnFopaW@d0@`mv!-S?9U8EZc<2(b4Jjr8 zO*#)9!U88O?B z8jP@kYHUsb#0R{z$O3T46kmYQnoT#9?YvzZZ}uJ*H~eN;7~uS3zN+AaJUYRa(8(z2 ztO6y7k>`L%AG%x%wViH}cW3`p(0|kP$N!}IxVq+7dU|?lyF__E_rBl4rm3%B^vX5S z;!w!j?R}8zNxTu@l9()19icjH4I18+qEz)RPJhKI|EaeU|6aw-&BT~NSYn)^X@n+` zW&2Rt2)lAHe)Px6e^JbzFDac?HwtmlTMNGr8-)dLLY|(D=+Uj)^&0_3ooMWY0iTIA z!ZeQIUn1(f)O~4vu-N*+xBfe4wpYU&SLanGCaNA@mvasYfLtm2bHL25!=L5!db04m z00EGbsJj1X^Zj_wT z#GpDA<5L3F)M>M~a2zOABFy2a?(y{^d-u&ojS*=g>Lq0o=S#s2NGv7q3MVw&RzK83 z|56x?#X&@`UeO!eW{8@L5s{UtU3u|2Tu;L{&SggAJ;s=pTG zZh9z(t12XFrj&HLPu-A;tJR(NDc-DBEgg1H? z4o?;slx08}!ao8_ayJ7mtRB(rr_5_f!|V@#d6~PSX8EaQM`-G?M~_qpCls7N0bk~a zk0s$I)J&Pj!jB-y)+6taun7C}tnt~>gO#6$J)QMz_|@Dxp0o^Cw3mDZoSXjqz!w8{ z+OYnM+$M1}Nm(yO;8-jc1_JW@@@Rix=~trUEfoI3!~~i?Qi0yf35Z4&Dv!3XBX?s$ zX(2cL=3#MMR`;&?3xbMpi$y{}OD1>64*1S=mbAdx2aR*E;#us|lH-*A@03a^qfDSD z;mGj-_B~%_MITR8Y~(6EUf7ZbY_-~U%+hR3@cD*5&FZ|ICM<+5o33BKF6`7Mt!5;Z z|IE6-J(DCT@$2iuVnran8qzI*J!J(z61%#@Mjp|4QyUuK0y#)64jdrvOwtd4M`S@e zsa1y#k?8nCi~jYJ8e2H8}`POF5^_P4J1iY zWgInWC>=~bK|U#FWxknw8~&F?v5)~UB{bH;Kyt=Rw$ET%R4ubgu~Q^4>EaVh_4A0^ zNs8B3GI}v{<)|q`QXVOL>rlU8Sr4cu#G~*V4OLMpO2I^=O5@m-*kX6tW7>V=&w*Me zJwCi*B+dh(uv8HO)riLs({Qlcs zU)d-P!nVcj-)3O5ZcjZR{uFzkCI33L?*wz>MX4RVX4JdZf=c(w9;k%Uy&(&qC{w1& z;IKPWPQZ}re{(1K8Jg}x1LkcO@7{UdGj(${967lJI2cnu+Ok&+3=G81l762Ut|MkN zoaexap-)<`@02zw{rUY;l3FlF#c5!rjlZZrt#CoL0Hy{Kd3xy(uf>-J2x>gv2)uQpP?mFpJkE!!4(JN*Y*`%a!}!A{(9e?1QN zymWeOa}CppJEKd4&Ow<3B4WA<&aNm)0UD2qKbc3G-57xRGExAUZ6#ENvyF!J`RC@% zB9=4a2QtIp2OGq=1u|n6TVO`Ws5&PaHho&M`bTxQ4S=!sr1zJ_ znTzL6g;;ZKz-@Pg2GXB_?Ix7r6@7H~WEAgy7dLzuf)%WowqLdCJfwmr_5(R*#fb zwq9OdJzPGFSxNtg$Tax2q{0EAkGj$3_?}&F)aJ3WH=!8{dEpFAxQSjfOqv@<(34RK zxW{QRt259=RNoE?TZn_eaVu(Oj|*N1n@h4akgDK3OCzNi@8teFUODaRIv-&QPEI{? z$bRqCKVct+etGO|H3OAz&!&+8gi+@pM(1|*SCTRJI}!9y@jgZZd+6bwVV!5K^fn*) z2Ma|2$Of&d?vqc60YcsfcId?OeqLl-Wg%Bi>|bO?_Ar@#vgt-=zwMB}woS-Uc(kX! zvPT(a`e*=JfRz5xd)kFHQ%r3mS!4Pvh@KgkKP3VnfCBCo%G|k|J*`{C6G7Z}? z-!2*)sHmt|?LJJ9nwJ*pSDl^k+oaqNCjF%;qxc{l_1)utKECJI4U6RuJo?Mmvyf31 zf?Xpc=au#T9xQ*SB|`}OT6Q}ye$3Cu*Z;023i8LbL_xp#wSTV0Q@^V7i*6ehYyCb) z&7reU|L9nHaxUDB_Hb^VFz&&ug5J<;Y2TL4+4pf|elDVCJIH-xjH6TXu1j>o%$*afh=k z0%n1jOb^An#SRG2pC%=*d>?Z@yM!c6?&t2G?IY`4tSfEZseZuPhUKHB)haIYR6A{E zs>zbqk`Af~bEb*jNrhre1N4_hYp~$@pYjD2rXAHkzngdf^J2DO3Xv1#VijGcslQNH zu*Wb5-zrOrh2`$L@;3^tUvMW?c+~xU^X4KUrPHbX54fznDZD$)Rcv?X=LteHJ4H!* z3FugUskV!stkG-nxDc2{hRneWjzKObK&E)7L%d^D$IGJTbw&x>mfP6;Zx?-q)f_2g zcg0gL>A}_zN(PaA20KT*EXTu+j1;F4_-;gP)?}og&(Z98@{!olG9-wQ)Vz284?wumEmn?3D z;umlZ?eHn{faPWoe;|PP8h>0Ren=d@k3e3h>rW*g!*VlrMpg@e8E%SJVRy)dnFTDV zb}i%~@j3q8!>gedicz)?xqAw#fXF*uy~P!e(7syvvOQTYrp?T(JMJ9x^5x6<_kM$= zCZ-8k#Dg^qin~uj#z8zWy`v-nRB(IkT+EEZ?f!mRV?IzAa{q+$^N>{imMc zKCn#^4^iZTWl+3=KuqUqIvY+PCO?e4?9n_@T9)Fn$gkV4evatWE5f_t?bo!Br)rAo z$m@*;^-cEiX#P%VQJe$XI-vT+ypA=r$vnjp?_TPD8;yuI9_`=jYt%Nzc{fweE8yD& zr^2W74xe(1^iXsyQgyKr*E}^~Y!nioA_3n>DUMV~!6X!al4&s}6egxrmjdmGXo{nj zyk_5k5g2m7TY%}9=9XT$l_MearVOv2RVgv*e_r6W?`IDRf* z9eyJ}jzyBDOc!Gs;XU%@)bh9JX)3V<;HQK-0nZH{N-#KE7+KtH=N&ck-qF7ieh!2! zheik<>8fG-)bAu=PaWIBToMW6e|wn9PV}3x;-^fK9t&%}n6a(gB6`ts{V^(ja;VJN>ROQ(eEbBA~;zz;DXowE7JO;^OO9NY-;A z4|%KRbZ9Xo%{Fq%bL`Gr!gIZ(b&|wdw2GqMp2we5BDw68_%WrIN{hC&G9CCLlVvK9 zxpj#*y&s9s%u#~r z3inYS>hHKgTo;Mf&UC#Ya&Rs(7AsI@`=SJJhFdWus}qe0-n&CNF<^q!l%iQC(Iz@* z7Y5~&I)3ifRm=g!N}0UN35-I|BJ_{YOLe@!YhYjWE%fbmyc3zJ_$qUoWwi}?H&rNk zd|xRK1iX}@PP*p82j)Ptc>d6ns31;L(18BpNAw73s&^w4A1Slacq#&fNX?Y~-133= z6KO5xX$rKmBVGu{;)RM6N*37u!!QZ6LnS_Hc1_m!A}W6HEnC~kMI_y^AA_{t>Ly&6hzWj`S$KoDWHB=kgIvY zTHT+y$GH7WhZSZ+;JU@)+b72a@U~l{6WWCWO!3_%Z-uk+;^d86tU2LO4>b;qCX4p^ zcT<~|En9|)od};K0x8Oo-ta3}6736y9GoPMwRB0l(@sP6bRvO~1aD^SBDawqGZCm= zFR$sZgvD4Nli{$)PMbOU*?PbFF&@ z=3nS9+2Gd;bHR(E`u0(h9#HLyc^d2a4q)K(=SNy)^tHf9h_Ypt&<)GW18PgQ&IvZn zO&g)(o6)Hht_HMW0niDfmVf0^s|t?%2Ko`e_yI^s!{F{6rwY{pO%}T`;*-|%rr}G6 z9OeaY22bVpuM#yyil++Yr8kF1Q{2!D2oWYc`|X3*97x!AKJ1i4T&rELZ8cvDI?IzI zrT??dEeLu&A8VQTl$K|zA`YMtaFKHSW^dvLnYNI46SaT1Y+=d^Wz5c5-kkI*2o z-|jSYxq4!88!1y>{w8Bk5$z#vU$&NB`|w#KZ(UMRmhk`xRJ9T*eG<_^-@n~hdf?`o z=mUBbUZj)kgd`HSkD4>hafXqzHAIXKEsM43?6GXZt6<%A8gz)%w!Q8 zCQyE0xSK!!JkXHSL@h*Xm2vH%uI}G%7GF>>1Hz5(TtuLV9JRi#fDl?IQQJ#sMAWhU z{nH2Uy6di*XhEt}MtDDQ@n*vnG38%;E8ZCy{i#QIeSReDyvdj@UFv8#-tp^wuKuPj z-8MAWI<}`8Aw-efYd%0DnLF!J;%pXxoe#WSU6z=Y;I)gV^R39Ehu`KdWCS#_6?}63M?CR)yZCcFhOn zn;m9e)9v{wpIj7ccB&d3j~3RN*aB0a2q-9ynXWF>MIcotLApecBQc5n-DvPThw6`Q zmO}aF5V@jV$DJlA-SKix8QnpH24$V=F7)<+g9b5P>{o7jz?ZMvM64o{*f`;YGqGhz z_Tf4UlqA1G+9RBbr$Mu&2M-<;B$m;D2*vf3qIe)pInLkbe?7i=Y14P)gE`+`)lrNJ z`eKBRDePA3qC@u+evL`Fsm--<9#Z*uhF86Ez^1-No8cl`MAW-89PK$m^X7cM*w zp)8EutJkN`X;b$~)(puomb?#3&A ziSvKynSEl3*{k)};tbl!WS_{T)EVOH%OU>ubM_-Ke&&HLf8$HS1Zh{NOP6c^cKuU( zRzD`43|k1i!2cheJz(B9FO)PA!^Ox(*{n`1XA?=brkwmUBV*0$xN~=fLC(PjH@RGF z>1b!WGbN!H!MmY`CN*glYSUSHPa&qBCd+gp5itZDdzbjx9)Q&hV%JuV5*&Bz^3J-( zPSErm3m0d-V7SJg?hcY8lBX=A(8U815B#Ub-%bD)m`F(vLUf{v%uRr{kGZH)zM#g{>i`HgYw6D zo~~Sdqqt7XXXN@c3Qmma+y*YLbc1S5U4@STJ-Oq;x^>4*qf?70=F;Q8V@kZeNlCo0 zG%-7K{jak&rwy6n&PaJ-$Xt4t5cyB%kB~NI$)HzAU+79rxAxvM%?3*r|2=yg9W=Z5 z>NSzHO=cSMS=;ni6NAKYnpDEpbwtZd?wK3&T_8Wfo3jI?HFz7mckeNfA|XYh_^8;j zeBaE(QJ!aT{1A@}=^fDPdRyb`Sn1oOWok*a!Gn@@K03yCIhuLYFUfwWMSGw9kdl%# zXL66daCS@1M28kjR}A}rm3MCTTP&m=O5nVA@1jWMQXW2hX#Yvfp_BSEULS?rSl|e((ARVV!wgMDal0c)!2O! z3H_gLQSh%8gZBwJMqG=S+wo(fi44%#a}zeEj*eVI;2bIw*$^P!(t=pDfBO6jcia`D zW}z0bBthNx^eYEC<0>qj1&lc(dC;@7^yNcYhJ|!TWOAoZ(iLK}ND#!F%1(qKkK$<{ zMDv<}TX$msD`b(zS9n9lX9^!nnKYy_vvyW->&mrRdbdT}!SeN#4ZG$PjU4HTQH&at zxWw8pf3v%i$veF(;=%qFZ`!gY5{5*xN}Tdfa5RWKtp2(*%e8ASC&qmj6%Xr&l*`}V zGr!frd#}4?HB*dpY`gjVL_T6vyMG)`C=^3SlR(QQV_9|Gv4(6EE&BA(F48PXNl7^; z!#d=&5i*N_b$AB@zgf`64>)*mmW60Qd9OW!=q_(;iDz!^o~I}LpRuA3688GNjtwL*xB7@i1KT$zq>iI}dCPMfLTU8!6KsOSPrs6EpK zaUUJBKFV**iv@i6+tx?eX+p$eC(h)Qktix=rE(ZhTsCsQLb(^gDI&3v{9Cd@$~YP8 zCj!Sr2R1Y2eGP--w?%AuVYIH_?Kp=reN#-Dz^Gxvpr(VzD9+VQur%pHH3$l4%+XVr^Ds z%47v`+2UfQ&=4lARP@4DgG(b}ljHaFsDgGJ2!dFGu*+B*;aLJ4_vADX?+OtfU?;By zOb|{o$^@t^+y+&p2cRm+f+E$iDC5lMm(FxG7Ib5dh+QF2WwNeiCJaE&RUM|&xIsr)nz{r4wRgZxI9VCK^^_Ot7lIaSPsqLXc?=mC^>#_v|EsWQjOU|L(l#!R2yN2 z!r~FG;Fje!2?;+bIsM(>HOrSV=s0rFpy{yQA5hD8gvrMAUvp{uCzr>~j&V9lVkfgS z$cj_+UsH2yD_LJS+)wQq)q2#dIc*zY%im%4uqTR;7}-kZb(vEo{-w5+ODDSV1^2I3 zemz%TyzDA`;$nkop7-(ft+7o$FsX<4~9Im^FOTo+OsXB-$X@8c4UX3bLF*A*7^ zb}lTk(9D{R>6O;vm)F)C7#YaLhpctns-aMrjiN`J|LT>>A=@&D4gM;oU8?*9jbnsDlBn~p)RI1Bn znnvN?@8Tz@govv{qf&q@zN*9f#_C>%zbEOo-|$D}cdVb?igk>55?EQ!^n!EZo7w*Y zV(!%f(tY}wQ}D)%g)aA}T3Dnv&7MAeIx#qm7{DZC5JS?UJ8;18Z2Ja!$;6)B#Y1V9 z2!tjJ5RgbFeJj(+Jhg;BIH^wbuX6e3_GkWSsCc<^f%mTtqxa=pWFuD<y@bK#KTP z(`EMSdGzH|Zr|RM3EeboB9p%&ApExZoUZP~ESU85;Jf;YQNKjmU5e9UZLU)}wlxTx z)`{p|STw0nuxT@&3uA}`99mtK&DKYk*au>%^Z5MVR;R4H)&hs{d?TxfHD5Ke+2V*f zXNapYq#HRSA-{#QM&KLUoUJwP@K@&t&(6x|MYcRnfWq6pb0L_#yPp9K0$m4(oOHdj zV|lIhd zBSBvFX<@Kn%_3K68CSkNZ(vM1&RY9kds>p@F4G{{?G(W~zAjn?g-swcQJ7ON1M~Ub zDZ<_*#@7e%r+J6Zz}2G5)%18-hdM-RADNBQ=^I8tN{$apCO-sawWZlXOY0HS5GkFP0tntx`bw)QyTWWes!%J1;|XFdtBEiVfYi*vMo1mD(Qyz2N*-t2frnCSr< zn<~sYOk`B(J10rtrBTKg6+N6V49L!VQSeK~&o?%Ac8)-VF1R^?{O!I>L5)N}Nd@>& zhRsls345{rx}h&Ah3Kw!ue@52mnZWDIKX2{i;)%yyt<{oZNj&g$%r@Lq9{q3(s(&8 zjM{vbFjR!0EQmkEI8gtWo8675DWvw4+(Rb4Aq$t3lqxBN!`qo#hmX=;Q7uU=Cv(N% zzE^qR_obG37o-NL2N35Mb@i8r-8=8eAOZio^r}|B_~derFm(8B<#d z#Uaz@$m`hx8J=l)8BFUDEv`l#%}<1d6_T&5cw7J~#_3W_uAk*sr{3F%o_S#$no_q2 z@t*%&41!t_2WddaTkQwT{bzEqzv&-&hJpZvERp1am$4NcK!~@(mw!t+ICgG22c`zxV|hL)cIP&<;4e@jJR6or6H<9 zTn3*kXlWx=fTg{|E}SW)#KR~Z-n)27P)(b~ZYDug&()!+YPaUOWkf-o(#%YBA&g~o z&$3(7s<6%`nN1Bc1J{SGlAoKKo3;+Xzuo3NE?2drti%YxnFE&vFf*@Mr=JY1Vkx!y zCWBPza9Q(#KV*@uTDH`p9-nQlZSFCy>O}*^P%DUxGA0MYt{M6%E0P?agF{-{dR83{ z4_WrP*rd?00e#b~aFjz(pWb=h*_)MLbsp5R3Na4(oWxg1r1nD(PMY?GJjN=rpLeJ&A#Y6|Q&Yv=L zrc(zH0ix)HxzB zmwp8$|E*Pbws8r9F9WcOPm{1XKKwE8Hdd>^X#<;>@U*FzN!R>YL+2;fQ{?@PL6h2} zDd|cB{XyY%o|&B*-L%X8j|n?x)Ru+ouOFaA7vx**iI7B!V_{ffWKi_>oEdTEPvN!z z71_?V7sNO>w>#`6O7ZNg6(2st&)eYRufbsPX0vC`+*jp7zLyKof+2Hd^SN2ebON)h zT>r|h`eyD$PM>FCU(FqWsQ(_`C>S8pu>yOAF-)T>!djL(ra*>};w z-?y!qe^A3;J8q;wlceR36^aG>Z_i)(cuA%C&;!2w_!RH zb-B=?z1Qy!2nf4-_pVW8_Coz9nkuKtH6f?E@(zk?JKkG0k?j9vQIWI8iKa=rIRIQe zF-?S1G!Va-lBct9pW%qqRBN>S>%bl2d^P(0XZh()n|i*Hzm;Es|IMnXr66auzfSy@ z$J5aI!bVX4x~5wTL4^1 z+f^Gj2=zj$gv6yVzG=xWjG9;50&rtzOQ4}N*-tF+O8P`CE0Yw(DpbrJ$bofx_ME`^ z@$HU7wbVgEte?(g9ml@yvL0hd!a*BGXd4>46J$ry>&=d)Dlz`Y(RgU=@pF1qal(j% zOgeG?Wy(4k6T;--zI#5T93#cV;KNchjOQoT@w2G)@VZ9+S#AZmWvasFaAV#286p)C zSV_%*==hYGG9|hB;*Tk$VKU4xl8k~U1)Q5+(Bp&`Xr>~r&7g!OunG@MvUPeY5A;RjG%&o>QlaDC z`-5aOA(O(v7Q}*sGX2Gw9KJ?5xxWlHG-9Th4$LUWjk~4l+vRX3=3ucSuL}AjbtQ(R2Ve>j-Z|NH>^c37F;Ji?X9GZ`>tu_ zlcyZ+9uEhoD_7zSN9u7fh#Am4_7hFcojdo@b^@tzwRDXsH>t>v2|fXEQ?VL&U(i1U z%83YmdJr0E#*D@$c~ks+r3Jx3G!a-%*DW__Vj?9T^DIuybUB(z9p_&C-IH8bJ7)yn zqxX9ciN#Z=_WNMiIFg6PS_$?~`g9lIk{`>E1OGW*)t!()9 zoBf+;Yia4|>Yf}`g(r2ufdd|Qe5-MjuHCWzzmWIgZ9UV?&F#v1GcJQpxfVnm*nA1M z3S=NR^{pVkQ3DF5HNuD~|wQyq!)2d6|))3#!G-IQm-P zQS#!2FJu%!3vgxDR(gHB-BW6D>Ugt+Z(T0Y&5<=Hmac3@@p*;29A@oqm`!>iFC;rv zs>E`JR~o;+RG(=}i!Y7paa>e|(9`)nn`-6btMH}8LWfSA%$=Z{W*NV7!H%{Ay4`;n&)2AM zSx2%fmb0|z3aOIMe||zo9*RMCAUTK*b$K0B)0H~|31gGh|HNIR? zAo6QaK^giYJ$BApClXn(W&Q!ts}2JPiIhx*41pxmXKS~^68(k_9eO`(0~Q=Ih5-G~ z)8uFhofn=3ornX&%I>A4s7K&PX~@|vym?Xsxv$i_t%p8q;FtbSd-Z9O^1Jj6v1VqIx?z_8zsCwvz1iPOG0@=SN+7mrg0w z$i132vb7Mx@%qzT_r;M=JCS@}?8d~D48DqZ$HaclA*<#NG2}x_R2FjOpxEMDAjA*U zD&|yV_ktTq+ z8jJP{e;id*YO@`?c3AOQwDgl6fDjbxk;8R7HB|o7IWu{o52bGC!;SbM)1l^ z%2}D{0WW$c7@~wCVR1vBk*Zvjm_8M;m_hU2l;^^~kO6hvwi#2V?BtdlXTVc2RPzU@ z96LEb!Hml1E-ow zS3KjHIF5vYRFzXx6^w~}ZE|a%v~oz^qUmLd5sf&9itBD!nVV3&(OJlZwxjQ4&WaH7 z;o25LUzo{f=hKuY4HSnY=@^*QkOjz`7dbpcoXk@wXupalEyn{2sl~qMQ&!s{!O5bP z6hSpEuv$9r8w=4^4C8khef*i@EPxCV!r~_9gzJ$|8w_&R$^f@s zJTZajpxu`L(_*R)_`TFc;?POF6T^MUqTTeDlb+r$fB6VNDT4EyVpMvb8R#r!%$acK zd1)*WQECEmD3y9pAXAiKM$X_8xj&(tRW87IlJ3ZiWih8A$Jpf6w9dW(^EhxN7I4Qv>MpP;|PuT4mfKrU$J6<8c1M3w<5n zh7HzgH*40cahk`Nlghi$?gQn%AM9#65YTtKj0Qd0hXF2t98q0wdjP|{ES zjPAGAtrtRfNPSU30@zlyogmT{%l>A$RQ&BZs3j5G&JN3qI7+EHy z&}OY;Y~=B5Ta#?UwoB(|)-amw^*i1$iWE z_dW&vmG}IeZ;R*evbLlrHSa75b;@>7>=XbtC@wQeR!p+_t|Hx#k=~%niG|i2B-81T zzMW07Ri0*|A?yutfMaS*1hSaa5iS`whiuf3o^@E;k(^Sx@GMtN%nG)|k;hB(Gl2w3 zbVSN@T5~Nsbm+yE3qw8rrZW)P&@o%(Gxywfe)q9ZWY3=Z3lPgygA5#$hyta59DGhHuPZFUT&1H@@}zUlxh zo|7Euz%nAsFR5I$a;2CQ(Opra8AJ+qL?PbZKt41p#o~t4Mwwt3=_4b?p{)t8F*1%2 zr99Vfz2m&d`_#RBpZ!n1Uv+xP;gg1dLtDw8T=*QI1^7n8B6g0!yY_KemF!BUkO!Mc z;SyLd&=5ZJ9xXMk_x9}-P80b`ge9R@)v4DY&oGWsTfBObuI%3ZYvrV3m#YQayfl_l zlZuVs7N59-rGVa#=zeq@u>jG0Q;~bAXb}8slpMnz+C~UGb?w@ys@%>s%bgnISjq_? zV}mNsX~#|7^myOa_#*#5JRjQ$Pz!=?$YP%RB?LEep`I|4zitoN?H%M z&ijBy3c~Kw$0j@B9;uH*kjDE#2Paj`DWMv;l}Yzz)fT_IH{<;MO1v7=Of0Zjlb}`9 zJDd+;h|#bmaDw(}$;b@nUa2O947HHMNm=9lM?v`#jbbBw^IpArwKBFK_V}4TXRfTi zS3oPDoL5E$#$K)Si9+Hj=SNNyWeR?v53ckcOW+o9@>7$K#ZShheAi4pV6~J^&0fd0 zlJIa)u8IXbna#m@Z`I|?qzyVSc+xXVcN7DE7?-7)cs`m*drE8vNMKhv&1Mn@S`v}K zJ*S}%zcaC->3Wy^a$|>Q-jBg>fv$$WZQQ8QIp3w;gStjn9QJX2y3R_P<5?XRtIBGa z{q>L@vQQ1xo}%KlfN1aY3-5ke8@sw>gdYX{%{iB|RAD}&wj59Kr?MmS^}Fr@!wFBN zV$kNkX+t?UAJPKv7{{6{#+Sf~rz!$lwe}4&gQOMY;HqVsnymu*{Bw#$cl2A%8^;}S zyWjD^?q|*Ph(Z@Uo6v*7b1$xoWiCJWZKYg!fUdTI4*4@bdF zDMnNuuSRi(+{r92%02QiA?otZ^Yy>SDJ)dR^DksB9eA)K_}<3hpKK0G5+Z~Y5$(~I z9D~$v7mG&{8D+|v#EQ$#=Y2N3{MPPbR!XV2Uca~x%LXsB&jtH=z4B?m=$y> z+`V-M$5x>d~5y0%wh)}w1EIJat1jZf9F8@^Dy}P1 z!IKwAvGNf9;c2iI8yUh%PB%j4CO(qhPu?NL%b9Zo#8mhVEciAngUj2iB}5tR)jJz% zdW>Q_S;8TLf$NgvI=k~~$Sw6X?+m#U*Ja7kIW{&n>%V1hc)%RMwUv)=CQKW;HZowp zVdLDVhOTaQad8{$_rqKeayDeqLb|LnjGLs%9a2E{{!%BbP=p5{gw_EArtv^!8&lv- zt{f^8C?$dCc`ZhpDH$PICbx2+=tfDjbjtLzucvL_zI|<_Z3n|PZkIdkIGE!hSv8n{ z0C*e8@kEt$f@p?O$6hcW?YVB*4ddRro2u*_A0pR~v3ICLWGa!&T$XkL#r_P==op6g z&7Rg*(XyQyF!LLDf^w2PGm9*gKKn^}OF|fcV%G1$r^h4We6ee+yu#sZmXOf8G55m!$|Je7p6W*> zAE!E1McG@e!&uk;4IA-L&MdOFKtj=<{TGqChv4dk^{l`OyNTr6ldRKdEd%U zkYws8kwV_qua@cpVrkv1d1ShuO;wayAJ-=3ll5X0KfLm=?1b@zO~Pm$F?b3?nL6b0}BaVnN$jUM;Nl{p-Q36AQT7}Y60N25bxyMQ8me! zx-{~UXj)~J9XcBfgv=vdOJT~&?GYe>p?or6x-UNC(i*UhADDG`$X6=i7!g_sf(#93 z1_{mlv(}Qvv+QSzfSbnar%tXgeuSzlVys#jL+dw}>#vA>=crtc2@fnr@s*O|L+oh4 zDcx{s5}l!_$ylqRR#&D=lYxqN%nze?fhfZPq=5j^IrF7H0&W+p@-T~$ik1iJD6$wN zoZ9!OX^PA93%%?$6r)BXFnF?i{wcn_RW^%^`V1BQ{>Y{5F$z+68qF$uPChi>G-Vai z%K@N+JLn^}l{0G3?%m6io)T=f6Y|>9pib4_M~`~|lCWIDMz3^tPOt26^6jJp(7$d1 z@3|&9$G$GA0p@NCJ~W6DIcWeX&_GK4z{naDEC&jz&cb||+C({?gP>DIXCsd^IVg7J z0B$DrZ!!;1b+cCY!Gp^-XIVZrlmP5 zvn;_^bLrgPFY>(T#S`iSjTepqe*_ zh((B$F72SR{r~LU9#Bt+0TS^i-w>3h(vQ#_kXFIoMSQd+WduP;_yH2L9f z$)Ene>XFL3P5D8LfyW5|xUK1k%9mp*WP#0}ZrmVBUquAxF0`a1>J( z4uqkw`Zq;NYxVT1qPGJZoxc1C)~k|&XBrtOFKZNv%I_Ptm3bgBXP|&ItZpztsV1~2 zmLEP-wPz@U26|f6%`2tqu_4dolTh)9D2ZUjKe7$pjZ#A!*Op#~?;(Ujve-boM!WmX z9(9C538WA$&F%REZ)av&!U9R{3(L`epn3zn9P)ROkIvL3bz-&R6B1IAl9Fz>B8gk{ zgRkJ90Q3_U8u~b?Ffg#=A{p%zcovRMrIbB zxzA_}+?J?irbI-%`5DuYi^jsvWiKt29v+DC-PoKZLwb==YmE3366F1E7-ETlTVEwR z1qmwb=lgZy*6oYL_vBiMc!%(7**r3Z0J=w7!U$fJ8YrV^2IEe5FL2m&@qFwt=|GqB z@gBQ$q%S0R0@cj^u}>=WkLNHlPaIR0&8uA`%R6*HLrxRAQw0JN^4>|}Kn@E`D7kp> zwJbJL6Y)O``HAU^&u0DkxwYDqB_vI(LSYW-LKl#6KQjA-yxW2F@S*q?bH)uVs{#BZ zg*szo@{%M#+(W62^xl8HxA43ee(-TZzXDqjO3A8nD%!)+I%mQR(0L?OSlR3T0Ran2 z&(j8zpO?e|g8wNQWdji){?XMd?UMS-P-kih^=DAOp<2x#t;$IDaKVzRACzMZ(7AJ-|r^0 z9fj6|)JHVIGLeRs?F`zukg?tt;Qtp%e=LCeCv`9g(ZD9hM(MZRd=y-Y!Lu81mhl_@ ztFT0ghq|lW6Th8!ObHf^hSh@p{ywd+urRSs5JyQF@x!gj`Di13W>{rbGUVMS2dT?o zDq%6<5*UEwTALAr^XVc(LW!!T+vrSZLCxX8P9OzW95{oU8Xdr>r=fICu{6*ryhC8h z|1nT)=SD$}Xd;g~@BlZmRz^bW3nEb1|)+C?CF(YQDpm z+?k&k-RS1N+KqY}qCgDoa%DO+(F2QoxDA;yBMXrF%$YOYju>F6LPJu9q%&2h@Gl3y zP@17wK15j0{{1~-76SKoeKjM2oYQ>BVT1=)p`qXh38H^Oe6Z&2r& zOeu-4c(U$-|D)0@wcG=UQU|&clnenaB_)Qu_|ZsSXt*-(4`8qR-5O@lkd?L0Z97U; zDubIc5nXOuiwZPkt=b%XlWO;FK@oNFvF4~t?R#RzcQhlaP^hD|G&`7fWMeu*u*QDd zT&$Rt7MP}J>C!PyuX%Jkb=&ECk1h-amW6X21rT2(EqCMrOc#4FcUNO_$%b-{nCrx~ z^MSj4bza>fBu%1gH&ZIl`nG&g%V?Yu)kG1RmzOuU6DsiFi3#5>O;KG=Ok~nz6rFSi zmuLHy+7(mXs6zex{N^61wBaFoq1$79jqGE6#xF#c?C(z``1yUzpJJWfeIzt0v&oY! z;o(ti1ll$88~#U0)3X^F8MaHW(um%ZT=nrzKMH<5Se%9Q4B|>%(?hq0M!Jr-hkRth zYa;2F{(1Rw@{Y$FiYTu|T_c*Rt-*vx|4tH~Ld@0K*z!Xz^@>15iNPTF4}OkPpOl`^v6rkAw@>$Zp~~ zTn`l&2MLK^q2oW?vw=JB0l)-r;6@W?MOXwqSFe)TESdQ&vCM60aQ_KDxc2!R!`xVtMXJEVbm zgfup=q7Ztk7%FNhf2eWG@n9)rU0UK^7L*LLExlSe9Rv$ukw}FE-gC-lbww=2RXOb| zXX2l@e}mwZBu~Jr60VSxVo3u4S~4U?Nd!_M2#Rd}OZd_yquq0Ab|nti*mce*d*r&* zph*Mg&-*=@Jrh(t4b(&W4$~e|70QZpO>{+oF9YTApd-+$u8tuem>E%nbjz zo8_qb6YUiB*Vk=ic3rb>1CxsR=Th&M?)pBk`QF`|w5IMWEg1T`i&3$DlF7hf1?!?J zE3&Ibm(%AjD*3|s*|zooOS=WmKRZ8xF8^YXp;r5HJPVFceO;C!^w~4pBlxbbjJH+k zUA?3|XwauXXZ%SLg}+aRChU04T@xs8rx8VaJzocQl28(tEpkdac+*f_7PwVMK;@Kf zTmJYNJcHNUYR}+9zkrh>@H8kIX4iK=^`XPDr6wjO)TmFdwF)pC6rOtT-ctWnbrpW! zlC{+1^ZXXxagH5gql2QWs1~f2Sa|K%|H{_>Th-S@eS)R^aplXa#?smtFQZKripgqo zuTk9@j@{4go_^Mwh{&jPR@x3pKNN~XKdHk(H0*Hk!G#X97^15F(?xKj1T&{WRkZd6 zrMZ~WLCr!}9<{ns#xzs=i;%E*ZR(-Nc=3)rP)(%jP-ZLdn)b|khh*N)~#W%FI8zV8KR^DCZf8-^k}o^aA`(X z6;Ac-<}XKq-}yN|(Z~s3BOWF+oo85EhYLP1A~-s|1jam~6r`HKPNx8C^%ov&`qz?w z%FwtehpG$aOm)0(`by?_B6AaUIc?>c3HN~N+Tr(c0t z#5=poNhpcz-S~2VnVDdd1RQZc1%2tBVzfxSK2e<3n!SZI6E@?hjR(o8#6X>hL>oTy z>gDioQ@YK;Yexi&h#xTQ@io2Tga%+LA?x! zR;i^G(tZVBDt|n0S36tBlDClG<@>CwDt2fw>DVk9Si%vLfpVP6MNrH3W);iCJ>VgI zK(3W7_Ps7B=!LG3d^fVp1H^YauP4|&x1K7W-N!U((rgVyg8_%XK5OdaWI5Tx8ir?E zI*U_9t_B_^II65tEO)%Hww!+-MeMv=MbI%zpEFmQVVNd87=lZ__6D)7dXUn>POnGh|EBv|~$G ztyhUpgRLX^fI?u zv2^){iY`3?(Eo7eE!U8NbaSsj2%52s#{F~*0ay$)PMSnOlvkYoMZM-)0-uS@_q{Kl_!*TI7`YTTzxnG%}~@;^}pSZIFYlSHk4iDfDg&e8m8KI7PvlOyS)d!^49oU(ZEJ(c{+DmGh&EUH2|e@X*WJ9O%F8YRxbkoKo%P9E{Al&ukJjfTy3 zPP>UJ=y{f;xcRH0GG_^?kr-#TdtQ@!qK;rkR_YZ_HY?(A;=IN=x5MKZOiCIMVlu*xr%+7h|a=b1h?6#s3pfuag**0b59?Li!N!ZW&-J??~|5j8(dIRalT|IDPd?>Y?nHir`Jc8oBG%0ANU5pY5QM_jX6K7F%fx% zUsrkG+F$>(7ppIpQ137z`2L&betIi>I2#8IP|xp>pR9d5zxE;`IXLOZdDo|be2Y=1 z{p-i{-9Hig$Cu9SCrq2RCp5j_t(zfxs7qwPg=zBR%cH*BYot(Qm=OsBfLPef=#RoL zUVL2RK2a(cE*TyejPY8-onun{DEt=glH)uhrTZ?UzbJ8f(Lt!Wm6|#e6#GG#e%t+b z!hos2OfVnbu~}W`!HXx2*>Ze)aSKaNdY(e*@=Ub*PAoO0st0R4_99?p6vk$qH^x+~gg_(HdK zyYXZZv(MZAF2BV;^9KSLIQ3WO!!tRaIyRdSAH8wI298Rn5c<^J19%s4D&yDeD5AH z`>%7eXG7XoD9WYs79-LJ3u0^lMpU62Hq=EEngS-a@ce^?^+#<~*Eg|ahEjs0oeS6z z!X!*`SC&ayh*OtZc)dyTPlqN?m^JE1aB%diS94h_P5=B&NZF$ro7~zcchTt^ERq0N z2RsxsxpJB2+iNRCUiSZBrOi=|<%OIKU)SF^#Ac-|CfLMza?+ANfM9p?iOByoYJO4`x4 z{-{B2UzQ(zeOEakp-If#H91Qytq+(qn^2gfUh?ueX&K^~3IFk;@M=&7v@owXwC54m zrqe&S>qMM7eamWIGu@i8XTf>rpC6WA?u8_Aer(*8i*e<~0thHZomVr;NjODw&4-3_ zKGhRw1>bQQvDd8A&qjy0{c&;5xPR)mvuj|phyT=$PBDj?Teq%R(qh~XqqAJx7Vi)z zU?S+~wIezk4|<^UY}o~IWvu1;O_LMsL7&_1i6eeF{=i>c?5UJ4aFV}vSO{do!6e>? z)FBOKHVo0dlaiu(s7sEC-@%V=jQJ7IPp<{++eX^({SK)_sEmT4E{@Jx_1mOlCnr+t zl8OeXRDX$UB$b+;j!v3ObmDQ0F7>FWsaN$ACMhP*07bFwyA8rBzC0Ov^@<*4u28!! zET1AJDP62FlKcwG9dxCT5k==ofbo2Iyt2FL{TrZ#Wswe5W>&T<>i;|VP>e{Egv>6A zWEqhJ1LfglgXf|0vnjozGdN|+@cV0P*Tb)e9!*4;Wzc=CrcL_+q$(_x_%^|Bkugus zps5MDJBN!@OvEwLhNmtmr~MuHaG0d;;CfuG1!}BAMxN)ya%orcF)2{wjECwPad=sR z66Ra)3qJPtLDbKuAm+rR=Ymp<5+_HDF!syhZ_iH$;MYUSI1Iy zOhrvD{95;@#Q;DnFFjA7*$<;4=(~(74qUd6#&=HKB1z6M_jqyDXWhCh@*-@~%yz*=F@zQ&IyU!+0{Z-h z4Ce*;I+m7})~Go8;;Tk#&U2C5T7^?n^&#xn`uLK+#%kWH?5dPzzsNdB>Sn>y#60{2 z(@FmlR-Lds#4c)O<>ktEm&6p6YO7aAR{&D^%@r=mFiXe}%JhEgK5R3~s(IDs>h2ka zjS>xF@(W*8{qC`oLuH#AfDsqMYl>ti`LX`YC?-#2{U+g+*ydWEM4{ga%XCrc#Z&USKVOnJPeD(rW0~^ zt9I?4=)_{gVy4&hncnoc%e`ZU9|mWD2$wn04mCp5{@iQUBRkvq4*9><1mw>C`}bSr zyW0fglB=y;eJL?zY04uK!ohT|(>m5R21d28dX!u-iS%**jP_+i_7z>fcrJfI_L0eu z-o^TsYRQRvUH$%2Urg1EpSwMr?Ea$jWD}a%qy{_UN+A1$-RaYMTjytloRKtIoI`jy}7lTn4k7LISL75RC>NeZv7P&<&L&bKAtOyXn`vg8HAm9l8GRc zxi;T)TlNW}A%ooY8(k&= z96YpkYHgoM^LB=hOVX>naOz!7%(?QB4l`T-L)vBN(7lcA^7UdfkGte32JD2NsV2Is zLIFW%c!#G<5#!RLOQM)6b;le)tmoBtd4o@z7u=_8|(pcS?%w|o0E2hNJP(*55z9Tpq5 z4AxWX8&)i`(!MqQ&GeeDM_T=6bv89+zR%w1PMsL!0ZO4qcKCE+%1b7gKvf!HU6X0$3xxfv3L$$IOia#if=aho3;p2T)#M^SR z2^wr+p+evAy;!WCGBP5PP8^a3loWsh`KY+!;9$qcJ)ck zoOYKyx2+wSIjf%bll=P!KlzM4UD2=4%M<-RO>LRwRMUCHU5i(rPPBA4`J=D*rNcEj z9rSCJ*NQ%4*{(Qg9D^7Bb+(3Sx+t3A#~=ChK|nW1UJ@`VK*z_g5<68nVv*{+b*;ZL z>gR|RQMKjjNf@FocMlAZLKQ)r3=9wiNesC-F}$a&37T}o&{D#D^@m-)Iv6mZ**S(m zyOi9mL#fuN62EV92m3?XNpKe8amnhRcr|YG5 zH9u&4b?VHg`{NrGy-r);>TdZ@e*T2q)?ooo_e=M>m)#p&;;A#MWL=@f<7=CbczjB( z$>Z*gD%q0Y?;T>kc=jj%^2t}m^o@98RIflUWaDIm1!W_PhYV}jsO-t^v7XQD%MaOo z%{%#gQH)jT$Z?r(0(-dJc#!w+{`!T7?iuU#{+#C>uqMV-c_%&K%PZX@wtK@meeT%D z;Li^FCZV+8DM|m~H0`I9$Sg!X(PWBh43(zg^VQi)RjH@zZ+}>rAp`sZ$O?uY;9b$j)1nP5QfBi3Q2AtKV* z%i_mD43eQ1WyvF}r*VTHO`mEUhMF?&N>HOFbK?%T9DcT}GWFka0aO3*Jl;Jzc;EeA z@m>wWpZ{uA-t)aOc|V9vgYU7cHZ8YNz5Q)wQ?Fx@V=4nSCn;S|WW8#aaqsA+ur+P} zdDc2A^+Le9_a0wjS6wX*xj9MIC&MB?X^Fw&WaH$#72fXEH8IogUQ8L;F?Q(PMaHQI zcj-+$dq~gdRNrl5zBPy_9dsn4=0>V2G`DoV&dB)i(e^v{ZLh74(#AZ)j6~94IC!m$ zF$YkYdFNH<_w!`}(E~a`me$rDQwrJwmI$l~w^Fw1)wPfxg(GYewP}n`V>HsjYK43{ zSuiK@zNqY;0qY{s4|3p7g4c`blH2dOLVzSK`5IU(!$qOC z9^JmZ2{RYRHrrxCJMHs@=c7Mml~l)>dGsAOE9Z4DBEj{3k(aNYyFdHsg;^0_-xR*; zxXbLLSx*zU)y9?jt8Q$0zM$!lJCBY#_I%#^%Teo|FYfz4S!Qt}=Xc73JP3sdM=Gm{AGR5N2g|CgS_BO4uf3w7?&wvYCimyIYX$AYo_jFkQ;CoJVQ#H5| zvdK~P&%W+#EQ|b_bEhdd+*-ci_dOq^rw4xQfybV zuR0UAv)1mo=)xYJB9~RT!GWmtsUgPT#4yWGsIGoJx6?6l~p`?tIkmj*Kop4m7ExM$f^%4+{!x#bwQ$@}gPTz?CSEFK^k z($d+(_h2`y|BP>htKU<D4A+1{5}1|Xm|jtoOi(?^&=37ACdl38rN02 zek)yG036mdyB4EUCvQpodVFQy5?=($TA2N2zUc_7e)HplbRSP>oW+lv=I*vV_WJdM ztEi@)mBd$^{-pL^GX5n^i^ZnYHj9Qh%hyA{j7RN%W;Y7wv#Z@mWRsr}Jk7xzl4F>0 zOS{Sb!s)4B7h@UBcW^gwhA})Fw%n~R+{cq!u;2SR+X>m86OFw;)%AYKmsJZXZ;#Y! zcbHp`r?<)3e8<$ou4giHapu)(?JY$nxu!1CR&iAJ(~iq#C%vz)suEjvrk{~RRpqi5 zYhqVWDSck7f}hDz;?agbb(O2peSq5Ia_TgYwv$UWmh!CunHy2h7Pb9$M3ZJu^~{mM zj=wQv)_BwRdu(y$hfIWpE*QK#K)Y|$a&$QQotQvespMLj=X-UI=Asw&WH%!-yPvgRZ>!i=E>q<`H|2@0@IZO( zNG`n35H26Ct&buVCm$TNFHW9y|Iizv*{xN?S{~DQ+9tU(y-S1JzA!yC+Hz%J+awN#M#^rA1e4iR=E6vYdG|aI%6w>qE$~Eaz zV9Jq82)Fs;4ewN~b7T=RLWWhk?y9yJ4$tLFyfr&KxRtZ8f=OlKIPXuvIu$+x-N=Nstt%cDOgza_8?RiA zb^pZYvwZ+tT}s)4hs~=rN9ng8Qx;TSBV9ez_tL=n!BoHHT8^nqhRv~?ICnO%%l%%t zUBMW@oiwahvDaf8`+*ib=wJGdJSpUPF&7N@*3U zO@1u$?zt9$yX95gZSL_n4sbrrEN=H&UdBC>m}_tSq?}oK`D@|(IPiS+#!PBMa#FLH zZR?!-PhV#^8~XTVBO9*Kl=4d!`YMB*%K)jo6Q9$ns)*5hFAS%B&u{FTmn@g*XdbB8JNmoJ+Krbt-)=)S_ldiww%ScDGOf7A$NO(9ow@gz zxy2xnwY-lm&D3<@KCAij-l*Rq{BlX3X|ASNPO!;c3X{81FLC2>eata7If;~t0FlIm z%6VF)&BO0c$Hl~6d=q3TttudNA*68Ern*vg+0&+R{sLQR_Kp__#Lc~mOzR3vjnkuJ z(8B4Moqtd=q^M<*eW$30j>`?YRjP7nS|Ve>jD@PV5Uz#%QKmIW6?Ol4P(e2MKFxFc z2!?0nUqy~TX%KJeU}sQKxhxmgVKC;f80_Q#qy_T-j6ap$_~qaY-A#K>UeOAZWm2#C zeRb8`z{r8RC&O!#1RG;MjIFhwA3eEo`6Uz^SqBvMEEy3{02MK__xx@oo`6`H{Kr=p{HvR`9rdefVo zo}W>%&cDcA2LWYf&&d=Y2d(_3g&Q^P6Lr>X^SpW$UoRnN3c>l^c8qIZ<>X$|lg}F* zKgo%6{A$=awO~46Q+}{Rk^a~3nb`}cqBEvHo4S293)S^xBKX(Z3fBBhjc{#<`OLDB-P$bt&EBjw3HWNbS}1gWi}#aZjhtV zote*GP-~Q)qeacxo4+Z}H9S>qM@CiZ;ZTKRU1=LTk~-XiA~_S1^~!$eCd}79A<0af zTj((JW-htX0ijL2;<{qL*J)c!+G9nO2M5vQ@oyL7l4JLg)AQ)i?2(wcwzfQ8b43EJ zM~dqvr_hYh&=hm9YzB2w-Q@hA6b`)~cBbhu&gQDt&&k~YN>V;KGp39bsC4cA-`DsW3>7I z;H(IdLP(1LoRqh%okpp)UHLzFg}>v#>h$X_EurXam047Gl22G&x&qp)KlFsJxKtqC zJcUy8YVnKY`v*NS8UT|X76%0I6fhh+_?HXg+m2gMGIbwylyK5r<~3&HxGVc}>(Vs` zoHd@XazFtGS`hx+_p2`jwsp?F*$7aa{60!lb4SU+>MtAWR>6~GFFeU1*T4jv|GY3$ z>zV%2_F@rDz+077Rca77=Sy6&BAoJ5d2z3sR8!^cThm2y=WRpyCP@v$kzqXldE zUWuMPxuSQ+Yzk9t3P)u^Zf!}~AGH=2(m2Tam$orl^>baBVCqzMu1>0rAVdH2 zwX0UCoc#Kkbq9W!nAv7PM@d~xWxrGeuC(A?e@W;UpE)35wFWlD zO=w7ifD{ezRr)+LiwGK?xbG3kt{1?yp8x=isWO@Zm`k&I+s=?GHq7>bCm$U&W{ZEe zUGprs0Dn7^BSk|ViPejeu1z6a;^Y*S@p2$taFGhTpJySA9M80O4;Yh_?DT{ zy;worwC0^Y4|(zM9FhrXFad+4{QlEnVOByMDD2r+iX@}<#g6eKkgX`===NxTAB@yH6hEUXO31FI_{ zV5p}K2W>*%$B#OSl8m6-(M3u;^sq!0*Gk!!49|UsmaJ9W-ns|gj~fG7K`84OS8t?% z5q~@C0L7>>9<2;fZc`}LOypHdre=M1LZ%g_#J?t^v=GV&PxqfkR zC?A8_0NEnv&$IjqtW8V*hwL_5^sLJ|*ct6dc+BQrdMeMB8~?}ou;_e2uHYc`pYeNG zY)9x!`o6UyEd|ul zaM3&)<3=>o&~an-MqJH+BJe(Z2aOoS9mFKp1DYlxjDQ-cq-G$+i}%`Khee?x7ME$= z1QkSN+r?faW=l|_p4EShxz{uuR#ZEn1}a~a>12e5WK;Q}*oG(;35l~1-Sr2jL)C-Q zGidB0Pbi@*fEEv7+WGyq&R^oRa_}^P!N?TU6&vN3N^((Snm2572(gsq^20*UWi%TN z%vwPm7(}2$yOkRUB?L{b5w)kGE<*>dTtKIVV|K?LHa25m)EPQEK9AmQ9d3rAk~k9{ zg*qNGg(Xz>8=EnRx(jl`o6N@+C*a{>kPiw<`wE*!RQn5Gp`(MAvl%1230K0lXU}lE zX?jqFzVLt!{V~%uPu#>K#pfs0FkIFU?~(Wp66#COZK}EYbI)4I#zSaNCF)P&N=2rD zV1`9v!&f$}KH`AGItZDJ#d^2UzXCMO;801$Ta2)GgRF`(;^09Dh0(9BVq=p*=DW~x z=S4l1PuWV?GhV|xP&sxdT{V_l914tIPCU1M8=$G68BdSc0h6{=pfY?B z>Sp;$-n>Y?-Z9z&vyOC}Z1+z;C+1z+nKFQUgBu6cA<7}HGEhL0mSU_OLov*~x%qtI zx63?w#1|+3L9hm57>D#QZd(V2rT80cLDP?ZV<~)^x(te&P{44K)e)l$sp^+#$rts9 zf6GZS&<`y5Co4g9Rn=&8ft#X5b{6BnlTu?c4hxbc0iPl6qHjk*Af;&BY!r_qD#5cs zy>_7xgc8TvA{A<7Q=;?*Y84a_p=UvDkNvqYS4q^AFjLua%Am`s_zVdd#6k`;URT7| zkECFeId<8H5HfInI=7@5`prFZ1hFRF;YCHF*Ek^vku-g=Vn{|gTAoZ=*fmHlDk1Bx9eyO3MNkYXniEAgX@oxdioq{PE*;xYT$u zbp>V(q@f*>XVUBnxJ^s@tVS*AWDUR)BeoYX=&MUJU*vdU)!HFHIHb3MbR;iCbfrm0OAxO(MpVCDv@|OC6rk|yahf{4@L7mN2~rEPj1ZBUB8d@=gZsBm z%QxU?iU+G{ps(MgPz>UIaUdIpfaTQG?nYD%*q1eKB^&{x?CV8mEVFZO%(;paqeIwe z5_6P}p=*OYm^7%Jn!5)w^3(9^CO*1k1RilwfnOh)SpyBnBpRZjG1{Sb!H@V(kbB0) zMX*Z9#HSOlellzgxOy}(u!>tsix)pyl397-**0x<3dQdu-YP~)K3f>Gj;z%hI~nyD zGY62*iOqhAK@i4{BBL5l?cc1^lVeBSikgNd1 z)PN!fF~y7w!^?S9`Q9DXS6Zdv8`^t+QNXkDlQGDMmDAyIhp;K2i7 z34fRZkaitR_Nzx%c^bNpXP5rOyc?6V7b9!qk;D-xDk4^LDd*W@{FTc>cj`@C^w7{V$O!?O1g~r4b9Fmxzb!3h5`7&)N4;HR9+BUa$+eKSL@j3we zh@JS|pj5kJYef&puXHv;tp34w&KUxqP~1GFyO?U2R%_AJ8*RA)@84Iq zh)(}Zz7p6t?=q=|*uNh6s3Q1e5vNeZ^Q)G5~xjqjWVOsDd+6UxzJPFk3_pyAi zWJeuZ4{**o7~^<{cdTgq#Y4N6uVI11)fS)r6sluwF1fHIQ-41OdEyU`YAFMwy}DYI zhtcHZg_Zk3Jd0fzF`@$3xJ7wS8;(Dm`#J_;*)2bO*C@9#H5rOmS{&dwt+hR{LfxXH zQU;zB!50bz%yq&HYrAkf72td(mJVd6U;I2bS)im0@Ue3=pl4on7%F^GJqzv$49Qw3?c%wnCEsqQQ*#(!jVs8?)Z~xLI#7A z)Pz`D!oxMK`8ihwWb~=%wrVlzfMM4lJccYjcvGUrYKmO;?%5OHK8hhvY#QNxQS4AJ z5=x!$XK3xw1c#C8Pqhv53@O!S=9;GIGA2rbWzd?ukM&M;6hBU6DM-1YS5jV-1wG8k zIO|^J{#ZP)yiWycB?NZNrP)!KrIYACupXr-xYA^}5P^7@#abFxuX=jEEDQQ5%^m7t zC)km-Bs8p7Hzch68JAIh_w&o@VzD)5N+Pi$liNp0|B+l=Aa$z-rYRG<)~hCboTS-I zeQ@vK%a=#d;=}JChvPO?6Q2&Y6w}@QNEG>eZD*Rs-<7~IMk9bYDy`~Zg0cWr1o1#= zGfNxarolBgvnNhs!37-?ekLguF#@$^AjQcbRD=-ZF8jPk z@;i*#SXldHRsA6}MxvpDuk6U@p(GR|Xf5ih=OuRU%Y$b;S8-rxZPlb zbO4+UOIHmONsi`>!g^#|D$wc>-Q=%dt}@CogN5)LAci<-3WYRGk<4-+0^)_=KT3#r z28k;C9M#hYvf9i52khIoZ_#7KW8t?glZIN@z%UZNw;^Z{8KVlC)iEu-)aq%6g&m_L=5m+w&7fLEh) zh+WQ%HG8yga$PyKPHwz}~U?%ebJ(W*L zc?w?HDfz*PdVQY3ho))u{H?kd?O+ja2wJk0j1(e@btur9h{ZStG!j4OK%xd5>l|Fj z=LO9Nx3AbELwjkHzzhGAofz^+^Ce{#FKidGiG75aJKCAAMp7-V+|FI;t;rtf)gbQ; z@@pt-WDcnEZA;@+zh(OBZ~5Hyjubg>@3QRU<|f+YzqvGe2+nulNhpR3KI_aNW|&yB zK};R}HX85G1d!nuFr2Lzbs&zk09OL|&cD1arrWZT@h6@sRNelkv5HYB zVh&;C`}jkI`N#48V|g>hvtR~&sr3D#NNz}H8yPr=_&S6Hjf`5yFaVSqVKRxmCm=5; z4!<5!Q=yJ@R5D+>iwgJ&ig6sFr2Ceb_&v6tbrsix%?+7_?a_vFi~vbQ))rvb;;dh1gL_&MS-U4Q-(mDQ z+%$Zii+%(eO#5ndex( zy#beaoiHB7Ro*=tkY*SLbpV6N@3H`n320&;e5`R9CzNgPIE|80LXG*WhZCLm&6oDe zC{umtT_MvLNnuGWm4K1;w;=4pgXbtt#AeL0BJEOSG66!mnw6DRT=~1|>Y$EMP4wxQ z37k$Hs(AbMZtTA0r-QS?gM)98SZyaN)!um?&D}G zZCp9j5a&ZCw;oR?XjPU4WUY-@4|>B}>;c1mhjLSzI6kw=5xVn+)$9o@RN`X@Q!kCd zA{}II7#{HizhM^|43LfyXFfD@G(oF!=KLJ$RSrxJC1Wh0sEWq(AnWh%93G+!#!P_H zl6l0A&A4YYKFaMIJ-8147bAe&KqEtmHMM|#oDh*#WVnlJUTj1xaO4EDU0>bfQIz4N zS3uV`@yHyWq>mmvN1aWz_6h5k!i_GQjcD&Tu8%DhOAA0L)7|HypZ$w(J8?hh+n>;&b{;O11@1FdXNTbBs0M$p$&!V+ z5+ZoO=}p=%NtOrng>;&EL;{v1g$cr9Bcx$5sJoR(mluZqWtp~ell-~*e%J38bY2C0*#uP7d4l?<8pT0wmqQrdt%02orOxR2uW<#r28>vBQ7WG)WBwfl z6JsT03+-@8N8;qsh2xM9Z6d_pYqr|bJ%-o|%o>XaVV|PgNdtB#c-KF^qRjn4X226Y z?pVPvH@v484;^AT(d9Z(O{|Bz-G1mTOru95sAU?B{`-KRk*RzLQ+*|cK!&u5*RRKg z<72ZXzkhBulQKA;*?d9E!98b{53RbcDr;t2beF7Sf@8do{-M*AQz_gX%Li`WT@<K_>kEh_8Z6xvjF`g)pFA|D zH|+U>zqf)n|4xUyJhqKNS`XqM+)f*J68l{b;G(vK=)siLTm}7~jOeD-qMyDxlvfsR za|4cXTaq{w>xQN-W5%#JGE#6FiEzRMU&2?ALes2%#zp-AK;!_lj}5u{5-BuNM|`(w zvzD&Ppa3~B&}5&8Btr9)A%-6TAXrtkd-v|D=2Ke@Vq!!P-;GyDz$=*)^LPsO;>#bd z#RB>^QD3*d)ZD(0?IO;~PgBm>yA~uz$VRY*zpJP)MTR?pHlqt}~My zhw?B0>yxyZk(r;qzQogg$B2OGRb##!#9t${GR^|^`BjBgLmGK-gmt~B^i;((0Dz8S z@hYz{&-@-MOc3ECpkAN|D`7S{^yL%yf0`+P9E$QkGTY1oDt`o~rDm-E2zQAzyJC!; zf_@uq5y8TdOBI(>+3!ZP-S?8veY6q&znqR=5r}W6&Z?g-J8NuIJA7a5Zk*-Gk%sg509rJ$3pMH|e;4r;lS8nez)1zDKh7=>t!$Zb70206GWu5S)HPc`{&VXj3|%5^#eaBzoyh-sGG6Hy+z*J=TPov|FCWmX zPjxESLho9xkjJ`M-Wgp0Hv*KGHI1>-@lzVwOOK(BmZ)Kcayo)gj|5m3KpF^yhrA6u3UI_s;Tz< z^e^o~*BP6rCvPI#%-)oh^+n&=w*Kht9Gp6lK)V=WeFh=l5kPiS+e94pBcOePC#C_= zffSgd$<4u_ej6MI_QRPq|JT`srKWGcZbBmc3Hy!Z@Zk_-peQP905*mmwq$}x-xXGs z+`Gkgpr_Ff2go5H=Qw2hH~2(GGNY^eBO-1;lJF1s5cA339R<_B=5%fI=1vs$-rS+X z6428LX?zjf{6+vbW9~eyyUS~fTwV+}MPgim=5NVUr?vruquz<)L~~|sJYGG8zh9JF zbNe1gbR19wfg*SG*U*2DcDuWvt#>qA-c^uca^;YJmxZj#CK|^bN@wJ_FGo;W2KpEH ztvkA+K3LGhdLH%;Izj;oaB$z@^ZIJNQA(y#JnL^YfVS_1(~#js1+5%v{7@&0RwneHJj z?k_lidtp8Jvb_8jB4SLvkrN&p0E@F2B>e$sf};D~BK&m%tBV0 zsN@2%n;HoI`Q*4Q;7l~BZ$rXZilx<7V9yDj&OVJynE~?HR^lN6k>>{G#>h1^OW{xh z_6c8BGc7~~M#UfbJw#PspH(??5G)zA%`Fhf#{I9=Hcx1o?FR#*Jql3+`zcjbKqcrAcl9nfAj@+3<=&;fzDs$;K8ZH@7u9&L~?)eD=$3)QvJGCi_Uklx5L(8e)o2b^HtD zti-&2Ew?io;+xz7p&42f46>8iw3xtUOr|S_s5BtsQOkAtaAi4nZ`%#}rK;3fNN|d-vwWT6_5%!^6a+jt(6WYL@HkqP3AEVZL5WN=iygaxvgR z?gMhlv@GpO2)~bz=)v-R&SO7-J@W*`nCRhg3C>xHqg*Mr;&kO}VdQ?Lu zZ`dm<+2Wov+ve7#rnAr~!F|XtdT*z7IiraU{R=EGc!u~QpDWZpgi2tE{mJK_F?K8C z(HB9TkC{nvU~F!|6u=fY91tA*1_EPQAY|Kw?e=0sQEYsCCL*$#zz%ew;u-l#00`xt z`-y?@)gUA>hC&RPOE1LSFI!tffJlvFTdQblzKQ%j8NVfhqWYzc<)0QH_c8IwvdHh> z((OKH4(96xY;f^>KA=bE19}!mNl}l{nVCQozK`uEZ zr6+jgKyyFz_iLEWpyYZO5#_)h5FuAEvE5I`lSV9yZ`@K-=Yq_!`v%~Y{u%!ueU2W}bQdKfMJ2UePUJSUw6Algn zXqR|pk=ux|=P_% z>?>;1KKv&pFK=5|^=F=iBe>gd9c3!kGXd07(w~2=JFcKmb}2J)Bb%m{R<4ac<1Ni( z>Gal^*5O0jR)4V*b}nGQzOH&_g*MA|qp*7o%>NPF%Jjr| zO;1lGx-F4vZ21OOf_N+d|EwQ5B|WE~+2&awdqTi*X8`YvM(Zb2WIvLMC7VZE$^@3Qdr-Pd}tZK(WY2)F)^|2u(+U=ZL$L#hECAk1ek95@)d2? zaJ^ZAVarQ(c8k_uWj*3{kMxJBYiX(J>G^>M_Xwt*ii>GB!Ho=6g+>LhZ#udHcj5o3 z<1%AwzE>gb)RiaG_n>X1;+HGHEM%Rkhzb!2i4tLd6)hL!m(FlRa>5znjFJrOlcTE(!yR*K3$hMH+58Jk7)-`>^q$}DyP-*hU+Y&()keqxU_JIYwtRfAScc-1_C^P?|4 ze^Suy&DCrxhkV{?N2-=DkLAaY^(NVPH>q7-GqEN~b0Hnj2miy$#&!r6A0Y;y zu8pRp9Dp9-uqXYgHHLO+DEcUXF?&+_Zs#Z_HzN#|0YqvVcNj=cN{Yrtf*hbSH|szE z660X}FpT76Aw_r~>dfOEefPnGx2W84zL3Vt@zELw$qh|gLwOIaDPfamTq_$EKA$aS zd*`9Ub35UPe>YOt5F<LKW?cz`lYU~U^+IHG|gf@yvTX3`T#WO5bZE?`kp zjbd7t{h3J`#@WcJ(>5dMlp_kf1rPN=%thdEm)*v-x2}QlyopY2m|!=n@+wXtMauyE zMclO@R;a@0HMo+pGP_N#$Ip8F>SMN$ZStNj$MpkSCDVo76_!P1W!(p#(mOod;azXP z8Y~+E^mUkmC~Ig)YiI;@czM3=SH!Dq%lA^Ask}uN;PrJMFA#8CJI{*6HXisBfAeiQdNM;}4$y;M%ZD3${2R!#D8ezXrO!N^bHZ)Mk zpEXJ3&g{_69ey(6GoLmRZUc*}9H|zCq&N|d-MxUUResbrU9Lox2uE#SK$GEw(>DLw zmbnMsko0KVGbl`L?mSbQER>?jiBm=2(D35HZ3$^%~lJD z+Ol{b(GmBkkCoe(GdkW43#-Cx6eny|&xh?z=p)`sbL|R}Kz^59)zJ0F>|KR3OcaH6 zLAy9KCL~fLMH%Yyu$>#o_ZlK;31UF5_T0vhNW95RI5WZ$26DxnJ9l~luBPJp;n$BR z{S2TTvNVhhdyGQSlTd zlq0=ILaNtvkxvRKxaqB(?FmCeLo$8|Ek=B*;*w1j=MHAQrG9!eyw-^C!oh8IgLcRd z)(ROu{JYx`QLkAGGWapsr?d{^4z_aaVsUTWVBoz;x)-(eXYk=`x@F=U?*#|9j9ffh zBW&Z9085zbqy@T|hH*ycwn9v7EOkPtZzE1w3|z(uc`Lk3X~W`KPwJFX47d#jG5u7f zLlhjm=&x^O9$9h?=Ro}jUKmY3X`uC#_Ad{*hfw^aw zZ5%x%DM=BIxPxecv16=9xc-alS8=(SPy+gnkc8NX==NmD($qJb|e(Vcigs=x~bFN6AQZ-%L zTDO_XGjCYjL-?;(miRz=Kbqn*{$;&Z&MU2-;aanFQaiL{8)K>Mk%s`$VG$7tgL}=? z@Zn3Dq1<`}bztzpkDlhMdxPeGLA1Zy6uz60dWYi)Slqd6odec({K%`Zs3oiSx37`o zzI5r5>Zw!jFAbNV8T=YnWr7D(o94TYL}2qG_z2k#ZNd$_Hxu}fE52jP78xn2R|Y>= z>h6oP{(Xl>Z}-?W#YU|&myX%kgriwwV+o!oH4V)Lp3sDpl-Q^!PZT)Vx5wq=&V%dV z;yF5nb;*Du8{OV~P9FI6uqA3LtU*mG9 z&KkscXo+;&aqn^(8k;d+FJyns8yNEMr2vj2;X3s@oW7^)!XjmAq<&QxCoX=~(P5FdFvLWIy7H#D`juZX=M0F%Si@&4LCpDL5fh653~?_g z=rycFLIuoqGB291Eb}Fm+e&4o13-MlYmbuQ$_U1p<9Fvv@e5x#VGXcJ_eq@Dt9?LX@+v-9`81;dS~ z!2-@x@5$a(SXI5K{Xg~hCyv5mb8@KJhkV4rZMxHNDJew=ffUwf;p6wJ)q%E~1s0kb5}LMPdpea_$U zX$b?gPXV&=(XSz9#=^q%33A+D@jIcI!*lg~c=-eN$LehdJ#n1{BfPtJUxQ`|hro7m zS3wMz-3cHK<{BuY3oiF1rlfp=ijYDc92oDGLU+`Wp^Ir~UaFi$X4_AsdO|?u@Y+py zx>4W*YvKi09#TJ#4YvR6^P=vHOHMozZDSL_cELBB`y{K2SFBof0aUad%G4S+V?KH^ zWSq!P&z!|63nu6Vd}>U^luhIp{v;1aZ~G|9wB<78PanPAQkOuT{rUpSfdl?%3C0gA z8|JS;oe7+**?oFM8>x9VpNK#(G&PD^NEW;!lT0ZPc|6tXnfoT@bgcgykPmxL32K2M23=L`i3;# zTz7Z(p%nEK-gdoV>l2c`onbGy-qLBLF z_Mn3$b2b#J%?rDfaOKd&;W~ogx&5K9uQ6(zTnA2f1RI!Qpsum&7_$;2Fbfu` z)8)$zxt3?~)KFQm^YaIRLBRl~Dky225poqHyyi19cwkaun%!Y0kj?7VW_Aht8;A%T zyRcn$b8`9sD}=1%lS2vWr9Ko7kD>QP-h*CeDH$0WoGFiSvSGkXUP{mV-~Vz0Y;#O`uXiVhJffAj*okDrtL(az$IYbX(*1)WwnnJ5^h6cywnQtQ9)B8 z)1eF0a3J4}6-f#&?Al{`pHB#X?aU8ESZ#z`vb{Ej?VNR++KerwZ(vZ50_O6?ZHFxw zaCW-F*pqVN!NF34h6+4z2$WZ>Uab$0U2eHrU?)#8Xc*=DQLxg&SKGGtAd{#!;_C!j zvgXl?I_)h_aOoDz!F)10I`rfnIyqS(Wi1fhIoO)c46cw7FMtF~lP;^6`Y#UmbGt0a z6QH0{EHjwQ%4GEdZjPhL(Mtrc4_cfn;DZ`4Q7w0>f{syG3Z!Ti^+RK0<5#a=TSFF0 z1`t}b-v9r{EbB}02MuX5e8wq6`oLX>d2`CR=mQFq9UVs+RtE^ zZWF6??aA2F`?nBtPw3C>OE+-%k%2S}YnLOT$Yxh*GUD@P5ZaI+*S-bNNPBl}Z7rM7 znh&F+sdu^^auqMCYiJ}+RUrgun@U;W!~hB~lpmg+m-he)H-15(H3_>LF+ewr9<0%` zgJAU_UDY$4bcZ(NvhS?P0}&|D{eYqt6%{?ewYTHU6mrzsKAGk)v=VSrR1I*j^PiHSEatpw`CI zT)AdVs>aD#&~}oFJQ-;lNCB=UJJOjd7Cb6t*>)@xRmSh8xg8bN z)yonz2HGmxF!vcW^C28*LtmQi-`^B+?}ZX(4{+Hi4cNB$IcSM&Hwmal1l+WF^S!{p zKAW>c9W|l8=9NrJ(-m`Nx zEv?#|v9j_yi0SOd$}c=qhD$`L90#_Rkj^nEVB(r6C3s!E(v7;suFJdSHp0IeT!n8`UR|xSFBiJrR_d;{Wc_-4uy0B ztZ_B=wOXx4)&u%+H5D>{pU`4et8yA?@#W&@ud5nzuH)o@enJVg0!-&F_P)G9a0nPV zZP>A+3eZktJIk33jNii#?5Tb<|E{Xa2YzG;E4yL3lVi~(f94F6#Qfw&yc!&o{3@r& z`=hqAl^uD+99byWkFfdt`E#WCEwdD%!^YZYFQ~^i_F4%xIc4i`0kr2r@$nIPK3?Lz z`}d___amGk%Fa&X^Sa@v;Q;;D8yCbJyKMDF=<061avTF0@p=5mlM@XCCn{Tl-rabZ z!870=B@V@Gswvzn-$Ldgi3N*ggERi(vr~1$Qz(a$l9Pu|*d43B-4A)8s-B*rksk060wf#xl?+}rz28kBLuv3y z8!MVn%AHWvGl5q%J4DEgPVW%{AqabmOzKp&Nsx|3_Uh^Tx->S&q`7%WdS!x_rDKaj;c^y4&t%56j!LQ{S9nBcIIi;GL^Lx!dbXFqf~ z<-tyx+sS>bNh9skFPEP1{~q6V`h>uPhly&H>+H{;>(;NwJyHQl*9W^zpdwUm--uKH zPf>lp?+|1@-n$B55gVC>F+q`MJ@>+Mi%%=oFA|aL`!_{u8kkbQmwpj_QaHX#sm5R`h{~9Z z7LRTne%5A&7;-#fGW{7+ho%a;a^LN0oI-1+BDIhc;OsvFOcd74xn4F#p+97ejT1X~ zcxpc7^G*j3{aZWMhmNlqYT4cT+#S#7BhaJe6!16ub@T3mAm#TbH6JoO=dt{OQ^lA0 zfk$3I){`pmjgyymqo>GNUYUW?Q^Z7GB>b9QgBMO`n{0-BAbZxvMSNg7sxfG~9lb6Nx3hb!dU3epFg;XNI zwU?JKPmoi;q_3Z+*HcxMtSL4Y{IMvf#%ph_mZs*DV2wn}j1nKVojaju_5sQtSUg~R zrZ(97B&eH{uibyofMXG|Ej5Su+|=)pcy78dgaqOIiCrA~_v<6$rNDYP!L2d^Z8%67 zf$r3ZYhg~VrE_=z`$@YqwjlPS2Io%`Kr@6+TICa4y1@K~qqnOR0o8)FW}rY9K@BEf z9~cq$wGo)^Im7GC%f|;;%2U*eXHXXk|1e$*R_3aY5()|)qT9g!T!KgqgRGi-HE5P( zTW!5n$atqdi(+4JJ3XhM5y8@Cb#*>7Fxk$|PMT^pb`w*ps|Cm$k@_g(D8LRF8sUS| zfs8H{WLOJ8aF8&!5Q_YqoyE~njRj(2j%hFN$k1u%p(T*yCg((oFf6D(KZL=Q%U_`A zH)U+Slu25JLZ&Eai>X31I4&=rlr4c^0jf!PLI38r4xz&s7d$=cBNNk${bvz)zh;;b z@F8ky`mEYuNF!I!(P>>2TG-kEfjRWkYb60poF2(T`3mLkxy-gxK-94ot>>z&?Ctr% z+vtP0!!E%^H>u@Dqu~3JCOLUT(RNmq9B3vZZyCLeSqAW$w2^DsNm;Ghe8zg_`4{yTlT*&$u1D{N44yWXMB;5 z{!myosl8!PR>p?I*`Z0e1(-Nggs^(Bfo(6BtX=%Wl*hShn+`)%RMX(oe_pK@H=j#r}u6)=?6($^g-zGv^zSs zRwOr9V(TJJ>IVsjoMqj%ZMQ>0wjR5=`aBXWi}YJo>7_k~a9_Zug3t*V7#UfcVNiwe z90fcFmC}XGw!QoJe}a?(Xdp@^R0c^pX_7JZ2ceE5&0p}gJnrr;jv7lUzR?;*EZ)Bo zA~vEHH)BIt*;16nz*DdvV(|Wc&dekR@|@X^`V`MN2!SO7Hs+{=3FwJ)l0oe9eT<>m zux{N4RpDI#L=Xlu&@u=+Es&Zo&mB}oo`=VQxhhUBE{#Z~FZ+**E+Efb(a+{!3|BQ7_7#08` zcw0zgl!wJJh*sGxs(wRwpe8#j>s^rg$X;F{S`&J8&J4jD?@Ogh8+9My%fJ#T^R^3$ ziiRUmgJbVohyuTV`=-3-#Ry6i@cwv2RwM5ZVa*d%9ajSVTNJxCBfR(p-aLJJkl&gN zR3X)}g#|a{eDUZ4L4*bHfgMQ_K%7t^&pk|y{L%5flOiW|MBU=0n4sVTh*0YQ+8|9Q zuL>V4spuj8!>#$h)l0}#-waDJo6)YS+FC>CbtM&dWwFG`ZROJWlrs8Dx;UCTx*HwuC_f1^Dvp!mJjOOCnMNCuxB0rj)ZKkI*G3!*z%r zLG{E5Aj4*)Qb+2wZo`J7NG&OZX1R2!VD-9XPGG;A--9rGgIWj8M;9;}29@HyfPkau zMo}SYP~O5KO<1_GqN5`W{dA;uPG+(| zQ9vFahLN-K@-`Y|rK8|O25%wz@D7vMLnP@_P6UehveLv)rVKW@uR#*MBls&uloq@* z;v@4m!8hp2INiE&!@5~R>luW1^C7OFK&MMwOsu%(Cz7uw>THM?(7bjITsiBWJ%z4~FJNZ9eY>2UT~?I@^?;JT zAod9nDB*IO*S`mSg37-sDYNOnEA^F*MEX6N4J4i9$3d_1Kb)x3IQ);5fcm;Iue7i&U`$v`|#eo z0aENf_{QcPr{Qw|pWfvkpjz(3y4MT=bjFK;a{SBdJM;U)JCJ6KqZ5dbb!pFMO+xI+ z2n8EVSYcNRe+x8R?+)p+7;jQL`5m#?76fK=z0m_+!?lEML*vZy4>0^IIyd_DbGJX~ zOM3O@&3Nf@=7gC*fMM87LRKFraGYIPD>`w4H%+~~{>#2m5I|h}Nl^jk_A+hw)kHQ>1P#MNu~;owZPUDDFa zVDP7C0-Ube{!IsvZFt2h{z2UU!sE3MxgL8o_r}%`Y&oW(C3f4_81T^#i!vt+ijY(h zmI3(e#NDIA53e8K_!SSOCC{ zGxj|JY*A++=i=@cjAECt1IsZ14Hl$~1Gb+T zz**v7Qi8%Am>Ln+(SM{fa~H_g1eOZ2YH4%{D9AOwF&dkvI6<_iNAm+sDHeIX0G|X& zau&8pOSUP+SPGh#W}RX`JlC!}nyEf#(k+8#E0ibRe4m;!ZxX=WZ$069CdCbm-zB$m zXUr}aGeNbYDU_oWAA?1&*gk!fPBq-%h|X2bWOEx2%aJMbKj^en2U}pBSB}zb{JM`e zYQUUyos^!@wt;D)XzAGpb@T(TA!!RhSL|99PL?6=d$b7#`@^jTUSkUrp=(M9l?}#b zyZEpr)RK8cxuZ}Ws6}*8FJBH+2gJ{mQi>BxbulyEn1yW^m$iiO!%J-c!$ax>h`ur+ zp~-gb5>5^d;}-YujboSs*_^M0+7}>Oc4goa5a5lY0yUOOA_J`^s|NO38+!Rp=%olH z4Ha%7(HnK@oX%z0 zfKGc}&UNIrO>;@3ZC1CqgL=zTn3HYWY8v!q7MxK?9CjiiU8t90L6YF0Mc7(NOAC{J z&}o8@{AV&JWRVYg8?@=ayiW950I;(0jpsqz}Xl%P$Se44w@XdhE3O$ujuTY zUoaN;Y5!>*4UK*WXuh>Bn)*KR-6an2knqRY0fd|WH4(Xx67=>A`VEAQFREpe5970J zSN!i?ZTXlIb0EFNdF2mSSZ)W!M&JJ~sK`cl>(HO6K z0YzYfgc!3a{Dq{!V(zx>%d&!i*EJKyzQ7QWcaVe=D6~!Lr%5oGVG-dsbKE3^>50Vh z-!omI;!j$2D3HAp0Zdb&1uA_w&-UVTU{!IzWDbZ46;YaYSvUgDLh?)V0!jp(T_ZF; zzP{AwAOf`r!I-iEiw>?Wv=p?;W(ULKC&#j<`^zZ+?pGdCXDG!dXa7Q0Vm>L}WnA?m zCgvV>_M+Zl-RKEZ{}+KFMck95Bm+sJTZUM^`~`GpPG|-)Wiw!0yBzKi{J<CgjAZm2#*Pmo$kH;! zbM^S&kumw-CumIt592LRU*gmW%I&`FVQAt|dF z1n{*p_veL@F$dq{U{?1z&MV89=XMI zIiZG{KWlWZ{+`jMnaTBwM@6*W3R>D@G4)TO`)OG#eOnkj@ga2jA`^5%20Mt&2aJjo zoL|58wD)>xvPPaywS<@zuBc)}olSai-8|6tsKJ%E(;Xs5MH?F)WaY1+sKUTd89gHE zC(;a*CQx3f4h!W9NUK0>^a5pBX4`5>ZS7F{9;|P#+qbRS-t=0L@&q9n+uqdY#J+^R ztDsB)1DDqoAH5ggR`0;TKmgwXPR`w8VnhlDtf1vXUv~3c{ra~n{z;I?zNClBhc8*c z8uJte5hrM6n(VMuKq22E;1+y?k;Lc*L&i%>$!t3Ts@v2&z1ie+ky55i%wtqNQ`}#% zc~TwlO#lfy+=_DS{^!`KuX!87Dt(%LUQPG*-2fW&ax$3ql?)Bt3_HPqm*x1Kc{ zRhAZE{U?!X56!n7$Z=S4RQE1Yus_NqUzr%z7uV&*%XA{+NX8g56ku zMHq8v)?E~evXO#9>g6#yu~)FFki*O^PFarj-Jz^1i=pkd!$*uKVc)ao7N`Q911#;U zfP=M}v7-=03SXC~wue*=IOb6vn~Z*w;+(}Va)4X--??9}1a3kZ-XG6v<_p)E-_T5cjXScmv?Qlf zWN~pZ(2i8bM@G_MX3@}HP!w+du>trthCoeWhHi7DpUg#Qovs_j$G-Y26&$?|?GYGe zIq~8%=`HIwN2fwahAT=sklE&2ZvJ@9=AqRp7@K zewItIL9j@6B>r*2Z`J1ZM^0 zs~3l>`Mr7AcU-|Qu*OK3Ep2CyfndE#6rVt?Q8++kjN%!ygKJBWnWvmB-TB`Fhta`!xjC4Z> z;wg0W&(_sK(YzHy{pCnKB1F9hoc~e4GI#%&ePKBwTo2pW9T!1^7nc-V4dWVIOjZZ2^H@^!x)M( zLbwSZ@uDFuXPr4`eZ>on>UU+c%3yWCc#d`7zEyrQOC`b8Ap`BtA9!>TXc0<8FK7y4 zS{3MJRt)nBat=-1vRfeM8 z3p8s0q~+TnLCC|wK5ZT0 zMf5ba3m?F&o=?bm))bsYARr`Va&nmcyPGC&i%pHTQwE+x$-l$?7lu*Mq@Wx!>JGkt|88JlW9*!UvU1&pn9h#mC;zXnE03o#{o4F!>u%n8-``*F{4+D3F*(n3 z?)$r3*Y&-=Cz7ya=vG8TC6Y3GCF&_Utn{Ff4TylkDy5(@zr-(~A8!UYvh#o@dJka2X4($kUD#c7;PZBqzrU z5C`k#pCS1+O##QSfs7fLm)qzQyfl1~_*baKj5KFE7x2uTN5rAm?WM88i7DD2aV1mF3=^e5UmOHPt0FwdH zut2?x5rbmYZ%}33SMydzK5_x5?XX z-jdo(%Bh~u4PsSns z=LZFVXN5Qd@`160#UK6GzUQbq3@Tz<%|;vvAnvR@%qL<04|9T7BVch4u4H7U6ESIg za#BJ{iUUujrS%K(3d;&|N36tQAT(3cQ)m1wWx=Z-qCp(V?r$1(7!n~|ZIhdUqyUQ4 zbIqKa0(qb5m78z;84hH;fNcRXz8w=&16kHV!9W~TkZ(6aLTIQ6Nqj?77m3=^6*|m` zsEQwfm@vf`sg02q`w>^LOHfbpty{-HVNUIYJOfbFCy*FKwu~kMlvL|qHp_@qsmpg! z6-qS!{hog5>05vbzeeu@{~j`j25YBoCUZq4m^7kdLcxS|CWj&kQUO?sodJbH#NW^}fMks1aLdqrO3?77-o&8gqZs$iVtg zd%zZ;2h}klAi%-6qK$RweZ?2mmP0YriVQFk2#kzHF2|Z5v~29^n3=f&&IvA4fXf=dHvnxQs|?CoQc?TDwTaO8 zMDw|RVT>KW4TAQkaBU*=05S?0DZT`=Lei{a{)(uU@pds1MhBtd3D^t^g`J7U8A&Gd z`@)HRG^Qo63lLReV3||XinAJQEf^l43!cJA%w8`Pp$?s>S^1S+czfbVTbLLtnS4Te zU9?*qM5IdKYUF|YK#yTM0+~8qd$8$T`}CH6FEwaKA;vdGX2F?-u0`)VCKO2RfiD}( zJs6(=TwV0DY)zq+?(yG?lT5yT{VI%O02#<5bXh;JFNTf=7i}$kL{7@V0}B7UCq#?Q zj&TW`KrQRyRb!bIi}83%JvjOR{SN>)CqpApc#?U*HzhBlNCv@Myb~Rr7Ov^EFTpVr z`6lX@OO^@I{lv7V{Tq&!KfGO7aj_T1g7)yPTW1JnmZW+Nmk;(QJ~`>2AcBJny&T!2 zK{|f?MshK3-?~MI9p15$iZz^--VRL?^j4oS$O($z393O1biGA}>Fgas zy}3>EpZ@gIc6mhcCC2N#Cl5w_+XGJ*u0)V-(tsyYKnph6pi-=XGE-396GPX{-%#`{ zF+PEeRQNImEFr5D0D-RN0`MHteiy)$OF{W-GY|U~4VJsW5t7r>^&L;4*}3DQ1L^%Y%{G?AM6VPRy?FkPb)TAnnI0g?p%v4V6U1xx|>5gJ8qg zS7=qhbW9eN&{cFpTo6Dqx5kh?m}4< zMT3V!Y75zRBU20G!DLF`!otEoy8*rkIaCdxfGhxhsUwJJHHeif)n`Ciq!zHVvrGH^ z1xn-d6{1R^K4TWxJIlrmGcR0;MEWW-Vo>Y>RjzT)emA%LS|uoVOsT3rkxF~CiDTp<78p$WZo4wMI1B2j!5<5k@S^A+V7d)XNW zIE$(Qf@IkS!D5pmL-n5PJmL!quqK;W7#D5^@5XW$9e&yfvY|%o(X zP|+QRc1+^rRmhC~^MrW+*|uWuvU0U{K`1dFuu3UEV0`}iM=qJtT4Ibs?k?K?(yg_y zlqQSjs2ZSiP2aGkYRaO-eIy+Y1uZtEWK4@i5F8zuY#2cj`GZvof<=A!r`IZxWZvX$ z7^{@^x?ddQr(*wGHckLzEDdc^j6N{<;u!HqYiHs>P*HsOhRDtMI^hY>G=(+BFOV$I zRe!`eHp)8a3cpez{PD;VBeZdcCV46#!&C@0G{3+%nfXe0mWKFzw)csbNYFX585Ab)+kRp33wSx(>vbQfwIdlzeF5M+pk>6hJDX za>*JRz`4oM52BUHd0I5Mdbz(H-PO|er2m+jBUF($LPG=m{n^%E zQ{NK#348?=zzl{Uf5Wf8vLkafN3=I3DARUC0Fk6Bk*T;=uU;Lf0HZHpVgl;Sn|2Hs z;54+gq1#yIvr-WwhpUN6p4C(L(efP7LTCo%phXXyY!GUPQmY%i-7obj1(4ZteHk4c z{ZZ3|_q#dL+0IVG!eR|#SdPmN6i{q((>}h@WjDNEWL4kWC-I}E?_qbmKN9P62E$Oy z5$~|I+*^g(0K!1-#5}6?nH(=d^74jM4UCSOa=d*$uksyRUB^x?aRHA%ozU75$vbf> ziPQdPw>{D5;_BM7_v~bqH{(NNCaKB zEa3(u>6@DYG_s%ur^-Zar&2I>!|M_Ayox_8I9LO!tV1P`Wnf-=nzQ(00UMDkK%dBQ zYhM70k}JqB*Jhe>t>~=u9!>AJmi`qL6;{Ig_wScD87)Ckyevcu(AwUA1k7Z_*%Ugm+%0&YQx!ZxqYx#qJp&%gGDReaF1U#PExw*OJ zK;R@#q8JChGj=|!>XbbEa9OEo6s}{)UQ)d+EiFNXg@u{}cRn6EJ8bf0aPZSh{Q))h z%QQWbM;$$iO(u;?6WCpN$C z!7BPE$$Led4RqKk=%QnSnPo5pCBS1pfiT&_O#Rcuwr!<3Yl{X<4aiW`K?M`PxKz72%z zr-eQ&E3*`nc(HGCu69&=Slrdsl`4~6+t%LBzQ*^+teR|kjf1VN`q87Sw{6?THM7fq zB)0sm;o|CeBNbS|Bz>-$%m4M~fj)hgh`6p_zmBxN2hykIDk>^*yFK|SUKqvlw%m&~ z`G@}85tSEbKx$cH{e9L1r7=mgWl+T(r1oMO;w?E z8AoffrAl?3U58>vh3{fo%UGslDt_;DQ6(UYYXa%>v^y&Tlan|3bsEhE@rSE#T}4A9 z)mrAY17Or+Xn80C{ses}xTWAZaoT4hD?58Mt*54Jx4fd_=UNW1d?qT~FKxrNtzPvI zk{TOP2YY*J^18#Nd$Q)Q2IPPHt*1iaqta6E7&Minw_M>Zerw-!S3kZK`t}v;)q0=z zxm1Ynb#(4><-(|z(8LU<*lCISiim%r;-0XO(zy8eaLm<0ib1cb;ldyU*Gy3a@g1{0 zcTRBkQAl7gPOC6b;&u5lkbMAC7W9ko148^`q1H2cj^*u@;1^R=T!|PgB`d3IWtCX7 zGE4kw>2-XhNKe3z@n6!*%e`Cn6i8DLbdBf!6-2`W1ef4dEs1fiC^a%!`HR`%Q&Z?oVJECovkk`HI?{U;U0`()Sh*~l1~*n z9%`{mp=!$7TV6rIDIjC|{g^K#IRS2(2ag|DPAwT48j?r;#3)-2y6Z#wIT;x} zM+?qpgePdL_;rR=V0OdW%E}MgUjRnSOEOC^`^Dqv=x9-XC3=NrV+(2_c~E0JRaCx8 zbLht8=PT~128hCChjzydWMc*0a&a|_Me21#)s%O%bLT=qtx{wGk6^a0OcV_-0N>>c zzz+}8(t=?#CJf^t7>Y!9OP;f+bHXFjjiwUv%dNM}p7cd)O?Bh`UK6N9!UJXb0*+eK zH1wtQO-)+h1shLa1Vw~~_F-05mZ_+&!@01~&}msYWYb8~S6#)f?8ulDg>8*GSq>Y- z&n9}A%0yw4CpR2_ph8?TxoduYJ{Jl?suEy6keKYrL&o?C93gzH)Qk-OJ9h*Smu4o> z4|Aw9S8wIv;o-7rN3if@8PSB3x+j zPm6XUA|ht!Z*kwqtT*NN>=zMT6bI1x2I1R*I`sx-Q^cSSS5{UgNd|xb9x;HFgLJyJ zv71qh0w<~tZD@+|7w?)i+(`S#8Xct`DuLRc72=cOK=*4Ea_h6X+``~b=8`qd%*x_hwdy)a;GiJjBIKWxBo4LUt(cgb_zK~` zWRSIT{`p#S3UbzKD0{Rv@`36$d517bsRpZu zt9rP#WrKo(AU-{02>}Q$Xwy(92F`tFip$IML1*LQ;qeluH0T3646-#qqC0gIv0#E3 z0I3zQ##anRHEt&kaX2WV&4KB~m#4N6uOVQ?e0J<&0jV$kVcBM)2m{7eiFWf3mn%@P zb5qNBcvf!pz|`JSKjM`tR|Y2)7Axw0Vc~Z_$0E0(Rpziq5eU}0B23jW7~y0=!{$ZQ zFGAEAoV-hF#TE?W(t;?nlELnWl%5Mt>2Y>;wsoO6oCn|j-OwQ4t*E1``_=Q#?b|}M zP^{tkjlz!`h%upzXHbs;uX4mYd}&PPrb2GsblZbYq^3`ZvgedpONR^&1gLEis@2)4l!|U ze|bfZxD7wUTtGLX4y-2v4oE)#L+_52lat!8>$|2V&;aaKp>*gT#7yY61+={8mX@S~ zf>mv8ZMX<>+qa7}&!SKMhgffMgnn|7&%8345QO+lURj}m z4*?8xcIXOh?d>n05j#n)@Wc;e?ti*`l4SzXfMT5I{NjQ)=|f+n`23ibS6lpnVm`On z?m`R_kms%juK)WV3^1GwGaW3e=P^{gXfxNJ!pUMY@>YM4Ah6??_Y=m1I(ik%)SVjCrhDu@OCwKa8GkTvw{@H9tu`rC9hjTf%>=Mo~0c^AOL6OhBKB9@dc z2Fcm;-*7Lf9kN)yF9L>$-#!cCZ-|y`mP*nYyAL%b-hGmb&73dRCWHR!tjakQeeOuT zYbD&NZ2!<8;@K+UjSuPqD58lvmq^1gD6 z#JTmv{ipg>q)YFLfuNkjDU8Sw+W5KcI_j9tW07XOqLQ!Isws|lz`(VqMkiHZ_=uV^ zs+;M!H*LqxSZbOwlY7!{kEp6qFk2-evO#oU#o&(vj%0$TY5j^p-p24v($kUoBA0CSKaKaZ*JKb zGeA|ePszw}LH1phE{7yp&f!ME`TV{=S?rnAH&D3g2(TbEmCNZkZ*+m(6R}Uk;HGjzdz+`mkTzdB+5jhbPrGvA>#Hpdj>6G{|EoX6PJHkVR#MDILP=tQ%2)=QnnXipx@hq(HH zP}-Od_|8IOvX99WGoIgv>#pk_C9--CJR?#siTeul982}y-?WO)9t)AhfRD6Q#w8T= zrX_H4;|Mmlpt&Jb3EMbDh(lz~%VY!x;`1RPQ~eScP6NSARJp>l90C^SsS`0|C7_F^ zBsUp6AMy@#sr{=>hv&h;w@L4x$pZlYY{#DmsuMovn9R88g9|lY@ZU); zs$Zz!+Uy*EjPUEq(M)q=ktOXQ?1L<_A8P8An~C%89NO5^G>(UyN7U z3U^!@jE*(0v7QoW-F_w(#Z?qM%Be80XgXc+&Edz&Mn)XC60DgfB|k9q_v7Ec-}uw7 zCU4W-r>7iTr}I@_mexznEUxpnFFgMaS#q3CxLs+!#GxxHm>UiXoAN3O;@rqV!MdF# ze)NV-Mo8%uOA~jLJz6YM8QlTGcKW<#ZQz&!4*)wRmuN9Re+N}>a;5$a#MeptB_(UMgKir9^b@lcm zjN@Q=^H`r*1%UbyOR)t3kp=v>`L_LM+0J21b9atgeUh#%)o%+C4}5T{Y9-|E zu5u0j*XYGtsE70UhgtCy-lMGA<6U&dkBqz$M0MrL3qyZ{f9Z=21f+z{n-?*K(`UiE z3Y)BC-{kRR}<uZh1 zFS3gTKjS-{7HZhEw2JyCexr0F{<5vgNo#~wioh5C0JyhXmUJR7C!E)5TWSWwf1_J4 z+)Z_!a@|Buz$u&`9pt{dN_4qmOMf!$QbAT-^f_2$&^k;lE0jb|#Z5dJfg6}h``GUl zz8n}pnu1%Q7Q%&uN~w8V^6mHb+t{$jol%2Xr```7)21Miv~s3tzuajEG!B=#S2#C+ zNt6kOj_AFK|tN<|?ph;}6W5k(N`=yCECJJ7TA@%beB=(XgSn}Q*=AV9XPu=u}rL)Fl z-)MmVKX#9{yW%02H6eg`ntK-7Kn^Hk$r}idlaL zv-FCQ8%p-Z{hi;!FwtmF-{?f&=sB|tzn#Po$dm9ol3SVKcQ&zEX_~W2;Ao~_#_<{d z&N;=MrJj`UOE;ldtO=7X#i0OyNAbopa@9a(oySgHhyWFU0sR2yZ>p= z8%B#+vv=?zQVsWq?8Hy^wG=*;)cQD9^r7!oBnOPChq}lL=d2{u8(7$-FMf5ceOM%+ z24f{GDRor7YPaD9L$u??w=Bgkf*uNhIU`lklS^Js9~s-LOtOzN`!StQ`wIAmk~)#q zU=hx74PIxB_rw*_1q$OE3RRg3St;M#iU`#))7V;aNXE58UFEt}38k@EPD@!%oY5G2 zjMOf&{6;B5q3Wht>f~~=kNBgn(BvTUL8_*2H^0j8d*%*%{N7e}OU4Si{>AnHA1T^4l`w&Qf1LN!QC;@rpOMPD+4rvaaad<8 zt@C6GY-Z_WJyOH0sBSRdPT_a_W_u3L2P6|N4ic01QE7VV1VREWfSz-~EsFv-*68ua&BjXag4pDWa({qKXE zsev{$>0RMETSQ!eM!zWuF@Z+eU=6A_#?u$_$=@1Y{*aC8zc=o? zH$t;>C)MCN*R0K{G7Gp0v;n-D<2~}@VS@t?J>e2!#j3D+T$uaD7W&k(mDIq@gf~$a z2Q+kS92X;x2@{TVyoUrcMPej7K3X+Cs4Vn|VeT#prUj(Kwg0B%yBks@*zmg-bzhsg zNNw%ifpq0S>Dsc*4d1a!34t%^6!|XC?5oTb&6$*xKjAg1650!gKpWxsja{B#u`UTr zgJegRkt^;xgoeo=-Y>@7N-IVy6urZ`1442hs;AB>}`WRpSHA;T4#h%B@ zR&h>d^q0pp!Pnt|fc4lTslIp&ZO-caL(9q z5;7~W6T~Jc^SHBxbd1TpRZ5^A^AQ%lxAon^&UyDs$S8r>4&+Q^xQA;d0hrk=P4#~_ zpSzK!mfr<~8uhp+b!MamD_=q?0@rv)!H4wshDCg|hxGscf5rn9;K7h-Xo`dO>u-ub zEN6A)5-6@SI_YhUzu)tBwK5Gde^G?rTD@3;+l9vkpDFgLoWrM+_PgbU;OS49S&AgL~d8QPfwP;!P zS~>f;>Q2N`+}3|?a@yH^xgbWXD*80=@_K~=-@gNt0SPJ9naDL07a4jo;i1j@)Sl*g z{o*+I^*sB4jElgf@L=pQmQ2k9j3>vQW~XIOEzTF&$pD^*p@e*#^d^r-AwbgSYjCZn~a5%XM&vng;$H;nH+Wfu?8$Wip80NAR}$m5JAB>($tjH8n>?zw1gL}J5~3O#^7-fGZ4+j88dJVadRI8V zeEzX@m=S5Dv6&B2agj`lC1j13k{Ep+-trl=1c3cI$-y1(-7S)qr|XtVZMPPw2~+=M z)E{l`czlvp3s@YU(zv@hxl~4thni{xm8Ntr834n zixy6#%FL&|12Xk?AXu2-&wXwbVbTg?kM9P(SVx(Ms3$CPdKmdSsdvRC2HkTQbNHBe z0Dx{2G$1*@;Y&ewd$9^4I}Xhjr&}L~&7DJ?*M$roB+DX&qda=`?7>Yf%l=CC9oC5h z+U?hy8wja$iRZ%(J=B>!&q4F(C8v0PeXAk(+BKr>wq5AL)gDSBM$Yg3Ez1v1?p&9- zV@NdSxpFnBueiJbv;>$-t}}L8MUMc21EL(FcNHVlmeJW;oAxyBmCB{RA5IHr?3yxm zDvUpha`_tDHR@DC1*%QVtrj^!U-VBSH(&ilAR{3m5GOh^&Tj;WJ*U6f>PFR3t7;oW zHPq-&oqhAd>-sfQlC1S*F3RDSQk;mH$D{wv$_#&2b2q`wi$|0Ikz#9%Gme=Ge~v^aZhQm&!8y!@nje#pAF2F;^OiNgGBESUCB`-0uDAFL zj9oJ#BH}JVlueH%SYV})MfTKhwwVmhqH?zSR^t@#y5DJf#a$DY|MNEfv#lzMkU$4x z#VofO3oYTn=L22SUVS6RSx?}R^b$|pr$}zrhu!=@KN%cFHY()beH?2>rvBi!bo0)# zQdmb9S}AcD8oS0T#LgbgQCK3+?ufhIsQes8mE%>eze zs3Qx;RZ1p-r?8XU3v_f%WV9Qp^^MNzf-#l=XjCz{AW8UQv-koJ2Kj~j%qmS4o9661 z2r51gj0%3$mAs*q)VTZhG{_0Eb9aE7sNb-g;_J3C@3!s%TU95=%(%_@WMD-UxTrfX z!R7C)^T5`M6O8d=_ZfJ+ySVbR@MC7D>)@(52>Xop$3&GGlYp0vE4nLU#E;P#Tb6C& z68~O@1M7!JQ`mw3$h7{{s`aAmZPH8HAv9?fqqBAch0Dv~G|$}xI4my}oPbwH5d@18BTpXT7h}4|Ka{3j0Ymhw*<)` zN&VG$!kBZ+-BO=o@KH{uv`_tncOFCwWbTHiL&xhY4~;F>@M^d_i-${QIoGsFD#IU% zTED2;J=5+rpJO9cesP$$&@dW^@{#@?)o*XrK8**s>b{Z<@-O?~AdM^&%D^&N;{0}T zz38f9o6CnS#tp!&4(fJSZP*CHe&CgXc4mo#-V_O|_75LhlPu%zncJC$&l8CB1bpSn zSr53}OxytMM!-$0&9IT%)wE|Xj{LQiDw>J9U1eQ2NFeJ#Mf~>VZTX?vWsb`A-cP>W z;wSAfga=zLxI&2$DOs&NH;OuRnjqv#Llzwceb7K<5{N)C7O_^chN*4ei<67feH0_G zj`Nd~-9D+5xVLazyT95R4#w+H+txFcHbNFQY4A!NgV=ReN8fKF0)}b(C2hM3Il_ia z=0RHv4qw22_~n$%M;IE6(w*n~op`SBD?rZ4vNKt2MRadkEXUYC z%ZvS7D=%;kCJNB6$Lqe?5&42gbR6UN@!0p#ngIJje%{w3%YX0b>;i{;B3|iH^Y=s4 zH8G*V0_I!iM_QM1zU3|PdAhwe845*~&U>zIAs}~bKl=7)!*VAx9_64IZP}Yyq#0)! zlW^30Cl`ltV~VRwHeSsuo?(0}rXnWZN$)W#1)3R2E86Pk_strh-$hhZq~OsQ?i9{kB;NZ|+_9YB{)q0^)OD5G z17hBsT5G@!Uu-a*-YV>XA(+poBG3(nUro-+vVd>QqE(yJi z#9tvM+JE`*Dd>4GqO%M1$iH1y_+%6?@6)lv)G=Y<-uy%W1L^oY zL%qpVelpq)XWe}e-w{$IX3qyJn(uXTieap*8VszugNWy+5RbIT%HS^B36E(~f&8EA zqbK5nl_hj*dYzzB@-h7^iPK~WoT47_h6?KtAFi*bd(OPQZ3>bW0Z8_X7-hd>e$NC+ z9L*X~WhfoeT4^d`nVEHmRbssdz(!2H=ASOvYV@ew{!#ojS;)6Hh8-^T6(njAg*k?3wT`2y`%S#r=4Dmg& zyJl+(75K1zJV$D1iXV9YE{F8nlDW;+G;ZZ*-+qy2L|JE}pr)UA_WsuED<9VNX83zU zFfovcaV@=@PAs?Da#bUb>0Va6sCItO2z=6q`Zr%*7Xt`PQ|8-g=5;SjtGZ7Sjr|?! zmY{u;uePxe-5rhewGvK1>-dtb+KD9Grzp9Y6uf59m$b9Q)~JAMH$Ji07^=jgxa)n5 z+D!>Lpw;E;E2<2pm+fw|5wTz(2_Gn15>z!`k*Q+0j_|&`1I_Fy-{vm8c|19jwCd=c z*q_IP%$EwrNQ*kQAEQX2X3RsZ^Cl*cn(+g|?h=n+e0@ejcj-VS34&e9-m(17`&l61 zrfvJF;+{hI>KwfDC423QTps+f(om9gPrGBZG>7rPIR4;OR-tIVBA5EW2hqaTU`4&A zdz>ON0$QsD*_V|<+s-vaYZyfvT7K&ucig?X+K>KH zi?qf*E08bX-qeq{KFM4(Oug#SyPBJVcRyB#L-%zLVI@`25{3(icTz^Y(?v-UOo9Yz zyjh=Fv1i?U46=a$0~YG`A9icRJCdPWD05?xg2z38`| z6BaJgauahC2Od!ePqQ?}G%CK-bEFRq$?j*(Ys_hA1BQmzwnhF(F3>OECA>GHckBkM zfwf9)msOZ$K&gF{N7-zu{6_druvGfJddj`e?#TaYwa1%1l3KgljQlq1>KTpMBah%MEr=uB5lw>m zKuj|0RXhI>_85OmcHFe`eNwDwvmvwRszx7X<*%{6#lv5t#_R{v-^`1H1b&7D0L{`< z5di`;M?nk9ZRrbT@(=G=t7nwtvfZeBEH~}qRs@8PY%Zxcd^P{nW9oJl@Ziw{zhA|+ zYFyvI78|hjxOIP>HFRZD)qo%m%vmnf9;^LRn3QeRqxY%pkDR)eSCkagO--N z_)643m+@=*=1BH{oFet^8tV%1_^hyV1RV?&HKO{&1w*IQv&e0NBQlWVqJg z{SoHwp%A~Ud>A3EVsp3nV{Ov8qM5wo%I?_oDyy}~ucO?qy3t6>2@8gm)RE`+(@dmV z?4DqghOaFmO|d6wzBf`4y$`GUrrnXN?8ia|7C!GOyShjPzFO^KM&U4Kb(CQCrseS_ z=2>W5_NK`4DB(khyw+KB5P(qDqsW~2>9Q0lW|UyjqKoqw_$htQD@28Vxjmz%%PU3v z9X3iSq(yhc-qIhRVy}JYR_zVCj%p;8uUfz6E3B0TZAV+NifNGrP5B_2f?_n1DJVLt z=cj}sTrBtBj7;ytt`{*0?L>4gmk(r-we%ySxGZUAsi*e!OtT6wwSbEc-;xYf$4PU* ze;fOq*~3OHCBM^}g@7Co%G5W(jrp@dIQG7sxh0F;TYE^Xf~9dfBEP2f)(P}{wQwkT z&XkW;te41Mf5H|L^7>{;>?vK#pO*c zAb8pjqF3Z|vlH%CRt6Lz%EnTE>|?p43qn_TrZ{q|nMOe4Rmf z=}J9-H9<#wNhD0mb+-`O8Dy?|z~Q4!=^L={}{`p+cvpSFu4&3p`pK069tbwt0 zl4-w<`4!Yzk*7j-rH=NL9+X3RB}IA#*11qv{n+xgMkTVX;hfORHkZ`Pc7x-ro|M{a zr@7IjR6wQ2X6!)VR_CN)U-MDXs7<7?x6pt0ocjL6E>PE)*tdUMg{Nu@a#=uLPbyBl zV}cx?(>6hvYX+P*ZRSfDT0=_(@N_OzPh|gT;X23@MxAtxy`=*2Iv0}}3;K_?)p8u( z7$dH)X!T(FXyOk-Yd9%)A$uWcc&05=+bTNBdKap=PIa3MF-@cwqZd!fOp>PUOF`k> z0V~@og?mYFfmE)1YK_V(ermEF=Q$7&~6?jtM_OVO}AnR_q|oZ@DwH zw;1^w%*B8hs6BklL26w1&l711=8EIr05LdGide{# za%1i$wFdcuZHkH;?&oJLIhOrT?}%L4OcB+t;#gsPbUW}3SMi}sDHp%oJz(2g zp%;+qxdFBgUcs3f5mW<~rW;mp;(*7HJ=;fbrMtBV|Ge?- zHv%`#shg)0aQMRH4XQ(FhA_*#Xs@Rpf_ub5>MyTpOOEAVw`9xM;uFc6pQ{7UJn&|x zQAykge`;d5FyoD@&dCnOK;CkvekIH>08sW2S-f*_iM=`iZVn zVg!n?EVtKRBP9JtoQ6#yUlusF>gG7MvWRX#3=50l>#!X2^zB$NmDb@LAy=7nSSQH| zL`mFC%z2yGf4!IWp*>Rl3PeBvBS?{}f{aWb`9OP{V6OzyGc=xP+NoBDa@jFT+O z!5%5BUJ#uUzZ~oIOAxXmAkMW*7V;>|0LCbB4q!ZyBE9_bejWdr%=9Yzd#A5MyRec(}5cLjvR=~3z(&r0oA&`*!%1jLdYXvta`@(|0Ea+$PEh8=0AFd zJ}G3?A&05ktT*d%t)O2~)hp6fe(%65vvTy8zOI0OKhnNMo~u4XEjM06Kc+5xm)kCE zN+&3pFFNn{-lOt{%s#ZPBI_ysj8QmXYoGdS*{j}*-`x;h2|LZm2p(8%6bSp zY)IeaZr739?-Dn8Cn;A(MDm`AT=HJx@%%1ieQo5bTW}F>f^;L9hiLdlH+hb{hJLk? za)$cg2;{+~5d)ejDgl!Ju@^*!o!gEcrV`NIX$iZl_yKynP7{B&QltggFYm+>2W~&o zO5uYZ6U;wL-lP6o00TSM4?9dn61j&tH)tzqqU&M^^3RZ0HLxMzzd^`}u5AqQN8*4E zvbK>w7-ds*xR{Y|xv_KdKG>32GQ5oj-lg7Bukfu*^8LBF`FxZ6d|ADfd)r;?U9c*Y z14TL~QhU+PB~D~s(7EiN?K7h>G5wve-f&3h`a1LW$ouIiQ{g4jCglg5t3UP>;JD_% z=A0#GlgCf}&SuZHR;j1hz24i`5sde1v1wf=9x=H}lDG73qw6v^z620%c#8!XpmU{# zqT`d{120VZ5TU(f3~y_%mER&XBVH}#yd(7`Sfc%Cp!mqs%hXFJx0?V3Dpg0D@q5tpXq zhyJf|M7fuGd*byzA%M#he+P@W&l{gc=|{ZE7Qwj^g`r$k^Kk_kSsS4M9pmlT3f4x* z#eB}eMk?!p3KCF(T|-sMOIj8j5FMMqhf*p!X@p)aS+bTN@|MRt{aI0fq+sseKCc>P z_>b1|jvhT!J)U2di2_6#+2D`nALgR*Do$ie$>;*7qb2gLws#6gcbExacebBtZ$o#X zd?^D@@8Z#QXz}XA|M?$tmySEaWFtQ`1+LDQ#bg`br=y)^4*R_RP4rdufud&VbR?Yr z%|8$LK&-3$#X>3se{}t$_{sVOkw%XDTFa9zAA*YkhM;tP-iR}K-WYmo-&;LKJGroL zJc(FJsC{0#qncexpnG28z9;pHbpydMSmKw^_-~ zt(p8I{GHzUHSpSQR?nM<__zbW=L3R=;AtY}K+4FRuHOfw@*`vQ^K;$(@(GwZ_`Zo3 zk&lcB`wsS#{1bnM6K6T;owVdF5D1NtR+Ly*4LXiopsNPxs{ztzy!GEj#x$^0ustS* z{cU>3S|=bjy4m_|=hEY1Y3h!nmbZI!s1T9N8|0de}SY2bEDMs zei>_poN~+WF$Y6l&;B2lurZ_3tNVRuxeAzc=i$g6wYYPh?6#8qIbF~S zoHdD(sL_f-Py74!Cqz;7XE~Lxcy0buX7J(T9J1i zZwb&4QS}^GhISn*()7xz??|5gKL%`GU9OwFzlO7d7akBxNB#1$*S;gdW$oxg5Zl*k zghsEqRx_;c1GQ99*tUP5mi;15{vM*C zyhe(zp;~4W-OE}@Q_2I)J@EhlC+gc;jl)r|Eb2EU^D^ZVEKs^+Wy4 z2Q6fu>Fh(8JJ%qDa|cjXZ#zfzleY@k<`mAO0^x5V~)kb1(0O+V*G1aHAa%MB( zqN=UV8>e5gT+DTDv3S<@gi1Yj=4)wm3(ooe__7bd?3d;J79zm7RB}ke>_Yr%2_sW* zvprB#c8dJfQ5A#v9Dg~E#VmjEn=@2`b2_|s#EYGgD)UqU^-p@B0IS3CQHT5Iz2V}D zufn)$H@uEFym{0FV4&kbrUboyXxr-tbR^LG;tr|Zl$HLoho(JQWxD4~ZNj5mYsq$) zoyOtc@=zC4Zm?0`Nhw7X`(E}#DAqf{Gx>NDiqvFHH$Q=psiMN8suIevk1;iD8wNS- z%7Zu`ofP(c=Cmx-n+{g08I+O|RXWz&L~6GK-;HnKTLUH`bobxsvfPk;*RJMN=rB9{Uq)9<_a12<_f*roug?8D zUm(Ycefx){Ej_^y*eLa#<@(z(YfJiKCljrq^_i*T=|O@x5i*dzF}waHq4GSIJ7=8I z?EyI}!mPb@Rx)<8qTRqDnqr$rFq)>XOSJ559r?{sPB)q1IY zr1mJOpsL{?!=2rL@JYTgR@i1r|WxZZT> zcOrOI1iDMt=-$7jh!3|Gd$3DhGZ^F?fg?So?Ym2DK5(Ij951LFY!FrDiWW6XdY{KF z$5a2?6QWI@(1tKwGIO^y1Xm8kddbDG@m0d84H2i?L!tk@_5Vva0xR!#a-}Epf$*dm?L8O@j-=~OKf_+bWnbZ7XIUb9 zJL0=`nc* zdW)c`sQW$SJL`)Z!-lrG&hf#b#(`r4=8%B+%b{&*LQ%8ANY-x6g+|+p7Qk9~v2WH> z=7>~)BN&6^sIyporcsM>v7EqMe;xjMpAK_Oit*~lc?*--aauB|RjSk2_|G!~PUBWR zO&ES?smG?RYSf^yP{8LqCCHDx^R&rS+53chqqwtW$ujXI~a22(xu z$37DDnU@jGM!l7C+Lyx~)j&XgOjPX3TzHsrX^cmszknjN$Xd|fOwZnnwWK7#zvXMz zMcL`-eRADc)$muoTbw8HHvlj`;=1;b7Wv~tP9mub#$h~eO0WY<;G70fc}Ynw)pkl(>&&`v zC{|H;oDj7hERUmj`W)LYE-Qvkoa~7feOPPqxz1egRtdgYSCFa_3;3DlBl^tJk0{%o z@bEPDI$R7gBqFX{aF;RyNg9a+5dDdX=PH=sfk6c5xaE->wAoB zpxz6(7XwTi) z5SGsJ9WU`fm*1?^54jB^W4cP03qzT&29X)!9+5Fz<$kiAeSXATv{zXW)8%bT3dBz= z^}p;)zwfwxe0=P8wn9R(k2;PrRuzAVtpw<`?ANqNPBBlg1PHG8EV$j(QX5Huw-+-x zYqYg*h$ZHld!SQ~S%2!Po6(p8k*yUo;3!N9vO~gy7Hm8pd!y(TY*}~;eR+y-f?5NB z-8%`Mu4j*d=gOZQ8ZPlQ_=L{xPg?`WUhSv1T3Q=;m#*YR)$^s*FWZr!R4*a=tHAQ2 z4b*pNxJ_BS?ha{59^ZxaZxP}z@(%oU;1;X`U8g_o>=6Bykj32O>7mg?QjN1pk8~=Q z1YlUll9bIic<=bf4~J!|pzC+CNh|nGe(zRz8`UcqgFVymZnlQPpByg+24w78NNQYQ zY6k1Pd5Nd%)E$KE?$&HvibHnT1}N?OYFbV9TmMs44mM=pFaSI&697(YSHGHF^L1W= z=3bh+eW%l=k>aKFx{`3Ynsa}!n{QIgjXTWRHV)9bdRlmXFP7$v$a$RYxL+d+@bXK3 zSQF=()E_xK9fR+wZ<9Tm-ayD*cEny@Rf(!J^B&)<;Bj30j-4Bcx9)lE0aEj^C*W=E zpAct#@Sn~BlLj9lM@tDpmR;tA&3@U%7beZ3+&B-nu_^EM0--61SMzl#pxKm%?0SCkV+Y#L`IXE(ZRhCmfrW>C>s=f+g3^Rl zSpvmKzb@T6%p^AtoN3%a0HU5Au)mQd_bGT>-_q5ddDAYhsgXi?M4l3#S}VBv;Yn+E zqBu)R&&shEv+^z7BMLlt-Rt$Pu(bYsbbVy>O8kVLNO5e{7esNhxF`dsI_h@ab}y4Y zd?c@l%vk%eWE-uoJoI?}gQIno(V6&Nx!IijI^2YPilau@Q@G&)h<__LBW1qF9F>1m zwPM31=*8q66hXdGk>t*ZZ@@MuK0`m5)J?sFymA?a4cS7qCl;Tfx2m+ciP3Z+R2p=v z?jw~ySPIJ=3`gqPqV7`$5FohU&e$XHf_VV(Ws(ly@a3^*sFpXS@)3LY13iJAU_=;4 zd>rMiCdk$^3q0~#2_NsPBIOw>I?o#$5tG<)x52XA82nx!RL-S~;oN(l@63&_Z@rew z|NQ7k^QPV8X!t1o3StQl1s_kSh#~4c*^h(cJ1@uX?y|07;B~>B5GdW~64b}?XoU))ccOOC@z44`g4e9!cCT!KgF$IU*cKMpSG^ z3+7#nDMveUHks_WN!mYBQOJj;1+n>PO)hlPTGkAOBzQVaKfxpE+ork689)$`9M2HL= zjZ>`2o{r7_;=BH)Mon_74OiN5o>pIgtx6Q1-%rp{9B!i|4CMqW!u$%_SaADkl~ VD924+lv3gz9?LMC3;BHT{{ee?9V!3- delta 8247 zcmY+}bx_n#{4j6?6=~@%X+gRr1Vu_fIwS;?kOt}FJ}8ZJcMC{M*KvTHbV#Z+932N7 za2yABkKgC_n`h?P+1GnEX7-QW*`1x;X{P~|BuNI-_wP7#Y`r-+q&dW-<)tL##l<+p zMaATj^|_MY-UARsc>j~`XzEb>D@{JY!wbkeG#~e1k)lDlOFK`eJf@W^i+-LI%5BhJ z2C7UC8iJQPLDYN~8BT#Np1-6T5KAoG+}g-`6zS)(GSkBm@k`LQoEq00Cb4>fN_5;z~;#Rc0e;^PTq`)#^=% zVX)CJC9nhUwY(s(`A2J1GL?RW^>ECCLMnFs1WE!w+NV6R3Sy(6clNvCf)iiB_arZd zcy`4~UjE^wvHF=FTB3_h=DOru%*vrZTFJhlFNYTf)jfSWfGIHeXbd_G%95D+a~5u! z0K`;Gt`3mCA>TKHYO$zN!)xpbA%x51kvfT`#3jr+Z`9pL0{e&$)6%%P@+d4xezA^8 z<@>$mah7ZU#L$z^AwU6B;dAYLYZ%_Gp!AG^&)gFORSpA%=KFr4^@9hQA*+4T zX$JEgQDkywd9OU13-j(Bfgd?{5eqcSl>jvYd=v?nnJiAIOy^(F4#B_5ZDYgL%F4h+ zlbg@x#AvR653ou~zZ@iZUJ9eRZFIYS|4A|pzNqobQDa8^dKI3yfNNJoztv>_+FfXH ze)Xa$wJiRzSMsN$UU3$?=M}pty_;hVFk8knvv3l^+F5!tj)_0S16JCG2l!Oz+^fTzFf#%m9`{T7HeQ=XtDwF? z6@+a)X6)R#&@qC7zo+@?=DUNbW}zj>uWKFy%`qiqvSIJJ4!DU$=u&~nEat8UVi{5T|WR_8#7EQ0}MxGP(GRvy>s7U9hdmB1?_vf%#{2k2X5)n)&?~XT)(|u zfBv=7&cZl=TG_@eN1{aFGfVB_X}I;m?nUHm+Z2^EX510BqqbKyjs_We}GuAPkc* zSb0$=;PFVsx*9if0j0RuL3mSBa_g%aRrM!{kf+Fe2#!=pP^7WZdrhwJM&3rh__>U) z<_-2v<*mlw2MqBK=YLzQ1o{WU+E1@Hz6v(vUKKj0X5P8KE9^0aeg#9hwaQ>%zVq&1 z6zwq0_?3+e^VY_P0Cnde$VtWlC01`&&reSmGW5gp&Hjb&2zVsNif4SJUMQz&&h3Ef z9Wu~}deD?gR&0UN!@==zx2iV0t&D+SW(-7#?0#V89S55jzeq&A=63O=G_-%E^R%tK zWkj=a@}8U3OO3x4%fcGhk9JQMOTZ60*%MuY2C^7OGn+dH0qA>`uEKXzaNl5&ST*={ zw{@Dn{wxK|48fPK$Nt_%LJh}#jAXGeQ#@s`OWxjwUb@SABp`XqdV;BQv>B=uyl zQsP-b*mvwk=#A`N_f8JsqP2uNVav-)2tWNavP24ARC-qC|5{Ox(*86SXSniFHapIx9bcTrNX1&QB#h|KDXTS816oJ&KrnS(T zJxPxs%M}^`FgfF8E$qAn)Nzn`pTNH$hY@Vb#;Y$#iW>|B9)ZKh!^7c@OvZJY#U2*u z;6JXdoMzTg^!$cLB+$&4z^u9_f-g0tO(E!LPX*8qTGv7%&dJ*KzeQO;KBdXSFjQ6= z*c2JC7yC6%$ZLM{zmwctLCXVfFpc4Igbdn@y%AJH_Y9gumWbWU5cTn|In zd=D=b?wI^)Ct7rHe3E*N^?F*=Yn8P5*#fzGNut^)*^X7Bai&$rgPxU|a0bmKD;J9& zG>f1-a>byC#W$RE{Y(6WcX*Vhd22ai?iO>;Lfcbh;r%Wv%c23Urt zJ`ftC3fE?d9Z0Y}H{zk#mJVQ@`E`Cb*=atdad-F6=P$?xyxTxo$2?++<;|HN^%YTp zsKYW}zq9H?zt&}Ql=wvkVb{u8eoKZR9aYB$p4`*=i`zo<`u;QKMb(rNi=mFg0uq3w zGE_6YFVZnZbkjFqnE*6 zU(~#Mf6wRoT<-N}K>8SUZdjEd$Ei*=8iTci2a2Mu< zG!gP#P2^c4(RV8w@s-Vro2tO?7$@M5m6QAYioK2SI>lpXV=p5u6<#y?P?JY&n~a!jeBSx zpCkP^*LZxgzoYwNT18{$^y3Km40AkqeDY(aviE%Uw>{f~ex#>hlA9jx$tm#ZJZ{{ozrgF30V$?lbltdr9GjA*6B;7FzKizQ5?!iIN;BNhHOyw;r zki+#>#6sv|uh-9ZvVVE)r6;oqkJL}MjGlvDw{p`suf_4TR_+*e9M-MZ%^bmaug&?I z04GQ;@!vfs>7RhS%~VpRFhE05<{<$d$0d9Ce?8-L|Md)*n`bhcQVPej4xVPn4rp_H zat(hH{fvVs2_}&f_ar5ovh+KdZFaMTf7xYo=hktCBC$3;_aRYI0|S*tKRNWXy7hXD zPry3g33cUu-Sm@|`ghj?@;e1}7~kbiW8dnY;{rK+JX{FlTXw)2wb_0rcGXv*e*J23 z6qXJqQHZkeN#9oYTq>Ryk)0pqKY^ejOb0d6^NV&nHX`Ov)y<*O9mnIo&R~W?t%o_t z7#WtFcP?KRZ1fL~z_1%-9%T(20yUO^8Bv_MCW$-hJIL}5)tcNaxh2HA-kmyLhJ~O5 ze=)SGCl}uH0-EPgCGJkG!h^%Q^5-A|K`-%>M#2opSeQxP{jwG&e=^c;CwPRXG;yOO z;a~H-07Wh>Sbi<vexLXLpWlC-ro@IFlD-Q_4R+PaCVpK; zMV6~vsDJ_IlUI2yQC=@a2UmYc?-GLLhgEMSx#DHTYPo;14dbWIFY*pk@7sz`NHdF7 zAspaOhCZ7_&;3dKQKvaFFNkPmq$m{6e8u0Da~!nu`BcJ8TxIr+bn)=XI~j;|!M$#~ zbF<9Kp@>(H-ro(=)zUFgLS1wGCMyXNR0%Dq)_bx6l!S8h?|Y@|RJZO_ zW_}1v*#4OP)cy*vUuB_)ujwG>*C?Vr%n^0VE{h2i;F%xXIQcxpeev_f^w!KSp{m)KZV0v!w{^;fq^IfoRf_Sk-{;P?=^#_h+ zp;2kqnxbRoA%EUlMV428pW%IT9-<)gs#asvK_fd_{FQT>0~_&|e92OEdRJcSG=o3A z*+pOE-@S#T=D-aKt}ka@zfpi_GQjj%+A~miEM!hl0m&}JJZY3z$a+oukV#5xV~nbk>o47VEU$K z`o_{33v$MqMk)WTZ?qI#C)uw$Rs076><3MKXYUE>|3szRiU{9pqFr2&^K69Z9q}kn zLkjAz`4Hn6ZP`6GVYG;ghv#8eVTj0PS#Z+P9{U~}KBh-ld0KJfBfR7cMbmR}n-*7G>1khaa z2=7Df;FaxO=gCD@?CTnY-_`S;rS*T+u31ae0n}@XFJqQ}Zcd5EW-=)1x6<(7@ABQH za`>Y8jx2`19lm)N!+(_@S7@X}1(q3wF9y?sMGg$fS1L|W61qy@(D!GjVM3{i>~`6p znx{&)UX~wqh^r<#t zvJtE7<=QI=)mPqaN?|HRDi@uEbw^HICG}G@K^%*A)%rZ{V%oxUX~1b-e~}I?nVxs% zMxF?ziVT|Z=5Gq6+VGjtt<*9NuE%<@WK%|+ypkhd&t?J+v{?F?r5Y^t?XA9Z_9)B# zKy__jVUsK{qQ7^h_{__#uV_YRse`pJdlx0@imDf2t)q@1RHa@9O`Tj(pZu)yC^7Ka zYyT^4M4?08irYlPE5%F3jNvxE^q%vLA8&(`H`1)RafJrg)mnZR{1D|@0Z_izS%`^S z$93GsOb9S<;v$m{%0L)`j1MyHY7C&Lur9EGpXn`?spkJjX-K;2=cxuvmMx!h&|YFA zz!K)A1^ZsPVP@bZ`TOT5Mz6eF%E6I5SvCWguYBL;|E~o&YrY-&k-ZVp#x8pwJt}0V z=<&V#XK?Z>--GY3F8vJ>(nEeCq!G-#D%YQtw5@)#>Q9SJ+P`1 zA_$?;(T~aJElMD!r4WtP}%j%K90x6vx{4 zN>`l?d+M?XD|I}`(4%SqJwim~=`b8djP#bd zo_O-+!IFZ{y{gkOkz%IFoBLI$owgzipa<_53!+@QMY}cJqFMj3`bY7fkXC)E1a{N7 zO}>9C@%|-CeU&V@YU3(9c5~-(f=xqwl>)L_&Z|9OTdF|;DQ{MQmOA)Q5J(_2*-ez; zxPm__0loJR!6+aBB^;Fi@cs#J%XHlJAgD0v33tTaUD(c64$F1K;+rDJaQ|*5INYJR zwkxw51KXfx&Rq*jCaomda7XIokF4CY&nR4DA8O7$E_N%TqHy$ww6>WevO*q$o_)4= z*VhZZyU`nOI?C0(ity*LF6O5qtkYmw|EatJoI+iIrwOdjjeXUx&y{a*5;$cf;&4p9 zk0F>&luQG!fPm!%*0`5!s2>0F5svAZD%Q7`bGwc0SQ{imI!TSi!u?-`95;{Fb?j`oubsfFsW)H_ z^sUAjCI*g@81{&VqezAkhjJ4L+y}cuBg5*3+he+;de>jr{b-O5YZq&HtfIMsONf(* z#mU$`WG=_$%m6rephw0|1kPD#m3W!ni{;e>tdg-*l!a{bR@8bd01M7!qmT;={PIs| zIN-h9Se71RYN>z|qutaJej3-?FsUn7_iU~`=}^NZ#{xN5{tKswM_7TR_N~c5^v1$O zr{bw>Gn5|QcU$3q1bj3pPzpR68+JU-5PV4@Gn`*F>Biu1X#?(hz3=jJb_PM>triHH zuQ+Vi+X`W=u76wqrOqC5v)cpYshUf4lrCNq^y7@EaZy}Y)ePe9O6|z)Zz;o4urZnr z$Bu-u(;DGFmRrIyfjtVx0X)klia*AvYMYJL%9ZNQTmvmPGhBRTX5G}q$>vNB8;y)C z4fO|+1)Op(e@T$JoK{x1NBRxkjIHD)JLbMwc*lRtz1d+oj;SbE)ujU_Ooybr)(~%E zxRw{%dkh{}*vt{0Oun&an#uYpSuPg87x5g{q2jKk^-qchZWI|mC~(?Recks9a1e zB;#V;|9xA|K8QHHML!1Okk<(?xlpBFJB=&yqz#^m#OTW?QvWMa%bZs>tL{D3FdNe^ zD3-KB>o;I;H#vbq+F0fC8_<^EhqKxR5AkWl>+zciR3h@WYt?3WexqX2ToSt*00G~t zA%Uj9bMd;5Qes@QiEx{+!uhL<2()f^=iY1FVlsKH*Z>X8*an!EMrddRUG{T35#--J zt%%LfSQepww4*8#Saak#SUbcN#lE>ZKsCd!wT-YCw?Vy^(2lzKZIO29WGec>c2vh! z%7oJ>ql4M^fg`nwGAG=JOwzWuW{kF30s`)zY%LR|hWV~UgtAOIpLQHfpJ~{dC>FaM zXJI*nY)rh;e*rlN60YEnznRDb>PWr>=Gx^{l{`6StUhHK4UQ~QDJ(4&iq>M~QnTAlHZs4G+J%M_OqIC*;_Uts-Ft2&lNYly)p_68QaC=qva6@`VwH zh%NMT@FXe_&q91Re&(T8!nnOF+r>{-57O_Lmv@&Q{{1*Dl{P1v# zp#T-O-&i9pG@V8}!?7=&>{3_!a9pzFhWT)9Rd$En^PmqY>~s0qcLjQ9Vx>W*y!&*k ziI2SiDBW=Q+JGLvhTDK}q-Z2Hy?$2{rJv8|WltO4>jC-@5M$?{A_Zs(`If0RfQ8;N zC1la<5FyRBs^)?6TRA+^*cG(85=*?vouW-M!6u4AXGTn8#=SOpT437vO(q8Rjo1859L@95GGEOcOCJLBe@G^*tsD4`H;Z;I9)!14*+(f1bedSf z4tfuZqXSsqulUoR?-s(X_9yRb&7Q4w%jl|fBY(bkT9v612^u6_?7e={5^0q$v{>E} zUi)Ya3TEtWL9j)EXNg-!bTYEZ(AQr3r#?u5; z?XUZAPhzU0ZatJsaK4R#M1Fh}N7y_9l}bor9z?Xh|K=dWM)QE2@k|N$6kK}{jmHQv; zl#IMtyUKEj=e4zw)u?THEeiLv`bl6z_EuoQL4@!t`RSLi=cH|Uh$mqBte&4Ru4sy+ z@~`+JQaTzhfK_w*hAZN{19G;-7ev+Fyxcz!S^ygJZn-8b^mcAEk|)>-{au0ak^B>M#L z_d{*_l&kE2Nam2u`2G%3zLkv+fSKBm^II28XYj)|rDMZag zqdYRxu%^lD3K|e%5x8nBZmswA2?a05<^^LtB7UKe_QVJ^aVotY9RgjIx<$=*slxro z+qgYkWs^zy8>UxyLa|1iTGT_$Zf47(3e;+YG*J8Ml zaTNz`PU{U36@LtkGXVsiq4|bi?M1xPm&%~5AV)=W>d64fOV=*Xt=L&EMs{(Ukf zybXfa7x<24Y{3Hs-~AO+kFdF9PmV0kCKM6@kHSI#5x;5#aJV2FMI77os?I7(FpWJ0 z8Mi))9x9jAOfnVkgX=zMTliVAyeQcW5~8{4@x0|eziVr&ziOwln)g2JJ0k8qyV+*Z z(PGniw%1}0bw*EWWBm|z5Yc#hR@yZ0Uek(;1#7b_-SlbJ`%|m$Ei;w08BBFewH()` zT(!_ZiNbCH>wAj`0|d|VP9W7XfYT4(v-<*Z4HxsN&%4# ze!EvqcTS7)(zSq8zW4na{iU9gXFS)5y;;dI8k&#_pa^n(FcBy2`v;-p-%i&)AA0mU z?ZmfnLOry4coP}DqIYQzOT%L!S!!MUIYO%?4Fs@mabk8iG}JoExmC%2u&llIPCN1W z<@{P23lEuA_ug7Q=M@a!dN|YkA!V$opE?Zks4epEs7CEw!kTX( zZZCnf9`%eKEtNu6K?khl26>VHgvcn$vE}rzEzrCfdIcaE2@1*(F@FR6gq{B+D?a;N z@^UO|sKAG%n1P5cEcv-gf113rgzuL{Rom2!n1kY={!RJ!Y$X@V`MEa7mQ}#T@b=&v zr?eZR;l1i-kVM?eJ~^f~Q`Skq;{!TlivrkWf0+LuY#GD|?)Vfeq_jqP)>(>*c1oTy zre1y!L+8{5W@==Eu9@=q;7WBIYkeW7Pc8@UyN@2ZJo+>*M?|I7AI6MNv|$DDb#t2= zHF$pM?JvTm#XR)XY*`k6<+omRCsWJ8LFra$>OD+aoGP6keuk%_H7~m!*UD;`bvK~b zziDozTrHD;$dkLq_!(fK6!m&ms|Zk<_UHe%l5alIn=Y{PAqZQ|t?Dfj2-$mU>I&yB z0&iqaK-Sq3&EwS+O^a!!k)!)>@~gBbMzD1)+sz4Cg+i;99?6Xc?Pq%z8m!LfWO(U? zaxSwT&iM)ZNCMosBYqh$xil&Yp}KxG`QD9XuxT z1veo#=cW-h-{N*;kRc{p51qZJ9KRU(MitQVbkFUE)I4$HOH=R%(qM%G7@YcA+VQJTT! zx+5p4Dcxu;bz!&P@yx06ecqLoCOV$Xx*L*o3088guan!6URSChLM42^)KGiXqhytc zfz*Pf%xx5aMZhwSIPB$!J%vg`8yHe@^E4mFaySqX%YMmv8(!Nsqw4-Jk()Q>R5GAY z>~lG#cI!-*ejg)eH2s}E$|w*s{PjZyodqhwdlNi;=gV5pK5b?$tP2tTW#h-yfXZ$P z&hGHLnZXpd*sJvyooNXPtDnLSs*W8KIumO&iZ(ncF<2)`!g zq12v`CTSNq2;3j?pB?hQ%dw1|Rfk3#FfP4*FTDv}R(Esg=}cJNvP8-@%CdUAd*KO^ z=|sb?3UwI_8_M|aZPW7CaHcsOc zqwnKCp@z-{D&+Pp5$nmm6juID~t^4S1VQrA|{Gp zU%o!93O=5KRQuYmN|@(h4VMZnRtc4%6k~w+l@|%_X8_a&RzxrSVWpc7v!onpTthYp QyP2z+C_Q4{pY`_sKi#~&xBvhE diff --git a/library/matlab-gui/simulinkStaticGUI.m b/library/matlab-gui/simulinkStaticGUI.m index ea4d06c..3590ea1 100644 --- a/library/matlab-gui/simulinkStaticGUI.m +++ b/library/matlab-gui/simulinkStaticGUI.m @@ -149,9 +149,13 @@ function stopButton_Callback(hObject, eventdata, handles) %#ok % Check the status of the simulation and stop it if it's running if strcmp(status,'running') - % Disable the start button - set(handles.startButton,'Backgroundcolor',[0.8,0.8,0.8]); - set(handles.startButton,'Enable','off') + % Disable the start button unless the user checked the proper + % checkbox for avoiding to disable it + if get(handles.checkbox_recompile, 'Value') + + set(handles.startButton,'Backgroundcolor',[0.8,0.8,0.8]); + set(handles.startButton,'Enable','off') + end % Enable compile and exit buttons set(handles.compileButton,'Backgroundcolor',[1.0,0.6,0.0]); @@ -181,4 +185,14 @@ function exitButton_Callback(hObject, eventdata, handles) %#ok % Assign handles and the startstop object to the base workspace assignin('base','sl_synch_handles',handles) +end + +% --- Executes on button press in checkbox_recompile. +function checkbox_recompile_Callback(hObject, eventdata, handles) %#ok + + % hObject handle to checkbox_recompile (see GCBO) + % eventdata reserved - to be defined in a future version of MATLAB + % handles structure with handles and user data (see GUIDATA) + + % Hint: get(hObject,'Value') returns toggle state of checkbox_recompile end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/checkInputRange.m b/library/matlab-wbc/+wbc/checkInputRange.m new file mode 100644 index 0000000..305eca9 --- /dev/null +++ b/library/matlab-wbc/+wbc/checkInputRange.m @@ -0,0 +1,33 @@ +function [inRange, res_check_range] = checkInputRange(umin, umax, u, tol) + + % CHECKINPUTRANGE checks if the current input value is inside the limits. + % + % FORMAT: [inRange, res_check_range] = checkInputRange(umin, umax, u, tol) + % + % INPUT: - umin = [n * 1] min values; + % - umax = [n * 1] max values; + % - u = [n * 1] current values; + % - tol = tolerance; + % + % OUTPUT: - inRange = boolean for checking if u is inside the limits; + % - res_check_range = vector of booleans to check the single joints. + % + % 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 --- + + res_check_range = u < umin + tol | u > umax - tol; + res_tot = sum(res_check_range); + + if res_tot == 0 + inRange = 1; + else + inRange = 0; + end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/checkInputSpikes.m b/library/matlab-wbc/+wbc/checkInputSpikes.m new file mode 100644 index 0000000..872e3a1 --- /dev/null +++ b/library/matlab-wbc/+wbc/checkInputSpikes.m @@ -0,0 +1,47 @@ +function [noSpikes, res_check_spikes] = checkInputSpikes(u, delta_u_max) + + % CHECKINPUTSPIKES checks the (unsigned) difference between two consecutive + % measurements of the input u, i.e. delta = abs(u(k)-u(k-1)) + % returns FALSE if delta is bigger than a user-defined threshold. + % + % FORMAT: [noSpikes, res_check_spikes] = checkInputSpikes(u, delta_u_max) + % + % INPUT: - u = [n * 1] input values; + % - delta_u_max = user-defined threshold; + % + % OUTPUT: - noSpikes = FALSE if delta is greater than delta_u_max; + % - res_check_spikes = vector of booleans to check the single joints. + % + % Authors: Gabriele Nava + % + % all authors are with the Italian Istitute of Technology (IIT) + % email: name.surname@iit.it + % + % Genoa, Dec 2018 + % + + %% --- Initialization --- + + persistent u_previous + + if isempty(u_previous) + + u_previous = u; + end + + delta = abs(u - u_previous); + + % u may be a vector + res_check_spikes = delta >= delta_u_max; + res_tot = sum(res_check_spikes); + + if abs(res_tot) < 0.1 + + noSpikes = 1; + else + noSpikes = 0; + end + + % now, set u_previous to be u + u_previous = u; +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/checkRange.m b/library/matlab-wbc/+wbc/checkRange.m index a5ab6c5..2818de4 100644 --- a/library/matlab-wbc/+wbc/checkRange.m +++ b/library/matlab-wbc/+wbc/checkRange.m @@ -21,6 +21,9 @@ %% --- Initialization --- + % deprecation warning + disp('WARNING: checkRange is deprecated and it will be removed in a future release. Use checkInputRange instead.') + res = u < umin + tol | u > umax - tol; res = sum(res); diff --git a/library/matlab-wbc/+wbc/checkSpikes.m b/library/matlab-wbc/+wbc/checkSpikes.m index 916db42..b97df99 100644 --- a/library/matlab-wbc/+wbc/checkSpikes.m +++ b/library/matlab-wbc/+wbc/checkSpikes.m @@ -21,6 +21,9 @@ %% --- Initialization --- + % deprecation warning + disp('WARNING: checkSpikes is deprecated and it will be removed in a future release. Use checkInputSpikes instead.') + persistent u_previous if isempty(u_previous) diff --git a/library/simulink-library/CMM_iCub_23_25_DoFs.mdl b/library/simulink-library/CMM_iCub_23_25_DoFs.mdl index 54a8ec9..0354334 100644 --- a/library/simulink-library/CMM_iCub_23_25_DoFs.mdl +++ b/library/simulink-library/CMM_iCub_23_25_DoFs.mdl @@ -1,12 +1,12 @@ Model { - Name "CMM_iCub_23_25_DoFs2017" + Name "CMM_iCub_23_25_DoFs" Version 9.0 SavedCharacterEncoding "UTF-8" GraphicalInterface { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.21" + ComputedModelVersion "1.26" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -19,224 +19,220 @@ Model { NumExternalFileReferences 31 ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum21" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum21" SID "496" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum22" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum22" SID "497" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum23" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum23" SID "498" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum24" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum24" SID "499" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum25" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum25" SID "500" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum31" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum31" SID "501" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Ce" - "ntroidalMomentum1" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Centro" + "idalMomentum1" SID "519" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Ce" - "ntroidalMomentum16" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Centro" + "idalMomentum16" SID "520" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Ce" - "ntroidalMomentum2" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Centro" + "idalMomentum2" SID "521" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Ce" - "ntroidalMomentum3" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Centro" + "idalMomentum3" SID "522" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Ce" - "ntroidalMomentum4" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Centro" + "idalMomentum4" SID "523" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Ce" - "ntroidalMomentum1" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Centro" + "idalMomentum1" SID "533" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Ce" - "ntroidalMomentum16" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Centro" + "idalMomentum16" SID "534" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Ce" - "ntroidalMomentum2" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Centro" + "idalMomentum2" SID "535" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Ce" - "ntroidalMomentum3" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Centro" + "idalMomentum3" SID "536" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Ce" - "ntroidalMomentum4" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Centro" + "idalMomentum4" SID "537" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Ce" - "ntroidalMomentum1" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Centro" + "idalMomentum1" SID "547" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Ce" - "ntroidalMomentum16" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Centro" + "idalMomentum16" SID "548" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Ce" - "ntroidalMomentum2" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Centro" + "idalMomentum2" SID "549" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Ce" - "ntroidalMomentum3" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Centro" + "idalMomentum3" SID "550" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Ce" - "ntroidalMomentum4" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Centro" + "idalMomentum4" SID "551" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Ce" - "ntroidalMomentum1" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Centro" + "idalMomentum1" SID "561" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Ce" - "ntroidalMomentum16" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Centro" + "idalMomentum16" SID "562" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Ce" - "ntroidalMomentum2" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Centro" + "idalMomentum2" SID "563" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Ce" - "ntroidalMomentum3" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Centro" + "idalMomentum3" SID "564" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Ce" - "ntroidalMomentum4" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Centro" + "idalMomentum4" SID "565" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5/Ce" - "ntroidalMomentum1" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5/Centro" + "idalMomentum1" SID "575" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5/Ce" - "ntroidalMomentum16" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5/Centro" + "idalMomentum16" SID "576" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5/Ce" - "ntroidalMomentum2" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5/Centro" + "idalMomentum2" SID "577" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5/Ce" - "ntroidalMomentum3" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5/Centro" + "idalMomentum3" SID "578" Type "LIBRARY_BLOCK" } ExternalFileReference { Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" - Path "CMM_iCub_23_25_DoFs2017/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5/Ce" - "ntroidalMomentum4" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5/Centro" + "idalMomentum4" SID "579" Type "LIBRARY_BLOCK" } OrderedModelArguments 1 } - DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" - LogicAnalyzerGraphicalSettings "" - LogicAnalyzerPlugin "on" - LogicAnalyzerSignalOrdering "" - CustomCodeFunctionData "" SLCCPlugin "on" + LogicAnalyzerPlugin "on" + DiagnosticSuppressor "on" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on DisableAllScopes off @@ -316,9 +312,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Tue Mar 12 18:43:35 2019" - RTWModifiedTimeStamp 474317015 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Mar 23 00:04:34 2020" + RTWModifiedTimeStamp 506822674 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -353,7 +349,6 @@ Model { FunctionConnectors off BrowserLookUnderMasks off SimulationMode "normal" - VisualizeLoggedSignalsWhenLoggingToFile off PauseTimes "5" NumberOfSteps 1 SnapshotBufferSize 10 @@ -370,12 +365,12 @@ Model { $PropName "DataLoggingOverride" $ObjectID 7 $ClassName "Simulink.SimulationData.ModelLoggingInfo" - model_ "CMM_iCub_23_25_DoFs2017" + model_ "CMM_iCub_23_25_DoFs" overrideMode_ [0.0] Array { Type "Cell" Dimension 1 - Cell "CMM_iCub_23_25_DoFs2017" + Cell "CMM_iCub_23_25_DoFs" PropName "logAsSpecifiedByModels_" } Array { @@ -425,6 +420,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] StartTime "0.0" StopTime "10.0" AbsTol "auto" @@ -464,6 +460,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] Decimation "1" ExternalInput "[t, u]" FinalStateName "xFinal" @@ -505,7 +502,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 8 + Dimension 9 Cell "BooleansAsBitfields" Cell "PassReuseOutputArgsAs" Cell "PassReuseOutputArgsThreshold" @@ -514,9 +511,11 @@ Model { Cell "OptimizeModelRefInitCode" Cell "NoFixptDivByZeroProtection" Cell "UseSpecifiedMinMax" + Cell "EfficientTunableParamExpr" PropName "DisabledProps" } Description "" + Components [] BlockReduction on BooleanDataType on ConditionallyExecuteInputs on @@ -572,6 +571,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] RTPrefix "error" ConsistencyChecking "none" ArrayBoundsChecking "none" @@ -682,6 +682,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] ProdBitPerChar 8 ProdBitPerShort 16 ProdBitPerInt 32 @@ -730,6 +731,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" EnableRefExpFcnMdlSchedulingChecks on CheckModelReferenceTargetMessage "error" @@ -749,6 +751,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] SimCustomSourceCode "" SimCustomHeaderCode "" SimCustomInitializer "" @@ -798,6 +801,7 @@ Model { Cell "CodeProfilingInstrumentation" PropName "DisabledProps" } + Description "" SystemTargetFile "grt.tlc" HardwareBoard "None" TLCOptions "" @@ -808,9 +812,7 @@ Model { PackageName "" TemplateMakefile "grt_default_tmf" PostCodeGenCommand "" - Description "" GenerateReport off - SaveLog off RTWVerbose on RetainRTWFile off RTWBuildHooks [] @@ -898,6 +900,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] Comment "" ForceParamTrailComments off GenerateComments on @@ -953,7 +956,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 15 + Dimension 16 Cell "IncludeMdlTerminateFcn" Cell "SuppressErrorStatus" Cell "ERTCustomFileBanners" @@ -969,9 +972,11 @@ Model { Cell "SupportNonInlinedSFcns" Cell "RemoveDisableFunc" Cell "RemoveResetFunc" + Cell "PreserveStateflowLocalDataDimensions" PropName "DisabledProps" } Description "" + Components [] TargetFcnLib "ansi_tfl_table_tmw.mat" TargetLibSuffix "" TargetPreCompLibLocation "" @@ -1050,6 +1055,7 @@ Model { Version "1.17.1" DisabledProps [] Description "Simulink Coverage Configuration Component" + Components [] Name "Simulink Coverage" CovEnable off CovScope "EntireSystem" @@ -1088,9 +1094,9 @@ Model { PropName "Components" } Name "Configuration" - ExtraOptions "" CurrentDlgPage "Solver" ConfigPrmDlgPosition [ 145, 177, 1135, 847 ] + ExtraOptions "" } PropName "ConfigurationSets" } @@ -1106,7 +1112,6 @@ Model { DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" DefaultExtrapolationMethodBetweenContTasks "None" - AutoInsertRateTranBlk [0] } ExplicitPartitioning off BlockDefaults { @@ -1231,7 +1236,7 @@ Model { MustResolveToSignalObject off OutputWhenUnConnected off OutputWhenUnconnectedValue "0" - VectorParamsAs1DForOutWhenUnconnected off + VectorParamsAs1DForOutWhenUnconnected on } Block { BlockType S-Function @@ -1272,14 +1277,13 @@ Model { TreatAsGroupedWhenPropagatingVariantConditions on ContentPreviewEnabled off IsWebBlock off - IsObserver off } Block { BlockType Terminator } } System { - Name "CMM_iCub_23_25_DoFs2017" + Name "CMM_iCub_23_25_DoFs" Location [134, 55, 3840, 2160] Open off PortBlocksUseCompactNotation off @@ -1389,6 +1393,11 @@ Model { 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 { @@ -1402,6 +1411,11 @@ Model { 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 { @@ -1415,6 +1429,11 @@ Model { 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 { @@ -1428,6 +1447,11 @@ Model { 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 { @@ -1441,6 +1465,11 @@ Model { 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 { @@ -1454,6 +1483,11 @@ Model { 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 { @@ -1539,6 +1573,7 @@ Model { Position [955, 518, 985, 532] ZOrder 695 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -1903,6 +1938,11 @@ Model { 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 { @@ -1916,6 +1956,11 @@ Model { 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 { @@ -1929,6 +1974,11 @@ Model { 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 { @@ -1942,6 +1992,11 @@ Model { 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 { @@ -1955,6 +2010,11 @@ Model { 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 { @@ -2001,7 +2061,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "40" + SIDHighWatermark "55" Block { BlockType Inport Name "SelMatrix" @@ -2031,24 +2091,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "526::38" + SID "526::54" Ports [1, 1] Position [270, 315, 320, 355] - ZOrder 29 + ZOrder 45 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "526::37" - Tag "Stateflow S-Function CMM_iCub_23_25_DoFs2017 9" + SID "526::53" + Tag "Stateflow S-Function CMM_iCub_23_25_DoFs 9" Ports [3, 6] Position [180, 102, 230, 243] - ZOrder 28 + ZOrder 44 FunctionName "sf_sfun" PortCounts "[3 6]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -2074,9 +2134,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "526::39" + SID "526::55" Position [460, 326, 480, 344] - ZOrder 30 + ZOrder 46 } Block { BlockType Outport @@ -2085,6 +2145,7 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -2094,6 +2155,7 @@ Model { ZOrder 14 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -2103,6 +2165,7 @@ Model { ZOrder 15 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -2112,6 +2175,7 @@ Model { ZOrder 16 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -2121,23 +2185,25 @@ Model { ZOrder 17 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 1 + ZOrder 51 SrcBlock "SelMatrix" SrcPort 1 + Points [120, 0] DstBlock " SFunction " DstPort 1 } Line { - ZOrder 2 + ZOrder 52 SrcBlock "groupNumber" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 3 + ZOrder 53 SrcBlock "jointPos" SrcPort 1 DstBlock " SFunction " @@ -2145,7 +2211,7 @@ Model { } Line { Name "v1" - ZOrder 4 + ZOrder 54 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -2154,7 +2220,7 @@ Model { } Line { Name "v2" - ZOrder 5 + ZOrder 55 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -2163,7 +2229,7 @@ Model { } Line { Name "v3" - ZOrder 6 + ZOrder 56 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -2172,7 +2238,7 @@ Model { } Line { Name "v4" - ZOrder 7 + ZOrder 57 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -2181,7 +2247,7 @@ Model { } Line { Name "v5" - ZOrder 8 + ZOrder 58 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -2189,14 +2255,14 @@ Model { DstPort 1 } Line { - ZOrder 9 + ZOrder 59 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 10 + ZOrder 60 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -2223,6 +2289,7 @@ Model { Position [440, 203, 470, 217] ZOrder 778 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -2513,6 +2580,11 @@ Model { 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 { @@ -2526,6 +2598,11 @@ Model { 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 { @@ -2539,6 +2616,11 @@ Model { 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 { @@ -2552,6 +2634,11 @@ Model { 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 { @@ -2565,6 +2652,11 @@ Model { 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 { @@ -2612,7 +2704,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "40" + SIDHighWatermark "55" Block { BlockType Inport Name "SelMatrix" @@ -2642,24 +2734,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "540::38" + SID "540::54" Ports [1, 1] Position [270, 315, 320, 355] - ZOrder 29 + ZOrder 45 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "540::37" - Tag "Stateflow S-Function CMM_iCub_23_25_DoFs2017 22" + SID "540::53" + Tag "Stateflow S-Function CMM_iCub_23_25_DoFs 22" Ports [3, 6] Position [180, 102, 230, 243] - ZOrder 28 + ZOrder 44 FunctionName "sf_sfun" PortCounts "[3 6]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -2685,9 +2777,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "540::39" + SID "540::55" Position [460, 326, 480, 344] - ZOrder 30 + ZOrder 46 } Block { BlockType Outport @@ -2696,6 +2788,7 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -2705,6 +2798,7 @@ Model { ZOrder 14 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -2714,6 +2808,7 @@ Model { ZOrder 15 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -2723,6 +2818,7 @@ Model { ZOrder 16 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -2732,23 +2828,25 @@ Model { ZOrder 17 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 1 + ZOrder 51 SrcBlock "SelMatrix" SrcPort 1 + Points [120, 0] DstBlock " SFunction " DstPort 1 } Line { - ZOrder 2 + ZOrder 52 SrcBlock "groupNumber" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 3 + ZOrder 53 SrcBlock "jointPos" SrcPort 1 DstBlock " SFunction " @@ -2756,7 +2854,7 @@ Model { } Line { Name "v1" - ZOrder 4 + ZOrder 54 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -2765,7 +2863,7 @@ Model { } Line { Name "v2" - ZOrder 5 + ZOrder 55 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -2774,7 +2872,7 @@ Model { } Line { Name "v3" - ZOrder 6 + ZOrder 56 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -2783,7 +2881,7 @@ Model { } Line { Name "v4" - ZOrder 7 + ZOrder 57 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -2792,7 +2890,7 @@ Model { } Line { Name "v5" - ZOrder 8 + ZOrder 58 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -2800,14 +2898,14 @@ Model { DstPort 1 } Line { - ZOrder 9 + ZOrder 59 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 10 + ZOrder 60 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -2834,6 +2932,7 @@ Model { Position [440, 203, 470, 217] ZOrder 778 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -3124,6 +3223,11 @@ Model { 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 { @@ -3137,6 +3241,11 @@ Model { 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 { @@ -3150,6 +3259,11 @@ Model { 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 { @@ -3163,6 +3277,11 @@ Model { 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 { @@ -3176,6 +3295,11 @@ Model { 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 { @@ -3223,7 +3347,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "40" + SIDHighWatermark "55" Block { BlockType Inport Name "SelMatrix" @@ -3253,24 +3377,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "554::38" + SID "554::54" Ports [1, 1] Position [270, 315, 320, 355] - ZOrder 29 + ZOrder 45 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "554::37" - Tag "Stateflow S-Function CMM_iCub_23_25_DoFs2017 27" + SID "554::53" + Tag "Stateflow S-Function CMM_iCub_23_25_DoFs 27" Ports [3, 6] Position [180, 102, 230, 243] - ZOrder 28 + ZOrder 44 FunctionName "sf_sfun" PortCounts "[3 6]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -3296,9 +3420,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "554::39" + SID "554::55" Position [460, 326, 480, 344] - ZOrder 30 + ZOrder 46 } Block { BlockType Outport @@ -3307,6 +3431,7 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -3316,6 +3441,7 @@ Model { ZOrder 14 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -3325,6 +3451,7 @@ Model { ZOrder 15 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -3334,6 +3461,7 @@ Model { ZOrder 16 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -3343,23 +3471,25 @@ Model { ZOrder 17 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 1 + ZOrder 51 SrcBlock "SelMatrix" SrcPort 1 + Points [120, 0] DstBlock " SFunction " DstPort 1 } Line { - ZOrder 2 + ZOrder 52 SrcBlock "groupNumber" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 3 + ZOrder 53 SrcBlock "jointPos" SrcPort 1 DstBlock " SFunction " @@ -3367,7 +3497,7 @@ Model { } Line { Name "v1" - ZOrder 4 + ZOrder 54 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -3376,7 +3506,7 @@ Model { } Line { Name "v2" - ZOrder 5 + ZOrder 55 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -3385,7 +3515,7 @@ Model { } Line { Name "v3" - ZOrder 6 + ZOrder 56 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -3394,7 +3524,7 @@ Model { } Line { Name "v4" - ZOrder 7 + ZOrder 57 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -3403,7 +3533,7 @@ Model { } Line { Name "v5" - ZOrder 8 + ZOrder 58 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -3411,14 +3541,14 @@ Model { DstPort 1 } Line { - ZOrder 9 + ZOrder 59 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 10 + ZOrder 60 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -3445,6 +3575,7 @@ Model { Position [440, 203, 470, 217] ZOrder 778 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -3735,6 +3866,11 @@ Model { 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 { @@ -3748,6 +3884,11 @@ Model { 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 { @@ -3761,6 +3902,11 @@ Model { 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 { @@ -3774,6 +3920,11 @@ Model { 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 { @@ -3787,6 +3938,11 @@ Model { 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 { @@ -3834,7 +3990,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "40" + SIDHighWatermark "55" Block { BlockType Inport Name "SelMatrix" @@ -3864,24 +4020,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "568::38" + SID "568::54" Ports [1, 1] Position [270, 315, 320, 355] - ZOrder 29 + ZOrder 45 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "568::37" - Tag "Stateflow S-Function CMM_iCub_23_25_DoFs2017 28" + SID "568::53" + Tag "Stateflow S-Function CMM_iCub_23_25_DoFs 28" Ports [3, 6] Position [180, 102, 230, 243] - ZOrder 28 + ZOrder 44 FunctionName "sf_sfun" PortCounts "[3 6]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -3907,9 +4063,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "568::39" + SID "568::55" Position [460, 326, 480, 344] - ZOrder 30 + ZOrder 46 } Block { BlockType Outport @@ -3918,6 +4074,7 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -3927,6 +4084,7 @@ Model { ZOrder 14 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -3936,6 +4094,7 @@ Model { ZOrder 15 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -3945,6 +4104,7 @@ Model { ZOrder 16 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -3954,23 +4114,25 @@ Model { ZOrder 17 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 1 + ZOrder 51 SrcBlock "SelMatrix" SrcPort 1 + Points [120, 0] DstBlock " SFunction " DstPort 1 } Line { - ZOrder 2 + ZOrder 52 SrcBlock "groupNumber" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 3 + ZOrder 53 SrcBlock "jointPos" SrcPort 1 DstBlock " SFunction " @@ -3978,7 +4140,7 @@ Model { } Line { Name "v1" - ZOrder 4 + ZOrder 54 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -3987,7 +4149,7 @@ Model { } Line { Name "v2" - ZOrder 5 + ZOrder 55 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -3996,7 +4158,7 @@ Model { } Line { Name "v3" - ZOrder 6 + ZOrder 56 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -4005,7 +4167,7 @@ Model { } Line { Name "v4" - ZOrder 7 + ZOrder 57 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -4014,7 +4176,7 @@ Model { } Line { Name "v5" - ZOrder 8 + ZOrder 58 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -4022,14 +4184,14 @@ Model { DstPort 1 } Line { - ZOrder 9 + ZOrder 59 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 10 + ZOrder 60 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -4056,6 +4218,7 @@ Model { Position [440, 203, 470, 217] ZOrder 778 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -4346,6 +4509,11 @@ Model { 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 { @@ -4359,6 +4527,11 @@ Model { 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 { @@ -4372,6 +4545,11 @@ Model { 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 { @@ -4385,6 +4563,11 @@ Model { 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 { @@ -4398,6 +4581,11 @@ Model { 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 { @@ -4445,7 +4633,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "40" + SIDHighWatermark "55" Block { BlockType Inport Name "SelMatrix" @@ -4475,24 +4663,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "582::38" + SID "582::54" Ports [1, 1] Position [270, 315, 320, 355] - ZOrder 29 + ZOrder 45 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "582::37" - Tag "Stateflow S-Function CMM_iCub_23_25_DoFs2017 29" + SID "582::53" + Tag "Stateflow S-Function CMM_iCub_23_25_DoFs 29" Ports [3, 6] Position [180, 102, 230, 243] - ZOrder 28 + ZOrder 44 FunctionName "sf_sfun" PortCounts "[3 6]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -4518,9 +4706,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "582::39" + SID "582::55" Position [460, 326, 480, 344] - ZOrder 30 + ZOrder 46 } Block { BlockType Outport @@ -4529,6 +4717,7 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4538,6 +4727,7 @@ Model { ZOrder 14 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4547,6 +4737,7 @@ Model { ZOrder 15 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4556,6 +4747,7 @@ Model { ZOrder 16 Port "4" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4565,23 +4757,25 @@ Model { ZOrder 17 Port "5" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 1 + ZOrder 51 SrcBlock "SelMatrix" SrcPort 1 + Points [120, 0] DstBlock " SFunction " DstPort 1 } Line { - ZOrder 2 + ZOrder 52 SrcBlock "groupNumber" SrcPort 1 DstBlock " SFunction " DstPort 2 } Line { - ZOrder 3 + ZOrder 53 SrcBlock "jointPos" SrcPort 1 DstBlock " SFunction " @@ -4589,7 +4783,7 @@ Model { } Line { Name "v1" - ZOrder 4 + ZOrder 54 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -4598,7 +4792,7 @@ Model { } Line { Name "v2" - ZOrder 5 + ZOrder 55 Labels [0, 0] SrcBlock " SFunction " SrcPort 3 @@ -4607,7 +4801,7 @@ Model { } Line { Name "v3" - ZOrder 6 + ZOrder 56 Labels [0, 0] SrcBlock " SFunction " SrcPort 4 @@ -4616,7 +4810,7 @@ Model { } Line { Name "v4" - ZOrder 7 + ZOrder 57 Labels [0, 0] SrcBlock " SFunction " SrcPort 5 @@ -4625,7 +4819,7 @@ Model { } Line { Name "v5" - ZOrder 8 + ZOrder 58 Labels [0, 0] SrcBlock " SFunction " SrcPort 6 @@ -4633,14 +4827,14 @@ Model { DstPort 1 } Line { - ZOrder 9 + ZOrder 59 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 10 + ZOrder 60 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -4689,6 +4883,7 @@ Model { Position [660, 223, 690, 237] ZOrder 778 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4698,6 +4893,7 @@ Model { ZOrder 800 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -4970,6 +5166,7 @@ Model { Position [1635, 58, 1665, 72] ZOrder 771 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4979,6 +5176,7 @@ Model { ZOrder 798 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -4988,6 +5186,7 @@ Model { ZOrder 799 Port "3" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -5265,6 +5464,7 @@ Model { Position [195, 113, 225, 127] ZOrder 735 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -5274,6 +5474,7 @@ Model { ZOrder 736 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -5385,7 +5586,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "34" + SIDHighWatermark "49" Block { BlockType Inport Name "ROBOT_DOF" @@ -5406,24 +5607,24 @@ Model { Block { BlockType Demux Name " Demux " - SID "604::32" + SID "604::48" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 23 + ZOrder 39 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "604::31" - Tag "Stateflow S-Function CMM_iCub_23_25_DoFs2017 30" + SID "604::47" + Tag "Stateflow S-Function CMM_iCub_23_25_DoFs 30" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 22 + ZOrder 38 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off - EnableBusSupport on + EnableBusSupport off SFcnIsStateOwnerBlock off Port { PortNumber 2 @@ -5433,9 +5634,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "604::33" + SID "604::49" Position [460, 241, 480, 259] - ZOrder 24 + ZOrder 40 } Block { BlockType Outport @@ -5444,16 +5645,18 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 1 + ZOrder 26 SrcBlock "ROBOT_DOF" SrcPort 1 + Points [120, 0] DstBlock " SFunction " DstPort 1 } Line { - ZOrder 2 + ZOrder 27 SrcBlock "SelMatrix_0" SrcPort 1 DstBlock " SFunction " @@ -5461,7 +5664,7 @@ Model { } Line { Name "SelMatrix" - ZOrder 3 + ZOrder 28 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -5469,14 +5672,14 @@ Model { DstPort 1 } Line { - ZOrder 4 + ZOrder 29 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 5 + ZOrder 30 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -5491,6 +5694,7 @@ Model { Position [1160, 158, 1190, 172] ZOrder 737 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -5500,6 +5704,7 @@ Model { ZOrder 738 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 1 @@ -5625,14 +5830,14 @@ Model { } #Finite State Machines # -# Stateflow 80000014 +# Stateflow 80000019 # # Stateflow { machine { id 1 - name "CMM_iCub_23_25_DoFs2017" - created "28-Feb-2019 16:30:20" + name "CMM_iCub_23_25_DoFs" + created "28-Feb-2019 16:10:03" isLibrary 0 sfVersion 80000012 firstTarget 75 diff --git a/src/driver.cpp.in b/src/driver.cpp.in new file mode 100644 index 0000000..752aa65 --- /dev/null +++ b/src/driver.cpp.in @@ -0,0 +1,57 @@ +#ifdef WIN32 +#error "Windows is not yet supported" +#endif + +#include +#include "@MDL_NAME@.h" + +#include +#include +#include + +// Unix dependent +#include + +volatile static bool stop = false; + +void termination_handler(int /*sig_number*/) +{ + std::cerr << "CTRL-C pressed. Shutting down gracefully." << std::endl; + stop = true; +} + +int main() +{ + struct sigaction new_action, old_action; + new_action.sa_handler = termination_handler; + sigemptyset(&new_action.sa_mask); + sigaddset(&new_action.sa_mask, SIGTERM); + new_action.sa_flags = 0; + sigaction(SIGINT, nullptr, &old_action); + if (old_action.sa_handler != SIG_IGN) { + sigaction(SIGINT, &new_action, nullptr); + } + + blockfactory::coder::GeneratedCodeWrapper<@MDL_NAME@ModelClass> model; + + if (!model.initialize()) { + std::cerr << model.getErrors(); + return EXIT_FAILURE; + } + + while (true) { + bool ok = model.step(); + + if (!ok || stop) { + std::cerr << model.getErrors(); + break; + } + } + + if (!model.terminate()) { + std::cerr << model.getErrors(); + return EXIT_FAILURE; + } + + return EXIT_SUCCESS; +} diff --git a/utilities/CMakeLists.txt b/utilities/CMakeLists.txt new file mode 100644 index 0000000..71a95e2 --- /dev/null +++ b/utilities/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(homePositions) diff --git a/utilities/README.md b/utilities/README.md index ef80274..b2cb354 100644 --- a/utilities/README.md +++ b/utilities/README.md @@ -1,6 +1,8 @@ # Utilities -Simulink models for debugging. +Simulink models for sensors debugging and other utilities. + +- [homePositions](homePositions/README.md): home positions of the iCub robots to be used with the `yarpmotorgui`. - `debug_FTExternalForces.mdl`: this model is used for checking the measurements coming from iCub legs, arms and feet FT sensors. **USAGE:** this model is supposed to be used only with `iCubGenova02` and `iCubGenova04` robots. @@ -14,4 +16,4 @@ Simulink models for debugging. - `debug_seesawIMU.mdl`: this model is used for checking if the seesaw IMU (located in the seesaw board) is correclty aligned with the gravity. **USAGE:** this model is supposed to be used only with the `seesaw board`. -- `releaseLegStressesWhileStanding.sh`: run this script to release the internal stresses in the robot legs while standing on two feet. **USAGE:** this script is supposed to be used only with `iCubGenova02` and `iCubGenova04` robots. Run on a terminal `cd PATH/TO/THIS/FOLDER && sh releaseLegStressesWhileStanding.sh`. \ No newline at end of file +- `releaseLegStressesWhileStanding.sh`: run this script to release the internal stresses in the robot legs while standing on two feet. **USAGE:** this script is supposed to be used only with `iCubGenova02` and `iCubGenova04` robots. Run on a terminal `cd PATH/TO/THIS/FOLDER && sh releaseLegStressesWhileStanding.sh`. diff --git a/utilities/debug_BoschIMU.mdl b/utilities/debug_BoschIMU.mdl index 961e531..d1fb8a4 100644 --- a/utilities/debug_BoschIMU.mdl +++ b/utilities/debug_BoschIMU.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.39" + ComputedModelVersion "1.44" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -31,13 +31,9 @@ Model { } OrderedModelArguments 1 } - DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" - LogicAnalyzerGraphicalSettings "" - LogicAnalyzerPlugin "on" - LogicAnalyzerSignalOrdering "" - CustomCodeFunctionData "" SLCCPlugin "on" + LogicAnalyzerPlugin "on" + DiagnosticSuppressor "on" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on DisableAllScopes off @@ -118,9 +114,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Mar 08 16:25:43 2019" - RTWModifiedTimeStamp 473963143 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Mar 23 00:04:35 2020" + RTWModifiedTimeStamp 506822675 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -155,7 +151,6 @@ Model { FunctionConnectors off BrowserLookUnderMasks off SimulationMode "normal" - VisualizeLoggedSignalsWhenLoggingToFile off PauseTimes "5" NumberOfSteps 1 SnapshotBufferSize 10 @@ -227,6 +222,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] StartTime "0.0" StopTime "inf" AbsTol "auto" @@ -266,6 +262,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] Decimation "1" ExternalInput "[t, u]" FinalStateName "xFinal" @@ -307,7 +304,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 8 + Dimension 9 Cell "BooleansAsBitfields" Cell "PassReuseOutputArgsAs" Cell "PassReuseOutputArgsThreshold" @@ -316,9 +313,11 @@ Model { Cell "OptimizeModelRefInitCode" Cell "NoFixptDivByZeroProtection" Cell "UseSpecifiedMinMax" + Cell "EfficientTunableParamExpr" PropName "DisabledProps" } Description "" + Components [] BlockReduction on BooleanDataType on ConditionallyExecuteInputs on @@ -374,6 +373,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] RTPrefix "error" ConsistencyChecking "none" ArrayBoundsChecking "none" @@ -484,6 +484,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] ProdBitPerChar 8 ProdBitPerShort 16 ProdBitPerInt 32 @@ -532,6 +533,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" EnableRefExpFcnMdlSchedulingChecks on CheckModelReferenceTargetMessage "error" @@ -551,6 +553,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] SimCustomSourceCode "" SimCustomHeaderCode "" SimCustomInitializer "" @@ -600,6 +603,7 @@ Model { Cell "CodeProfilingInstrumentation" PropName "DisabledProps" } + Description "" SystemTargetFile "grt.tlc" HardwareBoard "None" TLCOptions "" @@ -610,9 +614,7 @@ Model { PackageName "" TemplateMakefile "grt_default_tmf" PostCodeGenCommand "" - Description "" GenerateReport off - SaveLog off RTWVerbose on RetainRTWFile off RTWBuildHooks [] @@ -701,6 +703,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] Comment "" ForceParamTrailComments off GenerateComments on @@ -756,7 +759,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 15 + Dimension 16 Cell "IncludeMdlTerminateFcn" Cell "SuppressErrorStatus" Cell "ERTCustomFileBanners" @@ -772,9 +775,11 @@ Model { Cell "ExistingSharedCode" Cell "RemoveDisableFunc" Cell "RemoveResetFunc" + Cell "PreserveStateflowLocalDataDimensions" PropName "DisabledProps" } Description "" + Components [] TargetFcnLib "ansi_tfl_table_tmw.mat" TargetLibSuffix "" TargetPreCompLibLocation "" @@ -853,6 +858,7 @@ Model { Version "1.17.1" DisabledProps [] Description "Simulink Coverage Configuration Component" + Components [] Name "Simulink Coverage" CovEnable off CovScope "EntireSystem" @@ -891,9 +897,9 @@ Model { PropName "Components" } Name "Configuration" - ExtraOptions "" CurrentDlgPage "Solver" ConfigPrmDlgPosition [ 100, 58, 1180, 720 ] + ExtraOptions "" } PropName "ConfigurationSets" } @@ -909,7 +915,6 @@ Model { DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" DefaultExtrapolationMethodBetweenContTasks "None" - AutoInsertRateTranBlk [0] } ExplicitPartitioning off BlockDefaults { @@ -1044,7 +1049,7 @@ Model { "72549;1 1 0.0666666666666667;0.717647058823529 0.274509803921569 1;0.392156862745098 0.831372549019608 0.0745098" "039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863])),extmgr.Configuration('Tools','Plot Na" "vigation',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Ve" - "rsion','2016b')),'Version','2018a','Position',[71 7 1270 670])" + "rsion','2016b')),'Version','2019b','Position',[71 7 1270 670])" NumInputPorts "2" Floating off } @@ -1062,7 +1067,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" period "0.01" } Block { @@ -1091,7 +1095,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/inertial'" signalSize "12" timeout "0.5" diff --git a/utilities/debug_FTExternalForces.mdl b/utilities/debug_FTExternalForces.mdl index 6ec97bc..f53a8f9 100644 --- a/utilities/debug_FTExternalForces.mdl +++ b/utilities/debug_FTExternalForces.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.49" + ComputedModelVersion "1.54" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -61,13 +61,9 @@ Model { } OrderedModelArguments 1 } - DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" - LogicAnalyzerGraphicalSettings "" - LogicAnalyzerPlugin "on" - LogicAnalyzerSignalOrdering "" - CustomCodeFunctionData "" SLCCPlugin "on" + LogicAnalyzerPlugin "on" + DiagnosticSuppressor "on" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on DisableAllScopes off @@ -148,9 +144,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Mar 08 16:25:44 2019" - RTWModifiedTimeStamp 473963144 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Mar 23 00:04:36 2020" + RTWModifiedTimeStamp 506822676 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -185,7 +181,6 @@ Model { FunctionConnectors off BrowserLookUnderMasks off SimulationMode "normal" - VisualizeLoggedSignalsWhenLoggingToFile off PauseTimes "5" NumberOfSteps 1 SnapshotBufferSize 10 @@ -257,6 +252,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] StartTime "0.0" StopTime "inf" AbsTol "auto" @@ -296,6 +292,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] Decimation "1" ExternalInput "[t, u]" FinalStateName "xFinal" @@ -337,7 +334,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 8 + Dimension 9 Cell "BooleansAsBitfields" Cell "PassReuseOutputArgsAs" Cell "PassReuseOutputArgsThreshold" @@ -346,9 +343,11 @@ Model { Cell "OptimizeModelRefInitCode" Cell "NoFixptDivByZeroProtection" Cell "UseSpecifiedMinMax" + Cell "EfficientTunableParamExpr" PropName "DisabledProps" } Description "" + Components [] BlockReduction on BooleanDataType on ConditionallyExecuteInputs on @@ -404,6 +403,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] RTPrefix "error" ConsistencyChecking "none" ArrayBoundsChecking "none" @@ -514,6 +514,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] ProdBitPerChar 8 ProdBitPerShort 16 ProdBitPerInt 32 @@ -562,6 +563,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" EnableRefExpFcnMdlSchedulingChecks on CheckModelReferenceTargetMessage "error" @@ -581,6 +583,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] SimCustomSourceCode "" SimCustomHeaderCode "" SimCustomInitializer "" @@ -630,6 +633,7 @@ Model { Cell "CodeProfilingInstrumentation" PropName "DisabledProps" } + Description "" SystemTargetFile "grt.tlc" HardwareBoard "None" TLCOptions "" @@ -640,9 +644,7 @@ Model { PackageName "" TemplateMakefile "grt_default_tmf" PostCodeGenCommand "" - Description "" GenerateReport off - SaveLog off RTWVerbose on RetainRTWFile off RTWBuildHooks [] @@ -731,6 +733,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] Comment "" ForceParamTrailComments off GenerateComments on @@ -786,7 +789,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 15 + Dimension 16 Cell "IncludeMdlTerminateFcn" Cell "SuppressErrorStatus" Cell "ERTCustomFileBanners" @@ -802,9 +805,11 @@ Model { Cell "ExistingSharedCode" Cell "RemoveDisableFunc" Cell "RemoveResetFunc" + Cell "PreserveStateflowLocalDataDimensions" PropName "DisabledProps" } Description "" + Components [] TargetFcnLib "ansi_tfl_table_tmw.mat" TargetLibSuffix "" TargetPreCompLibLocation "" @@ -883,6 +888,7 @@ Model { Version "1.17.1" DisabledProps [] Description "Simulink Coverage Configuration Component" + Components [] Name "Simulink Coverage" CovEnable off CovScope "EntireSystem" @@ -921,9 +927,9 @@ Model { PropName "Components" } Name "Configuration" - ExtraOptions "" CurrentDlgPage "Solver" ConfigPrmDlgPosition [ 100, 58, 1180, 720 ] + ExtraOptions "" } PropName "ConfigurationSets" } @@ -939,7 +945,6 @@ Model { DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" DefaultExtrapolationMethodBetweenContTasks "None" - AutoInsertRateTranBlk [0] } ExplicitPartitioning off BlockDefaults { @@ -1053,7 +1058,7 @@ Model { MustResolveToSignalObject off OutputWhenUnConnected off OutputWhenUnconnectedValue "0" - VectorParamsAs1DForOutWhenUnconnected off + VectorParamsAs1DForOutWhenUnconnected on } Block { BlockType S-Function @@ -1098,7 +1103,6 @@ Model { TreatAsGroupedWhenPropagatingVariantConditions on ContentPreviewEnabled off IsWebBlock off - IsObserver off } Block { BlockType Terminator @@ -1264,7 +1268,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "57" + SIDHighWatermark "72" Block { BlockType Inport Name "u" @@ -1276,20 +1280,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "43::56" + SID "43::71" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 47 + ZOrder 62 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "43::55" + SID "43::70" Tag "Stateflow S-Function debug_FTExternalForces 2" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 46 + ZOrder 61 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1303,9 +1307,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "43::57" + SID "43::72" Position [460, 241, 480, 259] - ZOrder 48 + ZOrder 63 } Block { BlockType Outport @@ -1314,9 +1318,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 53 + ZOrder 73 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1324,7 +1329,7 @@ Model { } Line { Name "y" - ZOrder 54 + ZOrder 74 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1332,14 +1337,14 @@ Model { DstPort 1 } Line { - ZOrder 55 + ZOrder 75 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 56 + ZOrder 76 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1375,7 +1380,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "57" + SIDHighWatermark "72" Block { BlockType Inport Name "u" @@ -1387,20 +1392,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "59::56" + SID "59::71" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 47 + ZOrder 62 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "59::55" + SID "59::70" Tag "Stateflow S-Function debug_FTExternalForces 1" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 46 + ZOrder 61 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1414,9 +1419,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "59::57" + SID "59::72" Position [460, 241, 480, 259] - ZOrder 48 + ZOrder 63 } Block { BlockType Outport @@ -1425,9 +1430,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 13 + ZOrder 33 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1435,7 +1441,7 @@ Model { } Line { Name "y" - ZOrder 14 + ZOrder 34 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1443,14 +1449,14 @@ Model { DstPort 1 } Line { - ZOrder 15 + ZOrder 35 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 16 + ZOrder 36 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1486,7 +1492,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "57" + SIDHighWatermark "72" Block { BlockType Inport Name "u" @@ -1498,20 +1504,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "60::56" + SID "60::71" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 47 + ZOrder 62 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "60::55" + SID "60::70" Tag "Stateflow S-Function debug_FTExternalForces 3" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 46 + ZOrder 61 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1525,9 +1531,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "60::57" + SID "60::72" Position [460, 241, 480, 259] - ZOrder 48 + ZOrder 63 } Block { BlockType Outport @@ -1536,9 +1542,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 13 + ZOrder 33 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1546,7 +1553,7 @@ Model { } Line { Name "y" - ZOrder 14 + ZOrder 34 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1554,14 +1561,14 @@ Model { DstPort 1 } Line { - ZOrder 15 + ZOrder 35 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 16 + ZOrder 36 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1597,7 +1604,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "57" + SIDHighWatermark "72" Block { BlockType Inport Name "u" @@ -1609,20 +1616,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "61::56" + SID "61::71" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 47 + ZOrder 62 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "61::55" + SID "61::70" Tag "Stateflow S-Function debug_FTExternalForces 4" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 46 + ZOrder 61 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1636,9 +1643,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "61::57" + SID "61::72" Position [460, 241, 480, 259] - ZOrder 48 + ZOrder 63 } Block { BlockType Outport @@ -1647,9 +1654,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 13 + ZOrder 33 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1657,7 +1665,7 @@ Model { } Line { Name "y" - ZOrder 14 + ZOrder 34 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1665,14 +1673,14 @@ Model { DstPort 1 } Line { - ZOrder 15 + ZOrder 35 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 16 + ZOrder 36 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1708,7 +1716,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "57" + SIDHighWatermark "72" Block { BlockType Inport Name "u" @@ -1720,20 +1728,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "62::56" + SID "62::71" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 47 + ZOrder 62 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "62::55" + SID "62::70" Tag "Stateflow S-Function debug_FTExternalForces 5" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 46 + ZOrder 61 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1747,9 +1755,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "62::57" + SID "62::72" Position [460, 241, 480, 259] - ZOrder 48 + ZOrder 63 } Block { BlockType Outport @@ -1758,9 +1766,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 13 + ZOrder 33 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1768,7 +1777,7 @@ Model { } Line { Name "y" - ZOrder 14 + ZOrder 34 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1776,14 +1785,14 @@ Model { DstPort 1 } Line { - ZOrder 15 + ZOrder 35 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 16 + ZOrder 36 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1819,7 +1828,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "57" + SIDHighWatermark "72" Block { BlockType Inport Name "u" @@ -1831,20 +1840,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "63::56" + SID "63::71" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 47 + ZOrder 62 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "63::55" + SID "63::70" Tag "Stateflow S-Function debug_FTExternalForces 6" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 46 + ZOrder 61 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1858,9 +1867,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "63::57" + SID "63::72" Position [460, 241, 480, 259] - ZOrder 48 + ZOrder 63 } Block { BlockType Outport @@ -1869,9 +1878,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 13 + ZOrder 33 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1879,7 +1889,7 @@ Model { } Line { Name "y" - ZOrder 14 + ZOrder 34 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1887,14 +1897,14 @@ Model { DstPort 1 } Line { - ZOrder 15 + ZOrder 35 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 16 + ZOrder 36 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1942,7 +1952,7 @@ Model { " 0.411764705882353 0.16078431372549;1 1 0.0666666666666667;0.717647058823529 0.274509803921569 1;0.3921568627450" "98 0.831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]),'Display" "LayoutDimensions',[2 2],'TimeRangeSamples','60','TimeRangeFrames','60'),extmgr.Configuration('Tools','Plot Navig" - "ation',true),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2018a','Position',[" + "ation',true),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2019b','Position',[" "409 227 1301 663])" NumInputPorts "4" Floating off @@ -1987,7 +1997,7 @@ Model { " 0.411764705882353 0.16078431372549;1 1 0.0666666666666667;0.717647058823529 0.274509803921569 1;0.3921568627450" "98 0.831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]),'Display" "LayoutDimensions',[2 2],'TimeRangeSamples','60','TimeRangeFrames','60'),extmgr.Configuration('Tools','Plot Navig" - "ation',true),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2018a','Position',[" + "ation',true),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2019b','Position',[" "404 208 1301 663])" NumInputPorts "6" Floating off @@ -2033,7 +2043,7 @@ Model { "5098 0.831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]),'Displ" "ayLayoutDimensions',[2 2],'DisplayContentCache',[],'TimeRangeSamples','60','TimeRangeFrames','60'),extmgr.Config" "uration('Tools','Plot Navigation',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr.Configuration('Tools'" - ",'Measurements',true,'Version','2018a')),'Version','2018a','Position',[135 17 1855 1001])" + ",'Measurements',true,'Version','2018a')),'Version','2019b','Position',[135 17 1855 1001])" NumInputPorts "4" Floating off } @@ -2078,7 +2088,7 @@ Model { "5098 0.831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]),'Displ" "ayLayoutDimensions',[2 2],'DisplayContentCache',[],'TimeRangeSamples','60','TimeRangeFrames','60'),extmgr.Config" "uration('Tools','Plot Navigation',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr.Configuration('Tools'" - ",'Measurements',true,'Version','2018a')),'Version','2018a','Position',[135 16 1865 922])" + ",'Measurements',true,'Version','2018a')),'Version','2019b','Position',[135 16 1865 922])" NumInputPorts "4" Floating off } @@ -2096,7 +2106,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" period "0.01" } Block { @@ -2112,7 +2121,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/right_foot/analog:o'" signalSize "6" timeout "0.5" @@ -2138,7 +2146,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/left_foot/analog:o'" signalSize "6" timeout "0.5" @@ -2164,7 +2171,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/right_leg/analog:o'" signalSize "6" timeout "0.5" @@ -2190,7 +2196,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/left_leg/analog:o'" signalSize "6" timeout "0.5" @@ -2216,7 +2221,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/right_arm/analog:o'" signalSize "6" timeout "0.5" @@ -2242,7 +2246,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/left_arm/analog:o'" signalSize "6" timeout "0.5" @@ -2637,14 +2640,14 @@ Model { } #Finite State Machines # -# Stateflow 80000014 +# Stateflow 80000019 # # Stateflow { machine { id 1 name "debug_FTExternalForces" - created "05-May-2016 17:01:28" + created "04-Nov-2015 16:59:24" isLibrary 0 sfVersion 80000012 firstTarget 44 diff --git a/utilities/debug_FTMeas.mdl b/utilities/debug_FTMeas.mdl index 42b10fd..c160cde 100644 --- a/utilities/debug_FTMeas.mdl +++ b/utilities/debug_FTMeas.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.43" + ComputedModelVersion "1.48" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -97,13 +97,9 @@ Model { } OrderedModelArguments 1 } - DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" - LogicAnalyzerGraphicalSettings "" - LogicAnalyzerPlugin "on" - LogicAnalyzerSignalOrdering "" - CustomCodeFunctionData "" SLCCPlugin "on" + LogicAnalyzerPlugin "on" + DiagnosticSuppressor "on" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on DisableAllScopes off @@ -184,9 +180,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Mar 08 16:25:44 2019" - RTWModifiedTimeStamp 473963144 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Mar 23 00:04:37 2020" + RTWModifiedTimeStamp 506822677 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -221,7 +217,6 @@ Model { FunctionConnectors off BrowserLookUnderMasks off SimulationMode "normal" - VisualizeLoggedSignalsWhenLoggingToFile off PauseTimes "5" NumberOfSteps 1 SnapshotBufferSize 10 @@ -293,6 +288,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] StartTime "0.0" StopTime "inf" AbsTol "auto" @@ -332,6 +328,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] Decimation "1" ExternalInput "[t, u]" FinalStateName "xFinal" @@ -373,7 +370,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 8 + Dimension 9 Cell "BooleansAsBitfields" Cell "PassReuseOutputArgsAs" Cell "PassReuseOutputArgsThreshold" @@ -382,9 +379,11 @@ Model { Cell "OptimizeModelRefInitCode" Cell "NoFixptDivByZeroProtection" Cell "UseSpecifiedMinMax" + Cell "EfficientTunableParamExpr" PropName "DisabledProps" } Description "" + Components [] BlockReduction on BooleanDataType on ConditionallyExecuteInputs on @@ -440,6 +439,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] RTPrefix "error" ConsistencyChecking "none" ArrayBoundsChecking "none" @@ -550,6 +550,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] ProdBitPerChar 8 ProdBitPerShort 16 ProdBitPerInt 32 @@ -598,6 +599,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" EnableRefExpFcnMdlSchedulingChecks on CheckModelReferenceTargetMessage "error" @@ -617,6 +619,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] SimCustomSourceCode "" SimCustomHeaderCode "" SimCustomInitializer "" @@ -666,6 +669,7 @@ Model { Cell "CodeProfilingInstrumentation" PropName "DisabledProps" } + Description "" SystemTargetFile "grt.tlc" HardwareBoard "None" TLCOptions "" @@ -676,9 +680,7 @@ Model { PackageName "" TemplateMakefile "grt_default_tmf" PostCodeGenCommand "" - Description "" GenerateReport off - SaveLog off RTWVerbose on RetainRTWFile off RTWBuildHooks [] @@ -767,6 +769,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] Comment "" ForceParamTrailComments off GenerateComments on @@ -822,7 +825,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 15 + Dimension 16 Cell "IncludeMdlTerminateFcn" Cell "SuppressErrorStatus" Cell "ERTCustomFileBanners" @@ -838,9 +841,11 @@ Model { Cell "ExistingSharedCode" Cell "RemoveDisableFunc" Cell "RemoveResetFunc" + Cell "PreserveStateflowLocalDataDimensions" PropName "DisabledProps" } Description "" + Components [] TargetFcnLib "ansi_tfl_table_tmw.mat" TargetLibSuffix "" TargetPreCompLibLocation "" @@ -919,6 +924,7 @@ Model { Version "1.17.1" DisabledProps [] Description "Simulink Coverage Configuration Component" + Components [] Name "Simulink Coverage" CovEnable off CovScope "EntireSystem" @@ -957,9 +963,9 @@ Model { PropName "Components" } Name "Configuration" - ExtraOptions "" CurrentDlgPage "Solver" ConfigPrmDlgPosition [ 100, 58, 1180, 720 ] + ExtraOptions "" } PropName "ConfigurationSets" } @@ -975,7 +981,6 @@ Model { DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" DefaultExtrapolationMethodBetweenContTasks "None" - AutoInsertRateTranBlk [0] } ExplicitPartitioning off BlockDefaults { @@ -1236,7 +1241,7 @@ Model { " 1;1 0.411764705882353 0.16078431372549;1 1 0.0666666666666667;0.717647058823529 0.274509803921569 1;0.392156862" "745098 0.831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]),'Dis" "playLayoutDimensions',[2 2],'TimeRangeSamples','60','TimeRangeFrames','60'),extmgr.Configuration('Tools','Plot N" - "avigation',true),extmgr.Configuration('Tools','Measurements',true,'Version','2015b')),'Version','2018a','Positio" + "avigation',true),extmgr.Configuration('Tools','Measurements',true,'Version','2015b')),'Version','2019b','Positio" "n',[409 227 1301 663])" NumInputPorts "4" Floating off @@ -1284,7 +1289,7 @@ Model { "1;0.392156862745098 0.831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392" "156863]),'DisplayLayoutDimensions',[2 2],'DisplayContentCache',[],'TimeRangeSamples','60','TimeRangeFrames','60'" "),extmgr.Configuration('Tools','Plot Navigation',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr.Config" - "uration('Tools','Measurements',true,'Version','2015b')),'Version','2018a','Position',[1 5 1862 980])" + "uration('Tools','Measurements',true,'Version','2015b')),'Version','2019b','Position',[1 5 1862 980])" NumInputPorts "4" Floating off } @@ -1329,7 +1334,7 @@ Model { "5098 0.831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]),'Displ" "ayLayoutDimensions',[2 2],'DisplayContentCache',[],'TimeRangeSamples','60','TimeRangeFrames','60'),extmgr.Config" "uration('Tools','Plot Navigation',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr.Configuration('Tools'" - ",'Measurements',true,'Version','2016b')),'Version','2018a','Position',[66 5 1855 968])" + ",'Measurements',true,'Version','2016b')),'Version','2019b','Position',[66 5 1855 968])" NumInputPorts "4" Floating off } @@ -1347,7 +1352,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" period "0.01" } Block { @@ -1447,7 +1451,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/right_foot/analog:o'" signalSize "6" timeout "0.5" @@ -1473,7 +1476,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/left_foot/analog:o'" signalSize "6" timeout "0.5" @@ -1499,7 +1501,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/right_leg/analog:o'" signalSize "6" timeout "0.5" @@ -1525,7 +1526,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/left_leg/analog:o'" signalSize "6" timeout "0.5" @@ -1551,7 +1551,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/right_arm/analog:o'" signalSize "6" timeout "0.5" @@ -1577,7 +1576,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/left_arm/analog:o'" signalSize "6" timeout "0.5" @@ -1603,6 +1601,11 @@ Model { 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 { @@ -1618,6 +1621,11 @@ Model { 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 { @@ -1633,6 +1641,11 @@ Model { 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 { @@ -1648,6 +1661,11 @@ Model { 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 { @@ -1663,6 +1681,11 @@ Model { 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 { @@ -1678,6 +1701,11 @@ Model { 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 } Line { @@ -1999,14 +2027,14 @@ Model { } #Finite State Machines # -# Stateflow 80000014 +# Stateflow 80000019 # # Stateflow { machine { id 1 name "debug_FTMeas" - created "05-May-2016 17:01:28" + created "04-Nov-2015 16:59:24" isLibrary 0 sfVersion 80000012 firstTarget 2 diff --git a/utilities/debug_FTMeas_shoes.mdl b/utilities/debug_FTMeas_shoes.mdl index ab5c00d..46ab98e 100644 --- a/utilities/debug_FTMeas_shoes.mdl +++ b/utilities/debug_FTMeas_shoes.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.45" + ComputedModelVersion "1.50" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -73,13 +73,9 @@ Model { } OrderedModelArguments 1 } - DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" - LogicAnalyzerGraphicalSettings "" - LogicAnalyzerPlugin "on" - LogicAnalyzerSignalOrdering "" - CustomCodeFunctionData "" SLCCPlugin "on" + LogicAnalyzerPlugin "on" + DiagnosticSuppressor "on" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on DisableAllScopes off @@ -160,9 +156,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Mar 08 16:25:45 2019" - RTWModifiedTimeStamp 473963145 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Mar 23 00:04:38 2020" + RTWModifiedTimeStamp 506822678 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -197,7 +193,6 @@ Model { FunctionConnectors off BrowserLookUnderMasks off SimulationMode "normal" - VisualizeLoggedSignalsWhenLoggingToFile off PauseTimes "5" NumberOfSteps 1 SnapshotBufferSize 10 @@ -269,6 +264,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] StartTime "0.0" StopTime "inf" AbsTol "auto" @@ -308,6 +304,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] Decimation "1" ExternalInput "[t, u]" FinalStateName "xFinal" @@ -349,7 +346,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 8 + Dimension 9 Cell "BooleansAsBitfields" Cell "PassReuseOutputArgsAs" Cell "PassReuseOutputArgsThreshold" @@ -358,9 +355,11 @@ Model { Cell "OptimizeModelRefInitCode" Cell "NoFixptDivByZeroProtection" Cell "UseSpecifiedMinMax" + Cell "EfficientTunableParamExpr" PropName "DisabledProps" } Description "" + Components [] BlockReduction on BooleanDataType on ConditionallyExecuteInputs on @@ -416,6 +415,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] RTPrefix "error" ConsistencyChecking "none" ArrayBoundsChecking "none" @@ -526,6 +526,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] ProdBitPerChar 8 ProdBitPerShort 16 ProdBitPerInt 32 @@ -574,6 +575,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" EnableRefExpFcnMdlSchedulingChecks on CheckModelReferenceTargetMessage "error" @@ -593,6 +595,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] SimCustomSourceCode "" SimCustomHeaderCode "" SimCustomInitializer "" @@ -642,6 +645,7 @@ Model { Cell "CodeProfilingInstrumentation" PropName "DisabledProps" } + Description "" SystemTargetFile "grt.tlc" HardwareBoard "None" TLCOptions "" @@ -652,9 +656,7 @@ Model { PackageName "" TemplateMakefile "grt_default_tmf" PostCodeGenCommand "" - Description "" GenerateReport off - SaveLog off RTWVerbose on RetainRTWFile off RTWBuildHooks [] @@ -743,6 +745,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] Comment "" ForceParamTrailComments off GenerateComments on @@ -798,7 +801,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 15 + Dimension 16 Cell "IncludeMdlTerminateFcn" Cell "SuppressErrorStatus" Cell "ERTCustomFileBanners" @@ -814,9 +817,11 @@ Model { Cell "ExistingSharedCode" Cell "RemoveDisableFunc" Cell "RemoveResetFunc" + Cell "PreserveStateflowLocalDataDimensions" PropName "DisabledProps" } Description "" + Components [] TargetFcnLib "ansi_tfl_table_tmw.mat" TargetLibSuffix "" TargetPreCompLibLocation "" @@ -895,6 +900,7 @@ Model { Version "1.17.1" DisabledProps [] Description "Simulink Coverage Configuration Component" + Components [] Name "Simulink Coverage" CovEnable off CovScope "EntireSystem" @@ -933,9 +939,9 @@ Model { PropName "Components" } Name "Configuration" - ExtraOptions "" CurrentDlgPage "Solver" ConfigPrmDlgPosition [ 100, 58, 1180, 720 ] + ExtraOptions "" } PropName "ConfigurationSets" } @@ -951,7 +957,6 @@ Model { DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" DefaultExtrapolationMethodBetweenContTasks "None" - AutoInsertRateTranBlk [0] } ExplicitPartitioning off BlockDefaults { @@ -1162,7 +1167,7 @@ Model { "Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('" "Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false)},'TimeRa" "ngeSamples','60','TimeRangeFrames','60'),extmgr.Configuration('Tools','Plot Navigation',true,'PreviousAutoscale'" - ",'XY','OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2016b')),'Version','2018a'" + ",'XY','OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2016b')),'Version','2019b'" ",'Position',[66 1 1855 1001])" NumInputPorts "2" Floating off @@ -1197,7 +1202,7 @@ Model { "Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('" "Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false)},'TimeRa" "ngeSamples','60','TimeRangeFrames','60'),extmgr.Configuration('Tools','Plot Navigation',true,'PreviousAutoscale'" - ",'XY','OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2016b')),'Version','2018a'" + ",'XY','OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2016b')),'Version','2019b'" ",'Position',[66 1 1855 1001])" NumInputPorts "2" Floating off @@ -1232,7 +1237,7 @@ Model { "Cache',{{}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LineProperties" "Cache',{{}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false)},'TimeRangeSamples','60','TimeRangeFrames','60'" "),extmgr.Configuration('Tools','Plot Navigation',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr.Config" - "uration('Tools','Measurements',true,'Version','2016b')),'Version','2018a','Position',[66 1 1855 1001])" + "uration('Tools','Measurements',true,'Version','2016b')),'Version','2019b','Position',[66 1 1855 1001])" NumInputPorts "2" Floating off } @@ -1266,7 +1271,7 @@ Model { "Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('" "Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false)},'TimeRa" "ngeSamples','60','TimeRangeFrames','60'),extmgr.Configuration('Tools','Plot Navigation',true,'PreviousAutoscale'" - ",'XY','OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2016b')),'Version','2018a'" + ",'XY','OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2016b')),'Version','2019b'" ",'Position',[66 1 1855 1001])" NumInputPorts "2" Floating off @@ -1285,7 +1290,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" period "0.01" } Block { @@ -1357,7 +1361,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'ft/ftShoe_Right_Front/analog:o'" signalSize "6" timeout "0.5" @@ -1383,7 +1386,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'ft/ftShoe_Left_Front/analog:o'" signalSize "6" timeout "0.5" @@ -1409,7 +1411,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'ft/ftShoe_Right_Rear/analog:o'" signalSize "6" timeout "0.5" @@ -1435,7 +1436,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'ft/ftShoe_Left_Rear/analog:o'" signalSize "6" timeout "0.5" @@ -1461,6 +1461,11 @@ Model { 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 { @@ -1476,6 +1481,11 @@ Model { 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 { @@ -1491,6 +1501,11 @@ Model { 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 { @@ -1506,6 +1521,11 @@ Model { 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 } Line { @@ -1719,14 +1739,14 @@ Model { } #Finite State Machines # -# Stateflow 80000014 +# Stateflow 80000019 # # Stateflow { machine { id 1 name "debug_FTMeas_shoes" - created "05-May-2016 17:01:28" + created "04-Nov-2015 16:59:24" isLibrary 0 sfVersion 80000012 firstTarget 2 diff --git a/utilities/debug_seesawIMU.mdl b/utilities/debug_seesawIMU.mdl index 410d2ca..82f313b 100644 --- a/utilities/debug_seesawIMU.mdl +++ b/utilities/debug_seesawIMU.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.51" + ComputedModelVersion "1.56" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -31,13 +31,9 @@ Model { } OrderedModelArguments 1 } - DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" - LogicAnalyzerGraphicalSettings "" - LogicAnalyzerPlugin "on" - LogicAnalyzerSignalOrdering "" - CustomCodeFunctionData "" SLCCPlugin "on" + LogicAnalyzerPlugin "on" + DiagnosticSuppressor "on" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on DisableAllScopes off @@ -117,9 +113,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Mar 08 16:25:45 2019" - RTWModifiedTimeStamp 473963145 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Mar 23 00:04:39 2020" + RTWModifiedTimeStamp 506822679 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -154,7 +150,6 @@ Model { FunctionConnectors off BrowserLookUnderMasks off SimulationMode "normal" - VisualizeLoggedSignalsWhenLoggingToFile off PauseTimes "5" NumberOfSteps 1 SnapshotBufferSize 10 @@ -226,6 +221,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] StartTime "0.0" StopTime "inf" AbsTol "auto" @@ -265,6 +261,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] Decimation "1" ExternalInput "[t, u]" FinalStateName "xFinal" @@ -306,7 +303,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 8 + Dimension 9 Cell "BooleansAsBitfields" Cell "PassReuseOutputArgsAs" Cell "PassReuseOutputArgsThreshold" @@ -315,9 +312,11 @@ Model { Cell "OptimizeModelRefInitCode" Cell "NoFixptDivByZeroProtection" Cell "UseSpecifiedMinMax" + Cell "EfficientTunableParamExpr" PropName "DisabledProps" } Description "" + Components [] BlockReduction on BooleanDataType on ConditionallyExecuteInputs on @@ -373,6 +372,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] RTPrefix "error" ConsistencyChecking "none" ArrayBoundsChecking "none" @@ -483,6 +483,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] ProdBitPerChar 8 ProdBitPerShort 16 ProdBitPerInt 32 @@ -531,6 +532,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" EnableRefExpFcnMdlSchedulingChecks on CheckModelReferenceTargetMessage "error" @@ -550,6 +552,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] SimCustomSourceCode "" SimCustomHeaderCode "" SimCustomInitializer "" @@ -599,6 +602,7 @@ Model { Cell "CodeProfilingInstrumentation" PropName "DisabledProps" } + Description "" SystemTargetFile "grt.tlc" HardwareBoard "None" TLCOptions "" @@ -609,9 +613,7 @@ Model { PackageName "" TemplateMakefile "grt_default_tmf" PostCodeGenCommand "" - Description "" GenerateReport off - SaveLog off RTWVerbose on RetainRTWFile off RTWBuildHooks [] @@ -699,6 +701,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] Comment "" ForceParamTrailComments off GenerateComments on @@ -754,7 +757,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 15 + Dimension 16 Cell "IncludeMdlTerminateFcn" Cell "SuppressErrorStatus" Cell "ERTCustomFileBanners" @@ -770,9 +773,11 @@ Model { Cell "ExistingSharedCode" Cell "RemoveDisableFunc" Cell "RemoveResetFunc" + Cell "PreserveStateflowLocalDataDimensions" PropName "DisabledProps" } Description "" + Components [] TargetFcnLib "ansi_tfl_table_tmw.mat" TargetLibSuffix "" TargetPreCompLibLocation "" @@ -851,6 +856,7 @@ Model { Version "1.17.1" DisabledProps [] Description "Simulink Coverage Configuration Component" + Components [] Name "Simulink Coverage" CovEnable off CovScope "EntireSystem" @@ -891,6 +897,7 @@ Model { Version "1.17.1" DisabledProps [] Description "HDL Coder custom configuration component" + Components [] Name "HDL Coder" Array { Type "Cell" @@ -903,9 +910,9 @@ Model { PropName "Components" } Name "Configuration" - ExtraOptions "" CurrentDlgPage "Solver" ConfigPrmDlgPosition [ 420, 280, 1500, 920 ] + ExtraOptions "" } PropName "ConfigurationSets" } @@ -921,7 +928,6 @@ Model { DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" DefaultExtrapolationMethodBetweenContTasks "None" - AutoInsertRateTranBlk [0] } ExplicitPartitioning off BlockDefaults { @@ -1035,7 +1041,7 @@ Model { MustResolveToSignalObject off OutputWhenUnConnected off OutputWhenUnconnectedValue "0" - VectorParamsAs1DForOutWhenUnconnected off + VectorParamsAs1DForOutWhenUnconnected on } Block { BlockType S-Function @@ -1087,7 +1093,6 @@ Model { TreatAsGroupedWhenPropagatingVariantConditions on ContentPreviewEnabled off IsWebBlock off - IsObserver off } Block { BlockType Terminator @@ -1151,7 +1156,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "69" + SIDHighWatermark "84" Block { BlockType Inport Name "u" @@ -1163,20 +1168,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "26::68" + SID "26::83" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 59 + ZOrder 74 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "26::67" + SID "26::82" Tag "Stateflow S-Function debug_seesawIMU 1" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 58 + ZOrder 73 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1190,9 +1195,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "26::69" + SID "26::84" Position [460, 241, 480, 259] - ZOrder 60 + ZOrder 75 } Block { BlockType Outport @@ -1201,9 +1206,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 13 + ZOrder 33 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1211,7 +1217,7 @@ Model { } Line { Name "y" - ZOrder 14 + ZOrder 34 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1219,14 +1225,14 @@ Model { DstPort 1 } Line { - ZOrder 15 + ZOrder 35 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 16 + ZOrder 36 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1262,7 +1268,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "69" + SIDHighWatermark "84" Block { BlockType Inport Name "u" @@ -1274,20 +1280,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "10::68" + SID "10::83" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 59 + ZOrder 74 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "10::67" + SID "10::82" Tag "Stateflow S-Function debug_seesawIMU 3" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 58 + ZOrder 73 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1301,9 +1307,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "10::69" + SID "10::84" Position [460, 241, 480, 259] - ZOrder 60 + ZOrder 75 } Block { BlockType Outport @@ -1312,9 +1318,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 65 + ZOrder 85 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1322,7 +1329,7 @@ Model { } Line { Name "y" - ZOrder 66 + ZOrder 86 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1330,14 +1337,14 @@ Model { DstPort 1 } Line { - ZOrder 67 + ZOrder 87 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 68 + ZOrder 88 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1373,7 +1380,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "69" + SIDHighWatermark "84" Block { BlockType Inport Name "acc" @@ -1385,20 +1392,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "12::68" + SID "12::83" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 59 + ZOrder 74 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "12::67" + SID "12::82" Tag "Stateflow S-Function debug_seesawIMU 4" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 58 + ZOrder 73 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1412,9 +1419,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "12::69" + SID "12::84" Position [460, 241, 480, 259] - ZOrder 60 + ZOrder 75 } Block { BlockType Outport @@ -1423,9 +1430,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 65 + ZOrder 85 SrcBlock "acc" SrcPort 1 DstBlock " SFunction " @@ -1433,7 +1441,7 @@ Model { } Line { Name "theta" - ZOrder 66 + ZOrder 86 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1441,14 +1449,14 @@ Model { DstPort 1 } Line { - ZOrder 67 + ZOrder 87 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 68 + ZOrder 88 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1484,7 +1492,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "69" + SIDHighWatermark "84" Block { BlockType Inport Name "u" @@ -1496,20 +1504,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "27::68" + SID "27::83" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 59 + ZOrder 74 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "27::67" + SID "27::82" Tag "Stateflow S-Function debug_seesawIMU 2" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 58 + ZOrder 73 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1523,9 +1531,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "27::69" + SID "27::84" Position [460, 241, 480, 259] - ZOrder 60 + ZOrder 75 } Block { BlockType Outport @@ -1534,9 +1542,10 @@ Model { Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { - ZOrder 13 + ZOrder 33 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1544,7 +1553,7 @@ Model { } Line { Name "y" - ZOrder 14 + ZOrder 34 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1552,14 +1561,14 @@ Model { DstPort 1 } Line { - ZOrder 15 + ZOrder 35 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 16 + ZOrder 36 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1592,7 +1601,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" period "0.01" } Block { @@ -1643,7 +1651,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/seesaw'" signalSize "9" timeout "0.5" @@ -1670,7 +1677,7 @@ Model { "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%" "','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'" "Placement',1)),extmgr.Configuration('Tools','Plot Navigation',true),extmgr.Configuration('Tools','Measurements'," - "true,'Version','2018a')),'Version','2018a','Location',[183 400 517 593])" + "true,'Version','2018a')),'Version','2019b','Location',[183 400 517 593])" NumInputPorts "1" Floating off } @@ -1692,7 +1699,7 @@ Model { "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%" "','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'" "Placement',1)),extmgr.Configuration('Tools','Plot Navigation',true),extmgr.Configuration('Tools','Measurements'," - "true)),'Version','2018a','Location',[188 365 512 604])" + "true)),'Version','2019b','Location',[188 365 512 604])" NumInputPorts "1" Floating off } @@ -1714,7 +1721,7 @@ Model { "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%" "','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'" "Placement',1)),extmgr.Configuration('Tools','Plot Navigation',true),extmgr.Configuration('Tools','Measurements'," - "true)),'Version','2018a','Location',[188 365 512 604])" + "true)),'Version','2019b','Location',[188 365 512 604])" NumInputPorts "1" Floating off } @@ -1736,7 +1743,7 @@ Model { "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%" "','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'" "Placement',1)),extmgr.Configuration('Tools','Plot Navigation',true),extmgr.Configuration('Tools','Measurements'," - "true,'Version','2018a')),'Version','2018a','Location',[188 349 512 588])" + "true,'Version','2018a')),'Version','2019b','Location',[188 349 512 588])" NumInputPorts "1" Floating off } @@ -1764,7 +1771,7 @@ Model { "9 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','L" "inePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Plac" "ement',1)),extmgr.Configuration('Tools','Plot Navigation',true),extmgr.Configuration('Tools','Measurements',true" - ",'Version','2018a')),'Version','2018a','Location',[188 349 512 588])" + ",'Version','2018a')),'Version','2019b','Location',[188 349 512 588])" NumInputPorts "1" Floating off } @@ -1889,14 +1896,14 @@ Model { } #Finite State Machines # -# Stateflow 80000014 +# Stateflow 80000019 # # Stateflow { machine { id 1 name "debug_seesawIMU" - created "28-Nov-2016 15:57:28" + created "24-Sep-2015 17:50:06" isLibrary 0 sfVersion 80000012 firstTarget 30 diff --git a/utilities/debug_xSensIMU.mdl b/utilities/debug_xSensIMU.mdl index 065a924..8c5958d 100644 --- a/utilities/debug_xSensIMU.mdl +++ b/utilities/debug_xSensIMU.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.48" + ComputedModelVersion "1.53" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -61,13 +61,9 @@ Model { } OrderedModelArguments 1 } - DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" - LogicAnalyzerGraphicalSettings "" - LogicAnalyzerPlugin "on" - LogicAnalyzerSignalOrdering "" - CustomCodeFunctionData "" SLCCPlugin "on" + LogicAnalyzerPlugin "on" + DiagnosticSuppressor "on" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on DisableAllScopes off @@ -148,9 +144,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Mar 08 16:25:46 2019" - RTWModifiedTimeStamp 473963146 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Mar 23 00:04:40 2020" + RTWModifiedTimeStamp 506822680 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -185,7 +181,6 @@ Model { FunctionConnectors off BrowserLookUnderMasks off SimulationMode "normal" - VisualizeLoggedSignalsWhenLoggingToFile off PauseTimes "5" NumberOfSteps 1 SnapshotBufferSize 10 @@ -257,6 +252,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] StartTime "0.0" StopTime "inf" AbsTol "auto" @@ -296,6 +292,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] Decimation "1" ExternalInput "[t, u]" FinalStateName "xFinal" @@ -337,7 +334,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 8 + Dimension 9 Cell "BooleansAsBitfields" Cell "PassReuseOutputArgsAs" Cell "PassReuseOutputArgsThreshold" @@ -346,9 +343,11 @@ Model { Cell "OptimizeModelRefInitCode" Cell "NoFixptDivByZeroProtection" Cell "UseSpecifiedMinMax" + Cell "EfficientTunableParamExpr" PropName "DisabledProps" } Description "" + Components [] BlockReduction on BooleanDataType on ConditionallyExecuteInputs on @@ -404,6 +403,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] RTPrefix "error" ConsistencyChecking "none" ArrayBoundsChecking "none" @@ -514,6 +514,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] ProdBitPerChar 8 ProdBitPerShort 16 ProdBitPerInt 32 @@ -562,6 +563,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" EnableRefExpFcnMdlSchedulingChecks on CheckModelReferenceTargetMessage "error" @@ -581,6 +583,7 @@ Model { Version "1.17.1" DisabledProps [] Description "" + Components [] SimCustomSourceCode "" SimCustomHeaderCode "" SimCustomInitializer "" @@ -630,6 +633,7 @@ Model { Cell "CodeProfilingInstrumentation" PropName "DisabledProps" } + Description "" SystemTargetFile "grt.tlc" HardwareBoard "None" TLCOptions "" @@ -640,9 +644,7 @@ Model { PackageName "" TemplateMakefile "grt_default_tmf" PostCodeGenCommand "" - Description "" GenerateReport off - SaveLog off RTWVerbose on RetainRTWFile off RTWBuildHooks [] @@ -731,6 +733,7 @@ Model { PropName "DisabledProps" } Description "" + Components [] Comment "" ForceParamTrailComments off GenerateComments on @@ -786,7 +789,7 @@ Model { Version "1.17.1" Array { Type "Cell" - Dimension 15 + Dimension 16 Cell "IncludeMdlTerminateFcn" Cell "SuppressErrorStatus" Cell "ERTCustomFileBanners" @@ -802,9 +805,11 @@ Model { Cell "ExistingSharedCode" Cell "RemoveDisableFunc" Cell "RemoveResetFunc" + Cell "PreserveStateflowLocalDataDimensions" PropName "DisabledProps" } Description "" + Components [] TargetFcnLib "ansi_tfl_table_tmw.mat" TargetLibSuffix "" TargetPreCompLibLocation "" @@ -883,6 +888,7 @@ Model { Version "1.17.1" DisabledProps [] Description "Simulink Coverage Configuration Component" + Components [] Name "Simulink Coverage" CovEnable off CovScope "EntireSystem" @@ -921,9 +927,9 @@ Model { PropName "Components" } Name "Configuration" - ExtraOptions "" CurrentDlgPage "Solver" ConfigPrmDlgPosition [ 100, 58, 1180, 720 ] + ExtraOptions "" } PropName "ConfigurationSets" } @@ -939,7 +945,6 @@ Model { DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" DefaultExtrapolationMethodBetweenContTasks "None" - AutoInsertRateTranBlk [0] } ExplicitPartitioning off BlockDefaults { @@ -1066,7 +1071,7 @@ Model { MustResolveToSignalObject off OutputWhenUnConnected off OutputWhenUnconnectedValue "0" - VectorParamsAs1DForOutWhenUnconnected off + VectorParamsAs1DForOutWhenUnconnected on } Block { BlockType Product @@ -1127,7 +1132,6 @@ Model { TreatAsGroupedWhenPropagatingVariantConditions on ContentPreviewEnabled off IsWebBlock off - IsObserver off } Block { BlockType Sum @@ -1175,6 +1179,11 @@ Model { 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'" @@ -1231,7 +1240,7 @@ Model { ".717647058823529 0.274509803921569 1;0.392156862745098 0.831372549019608 0.0745098039215686;0.0588235294117647 1" " 1;1 0.0745098039215686 0.650980392156863]),'DisplayLayoutDimensions',[2 1],'DisplayContentCache',[]),extmgr.Con" "figuration('Tools','Plot Navigation',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr.Configuration('Too" - "ls','Measurements',true,'Version','2018a')),'Version','2018a','Position',[135 17 1920 947])" + "ls','Measurements',true,'Version','2018a')),'Version','2019b','Position',[135 17 1920 947])" NumInputPorts "2" Floating off } @@ -1263,7 +1272,7 @@ Model { ".717647058823529 0.274509803921569 1;0.392156862745098 0.831372549019608 0.0745098039215686;0.0588235294117647 1" " 1;1 0.0745098039215686 0.650980392156863]),'DisplayLayoutDimensions',[2 1],'DisplayContentCache',[]),extmgr.Con" "figuration('Tools','Plot Navigation',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr.Configuration('Too" - "ls','Measurements',true,'Version','2018a')),'Version','2018a','Position',[135 1 1920 947])" + "ls','Measurements',true,'Version','2018a')),'Version','2019b','Position',[135 1 1920 947])" NumInputPorts "2" Floating off } @@ -1301,7 +1310,7 @@ Model { ".0666666666666667;0.717647058823529 0.274509803921569 1;0.392156862745098 0.831372549019608 0.0745098039215686;0" ".0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]),'DisplayLayoutDimensions',[3 1],'DisplayContentCa" "che',[]),extmgr.Configuration('Tools','Plot Navigation',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr" - ".Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2018a','Position',[135 17 1920 947])" + ".Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2019b','Position',[135 17 1920 947])" NumInputPorts "3" Floating off } @@ -1333,7 +1342,7 @@ Model { ".717647058823529 0.274509803921569 1;0.392156862745098 0.831372549019608 0.0745098039215686;0.0588235294117647 1" " 1;1 0.0745098039215686 0.650980392156863]),'DisplayLayoutDimensions',[2 1],'DisplayContentCache',[]),extmgr.Con" "figuration('Tools','Plot Navigation',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr.Configuration('Too" - "ls','Measurements',true,'Version','2018a')),'Version','2018a','Position',[650 168 1920 954])" + "ls','Measurements',true,'Version','2018a')),'Version','2019b','Position',[650 168 1920 954])" NumInputPorts "2" Floating off } @@ -1373,7 +1382,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" period "0.01" } Block { @@ -1577,6 +1585,11 @@ Model { 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" } @@ -1591,6 +1604,11 @@ Model { 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 "'imu_frame'" } @@ -1757,6 +1775,11 @@ Model { 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 "'root_link_imu_frame'" } @@ -1767,6 +1790,7 @@ Model { Position [1100, 278, 1130, 292] ZOrder 613 IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Block { BlockType Outport @@ -1776,6 +1800,7 @@ Model { ZOrder 628 Port "2" IconDisplay "Port number" + VectorParamsAs1DForOutWhenUnconnected off } Line { ZOrder 158 @@ -2050,7 +2075,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/xsens_inertial'" signalSize "12" timeout "0.5" @@ -2076,7 +2100,6 @@ Model { SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" SourceProductName "WholeBodyToolbox" - MultiThreadCoSim "auto" portName "'/icub/inertial'" signalSize "12" timeout "0.5" @@ -2291,14 +2314,14 @@ Model { } #Finite State Machines # -# Stateflow 80000014 +# Stateflow 80000019 # # Stateflow { machine { id 1 name "debug_xSensIMU" - created "29-Dec-2017 10:37:13" + created "04-Nov-2015 16:59:24" isLibrary 0 sfVersion 80000012 firstTarget 2 diff --git a/utilities/homePositions/CMakeLists.txt b/utilities/homePositions/CMakeLists.txt new file mode 100644 index 0000000..27e2a9f --- /dev/null +++ b/utilities/homePositions/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(robots) diff --git a/utilities/homePositions/README.md b/utilities/homePositions/README.md new file mode 100644 index 0000000..0851f21 --- /dev/null +++ b/utilities/homePositions/README.md @@ -0,0 +1,3 @@ +## Home positions + +Home positions of the iCub robots to be used with the `yarpmotorgui`. Follow the instructions in the `WBC main README`. diff --git a/utilities/homePositions/robots/CMakeLists.txt b/utilities/homePositions/robots/CMakeLists.txt new file mode 100644 index 0000000..8de2653 --- /dev/null +++ b/utilities/homePositions/robots/CMakeLists.txt @@ -0,0 +1,27 @@ +set(ROBOT_NAME "$ENV{YARP_ROBOT_NAME}" CACHE PATH + "Name of your robot") + +set(ROBOT_DIR "${CMAKE_CURRENT_SOURCE_DIR}/${ROBOT_NAME}") + +# If no env variable YARP_ROBOT_NAME is defined +if(WBC_INSTALL_ALL_HOME_POS) + add_subdirectory(bigman) + add_subdirectory(bigman_only_legs) + add_subdirectory(iCubGenova01) + add_subdirectory(iCubGenova02) + add_subdirectory(iCubGenova03) + add_subdirectory(iCubGenova04) + add_subdirectory(iCubGenova05) + add_subdirectory(iCubParis01) + add_subdirectory(iCubParis02) + add_subdirectory(iCubHeidelberg01) + add_subdirectory(icubGazeboSim) + add_subdirectory(iCubDarmstadt01) + add_subdirectory(iCubNancy01) + add_subdirectory(iCubGazeboV2_5_plus) + add_subdirectory(iCubGazeboV2_5) +else() + if(ROBOT_NAME) + add_subdirectory(${ROBOT_NAME}) + endif() +endif() diff --git a/utilities/homePositions/robots/bigman/CMakeLists.txt b/utilities/homePositions/robots/bigman/CMakeLists.txt new file mode 100644 index 0000000..3b5060e --- /dev/null +++ b/utilities/homePositions/robots/bigman/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname bigman) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/bigman/homePoseBalancing.ini b/utilities/homePositions/robots/bigman/homePoseBalancing.ini new file mode 100644 index 0000000..63dcede --- /dev/null +++ b/utilities/homePositions/robots/bigman/homePoseBalancing.ini @@ -0,0 +1,19 @@ +//name of the robot +robot bigman +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[customPosition_YogaPP] +/$robot/head_Position 0 0 +/$robot/head_Velocity 5 5 +/$robot/torso_Position 1 0 -1 +/$robot/torso_Velocity 5 5 5 +/$robot/left_arm_Position 13 6 -14 -7 5.5 0 0 +/$robot/left_arm_Velocity 5 5 5 5 5 5 5 +/$robot/right_arm_Position 13 -6 14 -7 -5.5 0 0 +/$robot/right_arm_Velocity 5 5 5 5 5 5 5 +/$robot/right_leg_Position 0 0 -14 17 -1.5 0 +/$robot/right_leg_Velocity 5 5 5 5 5 5 +/$robot/left_leg_Position 0 0 -14 17 1.5 0 +/$robot/left_leg_Velocity 5 5 5 5 5 5 + diff --git a/utilities/homePositions/robots/bigman/homePoseCalib.ini b/utilities/homePositions/robots/bigman/homePoseCalib.ini new file mode 100644 index 0000000..a0bf16e --- /dev/null +++ b/utilities/homePositions/robots/bigman/homePoseCalib.ini @@ -0,0 +1,34 @@ +//name of the robot +robot bigman +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[customPosition_calibAir] +/$robot/head_Position 0 0 +/$robot/head_Velocity 5 5 +/$robot/torso_Position 0 0 0 +/$robot/torso_Velocity 5 5 5 +/$robot/left_arm_Position 0 20 0 0 0 0 0 +/$robot/left_arm_Velocity 5 5 5 5 5 5 5 +/$robot/right_arm_Position 0 -20 0 0 0 0 0 +/$robot/right_arm_Velocity 5 5 5 5 5 5 5 +/$robot/right_leg_Position -20 0 0 0 0 0 +/$robot/right_leg_Velocity 5 5 5 5 5 5 +/$robot/left_leg_Position 20 0 0 0 0 0 +/$robot/left_leg_Velocity 5 5 5 5 5 5 + +[customPosition_calibPole] +/$robot/head_Position 0 0 +/$robot/head_Velocity 5 5 +/$robot/torso_Position 0 0 0 +/$robot/torso_Velocity 5 5 5 +/$robot/left_arm_Position 0 20 0 0 0 0 0 +/$robot/left_arm_Velocity 5 5 5 5 5 5 5 +/$robot/right_arm_Position 0 -20 0 0 0 0 0 +/$robot/right_arm_Velocity 5 5 5 5 5 5 5 +/$robot/right_leg_Position 0.2388 0 -39.5112 62.9325 -23.4216 -0.2646 +/$robot/right_leg_Velocity 5 5 5 5 5 5 +/$robot/left_leg_Position -0.2388 0 -39.5112 62.9325 -23.4216 0.2646 +/$robot/left_leg_Velocity 5 5 5 5 5 5 + + diff --git a/utilities/homePositions/robots/bigman/homePoseYogaPP.ini b/utilities/homePositions/robots/bigman/homePoseYogaPP.ini new file mode 100644 index 0000000..d167403 --- /dev/null +++ b/utilities/homePositions/robots/bigman/homePoseYogaPP.ini @@ -0,0 +1,100 @@ +//name of the robot +name bigman + +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 +PositionZero 0 0 +VelocityZero 5 5 + +[torso_zero] +// 0 1 2 +PositionZero 1 0 -1 +VelocityZero 5 5 5 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero 13 6 -14 -7 5.5 0 0 +VelocityZero 5 5 5 5 5 5 5 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero 13 -6 14 -7 -5.5 0 0 +VelocityZero 5 5 5 5 5 5 5 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 -14 17 -1.5 0 +VelocityZero 5 5 5 5 5 5 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 -14 17 1.5 0 +VelocityZero 5 5 5 5 5 5 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/bigman/torqueBalancing.ini b/utilities/homePositions/robots/bigman/torqueBalancing.ini new file mode 100644 index 0000000..9c1e978 --- /dev/null +++ b/utilities/homePositions/robots/bigman/torqueBalancing.ini @@ -0,0 +1,19 @@ +name torqueBalancing +robot bigman +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS + +constraint_links ("l_sole" "r_sole") +smooth ("joints", 1.0) + +comKp (30 30 30) +comKd (11 11 11) +comKi (0 0 0) + +kw 1 +kImp (20 20 10 20 20 20 20 20 20 20 20 20 20 30 30 30 60 10 10 30 30 30 60 10 10 ) + +tsat (250 250 250 250 250 250 250 250 250 250 250 250 250 250 250 250 250 250 250 250 250 250 250 250 250 ) diff --git a/utilities/homePositions/robots/bigman_only_legs/CMakeLists.txt b/utilities/homePositions/robots/bigman_only_legs/CMakeLists.txt new file mode 100644 index 0000000..96cbaa1 --- /dev/null +++ b/utilities/homePositions/robots/bigman_only_legs/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname bigman_only_legs) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/bigman_only_legs/homePoseBalancing.ini b/utilities/homePositions/robots/bigman_only_legs/homePoseBalancing.ini new file mode 100644 index 0000000..e92dc59 --- /dev/null +++ b/utilities/homePositions/robots/bigman_only_legs/homePoseBalancing.ini @@ -0,0 +1,11 @@ +//name of the robot +robot bigman +//parts to be opened by the GUI +parts (right_leg left_leg) + +[customPosition_YogaPP] +/$robot/right_leg_Position 0 0 0 0 1.4 0 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 0 0 0 0 1.4 0 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + diff --git a/utilities/homePositions/robots/bigman_only_legs/homePoseYogaPP.ini b/utilities/homePositions/robots/bigman_only_legs/homePoseYogaPP.ini new file mode 100644 index 0000000..3815716 --- /dev/null +++ b/utilities/homePositions/robots/bigman_only_legs/homePoseYogaPP.ini @@ -0,0 +1,80 @@ +//name of the robot +name bigman + +//parts to be opened by the GUI +parts (right_leg left_leg) + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 15 -10 0 +VelocityZero 5 5 5 5 5 5 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 15 -10 0 +VelocityZero 5 5 5 5 5 5 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/bigman_only_legs/torqueBalancing.ini b/utilities/homePositions/robots/bigman_only_legs/torqueBalancing.ini new file mode 100644 index 0000000..b4d8f00 --- /dev/null +++ b/utilities/homePositions/robots/bigman_only_legs/torqueBalancing.ini @@ -0,0 +1,19 @@ +name torqueBalancing +robot bigman +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS + +constraint_links ("l_sole" "r_sole") +smooth ("joints", 1.0) + +comKp (30 30 30) +comKd (11 11 11) +comKi (0 0 0) + +kw 1 +kImp (30 30 30 60 10 10 30 30 30 60 10 10 ) + +tsat (250 250 250 250 250 250 250 250 250 250 250 250 ) diff --git a/utilities/homePositions/robots/iCubDarmstadt01/CMakeLists.txt b/utilities/homePositions/robots/iCubDarmstadt01/CMakeLists.txt new file mode 100644 index 0000000..470b4ba --- /dev/null +++ b/utilities/homePositions/robots/iCubDarmstadt01/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname iCubDarmstadt01) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingHeadTorsoLegsZero.ini b/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingHeadTorsoLegsZero.ini new file mode 100644 index 0000000..4a75e11 --- /dev/null +++ b/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingHeadTorsoLegsZero.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingLeftFoot.ini new file mode 100644 index 0000000..cf0f208 --- /dev/null +++ b/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingLeftFoot.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 13 -1 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 94 0 50 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 51 0 35 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 17 0 -10 -6 4 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 22 22 0 -17 -5 2 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingLegsZero.ini b/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingLegsZero.ini new file mode 100644 index 0000000..170e5c9 --- /dev/null +++ b/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingLegsZero.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingTorsoHeadZero.ini b/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingTorsoHeadZero.ini new file mode 100644 index 0000000..a9b706f --- /dev/null +++ b/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingTorsoHeadZero.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -4 -5 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -4 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingTwoFeet.ini new file mode 100644 index 0000000..acbfcac --- /dev/null +++ b/utilities/homePositions/robots/iCubDarmstadt01/homePoseBalancingTwoFeet.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -4 -5 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -4 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubDarmstadt01/homePoseFineCalibration.ini b/utilities/homePositions/robots/iCubDarmstadt01/homePoseFineCalibration.ini new file mode 100644 index 0000000..4f42844 --- /dev/null +++ b/utilities/homePositions/robots/iCubDarmstadt01/homePoseFineCalibration.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 0.00 0.00 0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 0.00 0.00 0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubDarmstadt01/homePoseWholeBodyImpedance.ini b/utilities/homePositions/robots/iCubDarmstadt01/homePoseWholeBodyImpedance.ini new file mode 100644 index 0000000..20238e6 --- /dev/null +++ b/utilities/homePositions/robots/iCubDarmstadt01/homePoseWholeBodyImpedance.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 35 0 -45 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 35 0 -45 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubDarmstadt01/homePoseYogaPP.ini b/utilities/homePositions/robots/iCubDarmstadt01/homePoseYogaPP.ini new file mode 100644 index 0000000..a439e64 --- /dev/null +++ b/utilities/homePositions/robots/iCubDarmstadt01/homePoseYogaPP.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 1.4 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 1.4 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubDarmstadt01/oneFootBalancingGains.ini b/utilities/homePositions/robots/iCubDarmstadt01/oneFootBalancingGains.ini new file mode 100644 index 0000000..38455a8 --- /dev/null +++ b/utilities/homePositions/robots/iCubDarmstadt01/oneFootBalancingGains.ini @@ -0,0 +1,2 @@ +l_hip_yaw = ("kp", 0.0) + diff --git a/utilities/homePositions/robots/iCubDarmstadt01/torqueBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubDarmstadt01/torqueBalancingLeftFoot.ini new file mode 100644 index 0000000..ea286d1 --- /dev/null +++ b/utilities/homePositions/robots/iCubDarmstadt01/torqueBalancingLeftFoot.ini @@ -0,0 +1,22 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole") + +torqueGains oneFootBalancingGains.ini + + +comKp (50 100 50) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +#kImp (30 30 45 20 20 20 20 20 20 20 20 60 60 40 100 10 10 30 30 20 60 10 10 ) +kImp (30 30 45 20 20 20 20 20 20 20 20 45 45 45 180 15 15 30 30 20 60 10 10 ) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) diff --git a/utilities/homePositions/robots/iCubDarmstadt01/torqueBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubDarmstadt01/torqueBalancingTwoFeet.ini new file mode 100644 index 0000000..27caafa --- /dev/null +++ b/utilities/homePositions/robots/iCubDarmstadt01/torqueBalancingTwoFeet.ini @@ -0,0 +1,21 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole" "r_sole") + +torqueGains twoFeetBalancingGains.ini + +comKp (60 60 50) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +kImp (80 80 30 20 20 20 20 20 20 20 20 30 30 20 60 10 10 30 30 20 60 10 10 ) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/iCubDarmstadt01/twoFeetBalancingGains.ini b/utilities/homePositions/robots/iCubDarmstadt01/twoFeetBalancingGains.ini new file mode 100644 index 0000000..262a23d --- /dev/null +++ b/utilities/homePositions/robots/iCubDarmstadt01/twoFeetBalancingGains.ini @@ -0,0 +1,8 @@ +l_hip_yaw = ("kp", 0.0) +l_ankle_pitch = ("kp", -100.0) +l_ankle_roll = ("kp", -100.0) + +r_hip_yaw = ("kp", 0.0) +r_ankle_pitch = ("kp", 100.0) +r_ankle_roll = ("kp", 100.0) + diff --git a/utilities/homePositions/robots/iCubGazeboV2_5/CMakeLists.txt b/utilities/homePositions/robots/iCubGazeboV2_5/CMakeLists.txt new file mode 100644 index 0000000..6c0e086 --- /dev/null +++ b/utilities/homePositions/robots/iCubGazeboV2_5/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname iCubGazeboV2_5) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/iCubGazeboV2_5/homePoseBalancing.ini b/utilities/homePositions/robots/iCubGazeboV2_5/homePoseBalancing.ini new file mode 100644 index 0000000..2fd9e5b --- /dev/null +++ b/utilities/homePositions/robots/iCubGazeboV2_5/homePoseBalancing.ini @@ -0,0 +1,74 @@ +//name of the robot +robot icubSim +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[customPosition_YogaPP] +/$robot/head_Position 0 0 0 +/$robot/head_Velocity 10 10 10 +/$robot/torso_Position 0 0 -10 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +/$robot/left_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +/$robot/right_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_leg_Position 0 0 0 0 1.4 0 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 0 0 0 0 1.4 0 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_TwoFeetStanding] +/$robot/head_Position 0 0 0 +/$robot/head_Velocity 10 10 10 +/$robot/torso_Position 0 0 -3 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -35.97 29.97 0.06 50.00 0.00 0.00 0.00 +/$robot/left_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_arm_Position -35.97 29.97 0.06 50.00 0.00 0.00 0.00 +/$robot/right_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_leg_Position 12 5 0 -10 -1.6 -5 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 12 5 0 -10 -1.6 -5 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_OnChair] +/$robot/head_Position 0 0 0 +/$robot/head_Velocity 10 10 10 +/$robot/torso_Position 0 0 -4.50 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -90.00 11.0000 80.00 15.00 0.00 0.00 0.00 +/$robot/left_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_arm_Position -90.00 11.0000 80.00 15.00 -0.00 0.00 -0.00 +/$robot/right_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_leg_Position 84.00 12 0 -100.00 -12.00 0.00 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 84.00 12 0 -100.00 -12.00 0.00 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_OnLeftFoot] +/$robot/head_Position 0 0 0 +/$robot/head_Velocity 10 10 10 +/$robot/torso_Position 0 -6.0 2.7 +/$robot/torso_Velocity 5 5 5 +/$robot/left_arm_Position -17.85 19.2 1.17 39.130 0 0 0 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 +/$robot/right_arm_Position -17.85 19.2 1.17 39.130 0 0 0 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 +/$robot/right_leg_Position 1.2 11.55 0 -4 -1.8 -13.2 +/$robot/right_leg_Velocity 10.00 10.00 10.00 10.00 10.00 10.00 +/$robot/left_leg_Position 1.2 -9.45 0 0 1.2 13.6 +/$robot/left_leg_Velocity 5 5 5 5 5 5 + +[customPosition_OnRightFoot] +/$robot/head_Position 0 0 0 +/$robot/head_Velocity 10 10 10 +/$robot/torso_Position 0 6.0 2.7 +/$robot/torso_Velocity 5 5 5 +/$robot/left_arm_Position -17.85 19.2 1.17 39.130 0 0 0 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 +/$robot/right_arm_Position -17.85 19.2 1.17 39.130 0 0 0 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 +/$robot/left_leg_Position 1.2 11.55 0 -4 -1.8 -13.2 +/$robot/left_leg_Velocity 10.00 10.00 10.00 10.00 10.00 10.00 +/$robot/right_leg_Position 1.2 -9.45 0 0 1.2 13.6 +/$robot/right_leg_Velocity 5 5 5 5 5 5 diff --git a/utilities/homePositions/robots/iCubGazeboV2_5_plus/CMakeLists.txt b/utilities/homePositions/robots/iCubGazeboV2_5_plus/CMakeLists.txt new file mode 100644 index 0000000..435809b --- /dev/null +++ b/utilities/homePositions/robots/iCubGazeboV2_5_plus/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname iCubGazeboV2_5_plus) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/iCubGazeboV2_5_plus/homePoseBalancing.ini b/utilities/homePositions/robots/iCubGazeboV2_5_plus/homePoseBalancing.ini new file mode 100644 index 0000000..1a18435 --- /dev/null +++ b/utilities/homePositions/robots/iCubGazeboV2_5_plus/homePoseBalancing.ini @@ -0,0 +1,88 @@ +//name of the robot +robot icubSim +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[customPosition_Jump] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 30 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position 5 10 0.06 50.00 -0.00 0.00 -0.00 +/$robot/left_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_arm_Position 5 10 0.06 50.00 -0.00 0.00 -0.00 +/$robot/right_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_leg_Position 0 0 0 -30 -15 0 +/$robot/right_leg_Velocity 10 10 10 10 5 10 +/$robot/left_leg_Position 0 0 0 -30 -15 0 +/$robot/left_leg_Velocity 10 10 10 10 5 10 + +[customPosition_YogaPP] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 -10 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +/$robot/left_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +/$robot/right_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_leg_Position 0 0 0 0 1.4 0 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 0 0 0 0 1.4 0 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_TwoFeetStanding] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 -3 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -35.97 29.97 0.06 50.00 0.00 0.00 0.00 +/$robot/left_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_arm_Position -35.97 29.97 0.06 50.00 0.00 0.00 0.00 +/$robot/right_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_leg_Position 12 5 0 -10 -1.6 -5 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 12 5 0 -10 -1.6 -5 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_OnChair] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 -4.50 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -90.00 11.0000 80.00 15.00 0.00 0.00 0.00 +/$robot/left_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_arm_Position -90.00 11.0000 80.00 15.00 -0.00 0.00 -0.00 +/$robot/right_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_leg_Position 84.00 12 0 -100.00 -12.00 0.00 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 84.00 12 0 -100.00 -12.00 0.00 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_OnLeftFoot] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 17 -1 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -37 94 0 50 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -37 51 0 35 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 12 17 0 -10 -6 4 +/$robot/right_leg_Velocity 10.00 10.00 10.00 10.00 10.00 10.00 +/$robot/left_leg_Position 22 28 0 -17 -5.5 1.5 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_OnRightFoot] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 -17 1 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -37 51 0 35 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -37 94 0 50 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 22 28 0 -17 -6 1.5 +/$robot/right_leg_Velocity 10.00 10.00 10.00 10.00 10.00 10.00 +/$robot/left_leg_Position 12 17 0 -10 -5.5 4 +/$robot/left_leg_Velocity 10 10 10 10 10 10 diff --git a/utilities/homePositions/robots/iCubGenova01/CMakeLists.txt b/utilities/homePositions/robots/iCubGenova01/CMakeLists.txt new file mode 100644 index 0000000..f811844 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova01/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname iCubGenova01) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/iCubGenova01/homePoseBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubGenova01/homePoseBalancingLeftFoot.ini new file mode 100644 index 0000000..e04d0e9 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova01/homePoseBalancingLeftFoot.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 17 -1 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 94 0 50 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 51 0 35 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 17 0 -10 -6 4 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 22 28 0 -17 -5 3 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubGenova01/homePoseBalancingRedBall.ini b/utilities/homePositions/robots/iCubGenova01/homePoseBalancingRedBall.ini new file mode 100644 index 0000000..5d307b2 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova01/homePoseBalancingRedBall.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -8 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubGenova01/homePoseBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubGenova01/homePoseBalancingTwoFeet.ini new file mode 100644 index 0000000..6ac0c72 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova01/homePoseBalancingTwoFeet.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -4 -5 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -3 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova01/homePoseFineCalibration.ini b/utilities/homePositions/robots/iCubGenova01/homePoseFineCalibration.ini new file mode 100644 index 0000000..762dbea --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova01/homePoseFineCalibration.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova01/homePoseWalking.ini b/utilities/homePositions/robots/iCubGenova01/homePoseWalking.ini new file mode 100644 index 0000000..3ae8089 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova01/homePoseWalking.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 33 2 0 -44 -19 -2 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 33 2 0 -44 -19 -2 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova01/torqueBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubGenova01/torqueBalancingLeftFoot.ini new file mode 100644 index 0000000..3866949 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova01/torqueBalancingLeftFoot.ini @@ -0,0 +1,19 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface_jtc.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole") + +comKp (120 180 120) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +kImp (20 20 20 13 17 13 5 13 17 13 5 70 70 65 300 0 0 20 20 20 10 0 0) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/iCubGenova01/torqueBalancingLeftFootYoga.ini b/utilities/homePositions/robots/iCubGenova01/torqueBalancingLeftFootYoga.ini new file mode 100644 index 0000000..30748a1 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova01/torqueBalancingLeftFootYoga.ini @@ -0,0 +1,20 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface_jtc.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole") +smooth ("joints", 5.0) + +comKp (45 75 45) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +kImp ( 60 60 60 15 15 15 7 15 15 15 7 90 110 65 30 0 0 70 70 20 10 10 10) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/iCubGenova01/torqueBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubGenova01/torqueBalancingTwoFeet.ini new file mode 100644 index 0000000..d1f3ada --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova01/torqueBalancingTwoFeet.ini @@ -0,0 +1,27 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface_jtc.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole" "r_sole") + +comKp (50 50 50) +comKd (0 0 0) +comKi (0 0 0) + +handPosKp (0 0 0 0 0 0 0 0 0 0 0 0 0 0) +handPosKd (0 0 0 0 0 0 0 0 0 0 0 0 0 0) +handPosKi (0 0 0 0 0 0 0 0 0 0 0 0 0 0) + +handForceKp (0 0 0 0 0 0 0 0 0 0 0 0) +handForceKd (0 0 0 0 0 0 0 0 0 0 0 0) +handForceKi (0 0 0 0 0 0 0 0 0 0 0 0) + +kw 1 +kImp (20 20 10 20 20 20 20 20 20 20 20 30 30 30 60 10 10 30 30 30 60 10 10 ) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/iCubGenova02/CMakeLists.txt b/utilities/homePositions/robots/iCubGenova02/CMakeLists.txt new file mode 100644 index 0000000..cd439b6 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova02/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname iCubGenova02) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/iCubGenova02/homePoseBalancing.ini b/utilities/homePositions/robots/iCubGenova02/homePoseBalancing.ini new file mode 100644 index 0000000..5b37f91 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova02/homePoseBalancing.ini @@ -0,0 +1,46 @@ +//name of the robot +robot icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[customPosition_YogaPP] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 -10 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 0 0 0 0 1.4 0 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 0 0 0 0 1.4 0 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_TwoFeetStanding] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 -10 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 12 5 0 -10 -4 -5 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 12 5 0 -10 -4 -5 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_OnChair] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 24.294 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -93.127 25 22.396 32 0 0 0 15 30 10 0 0 0 10 0 10 +/$robot/left_arm_Velocity 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -93.127 25 22.396 32 0 0 0 15 30 10 0 0 0 10 0 10 +/$robot/right_arm_Velocity 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 84 12.6 0 -98.976 -14.5 0 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 84 12.6 0 -98.976 -14.5 0 +/$robot/left_leg_Velocity 10 10 10 10 10 10 diff --git a/utilities/homePositions/robots/iCubGenova02/homePoseBalancingChair.ini b/utilities/homePositions/robots/iCubGenova02/homePoseBalancingChair.ini new file mode 100644 index 0000000..ae57705 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova02/homePoseBalancingChair.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +Position 0 0 0 0 0 0 +Velocity 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +Position 0 0 24.294 +Velocity 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +Position -93.127 25 22.396 32 0 0 0 15 30 10 0 0 0 10 0 10 +Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +Position -93.127 25 22.396 32 0 0 0 15 30 10 0 0 0 10 0 10 +Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +Position 84 12.6 0 -98.976 -14.5 0 +Velocity 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +Position 84 12.6 0 -98.976 -14.5 0 +Velocity 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova02/homePoseBalancingLegsZero.ini b/utilities/homePositions/robots/iCubGenova02/homePoseBalancingLegsZero.ini new file mode 100644 index 0000000..1e5db2c --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova02/homePoseBalancingLegsZero.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova02/homePoseBalancingRedBall.ini b/utilities/homePositions/robots/iCubGenova02/homePoseBalancingRedBall.ini new file mode 100644 index 0000000..4131259 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova02/homePoseBalancingRedBall.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -8 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubGenova02/homePoseBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubGenova02/homePoseBalancingTwoFeet.ini new file mode 100644 index 0000000..4ced5c9 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova02/homePoseBalancingTwoFeet.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -4 -5 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -4 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova02/homePoseFineCalibration.ini b/utilities/homePositions/robots/iCubGenova02/homePoseFineCalibration.ini new file mode 100644 index 0000000..8dcb662 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova02/homePoseFineCalibration.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova02/homePoseYogaPP.ini b/utilities/homePositions/robots/iCubGenova02/homePoseYogaPP.ini new file mode 100644 index 0000000..87d1e41 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova02/homePoseYogaPP.ini @@ -0,0 +1,35 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 1.4 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 1.4 0 +VelocityZero 10 10 10 10 10 10 + diff --git a/utilities/homePositions/robots/iCubGenova02/torqueBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubGenova02/torqueBalancingLeftFoot.ini new file mode 100644 index 0000000..9fb9707 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova02/torqueBalancingLeftFoot.ini @@ -0,0 +1,19 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface_jtc.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole") + +comKp (120 140 120) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +kImp (20 20 20 13 13 13 5 13 13 13 5 70 70 650 300 20 20 20 10 0 0) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/iCubGenova02/torqueBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubGenova02/torqueBalancingTwoFeet.ini new file mode 100644 index 0000000..81e89ba --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova02/torqueBalancingTwoFeet.ini @@ -0,0 +1,21 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole" "r_sole") + +torqueGains twoFeetBalancingGains.ini + +comKp (50 50 50) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +kImp (20 20 10 20 20 20 20 20 20 20 20 30 30 30 60 10 10 30 30 30 60 10 10 ) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/iCubGenova02/twoFeetBalancingGains.ini b/utilities/homePositions/robots/iCubGenova02/twoFeetBalancingGains.ini new file mode 100644 index 0000000..efdcb22 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova02/twoFeetBalancingGains.ini @@ -0,0 +1,2 @@ +l_hip_yaw = ("kp", 0.0) +r_hip_yaw = ("kp", 0.0) diff --git a/utilities/homePositions/robots/iCubGenova03/CMakeLists.txt b/utilities/homePositions/robots/iCubGenova03/CMakeLists.txt new file mode 100644 index 0000000..b927c42 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova03/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname iCubGenova03) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/iCubGenova03/homePoseBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubGenova03/homePoseBalancingLeftFoot.ini new file mode 100644 index 0000000..39a5444 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova03/homePoseBalancingLeftFoot.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 17 -1 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 94 0 50 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 51 0 35 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 17 0 -10 -6 4 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 20 23 0 -21 -3 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubGenova03/homePoseBalancingRedBall.ini b/utilities/homePositions/robots/iCubGenova03/homePoseBalancingRedBall.ini new file mode 100644 index 0000000..d1bbc36 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova03/homePoseBalancingRedBall.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -8 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubGenova03/homePoseBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubGenova03/homePoseBalancingTwoFeet.ini new file mode 100644 index 0000000..2353e0e --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova03/homePoseBalancingTwoFeet.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -3 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -1.6 -5 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -1.6 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubGenova03/homePoseFineCalibration.ini b/utilities/homePositions/robots/iCubGenova03/homePoseFineCalibration.ini new file mode 100644 index 0000000..36d23e6 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova03/homePoseFineCalibration.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova03/torqueBalancing.ini b/utilities/homePositions/robots/iCubGenova03/torqueBalancing.ini new file mode 100644 index 0000000..7e6c534 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova03/torqueBalancing.ini @@ -0,0 +1,27 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface_jtc.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole" "r_sole") + +comKp (50 50 50) +comKd (0 0 0) +comKi (0 0 0) + +handPosKp (0 0 0 0 0 0 0 0 0 0 0 0 0 0) +handPosKd (0 0 0 0 0 0 0 0 0 0 0 0 0 0) +handPosKi (0 0 0 0 0 0 0 0 0 0 0 0 0 0) + +handForceKp (0 0 0 0 0 0 0 0 0 0 0 0) +handForceKd (0 0 0 0 0 0 0 0 0 0 0 0) +handForceKi (0 0 0 0 0 0 0 0 0 0 0 0) + +kw 1 +kImp (20 20 10 20 20 20 20 20 20 20 20 30 30 30 160 0 0 30 30 30 160 0 0 ) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/iCubGenova04/CMakeLists.txt b/utilities/homePositions/robots/iCubGenova04/CMakeLists.txt new file mode 100644 index 0000000..cd17121 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova04/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname iCubGenova04) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/iCubGenova04/homePoseBalancing.ini b/utilities/homePositions/robots/iCubGenova04/homePoseBalancing.ini new file mode 100644 index 0000000..07d527d --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova04/homePoseBalancing.ini @@ -0,0 +1,104 @@ +//name of the robot +robot icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[customPosition_NewController] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 0 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -29.8 29.8 0 45 0 0 25 15 30 10 0 0 0 10 0 10 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -29.8 29.8 0 45 0 0 25 15 30 10 0 0 0 10 0 10 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 0 0 0 0 0 0 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 0 0 0 0 0 0 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_YogaPP] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 0 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 0 0 0 0 1.4 0 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 0 0 0 0 1.4 0 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_TwoFeetStanding] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 -10 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 12 5 0 -10 -4 -5 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 12 5 0 -10 -4 -5 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_CheckJointCalibration] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 0 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -90.0 15.0 15.0 90.0 -0.00 23.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -90.0 15.0 15.0 90.0 -0.00 23.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 0 10 0 0 0 -10 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 0 10 0 0 0 -10 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_OnChair] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 24.294 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -93.127 25 22.396 32 0 0 0 15 30 10 0 0 0 10 0 10 +/$robot/left_arm_Velocity 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -93.127 25 22.396 32 0 0 0 15 30 10 0 0 0 10 0 10 +/$robot/right_arm_Velocity 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 84 12.6 0 -98.976 -14.5 0 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 84 12.6 0 -98.976 -14.5 0 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_OnLeftFoot] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 17 -1 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -37 94 0 50 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -37 51 0 35 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 12 17 0 -10 -6 4 +/$robot/right_leg_Velocity 10.00 10.00 10.00 10.00 10.00 10.00 +/$robot/left_leg_Position 22 28 0 -17 -5.5 1.5 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_OnRightFoot] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 -17 1 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -37 51 0 35 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -37 94 0 50 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 22 28 0 -17 -6 1.5 +/$robot/right_leg_Velocity 10.00 10.00 10.00 10.00 10.00 10.00 +/$robot/left_leg_Position 12 17 0 -10 -5.5 4 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + + diff --git a/utilities/homePositions/robots/iCubGenova04/homePoseBalancingChair.ini b/utilities/homePositions/robots/iCubGenova04/homePoseBalancingChair.ini new file mode 100644 index 0000000..8bed3cd --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova04/homePoseBalancingChair.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +Position 0 0 0 0 0 0 +Velocity 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +Position 0 0 24.294 +Velocity 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +Position -93.127 25 22.396 32 0 0 0 15 30 10 0 0 0 10 0 10 +Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +Position -93.127 25 22.396 32 0 0 0 15 30 10 0 0 0 10 0 10 +Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +Position 84 12.6 0 -98.976 -14.5 0 +Velocity 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +Position 84 12.6 0 -98.976 -14.5 0 +Velocity 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova04/homePoseBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubGenova04/homePoseBalancingLeftFoot.ini new file mode 100644 index 0000000..d6bd42f --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova04/homePoseBalancingLeftFoot.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 17 -1 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 94 0 50 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 51 0 35 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 17 0 -10 -6 4 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 22 28 0 -17 -5.5 1.5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubGenova04/homePoseBalancingLegsZero.ini b/utilities/homePositions/robots/iCubGenova04/homePoseBalancingLegsZero.ini new file mode 100644 index 0000000..8e8163d --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova04/homePoseBalancingLegsZero.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova04/homePoseBalancingRedBall.ini b/utilities/homePositions/robots/iCubGenova04/homePoseBalancingRedBall.ini new file mode 100644 index 0000000..4131259 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova04/homePoseBalancingRedBall.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -8 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubGenova04/homePoseBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubGenova04/homePoseBalancingTwoFeet.ini new file mode 100644 index 0000000..4ced5c9 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova04/homePoseBalancingTwoFeet.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -4 -5 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -4 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova04/homePoseFineCalibration.ini b/utilities/homePositions/robots/iCubGenova04/homePoseFineCalibration.ini new file mode 100644 index 0000000..771ed27 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova04/homePoseFineCalibration.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova04/homePoseYogaPP.ini b/utilities/homePositions/robots/iCubGenova04/homePoseYogaPP.ini new file mode 100644 index 0000000..a226778 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova04/homePoseYogaPP.ini @@ -0,0 +1,35 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 1.4 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 1.4 0 +VelocityZero 10 10 10 10 10 10 + diff --git a/utilities/homePositions/robots/iCubGenova04/torqueBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubGenova04/torqueBalancingLeftFoot.ini new file mode 100644 index 0000000..9fb9707 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova04/torqueBalancingLeftFoot.ini @@ -0,0 +1,19 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface_jtc.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole") + +comKp (120 140 120) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +kImp (20 20 20 13 13 13 5 13 13 13 5 70 70 650 300 20 20 20 10 0 0) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/iCubGenova04/torqueBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubGenova04/torqueBalancingTwoFeet.ini new file mode 100644 index 0000000..81e89ba --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova04/torqueBalancingTwoFeet.ini @@ -0,0 +1,21 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole" "r_sole") + +torqueGains twoFeetBalancingGains.ini + +comKp (50 50 50) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +kImp (20 20 10 20 20 20 20 20 20 20 20 30 30 30 60 10 10 30 30 30 60 10 10 ) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/iCubGenova04/twoFeetBalancingGains.ini b/utilities/homePositions/robots/iCubGenova04/twoFeetBalancingGains.ini new file mode 100644 index 0000000..efdcb22 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova04/twoFeetBalancingGains.ini @@ -0,0 +1,2 @@ +l_hip_yaw = ("kp", 0.0) +r_hip_yaw = ("kp", 0.0) diff --git a/utilities/homePositions/robots/iCubGenova05/CMakeLists.txt b/utilities/homePositions/robots/iCubGenova05/CMakeLists.txt new file mode 100644 index 0000000..4e4cd95 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova05/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname iCubGenova05) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/iCubGenova05/homePoseBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubGenova05/homePoseBalancingLeftFoot.ini new file mode 100644 index 0000000..88e2864 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova05/homePoseBalancingLeftFoot.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 13 -1 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 94 0 50 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 51 0 35 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 17 0 -10 -6 4 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 22 22 0 -17 -5 2 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubGenova05/homePoseBalancingLegsZero.ini b/utilities/homePositions/robots/iCubGenova05/homePoseBalancingLegsZero.ini new file mode 100644 index 0000000..afd1232 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova05/homePoseBalancingLegsZero.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova05/homePoseBalancingTorsoHeadZero.ini b/utilities/homePositions/robots/iCubGenova05/homePoseBalancingTorsoHeadZero.ini new file mode 100644 index 0000000..6114b95 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova05/homePoseBalancingTorsoHeadZero.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -4 -5 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -4 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova05/homePoseBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubGenova05/homePoseBalancingTwoFeet.ini new file mode 100644 index 0000000..a4b0e48 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova05/homePoseBalancingTwoFeet.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -4 -5 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -4 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova05/homePoseFineCalibration.ini b/utilities/homePositions/robots/iCubGenova05/homePoseFineCalibration.ini new file mode 100644 index 0000000..014fb54 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova05/homePoseFineCalibration.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova05/homePoseWholeBodyImpedance.ini b/utilities/homePositions/robots/iCubGenova05/homePoseWholeBodyImpedance.ini new file mode 100644 index 0000000..adbe8b6 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova05/homePoseWholeBodyImpedance.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 35 0 -45 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 35 0 -45 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova05/homePoseYogaPP.ini b/utilities/homePositions/robots/iCubGenova05/homePoseYogaPP.ini new file mode 100644 index 0000000..ad87517 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova05/homePoseYogaPP.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 1.4 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 1.4 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubGenova05/oneFootBalancingGains.ini b/utilities/homePositions/robots/iCubGenova05/oneFootBalancingGains.ini new file mode 100644 index 0000000..38455a8 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova05/oneFootBalancingGains.ini @@ -0,0 +1,2 @@ +l_hip_yaw = ("kp", 0.0) + diff --git a/utilities/homePositions/robots/iCubGenova05/torqueBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubGenova05/torqueBalancingLeftFoot.ini new file mode 100644 index 0000000..ea286d1 --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova05/torqueBalancingLeftFoot.ini @@ -0,0 +1,22 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole") + +torqueGains oneFootBalancingGains.ini + + +comKp (50 100 50) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +#kImp (30 30 45 20 20 20 20 20 20 20 20 60 60 40 100 10 10 30 30 20 60 10 10 ) +kImp (30 30 45 20 20 20 20 20 20 20 20 45 45 45 180 15 15 30 30 20 60 10 10 ) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) diff --git a/utilities/homePositions/robots/iCubGenova05/torqueBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubGenova05/torqueBalancingTwoFeet.ini new file mode 100644 index 0000000..27caafa --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova05/torqueBalancingTwoFeet.ini @@ -0,0 +1,21 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole" "r_sole") + +torqueGains twoFeetBalancingGains.ini + +comKp (60 60 50) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +kImp (80 80 30 20 20 20 20 20 20 20 20 30 30 20 60 10 10 30 30 20 60 10 10 ) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/iCubGenova05/twoFeetBalancingGains.ini b/utilities/homePositions/robots/iCubGenova05/twoFeetBalancingGains.ini new file mode 100644 index 0000000..262a23d --- /dev/null +++ b/utilities/homePositions/robots/iCubGenova05/twoFeetBalancingGains.ini @@ -0,0 +1,8 @@ +l_hip_yaw = ("kp", 0.0) +l_ankle_pitch = ("kp", -100.0) +l_ankle_roll = ("kp", -100.0) + +r_hip_yaw = ("kp", 0.0) +r_ankle_pitch = ("kp", 100.0) +r_ankle_roll = ("kp", 100.0) + diff --git a/utilities/homePositions/robots/iCubHeidelberg01/CMakeLists.txt b/utilities/homePositions/robots/iCubHeidelberg01/CMakeLists.txt new file mode 100644 index 0000000..b3626ac --- /dev/null +++ b/utilities/homePositions/robots/iCubHeidelberg01/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname iCubHeidelberg01) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/iCubHeidelberg01/homePoseBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubHeidelberg01/homePoseBalancingLeftFoot.ini new file mode 100644 index 0000000..2bbb240 --- /dev/null +++ b/utilities/homePositions/robots/iCubHeidelberg01/homePoseBalancingLeftFoot.ini @@ -0,0 +1,85 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (torso right_leg left_leg) + + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -1 +VelocityZero 10 10 10 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 15 0 -10 -7 16 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 20 23 0 -21 -3 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubHeidelberg01/homePoseBalancingRightFoot.ini b/utilities/homePositions/robots/iCubHeidelberg01/homePoseBalancingRightFoot.ini new file mode 100644 index 0000000..5ea3b3b --- /dev/null +++ b/utilities/homePositions/robots/iCubHeidelberg01/homePoseBalancingRightFoot.ini @@ -0,0 +1,85 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (torso right_leg left_leg) + + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -1 +VelocityZero 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 15 0 -10 -7 15 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 20 23 0 -21 -3 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubHeidelberg01/homePoseBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubHeidelberg01/homePoseBalancingTwoFeet.ini new file mode 100644 index 0000000..7ef8859 --- /dev/null +++ b/utilities/homePositions/robots/iCubHeidelberg01/homePoseBalancingTwoFeet.ini @@ -0,0 +1,86 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (torso right_leg left_leg) + + +[torso_zero] +// 0 1 2 +PositionZero 0 0 3 +VelocityZero 10 10 10 + + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -20 -7 -5 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -20 -7 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubHeidelberg01/torqueBalancing.ini b/utilities/homePositions/robots/iCubHeidelberg01/torqueBalancing.ini new file mode 100644 index 0000000..5e10433 --- /dev/null +++ b/utilities/homePositions/robots/iCubHeidelberg01/torqueBalancing.ini @@ -0,0 +1,20 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS + +constraint_links ("l_sole" "r_sole") + +comKp (80 80 80) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +kImp (100 100 100 35 140 30 50 50 0 35 140 30 50 50 0) + +tsat ( 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24) + + diff --git a/utilities/homePositions/robots/iCubNancy01/CMakeLists.txt b/utilities/homePositions/robots/iCubNancy01/CMakeLists.txt new file mode 100644 index 0000000..6c57e6a --- /dev/null +++ b/utilities/homePositions/robots/iCubNancy01/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname iCubNancy01) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/iCubNancy01/homePoseBalancing.ini b/utilities/homePositions/robots/iCubNancy01/homePoseBalancing.ini new file mode 100644 index 0000000..507ea9b --- /dev/null +++ b/utilities/homePositions/robots/iCubNancy01/homePoseBalancing.ini @@ -0,0 +1,46 @@ +//name of the robot +robot icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[customPosition_YogaPP] +/$robot/head_Position -1.5 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 -10 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 0 0 0 0 1.4 0 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 0 0 0 0 1.4 0 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_TwoFeetStanding] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 -10 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 12 5 0 -10 -4 -5 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 12 5 0 -10 -4 -5 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_OnChair] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 24.294 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -93.127 25 22.396 32 0 0 0 15 30 10 0 0 0 10 0 10 +/$robot/left_arm_Velocity 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 +/$robot/right_arm_Position -93.127 25 22.396 32 0 0 0 15 30 10 0 0 0 10 0 10 +/$robot/right_arm_Velocity 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 +/$robot/right_leg_Position 84 12.6 0 -98.976 -11 0 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 84 12.6 0 -98.976 -11 0 +/$robot/left_leg_Velocity 10 10 10 10 10 10 diff --git a/utilities/homePositions/robots/iCubNancy01/torqueBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubNancy01/torqueBalancingLeftFoot.ini new file mode 100644 index 0000000..9fb9707 --- /dev/null +++ b/utilities/homePositions/robots/iCubNancy01/torqueBalancingLeftFoot.ini @@ -0,0 +1,19 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface_jtc.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole") + +comKp (120 140 120) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +kImp (20 20 20 13 13 13 5 13 13 13 5 70 70 650 300 20 20 20 10 0 0) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/iCubNancy01/torqueBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubNancy01/torqueBalancingTwoFeet.ini new file mode 100644 index 0000000..81e89ba --- /dev/null +++ b/utilities/homePositions/robots/iCubNancy01/torqueBalancingTwoFeet.ini @@ -0,0 +1,21 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole" "r_sole") + +torqueGains twoFeetBalancingGains.ini + +comKp (50 50 50) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +kImp (20 20 10 20 20 20 20 20 20 20 20 30 30 30 60 10 10 30 30 30 60 10 10 ) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/iCubNancy01/twoFeetBalancingGains.ini b/utilities/homePositions/robots/iCubNancy01/twoFeetBalancingGains.ini new file mode 100644 index 0000000..efdcb22 --- /dev/null +++ b/utilities/homePositions/robots/iCubNancy01/twoFeetBalancingGains.ini @@ -0,0 +1,2 @@ +l_hip_yaw = ("kp", 0.0) +r_hip_yaw = ("kp", 0.0) diff --git a/utilities/homePositions/robots/iCubParis01/CMakeLists.txt b/utilities/homePositions/robots/iCubParis01/CMakeLists.txt new file mode 100644 index 0000000..1aa91cc --- /dev/null +++ b/utilities/homePositions/robots/iCubParis01/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname iCubParis01) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/iCubParis01/homePoseBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubParis01/homePoseBalancingLeftFoot.ini new file mode 100644 index 0000000..a39d55c --- /dev/null +++ b/utilities/homePositions/robots/iCubParis01/homePoseBalancingLeftFoot.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 17 -1 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 94 0 50 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 51 0 35 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 17 0 -10 -6 4 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 20 23 0 -21 -3 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubParis01/homePoseBalancingRedBall.ini b/utilities/homePositions/robots/iCubParis01/homePoseBalancingRedBall.ini new file mode 100644 index 0000000..997dc77 --- /dev/null +++ b/utilities/homePositions/robots/iCubParis01/homePoseBalancingRedBall.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -8 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubParis01/homePoseBalancingRightFoot.ini b/utilities/homePositions/robots/iCubParis01/homePoseBalancingRightFoot.ini new file mode 100644 index 0000000..155c31b --- /dev/null +++ b/utilities/homePositions/robots/iCubParis01/homePoseBalancingRightFoot.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 -17 -1 +VelocityZero 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 94 0 50 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 51 0 35 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 17 0 -10 -6 4 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 20 23 0 -21 -3 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubParis01/homePoseBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubParis01/homePoseBalancingTwoFeet.ini new file mode 100644 index 0000000..627e934 --- /dev/null +++ b/utilities/homePositions/robots/iCubParis01/homePoseBalancingTwoFeet.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -3 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -1.6 -5 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -1.6 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubParis01/homePoseFineCalibration.ini b/utilities/homePositions/robots/iCubParis01/homePoseFineCalibration.ini new file mode 100644 index 0000000..8885431 --- /dev/null +++ b/utilities/homePositions/robots/iCubParis01/homePoseFineCalibration.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubParis01/torqueBalancing.ini b/utilities/homePositions/robots/iCubParis01/torqueBalancing.ini new file mode 100644 index 0000000..c5deb0e --- /dev/null +++ b/utilities/homePositions/robots/iCubParis01/torqueBalancing.ini @@ -0,0 +1,19 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface_jtc.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole" "r_sole") + +comKp (50 50 50) +comKd (0 0 0) +comKi (0 0 0) + +kw 1 +kImp (20 20 10 20 20 20 20 20 20 20 20 30 30 30 160 0 0 30 30 30 160 0 0 ) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/iCubParis02/CMakeLists.txt b/utilities/homePositions/robots/iCubParis02/CMakeLists.txt new file mode 100644 index 0000000..738fd69 --- /dev/null +++ b/utilities/homePositions/robots/iCubParis02/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname iCubParis02) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/iCubParis02/homePoseBalancingLeftFoot.ini b/utilities/homePositions/robots/iCubParis02/homePoseBalancingLeftFoot.ini new file mode 100644 index 0000000..ebff77e --- /dev/null +++ b/utilities/homePositions/robots/iCubParis02/homePoseBalancingLeftFoot.ini @@ -0,0 +1,98 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 17 -1 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 94 0 50 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -37 51 0 35 0 0 0 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 17 0 -10 -6 4 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 20 23 0 -21 -3 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubParis02/homePoseBalancingRedBall.ini b/utilities/homePositions/robots/iCubParis02/homePoseBalancingRedBall.ini new file mode 100644 index 0000000..65dd04c --- /dev/null +++ b/utilities/homePositions/robots/iCubParis02/homePoseBalancingRedBall.ini @@ -0,0 +1,98 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -8 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 14.82 30.01 1.71 0.24 7.85 1.81 6.67 0.17 -8.79 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubParis02/homePoseBalancingTwoFeet.ini b/utilities/homePositions/robots/iCubParis02/homePoseBalancingTwoFeet.ini new file mode 100644 index 0000000..18de1e6 --- /dev/null +++ b/utilities/homePositions/robots/iCubParis02/homePoseBalancingTwoFeet.ini @@ -0,0 +1,99 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -3 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -1.6 -5 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -1.6 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/iCubParis02/homePoseFineCalibration.ini b/utilities/homePositions/robots/iCubParis02/homePoseFineCalibration.ini new file mode 100644 index 0000000..82adfa5 --- /dev/null +++ b/utilities/homePositions/robots/iCubParis02/homePoseFineCalibration.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubParis02/homePoseWholeBodyImpedance.ini b/utilities/homePositions/robots/iCubParis02/homePoseWholeBodyImpedance.ini new file mode 100644 index 0000000..9ac19a4 --- /dev/null +++ b/utilities/homePositions/robots/iCubParis02/homePoseWholeBodyImpedance.ini @@ -0,0 +1,37 @@ +//name of the robot +name icub +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 35 0 -45 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 35 0 -45 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/iCubParis02/torqueBalancing.ini b/utilities/homePositions/robots/iCubParis02/torqueBalancing.ini new file mode 100644 index 0000000..d1f3ada --- /dev/null +++ b/utilities/homePositions/robots/iCubParis02/torqueBalancing.ini @@ -0,0 +1,27 @@ +name torqueBalancing +robot icub +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface_jtc.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole" "r_sole") + +comKp (50 50 50) +comKd (0 0 0) +comKi (0 0 0) + +handPosKp (0 0 0 0 0 0 0 0 0 0 0 0 0 0) +handPosKd (0 0 0 0 0 0 0 0 0 0 0 0 0 0) +handPosKi (0 0 0 0 0 0 0 0 0 0 0 0 0 0) + +handForceKp (0 0 0 0 0 0 0 0 0 0 0 0) +handForceKd (0 0 0 0 0 0 0 0 0 0 0 0) +handForceKi (0 0 0 0 0 0 0 0 0 0 0 0) + +kw 1 +kImp (20 20 10 20 20 20 20 20 20 20 20 30 30 30 60 10 10 30 30 30 60 10 10 ) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) + diff --git a/utilities/homePositions/robots/icubGazeboSim/CMakeLists.txt b/utilities/homePositions/robots/icubGazeboSim/CMakeLists.txt new file mode 100644 index 0000000..475818d --- /dev/null +++ b/utilities/homePositions/robots/icubGazeboSim/CMakeLists.txt @@ -0,0 +1,5 @@ +set(robotname icubGazeboSim) + +file(GLOB ini ${CMAKE_CURRENT_SOURCE_DIR}/*.ini) + +yarp_install(FILES ${ini} DESTINATION ${WBC_ROBOTS_INSTALL_DIR}/${robotname}) diff --git a/utilities/homePositions/robots/icubGazeboSim/homePoseBalancing.ini b/utilities/homePositions/robots/icubGazeboSim/homePoseBalancing.ini new file mode 100644 index 0000000..0a95280 --- /dev/null +++ b/utilities/homePositions/robots/icubGazeboSim/homePoseBalancing.ini @@ -0,0 +1,74 @@ +//name of the robot +robot icubSim +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[customPosition_YogaPP] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 -10 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +/$robot/left_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_arm_Position -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +/$robot/right_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_leg_Position 0 0 0 0 1.4 0 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 0 0 0 0 1.4 0 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_TwoFeetStanding] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 -3 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -35.97 29.97 0.06 50.00 0.00 0.00 0.00 +/$robot/left_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_arm_Position -35.97 29.97 0.06 50.00 0.00 0.00 0.00 +/$robot/right_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_leg_Position 12 5 0 -10 -1.6 -5 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 12 5 0 -10 -1.6 -5 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_OnChair] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 0 -4.50 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -90.00 11.0000 80.00 15.00 0.00 0.00 0.00 +/$robot/left_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_arm_Position -90.00 11.0000 80.00 15.00 -0.00 0.00 -0.00 +/$robot/right_arm_Velocity 10 10 10 10 10 10 10 +/$robot/right_leg_Position 84.00 12 0 -100.00 -12.00 0.00 +/$robot/right_leg_Velocity 10 10 10 10 10 10 +/$robot/left_leg_Position 84.00 12 0 -100.00 -12.00 0.00 +/$robot/left_leg_Velocity 10 10 10 10 10 10 + +[customPosition_OnLeftFoot] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 -0.6 2.7 +/$robot/torso_Velocity 5 5 5 +/$robot/left_arm_Position -17.85 19.2 1.17 39.130 0 0 0 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 +/$robot/right_arm_Position -17.85 19.2 1.17 39.130 0 0 0 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 +/$robot/right_leg_Position 1.2 11.55 0 -4 -1.8 -13.2 +/$robot/right_leg_Velocity 10.00 10.00 10.00 10.00 10.00 10.00 +/$robot/left_leg_Position 1.2 -9.45 0 0 1.2 13.6 +/$robot/left_leg_Velocity 5 5 5 5 5 5 + +[customPosition_OnRightFoot] +/$robot/head_Position 0 0 0 0 0 0 +/$robot/head_Velocity 10 10 10 10 10 10 +/$robot/torso_Position 0 -17 1 +/$robot/torso_Velocity 10 10 10 +/$robot/left_arm_Position -37 51 0 35 0 0 0 +/$robot/left_arm_Velocity 10 10 10 10 30 30 30 +/$robot/right_arm_Position -37 94 0 50 0 0 0 +/$robot/right_arm_Velocity 10 10 10 10 30 30 30 +/$robot/right_leg_Position 22 28 0 -17 -6 1.5 +/$robot/right_leg_Velocity 10.00 10.00 10.00 10.00 10.00 10.00 +/$robot/left_leg_Position 12 17 0 -10 -5.5 4 +/$robot/left_leg_Velocity 10 10 10 10 10 10 diff --git a/utilities/homePositions/robots/icubGazeboSim/homePoseBalancingChair.ini b/utilities/homePositions/robots/icubGazeboSim/homePoseBalancingChair.ini new file mode 100644 index 0000000..dcf2483 --- /dev/null +++ b/utilities/homePositions/robots/icubGazeboSim/homePoseBalancingChair.ini @@ -0,0 +1,36 @@ +//name of the robot +name icubSim +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -4.50 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90.00 11.0000 80.00 15.00 0.00 0.00 0.00 +VelocityZero 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90.00 11.0000 80.00 15.00 0.00 0.00 0.00 +VelocityZero 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 84.00 12 0 -100.00 -12.00 0.00 +VelocityZero 10 10 10 10 10 10 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 84.00 12 0 -100.00 -12.00 0.00 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE diff --git a/utilities/homePositions/robots/icubGazeboSim/homePoseBalancingLegsZero.ini b/utilities/homePositions/robots/icubGazeboSim/homePoseBalancingLegsZero.ini new file mode 100644 index 0000000..a45512f --- /dev/null +++ b/utilities/homePositions/robots/icubGazeboSim/homePoseBalancingLegsZero.ini @@ -0,0 +1,37 @@ +//name of the robot +name icubSim +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -3 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +VelocityZero 10 10 10 10 30 30 30 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +VelocityZero 10 10 10 10 30 30 30 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/icubGazeboSim/homePoseBalancingRedBall.ini b/utilities/homePositions/robots/icubGazeboSim/homePoseBalancingRedBall.ini new file mode 100644 index 0000000..36ed55c --- /dev/null +++ b/utilities/homePositions/robots/icubGazeboSim/homePoseBalancingRedBall.ini @@ -0,0 +1,99 @@ +//name of the robot +name icubSim +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -8 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +VelocityZero 10 10 10 10 30 30 30 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +VelocityZero 10 10 10 10 30 30 30 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 0 0 -10 -3.2 0.6 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/icubGazeboSim/homePoseBalancingTwoFeet.ini b/utilities/homePositions/robots/icubGazeboSim/homePoseBalancingTwoFeet.ini new file mode 100644 index 0000000..35565ea --- /dev/null +++ b/utilities/homePositions/robots/icubGazeboSim/homePoseBalancingTwoFeet.ini @@ -0,0 +1,99 @@ +//name of the robot +name icubSim +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -3 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +VelocityZero 10 10 10 10 30 30 30 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +VelocityZero 10 10 10 10 30 30 30 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -1.6 -4 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 12 5 0 -10 -1.6 -5 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utilities/homePositions/robots/icubGazeboSim/homePoseFineCalibration.ini b/utilities/homePositions/robots/icubGazeboSim/homePoseFineCalibration.ini new file mode 100644 index 0000000..0f2d856 --- /dev/null +++ b/utilities/homePositions/robots/icubGazeboSim/homePoseFineCalibration.ini @@ -0,0 +1,37 @@ +//name of the robot +name icubSim +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 0 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -90 15 15 90.00 -0.00 0.00 -0.00 15 30 10 0 0 0 10 0 10 +VelocityZero 10 10 10 10 30 30 30 10 10 10 10 10 10 10 10 10 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE + diff --git a/utilities/homePositions/robots/icubGazeboSim/homePoseYogaPP.ini b/utilities/homePositions/robots/icubGazeboSim/homePoseYogaPP.ini new file mode 100644 index 0000000..829ddea --- /dev/null +++ b/utilities/homePositions/robots/icubGazeboSim/homePoseYogaPP.ini @@ -0,0 +1,36 @@ +//name of the robot +name icubSim +//parts to be opened by the GUI +parts (head torso left_arm right_arm right_leg left_leg) + +[head_zero] +// 0 1 2 +PositionZero 0 0 0 0 0 0 +VelocityZero 10 10 10 10 10 10 + +[torso_zero] +// 0 1 2 +PositionZero 0 0 -10 +VelocityZero 10 10 10 + +[left_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +VelocityZero 10 10 10 10 30 30 30 + +[right_arm_zero] +//Joint 0 1 2 3 4 5 6 +PositionZero -35.97 29.97 0.06 50.00 -0.00 0.00 -0.00 +VelocityZero 10 10 10 10 30 30 30 + +[right_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 1.4 0 +VelocityZero 10.00 10.00 10.00 10.00 10.00 10.00 + +[left_leg_zero] +//Joint 0 1 2 3 4 5 +PositionZero 0 0 0 0 1.4 0 +VelocityZero 10 10 10 10 10 10 + +//DO NOT REMOVE THIS LINE diff --git a/utilities/homePositions/robots/icubGazeboSim/torqueBalancing.ini b/utilities/homePositions/robots/icubGazeboSim/torqueBalancing.ini new file mode 100644 index 0000000..3019915 --- /dev/null +++ b/utilities/homePositions/robots/icubGazeboSim/torqueBalancing.ini @@ -0,0 +1,19 @@ +name torqueBalancing +robot icubSim +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole" "r_sole") +smooth ("joints", 1.0) + +comKp (50 50 50) +comKd (14 14 14) +comKi (0 0 0) + +kw 1 +kImp (20 20 10 20 20 20 20 20 20 20 20 30 30 30 60 10 10 30 30 30 60 10 10 ) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) diff --git a/utilities/homePositions/robots/icubGazeboSim/torqueBalancingLeftFoot.ini b/utilities/homePositions/robots/icubGazeboSim/torqueBalancingLeftFoot.ini new file mode 100644 index 0000000..93d7440 --- /dev/null +++ b/utilities/homePositions/robots/icubGazeboSim/torqueBalancingLeftFoot.ini @@ -0,0 +1,19 @@ +name torqueBalancing +robot icubSim +period 10 +modulePeriod 0.01 + +wbi_config_file yarpWholeBodyInterface.ini +wbi_joint_list ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP + +constraint_links ("l_sole") + +comKp (40 46.67 40) +comKd (1 1 1) +comKi (0 0 0) + +kw 1 +kImp (20 20 20 13 13 13 5 13 13 13 5 70 70 65 30 0 0 20 20 20 10 0 0) + +tsat (24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 24 ) +