From 0567206c5c0fae359a9abf5c384759e058f9a7e9 Mon Sep 17 00:00:00 2001 From: Gabriele Nava Date: Fri, 8 Mar 2019 16:58:45 +0100 Subject: [PATCH] Porting to WBToolbox v.5.0 (#63) * cleanup * migration to wb5.0 * new yoga controller under testing * almost working yoga demo * working yoga in simulation * added compatibility with matlab 2017b * updated readme * bugfix after testing on iCubGenova04 * models ported to WBT5 * updated simulink GUI --- .gitattributes | 1 - .gitignore | 1 - CMakeLists.txt | 4 +- README.md | 106 +- cmake/CreateAutogeneratedCodeTargets.cmake.in | 2 +- cmake/RegisterMdl.cmake | 3 +- config/README.md | 11 +- config/{startup.m => startup_WBC.m} | 6 +- controllers/CMakeLists.txt | 1 + controllers/README.md | 13 + .../fixed-base-joints-control/README.md | 21 + .../iCubGazeboV2_5/configJointsControl.m | 13 + .../app/robots/iCubGazeboV2_5/configRobot.m | 36 +- .../iCubGazeboV2_5/gainsAndReferences.m | 28 + .../robots/iCubGenova02/configJointsControl.m | 13 + .../app/robots/iCubGenova02/configRobot.m | 36 +- .../robots/iCubGenova02/gainsAndReferences.m | 28 + .../robots/iCubGenova04/configJointsControl.m | 13 + .../app/robots/iCubGenova04/configRobot.m | 36 +- .../robots/iCubGenova04/gainsAndReferences.m | 28 + .../icubGazeboSim/configJointsControl.m | 13 + .../app/robots/icubGazeboSim/configRobot.m | 34 + .../robots/icubGazeboSim/gainsAndReferences.m | 28 + .../initJointsControl.m | 37 +- .../jointsControl.mdl | 5555 +++ .../src-static-gui/closeModel.m | 20 + .../src-static-gui}/compileModel.m | 9 +- .../startModelWithStaticGui.m | 8 +- .../stopJointsControl.m | 59 + .../README.md | 21 + .../iCubGazeboV2_5/configPositionBalancing.m | 22 + .../app/robots/iCubGazeboV2_5}/configRobot.m | 39 +- .../iCubGazeboV2_5/gainsAndReferences.m | 27 + .../initPositionControlBalancing.m | 57 + .../positionControlBalancing.mdl | 11281 ++++++ .../src-static-gui/closeModel.m | 20 + .../src-static-gui/compileModel.m | 24 + .../src/computeBasePoseDerivative.m | 20 + .../src/computePositionAndOrientationErrors.m | 33 + .../src/computeStateAccelerations.m | 51 + .../src/referenceGeneratorCoM.m | 18 + .../startModelWithStaticGui.m | 29 + .../stopPositionControlBalancing.m | 6 +- .../CMakeLists.txt | 1 + .../README.md | 23 + .../app/robots/iCubGazeboV2_5/configRobot.m | 77 +- .../iCubGazeboV2_5/configStateMachine.m | 160 + .../iCubGazeboV2_5/gainsAndReferences.m | 342 + .../app/robots/iCubGenova02}/configRobot.m | 64 +- .../robots/iCubGenova02/configStateMachine.m | 160 + .../robots/iCubGenova02/gainsAndReferences.m | 346 + .../app/robots/iCubGenova04}/configRobot.m | 62 +- .../robots/iCubGenova04/configStateMachine.m | 160 + .../robots/iCubGenova04/gainsAndReferences.m | 351 + .../app/robots/icubGazeboSim/configRobot.m | 64 +- .../robots/icubGazeboSim/configStateMachine.m | 159 + .../robots/icubGazeboSim/gainsAndReferences.m | 238 + .../initTorqueControlBalancing.m | 83 + .../src-static-gui/closeModel.m | 20 + .../src-static-gui}/compileModel.m | 9 +- .../src/momentumBasedController.m | 328 + .../src/processOutputQP_oneFoot.m | 57 + .../src/processOutputQP_twoFeet.m | 41 + .../src/stateMachineMomentumControl.m | 393 + .../startModelWithStaticGui.m | 8 +- .../stopTorqueControlBalancing.m | 6 +- .../torqueControlBalancing.mdl | 22965 ++++++++++++ ...How-to-run-torqueBalancing-on-real-iCub.md | 36 +- ...n-torqueBalancing-simulations-with-iCub.md | 47 +- ...-to-setup-the-robot-for-wbc-experiments.md | 45 +- doc/README.md | 2 +- library/README.md | 8 +- library/matlab-gui/closeModel_template.m | 21 + library/matlab-gui/compileModel_template.m | 26 + library/matlab-gui/simulinkStaticGUI.fig | Bin 0 -> 8388 bytes library/matlab-gui/simulinkStaticGUI.m | 184 + library/matlab-gui/torqueBalancingGui.fig | Bin 7378 -> 0 bytes library/matlab-gui/torqueBalancingGui.m | 164 - library/matlab-wbc/+wbc/addMotorsInertia.m | 70 - .../+wbc/addMotorsReflectedInertia.m | 69 + .../matlab-wbc/+wbc/analyticalSolutionQP.m | 12 +- library/matlab-wbc/+wbc/basePoseDerivative.m | 25 +- library/matlab-wbc/+wbc/checkRange.m | 8 +- library/matlab-wbc/+wbc/checkSpikes.m | 1 + library/matlab-wbc/+wbc/computeBaseVelocity.m | 35 - .../+wbc/computeBaseVelocityWithFeetContact.m | 37 + .../matlab-wbc/+wbc/computeMotorsInertia.m | 28 - .../+wbc/computeMotorsReflectedInertia.m | 32 + .../+wbc/computeRigidContactConstraints.m | 105 + library/matlab-wbc/+wbc/constraints.m | 94 - .../matlab-wbc/+wbc/contactsTransitionQP.m | 38 - .../matlab-wbc/+wbc/correctIMUWithNeckPos.m | 62 + .../matlab-wbc/+wbc/correctImuWithNeckPos.m | 42 - .../+wbc/diagonalMatrixFromVector.m | 22 + library/matlab-wbc/+wbc/evalDHMatrix.m | 28 - .../matlab-wbc/+wbc/fromPosQuatToTransfMatr.m | 6 +- .../+wbc/fromPosRpyToTransfMatrix.m | 6 +- library/matlab-wbc/+wbc/linearPID.m | 2 +- library/matlab-wbc/+wbc/pinvDamped.m | 4 +- .../matlab-wbc/+wbc/posRotToTransfMatrix.m | 8 +- .../matlab-wbc/+wbc/processOneFootOutputQP.m | 33 - library/matlab-wbc/+wbc/processOutputQP.m | 39 - .../matlab-wbc/+wbc/quaternionDerivative.m | 14 +- .../matlab-wbc/+wbc/referenceGeneratorCoM.m | 48 - .../+wbc/robotIsOnSingleSupportQP.m | 42 + .../+wbc/rotationDerivativeFromAngVel.m | 6 +- .../matlab-wbc/+wbc/rotationFromQuaternion.m | 4 +- .../+wbc/rotationFromRollPitchYaw.m | 6 +- .../+wbc/rotationalPID_acceleration.m | 4 +- .../matlab-wbc/+wbc/rotationalPID_velocity.m | 2 +- library/matlab-wbc/+wbc/rotx.m | 2 +- library/matlab-wbc/+wbc/roty.m | 2 +- library/matlab-wbc/+wbc/rotz.m | 2 +- .../matlab-wbc/+wbc/saturateInputDerivative.m | 73 + library/matlab-wbc/+wbc/skew.m | 3 +- library/matlab-wbc/+wbc/skewVee.m | 5 +- .../+wbc/worldToBaseTransformWithIMU.m | 83 +- .../simulink-library/CMM_iCub_23_25_DoFs.mdl | 7280 ++++ torque-controllers/CMakeLists.txt | 1 - torque-controllers/README.md | 14 - .../impedance-control/README.md | 23 - .../impedance-control/impedanceControl.mdl | 4751 --- .../impedance-control/stopImpedanceControl.m | 22 - .../momentum-based-standup/README.md | 51 - .../app/robots/iCubGenova02/configRobot.m | 104 - .../app/robots/iCubGenova02/initCoordinator.m | 215 - .../iCubGenova02/initStateMachineStandup.m | 209 - .../app/robots/icubGazeboSim/configRobot.m | 104 - .../robots/icubGazeboSim/initCoordinator.m | 215 - .../icubGazeboSim/initStateMachineStandup.m | 209 - .../initTorqueBalancingStandup.m | 110 - .../src-gui/closeModel.m | 17 - .../src/balancingControllerStandup.m | 275 - .../src/legsContactDetector.m | 15 - .../src/stateMachineStandup.m | 118 - .../torqueBalancingStandup.mdl | 30531 ---------------- .../momentum-based-yoga/CMakeLists.txt | 1 - .../momentum-based-yoga/README.md | 24 - .../robots/iCubGazeboV2_5/initCoordinator.m | 238 - .../iCubGazeboV2_5/initStateMachineYoga.m | 512 - .../app/robots/iCubGenova02/initCoordinator.m | 238 - .../iCubGenova02/initStateMachineYoga.m | 514 - .../app/robots/iCubGenova04/initCoordinator.m | 238 - .../iCubGenova04/initStateMachineYoga.m | 517 - .../robots/icubGazeboSim/initCoordinator.m | 238 - .../icubGazeboSim/initStateMachineYoga.m | 398 - .../initTorqueBalancingYoga.m | 105 - .../momentum-based-yoga/src-gui/closeModel.m | 17 - .../src/balancingControllerYoga.m | 227 - .../src/saturateInputDerivative.m | 58 - .../src/stateMachineYoga.m | 460 - .../torqueBalancingYoga.mdl | 27700 -------------- utilities/README.md | 15 +- utilities/debug_BoschIMU.mdl | 52 +- utilities/debug_FTExternalForces.mdl | 558 +- utilities/debug_FTMeas.mdl | 142 +- utilities/debug_FTMeas_shoes.mdl | 106 +- utilities/debug_seesawIMU.mdl | 282 +- utilities/debug_xSensIMU.mdl | 233 +- utilities/releaseLegStressesWhileStanding.sh | 5 +- 160 files changed, 52369 insertions(+), 70336 deletions(-) rename config/{startup.m => startup_WBC.m} (93%) create mode 100644 controllers/CMakeLists.txt create mode 100644 controllers/README.md create mode 100644 controllers/fixed-base-joints-control/README.md create mode 100644 controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/configJointsControl.m rename {torque-controllers/impedance-control => controllers/fixed-base-joints-control}/app/robots/iCubGazeboV2_5/configRobot.m (65%) create mode 100644 controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m create mode 100644 controllers/fixed-base-joints-control/app/robots/iCubGenova02/configJointsControl.m rename {torque-controllers/impedance-control => controllers/fixed-base-joints-control}/app/robots/iCubGenova02/configRobot.m (65%) create mode 100644 controllers/fixed-base-joints-control/app/robots/iCubGenova02/gainsAndReferences.m create mode 100644 controllers/fixed-base-joints-control/app/robots/iCubGenova04/configJointsControl.m rename {torque-controllers/impedance-control => controllers/fixed-base-joints-control}/app/robots/iCubGenova04/configRobot.m (65%) create mode 100644 controllers/fixed-base-joints-control/app/robots/iCubGenova04/gainsAndReferences.m create mode 100644 controllers/fixed-base-joints-control/app/robots/icubGazeboSim/configJointsControl.m create mode 100644 controllers/fixed-base-joints-control/app/robots/icubGazeboSim/configRobot.m create mode 100644 controllers/fixed-base-joints-control/app/robots/icubGazeboSim/gainsAndReferences.m rename torque-controllers/impedance-control/initImpedanceControl.m => controllers/fixed-base-joints-control/initJointsControl.m (60%) create mode 100644 controllers/fixed-base-joints-control/jointsControl.mdl create mode 100644 controllers/fixed-base-joints-control/src-static-gui/closeModel.m rename {torque-controllers/momentum-based-standup/src-gui => controllers/fixed-base-joints-control/src-static-gui}/compileModel.m (73%) rename torque-controllers/momentum-based-yoga/startYogaWithoutSimulinkGui.m => controllers/fixed-base-joints-control/startModelWithStaticGui.m (83%) create mode 100644 controllers/fixed-base-joints-control/stopJointsControl.m create mode 100644 controllers/floating-base-balancing-position-control/README.md create mode 100644 controllers/floating-base-balancing-position-control/app/robots/iCubGazeboV2_5/configPositionBalancing.m rename {torque-controllers/impedance-control/app/robots/icubGazeboSim => controllers/floating-base-balancing-position-control/app/robots/iCubGazeboV2_5}/configRobot.m (67%) create mode 100644 controllers/floating-base-balancing-position-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m create mode 100644 controllers/floating-base-balancing-position-control/initPositionControlBalancing.m create mode 100644 controllers/floating-base-balancing-position-control/positionControlBalancing.mdl create mode 100644 controllers/floating-base-balancing-position-control/src-static-gui/closeModel.m create mode 100644 controllers/floating-base-balancing-position-control/src-static-gui/compileModel.m create mode 100644 controllers/floating-base-balancing-position-control/src/computeBasePoseDerivative.m create mode 100644 controllers/floating-base-balancing-position-control/src/computePositionAndOrientationErrors.m create mode 100644 controllers/floating-base-balancing-position-control/src/computeStateAccelerations.m create mode 100644 controllers/floating-base-balancing-position-control/src/referenceGeneratorCoM.m create mode 100644 controllers/floating-base-balancing-position-control/startModelWithStaticGui.m rename torque-controllers/momentum-based-yoga/stopTorqueBalancingYoga.m => controllers/floating-base-balancing-position-control/stopPositionControlBalancing.m (85%) create mode 100644 controllers/floating-base-balancing-torque-control/CMakeLists.txt create mode 100644 controllers/floating-base-balancing-torque-control/README.md rename {torque-controllers/momentum-based-yoga => controllers/floating-base-balancing-torque-control}/app/robots/iCubGazeboV2_5/configRobot.m (65%) create mode 100644 controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/configStateMachine.m create mode 100644 controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m rename {torque-controllers/momentum-based-yoga/app/robots/iCubGenova04 => controllers/floating-base-balancing-torque-control/app/robots/iCubGenova02}/configRobot.m (70%) create mode 100644 controllers/floating-base-balancing-torque-control/app/robots/iCubGenova02/configStateMachine.m create mode 100644 controllers/floating-base-balancing-torque-control/app/robots/iCubGenova02/gainsAndReferences.m rename {torque-controllers/momentum-based-yoga/app/robots/iCubGenova02 => controllers/floating-base-balancing-torque-control/app/robots/iCubGenova04}/configRobot.m (70%) create mode 100644 controllers/floating-base-balancing-torque-control/app/robots/iCubGenova04/configStateMachine.m create mode 100644 controllers/floating-base-balancing-torque-control/app/robots/iCubGenova04/gainsAndReferences.m rename {torque-controllers/momentum-based-yoga => controllers/floating-base-balancing-torque-control}/app/robots/icubGazeboSim/configRobot.m (70%) create mode 100644 controllers/floating-base-balancing-torque-control/app/robots/icubGazeboSim/configStateMachine.m create mode 100644 controllers/floating-base-balancing-torque-control/app/robots/icubGazeboSim/gainsAndReferences.m create mode 100644 controllers/floating-base-balancing-torque-control/initTorqueControlBalancing.m create mode 100644 controllers/floating-base-balancing-torque-control/src-static-gui/closeModel.m rename {torque-controllers/momentum-based-yoga/src-gui => controllers/floating-base-balancing-torque-control/src-static-gui}/compileModel.m (70%) create mode 100644 controllers/floating-base-balancing-torque-control/src/momentumBasedController.m create mode 100644 controllers/floating-base-balancing-torque-control/src/processOutputQP_oneFoot.m create mode 100644 controllers/floating-base-balancing-torque-control/src/processOutputQP_twoFeet.m create mode 100644 controllers/floating-base-balancing-torque-control/src/stateMachineMomentumControl.m rename torque-controllers/momentum-based-standup/startStandupWithoutSimulinkGui.m => controllers/floating-base-balancing-torque-control/startModelWithStaticGui.m (83%) rename torque-controllers/momentum-based-standup/stopTorqueBalancingStandup.m => controllers/floating-base-balancing-torque-control/stopTorqueControlBalancing.m (85%) create mode 100644 controllers/floating-base-balancing-torque-control/torqueControlBalancing.mdl create mode 100644 library/matlab-gui/closeModel_template.m create mode 100644 library/matlab-gui/compileModel_template.m create mode 100644 library/matlab-gui/simulinkStaticGUI.fig create mode 100644 library/matlab-gui/simulinkStaticGUI.m delete mode 100644 library/matlab-gui/torqueBalancingGui.fig delete mode 100644 library/matlab-gui/torqueBalancingGui.m delete mode 100644 library/matlab-wbc/+wbc/addMotorsInertia.m create mode 100644 library/matlab-wbc/+wbc/addMotorsReflectedInertia.m delete mode 100644 library/matlab-wbc/+wbc/computeBaseVelocity.m create mode 100644 library/matlab-wbc/+wbc/computeBaseVelocityWithFeetContact.m delete mode 100644 library/matlab-wbc/+wbc/computeMotorsInertia.m create mode 100644 library/matlab-wbc/+wbc/computeMotorsReflectedInertia.m create mode 100644 library/matlab-wbc/+wbc/computeRigidContactConstraints.m delete mode 100644 library/matlab-wbc/+wbc/constraints.m delete mode 100644 library/matlab-wbc/+wbc/contactsTransitionQP.m create mode 100644 library/matlab-wbc/+wbc/correctIMUWithNeckPos.m delete mode 100644 library/matlab-wbc/+wbc/correctImuWithNeckPos.m create mode 100644 library/matlab-wbc/+wbc/diagonalMatrixFromVector.m delete mode 100644 library/matlab-wbc/+wbc/evalDHMatrix.m delete mode 100644 library/matlab-wbc/+wbc/processOneFootOutputQP.m delete mode 100644 library/matlab-wbc/+wbc/processOutputQP.m delete mode 100644 library/matlab-wbc/+wbc/referenceGeneratorCoM.m create mode 100644 library/matlab-wbc/+wbc/robotIsOnSingleSupportQP.m create mode 100644 library/matlab-wbc/+wbc/saturateInputDerivative.m create mode 100644 library/simulink-library/CMM_iCub_23_25_DoFs.mdl delete mode 100644 torque-controllers/CMakeLists.txt delete mode 100644 torque-controllers/README.md delete mode 100644 torque-controllers/impedance-control/README.md delete mode 100644 torque-controllers/impedance-control/impedanceControl.mdl delete mode 100644 torque-controllers/impedance-control/stopImpedanceControl.m delete mode 100644 torque-controllers/momentum-based-standup/README.md delete mode 100644 torque-controllers/momentum-based-standup/app/robots/iCubGenova02/configRobot.m delete mode 100644 torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m delete mode 100644 torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m delete mode 100644 torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/configRobot.m delete mode 100644 torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m delete mode 100644 torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m delete mode 100644 torque-controllers/momentum-based-standup/initTorqueBalancingStandup.m delete mode 100644 torque-controllers/momentum-based-standup/src-gui/closeModel.m delete mode 100644 torque-controllers/momentum-based-standup/src/balancingControllerStandup.m delete mode 100644 torque-controllers/momentum-based-standup/src/legsContactDetector.m delete mode 100644 torque-controllers/momentum-based-standup/src/stateMachineStandup.m delete mode 100644 torque-controllers/momentum-based-standup/torqueBalancingStandup.mdl delete mode 100644 torque-controllers/momentum-based-yoga/CMakeLists.txt delete mode 100644 torque-controllers/momentum-based-yoga/README.md delete mode 100644 torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m delete mode 100644 torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initStateMachineYoga.m delete mode 100644 torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m delete mode 100644 torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m delete mode 100644 torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m delete mode 100644 torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m delete mode 100644 torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m delete mode 100644 torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m delete mode 100644 torque-controllers/momentum-based-yoga/initTorqueBalancingYoga.m delete mode 100644 torque-controllers/momentum-based-yoga/src-gui/closeModel.m delete mode 100644 torque-controllers/momentum-based-yoga/src/balancingControllerYoga.m delete mode 100644 torque-controllers/momentum-based-yoga/src/saturateInputDerivative.m delete mode 100644 torque-controllers/momentum-based-yoga/src/stateMachineYoga.m delete mode 100644 torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl diff --git a/.gitattributes b/.gitattributes index 21a6cd9..516f0d9 100644 --- a/.gitattributes +++ b/.gitattributes @@ -2,4 +2,3 @@ *.mdl -crlf -diff -merge *.mat -crlf -diff -merge *.mlx -crlf -diff -merge - diff --git a/.gitignore b/.gitignore index b3f7e7d..8aea83d 100644 --- a/.gitignore +++ b/.gitignore @@ -43,4 +43,3 @@ slprj/ ##--------------------------------------------------- *.sublime-project *.sublime-workspace - diff --git a/CMakeLists.txt b/CMakeLists.txt index b82dd5e..b6fdf04 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,7 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") include(RegisterMdl) initialize_mdl_set(NAME WHOLE_BODY_CONTROLLERS) -add_subdirectory(torque-controllers) +add_subdirectory(controllers) # ============================ # EXPORT AUTOGENERATED SOURCES @@ -73,4 +73,4 @@ 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 diff --git a/README.md b/README.md index c85eca3..87e56e8 100644 --- a/README.md +++ b/README.md @@ -1,23 +1,23 @@ # whole-body-controllers -Simulink-based whole body controllers for humanoid robots. +Simulink-based whole-body controllers for humanoid robots. ## Dependencies -This repository depends upon the following Software/repositories: +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) +- [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) and [icub-gazebo-wholebody](https://github.com/robotology-playground/icub-gazebo-wholebody) to access iCub models. +- [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). -**NOTE:** it is suggested to install `whole-body-controllers` and most of its dependencies (namely, `codyco-modules`,`icub-gazebo`,`icub-gazebo-wholebody`, `gazebo-yarp-plugins` 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). - ## Installation and usage -- 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, `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). + +- 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. - 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: @@ -25,14 +25,18 @@ This repository depends upon the following Software/repositories: |:-------------:|:-------------:| | iCubGenova02 | [model.urdf](https://github.com/robotology/icub-models/blob/master/iCub/robots/iCubGenova02/model.urdf) | | iCubGenova04 | [model.urdf](https://github.com/robotology/icub-models/blob/master/iCub/robots/iCubGenova04/model.urdf) | - | iCubGazeboV2_5 |[model.urdf](https://github.com/robotology/icub-models/blob/master/iCub/robots/iCubGazeboV2_5/model.urdf)| + | 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: - - directly add the folder to the Matlab path; - - run **only once** the [startup.m](config/startup.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). +- 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. + + `[outputs] = wbc.myFunction(inputs)`. - **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 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). ## Troubleshooting @@ -42,7 +46,7 @@ Please refer to the [WBToolbox troubleshooting documentation](https://robotology - **config**: a collection of scripts for correctly configure this repo. [[README]](config/README.md) -- **torque-controllers**: Simulink torque controllers for balancing and walking of humanoid robots. [[README]](torque-controllers/README.md) +- **controllers**: Simulink whole-body position and torque controllers for balancing of humanoid robots. [[README]](controllers/README.md) - **doc**: guidelines on how to create/use Simulink models for control. [[README]](doc/README.md) @@ -50,49 +54,63 @@ Please refer to the [WBToolbox troubleshooting documentation](https://robotology - **utilities**: Simulink models for debugging sensors on the real robot. [[README]](utilities/README.md) -## Interesting features - -### Automatic generation of c++ code from Simulink - -There is the possibility to generate c++ code from the Simulink models using [Simulink coder](https://www.mathworks.com/products/simulink-coder.html) (**available only for the [momentum-based-yoga](torque-controllers/momentum-based-yoga)**). The repositiory that contains the generated c++ code is named [autogenerated-whole-body-controllers](https://github.com/robotology-playground/autogenerated-whole-body-controllers). Documentation on how to generate the code is available in the repository [wiki](https://github.com/robotology-playground/autogenerated-whole-body-controllers/wiki/How-to-generate-code-from-a-Simulink-model). +### Available controllers -### Static GUI for Simulink - -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. **Available only for [momentum-based-yoga](torque-controllers/momentum-based-yoga) and [momentum-based-standup](torque-controllers/momentum-based-standup)**. If you want to run Simulink with the static GUI, just run the [start{$SIM_NAME}WithoutSimulinkGui](torque-controllers/momentum-based-yoga/startYogaWithoutSimulinkGui.m) script. +- [fixed-base-joints-control](controllers/fixed-base-joints-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) -## Where do I find new features and legacy repos? +### Matlab functions library -#### Available controllers +- [matlab WBC library](library/matlab-wbc) -- [impedance-control](https://github.com/robotology/whole-body-controllers/tree/master/torque-controllers/impedance-control) -- [momentum-based-standup](https://github.com/robotology/whole-body-controllers/tree/master/torque-controllers/momentum-based-standup) -- [momentum-based-yoga](https://github.com/robotology/whole-body-controllers/tree/master/torque-controllers/momentum-based-yoga) -- [utilities](https://github.com/robotology/whole-body-controllers/tree/master/utilities) +## Additional features -#### Matlab functions library +### Automatic generation of c++ code from Simulink -- [matlab library](library/matlab-wbc) +There is the possibility to generate c++ code from the Simulink models using [Simulink coder](https://www.mathworks.com/products/simulink-coder.html) (**available only for the [floating-base-balancing-torque-control](controllers/floating-base-balancing-torque-control)**). The repositiory that contains the generated c++ code is named [autogenerated-whole-body-controllers](https://github.com/robotology-playground/autogenerated-whole-body-controllers). Documentation on how to generate the code is available in the repository [wiki](https://github.com/robotology-playground/autogenerated-whole-body-controllers/wiki/How-to-generate-code-from-a-Simulink-model). -#### Active Forks (new features) +### Static GUI for Simulink -- [force-parametrization](https://github.com/ahmadgazar/whole-body-controllers) -- [PhRI-standup](https://github.com/Yeshasvitvs/wholeBodyControllers) +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. -#### Legacy +## 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-playground/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. +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-playground/WBI-Toolbox-controllers/tree/whole-body-controllers-legacy/controllers/legacy) -- [seesaw controller and integration-based-ikin](https://github.com/robotology-playground/WBI-Toolbox-controllers/tree/whole-body-controllers-legacy/controllers/legacy) +- [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) - [elastic joints control](https://github.com/robotology/mex-wholebodymodel/tree/master/controllers/torqueBalancing_JE) -- [walkman control](https://github.com/robotology-playground/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/whole-body-controllers-legacy/controllers/legacy/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) -#### Associated repositories - -- [matlab-multi-body-sim](https://github.com/gabrielenava/matlab-multi-body-sim) - -### Mantainers - -Gabriele Nava ([@gabrielenava](https://github.com/gabrielenava)) +## Citing this work + +If you are using this code for your research activity and you're willing to cite it, you may add the following references to your bibliography: + +``` + @INPROCEEDINGS{Nava_etal2016, + author={G. Nava and F. Romano and F. Nori and D. Pucci}, + booktitle={2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + title={Stability analysis and design of momentum-based controllers for humanoid robots}, + year={2016}, + pages={680-687}, + doi={10.1109/IROS.2016.7759126}, + month={Oct}, + } +``` + +``` + @article{Nori_etal2015, + author="Nori, F. and Traversaro, S. and Eljaik, J. and Romano, F. and Del Prete, A. and Pucci, D.", + title="iCub whole-body control through force regulation on rigid non-coplanar contacts", + year="2015", + journal="Frontiers in {R}obotics and {A}{I}", + volume="1", + } +``` + +## Mantainers + +Gabriele Nava ([@gabrielenava](https://github.com/gabrielenava)) \ No newline at end of file diff --git a/cmake/CreateAutogeneratedCodeTargets.cmake.in b/cmake/CreateAutogeneratedCodeTargets.cmake.in index 19f25ba..85c38a1 100644 --- a/cmake/CreateAutogeneratedCodeTargets.cmake.in +++ b/cmake/CreateAutogeneratedCodeTargets.cmake.in @@ -19,4 +19,4 @@ add_executable(${CODER_MAIN} "${CMAKE_CURRENT_BINARY_DIR}/driver.cpp" ) -target_link_libraries(${CODER_MAIN} PUBLIC ${MDL_NAME}_LIB) +target_link_libraries(${CODER_MAIN} PUBLIC ${MDL_NAME}_LIB) \ No newline at end of file diff --git a/cmake/RegisterMdl.cmake b/cmake/RegisterMdl.cmake index 00b0345..82ae584 100644 --- a/cmake/RegisterMdl.cmake +++ b/cmake/RegisterMdl.cmake @@ -25,7 +25,6 @@ function(initialize_mdl_set) FULL_DOCS "Array with the path of the folders to export containing autogenerated code.") endfunction() - function(register_mdl) # =============== @@ -63,4 +62,4 @@ function(register_mdl) # Register the mdl by appending its name to the global property set_property(GLOBAL APPEND PROPERTY ${SETNAME} ${AUTOGENSOURCES_ABSPATH}) -endfunction() +endfunction() \ No newline at end of file diff --git a/config/README.md b/config/README.md index 4c91eac..4335ca9 100644 --- a/config/README.md +++ b/config/README.md @@ -1,12 +1,7 @@ # Configuration scripts -A collection of scripts used for correctly configure the repo. - -- [export_WBC.m](export_WBC.m): run this script once. Then, digit the Matlab version in which you want to export the Simulink models. All models in the repo will be exported in 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.m](startup.m): run this script once. Then, the path to the `matlab-wbc` folder will be permanently added to the `pathdef.m` file, which is 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 (in general, `~/Documents/MATLAB`). - - - +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 diff --git a/config/startup.m b/config/startup_WBC.m similarity index 93% rename from config/startup.m rename to config/startup_WBC.m index eaaf67f..5187368 100644 --- a/config/startup.m +++ b/config/startup_WBC.m @@ -1,9 +1,9 @@ -%% startup.m +%% startup_WBC.m % -% Run this script only once to permanently add the matlab-wbc folder to your MATLAB path. +% Run this script once to permanently add the matlab-wbc library to your MATLAB path. fprintf('\n## whole-body-controllers ##\n'); -fprintf('\nAdding "matlab-wbc" to pathdef.m...\n\n'); +fprintf('\nAdding "matlab-wbc" library to pathdef.m...\n\n'); % path to whole-body-controllers pathToWBC = pwd; diff --git a/controllers/CMakeLists.txt b/controllers/CMakeLists.txt new file mode 100644 index 0000000..795ada0 --- /dev/null +++ b/controllers/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(floating-base-balancing-torque-control) diff --git a/controllers/README.md b/controllers/README.md new file mode 100644 index 0000000..6a7fce2 --- /dev/null +++ b/controllers/README.md @@ -0,0 +1,13 @@ +# Controllers + +Simulink controllers for humanoid robots. + +## List of available controllers: + +- **fixed-base-joints-control** [[README]](fixed-base-joints-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) + +## 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 diff --git a/controllers/fixed-base-joints-control/README.md b/controllers/fixed-base-joints-control/README.md new file mode 100644 index 0000000..870d034 --- /dev/null +++ b/controllers/fixed-base-joints-control/README.md @@ -0,0 +1,21 @@ +## 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. + +### Compatibility + +The folder contains the Simulink model `jointsControl.mdl`, which is generated by using Matlab R2017b. + +### Supported robots + +Currently, supported robots are: `iCubGenova04`, `iCunGenova02`, `icubGazeboSim`, `iCubGazeboV2_5`. + +## Module details + +### 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 diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/configJointsControl.m b/controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/configJointsControl.m new file mode 100644 index 0000000..0a160cd --- /dev/null +++ b/controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/configJointsControl.m @@ -0,0 +1,13 @@ +% CONFIGJOINTSCONTROL configures all the options associated to the +% joints controller. +% + +%% --- Initialization --- + +% Default behaviour: gravity compensation. If Config.MOVE_JOINTS = true, +% the robot will also move all actuated joints following a sine trajectory +Config.MOVE_JOINTS = false; + +% Max unsigned difference between two consecutive (measured) joint positions, +% i.e. delta_jointPos = abs(jointPos(k) - jointPos(k-1)) [rad] +Sat.maxJointsPositionDelta = 15*pi/180; \ No newline at end of file diff --git a/torque-controllers/impedance-control/app/robots/iCubGazeboV2_5/configRobot.m b/controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/configRobot.m similarity index 65% rename from torque-controllers/impedance-control/app/robots/iCubGazeboV2_5/configRobot.m rename to controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/configRobot.m index e717386..1b578d9 100644 --- a/torque-controllers/impedance-control/app/robots/iCubGazeboV2_5/configRobot.m +++ b/controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/configRobot.m @@ -8,15 +8,11 @@ Config.ON_GAZEBO = true; ROBOT_DOF = 23; -% 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] - % Robot configuration for WBToolbox -WBTConfigRobot = WBToolbox.Configuration; -WBTConfigRobot.RobotName = 'icubSim'; -WBTConfigRobot.UrdfFile = 'model.urdf'; -WBTConfigRobot.LocalName = 'WBT'; +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'}; @@ -35,28 +31,4 @@ WBTConfigRobot.ControlledJoints = [WBTConfigRobot.ControlledJoints, ... ControlBoards.(WBTConfigRobot.ControlBoardsNames{n})]; Config.numOfJointsForEachControlboard = [Config.numOfJointsForEachControlboard; length(ControlBoards.(WBTConfigRobot.ControlBoardsNames{n}))]; -end - -% References for the demo movements -if MOVING - - % Impedance gains - Kp = 10*diag(ones(1,ROBOT_DOF)); - Kd = 2*sqrt(Kp); - - AMPLS = 5*ones(1,ROBOT_DOF); - FREQS = 0.25*ones(1,ROBOT_DOF); - -else - - % Impedance gains - Kp = 0*diag(ones(1,ROBOT_DOF)); - Kd = 0*diag(ones(1,ROBOT_DOF)); - - AMPLS = 0*ones(1,ROBOT_DOF); - FREQS = 0*ones(1,ROBOT_DOF); -end - -if size(Kp,1) ~= ROBOT_DOF - error('Dimension of Kp different from ROBOT_DOF') end \ No newline at end of file diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m b/controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m new file mode 100644 index 0000000..74fe404 --- /dev/null +++ b/controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m @@ -0,0 +1,28 @@ +% GAINSANDREFERENCES initializes the controller parameters: gains, +% regularization parameters and references. +% + +%% --- Initialization --- + +% References for the demo with joints movements +if Config.MOVE_JOINTS + + % Postural task gains + KP = 10*diag(ones(1,ROBOT_DOF)); + KD = 2*sqrt(KP)/10; + + Ref.Amplitude = 5*ones(1,ROBOT_DOF); + Ref.Frequency = 0.25*ones(1,ROBOT_DOF); +else + % Postural task gains + KP = 0*diag(ones(1,ROBOT_DOF)); + KD = 0*2*sqrt(KP); + + Ref.Amplitude = 0*ones(1,ROBOT_DOF); + Ref.Frequency = 0*ones(1,ROBOT_DOF); +end + +if size(KP,1) ~= ROBOT_DOF + + error('Dimension of KP different from ROBOT_DOF') +end \ No newline at end of file diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGenova02/configJointsControl.m b/controllers/fixed-base-joints-control/app/robots/iCubGenova02/configJointsControl.m new file mode 100644 index 0000000..0a160cd --- /dev/null +++ b/controllers/fixed-base-joints-control/app/robots/iCubGenova02/configJointsControl.m @@ -0,0 +1,13 @@ +% CONFIGJOINTSCONTROL configures all the options associated to the +% joints controller. +% + +%% --- Initialization --- + +% Default behaviour: gravity compensation. If Config.MOVE_JOINTS = true, +% the robot will also move all actuated joints following a sine trajectory +Config.MOVE_JOINTS = false; + +% Max unsigned difference between two consecutive (measured) joint positions, +% i.e. delta_jointPos = abs(jointPos(k) - jointPos(k-1)) [rad] +Sat.maxJointsPositionDelta = 15*pi/180; \ No newline at end of file diff --git a/torque-controllers/impedance-control/app/robots/iCubGenova02/configRobot.m b/controllers/fixed-base-joints-control/app/robots/iCubGenova02/configRobot.m similarity index 65% rename from torque-controllers/impedance-control/app/robots/iCubGenova02/configRobot.m rename to controllers/fixed-base-joints-control/app/robots/iCubGenova02/configRobot.m index 3a7e991..d556b89 100644 --- a/torque-controllers/impedance-control/app/robots/iCubGenova02/configRobot.m +++ b/controllers/fixed-base-joints-control/app/robots/iCubGenova02/configRobot.m @@ -8,15 +8,11 @@ Config.ON_GAZEBO = false; ROBOT_DOF = 23; -% 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] - % Robot configuration for WBToolbox -WBTConfigRobot = WBToolbox.Configuration; -WBTConfigRobot.RobotName = 'icub'; -WBTConfigRobot.UrdfFile = 'model.urdf'; -WBTConfigRobot.LocalName = 'WBT'; +WBTConfigRobot = WBToolbox.Configuration; +WBTConfigRobot.RobotName = 'icub'; +WBTConfigRobot.UrdfFile = 'model.urdf'; +WBTConfigRobot.LocalName = 'WBT'; % Controlboards and joints list. Each joint is associated to the corresponding controlboard WBTConfigRobot.ControlBoardsNames = {'torso','left_arm','right_arm','left_leg','right_leg'}; @@ -35,28 +31,4 @@ WBTConfigRobot.ControlledJoints = [WBTConfigRobot.ControlledJoints, ... ControlBoards.(WBTConfigRobot.ControlBoardsNames{n})]; Config.numOfJointsForEachControlboard = [Config.numOfJointsForEachControlboard; length(ControlBoards.(WBTConfigRobot.ControlBoardsNames{n}))]; -end - -% References for the demo movements -if MOVING - - % Impedance gains - Kp = 20*diag(ones(1,ROBOT_DOF)); - Kd = 2*sqrt(Kp)*0; - - AMPLS = 7.5*ones(1,ROBOT_DOF); - FREQS = 0.5*ones(1,ROBOT_DOF); - -else - - % Impedance gains - Kp = 0*diag(ones(1,ROBOT_DOF)); - Kd = 0*diag(ones(1,ROBOT_DOF)); - - AMPLS = 0*ones(1,ROBOT_DOF); - FREQS = 0*ones(1,ROBOT_DOF); -end - -if size(Kp,1) ~= ROBOT_DOF - error('Dimension of Kp different from ROBOT_DOF') end \ No newline at end of file diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGenova02/gainsAndReferences.m b/controllers/fixed-base-joints-control/app/robots/iCubGenova02/gainsAndReferences.m new file mode 100644 index 0000000..cfa76a6 --- /dev/null +++ b/controllers/fixed-base-joints-control/app/robots/iCubGenova02/gainsAndReferences.m @@ -0,0 +1,28 @@ +% GAINSANDREFERENCES initializes the controller parameters: gains, +% regularization parameters and references. +% + +%% --- Initialization --- + +% References for the demo with joints movements +if Config.MOVE_JOINTS + + % Postural task gains + KP = 20*diag(ones(1,ROBOT_DOF)); + KD = 2*sqrt(KP)*0; + + Ref.Amplitude = 7.5*ones(1,ROBOT_DOF); + Ref.Frequency = 0.5*ones(1,ROBOT_DOF); +else + % Postural task gains + KP = 0*diag(ones(1,ROBOT_DOF)); + KD = 0*2*sqrt(KP); + + Ref.Amplitude = 0*ones(1,ROBOT_DOF); + Ref.Frequency = 0*ones(1,ROBOT_DOF); +end + +if size(KP,1) ~= ROBOT_DOF + + error('Dimension of KP different from ROBOT_DOF') +end \ No newline at end of file diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGenova04/configJointsControl.m b/controllers/fixed-base-joints-control/app/robots/iCubGenova04/configJointsControl.m new file mode 100644 index 0000000..0a160cd --- /dev/null +++ b/controllers/fixed-base-joints-control/app/robots/iCubGenova04/configJointsControl.m @@ -0,0 +1,13 @@ +% CONFIGJOINTSCONTROL configures all the options associated to the +% joints controller. +% + +%% --- Initialization --- + +% Default behaviour: gravity compensation. If Config.MOVE_JOINTS = true, +% the robot will also move all actuated joints following a sine trajectory +Config.MOVE_JOINTS = false; + +% Max unsigned difference between two consecutive (measured) joint positions, +% i.e. delta_jointPos = abs(jointPos(k) - jointPos(k-1)) [rad] +Sat.maxJointsPositionDelta = 15*pi/180; \ No newline at end of file diff --git a/torque-controllers/impedance-control/app/robots/iCubGenova04/configRobot.m b/controllers/fixed-base-joints-control/app/robots/iCubGenova04/configRobot.m similarity index 65% rename from torque-controllers/impedance-control/app/robots/iCubGenova04/configRobot.m rename to controllers/fixed-base-joints-control/app/robots/iCubGenova04/configRobot.m index 3a7e991..d556b89 100644 --- a/torque-controllers/impedance-control/app/robots/iCubGenova04/configRobot.m +++ b/controllers/fixed-base-joints-control/app/robots/iCubGenova04/configRobot.m @@ -8,15 +8,11 @@ Config.ON_GAZEBO = false; ROBOT_DOF = 23; -% 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] - % Robot configuration for WBToolbox -WBTConfigRobot = WBToolbox.Configuration; -WBTConfigRobot.RobotName = 'icub'; -WBTConfigRobot.UrdfFile = 'model.urdf'; -WBTConfigRobot.LocalName = 'WBT'; +WBTConfigRobot = WBToolbox.Configuration; +WBTConfigRobot.RobotName = 'icub'; +WBTConfigRobot.UrdfFile = 'model.urdf'; +WBTConfigRobot.LocalName = 'WBT'; % Controlboards and joints list. Each joint is associated to the corresponding controlboard WBTConfigRobot.ControlBoardsNames = {'torso','left_arm','right_arm','left_leg','right_leg'}; @@ -35,28 +31,4 @@ WBTConfigRobot.ControlledJoints = [WBTConfigRobot.ControlledJoints, ... ControlBoards.(WBTConfigRobot.ControlBoardsNames{n})]; Config.numOfJointsForEachControlboard = [Config.numOfJointsForEachControlboard; length(ControlBoards.(WBTConfigRobot.ControlBoardsNames{n}))]; -end - -% References for the demo movements -if MOVING - - % Impedance gains - Kp = 20*diag(ones(1,ROBOT_DOF)); - Kd = 2*sqrt(Kp)*0; - - AMPLS = 7.5*ones(1,ROBOT_DOF); - FREQS = 0.5*ones(1,ROBOT_DOF); - -else - - % Impedance gains - Kp = 0*diag(ones(1,ROBOT_DOF)); - Kd = 0*diag(ones(1,ROBOT_DOF)); - - AMPLS = 0*ones(1,ROBOT_DOF); - FREQS = 0*ones(1,ROBOT_DOF); -end - -if size(Kp,1) ~= ROBOT_DOF - error('Dimension of Kp different from ROBOT_DOF') end \ No newline at end of file diff --git a/controllers/fixed-base-joints-control/app/robots/iCubGenova04/gainsAndReferences.m b/controllers/fixed-base-joints-control/app/robots/iCubGenova04/gainsAndReferences.m new file mode 100644 index 0000000..cfa76a6 --- /dev/null +++ b/controllers/fixed-base-joints-control/app/robots/iCubGenova04/gainsAndReferences.m @@ -0,0 +1,28 @@ +% GAINSANDREFERENCES initializes the controller parameters: gains, +% regularization parameters and references. +% + +%% --- Initialization --- + +% References for the demo with joints movements +if Config.MOVE_JOINTS + + % Postural task gains + KP = 20*diag(ones(1,ROBOT_DOF)); + KD = 2*sqrt(KP)*0; + + Ref.Amplitude = 7.5*ones(1,ROBOT_DOF); + Ref.Frequency = 0.5*ones(1,ROBOT_DOF); +else + % Postural task gains + KP = 0*diag(ones(1,ROBOT_DOF)); + KD = 0*2*sqrt(KP); + + Ref.Amplitude = 0*ones(1,ROBOT_DOF); + Ref.Frequency = 0*ones(1,ROBOT_DOF); +end + +if size(KP,1) ~= ROBOT_DOF + + error('Dimension of KP different from ROBOT_DOF') +end \ No newline at end of file diff --git a/controllers/fixed-base-joints-control/app/robots/icubGazeboSim/configJointsControl.m b/controllers/fixed-base-joints-control/app/robots/icubGazeboSim/configJointsControl.m new file mode 100644 index 0000000..5e47a0f --- /dev/null +++ b/controllers/fixed-base-joints-control/app/robots/icubGazeboSim/configJointsControl.m @@ -0,0 +1,13 @@ +% CONFIGJOINTSCONTROL configures all the options associated to the +% joints controller. +% + +%% --- Initialization --- + +% Default behaviour: gravity compensation. If Config.MOVE_JOINTS = true, +% the robot will also move all actuated joints following a sine trajectory +Config.MOVE_JOINTS = true; + +% Max unsigned difference between two consecutive (measured) joint positions, +% i.e. delta_jointPos = abs(jointPos(k) - jointPos(k-1)) [rad] +Sat.maxJointsPositionDelta = 15*pi/180; \ No newline at end of file diff --git a/controllers/fixed-base-joints-control/app/robots/icubGazeboSim/configRobot.m b/controllers/fixed-base-joints-control/app/robots/icubGazeboSim/configRobot.m new file mode 100644 index 0000000..1b578d9 --- /dev/null +++ b/controllers/fixed-base-joints-control/app/robots/icubGazeboSim/configRobot.m @@ -0,0 +1,34 @@ +% CONFIGROBOT initializes parameters specific of a particular robot +% (e.g., icuGazeboSim) +% + +%% --- Initialization --- + +% Gains and parameters for impedance controller +Config.ON_GAZEBO = true; +ROBOT_DOF = 23; + +% 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 \ No newline at end of file diff --git a/controllers/fixed-base-joints-control/app/robots/icubGazeboSim/gainsAndReferences.m b/controllers/fixed-base-joints-control/app/robots/icubGazeboSim/gainsAndReferences.m new file mode 100644 index 0000000..74fe404 --- /dev/null +++ b/controllers/fixed-base-joints-control/app/robots/icubGazeboSim/gainsAndReferences.m @@ -0,0 +1,28 @@ +% GAINSANDREFERENCES initializes the controller parameters: gains, +% regularization parameters and references. +% + +%% --- Initialization --- + +% References for the demo with joints movements +if Config.MOVE_JOINTS + + % Postural task gains + KP = 10*diag(ones(1,ROBOT_DOF)); + KD = 2*sqrt(KP)/10; + + Ref.Amplitude = 5*ones(1,ROBOT_DOF); + Ref.Frequency = 0.25*ones(1,ROBOT_DOF); +else + % Postural task gains + KP = 0*diag(ones(1,ROBOT_DOF)); + KD = 0*2*sqrt(KP); + + Ref.Amplitude = 0*ones(1,ROBOT_DOF); + Ref.Frequency = 0*ones(1,ROBOT_DOF); +end + +if size(KP,1) ~= ROBOT_DOF + + error('Dimension of KP different from ROBOT_DOF') +end \ No newline at end of file diff --git a/torque-controllers/impedance-control/initImpedanceControl.m b/controllers/fixed-base-joints-control/initJointsControl.m similarity index 60% rename from torque-controllers/impedance-control/initImpedanceControl.m rename to controllers/fixed-base-joints-control/initJointsControl.m index cf221ae..2c91e11 100644 --- a/torque-controllers/impedance-control/initImpedanceControl.m +++ b/controllers/fixed-base-joints-control/initJointsControl.m @@ -16,31 +16,30 @@ % */ %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% In the Simulink model, this script is run every time the user presses -% the 'start' button. -clear variables +% 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 -close all %% GENERAL SIMULATION INFO -% If you are simulating the robot with Gazebo, -% remember that you have to launch Gazebo as follow: +% +% If you are simulating the robot with Gazebo, remember that it is required +% to launch Gazebo as follows: % -% gazebo -slibgazebo_yarp_clock.so +% gazebo -slibgazebo_yarp_clock.so % -% and set the environmental variable YARP_ROBOT_NAME in the .bashrc file. +% and properly set the environmental variable YARP_ROBOT_NAME in the .bashrc. -% simulation time -Config.Ts = 0.01; -Config.SIMULATION_TIME = inf; +% Simulation time in seconds. For long simulations, put an high number +% (not inf) for allowing automatic code generation +Config.SIMULATION_TIME = 600000; -% Default behaviour: gravity compensation. If MOVING = true, the robot will also move -% all actuated joints following a sine trajectory -MOVING = false; +% Controller period [s] +Config.tStep = 0.01; % If Config.SAVE_WORKSPACE = True, every time the simulink model is run the % workspace is saved after stopping the simulation -Config.SAVE_WORKSPACE = false; +Config.SAVE_WORKSPACE = false; % if TRUE, the controller will STOP if the joints hit the joints limits % and/or if the (unsigned) difference between two consecutive joints @@ -48,6 +47,12 @@ Config.EMERGENCY_STOP_WITH_JOINTS_LIMITS = false; Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES = true; +% Verify that the integration time has been respected during the simulation +Config.CHECK_INTEGRATION_TIME = true; +Config.PLOT_INTEGRATION_TIME = false; + %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % CONFIGURATIONS COMPLETED: loading gains and parameters for the specific robot -run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configRobot.m')); \ No newline at end of file +run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configRobot.m')); +run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configJointsControl.m')); +run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/gainsAndReferences.m')); \ No newline at end of file diff --git a/controllers/fixed-base-joints-control/jointsControl.mdl b/controllers/fixed-base-joints-control/jointsControl.mdl new file mode 100644 index 0000000..b4fecbe --- /dev/null +++ b/controllers/fixed-base-joints-control/jointsControl.mdl @@ -0,0 +1,5555 @@ +Model { + Name "jointsControl" + Version 9.0 + SavedCharacterEncoding "UTF-8" + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.358" + NumModelReferences 0 + NumTestPointedSignals 0 + NumProvidedFunctions 0 + NumRequiredFunctions 0 + NumResetEvents 0 + HasInitializeEvent 0 + HasTerminateEvent 0 + IsExportFunctionModel 0 + NumParameterArguments 0 + NumExternalFileReferences 12 + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Configuration" + Path "jointsControl/Configuration" + SID "432" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "jointsControl/GetMeasurement" + SID "431" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "jointsControl/GetMeasurement1" + SID "434" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "jointsControl/GetMeasurement2" + SID "433" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/GetGravityForces" + Path "jointsControl/PD + GRAVITY CONTROL/Gravity Compensation/GetGravityForces" + SID "430" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/MassMatrix" + Path "jointsControl/PD + GRAVITY CONTROL/PD Feedback Controller/Reference Generator/Get Ms*jointAcc_des/M" + "assMatrix1" + SID "391" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "jointsControl/PD + GRAVITY CONTROL/PD Feedback Controller/Reference Generator/holder" + SID "398" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Actuators/SetReferences" + Path "jointsControl/SetReferences" + SID "137" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetLimits" + Path "jointsControl/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/GetLimits" + SID "478" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Simulator Synchronizer" + Path "jointsControl/synchronizer/GAZEBO_SYNCHRONIZER/Simulator Synchronizer" + SID "304" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Real Time Synchronizer" + Path "jointsControl/synchronizer/REAL_TIME_SYNC/Real Time Synchronizer" + SID "309" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Clock" + Path "jointsControl/synchronizer/Yarp Clock" + SID "648" + 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" + PostLoadFcn "%% Try to open the static GUI\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)));\ninitJointsControl;\n\n%% Try to edit the GU" + "I (the user may have already closed it)\ntry\n\n %% Update Start Button\n set(sl_synch_handles.startButton,'Ba" + "ckgroundcolor',[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" + 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" + 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 [9] + } + Object { + $PropName "ExplorerBarInfo" + $ObjectID 4 + $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.9274725274725273] + Offset [171.435366536094, 145.97789591494126] + } + Object { + $PropName "DockComponentsInfo" + $ObjectID 6 + $ClassName "Simulink.DockComponentInfo" + Type "GLUE2:PropertyInspector" + ID "Property Inspector" + Visible [0] + CreateCallback "" + UserData "" + Floating [0] + DockPosition "Right" + Width [640] + Height [480] + } + WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + "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" + } + } + HideAutomaticNames on + Created "Wed Nov 19 19:38:03 2014" + Creator "daniele" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "gnava" + ModifiedDateFormat "%" + LastModifiedDate "Fri Mar 08 16:25:37 2019" + RTWModifiedTimeStamp 473963137 + ModelVersionFormat "1.%" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "disabled" + WideLines off + ShowLineDimensions off + 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 off + FunctionConnectors off + BrowserLookUnderMasks off + SimulationMode "normal" + VisualizeLoggedSignalsWhenLoggingToFile off + 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 7 + $ClassName "Simulink.SimulationData.ModelLoggingInfo" + model_ "jointsControl" + overrideMode_ [0.0] + Array { + Type "Cell" + Dimension 1 + Cell "jointsControl" + 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 8 + Version "1.17.1" + DisabledProps [] + Description "" + Array { + Type "Handle" + Dimension 9 + Simulink.SolverCC { + $ObjectID 9 + Version "1.17.1" + DisabledProps [] + Description "" + 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 10 + Version "1.17.1" + DisabledProps [] + Description "" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + 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 11 + Version "1.17.1" + Array { + Type "Cell" + Dimension 8 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + Cell "UseSpecifiedMinMax" + PropName "DisabledProps" + } + Description "" + 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 128 + 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 12 + Version "1.17.1" + Array { + Type "Cell" + Dimension 1 + Cell "UseOnlyExistingSharedCode" + PropName "DisabledProps" + } + Description "" + 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 "Simplified" + MergeDetectMultiDrivingBlocksExec "error" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + MultiTaskDSMMsg "warning" + MultiTaskCondExecSysMsg "none" + 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 "error" + 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 "none" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + ChecksumConsistencyForSSReuse "none" + InitInArrayFormatMsg "warning" + StrictBusMsg "ErrorOnBusTreatedAsVector" + 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 13 + Version "1.17.1" + DisabledProps [] + Description "" + 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 14 + Version "1.17.1" + DisabledProps [] + Description "" + 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 15 + Version "1.17.1" + DisabledProps [] + Description "" + SimCustomSourceCode "" + SimCustomHeaderCode "" + SimCustomInitializer "" + SimCustomTerminator "" + SimReservedNameArray [] + SimUserSources "" + SimUserIncludeDirs "" + SimUserLibraries "" + SimUserDefines "" + SimCustomCompilerFlags "" + SimCustomLinkerFlags "" + SFSimEcho on + SimCtrlC on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + SimGenImportedTypeDefs off + ModelFunctionsGlobalVisibility "on" + CompileTimeRecursionLimit 50 + EnableRuntimeRecursion on + MATLABDynamicMemAlloc on + MATLABDynamicMemAllocThreshold 65536 + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 16 + 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" + } + SystemTargetFile "grt.tlc" + HardwareBoard "None" + TLCOptions "" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + PackageGeneratedCodeAndArtifacts off + PackageName "" + TemplateMakefile "grt_default_tmf" + PostCodeGenCommand "" + Description "" + GenerateReport off + SaveLog 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 17 + 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 "CustomSymbolStrUtil" + Cell "ReqsInCode" + Cell "CustomSymbolStrModelFcn" + Cell "CustomUserTokenString" + Cell "CustomSymbolStrEmxType" + Cell "CustomSymbolStrEmxFcn" + Cell "BlockCommentType" + PropName "DisabledProps" + } + Description "" + 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 18 + Version "1.17.1" + Array { + Type "Cell" + Dimension 16 + Cell "GeneratePreprocessorConditionals" + Cell "IncludeMdlTerminateFcn" + Cell "SupportNonInlinedSFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "GenerateAllocFcn" + Cell "PurelyIntegerCode" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "ExistingSharedCode" + Cell "RemoveDisableFunc" + Cell "RemoveResetFunc" + PropName "DisabledProps" + } + Description "" + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + GenFloatMathFcnCalls "NOT IN USE" + TargetLangStandard "C89/C90 (ANSI)" + 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 on + MultiInstanceERTCode off + CodeInterfacePackaging "Nonreusable function" + SupportNonFinite on + 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 19 + Version "1.17.1" + DisabledProps [] + Description "Simulink Coverage Configuration Component" + 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" + ExtraOptions "" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 396, 138, 1456, 890 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 8 + } + Object { + $PropName "DataTransfer" + $ObjectID 20 + $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 { + 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 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 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 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 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 off + } + 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 S-Function + FunctionName "system" + SFunctionModules "''" + PortCounts "[]" + } + Block { + BlockType Scope + DefaultConfigurationName "Simulink.scopes.TimeScopeBlockCfg" + } + Block { + BlockType Selector + NumberOfDimensions "1" + IndexMode "One-based" + InputPortWidth "-1" + SampleTime "-1" + } + Block { + BlockType Sin + SineType "Time based" + TimeSource "Use simulation time" + Amplitude "1" + Bias "0" + Frequency "1" + Phase "0" + Samples "10" + Offset "0" + SampleTime "-1" + VectorParams1D on + } + 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 + IsObserver 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 Terminator + } + Block { + BlockType ToWorkspace + VariableName "simulink_output" + MaxDataPoints "1000" + Decimation "1" + SaveFormat "Array" + Save2DSignal "Inherit from input (this choice will be removed - see release notes)" + FixptAsFi off + NumInputs "1" + SampleTime "0" + } + } + System { + Name "jointsControl" + 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 "393" + ReportName "simulink-default.rpt" + SIDHighWatermark "648" + Block { + BlockType Reference + Name "Configuration" + SID "432" + Ports [] + Position [175, 160, 245, 185] + ZOrder 635 + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Configuration" + SourceType "" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + ConfigSource "Workspace" + ConfigObject "'WBTConfigRobot'" + RobotName "'icubSim'" + UrdfFile "'model.urdf'" + ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" + ControlBoardsNames "{'left_arm','right_arm','torso'}" + LocalName "'WBT'" + GravityVector "[0,0,-9.81]" + } + Block { + BlockType SubSystem + Name "Dump and visualize" + SID "504" + Ports [] + Position [560, 301, 630, 324] + ZOrder 963 + ForegroundColor "blue" + BackgroundColor "[0.333333, 1.000000, 1.000000]" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 21 + $ClassName "Simulink.Mask" + Display "disp('VISUALIZER')" + } + System { + Name "Dump and visualize" + 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 "670" + Block { + BlockType From + Name "From" + SID "615" + Position [1825, 597, 1885, 613] + ZOrder 966 + GotoTag "jointPos" + TagVisibility "global" + } + Block { + BlockType From + Name "From1" + SID "617" + Position [1820, 722, 1890, 738] + ZOrder 968 + GotoTag "jointTorques" + TagVisibility "global" + } + Block { + BlockType From + Name "From2" + SID "620" + Position [1810, 672, 1905, 688] + ZOrder 969 + GotoTag "jointTorques_des" + TagVisibility "global" + } + Block { + BlockType From + Name "From3" + SID "623" + Position [1815, 697, 1900, 713] + ZOrder 972 + GotoTag "gravityTorques" + TagVisibility "global" + } + Block { + BlockType From + Name "From4" + SID "628" + Position [1820, 797, 1895, 813] + ZOrder 979 + GotoTag "jointVel_des" + TagVisibility "global" + } + Block { + BlockType From + Name "From5" + SID "629" + Position [1820, 822, 1895, 838] + ZOrder 980 + GotoTag "jointAcc_des" + TagVisibility "global" + } + Block { + BlockType From + Name "From6" + SID "630" + Position [1820, 772, 1895, 788] + ZOrder 981 + GotoTag "jointPos_des" + TagVisibility "global" + } + Block { + BlockType From + Name "From7" + SID "646" + Position [1820, 622, 1895, 638] + ZOrder 986 + GotoTag "jointPos_des" + TagVisibility "global" + } + Block { + BlockType SubSystem + Name "Joint Positions" + SID "443" + Ports [2] + Position [1960, 591, 2080, 644] + ZOrder 955 + RequestExecContextInheritance off + System { + Name "Joint Positions" + 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 "433" + Block { + BlockType Inport + Name "jointPos" + SID "496" + Position [130, 162, 160, 178] + ZOrder 668 + IconDisplay "Port number" + Port { + PortNumber 1 + Name "jointPos Measured" + } + } + Block { + BlockType Inport + Name "jointPos_des" + SID "444" + Position [130, 447, 160, 463] + ZOrder 629 + Port "2" + IconDisplay "Port number" + Port { + PortNumber 1 + Name "jointPos Desired" + } + } + Block { + BlockType Demux + Name "Demux1" + SID "467" + Ports [1, 5] + Position [550, 121, 555, 219] + ZOrder 665 + 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 "468" + Ports [1, 5] + Position [550, 256, 555, 354] + ZOrder 666 + 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 "469" + Ports [1, 5] + Position [550, 406, 555, 504] + ZOrder 667 + 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 Gain + Name "Gain" + SID "456" + Position [380, 155, 425, 185] + ZOrder 662 + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain1" + SID "457" + Position [380, 290, 425, 320] + ZOrder 663 + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain2" + SID "458" + Position [380, 440, 425, 470] + ZOrder 664 + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum2" + SID "451" + Ports [2, 1] + Position [245, 295, 265, 315] + ZOrder 147 + ShowName off + IconShape "round" + Inputs "+|-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "jointPos Error" + } + } + Block { + BlockType Scope + Name "jointPos Des" + SID "453" + Ports [5] + Position [655, 400, 760, 510] + ZOrder 655 + 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','-3.89489','MaxYLimReal','10.78123','YLabelReal','','MinYLimMag','0.00000','M" + "axYLimMag','10.78123','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0" + "],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;" + "0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608" + " 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6509803" + "92156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0" + ".5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" + "ble','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" + "',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marke" + "r','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'" + "Placement',1),struct('MinYLimReal','-55.86221','MaxYLimReal','56.64498','YLabelReal','','MinYLimMag','0','MaxYL" + "imMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTic" + "kColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980" + "39215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098" + "039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]," + "'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker" + "','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" + ",struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0]," + "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWid" + "th',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none'," + "'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" + ",2),struct('MinYLimReal','-37.08127','MaxYLimReal','60.75093','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.26658','MaxYLimReal','16.4551','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',4),struct('MinYLimRe" + "al','-14.29126','MaxYLimReal','25.50069','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility'," + "'on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68" + "6274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 " + "0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2" + "74509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineP" + "ropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct" + "('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" + "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5" + ",'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" + "e','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedC" + "hannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults'," + "struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745098039" + "22],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784313" + "72549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294" + "117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Col" + "or',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle'," + "'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 " + "1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'," + "0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayL" + "ayoutDimensions',[5 1],'DisplayContentCache',{struct('Title','%','LinePropertiesCache',{{struct('C" + "olor',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle" + "','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'M" + "arker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," + "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " + "0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagP" + "hase',false),struct('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',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" + "ible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','" + "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Mark" + "er','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "')}},'UserDefinedChannelNames',{{}},'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','-','LineW" + "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none" + "','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct" + "('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{" + "}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineSt" + "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5" + ",'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" + "e','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," + "[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-'," + "'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])" + NumInputPorts "5" + Floating off + } + Block { + BlockType Scope + Name "jointPos Error" + SID "454" + Ports [5] + Position [655, 250, 760, 360] + ZOrder 653 + 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','-2.22742','MaxYLimReal','1.53152','YLabelReal','','MinYLimMag','0.00000','MaxY" + "LimMag','2.22742','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'A" + "xesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07" + "45098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0" + "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215" + "6863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'" + "Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'" + ",'on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1" + " 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','L" + "ineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','" + "none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Plac" + "ement',1),struct('MinYLimReal','-12.46592','MaxYLimReal','9.87112','YLabelReal','','MinYLimMag','0','MaxYLimMag" + "','10','LegendVisibility','on','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',2),s" + "truct('MinYLimReal','-14.59955','MaxYLimReal','8.75497','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Leg" + "endVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" + "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352" + "9411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176" + "47058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" + "e','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," + "[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker'" + ",'none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}" + "},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinY" + "LimReal','-31.03289','MaxYLimReal','11.29037','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibil" + "ity','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922" + " 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706" + " 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882352" + "9 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','" + "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" + "truct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'L" + "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth" + "',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDef" + "inedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','" + "-21.78876','MaxYLimReal','18.91663','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','%','LineProper" + "tiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" + "or',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle'," + "'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChanne" + "lNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struc" + "t('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.058823529411764" + "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[" + "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','" + "LineWidth',0.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'),s" + "truct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'L" + "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'Li" + "neNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayout" + "Dimensions',[5 1],'DisplayContentCache',{struct('Title','%','LinePropertiesCache',{{struct('Color'" + ",[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-'" + ",'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker" + "','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" + ",struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1]," + "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase'" + ",false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWid" + "th',0.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],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'" + "Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'" + ",'on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0" + " 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','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',{{}},'PlotMagPhase',false),struct('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',{{}},'P" + "lotMagPhase',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,'Mar" + "ker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 " + "0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','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])" + NumInputPorts "5" + Floating off + } + Block { + BlockType Scope + Name "jointPos Meas" + SID "455" + Ports [5] + Position [655, 115, 760, 225] + ZOrder 651 + 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','-3.76082','MaxYLimReal','10.86268','YLabelReal','','MinYLimMag','0.00000','MaxYLimMa" + "g','10.86268','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesT" + "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509" + "8039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450" + "98039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863" + "],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Mark" + "er','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0" + "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineW" + "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none" + "','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" + "t',1),struct('MinYLimReal','-55.80993','MaxYLimReal','56.64084','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',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),stru" + "ct('MinYLimReal','-37.08714','MaxYLimReal','60.76943','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','-23.09357','MaxYLimReal','16.52336','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','-1" + "4.03854','MaxYLimReal','24.89524','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','X" + "Grid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509" + "803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.41176" + "4705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450980" + "3921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperti" + "esCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" + "',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marke" + "r','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" + "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelN" + "ames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct(" + "'YLabelReal','','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',0,'Line" + "Names',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDi" + "mensions',[5 1],'DisplayContentCache',{struct('Title','%','LinePropertiesCache',{{struct('Color',[" + "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','" + "LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker'," + "'none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" + "truct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'L" + "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',f" + "alse),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','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',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" + "on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" + " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','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',{{}},'PlotMagPhase',false),struct('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',{{}},'Plo" + "tMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marke" + "r','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" + "),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" + ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'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])" + NumInputPorts "5" + Floating off + } + Line { + Name "jointPos Measured" + ZOrder 1 + SrcBlock "jointPos" + SrcPort 1 + Points [90, 0] + Branch { + ZOrder 2 + DstBlock "Sum2" + DstPort 1 + } + Branch { + ZOrder 3 + Labels [1, 1] + DstBlock "Gain" + DstPort 1 + } + } + Line { + Name "torso" + ZOrder 4 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 1 + DstBlock "jointPos Meas" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 5 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 2 + DstBlock "jointPos Meas" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 6 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 4 + DstBlock "jointPos Meas" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 7 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 3 + DstBlock "jointPos Meas" + DstPort 3 + } + Line { + Name "right_leg" + ZOrder 8 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 5 + DstBlock "jointPos Meas" + DstPort 5 + } + Line { + Name "torso" + ZOrder 9 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 1 + DstBlock "jointPos Error" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 10 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 2 + DstBlock "jointPos Error" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 11 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 4 + DstBlock "jointPos Error" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 12 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 3 + DstBlock "jointPos Error" + DstPort 3 + } + Line { + Name "right_leg" + ZOrder 13 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 5 + DstBlock "jointPos Error" + DstPort 5 + } + Line { + Name "jointPos Error" + ZOrder 14 + Labels [1, 1] + SrcBlock "Sum2" + SrcPort 1 + DstBlock "Gain1" + DstPort 1 + } + Line { + Name "torso" + ZOrder 15 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 1 + DstBlock "jointPos Des" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 16 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 2 + DstBlock "jointPos Des" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 17 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 4 + DstBlock "jointPos Des" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 18 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 3 + DstBlock "jointPos Des" + DstPort 3 + } + Line { + Name "jointPos Desired" + ZOrder 19 + SrcBlock "jointPos_des" + SrcPort 1 + Points [90, 0] + Branch { + ZOrder 20 + Labels [1, 1] + DstBlock "Gain2" + DstPort 1 + } + Branch { + ZOrder 21 + DstBlock "Sum2" + DstPort 2 + } + } + Line { + Name "right_leg" + ZOrder 22 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 5 + DstBlock "jointPos Des" + DstPort 5 + } + Line { + ZOrder 23 + SrcBlock "Gain" + SrcPort 1 + DstBlock "Demux1" + DstPort 1 + } + Line { + ZOrder 24 + SrcBlock "Gain1" + SrcPort 1 + DstBlock "Demux2" + DstPort 1 + } + Line { + ZOrder 25 + SrcBlock "Gain2" + SrcPort 1 + DstBlock "Demux3" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Joint References" + SID "633" + Ports [3] + Position [1965, 769, 2070, 841] + ZOrder 985 + RequestExecContextInheritance off + System { + Name "Joint 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 "1000" + Block { + BlockType Inport + Name "jointPos_des" + SID "634" + Position [-100, 33, -70, 47] + ZOrder 957 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointVel_des" + SID "635" + Position [-100, 63, -70, 77] + ZOrder 958 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointAcc_des" + SID "636" + Position [-100, 93, -70, 107] + ZOrder 959 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Scope + Name "Visualize References" + SID "399" + Ports [3] + Position [10, 22, 105, 118] + ZOrder 956 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(ext" + "mgr.Configuration('Core','General UI',true),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration(" + "'Sources','WiredSimulink',true,'DataLoggingVariableName','jointReferences_DATA','DataLoggingSaveFormat','Struct" + "ureWithTime','DataLoggingDecimation','1','DataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuratio" + "n('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-15.42126','MaxYLimReal','15.42126'," + "'YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','15.42126','LegendVisibility','On','XGrid',true,'YGrid',tru" + "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745098039" + "22],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784313" + "72549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294" + "117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Vis" + "ible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct(" + "'Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),str" + "uct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on')" + ",struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','" + "on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visibl" + "e','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Vi" + "sible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct" + "('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),st" + "ruct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'" + "),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible'," + "'on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visib" + "le','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('V" + "isible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struc" + "t('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),struct('Visible','on'),s" + "truct('Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Plac" + "ement',1)},'DisplayPropertyDefaults',struct('MinYLimReal','-15.42126','MaxYLimReal','15.42126','YLabelReal',''," + "'MinYLimMag','0.00000','MaxYLimMag','15.42126','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase'" + ",false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder'" + ",[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" + "06 1990])" + NumInputPorts "3" + Floating off + } + Line { + ZOrder 41 + SrcBlock "jointAcc_des" + SrcPort 1 + DstBlock "Visualize References" + DstPort 3 + } + Line { + ZOrder 40 + SrcBlock "jointVel_des" + SrcPort 1 + DstBlock "Visualize References" + DstPort 2 + } + Line { + ZOrder 39 + SrcBlock "jointPos_des" + SrcPort 1 + DstBlock "Visualize References" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Joint Torques" + SID "315" + Ports [3] + Position [1965, 670, 2075, 740] + ZOrder 954 + RequestExecContextInheritance off + System { + Name "Joint Torques" + 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 "jointTorques_des" + SID "318" + Position [130, 447, 160, 463] + ZOrder 629 + IconDisplay "Port number" + Port { + PortNumber 1 + Name "torqueDesired" + } + } + Block { + BlockType Inport + Name "gravity torques" + SID "319" + Position [130, 598, 160, 612] + ZOrder 630 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointTorques" + SID "495" + Position [130, 162, 160, 178] + ZOrder 666 + Port "3" + IconDisplay "Port number" + Port { + PortNumber 1 + Name "torqueMeasured" + } + } + Block { + BlockType Demux + Name "Demux1" + SID "463" + Ports [1, 5] + Position [550, 121, 555, 219] + ZOrder 662 + 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 "464" + Ports [1, 5] + Position [550, 256, 555, 354] + ZOrder 663 + 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 "465" + Ports [1, 5] + Position [550, 406, 555, 504] + ZOrder 664 + 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 "Demux4" + SID "466" + Ports [1, 5] + Position [550, 556, 555, 654] + ZOrder 665 + 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 Sum + Name "Sum2" + SID "49" + Ports [2, 1] + Position [245, 295, 265, 315] + ZOrder 147 + ShowName off + IconShape "round" + Inputs "+|-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "torqueError" + } + } + Block { + BlockType Scope + Name "gravTorques" + SID "419" + Ports [5] + Position [655, 550, 760, 660] + ZOrder 657 + 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','gravTorque_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','Data" + "LoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisp" + "lays',{struct('MinYLimReal','-2.62369','MaxYLimReal','22.48453','YLabelReal','','MinYLimMag','0.00000','MaxYLim" + "Mag','22.48453','LegendVisibility','On','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',1),struct('MinYLimReal','-1.31451','MaxYLimReal','0.36849','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',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),stru" + "ct('MinYLimReal','-1.35483','MaxYLimReal','0.37657','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',3),struct('MinYLimR" + "eal','-80.67711','MaxYLimReal','54.64406','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',4),struct('MinYLimReal','-80." + "58848','MaxYLimReal','54.86216','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGr" + "id',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" + "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647" + "05882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039" + "21569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperties" + "Cache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.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')}},'UserDefinedChannelNam" + "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('Y" + "LabelReal','','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',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDime" + "nsions',[5 1],'DisplayContentCache',{struct('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',{{}},'PlotMagPhase',fal" + "se),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','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',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1" + "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineW" + "idth',0.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')}},'Use" + "rDefinedChannelNames',{{}},'PlotMagPhase',false),struct('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',{{}},'PlotM" + "agPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker'" + ",'none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')," + "struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'" + "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidt" + "h',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'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])" + NumInputPorts "5" + Floating off + } + Block { + BlockType Scope + Name "tauDes" + SID "417" + Ports [5] + Position [655, 400, 760, 510] + ZOrder 655 + 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','-2.62369','MaxYLimReal','22.48453','YLabelReal','','MinYLimMag','0.00000','MaxYLim" + "Mag','22.48453','LegendVisibility','On','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',1),struct('MinYLimReal','-1.31451','MaxYLimReal','0.36849','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',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),stru" + "ct('MinYLimReal','-1.35483','MaxYLimReal','0.37657','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',3),struct('MinYLimR" + "eal','-80.67711','MaxYLimReal','54.64406','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',4),struct('MinYLimReal','-80." + "58848','MaxYLimReal','54.86216','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGr" + "id',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" + "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647" + "05882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039" + "21569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperties" + "Cache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.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')}},'UserDefinedChannelNam" + "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('Y" + "LabelReal','','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',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDime" + "nsions',[5 1],'DisplayContentCache',{struct('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',{{}},'PlotMagPhase',fal" + "se),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','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',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1" + "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineW" + "idth',0.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')}},'Use" + "rDefinedChannelNames',{{}},'PlotMagPhase',false),struct('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',{{}},'PlotM" + "agPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker'" + ",'none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')," + "struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'" + "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidt" + "h',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'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])" + NumInputPorts "5" + Floating off + } + Block { + BlockType Scope + Name "tauError" + SID "415" + Ports [5] + Position [655, 250, 760, 360] + ZOrder 653 + 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','-2.62369','MaxYLimReal','22.48453','YLabelReal','','MinYLimMag','0.00000','MaxYLimMa" + "g','22.48453','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesT" + "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509" + "8039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450" + "98039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863" + "],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Mark" + "er','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0" + "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineW" + "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none" + "','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" + "t',1),struct('MinYLimReal','-1.31451','MaxYLimReal','0.36849','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',2),struct" + "('MinYLimReal','-1.35483','MaxYLimReal','0.37657','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVis" + "ibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980" + "3922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941176" + "4706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" + "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1" + "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineW" + "idth',0.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')}},'Use" + "rDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimRea" + "l','-80.67711','MaxYLimReal','54.64406','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','" + "on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686" + "274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0" + ".411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27" + "4509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePr" + "opertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" + "'Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5," + "'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" + "','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedCh" + "annelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-80.58" + "848','MaxYLimReal','54.86216','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid" + "',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098039" + "22 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705" + "882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921" + "569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCa" + "che',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1" + " 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','L" + "ineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','" + "none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" + "ruct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames" + "',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLa" + "belReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorO" + "rder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392" + "156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;" + "1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0]" + ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" + "'Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineName" + "s',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimens" + "ions',[5 1],'DisplayContentCache',{struct('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',{{}},'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','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',{{}},'PlotMagPhase',false),struct('Title','%'" + ",'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" + ",struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1]," + "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','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',{{}},'PlotMagPhase',false),struct('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',{{}},'PlotMag" + "Phase',false),struct('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',{{}},'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])" + NumInputPorts "5" + Floating off + } + Block { + BlockType Scope + Name "tauMeas" + SID "413" + Ports [5] + Position [655, 115, 760, 225] + ZOrder 651 + 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','-2.62369','MaxYLimReal','22.48453','YLabelReal','','MinYLimMag','0.00000','MaxYLi" + "mMag','22.48453','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','-1.31451','MaxYLimReal','0.36849','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','-1.35483','MaxYLimReal','0.37657','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legend" + "Visibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450" + "9803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" + "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470" + "58823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," + "'on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " + "1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'" + "UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLim" + "Real','-80.67711','MaxYLimReal','54.64406','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility" + "','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0." + "686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;" + "1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0" + ".274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Lin" + "ePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" + "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'Line" + "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0" + ".5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" + "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" + "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-80" + ".58848','MaxYLimReal','54.86216','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)},'DisplayPropertyDefaults',struct('" + "YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Col" + "orOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0." + "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','-','Lin" + "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','no" + "ne','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" + "ct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'Line" + "Style','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineN" + "ames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDim" + "ensions',[5 1],'DisplayContentCache',{struct('Title','%','LinePropertiesCache',{{struct('Color',[1" + " 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','L" + "ineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','" + "none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" + "ruct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'Li" + "neStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',fa" + "lse),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','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',{{}},'PlotMagPhase',false),struct('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',{{}},'PlotMagPhase',false),struct('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',{{}},'Plot" + "MagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-'" + ",'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker" + "','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" + ",struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]," + "'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])" + NumInputPorts "5" + Floating off + } + Line { + ZOrder 1 + SrcBlock "gravity torques" + SrcPort 1 + DstBlock "Demux4" + DstPort 1 + } + Line { + Name "torqueMeasured" + ZOrder 2 + SrcBlock "jointTorques" + SrcPort 1 + Points [90, 0] + Branch { + ZOrder 3 + Labels [1, 1] + DstBlock "Demux1" + DstPort 1 + } + Branch { + ZOrder 4 + DstBlock "Sum2" + DstPort 1 + } + } + Line { + Name "torso" + ZOrder 5 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 1 + DstBlock "tauMeas" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 6 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 2 + DstBlock "tauMeas" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 7 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 4 + DstBlock "tauMeas" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 8 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 3 + DstBlock "tauMeas" + DstPort 3 + } + Line { + Name "right_leg" + ZOrder 9 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 5 + DstBlock "tauMeas" + DstPort 5 + } + Line { + Name "torso" + ZOrder 10 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 1 + DstBlock "tauError" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 11 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 2 + DstBlock "tauError" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 12 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 4 + DstBlock "tauError" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 13 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 3 + DstBlock "tauError" + DstPort 3 + } + Line { + Name "right_leg" + ZOrder 14 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 5 + DstBlock "tauError" + DstPort 5 + } + Line { + Name "torqueError" + ZOrder 15 + Labels [1, 1] + SrcBlock "Sum2" + SrcPort 1 + DstBlock "Demux2" + DstPort 1 + } + Line { + Name "torso" + ZOrder 16 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 1 + DstBlock "tauDes" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 17 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 2 + DstBlock "tauDes" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 18 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 4 + DstBlock "tauDes" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 19 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 3 + DstBlock "tauDes" + DstPort 3 + } + Line { + Name "torqueDesired" + ZOrder 20 + SrcBlock "jointTorques_des" + SrcPort 1 + Points [90, 0] + Branch { + ZOrder 21 + Labels [1, 1] + DstBlock "Demux3" + DstPort 1 + } + Branch { + ZOrder 22 + DstBlock "Sum2" + DstPort 2 + } + } + Line { + Name "right_leg" + ZOrder 23 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 5 + DstBlock "tauDes" + DstPort 5 + } + Line { + Name "torso" + ZOrder 24 + Labels [-1, 0] + SrcBlock "Demux4" + SrcPort 1 + DstBlock "gravTorques" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 25 + Labels [-1, 0] + SrcBlock "Demux4" + SrcPort 2 + DstBlock "gravTorques" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 26 + Labels [-1, 0] + SrcBlock "Demux4" + SrcPort 4 + DstBlock "gravTorques" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 27 + Labels [-1, 0] + SrcBlock "Demux4" + SrcPort 3 + DstBlock "gravTorques" + DstPort 3 + } + Line { + Name "right_leg" + ZOrder 28 + Labels [-1, 0] + SrcBlock "Demux4" + SrcPort 5 + DstBlock "gravTorques" + DstPort 5 + } + } + } + Line { + ZOrder 32 + SrcBlock "From1" + SrcPort 1 + DstBlock "Joint Torques" + DstPort 3 + } + Line { + ZOrder 34 + SrcBlock "From3" + SrcPort 1 + DstBlock "Joint Torques" + DstPort 2 + } + Line { + ZOrder 35 + SrcBlock "From2" + SrcPort 1 + DstBlock "Joint Torques" + DstPort 1 + } + Line { + ZOrder 30 + SrcBlock "From" + SrcPort 1 + DstBlock "Joint Positions" + DstPort 1 + } + Line { + ZOrder 39 + SrcBlock "From6" + SrcPort 1 + DstBlock "Joint References" + DstPort 1 + } + Line { + ZOrder 40 + SrcBlock "From4" + SrcPort 1 + DstBlock "Joint References" + DstPort 2 + } + Line { + ZOrder 41 + SrcBlock "From5" + SrcPort 1 + DstBlock "Joint References" + DstPort 3 + } + Line { + ZOrder 42 + SrcBlock "From7" + SrcPort 1 + DstBlock "Joint Positions" + DstPort 2 + } + } + } + Block { + BlockType Reference + Name "GetMeasurement" + SID "431" + Ports [0, 1] + Position [175, 280, 245, 300] + ZOrder 634 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetMeasurement" + SourceType "GetMeasurement" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + measuredType "Joints Position" + } + Block { + BlockType Reference + Name "GetMeasurement1" + SID "434" + Ports [0, 1] + Position [175, 335, 245, 355] + ZOrder 660 + BackgroundColor "[0.827500, 0.576500, 0.603900]" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetMeasurement" + SourceType "GetMeasurement" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + measuredType "Joints Torque" + } + Block { + BlockType Reference + Name "GetMeasurement2" + SID "433" + Ports [0, 1] + Position [175, 225, 245, 245] + ZOrder 661 + BackgroundColor "[0.513700, 0.674500, 1.000000]" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetMeasurement" + SourceType "GetMeasurement" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + measuredType "Joints Velocity" + } + Block { + BlockType Goto + Name "Goto" + SID "614" + Position [315, 302, 385, 318] + ZOrder 964 + GotoTag "jointPos" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto1" + SID "616" + Position [315, 337, 385, 353] + ZOrder 966 + GotoTag "jointTorques" + TagVisibility "global" + } + Block { + BlockType SubSystem + Name "PD + GRAVITY CONTROL" + SID "498" + Ports [2, 1] + Position [425, 205, 530, 320] + ZOrder 662 + RequestExecContextInheritance off + System { + Name "PD + GRAVITY CONTROL" + 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 "584" + Block { + BlockType Inport + Name "jointVel" + SID "500" + Position [140, 48, 170, 62] + ZOrder 635 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "501" + Position [140, 158, 170, 172] + ZOrder 636 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Goto + Name "Goto1" + SID "619" + Position [665, 67, 760, 83] + ZOrder 967 + GotoTag "jointTorques_des" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto2" + SID "622" + Position [510, 232, 595, 248] + ZOrder 970 + GotoTag "gravityTorques" + TagVisibility "global" + } + Block { + BlockType SubSystem + Name "Gravity Compensation" + SID "226" + Ports [1, 1] + Position [245, 135, 400, 195] + ZOrder -36 + RequestExecContextInheritance off + Port { + PortNumber 1 + Name "gravityTorques" + } + System { + Name "Gravity Compensation" + 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 "693" + Block { + BlockType Inport + Name "jointPos" + SID "423" + Position [125, 172, 155, 188] + ZOrder 639 + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "Constant4" + SID "425" + Position [125, 122, 155, 138] + ZOrder 637 + ShowName off + Value "eye(4)" + } + Block { + BlockType Reference + Name "GetGravityForces" + SID "430" + Ports [2, 1] + Position [210, 105, 350, 205] + ZOrder 641 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/GetGravityForces" + SourceType "Gravity bias" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + Port { + PortNumber 1 + Name "gravityTorques floating base" + } + } + Block { + BlockType Selector + Name "Selector2" + SID "428" + Ports [1, 1] + Position [500, 136, 540, 174] + ZOrder 634 + ShowName off + InputPortWidth "ROBOT_DOF+6" + IndexOptions "Index vector (dialog)" + Indices "[7:ROBOT_DOF+6]" + OutputSizes "1" + } + Block { + BlockType Outport + Name "gravity torques" + SID "429" + Position [590, 148, 620, 162] + ZOrder 640 + IconDisplay "Port number" + } + Line { + ZOrder 77 + SrcBlock "Selector2" + SrcPort 1 + DstBlock "gravity torques" + DstPort 1 + } + Line { + ZOrder 84 + SrcBlock "Constant4" + SrcPort 1 + DstBlock "GetGravityForces" + DstPort 1 + } + Line { + ZOrder 85 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "GetGravityForces" + DstPort 2 + } + Line { + Name "gravityTorques floating base" + ZOrder 83 + Labels [1, 1] + SrcBlock "GetGravityForces" + SrcPort 1 + DstBlock "Selector2" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "PD Feedback Controller" + SID "310" + Ports [2, 1] + Position [245, 41, 405, 99] + ZOrder -41 + RequestExecContextInheritance off + Port { + PortNumber 1 + Name "PD control" + } + System { + Name "PD Feedback Controller" + 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 "606" + Block { + BlockType Inport + Name "jointVel" + SID "497" + Position [325, 92, 355, 108] + ZOrder 636 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "311" + Position [70, -18, 100, -2] + ZOrder 386 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "PD Controller" + SID "637" + Ports [5, 1] + Position [415, -92, 550, 182] + ZOrder 637 + RequestExecContextInheritance off + System { + Name "PD Controller" + 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 "663" + Block { + BlockType Inport + Name "Ms*jointAcc_des" + SID "641" + Position [375, 49, 405, 61] + ZOrder 640 + IconDisplay "Port number" + Port { + PortNumber 1 + Name "Ms*jointAcc_des" + } + } + Block { + BlockType Inport + Name "jointPos_des" + SID "643" + Position [375, 109, 405, 121] + ZOrder 642 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointVel_des" + SID "642" + Position [375, 184, 405, 196] + ZOrder 641 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointVel" + SID "638" + Position [375, 214, 405, 226] + ZOrder 637 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "639" + Position [375, 139, 405, 151] + ZOrder 638 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Gain + Name "Gain1" + SID "14" + Position [540, 115, 595, 145] + ZOrder 22 + ShowName off + Gain "KP" + Multiplication "Matrix(K*u)" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "Kp*(jointPos_des - jointPos)" + } + } + Block { + BlockType Gain + Name "Gain2" + SID "20" + Position [540, 190, 595, 220] + ZOrder 26 + ShowName off + Gain "KD" + Multiplication "Matrix(K*u)" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "Kd*(jointVel_des - jointVel)" + } + } + Block { + BlockType Sum + Name "Sum1" + SID "19" + Ports [3, 1] + Position [760, 19, 805, 241] + ZOrder 25 + ShowName off + Inputs "+++" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum2" + SID "644" + Ports [2, 1] + Position [465, 100, 485, 160] + ZOrder 644 + ShowName off + Inputs "+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum3" + SID "645" + Ports [2, 1] + Position [465, 175, 485, 235] + ZOrder 645 + ShowName off + Inputs "+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "tau_feedback" + SID "640" + Position [855, 123, 885, 137] + ZOrder 639 + IconDisplay "Port number" + } + Line { + Name "Kd*(jointVel_des - jointVel)" + ZOrder 395 + Labels [-1, 0] + SrcBlock "Gain2" + SrcPort 1 + DstBlock "Sum1" + DstPort 3 + } + Line { + ZOrder 425 + SrcBlock "Sum2" + SrcPort 1 + DstBlock "Gain1" + DstPort 1 + } + Line { + ZOrder 429 + SrcBlock "jointVel" + SrcPort 1 + DstBlock "Sum3" + DstPort 2 + } + Line { + Name "Kp*(jointPos_des - jointPos)" + ZOrder 394 + Labels [-1, 0] + SrcBlock "Gain1" + SrcPort 1 + DstBlock "Sum1" + DstPort 2 + } + Line { + ZOrder 424 + SrcBlock "jointPos_des" + SrcPort 1 + DstBlock "Sum2" + DstPort 1 + } + Line { + ZOrder 427 + SrcBlock "Sum3" + SrcPort 1 + DstBlock "Gain2" + DstPort 1 + } + Line { + Name "Ms*jointAcc_des" + ZOrder 417 + Labels [-1, 0] + SrcBlock "Ms*jointAcc_des" + SrcPort 1 + DstBlock "Sum1" + DstPort 1 + } + Line { + ZOrder 416 + SrcBlock "Sum1" + SrcPort 1 + DstBlock "tau_feedback" + DstPort 1 + } + Line { + ZOrder 426 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "Sum2" + DstPort 2 + } + Line { + ZOrder 428 + SrcBlock "jointVel_des" + SrcPort 1 + DstBlock "Sum3" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Reference Generator" + SID "88" + Ports [1, 3] + Position [180, -93, 295, 73] + ZOrder 638 + HideAutomaticName off + RequestExecContextInheritance off + System { + Name "Reference Generator" + 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 "400" + Block { + BlockType Inport + Name "jointPos" + SID "384" + Position [-300, -552, -270, -538] + ZOrder 666 + IconDisplay "Port number" + } + Block { + BlockType Gain + Name "Gain1" + SID "420" + Position [-125, -675, -70, -645] + ZOrder 989 + ShowName off + Gain "pi/180" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain2" + SID "421" + Position [-125, -605, -75, -575] + ZOrder 990 + ShowName off + Gain "pi/180" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain5" + SID "386" + Position [10, -370, 65, -340] + ZOrder 988 + ShowName off + Gain "pi/180" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType SubSystem + Name "Get Ms*jointAcc_des" + SID "491" + Ports [2, 1] + Position [110, -376, 220, -289] + ZOrder 680 + RequestExecContextInheritance off + System { + Name "Get Ms*jointAcc_des" + 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 "697" + Block { + BlockType Inport + Name "jointAcc_des" + SID "493" + Position [135, 138, 165, 152] + ZOrder 679 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "492" + Position [135, 63, 165, 77] + ZOrder 678 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "Constant1" + SID "385" + Position [135, 20, 165, 50] + ZOrder 674 + NamePlacement "alternate" + Value "eye(4)" + } + Block { + BlockType Reference + Name "MassMatrix1" + SID "391" + Ports [2, 1] + Position [210, 19, 350, 86] + ZOrder 673 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/MassMatrix" + SourceType "MassMatrix" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Product + Name "Product1" + SID "392" + Ports [2, 1] + Position [500, 11, 540, 189] + ZOrder 665 + ShowName off + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + } + Block { + BlockType Selector + Name "Selector" + SID "422" + Ports [1, 1] + Position [385, 46, 415, 64] + ZOrder 677 + NumberOfDimensions "2" + InputPortWidth "3" + IndexOptions "Index vector (dialog),Index vector (dialog)" + Indices "[7:ROBOT_DOF+6],[7:ROBOT_DOF+6]" + OutputSizes "1,1" + } + Block { + BlockType Outport + Name "Ms*jointAcc_des" + SID "494" + Position [590, 93, 620, 107] + ZOrder 680 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "MassMatrix1" + SrcPort 1 + DstBlock "Selector" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "Product1" + SrcPort 1 + DstBlock "Ms*jointAcc_des" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "Selector" + SrcPort 1 + DstBlock "Product1" + DstPort 1 + } + Line { + ZOrder 4 + SrcBlock "Constant1" + SrcPort 1 + DstBlock "MassMatrix1" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "MassMatrix1" + DstPort 2 + } + Line { + ZOrder 6 + SrcBlock "jointAcc_des" + SrcPort 1 + DstBlock "Product1" + DstPort 2 + } + } + } + Block { + BlockType Goto + Name "Goto1" + SID "625" + Position [120, -598, 195, -582] + ZOrder 994 + GotoTag "jointPos_des" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto2" + SID "626" + Position [115, -718, 190, -702] + ZOrder 995 + GotoTag "jointVel_des" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto3" + SID "627" + Position [125, -478, 200, -462] + ZOrder 996 + GotoTag "jointAcc_des" + TagVisibility "global" + } + Block { + BlockType Sum + Name "Sum1" + SID "396" + Ports [2, 1] + Position [25, -555, 45, -535] + ZOrder 670 + ShowName off + IconShape "round" + Inputs "++|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Reference + Name "holder" + SID "398" + Ports [1, 1] + Position [-125, -556, -85, -534] + ZOrder 672 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Sin + Name "jointAcc Ref" + SID "395" + Ports [0, 1] + Position [-130, -483, -85, -457] + ZOrder 987 + Amplitude "-Ref.Amplitude.*(Ref.Frequency.*2.*pi).^2" + Frequency "Ref.Frequency.*2.*pi" + SampleTime "0" + } + Block { + BlockType Sin + Name "jointPos Ref" + SID "393" + Ports [0, 1] + Position [-320, -605, -255, -575] + ZOrder 985 + Amplitude "Ref.Amplitude" + Frequency "Ref.Frequency.*2.*pi" + SampleTime "0" + } + Block { + BlockType Sin + Name "jointVel Ref" + SID "394" + Ports [0, 1] + Position [-320, -675, -250, -645] + ZOrder 986 + Amplitude "Ref.Amplitude.*Ref.Frequency.*2.*pi" + Frequency "Ref.Frequency.*2.*pi" + Phase "pi/2" + SampleTime "0" + } + Block { + BlockType Outport + Name "Ms*jointAcc_des" + SID "400" + Position [280, -337, 310, -323] + ZOrder 663 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "jointPos_des" + SID "402" + Position [145, -552, 175, -538] + ZOrder 657 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "jointVel_des" + SID "489" + Position [140, -667, 170, -653] + ZOrder 992 + Port "3" + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "jointPos" + SrcPort 1 + Points [23, 0] + Branch { + ZOrder 34 + DstBlock "holder" + DstPort 1 + } + Branch { + ZOrder 25 + Points [0, 235] + DstBlock "Get Ms*jointAcc_des" + DstPort 2 + } + } + Line { + ZOrder 5 + SrcBlock "Sum1" + SrcPort 1 + Points [44, 0] + Branch { + ZOrder 33 + Points [0, -45] + DstBlock "Goto1" + DstPort 1 + } + Branch { + ZOrder 32 + DstBlock "jointPos_des" + DstPort 1 + } + } + Line { + ZOrder 10 + SrcBlock "Get Ms*jointAcc_des" + SrcPort 1 + DstBlock "Ms*jointAcc_des" + DstPort 1 + } + Line { + ZOrder 36 + SrcBlock "Gain5" + SrcPort 1 + DstBlock "Get Ms*jointAcc_des" + DstPort 1 + } + Line { + ZOrder 15 + SrcBlock "jointPos Ref" + SrcPort 1 + DstBlock "Gain2" + DstPort 1 + } + Line { + ZOrder 18 + SrcBlock "jointVel Ref" + SrcPort 1 + DstBlock "Gain1" + DstPort 1 + } + Line { + ZOrder 19 + SrcBlock "Gain1" + SrcPort 1 + Points [138, 0] + Branch { + ZOrder 39 + Points [0, -50] + DstBlock "Goto2" + DstPort 1 + } + Branch { + ZOrder 38 + DstBlock "jointVel_des" + DstPort 1 + } + } + Line { + ZOrder 23 + SrcBlock "jointAcc Ref" + SrcPort 1 + Points [58, 0] + Branch { + ZOrder 42 + Points [0, 115] + DstBlock "Gain5" + DstPort 1 + } + Branch { + ZOrder 41 + DstBlock "Goto3" + DstPort 1 + } + } + Line { + ZOrder 31 + SrcBlock "Gain2" + SrcPort 1 + Points [105, 0] + DstBlock "Sum1" + DstPort 1 + } + Line { + ZOrder 30 + SrcBlock "holder" + SrcPort 1 + DstBlock "Sum1" + DstPort 2 + } + } + } + Block { + BlockType Outport + Name "tau_feedback" + SID "313" + Position [610, 38, 640, 52] + ZOrder 388 + IconDisplay "Port number" + } + Line { + ZOrder 422 + SrcBlock "jointVel" + SrcPort 1 + DstBlock "PD Controller" + DstPort 4 + } + Line { + ZOrder 423 + SrcBlock "jointPos" + SrcPort 1 + Points [24, 0] + Branch { + ZOrder 426 + DstBlock "Reference Generator" + DstPort 1 + } + Branch { + ZOrder 425 + Points [0, 165] + DstBlock "PD Controller" + DstPort 5 + } + } + Line { + ZOrder 424 + SrcBlock "PD Controller" + SrcPort 1 + DstBlock "tau_feedback" + DstPort 1 + } + Line { + ZOrder 430 + SrcBlock "Reference Generator" + SrcPort 1 + DstBlock "PD Controller" + DstPort 1 + } + Line { + ZOrder 431 + SrcBlock "Reference Generator" + SrcPort 2 + DstBlock "PD Controller" + DstPort 2 + } + Line { + ZOrder 432 + SrcBlock "Reference Generator" + SrcPort 3 + DstBlock "PD Controller" + DstPort 3 + } + } + } + Block { + BlockType Sum + Name "Sum1" + SID "314" + Ports [2, 1] + Position [525, 24, 580, 211] + ZOrder 633 + ShowName off + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "jointTorques_des" + SID "503" + Position [700, 113, 730, 127] + ZOrder 638 + IconDisplay "Port number" + } + Line { + ZOrder 396 + SrcBlock "jointPos" + SrcPort 1 + Points [33, 0] + Branch { + ZOrder 402 + Points [0, -80] + DstBlock "PD Feedback Controller" + DstPort 2 + } + Branch { + ZOrder 401 + DstBlock "Gravity Compensation" + DstPort 1 + } + } + Line { + ZOrder 395 + SrcBlock "jointVel" + SrcPort 1 + DstBlock "PD Feedback Controller" + DstPort 1 + } + Line { + ZOrder 321 + SrcBlock "Sum1" + SrcPort 1 + Points [39, 0] + Branch { + ZOrder 414 + Points [0, -45] + DstBlock "Goto1" + DstPort 1 + } + Branch { + ZOrder 413 + DstBlock "jointTorques_des" + DstPort 1 + } + } + Line { + Name "gravityTorques" + ZOrder 337 + SrcBlock "Gravity Compensation" + SrcPort 1 + Points [21, 0] + Branch { + ZOrder 416 + Points [0, 75] + DstBlock "Goto2" + DstPort 1 + } + Branch { + ZOrder 415 + Labels [-1, 0] + DstBlock "Sum1" + DstPort 2 + } + } + Line { + Name "PD control" + ZOrder 365 + Labels [-1, 0] + SrcBlock "PD Feedback Controller" + SrcPort 1 + DstBlock "Sum1" + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "SetReferences" + SID "137" + Ports [1] + Position [565, 250, 620, 280] + ZOrder 383 + BackgroundColor "[0.827500, 0.576500, 0.603900]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Actuators/SetReferences" + SourceType "SetReferences" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + controlType "Torque" + refSpeed "50" + refAcceleration "1000000*(pi/180)" + } + Block { + BlockType SubSystem + Name "emergency stop: joint limits" + SID "470" + Ports [1] + Position [300, 250, 405, 270] + ZOrder 636 + BackgroundColor "red" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 22 + $ClassName "Simulink.Mask" + Display "disp('EMERGENCY STOP')" + RunInitForIconRedraw "off" + } + System { + Name "emergency stop: joint limits" + 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 "904" + Block { + BlockType Inport + Name "jointPos" + SID "471" + Position [150, 238, 180, 252] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n1" + SID "472" + 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 "473" + 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 "474" + 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] + 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 "922" + Block { + BlockType Inport + Name "jointPos" + SID "475" + Position [60, 103, 90, 117] + ZOrder 552 + IconDisplay "Port number" + } + Block { + BlockType EnablePort + Name "Enable" + SID "476" + Ports [] + Position [227, -30, 247, -10] + ZOrder 553 + } + Block { + BlockType Assertion + Name "Assertion" + SID "477" + Position [340, 72, 400, 118] + ZOrder 207 + } + Block { + BlockType Reference + Name "GetLimits" + SID "478" + Ports [0, 2] + Position [20, 23, 140, 92] + ZOrder 551 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetLimits" + SourceType "GetLimits" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + limitsSource "ControlBoard" + limitsType "Position" + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "479" + Ports [4, 1] + 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 "3782" + Block { + BlockType Inport + Name "umin" + SID "479::18" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "umax" + SID "479::19" + Position [20, 136, 40, 154] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "u" + SID "479::1" + Position [20, 171, 40, 189] + ZOrder -3 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tol" + SID "479::20" + Position [20, 206, 40, 224] + ZOrder -4 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "479::3781" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 131 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "479::3780" + Tag "Stateflow S-Function jointsControl 18" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 130 + FunctionName "sf_sfun" + PortCounts "[4 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "inRange" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "479::3782" + Position [460, 241, 480, 259] + ZOrder 132 + } + Block { + BlockType Outport + Name "inRange" + SID "479::5" + Position [460, 101, 480, 119] + ZOrder -8 + IconDisplay "Port number" + } + Line { + ZOrder 29 + SrcBlock "umin" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 30 + SrcBlock "umax" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 31 + SrcBlock "u" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 32 + SrcBlock "tol" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "inRange" + ZOrder 33 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "inRange" + DstPort 1 + } + Line { + ZOrder 34 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 35 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Constant + Name "index1" + SID "480" + Position [70, 140, 95, 150] + 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 "Assertion" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "GetLimits" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 4 + 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 "481" + 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] + 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 "827" + Block { + BlockType Inport + Name "jointPos" + SID "482" + Position [15, 53, 45, 67] + ZOrder 552 + IconDisplay "Port number" + } + Block { + BlockType EnablePort + Name "Enable" + SID "483" + Ports [] + Position [217, -25, 237, -5] + ZOrder 553 + } + Block { + BlockType Assertion + Name "Assertion" + SID "484" + Position [340, 72, 400, 118] + ZOrder 207 + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "485" + Ports [2, 1] + Position [165, 24, 285, 166] + 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 "3782" + Block { + BlockType Inport + Name "u" + SID "485::1" + Position [20, 101, 40, 119] + ZOrder -3 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "delta_u_max" + SID "485::18" + Position [20, 136, 40, 154] + ZOrder -1 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "485::3781" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 131 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "485::3780" + Tag "Stateflow S-Function jointsControl 14" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 130 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "noSpikes" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "485::3782" + Position [460, 241, 480, 259] + ZOrder 132 + } + Block { + BlockType Outport + Name "noSpikes" + SID "485::5" + Position [460, 101, 480, 119] + ZOrder -8 + IconDisplay "Port number" + } + Line { + ZOrder 21 + SrcBlock "u" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 22 + SrcBlock "delta_u_max" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "noSpikes" + ZOrder 23 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "noSpikes" + DstPort 1 + } + Line { + ZOrder 24 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 25 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Constant + Name "index1" + SID "486" + Position [-40, 124, 100, 136] + 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 "Assertion" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "index1" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 2 + } + } + } + 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 SubSystem + Name "synchronizer" + SID "301" + Ports [] + Position [432, 163, 517, 188] + ZOrder 631 + ForegroundColor "red" + BackgroundColor "green" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 23 + $ClassName "Simulink.Mask" + Display "disp('SYNCHRONIZER')" + } + System { + Name "synchronizer" + 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 "910" + Block { + BlockType SubSystem + Name "GAZEBO_SYNCHRONIZER" + SID "302" + Ports [0, 0, 1] + Position [15, 15, 140, 75] + ZOrder -7 + RequestExecContextInheritance off + System { + Name "GAZEBO_SYNCHRONIZER" + 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 EnablePort + Name "Enable" + SID "303" + Ports [] + Position [177, 90, 197, 110] + ZOrder 538 + } + Block { + BlockType Reference + Name "Simulator Synchronizer" + SID "304" + Ports [] + Position [120, 24, 250, 61] + ZOrder 539 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" + SourceType "Simulator Synchronizer" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + period "Config.tStep" + serverPortName "'/clock/rpc'" + clientPortName "'/WBT_synchronizer/rpc:o'" + } + } + } + Block { + BlockType Logic + Name "Logical\nOperator" + SID "305" + Ports [1, 1] + Position [-25, -41, 5, -9] + ZOrder 307 + BlockMirror on + NamePlacement "alternate" + ShowName off + Operator "NOT" + AllPortsSameDT off + OutDataTypeStr "boolean" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n" + SID "306" + Position [115, -35, 230, -15] + ZOrder 304 + BlockMirror on + NamePlacement "alternate" + ShowName off + Value "Config.ON_GAZEBO" + } + Block { + BlockType SubSystem + Name "REAL_TIME_SYNC" + SID "307" + Ports [0, 0, 1] + Position [-170, 12, -45, 72] + ZOrder -9 + RequestExecContextInheritance off + System { + Name "REAL_TIME_SYNC" + 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 EnablePort + Name "Enable" + SID "308" + Ports [] + Position [167, 95, 187, 115] + ZOrder 538 + } + Block { + BlockType Reference + Name "Real Time Synchronizer" + SID "309" + Ports [] + Position [115, 34, 240, 71] + ZOrder 539 + ForegroundColor "[0.916667, 0.916667, 0.916667]" + BackgroundColor "gray" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" + SourceType "Real Time Synchronizer" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + period "Config.tStep" + } + } + } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "647" + Ports [1] + Position [135, -95, 195, -75] + ZOrder 701 + ShowName off + HideAutomaticName off + VariableName "yarp_time" + MaxDataPoints "inf" + SaveFormat "Structure With Time" + Save2DSignal "3-D array (concatenate along third dimension)" + FixptAsFi on + SampleTime "-1" + } + Block { + BlockType Reference + Name "Yarp Clock" + SID "648" + Ports [0, 1] + Position [-40, -94, 25, -76] + ZOrder 700 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Yarp Clock" + SourceType "YARP Clock" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + } + Line { + ZOrder 1 + SrcBlock "Logical\nOperator" + SrcPort 1 + Points [-75, 0] + DstBlock "REAL_TIME_SYNC" + DstPort enable + } + Line { + ZOrder 2 + SrcBlock "ON_GAZEBO\n" + SrcPort 1 + Points [-35, 0] + Branch { + ZOrder 3 + DstBlock "GAZEBO_SYNCHRONIZER" + DstPort enable + } + Branch { + ZOrder 4 + DstBlock "Logical\nOperator" + DstPort 1 + } + } + Line { + ZOrder 5 + SrcBlock "Yarp Clock" + SrcPort 1 + DstBlock "To Workspace" + DstPort 1 + } + } + } + Line { + ZOrder 347 + SrcBlock "GetMeasurement" + SrcPort 1 + Points [24, 0] + Branch { + ZOrder 413 + Points [0, 20] + DstBlock "Goto" + DstPort 1 + } + Branch { + ZOrder 412 + Points [0, -30] + DstBlock "emergency stop: joint limits" + DstPort 1 + } + Branch { + ZOrder 395 + DstBlock "PD + GRAVITY CONTROL" + DstPort 2 + } + } + Line { + ZOrder 396 + SrcBlock "PD + GRAVITY CONTROL" + SrcPort 1 + DstBlock "SetReferences" + DstPort 1 + } + Line { + ZOrder 407 + SrcBlock "GetMeasurement2" + SrcPort 1 + DstBlock "PD + GRAVITY CONTROL" + DstPort 1 + } + Line { + ZOrder 414 + SrcBlock "GetMeasurement1" + SrcPort 1 + DstBlock "Goto1" + DstPort 1 + } + } +} +#Finite State Machines +# +# Stateflow 80000014 +# +# +Stateflow { + machine { + id 1 + name "jointsControl" + created "12-Mar-2015 18:23:09" + isLibrary 0 + sfVersion 80000012 + firstTarget 20 + } + chart { + id 2 + machine 1 + name "emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/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 3 0 0] + viewObj 2 + ssIdHighWaterMark 8 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 14 + disableImplicitCasting 1 + eml { + name "checkSpikes" + } + supportVariableSizing 0 + firstData 4 + firstTransition 8 + firstJunction 7 + } + 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 noSpikes = checkSpikes(u, delta_u_max)\n\n noSpikes = wbc.checkSpikes(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 4 + 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 [2 0 5] + } + data { + id 5 + 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 [2 4 6] + } + data { + id 6 + 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 [2 5 0] + } + junction { + id 7 + position [23.5747 49.5747 7] + chart 2 + subviewer 2 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [2 0 0] + } + transition { + id 8 + 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 7 + 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 9 + machine 1 + name "emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/MATLAB Function" + chart 2 + } + chart { + id 10 + 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 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 18 + disableImplicitCasting 1 + eml { + name "checkRangeFCN" + } + supportVariableSizing 0 + firstData 12 + firstTransition 18 + firstJunction 17 + } + state { + id 11 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 10 + treeNode [10 0 0 0] + superState SUBCHART + subviewer 10 + 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" + 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 12 + 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 [10 0 13] + } + data { + id 13 + 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 [10 12 14] + } + data { + id 14 + 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 [10 13 15] + } + data { + id 15 + 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 [10 14 16] + } + data { + id 16 + 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 [10 15 0] + } + junction { + id 17 + position [23.5747 49.5747 7] + chart 10 + subviewer 10 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [10 0 0] + } + transition { + id 18 + 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 17 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 10 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 10 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [10 0 0] + } + instance { + id 19 + machine 1 + name "emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" + chart 10 + } + target { + id 20 + machine 1 + name "sfun" + description "Default Simulink S-Function Target." + linkNode [1 0 0] + } +} diff --git a/controllers/fixed-base-joints-control/src-static-gui/closeModel.m b/controllers/fixed-base-joints-control/src-static-gui/closeModel.m new file mode 100644 index 0000000..5d07306 --- /dev/null +++ b/controllers/fixed-base-joints-control/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('jointsControl.mdl'); +close_system('jointsControl.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/torque-controllers/momentum-based-standup/src-gui/compileModel.m b/controllers/fixed-base-joints-control/src-static-gui/compileModel.m similarity index 73% rename from torque-controllers/momentum-based-standup/src-gui/compileModel.m rename to controllers/fixed-base-joints-control/src-static-gui/compileModel.m index f048a71..d421e86 100644 --- a/torque-controllers/momentum-based-standup/src-gui/compileModel.m +++ b/controllers/fixed-base-joints-control/src-static-gui/compileModel.m @@ -3,14 +3,13 @@ % Compile the Simulink model through Matlab command line. %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -disp('Compiling the model...') +disp('Compiling the Simulink model...') pause(1.5) try - torqueBalancingStandup([], [], [], 'compile') - torqueBalancingStandup([], [], [], 'term') + jointsControl([], [], [], 'compile') + jointsControl([], [], [], 'term') catch ME @@ -20,4 +19,6 @@ 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/torque-controllers/momentum-based-yoga/startYogaWithoutSimulinkGui.m b/controllers/fixed-base-joints-control/startModelWithStaticGui.m similarity index 83% rename from torque-controllers/momentum-based-yoga/startYogaWithoutSimulinkGui.m rename to controllers/fixed-base-joints-control/startModelWithStaticGui.m index cb77798..ab20a56 100644 --- a/torque-controllers/momentum-based-yoga/startYogaWithoutSimulinkGui.m +++ b/controllers/fixed-base-joints-control/startModelWithStaticGui.m @@ -1,6 +1,6 @@ %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% RUN THIS SCRIPT TO USE SIMULINK WITHOUT OPENING THE SIMULINK GUI +% RUN THIS SCRIPT TO USE SIMULINK WITH THE STATIC SIMULINK GUI %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear variables @@ -8,19 +8,19 @@ % add the path to the static gui and to some utility functions addpath('../../library/matlab-gui'); -addpath('./src-gui'); +addpath('./src-static-gui'); disp('[startModel]: loading the model...') % open the model -open_system('torqueBalancingYoga.mdl','loadonly'); +open_system('jointsControl.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 "Close Model" button instead.') +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') diff --git a/controllers/fixed-base-joints-control/stopJointsControl.m b/controllers/fixed-base-joints-control/stopJointsControl.m new file mode 100644 index 0000000..e4b9935 --- /dev/null +++ b/controllers/fixed-base-joints-control/stopJointsControl.m @@ -0,0 +1,59 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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. + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% 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 \ No newline at end of file diff --git a/controllers/floating-base-balancing-position-control/README.md b/controllers/floating-base-balancing-position-control/README.md new file mode 100644 index 0000000..52aef16 --- /dev/null +++ b/controllers/floating-base-balancing-position-control/README.md @@ -0,0 +1,21 @@ +## Module description + +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. + +### Supported robots + +Currently, supported robots are: `iCubGazeboV2_5`. + +## Module details + +### 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 diff --git a/controllers/floating-base-balancing-position-control/app/robots/iCubGazeboV2_5/configPositionBalancing.m b/controllers/floating-base-balancing-position-control/app/robots/iCubGazeboV2_5/configPositionBalancing.m new file mode 100644 index 0000000..476fd9f --- /dev/null +++ b/controllers/floating-base-balancing-position-control/app/robots/iCubGazeboV2_5/configPositionBalancing.m @@ -0,0 +1,22 @@ +% CONFIGPOSITIONBALANCING configuration parameters related to the controller +% for the specific robot + +%% --- Initialization --- + +% booleans that describe the feet contacts activation/deactivation. +% If set to 1, the associated foot is in contact. Format: [l_sole; r_sole] +Config.feetContactStatus = [1, 1]; + +% if TRUE, the controller will STOP if the joints hit the joints limits +% and/or if the (unsigned) difference between two consecutive joints +% encoders measurements is greater than a given threshold. +Config.EMERGENCY_STOP_WITH_JOINTS_LIMITS = false; +Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES = true; + +% If TRUE, the CoM references are updated in order to follow a sinusoidal +% trajectory. +Config.COM_SINE_MOVEMENTS = true; + +% Max unsigned difference between two consecutive (measured) joint positions, +% i.e. delta_jointPos = abs(jointPos(k) - jointPos(k-1)) [rad] +Sat.maxJointsPositionDelta = 15*pi/180; \ No newline at end of file diff --git a/torque-controllers/impedance-control/app/robots/icubGazeboSim/configRobot.m b/controllers/floating-base-balancing-position-control/app/robots/iCubGazeboV2_5/configRobot.m similarity index 67% rename from torque-controllers/impedance-control/app/robots/icubGazeboSim/configRobot.m rename to controllers/floating-base-balancing-position-control/app/robots/iCubGazeboV2_5/configRobot.m index 396bf3f..f45f948 100644 --- a/torque-controllers/impedance-control/app/robots/icubGazeboSim/configRobot.m +++ b/controllers/floating-base-balancing-position-control/app/robots/iCubGazeboV2_5/configRobot.m @@ -1,16 +1,10 @@ % CONFIGROBOT initializes parameters specific of a particular robot -% (e.g., icuGazeboSim) -% +% (e.g., icubGazeboSim) %% --- Initialization --- -% Gains and parameters for impedance controller -Config.ON_GAZEBO = true; -ROBOT_DOF = 23; - -% 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] +Config.ON_GAZEBO = true; +ROBOT_DOF = 23; % Robot configuration for WBToolbox WBTConfigRobot = WBToolbox.Configuration; @@ -37,26 +31,7 @@ Config.numOfJointsForEachControlboard = [Config.numOfJointsForEachControlboard; length(ControlBoards.(WBTConfigRobot.ControlBoardsNames{n}))]; end -% References for the demo movements -if MOVING - - % Impedance gains - Kp = 10*diag(ones(1,ROBOT_DOF)); - Kd = 2*sqrt(Kp); - - AMPLS = 5*ones(1,ROBOT_DOF); - FREQS = 0.25*ones(1,ROBOT_DOF); - -else - - % Impedance gains - Kp = 0*diag(ones(1,ROBOT_DOF)); - Kd = 0*diag(ones(1,ROBOT_DOF)); - - AMPLS = 0*ones(1,ROBOT_DOF); - FREQS = 0*ones(1,ROBOT_DOF); -end - -if size(Kp,1) ~= ROBOT_DOF - error('Dimension of Kp different from ROBOT_DOF') -end \ No newline at end of file +% Frames list +Frames.LEFT_FOOT = 'l_sole'; +Frames.RIGHT_FOOT = 'r_sole'; +Frames.COM = 'com'; \ No newline at end of file diff --git a/controllers/floating-base-balancing-position-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m b/controllers/floating-base-balancing-position-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m new file mode 100644 index 0000000..c997cda --- /dev/null +++ b/controllers/floating-base-balancing-position-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m @@ -0,0 +1,27 @@ +% GAINSANDREFERENCES compute gains matrices, references and regularization +% parameters for the controller for a specific robot + +%% --- Initialization --- + +% references for the CoM (sine trajectory) +Config.referencesCoM.amplitude = 0.02; %[m] +Config.referencesCoM.frequency = 0.25; %[Hz] +Config.referencesCoM.direction = [0;1;0]; + +% pseudoinverse tolerances +Config.Reg.pinv_tol = 1e-4; +Config.Reg.pinvDampBaseVel = 1e-4; + +% Gains matrices + +% CoM gains +Config.Gain.KP_CoM = diag([50;50;50]); +Config.Gain.KD_CoM = 2*sqrt(Config.Gain.KP_CoM); + +% Feet pose Gains +Config.Gain.KP_feet = diag([50;50;50]); +Config.Gain.KD_feet = 2*sqrt(Config.Gain.KP_feet); + +% Postural task gains % torso % lArm % rArm % lLeg % rLeg +Config.Gain.KP_joints = diag([20;20;20; 10;10;10;10; 10;10;10;10; 30;30;20;30;40;40; 30;30;20;30;40;40]); +Config.Gain.KD_joints = 2*sqrt(Config.Gain.KP_joints); \ No newline at end of file diff --git a/controllers/floating-base-balancing-position-control/initPositionControlBalancing.m b/controllers/floating-base-balancing-position-control/initPositionControlBalancing.m new file mode 100644 index 0000000..38ba3fb --- /dev/null +++ b/controllers/floating-base-balancing-position-control/initPositionControlBalancing.m @@ -0,0 +1,57 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% /** +% * Copyright (C) 2016 CoDyCo +% * @author: Daniele Pucci, Gabriele Nava +% * Permission is granted to copy, distribute, and/or modify this program +% * under the terms of the GNU General Public License, version 2 or any +% * later version published by the Free Software Foundation. +% * +% * A copy of the license can be found at +% * http://www.robotcub.org/icub/license/gpl.txt +% * +% * This program is distributed in the hope that it will be useful, but +% * WITHOUT ANY WARRANTY; without even the implied warranty of +% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General +% * Public License for more details +% */ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% NOTE: THIS SCRIPT IS RUN AUTOMATICALLY WHEN THE USER STARTS THE ASSOCIATED +% SIMULINK MODEL. NO NEED TO RUN THIS SCRIPT EVERY TIME. +clearvars -except sl_synch_handles simulinkStaticGUI +clc + +% Add path to local source code +addpath('./src/') + +%% GENERAL SIMULATION INFO +% +% If you are simulating the robot with Gazebo, remember that it is required +% to launch Gazebo as follows: +% +% gazebo -slibgazebo_yarp_clock.so +% +% and properly set the environmental variable YARP_ROBOT_NAME in the .bashrc. + +% Simulation time in seconds. For long simulations, put an high number +% (not inf) for allowing automatic code generation +Config.SIMULATION_TIME = 600000; + +% Controller period [s] +Config.tStep = 0.01; + +%% PRELIMINARY CONFIGURATION + +% 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'),'/configPositionBalancing.m')); +run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/gainsAndReferences.m')); \ No newline at end of file diff --git a/controllers/floating-base-balancing-position-control/positionControlBalancing.mdl b/controllers/floating-base-balancing-position-control/positionControlBalancing.mdl new file mode 100644 index 0000000..13b6ef0 --- /dev/null +++ b/controllers/floating-base-balancing-position-control/positionControlBalancing.mdl @@ -0,0 +1,11281 @@ +Model { + Name "positionControlBalancing" + Version 9.0 + SavedCharacterEncoding "UTF-8" + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.3504" + NumModelReferences 0 + NumTestPointedSignals 0 + NumProvidedFunctions 0 + NumRequiredFunctions 0 + NumResetEvents 0 + HasInitializeEvent 0 + HasTerminateEvent 0 + IsExportFunctionModel 0 + NumParameterArguments 0 + NumExternalFileReferences 28 + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Configuration" + Path "positionControlBalancing/Configuration" + SID "4537" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "positionControlBalancing/GetMeasurement" + SID "4538" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "positionControlBalancing/GetMeasurement1" + SID "4887" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Jacob" + "ians and ForwardKinematics/Compute ForwardKinematics/ForwardKinematics_CoM" + SID "4949" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Jacob" + "ians and ForwardKinematics/Compute ForwardKinematics/ForwardKinematics_l_sole" + SID "4950" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Jacob" + "ians and ForwardKinematics/Compute ForwardKinematics/ForwardKinematics_l_sole_0" + SID "4951" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Jacob" + "ians and ForwardKinematics/Compute ForwardKinematics/ForwardKinematics_r_sole" + SID "4952" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Jacob" + "ians and ForwardKinematics/Compute ForwardKinematics/ForwardKinematics_r_sole_0" + SID "4953" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/DotJNu" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Jacob" + "ians and ForwardKinematics/Compute Jacobians and Jdot_nu/JDot_CoM nu" + SID "4977" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/DotJNu" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Jacob" + "ians and ForwardKinematics/Compute Jacobians and Jdot_nu/JDot_l_sole nu" + SID "4978" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/DotJNu" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Jacob" + "ians and ForwardKinematics/Compute Jacobians and Jdot_nu/JDot_r_sole nu" + SID "4979" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Jacob" + "ians and ForwardKinematics/Compute Jacobians and Jdot_nu/Jacobian CoM" + SID "4964" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Jacob" + "ians and ForwardKinematics/Compute Jacobians and Jdot_nu/Jacobian l_sole" + SID "4965" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Jacob" + "ians and ForwardKinematics/Compute Jacobians and Jdot_nu/Jacobian r_sole" + SID "4966" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/References and Initial Conditions/Compute State V" + "elocity/Feet Jacobians/Jacobian l_sole" + SID "4883" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/References and Initial Conditions/Compute State V" + "elocity/Feet Jacobians/Jacobian r_sole" + SID "4884" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/References and Initial Conditions/Get CoM Positio" + "n/CoM Position" + SID "5041" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/References and Initial Conditions/Transformation " + "Matrix From FixedLink to Base/RelativeTransform Between l_sole and Base" + SID "4862" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/References and Initial Conditions/Transformation " + "Matrix From FixedLink to Base/RelativeTransform Between r_sole and Base" + SID "4863" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/References and Initial Conditions/holder" + SID "5027" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/References and Initial Conditions/holder " + SID "5035" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/References and Initial Conditions/holder 1" + SID "5036" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "positionControlBalancing/POSITION CONTROLLER (IK)/References and Initial Conditions/holder1" + SID "5069" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Actuators/SetReferences" + Path "positionControlBalancing/SetReferences" + SID "5075" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetLimits" + Path "positionControlBalancing/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/GetLimits" + SID "2432" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Simulator Synchronizer" + Path "positionControlBalancing/synchronizer/GAZEBO_SYNCHRONIZER/Simulator Synchronizer" + SID "2426" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Real Time Synchronizer" + Path "positionControlBalancing/synchronizer/REAL_TIME_SYNC/Real Time Synchronizer" + SID "2431" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Clock" + Path "positionControlBalancing/synchronizer/Yarp Clock" + SID "4629" + 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" + PostLoadFcn "%% Try to open the static GUI\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)));\ninitPositionControlBalancing;\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.star" + "tButton,'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" + 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" + 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] + } + Object { + $PropName "EditorsInfo" + $ObjectID 5 + $ClassName "Simulink.EditorInfo" + IsActive [1] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [1815.0, 876.0] + ZoomFactor [3.028813559322034] + Offset [850.37772803581424, -288.61108002238387] + } + Object { + $PropName "DockComponentsInfo" + $ObjectID 6 + $ClassName "Simulink.DockComponentInfo" + Type "GLUE2:PropertyInspector" + ID "Property Inspector" + Visible [0] + CreateCallback "" + UserData "" + Floating [0] + DockPosition "Right" + Width [640] + Height [480] + } + WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + "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" + } + } + HideAutomaticNames on + Created "Tue Feb 18 10:13:36 2014" + Creator "daniele" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "gnava" + ModifiedDateFormat "%" + LastModifiedDate "Fri Mar 08 16:25:39 2019" + RTWModifiedTimeStamp 473963138 + 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" + VisualizeLoggedSignalsWhenLoggingToFile off + 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 7 + $ClassName "Simulink.SimulationData.ModelLoggingInfo" + model_ "positionControlBalancing" + signals_ [] + overrideMode_ [0.0] + Array { + Type "Cell" + Dimension 1 + Cell "positionControlBalancing" + 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 8 + Version "1.17.1" + DisabledProps [] + Description "" + Array { + Type "Handle" + Dimension 9 + Simulink.SolverCC { + $ObjectID 9 + Version "1.17.1" + DisabledProps [] + Description "" + 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 10 + Version "1.17.1" + DisabledProps [] + Description "" + 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 11 + Version "1.17.1" + Array { + Type "Cell" + Dimension 8 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + Cell "UseSpecifiedMinMax" + PropName "DisabledProps" + } + Description "" + 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 12 + Version "1.17.1" + Array { + Type "Cell" + Dimension 1 + Cell "UseOnlyExistingSharedCode" + PropName "DisabledProps" + } + Description "" + 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 13 + Version "1.17.1" + DisabledProps [] + Description "" + 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 14 + Version "1.17.1" + DisabledProps [] + Description "" + 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 15 + Version "1.17.1" + DisabledProps [] + Description "" + 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 16 + 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" + } + SystemTargetFile "grt.tlc" + HardwareBoard "None" + TLCOptions "" + GenCodeOnly on + MakeCommand "make_rtw" + GenerateMakefile on + PackageGeneratedCodeAndArtifacts off + PackageName "" + TemplateMakefile "grt_default_tmf" + PostCodeGenCommand "" + Description "" + GenerateReport off + SaveLog 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 17 + 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 "" + 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 18 + Version "1.17.1" + Array { + Type "Cell" + Dimension 17 + 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" + PropName "DisabledProps" + } + Description "" + Array { + Type "Handle" + Dimension 1 + Simulink.CPPComponent { + $ObjectID 19 + Version "1.18.0" + Array { + Type "Cell" + Dimension 10 + Cell "Description" + Cell "Components" + Cell "Name" + Cell "GenerateDestructor" + Cell "GenerateExternalIOAccessMethods" + Cell "ParameterMemberVisibility" + Cell "InternalMemberVisibility" + Cell "GenerateParameterAccessMethods" + Cell "GenerateInternalMemberAccessMethods" + Cell "UseOperatorNewForModelRefRegistration" + PropName "DisabledProps" + } + Description "" + 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 20 + Version "1.17.1" + DisabledProps [] + Description "Simulink Coverage Configuration Component" + 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" + ExtraOptions "-aBlockCommentType=\"BlockPathComment\" -aIgnoreTestpoints=0 " + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 730, 330, 2252, 1200 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 8 + } + Object { + $PropName "DataTransfer" + $ObjectID 21 + $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 { + 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 BusCreator + DisplayOption "none" + OutDataTypeStr "Inherit: auto" + NonVirtualBus off + } + Block { + BlockType BusSelector + } + 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 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 off + } + 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 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 + IsObserver 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 "Inherit from input (this choice will be removed - see release notes)" + FixptAsFi off + NumInputs "1" + SampleTime "0" + } + } + System { + Name "positionControlBalancing" + 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 "303" + ReportName "simulink-default.rpt" + SIDHighWatermark "5149" + Block { + BlockType Reference + Name "Configuration" + SID "4537" + Ports [] + Position [855, -240, 920, -220] + ZOrder 963 + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Configuration" + SourceType "" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + ConfigSource "Workspace" + ConfigObject "'WBTConfigRobot'" + RobotName "'icubSim'" + UrdfFile "'model.urdf'" + ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" + ControlBoardsNames "{'left_arm','right_arm','torso'}" + LocalName "'WBT'" + GravityVector "[0,0,-9.81]" + } + Block { + BlockType SubSystem + Name "Dump and visualize" + SID "2275" + Ports [] + Position [1345, -199, 1445, -171] + ZOrder 962 + ForegroundColor "blue" + BackgroundColor "[0.333333, 1.000000, 1.000000]" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 22 + $ClassName "Simulink.Mask" + Display "disp('VISUALIZER')" + } + System { + Name "Dump and visualize" + 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 "241" + Block { + BlockType Demux + Name "Demux" + SID "5128" + Ports [1, 2] + Position [0, 33, 5, 117] + ZOrder 1039 + ShowName off + Outputs "[16;ROBOT_DOF]" + DisplayOption "bar" + Port { + PortNumber 2 + Name "jointPos Desired" + } + } + Block { + BlockType Demux + Name "Demux1" + SID "5118" + Ports [1, 5] + Position [395, -239, 400, -141] + ZOrder 1036 + 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 "5148" + Ports [1, 2] + Position [0, 170, 5, 450] + ZOrder 1059 + ShowName off + Outputs "[6;ROBOT_DOF]" + DisplayOption "bar" + Port { + PortNumber 1 + Name "baseAcc Desired" + } + Port { + PortNumber 2 + Name "jointAcc Desired" + } + } + Block { + BlockType Demux + Name "Demux2" + SID "5119" + Ports [1, 5] + Position [395, -104, 400, -6] + ZOrder 1037 + 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 "5120" + Ports [1, 5] + Position [395, 46, 400, 144] + ZOrder 1038 + 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 "Demux4" + SID "5130" + Ports [1, 2] + Position [795, 33, 800, 117] + ZOrder 1051 + ShowName off + Outputs "[6;ROBOT_DOF]" + DisplayOption "bar" + Port { + PortNumber 2 + Name "jointVel Desired" + } + } + Block { + BlockType Demux + Name "Demux5" + SID "5131" + Ports [1, 5] + Position [1190, -239, 1195, -141] + ZOrder 1048 + 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 "Demux6" + SID "5132" + Ports [1, 5] + Position [1190, -104, 1195, -6] + ZOrder 1049 + 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 "5133" + Ports [1, 5] + Position [1190, 46, 1195, 144] + ZOrder 1050 + 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 "5145" + Ports [1, 5] + Position [395, 331, 400, 429] + ZOrder 1058 + 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 From + Name "From" + SID "5113" + Position [-110, 67, -60, 83] + ZOrder 1024 + GotoTag "q_ref" + TagVisibility "global" + } + Block { + BlockType From + Name "From1" + SID "5114" + Position [690, 67, 740, 83] + ZOrder 1025 + GotoTag "nu_ref" + TagVisibility "global" + } + Block { + BlockType From + Name "From2" + SID "5115" + Position [-110, 302, -60, 318] + ZOrder 1026 + GotoTag "nuDot_ref" + TagVisibility "global" + } + Block { + BlockType From + Name "From3" + SID "5116" + Position [-110, -198, -60, -182] + ZOrder 1027 + GotoTag "jointPos" + TagVisibility "global" + Port { + PortNumber 1 + Name "jointPos Measured" + } + } + Block { + BlockType From + Name "From4" + SID "5117" + Position [690, -198, 740, -182] + ZOrder 1028 + GotoTag "jointVel" + TagVisibility "global" + Port { + PortNumber 1 + Name "jointVel Measured" + } + } + Block { + BlockType Gain + Name "Gain" + SID "5121" + Position [225, -205, 270, -175] + ZOrder 1033 + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain1" + SID "5122" + Position [225, -70, 270, -40] + ZOrder 1034 + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain2" + SID "5123" + Position [225, 80, 270, 110] + ZOrder 1035 + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain3" + SID "5134" + Position [1020, -205, 1065, -175] + ZOrder 1045 + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain4" + SID "5135" + Position [1020, -70, 1065, -40] + ZOrder 1046 + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain5" + SID "5136" + Position [1020, 80, 1065, 110] + ZOrder 1047 + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain6" + SID "5143" + Position [225, 225, 270, 255] + ZOrder 1054 + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain7" + SID "5146" + Position [225, 365, 270, 395] + ZOrder 1057 + Gain "180/pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum1" + SID "5137" + Ports [2, 1] + Position [885, -65, 905, -45] + ZOrder 1041 + ShowName off + IconShape "round" + Inputs "+|-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "jointVel Error" + } + } + Block { + BlockType Sum + Name "Sum2" + SID "5124" + Ports [2, 1] + Position [90, -65, 110, -45] + ZOrder 1029 + ShowName off + IconShape "round" + Inputs "+|-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "jointPos Error" + } + } + Block { + BlockType Terminator + Name "Terminator" + SID "5129" + Position [45, 45, 65, 65] + ZOrder 1040 + } + Block { + BlockType Terminator + Name "Terminator1" + SID "5138" + Position [840, 45, 860, 65] + ZOrder 1052 + } + Block { + BlockType Scope + Name "baseAcc Des" + SID "5144" + Ports [1] + Position [500, 185, 605, 295] + ZOrder 1053 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr." + "Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),extmg" + "r.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true,'Da" + "taLoggingVariableName','baseAcc_des_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','" + "DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinY" + "LimReal','-99.58424','MaxYLimReal','93.76331','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','99.58424','Legen" + "dVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980" + "3922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.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','%','LineProp" + "ertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.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'),struct('Co" + "lor',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumL" + "ines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1)},'DisplayPropertyDefaults',struct('MinYLimReal','-99.5" + "8424','MaxYLimReal','93.76331','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','99.58424','LegendVisibility','O" + "n','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450" + "9803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.41176470" + "5882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569" + " 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])" + NumInputPorts "1" + Floating off + } + Block { + BlockType Scope + Name "jointAcc Des" + SID "5147" + Ports [5] + Position [500, 325, 605, 435] + ZOrder 1056 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr." + "Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),extmg" + "r.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true,'Da" + "taLoggingVariableName','jointAcc_des_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1'," + "'DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('Min" + "YLimReal','-13.19365','MaxYLimReal','48.48513','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','48.48513','Lege" + "ndVisibility','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.62352941176470" + "6 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0" + ".274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePro" + "pertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[1 0 1],'LineStyle','-','LineWidth',0.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','non" + "e','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',{{}},'Num" + "Lines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-7.55104','MaxYLimReal','11.180" + "99','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPh" + "ase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder'" + ",[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745" + "098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509803" + "9215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','no" + "ne','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" + "Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','" + "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker'," + "'none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placeme" + "nt',2),struct('MinYLimReal','-8.81697','MaxYLimReal','6.55594','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" + "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274" + "509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294117" + "64706 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','%','Lin" + "ePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" + "Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','" + "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker'," + "'none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" + "t('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}}," + "'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-92.48844','MaxYLimReal','1" + "07.15225','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'Plo" + "tMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Color" + "Order',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156" + "862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074" + "5098039215686 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,'Marke" + "r','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],'LineSt" + "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Ma" + "rker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'P" + "lacement',4),struct('MinYLimReal','-103.91569','MaxYLimReal','91.63591','YLabelReal','','MinYLimMag','0','MaxYLimMa" + "g','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor'," + "[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6" + "23529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176" + "47058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" + ",struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'Lin" + "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5," + "'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNam" + "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabe" + "lReal','','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.0745098039" + "215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','" + "LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" + "olor',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','" + "none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'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])" + NumInputPorts "5" + Floating off + } + Block { + BlockType Scope + Name "jointPos Des" + SID "5125" + Ports [5] + Position [500, 40, 605, 150] + ZOrder 1032 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr." + "Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),extmg" + "r.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true,'Da" + "taLoggingVariableName','jointPos_Desired_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation'," + "'1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct(" + "'MinYLimReal','-3.69416','MaxYLimReal','3.40945','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','3.69416','Leg" + "endVisibility','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.6235294117647" + "06 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 " + "0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePr" + "opertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" + "or',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','no" + "ne','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" + "Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'Nu" + "mLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-39.77077','MaxYLimReal','54.6" + "2671','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMag" + "Phase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrde" + "r',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3921568627" + "45098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.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,'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,'Place" + "ment',2),struct('MinYLimReal','-39.79768','MaxYLimReal','54.63755','YLabelReal','','MinYLimMag','0','MaxYLimMag','1" + "0','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68" + "6274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529" + "411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058" + "823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," + "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" + "ct('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,'Mark" + "er','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" + "truct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{" + "{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-3.48467','MaxYLimReal'" + ",'3.51702','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'Pl" + "otMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Colo" + "rOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.39215" + "6862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07" + "45098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyl" + "e','-','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'),s" + "truct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineS" + "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'M" + "arker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'" + "Placement',4),struct('MinYLimReal','-3.48855','MaxYLimReal','3.51696','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.623" + "529411764706 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'),s" + "truct('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,'M" + "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" + "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames" + "',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelR" + "eal','','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.074509803921" + "5686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('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,'Marker','no" + "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])" + NumInputPorts "5" + Floating off + } + Block { + BlockType Scope + Name "jointPos Error" + SID "5126" + Ports [5] + Position [500, -110, 605, 0] + ZOrder 1031 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr." + "Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),extmg" + "r.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true,'Da" + "taLoggingVariableName','jointPos_Error_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1" + "','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('M" + "inYLimReal','-1.43065','MaxYLimReal','3.6581','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','3.6581','LegendV" + "isibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098039" + "22 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1" + ";1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27" + "4509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProper" + "tiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," + "[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','Lin" + "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none'," + "'Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLin" + "es',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-1.29037','MaxYLimReal','1.48057'," + "'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.0745098039215" + "686 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','none'," + "'Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','" + "LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'," + "2),struct('MinYLimReal','-1.29644','MaxYLimReal','1.48563','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Lege" + "ndVisibility','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.62352941176470" + "6 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0" + ".274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePro" + "pertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[1 0 1],'LineStyle','-','LineWidth',0.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','non" + "e','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',{{}},'Num" + "Lines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-0.46864','MaxYLimReal','0.3892" + "5','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPha" + "se',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder'," + "[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3921568627450" + "98 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039" + "215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','" + "LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" + "olor',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','" + "none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" + "t',4),struct('MinYLimReal','-0.39228','MaxYLimReal','0.36328','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','L" + "egendVisibility','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.62352941176" + "4706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882352" + "9 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Line" + "PropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" + "olor',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','" + "none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct" + "('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'" + "NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal',''," + "'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.066" + "6666666666667;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.6" + "50980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth'" + ",0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" + "e','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0" + " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWid" + "th',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" + "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])" + NumInputPorts "5" + Floating off + } + Block { + BlockType Scope + Name "jointPos Meas" + SID "5127" + Ports [5] + Position [500, -245, 605, -135] + ZOrder 1030 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr." + "Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),extmg" + "r.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true,'Da" + "taLoggingVariableName','jointPos_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','Dat" + "aLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLim" + "Real','-0.02993','MaxYLimReal','0.02973','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','0.02993','LegendVisib" + "ility','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.274509" + "803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperties" + "Cache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0" + " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWid" + "th',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" + "ible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[" + "0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'," + "0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-39.14296','MaxYLimReal','54.3248','YL" + "abelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',fa" + "lse,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0" + ".0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.8" + "31372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" + " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vi" + "sible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," + "[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','Lin" + "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none'," + "'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2)," + "struct('MinYLimReal','-39.14341','MaxYLimReal','54.32468','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legen" + "dVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980" + "3922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.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','%','LineProp" + "ertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.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'),struct('Co" + "lor',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumL" + "ines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-0.1737','MaxYLimReal','0.18909'" + ",'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.074509803921" + "5686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('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,'Marker','no" + "ne','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" + ",4),struct('MinYLimReal','-0.06508','MaxYLimReal','0.1359','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Lege" + "ndVisibility','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.62352941176470" + "6 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0" + ".274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePro" + "pertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[1 0 1],'LineStyle','-','LineWidth',0.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','non" + "e','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',{{}},'Num" + "Lines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','Ax" + "esColor',[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.83137254" + "9019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6509" + "80392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0." + "5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," + "'on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0]" + ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','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',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])" + NumInputPorts "5" + Floating off + } + Block { + BlockType Scope + Name "jointVel Des" + SID "5139" + Ports [5] + Position [1295, 40, 1400, 150] + ZOrder 1044 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr." + "Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),extmg" + "r.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true,'Da" + "taLoggingVariableName','jointVel_Desired_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation'," + "'1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct(" + "'MinYLimReal','-5.6238','MaxYLimReal','5.63508','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','5.63508','Lege" + "ndVisibility','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.62352941176470" + "6 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0" + ".274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePro" + "pertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[1 0 1],'LineStyle','-','LineWidth',0.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','non" + "e','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',{{}},'Num" + "Lines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-2.47053','MaxYLimReal','3.0210" + "9','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPha" + "se',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder'," + "[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3921568627450" + "98 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039" + "215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','" + "LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" + "olor',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','" + "none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" + "t',2),struct('MinYLimReal','-2.44927','MaxYLimReal','3.00552','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','L" + "egendVisibility','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.62352941176" + "4706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882352" + "9 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Line" + "PropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" + "olor',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','" + "none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct" + "('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'" + "NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-6.15179','MaxYLimReal','6.6" + "9732','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMag" + "Phase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrde" + "r',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3921568627" + "45098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.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,'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,'Place" + "ment',4),struct('MinYLimReal','-6.66989','MaxYLimReal','6.1427','YLabelReal','','MinYLimMag','0','MaxYLimMag','10'," + "'LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" + "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411" + "764706 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','%','Li" + "nePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.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'),stru" + "ct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}}" + ",'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','" + "','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0" + "666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831" + "372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0" + ".650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidt" + "h',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','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','-','LineW" + "idth',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),'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])" + NumInputPorts "5" + Floating off + } + Block { + BlockType Scope + Name "jointVel Error" + SID "5140" + Ports [5] + Position [1295, -110, 1400, 0] + ZOrder 1043 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr." + "Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),extmg" + "r.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true,'Da" + "taLoggingVariableName','jointVel_Error_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1" + "','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('M" + "inYLimReal','-21.55725','MaxYLimReal','21.55924','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','21.55924','Le" + "gendVisibility','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.623529411764" + "706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529" + " 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineP" + "ropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" + "lor',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-'" + ",'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" + "'Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'N" + "umLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-11.08342','MaxYLimReal','11." + "35197','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],'ColorOrd" + "er',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862" + "745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509" + "8039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','" + "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker'," + "'none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" + "t('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,'Plac" + "ement',2),struct('MinYLimReal','-11.38599','MaxYLimReal','11.69948','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.62352" + "9411764706 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'),str" + "uct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')," + "struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames'," + "{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-23.42966','MaxYLimRea" + "l','28.78749','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true," + "'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'C" + "olorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.39" + "2156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0" + ".0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineS" + "tyle','-','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],'Li" + "neStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5" + ",'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',tru" + "e,'Placement',4),struct('MinYLimReal','-23.33166','MaxYLimReal','27.4078','YLabelReal','','MinYLimMag','0','MaxYLim" + "Mag','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.71" + "7647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'L" + "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0." + "5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," + "'on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelN" + "ames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLa" + "belReal','','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.39215686274" + "5098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07450980" + "39215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-'" + ",'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" + "'Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle'," + "'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker'" + ",'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])" + NumInputPorts "5" + Floating off + } + Block { + BlockType Scope + Name "jointVel Meas" + SID "5141" + Ports [5] + Position [1295, -245, 1400, -135] + ZOrder 1042 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr." + "Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),extmg" + "r.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true,'Da" + "taLoggingVariableName','jointVel_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','Dat" + "aLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLim" + "Real','-27.18034','MaxYLimReal','27.19424','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.19424','LegendVi" + "sibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980392" + "2 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.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','%','LinePropert" + "iesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[" + "1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','Line" + "Width',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" + "',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLine" + "s',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-13.2553','MaxYLimReal','14.27245'," + "'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.0745098039215" + "686 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','none'," + "'Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','" + "LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'," + "2),struct('MinYLimReal','-13.42222','MaxYLimReal','14.4567','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Leg" + "endVisibility','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.6235294117647" + "06 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 " + "0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePr" + "opertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" + "or',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','no" + "ne','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" + "Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'Nu" + "mLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-28.89711','MaxYLimReal','34.6" + "2355','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMag" + "Phase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrde" + "r',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3921568627" + "45098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.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,'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,'Place" + "ment',4),struct('MinYLimReal','-28.72109','MaxYLimReal','32.68361','YLabelReal','','MinYLimMag','0','MaxYLimMag','1" + "0','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68" + "6274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529" + "411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058" + "823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," + "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" + "ct('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,'Mark" + "er','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" + "truct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{" + "{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal" + "','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 " + "0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0." + "831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509803921568" + "6 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" + "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'" + ",[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',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])" + NumInputPorts "5" + Floating off + } + Line { + Name "left_leg" + ZOrder 1 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 4 + DstBlock "jointPos Des" + DstPort 4 + } + Line { + ZOrder 2 + SrcBlock "Gain2" + SrcPort 1 + DstBlock "Demux3" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 3 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 5 + DstBlock "jointPos Error" + DstPort 5 + } + Line { + Name "left_leg" + ZOrder 4 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 4 + DstBlock "jointPos Error" + DstPort 4 + } + Line { + Name "torso" + ZOrder 5 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 1 + DstBlock "jointPos Des" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 6 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 5 + DstBlock "jointPos Des" + DstPort 5 + } + Line { + Name "right_arm" + ZOrder 7 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 3 + DstBlock "jointPos Meas" + DstPort 3 + } + Line { + Name "jointPos Desired" + ZOrder 28 + SrcBlock "Demux" + SrcPort 2 + Points [90, 0] + Branch { + ZOrder 9 + Labels [1, 1] + DstBlock "Gain2" + DstPort 1 + } + Branch { + ZOrder 8 + DstBlock "Sum2" + DstPort 2 + } + } + Line { + ZOrder 11 + SrcBlock "Gain1" + SrcPort 1 + DstBlock "Demux2" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 12 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 5 + DstBlock "jointPos Meas" + DstPort 5 + } + Line { + Name "torso" + ZOrder 13 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 1 + DstBlock "jointPos Meas" + DstPort 1 + } + Line { + Name "torso" + ZOrder 14 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 1 + DstBlock "jointPos Error" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 15 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 2 + DstBlock "jointPos Error" + DstPort 2 + } + Line { + Name "right_arm" + ZOrder 16 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 3 + DstBlock "jointPos Error" + DstPort 3 + } + Line { + Name "right_arm" + ZOrder 17 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 3 + DstBlock "jointPos Des" + DstPort 3 + } + Line { + Name "left_arm" + ZOrder 18 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 2 + DstBlock "jointPos Des" + DstPort 2 + } + Line { + Name "jointPos Error" + ZOrder 19 + Labels [1, 1] + SrcBlock "Sum2" + SrcPort 1 + DstBlock "Gain1" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 20 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 2 + DstBlock "jointPos Meas" + DstPort 2 + } + Line { + ZOrder 21 + SrcBlock "Gain" + SrcPort 1 + DstBlock "Demux1" + DstPort 1 + } + Line { + Name "jointPos Measured" + ZOrder 26 + SrcBlock "From3" + SrcPort 1 + Points [155, 0] + Branch { + ZOrder 23 + DstBlock "Sum2" + DstPort 1 + } + Branch { + ZOrder 22 + Labels [1, 1] + DstBlock "Gain" + DstPort 1 + } + } + Line { + Name "left_leg" + ZOrder 25 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 4 + DstBlock "jointPos Meas" + DstPort 4 + } + Line { + ZOrder 27 + SrcBlock "From" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + ZOrder 29 + SrcBlock "Demux" + SrcPort 1 + DstBlock "Terminator" + DstPort 1 + } + Line { + ZOrder 30 + SrcBlock "Demux4" + SrcPort 1 + DstBlock "Terminator1" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 31 + Labels [-1, 0] + SrcBlock "Demux6" + SrcPort 2 + DstBlock "jointVel Error" + DstPort 2 + } + Line { + Name "right_arm" + ZOrder 32 + Labels [-1, 0] + SrcBlock "Demux6" + SrcPort 3 + DstBlock "jointVel Error" + DstPort 3 + } + Line { + Name "jointVel Measured" + ZOrder 57 + SrcBlock "From4" + SrcPort 1 + Points [150, 0] + Branch { + ZOrder 34 + DstBlock "Sum1" + DstPort 1 + } + Branch { + ZOrder 33 + Labels [1, 1] + DstBlock "Gain3" + DstPort 1 + } + } + Line { + Name "right_leg" + ZOrder 36 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 5 + DstBlock "jointVel Des" + DstPort 5 + } + Line { + Name "torso" + ZOrder 37 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 1 + DstBlock "jointVel Meas" + DstPort 1 + } + Line { + Name "left_leg" + ZOrder 38 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 4 + DstBlock "jointVel Des" + DstPort 4 + } + Line { + Name "left_leg" + ZOrder 39 + Labels [-1, 0] + SrcBlock "Demux6" + SrcPort 4 + DstBlock "jointVel Error" + DstPort 4 + } + Line { + Name "jointVel Error" + ZOrder 40 + Labels [1, 1] + SrcBlock "Sum1" + SrcPort 1 + DstBlock "Gain4" + DstPort 1 + } + Line { + Name "right_arm" + ZOrder 41 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 3 + DstBlock "jointVel Des" + DstPort 3 + } + Line { + Name "jointVel Desired" + ZOrder 44 + SrcBlock "Demux4" + SrcPort 2 + Points [90, 0] + Branch { + ZOrder 43 + Labels [1, 1] + DstBlock "Gain5" + DstPort 1 + } + Branch { + ZOrder 42 + DstBlock "Sum1" + DstPort 2 + } + } + Line { + Name "right_leg" + ZOrder 45 + Labels [-1, 0] + SrcBlock "Demux6" + SrcPort 5 + DstBlock "jointVel Error" + DstPort 5 + } + Line { + Name "left_arm" + ZOrder 46 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 2 + DstBlock "jointVel Meas" + DstPort 2 + } + Line { + ZOrder 47 + SrcBlock "Gain3" + SrcPort 1 + DstBlock "Demux5" + DstPort 1 + } + Line { + Name "right_arm" + ZOrder 48 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 3 + DstBlock "jointVel Meas" + DstPort 3 + } + Line { + ZOrder 58 + SrcBlock "From1" + SrcPort 1 + DstBlock "Demux4" + DstPort 1 + } + Line { + Name "torso" + ZOrder 50 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 1 + DstBlock "jointVel Des" + DstPort 1 + } + Line { + Name "torso" + ZOrder 51 + Labels [-1, 0] + SrcBlock "Demux6" + SrcPort 1 + DstBlock "jointVel Error" + DstPort 1 + } + Line { + ZOrder 52 + SrcBlock "Gain5" + SrcPort 1 + DstBlock "Demux7" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 53 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 5 + DstBlock "jointVel Meas" + DstPort 5 + } + Line { + ZOrder 54 + SrcBlock "Gain4" + SrcPort 1 + DstBlock "Demux6" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 55 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 2 + DstBlock "jointVel Des" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 56 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 4 + DstBlock "jointVel Meas" + DstPort 4 + } + Line { + Name "baseAcc Desired" + ZOrder 73 + Labels [1, 1] + SrcBlock "Demux10" + SrcPort 1 + DstBlock "Gain6" + DstPort 1 + } + Line { + ZOrder 66 + SrcBlock "Gain7" + SrcPort 1 + DstBlock "Demux9" + DstPort 1 + } + Line { + Name "left_leg" + ZOrder 67 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 4 + DstBlock "jointAcc Des" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 68 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 3 + DstBlock "jointAcc Des" + DstPort 3 + } + Line { + Name "left_arm" + ZOrder 69 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 2 + DstBlock "jointAcc Des" + DstPort 2 + } + Line { + Name "right_leg" + ZOrder 70 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 5 + DstBlock "jointAcc Des" + DstPort 5 + } + Line { + Name "torso" + ZOrder 71 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 1 + DstBlock "jointAcc Des" + DstPort 1 + } + Line { + Name "jointAcc Desired" + ZOrder 74 + Labels [1, 1] + SrcBlock "Demux10" + SrcPort 2 + DstBlock "Gain7" + DstPort 1 + } + Line { + ZOrder 75 + SrcBlock "From2" + SrcPort 1 + DstBlock "Demux10" + DstPort 1 + } + Line { + ZOrder 77 + SrcBlock "Gain6" + SrcPort 1 + DstBlock "baseAcc Des" + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "GetMeasurement" + SID "4538" + Ports [0, 1] + Position [855, -162, 925, -138] + ZOrder 964 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetMeasurement" + SourceType "GetMeasurement" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + measuredType "Joints Position" + } + Block { + BlockType Reference + Name "GetMeasurement1" + SID "4887" + Ports [0, 1] + Position [855, -102, 925, -78] + ZOrder 985 + BackgroundColor "[0.513700, 0.674500, 1.000000]" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetMeasurement" + SourceType "GetMeasurement" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + measuredType "Joints Velocity" + } + Block { + BlockType SubSystem + Name "POSITION CONTROLLER (IK)" + SID "5076" + Ports [2, 1] + Position [1145, -178, 1280, -62] + ZOrder 998 + RequestExecContextInheritance off + System { + Name "POSITION CONTROLLER (IK)" + 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 "418" + Block { + BlockType Inport + Name "jointPos" + SID "5077" + Position [-650, -152, -620, -138] + ZOrder 991 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointVel" + SID "5080" + Position [-650, -82, -620, -68] + ZOrder 999 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "Constant1" + SID "5089" + Position [-340, -33, -215, -17] + ZOrder 1006 + ShowName off + Value "Config.feetContactStatus" + } + Block { + BlockType Demux + Name "Demux" + SID "5102" + Ports [1, 2] + Position [110, -197, 115, -113] + ZOrder 1009 + ShowName off + Outputs "[16;ROBOT_DOF]" + DisplayOption "bar" + } + Block { + BlockType Goto + Name "Goto" + SID "5108" + Position [-485, -258, -435, -242] + ZOrder 1011 + GotoTag "jointPos" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto1" + SID "5109" + Position [-485, -228, -435, -212] + ZOrder 1012 + GotoTag "jointVel" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto2" + SID "5110" + Position [85, -3, 135, 13] + ZOrder 1013 + GotoTag "q_ref" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto3" + SID "5111" + Position [85, 27, 135, 43] + ZOrder 1014 + GotoTag "nu_ref" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto4" + SID "5112" + Position [85, 52, 135, 68] + ZOrder 1015 + GotoTag "nuDot_ref" + TagVisibility "global" + } + Block { + BlockType SubSystem + Name "Integration Based InverseKinematics" + SID "4907" + Ports [5, 3] + Position [-175, -189, 10, -1] + ZOrder 986 + RequestExecContextInheritance off + System { + Name "Integration Based InverseKinematics" + 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 "251" + Block { + BlockType Inport + Name "q_0" + SID "4908" + Position [1455, 393, 1485, 407] + ZOrder 499 + BlockMirror on + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "nu_0" + SID "4909" + Position [1455, 353, 1485, 367] + ZOrder 708 + BlockMirror on + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "desired_pos_vel_acc_joints" + SID "4911" + Position [980, -158, 1010, -142] + ZOrder 523 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "desired_pos_vel_acc_CoM" + SID "4910" + Position [980, -222, 1010, -208] + ZOrder 522 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "4912" + Position [980, 168, 1010, 182] + ZOrder 524 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "Compute Desired State Accelerations" + SID "4914" + Ports [7, 1] + Position [1125, -253, 1320, 213] + ZOrder 695 + RequestExecContextInheritance off + System { + Name "Compute Desired State Accelerations" + 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 "264" + Block { + BlockType Inport + Name "desired_pos_vel_acc_CoM" + SID "4915" + Position [740, 63, 770, 77] + ZOrder 697 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "desired_pos_vel_acc_joints" + SID "4916" + Position [740, 98, 770, 112] + ZOrder 698 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Jacobians" + SID "5095" + Position [280, 363, 310, 377] + ZOrder 709 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "forwardKinematics" + SID "5096" + Position [510, -77, 540, -63] + ZOrder 710 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "nu_ikin" + SID "4925" + Position [280, 233, 310, 247] + ZOrder 689 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "q_ikin" + SID "4926" + Position [510, 18, 540, 32] + ZOrder 691 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "5099" + Position [740, 483, 770, 497] + ZOrder 770 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType BusSelector + Name "Bus\nSelector" + SID "5104" + Ports [1, 5] + Position [755, -161, 760, 21] + ZOrder 771 + ShowName off + OutputSignals "pos_l_sole_error,pos_r_sole_error,rot_r_sole_error,rot_l_sole_error,posCoM" + OutputAsBus off + Port { + PortNumber 1 + Name "" + } + Port { + PortNumber 2 + Name "" + } + Port { + PortNumber 3 + Name "" + } + Port { + PortNumber 4 + Name "" + } + Port { + PortNumber 5 + Name "" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector1" + SID "5105" + Ports [1, 6] + Position [400, 261, 405, 474] + ZOrder 772 + ShowName off + OutputSignals "J_l_sole,J_r_sole,J_CoM,JDot_l_sole_nu,JDot_r_sole_nu,JDot_CoM_nu" + OutputAsBus off + Port { + PortNumber 1 + Name "" + } + Port { + PortNumber 2 + Name "" + } + Port { + PortNumber 3 + Name "" + } + Port { + PortNumber 4 + Name "" + } + Port { + PortNumber 5 + Name "" + } + Port { + PortNumber 6 + Name "" + } + } + Block { + BlockType Selector + Name "CoM6D -> \nCoMXYZ1" + SID "4927" + Ports [1, 1] + Position [740, 201, 770, 219] + ZOrder 671 + ShowName off + InputPortWidth "6" + IndexOptions "Index vector (dialog)" + Indices "[1 2 3]" + OutputSizes "1" + } + Block { + BlockType Demux + Name "Demux1" + SID "4928" + Ports [1, 2] + Position [400, 231, 405, 249] + ZOrder 680 + ShowName off + Outputs "[6,ROBOT_DOF]" + DisplayOption "bar" + } + Block { + BlockType Demux + Name "Demux2" + SID "4929" + Ports [1, 2] + Position [580, 6, 585, 44] + ZOrder 707 + ShowName off + Outputs "[16,ROBOT_DOF]" + DisplayOption "bar" + } + Block { + BlockType Product + Name "JcNu" + SID "4930" + Ports [2, 1] + Position [545, 199, 645, 221] + ZOrder 670 + ShowName off + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + } + Block { + BlockType Product + Name "JcNu1" + SID "4931" + Ports [2, 1] + Position [545, 130, 645, 150] + ZOrder 678 + ShowName off + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + } + Block { + BlockType Product + Name "JcNu2" + SID "4932" + Ports [2, 1] + Position [545, 165, 645, 185] + ZOrder 679 + ShowName off + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + } + Block { + BlockType Terminator + Name "Terminator" + SID "4933" + Position [445, 229, 455, 241] + ZOrder 681 + NamePlacement "alternate" + ShowName off + HideAutomaticName off + } + Block { + BlockType Terminator + Name "Terminator1" + SID "4934" + Position [635, 5, 655, 25] + ZOrder 708 + NamePlacement "alternate" + ShowName off + HideAutomaticName off + } + Block { + BlockType SubSystem + Name "trajectoryGeneratorCoM" + SID "4935" + Ports [19, 1] + Position [985, -142, 1270, 492] + ZOrder 669 + ShowName off + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "trajectoryGeneratorCoM" + 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 "1554" + Block { + BlockType Inport + Name "pos_l_sole_error" + SID "4935::780" + Position [20, 101, 40, 119] + ZOrder 87 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "pos_r_sole_error" + SID "4935::782" + Position [20, 136, 40, 154] + ZOrder 89 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "rot_r_sole_error" + SID "4935::1522" + Position [20, 171, 40, 189] + ZOrder 95 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "rot_l_sole_error" + SID "4935::1526" + Position [20, 206, 40, 224] + ZOrder 99 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "posCoM" + SID "4935::775" + Position [20, 246, 40, 264] + ZOrder 82 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "4935::777" + Position [20, 281, 40, 299] + ZOrder 84 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "desired_pos_vel_acc_CoM" + SID "4935::48" + Position [20, 316, 40, 334] + ZOrder 34 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "desired_pos_vel_acc_joints" + SID "4935::781" + Position [20, 351, 40, 369] + ZOrder 88 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "vel_l_sole" + SID "4935::1528" + Position [20, 386, 40, 404] + ZOrder 101 + Port "9" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "vel_r_sole" + SID "4935::783" + Position [20, 426, 40, 444] + ZOrder 90 + Port "10" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "velCoM" + SID "4935::776" + Position [20, 461, 40, 479] + ZOrder 83 + Port "11" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointVel" + SID "4935::778" + Position [20, 496, 40, 514] + ZOrder 85 + Port "12" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "J_l_sole" + SID "4935::1538" + Position [20, 531, 40, 549] + ZOrder 111 + Port "13" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "J_r_sole" + SID "4935::1539" + Position [20, 566, 40, 584] + ZOrder 112 + Port "14" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "J_CoM" + SID "4935::1545" + Position [20, 606, 40, 624] + ZOrder 118 + Port "15" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JDot_l_sole_nu" + SID "4935::1540" + Position [20, 641, 40, 659] + ZOrder 113 + Port "16" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JDot_r_sole_nu" + SID "4935::1541" + Position [20, 676, 40, 694] + ZOrder 114 + Port "17" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "JDot_CoM_nu" + SID "4935::1542" + Position [20, 711, 40, 729] + ZOrder 115 + Port "18" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "feetcontactStatus" + SID "4935::1543" + Position [20, 746, 40, 764] + ZOrder 116 + Port "19" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4935::1553" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 126 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4935::1552" + Tag "Stateflow S-Function positionControlBalancing 29" + Ports [19, 2] + Position [180, 75, 230, 475] + ZOrder 125 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[19 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "nuDot_ikin" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4935::1554" + Position [460, 241, 480, 259] + ZOrder 127 + } + Block { + BlockType Outport + Name "nuDot_ikin" + SID "4935::45" + Position [460, 101, 480, 119] + ZOrder 31 + IconDisplay "Port number" + } + Line { + ZOrder 322 + SrcBlock "pos_l_sole_error" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 323 + SrcBlock "pos_r_sole_error" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 324 + SrcBlock "rot_r_sole_error" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 325 + SrcBlock "rot_l_sole_error" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 326 + SrcBlock "posCoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 327 + SrcBlock "jointPos" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + ZOrder 328 + SrcBlock "desired_pos_vel_acc_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 7 + } + Line { + ZOrder 329 + SrcBlock "desired_pos_vel_acc_joints" + SrcPort 1 + DstBlock " SFunction " + DstPort 8 + } + Line { + ZOrder 330 + SrcBlock "vel_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 9 + } + Line { + ZOrder 331 + SrcBlock "vel_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 10 + } + Line { + ZOrder 332 + SrcBlock "velCoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 11 + } + Line { + ZOrder 333 + SrcBlock "jointVel" + SrcPort 1 + DstBlock " SFunction " + DstPort 12 + } + Line { + ZOrder 334 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 13 + } + Line { + ZOrder 335 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 14 + } + Line { + ZOrder 336 + SrcBlock "J_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 15 + } + Line { + ZOrder 337 + SrcBlock "JDot_l_sole_nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 16 + } + Line { + ZOrder 338 + SrcBlock "JDot_r_sole_nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 17 + } + Line { + ZOrder 339 + SrcBlock "JDot_CoM_nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 18 + } + Line { + ZOrder 340 + SrcBlock "feetcontactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 19 + } + Line { + Name "nuDot_ikin" + ZOrder 341 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "nuDot_ikin" + DstPort 1 + } + Line { + ZOrder 342 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 343 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "nuDot_ikin" + SID "5098" + Position [1345, 168, 1375, 182] + ZOrder 769 + IconDisplay "Port number" + } + Line { + ZOrder 3 + SrcBlock "Demux1" + SrcPort 1 + DstBlock "Terminator" + DstPort 1 + } + Line { + ZOrder 8 + SrcBlock "JcNu" + SrcPort 1 + DstBlock "CoM6D -> \nCoMXYZ1" + DstPort 1 + } + Line { + ZOrder 26 + SrcBlock "q_ikin" + SrcPort 1 + DstBlock "Demux2" + DstPort 1 + } + Line { + ZOrder 27 + SrcBlock "Demux2" + SrcPort 1 + DstBlock "Terminator1" + DstPort 1 + } + Line { + ZOrder 29 + SrcBlock "trajectoryGeneratorCoM" + SrcPort 1 + DstBlock "nuDot_ikin" + DstPort 1 + } + Line { + ZOrder 30 + SrcBlock "desired_pos_vel_acc_CoM" + SrcPort 1 + DstBlock "trajectoryGeneratorCoM" + DstPort 7 + } + Line { + ZOrder 31 + SrcBlock "desired_pos_vel_acc_joints" + SrcPort 1 + DstBlock "trajectoryGeneratorCoM" + DstPort 8 + } + Line { + ZOrder 32 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock "trajectoryGeneratorCoM" + DstPort 19 + } + Line { + ZOrder 33 + SrcBlock "nu_ikin" + SrcPort 1 + Points [40, 0] + Branch { + ZOrder 56 + Points [0, -25] + Branch { + ZOrder 65 + Points [0, -35] + Branch { + ZOrder 67 + Points [0, -35] + DstBlock "JcNu1" + DstPort 2 + } + Branch { + ZOrder 66 + DstBlock "JcNu2" + DstPort 2 + } + } + Branch { + ZOrder 64 + DstBlock "JcNu" + DstPort 2 + } + } + Branch { + ZOrder 53 + DstBlock "Demux1" + DstPort 1 + } + } + Line { + ZOrder 35 + SrcBlock "forwardKinematics" + SrcPort 1 + DstBlock "Bus\nSelector" + DstPort 1 + } + Line { + Name "" + ZOrder 36 + Labels [1, 1] + SrcBlock "Bus\nSelector" + SrcPort 1 + DstBlock "trajectoryGeneratorCoM" + DstPort 1 + } + Line { + Name "" + ZOrder 37 + Labels [1, 1] + SrcBlock "Bus\nSelector" + SrcPort 2 + DstBlock "trajectoryGeneratorCoM" + DstPort 2 + } + Line { + Name "" + ZOrder 38 + Labels [1, 1] + SrcBlock "Bus\nSelector" + SrcPort 3 + DstBlock "trajectoryGeneratorCoM" + DstPort 3 + } + Line { + Name "" + ZOrder 39 + Labels [1, 1] + SrcBlock "Bus\nSelector" + SrcPort 4 + DstBlock "trajectoryGeneratorCoM" + DstPort 4 + } + Line { + Name "" + ZOrder 40 + Labels [1, 1] + SrcBlock "Bus\nSelector" + SrcPort 5 + DstBlock "trajectoryGeneratorCoM" + DstPort 5 + } + Line { + ZOrder 41 + SrcBlock "Demux2" + SrcPort 2 + DstBlock "trajectoryGeneratorCoM" + DstPort 6 + } + Line { + ZOrder 42 + SrcBlock "Jacobians" + SrcPort 1 + DstBlock "Bus\nSelector1" + DstPort 1 + } + Line { + Name "" + ZOrder 43 + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 1 + Points [70, 0] + Branch { + ZOrder 63 + Points [0, -145] + DstBlock "JcNu1" + DstPort 1 + } + Branch { + ZOrder 62 + DstBlock "trajectoryGeneratorCoM" + DstPort 13 + } + } + Line { + Name "" + ZOrder 44 + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 2 + Points [87, 0] + Branch { + ZOrder 61 + Points [0, -145] + DstBlock "JcNu2" + DstPort 1 + } + Branch { + ZOrder 60 + DstBlock "trajectoryGeneratorCoM" + DstPort 14 + } + } + Line { + Name "" + ZOrder 45 + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 3 + Points [106, 0] + Branch { + ZOrder 59 + Points [0, -145] + DstBlock "JcNu" + DstPort 1 + } + Branch { + ZOrder 58 + DstBlock "trajectoryGeneratorCoM" + DstPort 15 + } + } + Line { + Name "" + ZOrder 46 + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 4 + DstBlock "trajectoryGeneratorCoM" + DstPort 16 + } + Line { + Name "" + ZOrder 47 + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 5 + DstBlock "trajectoryGeneratorCoM" + DstPort 17 + } + Line { + Name "" + ZOrder 48 + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 6 + DstBlock "trajectoryGeneratorCoM" + DstPort 18 + } + Line { + ZOrder 49 + SrcBlock "JcNu1" + SrcPort 1 + DstBlock "trajectoryGeneratorCoM" + DstPort 9 + } + Line { + ZOrder 50 + SrcBlock "JcNu2" + SrcPort 1 + DstBlock "trajectoryGeneratorCoM" + DstPort 10 + } + Line { + ZOrder 51 + SrcBlock "CoM6D -> \nCoMXYZ1" + SrcPort 1 + DstBlock "trajectoryGeneratorCoM" + DstPort 11 + } + Line { + ZOrder 52 + SrcBlock "Demux1" + SrcPort 2 + DstBlock "trajectoryGeneratorCoM" + DstPort 12 + } + } + } + Block { + BlockType SubSystem + Name "Compute Jacobians and ForwardKinematics" + SID "4939" + Ports [3, 2] + Position [660, -120, 810, 10] + ZOrder 706 + RequestExecContextInheritance off + System { + Name "Compute Jacobians and ForwardKinematics" + 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 "455" + Block { + BlockType Inport + Name "q_0" + SID "4942" + Position [-365, -22, -335, -8] + ZOrder 707 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "q_ikin" + SID "4940" + Position [-365, 78, -335, 92] + ZOrder 706 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "nu_ikin" + SID "4941" + Position [-365, 268, -335, 282] + ZOrder 712 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType BusCreator + Name "Bus\nCreator" + SID "5093" + Ports [5, 1] + Position [50, -67, 55, 137] + ZOrder 733 + ShowName off + Inputs "5" + DisplayOption "bar" + InheritFromInputs on + } + Block { + BlockType BusCreator + Name "Bus\nCreator1" + SID "5094" + Ports [6, 1] + Position [50, 167, 55, 308] + ZOrder 734 + ShowName off + Inputs "6" + DisplayOption "bar" + InheritFromInputs on + } + Block { + BlockType SubSystem + Name "Compute ForwardKinematics" + SID "4943" + Ports [2, 5] + Position [-205, -63, -70, 133] + ZOrder 657 + RequestExecContextInheritance off + Port { + PortNumber 1 + Name "posCoM" + PropagatedSignals "posCoM" + } + Port { + PortNumber 2 + Name "pos_l_sole_error" + PropagatedSignals "pos_l_sole_error" + } + Port { + PortNumber 3 + Name "rot_r_sole_error" + PropagatedSignals "rot_r_sole_error" + } + Port { + PortNumber 4 + Name "pos_r_sole_error" + PropagatedSignals "pos_r_sole_error" + } + Port { + PortNumber 5 + Name "rot_l_sole_error" + PropagatedSignals "rot_l_sole_error" + } + System { + Name "Compute ForwardKinematics" + 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 "307" + Block { + BlockType Inport + Name "q_0" + SID "4944" + Position [230, 523, 260, 537] + ZOrder 660 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "q_ikin" + SID "4945" + Position [230, 303, 260, 317] + ZOrder 668 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "Comput Position and Orientation Errors" + SID "4946" + Ports [5, 5] + Position [900, 138, 1135, 702] + ZOrder 729 + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Comput Position and Orientation Errors" + 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 "48" + Block { + BlockType Inport + Name "w_H_l_sole" + SID "4946::23" + Position [20, 101, 40, 119] + ZOrder 9 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_r_sole" + SID "4946::25" + Position [20, 136, 40, 154] + ZOrder 11 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_CoM" + SID "4946::1" + Position [20, 171, 40, 189] + ZOrder -1 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_r_sole_0" + SID "4946::26" + Position [20, 206, 40, 224] + ZOrder 12 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_l_sole_0" + SID "4946::24" + Position [20, 246, 40, 264] + ZOrder 10 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4946::47" + Ports [1, 1] + Position [270, 315, 320, 355] + ZOrder 33 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4946::46" + Tag "Stateflow S-Function positionControlBalancing 1" + Ports [5, 6] + Position [180, 102, 230, 243] + ZOrder 32 + FunctionName "sf_sfun" + PortCounts "[5 6]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "pos_l_sole_error" + } + Port { + PortNumber 3 + Name "rot_l_sole_error" + } + Port { + PortNumber 4 + Name "pos_r_sole_error" + } + Port { + PortNumber 5 + Name "rot_r_sole_error" + } + Port { + PortNumber 6 + Name "posCoM" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4946::48" + Position [460, 326, 480, 344] + ZOrder 34 + } + Block { + BlockType Outport + Name "pos_l_sole_error" + SID "4946::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "rot_l_sole_error" + SID "4946::27" + Position [460, 136, 480, 154] + ZOrder 13 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "pos_r_sole_error" + SID "4946::28" + Position [460, 171, 480, 189] + ZOrder 14 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "rot_r_sole_error" + SID "4946::29" + Position [460, 206, 480, 224] + ZOrder 15 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "posCoM" + SID "4946::30" + Position [460, 246, 480, 264] + ZOrder 16 + Port "5" + IconDisplay "Port number" + } + Line { + ZOrder 37 + SrcBlock "w_H_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 38 + SrcBlock "w_H_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 39 + SrcBlock "w_H_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 40 + SrcBlock "w_H_r_sole_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 41 + SrcBlock "w_H_l_sole_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + Name "pos_l_sole_error" + ZOrder 42 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "pos_l_sole_error" + DstPort 1 + } + Line { + Name "rot_l_sole_error" + ZOrder 43 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "rot_l_sole_error" + DstPort 1 + } + Line { + Name "pos_r_sole_error" + ZOrder 44 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "pos_r_sole_error" + DstPort 1 + } + Line { + Name "rot_r_sole_error" + ZOrder 45 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "rot_r_sole_error" + DstPort 1 + } + Line { + Name "posCoM" + ZOrder 46 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "posCoM" + DstPort 1 + } + Line { + ZOrder 47 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 48 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Demux + Name "Demux1" + SID "4947" + Ports [1, 2] + Position [295, 281, 300, 339] + ZOrder 669 + ShowName off + Outputs "[16;ROBOT_DOF]" + DisplayOption "bar" + Port { + PortNumber 1 + Name "w_H_b (vectorized)" + } + } + Block { + BlockType Demux + Name "Demux2" + SID "4948" + Ports [1, 2] + Position [295, 501, 300, 559] + ZOrder 661 + ShowName off + Outputs "[16;ROBOT_DOF]" + DisplayOption "bar" + Port { + PortNumber 1 + Name "w_H_b (vectorized)" + } + } + Block { + BlockType Reference + Name "ForwardKinematics_CoM" + SID "4949" + Ports [2, 1] + Position [660, 388, 810, 452] + ZOrder 653 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.COM" + } + Block { + BlockType Reference + Name "ForwardKinematics_l_sole" + SID "4950" + Ports [2, 1] + Position [660, 168, 810, 232] + ZOrder 654 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "ForwardKinematics_l_sole_0" + SID "4951" + Ports [2, 1] + Position [660, 608, 810, 672] + ZOrder 679 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "ForwardKinematics_r_sole" + SID "4952" + Ports [2, 1] + Position [660, 278, 810, 342] + ZOrder 657 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.RIGHT_FOOT" + } + Block { + BlockType Reference + Name "ForwardKinematics_r_sole_0" + SID "4953" + Ports [2, 1] + Position [660, 498, 810, 562] + ZOrder 659 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.RIGHT_FOOT" + } + Block { + BlockType Reshape + Name "Reshape1" + SID "4954" + Ports [1, 1] + Position [410, 503, 435, 527] + ZOrder 666 + NamePlacement "alternate" + ShowName off + HideAutomaticName off + OutputDimensionality "Customize" + OutputDimensions "[4,4]" + Port { + PortNumber 1 + Name "w_H_b (matrix)" + } + } + Block { + BlockType Reshape + Name "Reshape2" + SID "4955" + Ports [1, 1] + Position [410, 282, 435, 308] + ZOrder 670 + NamePlacement "alternate" + ShowName off + HideAutomaticName off + OutputDimensionality "Customize" + OutputDimensions "[4,4]" + Port { + PortNumber 1 + Name "w_H_b (matrix)" + } + } + Block { + BlockType Outport + Name "posCoM" + SID "4956" + Position [1235, 633, 1265, 647] + ZOrder 676 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "pos_l_sole_error" + SID "4957" + Position [1235, 193, 1265, 207] + ZOrder 681 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "rot_r_sole_error" + SID "4958" + Position [1235, 523, 1265, 537] + ZOrder 732 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "pos_r_sole_error" + SID "4959" + Position [1235, 413, 1265, 427] + ZOrder 733 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "rot_l_sole_error" + SID "4960" + Position [1235, 303, 1265, 317] + ZOrder 734 + Port "5" + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "q_0" + SrcPort 1 + DstBlock "Demux2" + DstPort 1 + } + Line { + Name "w_H_b (vectorized)" + ZOrder 2 + Labels [1, 1] + SrcBlock "Demux2" + SrcPort 1 + DstBlock "Reshape1" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "q_ikin" + SrcPort 1 + DstBlock "Demux1" + DstPort 1 + } + Line { + Name "w_H_b (vectorized)" + ZOrder 4 + Labels [1, 1] + SrcBlock "Demux1" + SrcPort 1 + DstBlock "Reshape2" + DstPort 1 + } + Line { + Name "w_H_b (matrix)" + ZOrder 5 + Labels [1, 1] + SrcBlock "Reshape2" + SrcPort 1 + Points [134, 0] + Branch { + ZOrder 6 + Points [0, -110] + DstBlock "ForwardKinematics_l_sole" + DstPort 1 + } + Branch { + ZOrder 7 + Points [0, 110] + DstBlock "ForwardKinematics_CoM" + DstPort 1 + } + Branch { + ZOrder 8 + DstBlock "ForwardKinematics_r_sole" + DstPort 1 + } + } + Line { + Name "w_H_b (matrix)" + ZOrder 9 + Labels [1, 1] + SrcBlock "Reshape1" + SrcPort 1 + Points [139, 0] + Branch { + ZOrder 10 + DstBlock "ForwardKinematics_r_sole_0" + DstPort 1 + } + Branch { + ZOrder 11 + Points [0, 110] + DstBlock "ForwardKinematics_l_sole_0" + DstPort 1 + } + } + Line { + ZOrder 12 + SrcBlock "Demux2" + SrcPort 2 + Points [248, 0] + Branch { + ZOrder 13 + DstBlock "ForwardKinematics_r_sole_0" + DstPort 2 + } + Branch { + ZOrder 14 + Points [0, 110] + DstBlock "ForwardKinematics_l_sole_0" + DstPort 2 + } + } + Line { + ZOrder 15 + SrcBlock "Demux1" + SrcPort 2 + Points [309, 0] + Branch { + ZOrder 16 + Points [0, -110] + DstBlock "ForwardKinematics_l_sole" + DstPort 2 + } + Branch { + ZOrder 17 + Points [0, 110] + DstBlock "ForwardKinematics_CoM" + DstPort 2 + } + Branch { + ZOrder 18 + DstBlock "ForwardKinematics_r_sole" + DstPort 2 + } + } + Line { + ZOrder 19 + SrcBlock "ForwardKinematics_l_sole" + SrcPort 1 + DstBlock "Comput Position and Orientation Errors" + DstPort 1 + } + Line { + ZOrder 20 + SrcBlock "ForwardKinematics_r_sole" + SrcPort 1 + DstBlock "Comput Position and Orientation Errors" + DstPort 2 + } + Line { + ZOrder 21 + SrcBlock "ForwardKinematics_CoM" + SrcPort 1 + DstBlock "Comput Position and Orientation Errors" + DstPort 3 + } + Line { + ZOrder 22 + SrcBlock "ForwardKinematics_r_sole_0" + SrcPort 1 + DstBlock "Comput Position and Orientation Errors" + DstPort 4 + } + Line { + ZOrder 23 + SrcBlock "ForwardKinematics_l_sole_0" + SrcPort 1 + DstBlock "Comput Position and Orientation Errors" + DstPort 5 + } + Line { + ZOrder 24 + SrcBlock "Comput Position and Orientation Errors" + SrcPort 5 + DstBlock "posCoM" + DstPort 1 + } + Line { + ZOrder 25 + SrcBlock "Comput Position and Orientation Errors" + SrcPort 1 + DstBlock "pos_l_sole_error" + DstPort 1 + } + Line { + ZOrder 26 + SrcBlock "Comput Position and Orientation Errors" + SrcPort 2 + DstBlock "rot_l_sole_error" + DstPort 1 + } + Line { + ZOrder 27 + SrcBlock "Comput Position and Orientation Errors" + SrcPort 3 + DstBlock "pos_r_sole_error" + DstPort 1 + } + Line { + ZOrder 28 + SrcBlock "Comput Position and Orientation Errors" + SrcPort 4 + DstBlock "rot_r_sole_error" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Compute Jacobians and Jdot_nu" + SID "4961" + Ports [2, 6] + Position [-210, 167, -70, 308] + ZOrder 650 + RequestExecContextInheritance off + Port { + PortNumber 1 + Name "J_l_sole" + } + Port { + PortNumber 2 + Name "J_r_sole" + } + Port { + PortNumber 3 + Name "J_CoM" + } + Port { + PortNumber 4 + Name "JDot_l_sole_nu" + } + Port { + PortNumber 5 + Name "JDot_r_sole_nu" + } + Port { + PortNumber 6 + Name "JDot_CoM_nu" + } + System { + Name "Compute Jacobians and Jdot_nu" + 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 "352" + Block { + BlockType Inport + Name "q_ikin" + SID "4962" + Position [110, -87, 140, -73] + ZOrder 649 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "nu_ikin" + SID "4972" + Position [110, 278, 140, 292] + ZOrder 720 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name "Demux" + SID "4963" + Ports [1, 2] + Position [190, -107, 195, -58] + ZOrder 650 + ShowName off + Outputs "[16;ROBOT_DOF]" + DisplayOption "bar" + Port { + PortNumber 1 + Name "w_H_b (vectorized)" + } + } + Block { + BlockType Demux + Name "Demux2" + SID "4975" + Ports [1, 2] + Position [190, 265, 195, 300] + ZOrder 717 + ShowName off + Outputs "[6;ROBOT_DOF]" + DisplayOption "bar" + } + Block { + BlockType Reference + Name "JDot_CoM nu" + SID "4977" + Ports [4, 1] + Position [525, 234, 690, 301] + ZOrder 713 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Jacobians/DotJNu" + SourceType "DotJNu" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.COM" + } + Block { + BlockType Reference + Name "JDot_l_sole nu" + SID "4978" + Ports [4, 1] + Position [525, 52, 690, 123] + ZOrder 714 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Jacobians/DotJNu" + SourceType "DotJNu" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "JDot_r_sole nu" + SID "4979" + Ports [4, 1] + Position [525, 145, 690, 210] + ZOrder 715 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Jacobians/DotJNu" + SourceType "DotJNu" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.RIGHT_FOOT" + } + Block { + BlockType Reference + Name "Jacobian CoM" + SID "4964" + Ports [2, 1] + Position [525, -35, 685, 20] + ZOrder 648 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" + SourceType "Jacobian" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.COM" + } + Block { + BlockType Reference + Name "Jacobian l_sole" + SID "4965" + Ports [2, 1] + Position [525, -185, 685, -130] + ZOrder 644 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" + SourceType "Jacobian" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "Jacobian r_sole" + SID "4966" + Ports [2, 1] + Position [525, -110, 685, -55] + ZOrder 647 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" + SourceType "Jacobian" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.RIGHT_FOOT" + } + Block { + BlockType Reshape + Name "Reshape" + SID "4967" + Ports [1, 1] + Position [310, -107, 335, -83] + ZOrder 651 + NamePlacement "alternate" + ShowName off + HideAutomaticName off + OutputDimensionality "Customize" + OutputDimensions "[4,4]" + Port { + PortNumber 1 + Name "w_H_b (matrix)" + } + } + Block { + BlockType Outport + Name "J_l_sole" + SID "4968" + Position [775, -162, 805, -148] + ZOrder 652 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "J_r_sole" + SID "4969" + Position [775, -87, 805, -73] + ZOrder 653 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "J_CoM" + SID "4970" + Position [775, -12, 805, 2] + ZOrder 654 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "JDot_l_sole_nu" + SID "4980" + Position [775, 83, 805, 97] + ZOrder 721 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "JDot_r_sole_nu" + SID "4981" + Position [775, 173, 805, 187] + ZOrder 722 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "JDot_CoM_nu" + SID "4982" + Position [775, 263, 805, 277] + ZOrder 723 + Port "6" + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "q_ikin" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + Name "w_H_b (vectorized)" + ZOrder 2 + Labels [1, 1] + SrcBlock "Demux" + SrcPort 1 + DstBlock "Reshape" + DstPort 1 + } + Line { + Name "w_H_b (matrix)" + ZOrder 3 + Labels [1, 1] + SrcBlock "Reshape" + SrcPort 1 + Points [92, 0] + Branch { + ZOrder 4 + DstBlock "Jacobian r_sole" + DstPort 1 + } + Branch { + ZOrder 5 + Points [0, -75] + DstBlock "Jacobian l_sole" + DstPort 1 + } + Branch { + ZOrder 6 + Points [0, 75] + Branch { + ZOrder 62 + Points [0, 85] + Branch { + ZOrder 70 + DstBlock "JDot_l_sole nu" + DstPort 1 + } + Branch { + ZOrder 69 + Points [0, 90] + Branch { + ZOrder 73 + DstBlock "JDot_r_sole nu" + DstPort 1 + } + Branch { + ZOrder 72 + Points [0, 90] + DstBlock "JDot_CoM nu" + DstPort 1 + } + } + } + Branch { + ZOrder 61 + DstBlock "Jacobian CoM" + DstPort 1 + } + } + } + Line { + ZOrder 7 + SrcBlock "Demux" + SrcPort 2 + Points [260, 0] + Branch { + ZOrder 8 + DstBlock "Jacobian r_sole" + DstPort 2 + } + Branch { + ZOrder 9 + Points [0, 75] + Branch { + ZOrder 75 + Points [0, 75] + Branch { + ZOrder 78 + Points [0, 90] + Branch { + ZOrder 81 + Points [0, 90] + DstBlock "JDot_CoM nu" + DstPort 2 + } + Branch { + ZOrder 80 + DstBlock "JDot_r_sole nu" + DstPort 2 + } + } + Branch { + ZOrder 77 + DstBlock "JDot_l_sole nu" + DstPort 2 + } + } + Branch { + ZOrder 74 + DstBlock "Jacobian CoM" + DstPort 2 + } + } + Branch { + ZOrder 10 + Points [0, -75] + DstBlock "Jacobian l_sole" + DstPort 2 + } + } + Line { + ZOrder 11 + SrcBlock "Jacobian l_sole" + SrcPort 1 + DstBlock "J_l_sole" + DstPort 1 + } + Line { + ZOrder 12 + SrcBlock "Jacobian r_sole" + SrcPort 1 + DstBlock "J_r_sole" + DstPort 1 + } + Line { + ZOrder 13 + SrcBlock "Jacobian CoM" + SrcPort 1 + DstBlock "J_CoM" + DstPort 1 + } + Line { + ZOrder 18 + SrcBlock "Demux2" + SrcPort 1 + Points [170, 0] + Branch { + ZOrder 65 + DstBlock "JDot_CoM nu" + DstPort 3 + } + Branch { + ZOrder 55 + Points [0, -90] + Branch { + ZOrder 64 + DstBlock "JDot_r_sole nu" + DstPort 3 + } + Branch { + ZOrder 38 + Points [0, -90] + DstBlock "JDot_l_sole nu" + DstPort 3 + } + } + } + Line { + ZOrder 23 + SrcBlock "Demux2" + SrcPort 2 + Points [190, 0] + Branch { + ZOrder 66 + DstBlock "JDot_CoM nu" + DstPort 4 + } + Branch { + ZOrder 56 + Points [0, -90] + Branch { + ZOrder 67 + DstBlock "JDot_r_sole nu" + DstPort 4 + } + Branch { + ZOrder 20 + Points [0, -90] + DstBlock "JDot_l_sole nu" + DstPort 4 + } + } + } + Line { + ZOrder 28 + SrcBlock "JDot_l_sole nu" + SrcPort 1 + DstBlock "JDot_l_sole_nu" + DstPort 1 + } + Line { + ZOrder 34 + SrcBlock "JDot_r_sole nu" + SrcPort 1 + DstBlock "JDot_r_sole_nu" + DstPort 1 + } + Line { + ZOrder 35 + SrcBlock "nu_ikin" + SrcPort 1 + DstBlock "Demux2" + DstPort 1 + } + Line { + ZOrder 37 + SrcBlock "JDot_CoM nu" + SrcPort 1 + DstBlock "JDot_CoM_nu" + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "Jacobians" + SID "4985" + Position [135, 233, 165, 247] + ZOrder 715 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "forwardKinematics" + SID "4989" + Position [135, 28, 165, 42] + ZOrder 732 + Port "2" + IconDisplay "Port number" + } + Line { + ZOrder 19 + SrcBlock "q_0" + SrcPort 1 + DstBlock "Compute ForwardKinematics" + DstPort 1 + } + Line { + ZOrder 20 + SrcBlock "q_ikin" + SrcPort 1 + Points [36, 0] + Branch { + ZOrder 29 + Points [0, 120] + DstBlock "Compute Jacobians and Jdot_nu" + DstPort 1 + } + Branch { + ZOrder 28 + DstBlock "Compute ForwardKinematics" + DstPort 2 + } + } + Line { + Name "posCoM" + ZOrder 21 + Labels [-1, 0] + SrcBlock "Compute ForwardKinematics" + SrcPort 1 + DstBlock "Bus\nCreator" + DstPort 1 + } + Line { + Name "pos_l_sole_error" + ZOrder 22 + Labels [-1, 0] + SrcBlock "Compute ForwardKinematics" + SrcPort 2 + DstBlock "Bus\nCreator" + DstPort 2 + } + Line { + Name "rot_r_sole_error" + ZOrder 23 + Labels [-1, 0] + SrcBlock "Compute ForwardKinematics" + SrcPort 3 + DstBlock "Bus\nCreator" + DstPort 3 + } + Line { + Name "pos_r_sole_error" + ZOrder 24 + Labels [-1, 0] + SrcBlock "Compute ForwardKinematics" + SrcPort 4 + DstBlock "Bus\nCreator" + DstPort 4 + } + Line { + Name "rot_l_sole_error" + ZOrder 25 + Labels [-1, 0] + SrcBlock "Compute ForwardKinematics" + SrcPort 5 + DstBlock "Bus\nCreator" + DstPort 5 + } + Line { + ZOrder 26 + SrcBlock "Bus\nCreator" + SrcPort 1 + DstBlock "forwardKinematics" + DstPort 1 + } + Line { + ZOrder 27 + SrcBlock "nu_ikin" + SrcPort 1 + DstBlock "Compute Jacobians and Jdot_nu" + DstPort 2 + } + Line { + Name "J_l_sole" + ZOrder 30 + Labels [-1, 0] + SrcBlock "Compute Jacobians and Jdot_nu" + SrcPort 1 + DstBlock "Bus\nCreator1" + DstPort 1 + } + Line { + Name "J_r_sole" + ZOrder 31 + Labels [-1, 0] + SrcBlock "Compute Jacobians and Jdot_nu" + SrcPort 2 + DstBlock "Bus\nCreator1" + DstPort 2 + } + Line { + Name "J_CoM" + ZOrder 32 + Labels [-1, 0] + SrcBlock "Compute Jacobians and Jdot_nu" + SrcPort 3 + DstBlock "Bus\nCreator1" + DstPort 3 + } + Line { + Name "JDot_l_sole_nu" + ZOrder 33 + Labels [-1, 0] + SrcBlock "Compute Jacobians and Jdot_nu" + SrcPort 4 + DstBlock "Bus\nCreator1" + DstPort 4 + } + Line { + Name "JDot_r_sole_nu" + ZOrder 34 + Labels [-1, 0] + SrcBlock "Compute Jacobians and Jdot_nu" + SrcPort 5 + DstBlock "Bus\nCreator1" + DstPort 5 + } + Line { + Name "JDot_CoM_nu" + ZOrder 35 + Labels [-1, 0] + SrcBlock "Compute Jacobians and Jdot_nu" + SrcPort 6 + DstBlock "Bus\nCreator1" + DstPort 6 + } + Line { + ZOrder 36 + SrcBlock "Bus\nCreator1" + SrcPort 1 + DstBlock "Jacobians" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "InverseKinematics Integration" + SID "4994" + Ports [3, 2] + Position [1135, 300, 1305, 420] + ZOrder 651 + BlockMirror on + RequestExecContextInheritance off + System { + Name "InverseKinematics Integration" + 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 "266" + Block { + BlockType Inport + Name "nuDot_ikin" + SID "4995" + Position [85, -7, 115, 7] + ZOrder 498 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "nu_0" + SID "4996" + Position [85, 43, 115, 57] + ZOrder 500 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "q_0" + SID "4997" + Position [505, 103, 535, 117] + ZOrder 501 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "Compute the Base Pose Derivative" + SID "4999" + 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 "45" + Block { + BlockType Inport + Name "nu_base_ikin" + SID "4999::23" + Position [20, 101, 40, 119] + ZOrder 9 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "pose_base" + SID "4999::33" + Position [20, 136, 40, 154] + ZOrder 19 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4999::44" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 30 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4999::43" + Tag "Stateflow S-Function positionControlBalancing 24" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 29 + 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 "4999::45" + Position [460, 241, 480, 259] + ZOrder 31 + } + Block { + BlockType Outport + Name "pose_base_dot" + SID "4999::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 16 + SrcBlock "nu_base_ikin" + 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 "5000" + Ports [1, 2] + Position [1090, 111, 1095, 149] + ZOrder 507 + ShowName off + Outputs "[7;ROBOT_DOF]" + DisplayOption "bar" + } + Block { + BlockType Demux + Name "Demux1" + SID "5001" + Ports [1, 2] + Position [445, -44, 450, 94] + ZOrder 509 + ShowName off + Outputs "[6;ROBOT_DOF]" + DisplayOption "bar" + } + Block { + BlockType Mux + Name "Mux" + SID "5002" + 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 "5003" + 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 "43" + Block { + BlockType Inport + Name "q_ikin_quat" + SID "5003::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "5003::42" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 28 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "5003::41" + Tag "Stateflow S-Function positionControlBalancing 31" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 27 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "q_ikin" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "5003::43" + Position [460, 241, 480, 259] + ZOrder 29 + } + Block { + BlockType Outport + Name "q_ikin" + SID "5003::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 13 + SrcBlock "q_ikin_quat" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "q_ikin" + ZOrder 14 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "q_ikin" + 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 "5004" + 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 "43" + Block { + BlockType Inport + Name "q_0" + SID "5004::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "5004::42" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 28 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "5004::41" + Tag "Stateflow S-Function positionControlBalancing 30" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 27 + 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 "5004::43" + Position [460, 241, 480, 259] + ZOrder 29 + } + Block { + BlockType Outport + Name "q_0_quat" + SID "5004::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + 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 "4998" + 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 "5005" + Position [1130, 132, 1145, 148] + ZOrder 508 + ShowName off + HideAutomaticName off + } + Block { + BlockType DiscreteIntegrator + Name "Velocity Integration" + SID "5006" + Ports [2, 1] + Position [935, -2, 985, 147] + ZOrder 497 + InitialConditionSource "external" + SampleTime "Config.tStep" + ICPrevOutput "DiscIntNeverNeededParam" + ICPrevScaledInput "DiscIntNeverNeededParam" + } + Block { + BlockType Outport + Name "nu_ikin" + SID "5007" + Position [350, -18, 385, -2] + ZOrder 505 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "q_ikin" + SID "5008" + Position [1410, 67, 1445, 83] + ZOrder 502 + Port "2" + IconDisplay "Port number" + } + 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_ikin" + 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 28 + Points [0, 55] + DstBlock "Demux" + DstPort 1 + } + Branch { + ZOrder 26 + DstBlock "Rewirte the Base Pose as Transf Matrix" + DstPort 1 + } + } + Line { + ZOrder 8 + SrcBlock "State Acceleration Integration" + SrcPort 1 + Points [39, 0] + Branch { + ZOrder 29 + DstBlock "Demux1" + DstPort 1 + } + Branch { + ZOrder 19 + Points [0, -35] + DstBlock "nu_ikin" + 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_ikin" + 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 Outport + Name "q_ref" + SID "5017" + Position [425, 383, 455, 397] + ZOrder 710 + BlockMirror on + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "nu_ref" + SID "5018" + Position [530, 323, 560, 337] + ZOrder 767 + BlockMirror on + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "nuDot_ref" + SID "5019" + Position [1455, -27, 1485, -13] + ZOrder 768 + Port "3" + IconDisplay "Port number" + } + Line { + ZOrder 23 + SrcBlock "desired_pos_vel_acc_CoM" + SrcPort 1 + DstBlock "Compute Desired State Accelerations" + DstPort 1 + } + Line { + ZOrder 24 + SrcBlock "desired_pos_vel_acc_joints" + SrcPort 1 + DstBlock "Compute Desired State Accelerations" + DstPort 2 + } + Line { + ZOrder 47 + SrcBlock "q_0" + SrcPort 1 + Points [-64, 0] + Branch { + ZOrder 72 + Points [0, 57; -991, 0; 0, -557] + DstBlock "Compute Jacobians and ForwardKinematics" + DstPort 1 + } + Branch { + ZOrder 71 + DstBlock "InverseKinematics Integration" + DstPort 3 + } + } + Line { + ZOrder 48 + SrcBlock "InverseKinematics Integration" + SrcPort 2 + Points [-229, 0] + Branch { + ZOrder 66 + Points [-399, 0] + Branch { + ZOrder 81 + DstBlock "q_ref" + DstPort 1 + } + Branch { + ZOrder 78 + Points [0, -445] + DstBlock "Compute Jacobians and ForwardKinematics" + DstPort 2 + } + } + Branch { + ZOrder 58 + Points [0, -280] + DstBlock "Compute Desired State Accelerations" + DstPort 6 + } + } + Line { + ZOrder 50 + SrcBlock "Compute Jacobians and ForwardKinematics" + SrcPort 1 + DstBlock "Compute Desired State Accelerations" + DstPort 3 + } + Line { + ZOrder 51 + SrcBlock "Compute Jacobians and ForwardKinematics" + SrcPort 2 + DstBlock "Compute Desired State Accelerations" + DstPort 4 + } + Line { + ZOrder 62 + SrcBlock "Compute Desired State Accelerations" + SrcPort 1 + Points [60, 0] + Branch { + ZOrder 69 + Points [0, 340] + DstBlock "InverseKinematics Integration" + DstPort 1 + } + Branch { + ZOrder 68 + DstBlock "nuDot_ref" + DstPort 1 + } + } + Line { + ZOrder 65 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock "Compute Desired State Accelerations" + DstPort 7 + } + Line { + ZOrder 67 + SrcBlock "nu_0" + SrcPort 1 + DstBlock "InverseKinematics Integration" + DstPort 2 + } + Line { + ZOrder 49 + SrcBlock "InverseKinematics Integration" + SrcPort 1 + Points [-282, 0] + Branch { + ZOrder 61 + Points [-235, 0] + Branch { + ZOrder 80 + DstBlock "nu_ref" + DstPort 1 + } + Branch { + ZOrder 77 + Points [0, -340] + DstBlock "Compute Jacobians and ForwardKinematics" + DstPort 3 + } + } + Branch { + ZOrder 56 + Points [0, -285] + DstBlock "Compute Desired State Accelerations" + DstPort 5 + } + } + } + } + Block { + BlockType SubSystem + Name "References and Initial Conditions" + SID "5055" + Ports [2, 4] + Position [-545, -182, -370, -43] + ZOrder 984 + RequestExecContextInheritance off + System { + Name "References and Initial Conditions" + 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 "272" + Block { + BlockType Inport + Name "jointPos" + SID "5057" + Position [-375, -22, -345, -8] + ZOrder 985 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointVel" + SID "5059" + Position [-375, -227, -345, -213] + ZOrder 987 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "CoM References Generator" + SID "4866" + Ports [1, 1] + Position [280, 201, 435, 259] + ZOrder 982 + RequestExecContextInheritance off + System { + Name "CoM References Generator" + 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 "589" + Block { + BlockType Inport + Name "pos_CoM_0" + SID "4867" + Position [80, 123, 110, 137] + ZOrder 890 + IconDisplay "Port number" + } + Block { + BlockType Clock + Name "Clock1" + SID "4868" + Position [185, 40, 205, 60] + ZOrder 888 + ShowName off + } + Block { + BlockType Constant + Name "Constant" + SID "4869" + Position [275, 82, 440, 98] + ZOrder 543 + ShowName off + Value "Config.COM_SINE_MOVEMENTS" + } + Block { + BlockType Constant + Name "Constant2" + SID "4870" + Position [65, 172, 130, 188] + ZOrder 548 + ShowName off + Value "zeros(6,1)" + } + Block { + BlockType Mux + Name "Mux" + SID "4871" + Ports [2, 1] + Position [200, 106, 205, 204] + ZOrder 547 + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType SubSystem + Name "Reference Generator CoM" + SID "4872" + Ports [2, 1] + Position [245, -24, 475, 74] + ZOrder 889 + NamePlacement "alternate" + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Reference Generator CoM" + Location [53, 45, 896, 1715] + 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 "3740" + Block { + BlockType Inport + Name "posCoM_0" + SID "4872::778" + Position [20, 101, 40, 119] + ZOrder 88 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "t" + SID "4872::768" + Position [20, 136, 40, 154] + ZOrder 78 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4872::3739" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 142 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4872::3738" + Tag "Stateflow S-Function positionControlBalancing 10" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 141 + FunctionName "sf_sfun" + Parameters "Config" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "references_CoM" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4872::3740" + Position [460, 241, 480, 259] + ZOrder 143 + } + Block { + BlockType Outport + Name "references_CoM" + SID "4872::45" + Position [460, 101, 480, 119] + ZOrder 31 + IconDisplay "Port number" + } + Line { + ZOrder 16 + SrcBlock "posCoM_0" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 17 + SrcBlock "t" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "references_CoM" + ZOrder 18 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "references_CoM" + 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 Switch + Name "Switch1" + SID "4873" + Position [535, -11, 585, 191] + ZOrder 546 + ShowName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "references_CoM" + SID "4874" + Position [630, 83, 660, 97] + ZOrder 891 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "Clock1" + SrcPort 1 + DstBlock "Reference Generator CoM" + DstPort 2 + } + Line { + ZOrder 2 + SrcBlock "Constant2" + SrcPort 1 + DstBlock "Mux" + DstPort 2 + } + Line { + ZOrder 3 + SrcBlock "pos_CoM_0" + SrcPort 1 + Points [24, 0] + Branch { + ZOrder 11 + Points [0, -130] + DstBlock "Reference Generator CoM" + DstPort 1 + } + Branch { + ZOrder 5 + DstBlock "Mux" + DstPort 1 + } + } + Line { + ZOrder 6 + SrcBlock "Constant" + SrcPort 1 + DstBlock "Switch1" + DstPort 2 + } + Line { + ZOrder 7 + SrcBlock "Mux" + SrcPort 1 + DstBlock "Switch1" + DstPort 3 + } + Line { + ZOrder 8 + SrcBlock "Reference Generator CoM" + SrcPort 1 + DstBlock "Switch1" + DstPort 1 + } + Line { + ZOrder 9 + SrcBlock "Switch1" + SrcPort 1 + DstBlock "references_CoM" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Compute State Velocity" + SID "4875" + Ports [4, 1] + Position [50, -235, 175, -115] + ZOrder 979 + 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 "460" + Block { + BlockType Inport + Name "jointVel" + SID "5067" + Position [-10, -97, 20, -83] + ZOrder 870 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "feetContactStatus\n" + SID "4878" + Position [-10, -132, 20, -118] + ZOrder 244 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_b" + SID "4877" + Position [-10, -167, 20, -153] + ZOrder 561 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "4876" + Position [-10, -202, 20, -188] + ZOrder 562 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "Compute Base Velocity" + SID "4879" + Ports [5, 1] + Position [285, -208, 635, -42] + 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 "3765" + Block { + BlockType Inport + Name "J_l_sole" + SID "4879::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "J_r_sole" + SID "4879::1472" + Position [20, 136, 40, 154] + ZOrder 42 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "feetContactStatus" + SID "4879::1473" + Position [20, 171, 40, 189] + ZOrder 43 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointVel" + SID "4879::23" + Position [20, 206, 40, 224] + ZOrder 9 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "pinvDampTolerance" + SID "4879::3756" + Position [20, 246, 40, 264] + ZOrder 125 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4879::3764" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 133 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4879::3763" + Tag "Stateflow S-Function positionControlBalancing 5" + Ports [5, 2] + Position [180, 100, 230, 220] + ZOrder 132 + FunctionName "sf_sfun" + PortCounts "[5 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "nu_b" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4879::3765" + Position [460, 241, 480, 259] + ZOrder 134 + } + Block { + BlockType Outport + Name "nu_b" + SID "4879::5" + Position [460, 101, 480, 119] + ZOrder -6 + IconDisplay "Port number" + } + Line { + ZOrder 32 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 33 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 34 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 35 + SrcBlock "jointVel" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 36 + SrcBlock "pinvDampTolerance" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + Name "nu_b" + ZOrder 37 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "nu_b" + DstPort 1 + } + Line { + ZOrder 38 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 39 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Constant + Name "Constant" + SID "5068" + Position [90, -65, 240, -45] + ZOrder 871 + Value "Config.Reg.pinvDampBaseVel" + } + Block { + BlockType SubSystem + Name "Feet Jacobians" + SID "4880" + Ports [2, 2] + Position [105, -211, 225, -144] + 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 "4881" + Position [585, 598, 615, 612] + ZOrder 673 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_b" + SID "4882" + Position [585, 483, 615, 497] + ZOrder 680 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Reference + Name "Jacobian l_sole" + SID "4883" + Ports [2, 1] + Position [745, 475, 905, 530] + ZOrder 687 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" + SourceType "Jacobian" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "Jacobian r_sole" + SID "4884" + Ports [2, 1] + Position [745, 565, 905, 620] + ZOrder 688 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Jacobians/Jacobian" + SourceType "Jacobian" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.RIGHT_FOOT" + } + Block { + BlockType Outport + Name "J_l_sole" + SID "4885" + Position [935, 498, 965, 512] + ZOrder 676 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "J_r_sole" + SID "4886" + Position [935, 588, 965, 602] + ZOrder 677 + Port "2" + IconDisplay "Port number" + } + 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 "4888" + Ports [2, 1] + Position [680, -187, 685, 62] + ZOrder -12 + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Outport + Name "nu" + SID "4889" + Position [725, -67, 755, -53] + ZOrder -15 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "Mux" + SrcPort 1 + DstBlock "nu" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "feetContactStatus\n" + SrcPort 1 + DstBlock "Compute Base Velocity" + DstPort 3 + } + Line { + ZOrder 3 + SrcBlock "Compute Base Velocity" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + ZOrder 4 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "Feet Jacobians" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "w_H_b" + SrcPort 1 + DstBlock "Feet Jacobians" + DstPort 2 + } + Line { + ZOrder 6 + SrcBlock "Feet Jacobians" + SrcPort 1 + DstBlock "Compute Base Velocity" + DstPort 1 + } + Line { + ZOrder 7 + SrcBlock "Feet Jacobians" + SrcPort 2 + DstBlock "Compute Base Velocity" + DstPort 2 + } + Line { + ZOrder 13 + SrcBlock "jointVel" + SrcPort 1 + Points [36, 0] + Branch { + ZOrder 17 + DstBlock "Compute Base Velocity" + DstPort 4 + } + Branch { + ZOrder 16 + Points [0, 90] + DstBlock "Mux" + DstPort 2 + } + } + Line { + ZOrder 18 + SrcBlock "Constant" + SrcPort 1 + DstBlock "Compute Base Velocity" + DstPort 5 + } + } + } + Block { + BlockType Constant + Name "Constant" + SID "5070" + Position [205, 385, 335, 405] + ZOrder 998 + Value "zeros(2*ROBOT_DOF,1)" + } + Block { + BlockType Constant + Name "Constant1" + SID "4890" + Position [-245, -198, -120, -182] + ZOrder 967 + ShowName off + Value "Config.feetContactStatus" + } + Block { + BlockType SubSystem + Name "Get CoM Position" + SID "5038" + Ports [2, 1] + Position [50, 184, 140, 271] + ZOrder 966 + HideAutomaticName off + RequestExecContextInheritance off + System { + Name "Get CoM Position" + 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 "500" + Block { + BlockType Inport + Name "jointPos" + SID "5040" + Position [30, 267, 60, 283] + ZOrder 582 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w_H_b" + SID "5039" + Position [30, 237, 60, 253] + ZOrder 581 + NamePlacement "alternate" + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Reference + Name "CoM Position" + SID "5041" + Ports [2, 1] + Position [110, 230, 230, 290] + ZOrder 578 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.COM" + } + Block { + BlockType Selector + Name "CoM6D -> \nCoMXYZ2" + SID "5042" + Ports [1, 1] + Position [265, 248, 295, 272] + 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 "posCoM" + SID "5043" + Position [340, 253, 370, 267] + ZOrder 583 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "CoM Position" + DstPort 2 + } + Line { + ZOrder 2 + SrcBlock "CoM6D -> \nCoMXYZ2" + SrcPort 1 + DstBlock "posCoM" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "CoM Position" + SrcPort 1 + DstBlock "CoM6D -> \nCoMXYZ2" + DstPort 1 + } + Line { + ZOrder 4 + SrcBlock "w_H_b" + SrcPort 1 + DstBlock "CoM Position" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "5020" + Ports [2, 1] + Position [35, -67, 195, 137] + ZOrder 995 + 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 "31" + Block { + BlockType Inport + Name "w_H_b" + SID "5020::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "5020::22" + Position [20, 136, 40, 154] + ZOrder 13 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "5020::30" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 21 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "5020::29" + Tag "Stateflow S-Function positionControlBalancing 25" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 20 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "q" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "5020::31" + Position [460, 241, 480, 259] + ZOrder 22 + } + Block { + BlockType Outport + Name "q" + SID "5020::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + 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 Mux + Name "Mux" + SID "5071" + Ports [2, 1] + Position [420, 321, 425, 419] + ZOrder 999 + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Reshape + Name "Reshape" + SID "5031" + Ports [1, 1] + Position [505, 219, 530, 241] + ZOrder 983 + ShowName off + OutputDimensionality "Customize" + OutputDimensions "[3,3]" + } + Block { + BlockType Reshape + Name "Reshape1" + SID "5072" + Ports [1, 1] + Position [505, 359, 530, 381] + ZOrder 1000 + ShowName off + OutputDimensionality "Customize" + OutputDimensions "[ROBOT_DOF,3]" + } + Block { + BlockType SubSystem + Name "Transformation Matrix From FixedLink to Base" + SID "4833" + Ports [1, 1] + Position [-250, -52, -120, 22] + ZOrder 978 + 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 "4834" + Position [820, -107, 850, -93] + ZOrder 890 + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "Constant1" + SID "5090" + Position [985, -193, 1110, -177] + ZOrder 968 + ShowName off + Value "Config.feetContactStatus" + } + Block { + BlockType Constant + Name "Constant7" + SID "4837" + Position [820, -285, 850, -255] + ZOrder 885 + ShowName off + Value "eye(4)" + } + Block { + BlockType Product + Name "Inverse " + SID "4846" + 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 "4862" + Ports [2, 1] + Position [965, -278, 1130, -242] + ZOrder 725 + NamePlacement "alternate" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "RelativeTransform Between r_sole and Base" + SID "4863" + Ports [2, 1] + Position [960, -128, 1130, -92] + ZOrder 896 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.RIGHT_FOOT" + } + Block { + BlockType Selector + Name "Selector" + SID "5092" + 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 "4864" + Position [1230, -296, 1305, -74] + ZOrder 895 + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "fixedLink_H_b" + SID "4861" + Position [1450, -192, 1480, -178] + ZOrder 904 + IconDisplay "Port number" + } + Line { + ZOrder 3 + SrcBlock "jointPos" + SrcPort 1 + Points [42, 0] + Branch { + ZOrder 44 + Points [0, -150] + DstBlock "RelativeTransform Between l_sole and Base" + DstPort 2 + } + Branch { + ZOrder 5 + DstBlock "RelativeTransform Between r_sole and Base" + DstPort 2 + } + } + Line { + ZOrder 7 + SrcBlock "Constant7" + SrcPort 1 + Points [24, 0] + Branch { + ZOrder 45 + Points [0, 150] + DstBlock "RelativeTransform Between r_sole and Base" + DstPort 1 + } + Branch { + ZOrder 9 + DstBlock "RelativeTransform Between l_sole and Base" + DstPort 1 + } + } + Line { + ZOrder 13 + SrcBlock "RelativeTransform Between l_sole and Base" + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + Line { + ZOrder 14 + SrcBlock "RelativeTransform Between r_sole and Base" + SrcPort 1 + DstBlock "Switch" + DstPort 3 + } + Line { + ZOrder 48 + SrcBlock "Constant1" + SrcPort 1 + DstBlock "Selector" + DstPort 1 + } + Line { + ZOrder 27 + SrcBlock "Switch" + SrcPort 1 + DstBlock "Inverse " + DstPort 1 + } + Line { + ZOrder 24 + SrcBlock "Inverse " + SrcPort 1 + DstBlock "fixedLink_H_b" + DstPort 1 + } + Line { + ZOrder 47 + SrcBlock "Selector" + SrcPort 1 + DstBlock "Switch" + DstPort 2 + } + } + } + Block { + BlockType Reference + Name "holder" + SID "5027" + Ports [1, 1] + Position [250, 19, 290, 51] + ZOrder 994 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled on + } + Block { + BlockType Reference + Name "holder\n" + SID "5035" + Ports [1, 1] + Position [255, 331, 290, 359] + ZOrder 972 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "holder\n1" + SID "5036" + Ports [1, 1] + Position [175, 216, 210, 244] + ZOrder 973 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "holder1" + SID "5069" + Ports [1, 1] + Position [250, -191, 290, -159] + ZOrder 997 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled on + } + Block { + BlockType Outport + Name "q_0" + SID "5066" + Position [340, 28, 370, 42] + ZOrder 996 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "nu_0" + SID "5063" + Position [340, -182, 370, -168] + ZOrder 991 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "desired_pos_vel_acc_joints" + SID "5064" + Position [610, 363, 640, 377] + ZOrder 992 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "desired_pos_vel_acc_CoM" + SID "5061" + Position [610, 223, 640, 237] + ZOrder 989 + Port "4" + IconDisplay "Port number" + } + Line { + ZOrder 306 + SrcBlock "CoM References Generator" + SrcPort 1 + DstBlock "Reshape" + DstPort 1 + } + Line { + ZOrder 320 + SrcBlock "Compute State Velocity" + SrcPort 1 + DstBlock "holder1" + DstPort 1 + } + Line { + ZOrder 291 + SrcBlock "Get CoM Position" + SrcPort 1 + DstBlock "holder\n1" + DstPort 1 + } + Line { + ZOrder 290 + SrcBlock "Constant1" + SrcPort 1 + DstBlock "Compute State Velocity" + DstPort 2 + } + Line { + ZOrder 318 + SrcBlock "Reshape" + SrcPort 1 + DstBlock "desired_pos_vel_acc_CoM" + DstPort 1 + } + Line { + ZOrder 292 + SrcBlock "holder\n1" + SrcPort 1 + DstBlock "CoM References Generator" + DstPort 1 + } + Line { + ZOrder 333 + SrcBlock "Transformation Matrix From FixedLink to Base" + SrcPort 1 + Points [53, 0] + Branch { + ZOrder 379 + Points [0, 265] + DstBlock "Get CoM Position" + DstPort 2 + } + Branch { + ZOrder 351 + Points [0, -145] + DstBlock "Compute State Velocity" + DstPort 3 + } + Branch { + ZOrder 350 + DstBlock "MATLAB Function" + DstPort 1 + } + } + Line { + ZOrder 331 + SrcBlock "MATLAB Function" + SrcPort 1 + DstBlock "holder" + DstPort 1 + } + Line { + ZOrder 328 + SrcBlock "jointPos" + SrcPort 1 + Points [34, 0] + Branch { + ZOrder 336 + DstBlock "Transformation Matrix From FixedLink to Base" + DstPort 1 + } + Branch { + ZOrder 335 + Points [0, 100] + Branch { + ZOrder 361 + Points [0, 260] + DstBlock "holder\n" + DstPort 1 + } + Branch { + ZOrder 360 + Points [274, 0] + Branch { + ZOrder 378 + Points [0, 120] + DstBlock "Get CoM Position" + DstPort 1 + } + Branch { + ZOrder 353 + Points [0, -215] + DstBlock "Compute State Velocity" + DstPort 4 + } + Branch { + ZOrder 352 + DstBlock "MATLAB Function" + DstPort 2 + } + } + } + } + Line { + ZOrder 337 + SrcBlock "holder" + SrcPort 1 + DstBlock "q_0" + DstPort 1 + } + Line { + ZOrder 356 + SrcBlock "jointVel" + SrcPort 1 + DstBlock "Compute State Velocity" + DstPort 1 + } + Line { + ZOrder 357 + SrcBlock "holder1" + SrcPort 1 + DstBlock "nu_0" + DstPort 1 + } + Line { + ZOrder 362 + SrcBlock "Constant" + SrcPort 1 + DstBlock "Mux" + DstPort 2 + } + Line { + ZOrder 363 + SrcBlock "holder\n" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + ZOrder 364 + SrcBlock "Mux" + SrcPort 1 + DstBlock "Reshape1" + DstPort 1 + } + Line { + ZOrder 365 + SrcBlock "Reshape1" + SrcPort 1 + DstBlock "desired_pos_vel_acc_joints" + DstPort 1 + } + } + } + Block { + BlockType Terminator + Name "Terminator" + SID "5100" + Position [100, -105, 120, -85] + ZOrder 1007 + } + Block { + BlockType Terminator + Name "Terminator1" + SID "5101" + Position [100, -45, 120, -25] + ZOrder 1008 + } + Block { + BlockType Terminator + Name "Terminator2" + SID "5103" + Position [180, -183, 195, -167] + ZOrder 1010 + } + Block { + BlockType Outport + Name "jointPos_ref" + SID "5086" + Position [175, -142, 205, -128] + ZOrder 1005 + IconDisplay "Port number" + } + Line { + ZOrder 334 + SrcBlock "jointPos" + SrcPort 1 + Points [13, 0] + Branch { + ZOrder 350 + Points [0, -105] + DstBlock "Goto" + DstPort 1 + } + Branch { + ZOrder 349 + DstBlock "References and Initial Conditions" + DstPort 1 + } + } + Line { + ZOrder 337 + SrcBlock "jointVel" + SrcPort 1 + Points [32, 0] + Branch { + ZOrder 352 + Points [0, -145] + DstBlock "Goto1" + DstPort 1 + } + Branch { + ZOrder 351 + DstBlock "References and Initial Conditions" + DstPort 2 + } + } + Line { + ZOrder 338 + SrcBlock "References and Initial Conditions" + SrcPort 1 + DstBlock "Integration Based InverseKinematics" + DstPort 1 + } + Line { + ZOrder 339 + SrcBlock "References and Initial Conditions" + SrcPort 2 + DstBlock "Integration Based InverseKinematics" + DstPort 2 + } + Line { + ZOrder 340 + SrcBlock "References and Initial Conditions" + SrcPort 3 + DstBlock "Integration Based InverseKinematics" + DstPort 3 + } + Line { + ZOrder 341 + SrcBlock "References and Initial Conditions" + SrcPort 4 + DstBlock "Integration Based InverseKinematics" + DstPort 4 + } + Line { + ZOrder 342 + SrcBlock "Constant1" + SrcPort 1 + DstBlock "Integration Based InverseKinematics" + DstPort 5 + } + Line { + ZOrder 343 + SrcBlock "Integration Based InverseKinematics" + SrcPort 2 + Points [35, 0] + Branch { + ZOrder 356 + Points [0, 130] + DstBlock "Goto3" + DstPort 1 + } + Branch { + ZOrder 355 + DstBlock "Terminator" + DstPort 1 + } + } + Line { + ZOrder 344 + SrcBlock "Integration Based InverseKinematics" + SrcPort 3 + Points [23, 0] + Branch { + ZOrder 358 + Points [0, 95] + DstBlock "Goto4" + DstPort 1 + } + Branch { + ZOrder 357 + DstBlock "Terminator1" + DstPort 1 + } + } + Line { + ZOrder 345 + SrcBlock "Integration Based InverseKinematics" + SrcPort 1 + Points [46, 0] + Branch { + ZOrder 354 + Points [0, 160] + DstBlock "Goto2" + DstPort 1 + } + Branch { + ZOrder 353 + DstBlock "Demux" + DstPort 1 + } + } + Line { + ZOrder 347 + SrcBlock "Demux" + SrcPort 1 + DstBlock "Terminator2" + DstPort 1 + } + Line { + ZOrder 348 + SrcBlock "Demux" + SrcPort 2 + DstBlock "jointPos_ref" + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "SetReferences" + SID "5075" + Ports [1] + Position [1360, -135, 1435, -105] + ZOrder 997 + BackgroundColor "[0.392200, 0.788200, 0.345100]" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Actuators/SetReferences" + SourceType "SetReferences" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled on + controlType "Position Direct" + refSpeed "10*(pi/180)" + refAcceleration "1000000*(pi/180)" + } + Block { + BlockType SubSystem + Name "emergency stop: joint limits" + SID "2416" + Ports [1] + Position [990, -197, 1095, -173] + ZOrder 960 + BackgroundColor "red" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 23 + $ClassName "Simulink.Mask" + Display "disp('EMERGENCY STOP')" + RunInitForIconRedraw "off" + } + System { + Name "emergency stop: joint limits" + 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 "904" + Block { + BlockType Inport + Name "jointPos" + SID "2419" + Position [150, 238, 180, 252] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n1" + SID "4660" + 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 "4663" + 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 "4661" + 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] + 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 "899" + Block { + BlockType Inport + Name "jointPos" + SID "4662" + Position [60, 103, 90, 117] + ZOrder 552 + IconDisplay "Port number" + } + Block { + BlockType EnablePort + Name "Enable" + SID "3207" + Ports [] + Position [227, -35, 247, -15] + ZOrder 553 + ShowName off + HideAutomaticName off + } + Block { + BlockType Assertion + Name "Assertion" + SID "2420" + Position [340, 72, 400, 118] + ZOrder 207 + HideAutomaticName off + } + Block { + BlockType Reference + Name "GetLimits" + SID "2432" + Ports [0, 2] + Position [15, 23, 135, 92] + ZOrder 551 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetLimits" + SourceType "GetLimits" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + limitsSource "ControlBoard" + limitsType "Position" + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "2421" + Ports [4, 1] + 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 "3782" + Block { + BlockType Inport + Name "umin" + SID "2421::18" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "umax" + SID "2421::19" + Position [20, 136, 40, 154] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "u" + SID "2421::1" + Position [20, 171, 40, 189] + ZOrder -3 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tol" + SID "2421::20" + Position [20, 206, 40, 224] + ZOrder -4 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "2421::3781" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 131 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "2421::3780" + Tag "Stateflow S-Function positionControlBalancing 18" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 130 + FunctionName "sf_sfun" + PortCounts "[4 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "inRange" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "2421::3782" + Position [460, 241, 480, 259] + ZOrder 132 + } + Block { + BlockType Outport + Name "inRange" + SID "2421::5" + Position [460, 101, 480, 119] + ZOrder -8 + IconDisplay "Port number" + } + Line { + ZOrder 22 + SrcBlock "umin" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 23 + SrcBlock "umax" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 24 + SrcBlock "u" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 25 + SrcBlock "tol" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "inRange" + ZOrder 26 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "inRange" + DstPort 1 + } + Line { + ZOrder 27 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 28 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Constant + Name "index1" + SID "2422" + Position [65, 140, 90, 150] + 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 "Assertion" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "GetLimits" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 4 + 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" + 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] + 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 "827" + Block { + BlockType Inport + Name "jointPos" + SID "4665" + Position [15, 53, 45, 67] + ZOrder 552 + IconDisplay "Port number" + } + Block { + BlockType EnablePort + Name "Enable" + SID "4666" + Ports [] + Position [217, -20, 237, 0] + ZOrder 553 + ShowName off + HideAutomaticName off + } + Block { + BlockType Assertion + Name "Assertion" + SID "4667" + Position [340, 72, 400, 118] + ZOrder 207 + HideAutomaticName off + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "4669" + Ports [2, 1] + Position [165, 24, 285, 166] + 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 "3782" + Block { + BlockType Inport + Name "u" + SID "4669::1" + Position [20, 101, 40, 119] + ZOrder -3 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "delta_u_max" + SID "4669::18" + Position [20, 136, 40, 154] + ZOrder -1 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4669::3781" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 131 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4669::3780" + Tag "Stateflow S-Function positionControlBalancing 14" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 130 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "noSpikes" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4669::3782" + Position [460, 241, 480, 259] + ZOrder 132 + } + Block { + BlockType Outport + Name "noSpikes" + SID "4669::5" + Position [460, 101, 480, 119] + ZOrder -8 + IconDisplay "Port number" + } + Line { + ZOrder 16 + SrcBlock "u" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 17 + SrcBlock "delta_u_max" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "noSpikes" + ZOrder 18 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "noSpikes" + 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 Constant + Name "index1" + SID "4671" + Position [-40, 124, 100, 136] + 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 "Assertion" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "index1" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 2 + } + } + } + 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 SubSystem + Name "synchronizer" + SID "2423" + Ports [] + Position [1152, -242, 1272, -212] + ZOrder 961 + ForegroundColor "red" + BackgroundColor "green" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 24 + $ClassName "Simulink.Mask" + Display "disp('SYNCHRONIZER')" + } + System { + Name "synchronizer" + 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 "947" + Block { + BlockType SubSystem + Name "GAZEBO_SYNCHRONIZER" + SID "2424" + Ports [0, 0, 1] + Position [5, 15, 130, 75] + ZOrder -7 + RequestExecContextInheritance off + System { + Name "GAZEBO_SYNCHRONIZER" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + 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 EnablePort + Name "Enable" + SID "2425" + Ports [] + Position [177, 85, 197, 105] + ZOrder 538 + } + Block { + BlockType Reference + Name "Simulator Synchronizer" + SID "2426" + Ports [] + Position [120, 24, 250, 61] + ZOrder 539 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" + SourceType "Simulator Synchronizer" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + period "Config.tStep" + serverPortName "'/clock/rpc'" + clientPortName "'/WBT_synchronizer/rpc:o'" + } + } + } + Block { + BlockType Logic + Name "Logical\nOperator" + SID "2427" + Ports [1, 1] + Position [-30, -34, -5, -16] + ZOrder 307 + BlockMirror on + NamePlacement "alternate" + ShowName off + Operator "NOT" + AllPortsSameDT off + OutDataTypeStr "boolean" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n" + SID "2428" + Position [95, -35, 210, -15] + ZOrder 304 + BlockMirror on + NamePlacement "alternate" + ShowName off + Value "Config.ON_GAZEBO" + } + Block { + BlockType SubSystem + Name "REAL_TIME_SYNC" + SID "2429" + Ports [0, 0, 1] + Position [-160, 17, -35, 77] + ZOrder -9 + RequestExecContextInheritance off + System { + Name "REAL_TIME_SYNC" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + 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 EnablePort + Name "Enable" + SID "2430" + Ports [] + Position [167, 90, 187, 110] + ZOrder 538 + } + Block { + BlockType Reference + Name "Real Time Synchronizer" + SID "2431" + Ports [] + Position [115, 34, 240, 71] + ZOrder 539 + ForegroundColor "[0.916667, 0.916667, 0.916667]" + BackgroundColor "gray" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" + SourceType "Real Time Synchronizer" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + period "Config.tStep" + } + } + } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "4628" + Ports [1] + Position [125, -95, 185, -75] + ZOrder 699 + ShowName off + HideAutomaticName off + VariableName "yarp_time" + MaxDataPoints "inf" + SaveFormat "Structure With Time" + Save2DSignal "3-D array (concatenate along third dimension)" + FixptAsFi on + SampleTime "-1" + } + Block { + BlockType Reference + Name "Yarp Clock" + SID "4629" + Ports [0, 1] + Position [-50, -94, 15, -76] + ZOrder 698 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Yarp Clock" + SourceType "YARP Clock" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + } + Line { + ZOrder 1 + SrcBlock "Logical\nOperator" + SrcPort 1 + Points [-60, 0] + DstBlock "REAL_TIME_SYNC" + DstPort enable + } + Line { + ZOrder 2 + SrcBlock "ON_GAZEBO\n" + SrcPort 1 + Points [-25, 0] + Branch { + ZOrder 3 + DstBlock "GAZEBO_SYNCHRONIZER" + DstPort enable + } + Branch { + ZOrder 4 + DstBlock "Logical\nOperator" + DstPort 1 + } + } + Line { + ZOrder 5 + SrcBlock "Yarp Clock" + SrcPort 1 + DstBlock "To Workspace" + DstPort 1 + } + } + } + Line { + ZOrder 340 + SrcBlock "POSITION CONTROLLER (IK)" + SrcPort 1 + DstBlock "SetReferences" + DstPort 1 + } + Line { + ZOrder 341 + SrcBlock "GetMeasurement" + SrcPort 1 + Points [30, 0] + Branch { + ZOrder 346 + DstBlock "POSITION CONTROLLER (IK)" + DstPort 1 + } + Branch { + ZOrder 343 + Points [0, -35] + DstBlock "emergency stop: joint limits" + DstPort 1 + } + } + Line { + ZOrder 344 + SrcBlock "GetMeasurement1" + SrcPort 1 + DstBlock "POSITION CONTROLLER (IK)" + DstPort 2 + } + } +} +#Finite State Machines +# +# Stateflow 80000014 +# +# +Stateflow { + machine { + id 1 + name "positionControlBalancing" + created "18-Feb-2014 10:14:40" + isLibrary 0 + sfVersion 80000012 + firstTarget 111 + } + chart { + id 2 + machine 1 + name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Jacobians and ForwardKinematics/Co" + "mpute ForwardKinematics/Comput Position and Orientation Errors" + windowPosition [357.12 483.496 167 391] + viewLimits [0 156.75 0 153.75] + screen [1 1 1280 1024 1.041666666666667] + treeNode [0 3 0 0] + viewObj 2 + ssIdHighWaterMark 13 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 1 + disableImplicitCasting 1 + eml { + name "computePositionAndOrientationErrorsFCN" + } + firstData 4 + firstTransition 15 + firstJunction 14 + } + 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 [pos_l_sole_error, rot_l_sole_error, pos_r_sole_error, rot_r_sole_error, posCoM] = ...\n" + " computePositionAndOrientationErrorsFCN(w_H_l_sole, w_H_r_sole, w_H_CoM, w_H_r_sole_0, w_H_l_sole_0)" + "\n \n [pos_l_sole_error, rot_l_sole_error, pos_r_sole_error, rot_r_sole_error, posCoM] = ...\n " + " computePositionAndOrientationErrors(w_H_l_sole, w_H_r_sole, w_H_CoM, w_H_r_sole_0, w_H_l_sole_0);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 4 + ssIdNumber 6 + 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 [2 0 5] + } + data { + id 5 + ssIdNumber 8 + 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 [2 4 6] + } + data { + id 6 + ssIdNumber 4 + name "w_H_CoM" + 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 5 7] + } + data { + id 7 + ssIdNumber 9 + name "w_H_r_sole_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 [2 6 8] + } + data { + id 8 + ssIdNumber 5 + name "pos_l_sole_error" + 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 7 9] + } + data { + id 9 + ssIdNumber 7 + name "w_H_l_sole_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 [2 8 10] + } + data { + id 10 + ssIdNumber 10 + name "rot_l_sole_error" + 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 9 11] + } + data { + id 11 + ssIdNumber 11 + name "pos_r_sole_error" + 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 10 12] + } + data { + id 12 + ssIdNumber 12 + name "rot_r_sole_error" + 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 11 13] + } + data { + id 13 + ssIdNumber 13 + name "posCoM" + 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 12 0] + } + junction { + id 14 + position [23.5747 49.5747 7] + chart 2 + subviewer 2 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [2 0 0] + } + transition { + id 15 + 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 14 + 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 16 + machine 1 + name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Jacobians and ForwardKinematics/Co" + "mpute ForwardKinematics/Comput Position and Orientation Errors" + chart 2 + } + chart { + id 17 + machine 1 + name "POSITION CONTROLLER (IK)/References and Initial Conditions/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 18 0 0] + viewObj 17 + ssIdHighWaterMark 10 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 5 + disableImplicitCasting 1 + eml { + name "computeBaseVelocityWithFeetContactFCN" + } + firstData 19 + firstTransition 26 + firstJunction 25 + } + state { + id 18 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 17 + treeNode [17 0 0 0] + superState SUBCHART + subviewer 17 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function nu_b = computeBaseVelocityWithFeetContactFCN(J_l_sole, J_r_sole, feetContactStatus, join" + "tVel, pinvDampTolerance)\n\n nu_b = wbc.computeBaseVelocityWithFeetContact(J_l_sole, J_r_sole, feetContactSta" + "tus, jointVel, pinvDampTolerance);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 19 + 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 [17 0 20] + } + data { + id 20 + 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 [17 19 21] + } + data { + id 21 + 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 [17 20 22] + } + data { + id 22 + 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 [17 21 23] + } + data { + id 23 + 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 [17 22 24] + } + data { + id 24 + ssIdNumber 10 + name "pinvDampTolerance" + 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 [17 23 0] + } + junction { + id 25 + position [23.5747 49.5747 7] + chart 17 + subviewer 17 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [17 0 0] + } + transition { + id 26 + 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 25 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 17 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 17 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [17 0 0] + } + instance { + id 27 + machine 1 + name "POSITION CONTROLLER (IK)/References and Initial Conditions/Compute State Velocity/Compute Base Velocity" + chart 17 + } + chart { + id 28 + machine 1 + name "POSITION CONTROLLER (IK)/References and Initial Conditions/CoM References Generator/Reference Generator" + " CoM" + windowPosition [319.546 -65.92 97 534.4] + viewLimits [0 112 0 601] + screen [1 1 1366 768 1.25] + treeNode [0 29 0 0] + viewObj 28 + ssIdHighWaterMark 19 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 10 + disableImplicitCasting 1 + eml { + name "referenceGeneratorCoMFCN" + } + supportVariableSizing 0 + firstData 30 + firstTransition 35 + firstJunction 34 + } + state { + id 29 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 28 + treeNode [28 0 0 0] + superState SUBCHART + subviewer 28 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function references_CoM = referenceGeneratorCoMFCN(posCoM_0, t, Config)\n\n references_CoM = r" + "eferenceGeneratorCoM(posCoM_0, t, Config);\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 30 + ssIdNumber 17 + name "posCoM_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 [28 0 31] + } + data { + id 31 + ssIdNumber 13 + name "t" + 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 [28 30 32] + } + data { + id 32 + ssIdNumber 4 + name "references_CoM" + 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 [28 31 33] + } + data { + id 33 + 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 [28 32 0] + } + junction { + id 34 + position [23.5747 49.5747 7] + chart 28 + subviewer 28 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [28 0 0] + } + transition { + id 35 + labelString "{eML_blk_kernel();}" + labelPosition [40.125 31.875 104 16.042] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 34 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 28 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 28 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [28 0 0] + } + instance { + id 36 + machine 1 + name "POSITION CONTROLLER (IK)/References and Initial Conditions/CoM References Generator/Reference Generator" + " CoM" + chart 28 + } + chart { + id 37 + machine 1 + name "emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/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 38 0 0] + viewObj 37 + ssIdHighWaterMark 8 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 14 + disableImplicitCasting 1 + eml { + name "checkSpikes" + } + supportVariableSizing 0 + firstData 39 + firstTransition 43 + firstJunction 42 + } + state { + id 38 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 37 + treeNode [37 0 0 0] + superState SUBCHART + subviewer 37 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function noSpikes = checkSpikes(u, delta_u_max)\n\n noSpikes = wbc.checkSpikes(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 39 + 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 [37 0 40] + } + data { + id 40 + 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 [37 39 41] + } + data { + id 41 + 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 [37 40 0] + } + junction { + id 42 + position [23.5747 49.5747 7] + chart 37 + subviewer 37 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [37 0 0] + } + transition { + id 43 + 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 42 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 37 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 37 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [37 0 0] + } + instance { + id 44 + machine 1 + name "emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/MATLAB Function" + chart 37 + } + chart { + id 45 + 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 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 18 + disableImplicitCasting 1 + eml { + name "checkRangeFCN" + } + supportVariableSizing 0 + firstData 47 + firstTransition 53 + firstJunction 52 + } + state { + id 46 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 45 + treeNode [45 0 0 0] + superState SUBCHART + subviewer 45 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function inRange = checkRangeFCN(umin, umax, u, tol)\n\n inRange = wbc.checkRange(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 47 + 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 [45 0 48] + } + data { + id 48 + 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 [45 47 49] + } + data { + id 49 + 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 [45 48 50] + } + data { + id 50 + 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 [45 49 51] + } + data { + id 51 + 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 [45 50 0] + } + junction { + id 52 + position [23.5747 49.5747 7] + chart 45 + subviewer 45 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [45 0 0] + } + transition { + id 53 + 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 52 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 45 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 45 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [45 0 0] + } + instance { + id 54 + machine 1 + name "emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" + chart 45 + } + chart { + id 55 + 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 + ssIdHighWaterMark 7 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 24 + disableImplicitCasting 1 + eml { + name "computeBasePoseDerivativeFCN" + } + firstData 57 + firstTransition 61 + firstJunction 60 + } + state { + id 56 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 55 + treeNode [55 0 0 0] + superState SUBCHART + subviewer 55 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function pose_base_dot = computeBasePoseDerivativeFCN(nu_base_ikin, pose_base)\n\n pose_base_d" + "ot = computeBasePoseDerivative(nu_base_ikin, pose_base);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 57 + 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 [55 0 58] + } + data { + id 58 + ssIdNumber 6 + name "nu_base_ikin" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 57 59] + } + data { + id 59 + ssIdNumber 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 [55 58 0] + } + junction { + id 60 + position [23.5747 49.5747 7] + chart 55 + subviewer 55 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [55 0 0] + } + transition { + id 61 + 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 60 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 55 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 55 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [55 0 0] + } + instance { + id 62 + machine 1 + name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/InverseKinematics Integration/Compute the " + "Base Pose Derivative" + chart 55 + } + chart { + id 63 + 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 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 25 + disableImplicitCasting 1 + eml { + name "getSystemConfiguration" + } + firstData 65 + firstTransition 69 + firstJunction 68 + } + 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 q = getSystemConfiguration(w_H_b,jointPos)\n\n q = [w_H_b(:);jointPos];\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 65 + 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 [63 0 66] + } + data { + id 66 + 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 [63 65 67] + } + data { + id 67 + 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 [63 66 0] + } + junction { + id 68 + position [23.5747 49.5747 7] + chart 63 + subviewer 63 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [63 0 0] + } + transition { + id 69 + 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 68 + 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 70 + machine 1 + name "POSITION CONTROLLER (IK)/References and Initial Conditions/MATLAB Function" + chart 63 + } + chart { + id 71 + 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 + ssIdHighWaterMark 36 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 29 + disableImplicitCasting 1 + eml { + name "computeStateAccelerationsFCN" + } + supportVariableSizing 0 + firstData 73 + firstTransition 95 + firstJunction 94 + } + state { + id 72 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 71 + treeNode [71 0 0 0] + superState SUBCHART + subviewer 71 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function nuDot_ikin = computeStateAccelerationsFCN(pos_l_sole_error, pos_r_sole_error, rot_r_sole" + "_error, rot_l_sole_error, ...\n posCoM, jointPos, desired_pos_" + "vel_acc_CoM, desired_pos_vel_acc_joints, vel_l_sole, vel_r_sole, ...\n " + " velCoM, jointVel, J_l_sole, J_r_sole, J_CoM, JDot_l_sole_nu, JDot_r_sole_nu, JDot_CoM_nu, feetcontactSt" + "atus, Config)\n \n nuDot_ikin = computeStateAccelerations(pos_l" + "_sole_error, pos_r_sole_error, rot_r_sole_error, rot_l_sole_error, ...\n " + " posCoM, jointPos, desired_pos_vel_acc_CoM, desired_pos_vel_acc_joints, vel_l_sole, vel_r_sole, ...\n " + " velCoM, jointVel, J_l_sole, J_r_sole, J_CoM, JDot_l_sole_nu, JDot_r_sole_nu, J" + "Dot_CoM_nu, feetcontactStatus, Config);\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 73 + ssIdNumber 22 + name "pos_l_sole_error" + 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 [71 0 74] + } + data { + id 74 + ssIdNumber 24 + name "pos_r_sole_error" + 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 [71 73 75] + } + data { + id 75 + ssIdNumber 26 + name "rot_r_sole_error" + 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 [71 74 76] + } + data { + id 76 + ssIdNumber 27 + name "rot_l_sole_error" + 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 [71 75 77] + } + data { + id 77 + ssIdNumber 17 + name "posCoM" + 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 [71 76 78] + } + data { + id 78 + ssIdNumber 19 + 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 [71 77 79] + } + data { + id 79 + ssIdNumber 6 + 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 [71 78 80] + } + data { + id 80 + ssIdNumber 23 + name "desired_pos_vel_acc_joints" + 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 [71 79 81] + } + data { + id 81 + ssIdNumber 29 + name "vel_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 [71 80 82] + } + data { + id 82 + ssIdNumber 25 + name "vel_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 [71 81 83] + } + data { + id 83 + ssIdNumber 18 + name "velCoM" + 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 [71 82 84] + } + data { + id 84 + ssIdNumber 20 + 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 [71 83 85] + } + data { + id 85 + ssIdNumber 30 + 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 [71 84 86] + } + data { + id 86 + ssIdNumber 31 + 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 [71 85 87] + } + data { + id 87 + ssIdNumber 21 + name "J_CoM" + scope INPUT_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 [71 86 88] + } + data { + id 88 + 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 [71 87 89] + } + data { + id 89 + 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 [71 88 90] + } + data { + id 90 + ssIdNumber 34 + name "JDot_CoM_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 [71 89 91] + } + data { + id 91 + ssIdNumber 35 + 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 [71 90 92] + } + data { + id 92 + ssIdNumber 4 + name "nuDot_ikin" + 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 [71 91 93] + } + data { + id 93 + ssIdNumber 36 + 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 [71 92 0] + } + junction { + id 94 + position [23.5747 49.5747 7] + chart 71 + subviewer 71 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [71 0 0] + } + transition { + id 95 + labelString "{eML_blk_kernel();}" + labelPosition [48.125 43.875 104 16.042] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 94 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 71 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 71 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [71 0 0] + } + instance { + id 96 + machine 1 + name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/Compute Desired State Accelerations/trajec" + "toryGeneratorCoM" + chart 71 + } + chart { + id 97 + 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 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 30 + disableImplicitCasting 1 + eml { + name "fromTransfMatrixToQuaternions" + } + firstData 99 + firstTransition 102 + firstJunction 101 + } + state { + id 98 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 97 + treeNode [97 0 0 0] + superState SUBCHART + subviewer 97 + 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 99 + 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 [97 0 100] + } + data { + id 100 + 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 [97 99 0] + } + junction { + id 101 + position [23.5747 49.5747 7] + chart 97 + subviewer 97 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [97 0 0] + } + transition { + id 102 + 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 101 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 97 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 97 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [97 0 0] + } + instance { + id 103 + machine 1 + name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/InverseKinematics Integration/Rewrite the " + "Base Pose as Pos+Quat" + chart 97 + } + chart { + id 104 + 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 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 31 + disableImplicitCasting 1 + eml { + name "fromQuaternionsToTransfMatr" + } + firstData 106 + firstTransition 109 + firstJunction 108 + } + state { + id 105 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 104 + treeNode [104 0 0 0] + superState SUBCHART + subviewer 104 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function q_ikin = fromQuaternionsToTransfMatr(q_ikin_quat)\n\n % define the base pose in quate" + "rnions\n q_base = q_ikin_quat(1:7);\n\n % define the transformation matrix\n w_H_b " + " = wbc.fromPosQuatToTransfMatr(q_base);\n\n % column-major serialization\n w_H_b_vectorized = w_H_b(:);\n\n" + " % new state vector\n q_ikin = [w_H_b_vectorized; q_ikin_quat(8:end)];\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 106 + ssIdNumber 4 + name "q_ikin_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 [104 0 107] + } + data { + id 107 + ssIdNumber 5 + name "q_ikin" + 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 [104 106 0] + } + junction { + id 108 + position [23.5747 49.5747 7] + chart 104 + subviewer 104 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [104 0 0] + } + transition { + id 109 + 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 108 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 104 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 104 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [104 0 0] + } + instance { + id 110 + machine 1 + name "POSITION CONTROLLER (IK)/Integration Based InverseKinematics/InverseKinematics Integration/Rewirte the " + "Base Pose as Transf Matrix" + chart 104 + } + target { + id 111 + machine 1 + name "sfun" + codeFlags "" + linkNode [1 0 112] + } + target { + id 112 + machine 1 + name "rtw" + linkNode [1 111 0] + } +} diff --git a/controllers/floating-base-balancing-position-control/src-static-gui/closeModel.m b/controllers/floating-base-balancing-position-control/src-static-gui/closeModel.m new file mode 100644 index 0000000..019a95e --- /dev/null +++ b/controllers/floating-base-balancing-position-control/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('positionControlBalancing.mdl'); +close_system('positionControlBalancing.mdl'); + +% close all figures +close all + +% remove paths (optional) +rmpath('../../library/matlab-gui'); +rmpath('./src-static-gui'); + +disp('[closeModel]: done.') \ No newline at end of file diff --git a/controllers/floating-base-balancing-position-control/src-static-gui/compileModel.m b/controllers/floating-base-balancing-position-control/src-static-gui/compileModel.m new file mode 100644 index 0000000..6863693 --- /dev/null +++ b/controllers/floating-base-balancing-position-control/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 + positionControlBalancing([], [], [], 'compile') + positionControlBalancing([], [], [], 'term') + +catch ME + + errorMessages = ME; +end + +clc + +disp('Compilation done.') + +% warning about Simulink timing +warning('The model will anyways start with FEW SECONDS OF DELAY after pressing the ''Start Model'' button.') \ No newline at end of file diff --git a/controllers/floating-base-balancing-position-control/src/computeBasePoseDerivative.m b/controllers/floating-base-balancing-position-control/src/computeBasePoseDerivative.m new file mode 100644 index 0000000..57197c4 --- /dev/null +++ b/controllers/floating-base-balancing-position-control/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/floating-base-balancing-position-control/src/computePositionAndOrientationErrors.m b/controllers/floating-base-balancing-position-control/src/computePositionAndOrientationErrors.m new file mode 100644 index 0000000..386e0e0 --- /dev/null +++ b/controllers/floating-base-balancing-position-control/src/computePositionAndOrientationErrors.m @@ -0,0 +1,33 @@ +function [pos_l_sole_error, rot_l_sole_error, pos_r_sole_error, rot_r_sole_error, posCoM] = ... + computePositionAndOrientationErrors(w_H_l_sole, w_H_r_sole, w_H_CoM, w_H_r_sole_0, w_H_l_sole_0) + + % COMPUTEPOSITIONANDORIENTATIONERRORS retrieves the feet and CoM + % position and orientation errors. + + %% --- Initialization --- + + % position errors + posCoM = w_H_CoM(1:3,4); + pos_l_sole = w_H_l_sole(1:3,4); + pos_r_sole = w_H_r_sole(1:3,4); + pos_l_sole_0 = w_H_l_sole_0(1:3,4); + pos_r_sole_0 = w_H_r_sole_0(1:3,4); + pos_l_sole_error = pos_l_sole - pos_l_sole_0; + pos_r_sole_error = pos_r_sole - pos_r_sole_0; + + % rotation matrices at feet + w_R_l_sole = w_H_l_sole(1:3,1:3); + w_R_r_sole = w_H_r_sole(1:3,1:3); + w_R_l_sole_0 = w_H_l_sole_0(1:3,1:3); + w_R_r_sole_0 = w_H_r_sole_0(1:3,1:3); + + % feet orientation is parametrized with Euler angles + rot_l_sole = wbc.rollPitchYawFromRotation(w_R_l_sole); + rot_r_sole = wbc.rollPitchYawFromRotation(w_R_r_sole); + rot_l_sole_0 = wbc.rollPitchYawFromRotation(w_R_l_sole_0); + rot_r_sole_0 = wbc.rollPitchYawFromRotation(w_R_r_sole_0); + + % feet orientation errors + rot_l_sole_error = rot_l_sole - rot_l_sole_0; + rot_r_sole_error = rot_r_sole - rot_r_sole_0; +end \ No newline at end of file diff --git a/controllers/floating-base-balancing-position-control/src/computeStateAccelerations.m b/controllers/floating-base-balancing-position-control/src/computeStateAccelerations.m new file mode 100644 index 0000000..6b10eb2 --- /dev/null +++ b/controllers/floating-base-balancing-position-control/src/computeStateAccelerations.m @@ -0,0 +1,51 @@ +function nuDot_ikin = computeStateAccelerations(pos_l_sole_error, pos_r_sole_error, rot_r_sole_error, rot_l_sole_error, ... + posCoM, jointPos, desired_pos_vel_acc_CoM, desired_pos_vel_acc_joints, vel_l_sole, vel_r_sole, ... + velCoM, jointVel, J_l_sole, J_r_sole, J_CoM, JDot_l_sole_nu, JDot_r_sole_nu, JDot_CoM_nu, feetcontactStatus, Config) + + % COMPUTESTATEACCELERATIONS computes the floating base reference state + % accelerations by using a three task stack + % of task inverse kinematics approach. + + %% --- Initialization --- + + % internal parameters + pinv_tol = Config.Reg.pinv_tol; + ndof = size(J_CoM(:,7:end),2); + + % feet accelerations + accFeet_star = [(-Config.Gain.KP_feet * pos_l_sole_error(1:3) - Config.Gain.KD_feet * vel_l_sole(1:3)); + (-Config.Gain.KP_feet * rot_l_sole_error(1:3) - Config.Gain.KD_feet * vel_l_sole(4:6)); + (-Config.Gain.KP_feet * pos_r_sole_error(1:3) - Config.Gain.KD_feet * vel_r_sole(1:3)); + (-Config.Gain.KP_feet * rot_r_sole_error(1:3) - Config.Gain.KD_feet * vel_r_sole(4:6))]; + + % CoM accelerations + accCoM_star = desired_pos_vel_acc_CoM(:,3) -Config.Gain.KP_CoM*(posCoM - desired_pos_vel_acc_CoM(:,1)) -Config.Gain.KD_CoM * (velCoM-desired_pos_vel_acc_CoM(:,2)); + + % Joints accelerations + accJoint_star = desired_pos_vel_acc_joints(:,3) -Config.Gain.KP_joints*(jointPos - desired_pos_vel_acc_joints(:,1)) -Config.Gain.KD_joints*(jointVel - desired_pos_vel_acc_joints(:,2)); + + % Update Jacobians + J_feet = [feetcontactStatus(1)*J_l_sole ; feetcontactStatus(2)*J_r_sole]; + J_CoM = J_CoM(1:3,:); + J_postural = [zeros(ndof,6), eye(ndof)]; + JDot_feet_nu = [feetcontactStatus(1)*JDot_l_sole_nu; feetcontactStatus(2)*JDot_r_sole_nu]; + JDot_CoM_nu = JDot_CoM_nu(1:3); + + % Null space projectors for primary and secondary task + Null_feet = eye(6+ndof) -pinv(J_feet, pinv_tol)*J_feet; + Null_CoM = eye(6+ndof) -pinv(J_CoM*Null_feet, pinv_tol)*J_CoM*Null_feet; + + %% Stack of tasks inverse kinematics. + + % Primary task: respect the constraints at feet + nuDot_feet = pinv(J_feet, pinv_tol)*(accFeet_star -JDot_feet_nu); + + % Secondary task: achieve a desired CoM dynamics + nuDot_CoM = pinv(J_CoM*Null_feet, pinv_tol)*(accCoM_star -JDot_CoM_nu -J_CoM*nuDot_feet); + + % Third task: achieve a desired posture + nuDot_joint = pinv(J_postural*Null_feet*Null_CoM, pinv_tol)*(accJoint_star -J_postural*nuDot_feet -J_postural*Null_feet*nuDot_CoM); + + % Floating base and joints accelerations + nuDot_ikin = nuDot_feet + Null_feet*(nuDot_CoM + Null_CoM*nuDot_joint); +end \ No newline at end of file diff --git a/controllers/floating-base-balancing-position-control/src/referenceGeneratorCoM.m b/controllers/floating-base-balancing-position-control/src/referenceGeneratorCoM.m new file mode 100644 index 0000000..513e203 --- /dev/null +++ b/controllers/floating-base-balancing-position-control/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/floating-base-balancing-position-control/startModelWithStaticGui.m b/controllers/floating-base-balancing-position-control/startModelWithStaticGui.m new file mode 100644 index 0000000..af70d78 --- /dev/null +++ b/controllers/floating-base-balancing-position-control/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('positionControlBalancing.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/torque-controllers/momentum-based-yoga/stopTorqueBalancingYoga.m b/controllers/floating-base-balancing-position-control/stopPositionControlBalancing.m similarity index 85% rename from torque-controllers/momentum-based-yoga/stopTorqueBalancingYoga.m rename to controllers/floating-base-balancing-position-control/stopPositionControlBalancing.m index 5f63bc2..3a5a225 100644 --- a/torque-controllers/momentum-based-yoga/stopTorqueBalancingYoga.m +++ b/controllers/floating-base-balancing-position-control/stopPositionControlBalancing.m @@ -47,14 +47,14 @@ % number of times the real time step was bigger than twice the % desired time step value - numOfTimeStepViolations = sum(diff(yarp_time0) > 2*Config.Ts); + numOfTimeStepViolations = sum(diff(yarp_time0) > 2*Config.tStep); if numOfTimeStepViolations > 1 && numOfTimeStepViolations <= 50 - warning(['The time step tolerance of ', num2str(Config.Ts), '[s] has been violated at least once.']) + 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.Ts), '[s] has been violated more than 50 times.']) + 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 diff --git a/controllers/floating-base-balancing-torque-control/CMakeLists.txt b/controllers/floating-base-balancing-torque-control/CMakeLists.txt new file mode 100644 index 0000000..17b0039 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/CMakeLists.txt @@ -0,0 +1 @@ +register_mdl(MODELNAME "torqueControlBalancing") \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control/README.md b/controllers/floating-base-balancing-torque-control/README.md new file mode 100644 index 0000000..b4ee29f --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/README.md @@ -0,0 +1,23 @@ +## Module description + +This module implements a torque control balancing strategy. It computes the interaction forces at the feet in order to stabilise a desired `centroidal momentum` dynamics, which implies the tracking of a desired center-of-mass trajectory. A cost function penalizing high joint torques - that generate the feet forces - is added to the control framework. + +For details see also: [iCub whole-body control through force regulation on rigid non-coplanar contacts](http://journal.frontiersin.org/article/10.3389/frobt.2015.00006/abstract) and [Stability Analysis and Design of Momentum-Based Controllers for Humanoid Robots](https://ieeexplore.ieee.org/document/7759126). + +### Compatibility + +The folder contains the Simulink model `torqueControlBalancing.mdl`, which is generated by using Matlab R2017b. + +### Supported robots + +Currently, supported robots are: `iCubGenova04`, `iCubGenova02`, `icubGazeboSim`, `iCubGazeboV2_5`. + +## Module details + +### 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 diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/configRobot.m b/controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/configRobot.m similarity index 65% rename from torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/configRobot.m rename to controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/configRobot.m index 65a9c26..cff9891 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/configRobot.m +++ b/controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/configRobot.m @@ -1,17 +1,17 @@ % CONFIGROBOT initializes parameters specific of a particular robot -% (e.g., icuGazeboSim) -% +% (e.g., icubGazeboSim) %% --- Initialization --- + Config.ON_GAZEBO = true; ROBOT_DOF = 23; -ROBOT_DOF_FOR_SIMULINK = eye(ROBOT_DOF); +Config.GRAV_ACC = 9.81; % Robot configuration for WBToolbox -WBTConfigRobot = WBToolbox.Configuration; -WBTConfigRobot.RobotName = 'icubSim'; -WBTConfigRobot.UrdfFile = 'model.urdf'; -WBTConfigRobot.LocalName = 'WBT'; +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'}; @@ -33,16 +33,16 @@ end % Frames list -Frames.BASE = 'root_link'; -Frames.IMU = 'imu_frame'; -Frames.LEFT_FOOT = 'l_sole'; -Frames.RIGHT_FOOT = 'r_sole'; -Frames.COM = 'com'; +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; +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 @@ -66,34 +66,35 @@ % 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 and Config.PITCH_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 de to the IMU drift in estimating this angle. Hence, -% when either of the flags Config.YAW_IMU_FILTER or Config.PITCH_IMU_FILTER -% is set to true, then the yaw and/or pitch angles of the contact foot are -% ignored and kept equal to the initial values. -Config.FILTER_IMU_YAW = true; -Config.FILTER_IMU_PITCH = true; - -% Config.CORRECT_NECK_IMU: when set equal to true, the kineamtics from the +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). -Config.CORRECT_NECK_IMU = true; +% 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; +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. It is necessary to set this variable in case the same -% port has different dimensions for different robots -Ports.NECK_POS_PORT_SIZE = 3; \ No newline at end of file +Ports.WRENCH_LEFT_FOOT = '/wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o'; +Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o'; +Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; +Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; + +% Ports dimensions +Ports.NECK_POS_PORT_SIZE = 3; +Ports.IMU_PORT_SIZE = 12; +Ports.IMU_PORT_ORIENTATION_INDEX = [1,2,3]; +Ports.WRENCH_PORT_SIZE = 6; \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/configStateMachine.m b/controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/configStateMachine.m new file mode 100644 index 0000000..fb21333 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/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/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m b/controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m new file mode 100644 index 0000000..7b2eae3 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/gainsAndReferences.m @@ -0,0 +1,342 @@ +% GAINSANDREFERENCES compute gains matrices, references and other control +% related quantities for each state of the state machine. + +%% --- Initialization --- + +% CoM gains +Gain.KP_CoM = [50 50 10 % state == 1 TWO FEET BALANCING + 50 50 10 % state == 2 COM TRANSITION TO LEFT + 50 50 10 % state == 3 LEFT FOOT BALANCING + 50 50 10 % state == 4 YOGA LEFT FOOT + 50 50 10 % state == 5 PREPARING FOR SWITCHING + 50 50 10 % state == 6 LOOKING FOR CONTACT + 50 50 10 % state == 7 TRANSITION TO INITIAL POSITION + 50 50 10 % state == 8 COM TRANSITION TO RIGHT FOOT + 50 50 10 % state == 9 RIGHT FOOT BALANCING + 50 50 10 % state == 10 YOGA RIGHT FOOT + 50 50 10 % state == 11 PREPARING FOR SWITCHING + 50 50 10 % state == 12 LOOKING FOR CONTACT + 50 50 10]; % state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_CoM = 2*sqrt(Gain.KP_CoM)/20; + +% Angular momentum gains +Gain.KI_AngularMomentum = 0.25 ; +Gain.KP_AngularMomentum = 2*sqrt(Gain.KI_AngularMomentum); + +% Postural task gains +% % TORSO %% LEFT ARM %% RIGHT ARM %% LEFT LEG %% RIGHT LEG %% +Gain.KP_postural = [10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 1 TWO FEET BALANCING + 10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 2 COM TRANSITION TO LEFT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 100 100, 30 30 20 20 100 100 % state == 3 LEFT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10, 100 200 100 400 100 100, 100 50 30 100 100 100 % state == 4 YOGA LEFT FOOT + 30 30 30, 5 5 10 10, 10 10 20 10, 200 250 20 20 10 50, 220 350 120 200 65 100 % state == 5 PREPARING FOR SWITCHING + 30 30 30, 10 10 20 10, 10 10 20 10, 100 350 20 200 10 100, 220 350 120 200 65 100 % state == 6 LOOKING FOR CONTACT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 5 5, 30 30 30 20 5 5 % state == 7 TRANSITION TO INITIAL POSITION + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 100 100, 30 30 30 20 100 100 % state == 8 COM TRANSITION TO RIGHT FOOT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 9 RIGHT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10, 100 200 100 400 100 100, 100 50 30 100 100 100 % state == 10 YOGA RIGHT FOOT + 30 30 30, 5 5 10 10, 10 10 20 10, 200 250 20 20 10 50, 220 350 120 200 65 100 % state == 11 PREPARING FOR SWITCHING + 30 30 30, 10 10 20 10, 10 10 20 10, 100 350 20 200 10 100, 220 350 120 200 65 100 % state == 12 LOOKING FOR CONTACT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100]; % state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_postural = 2*sqrt(Gain.KP_postural(1,:))/20; + +% symmetric gains +Gain.KP_postural(8:12,:) = Gain.KP_postural(2:6,:); +Gain.KP_postural(8:12,12:17) = Gain.KP_postural(2:6,18:23); +Gain.KP_postural(8:12,18:23) = Gain.KP_postural(2:6,12:17); + +%% Smoothing times + +% Smoothing time gain scheduling +Config.SmoothingTimeGainScheduling = 2; + +% Smoothing time CoM references +StateMachine.CoMSmoothingTime = [1; %% state == 1 TWO FEET BALANCING + 1; %% state == 2 COM TRANSITION TO LEFT FOOT + 1; %% state == 3 LEFT FOOT BALANCING + 2; %% state == 4 YOGA LEFT FOOT + 2; %% state == 5 PREPARING FOR SWITCHING + 2; %% state == 6 LOOKING FOR CONTACT + 1; %% state == 7 TRANSITION INIT POSITION + 1; %% state == 8 COM TRANSITION TO RIGHT FOOT + 1; %% state == 9 RIGHT FOOT BALANCING + 2; %% state == 10 YOGA RIGHT FOOT + 2; %% state == 11 PREPARING FOR SWITCHING + 2; %% state == 12 LOOKING FOR CONTACT + 10]; %% state == 13 TRANSITION INIT POSITION + +% Smoothing time for joints references +StateMachine.jointsSmoothingTime = [1; %% state == 1 TWO FEET BALANCING + 1; %% state == 2 COM TRANSITION TO LEFT FOOT + 1; %% state == 3 LEFT FOOT BALANCING + 2; %% state == 4 YOGA LEFT FOOT + 2; %% state == 5 PREPARING FOR SWITCHING + 2; %% state == 6 LOOKING FOR CONTACT + 1; %% state == 7 TRANSITION INIT POSITION + 1; %% state == 8 COM TRANSITION TO RIGHT FOOT + 1; %% state == 9 RIGHT FOOT BALANCING + 2; %% state == 10 YOGA RIGHT FOOT + 2; %% state == 11 PREPARING FOR SWITCHING + 2; %% state == 12 LOOKING FOR CONTACT + 10]; %% state == 13 TRANSITION INIT POSITION + +% scale factor smoothing time multiplies the smoothing factor during the +% Yoga (state 4 and 10). The purpose is to reduce the time necessary for +% the reference to converge to the next position, but without changing also +% the valuse stored in Sm.joints_leftYogaRef/Sm.joints_rightYogaRef +StateMachine.scaleFactorSmoothingTime = 0.9; + +%% CoM delta + +% To be summed to the reference CoM position +StateMachine.CoM_delta = [% THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE LEFT FOOT + 0.0, 0.00, 0.0; %% NOT USED + 0.0, 0.00, 0.0; %% state == 2 COM TRANSITION TO LEFT FOOT + 0.0, 0.00, 0.0; %% state == 3 LEFT FOOT BALANCING + 0.0, 0.005, 0.0; %% state == 4 YOGA LEFT FOOT + 0.0, 0.00, 0.0; %% state == 5 PREPARING FOR SWITCHING + 0.0, -0.09, 0.0; %% state == 6 LOOKING FOR CONTACT + 0.0, 0.00, 0.0; %% state == 7 TWO FEET BALANCING + % THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE RIGHT FOOT + 0.0, 0.00, 0.0; %% state == 8 COM TRANSITION TO RIGHT FOOT + 0.0, 0.00, 0.0; %% state == 9 RIGHT FOOT BALANCING + 0.0, -0.005, 0.0; %% state == 10 YOGA RIGHT FOOT + 0.0, 0.00, 0.0; %% state == 11 PREPARING FOR SWITCHING + 0.0, 0.09, 0.0; %% state == 12 LOOKING FOR CONTACT + 0.0, 0.00, 0.0]; %% NOT USED + +%% Joint references +StateMachine.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 2 COM TRANSITION TO LEFT + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % + [ 0.0864,0.0258,0.0152, ... %% state == 3 LEFT FOOT BALANCING + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 5 PREPARING FOR SWITCHING + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % + [ 0.0864,0.0258,0.0152, ... %% state == 6 LOOKING FOR CONTACT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [ 0.0864,0.0258,0.0152, ... %% state == 8 COM TRANSITION TO RIGHT FOOT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % + [ 0.0864,0.0258,0.0152, ... %% state == 9 RIGHT FOOT BALANCING + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 11 PREPARING FOR SWITCHING + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % + [ 0.0864,0.0258,0.0152, ... %% state == 12 LOOKING FOR CONTACT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369]; % + zeros(1,ROBOT_DOF)]; %% THIS REFERENCE IS IGNORED + +% YOGA MOVESET (joint references during state 4 and 10) +q1 = [-0.0790,0.2279, 0.4519, ... + -1.1621,0.6663, 0.4919, 0.9947, ... + -1.0717,1.2904,-0.2447, 1.0948, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3484,0.4008,-0.0004,-0.3672,-0.1060,-0.0875]; + +q2 = [-0.0790,0.2279, 0.4519, ... + -1.1621,0.6663, 0.4965, 0.9947, ... + -1.0717,1.2904,-0.2493, 1.0948, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... + 0.3714,0.9599, 1.3253,-1.6594,-0.1060,-0.0614]; + +q3 = [-0.0852,-0.4273,0.0821,... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... + 0.3714,0.9599, 1.3253,-1.6594, 0.5000,-0.0614]; + +q4 = [-0.0852,-0.4273,0.0821,... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.3473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q5 = [-0.0790,-0.2273, 0.4519, ... + -1.1621,0.6663, 0.4965, 0.9947, ... + -1.0717,1.2904,-0.2493, 1.0948, ... + 0.2092, 0.4473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q6 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q7 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253, -1.6217, 0.5000,-0.0614]; + +q8 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q9 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614]; + +q10 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q11 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q12 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q13 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q14 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614]; + +q15 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 1.5514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q16 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.2514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614]; + +q17 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + -0.3514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +StateMachine.joints_leftYogaRef = [ 0, q1; + 1*StateMachine.jointsSmoothingTime(4),q2; + 2*StateMachine.jointsSmoothingTime(4),q3; + 3*StateMachine.jointsSmoothingTime(4),q4; + 4*StateMachine.jointsSmoothingTime(4),q5; + 5*StateMachine.jointsSmoothingTime(4),q6; + 6*StateMachine.jointsSmoothingTime(4),q7; + 7*StateMachine.jointsSmoothingTime(4),q8; + 8*StateMachine.jointsSmoothingTime(4),q9; + 9*StateMachine.jointsSmoothingTime(4),q10; + 10*StateMachine.jointsSmoothingTime(4),q11; + 11*StateMachine.jointsSmoothingTime(4),q12; + 12*StateMachine.jointsSmoothingTime(4),q13; + 13*StateMachine.jointsSmoothingTime(4),q14; + 14*StateMachine.jointsSmoothingTime(4),q15; + 15*StateMachine.jointsSmoothingTime(4),q16; + 16*StateMachine.jointsSmoothingTime(4),q17; + 17*StateMachine.jointsSmoothingTime(4),q10; + 18*StateMachine.jointsSmoothingTime(4),q11; + 19*StateMachine.jointsSmoothingTime(4),q12; + 20*StateMachine.jointsSmoothingTime(4),q13; + 21*StateMachine.jointsSmoothingTime(4),q14; + 22*StateMachine.jointsSmoothingTime(4),q15; + 23*StateMachine.jointsSmoothingTime(4),q16; + 24*StateMachine.jointsSmoothingTime(4),q17; + 25*StateMachine.jointsSmoothingTime(4),q8]; + +StateMachine.joints_rightYogaRef = StateMachine.joints_leftYogaRef; +StateMachine.joints_rightYogaRef(:,1) = [0; + 1*StateMachine.jointsSmoothingTime(10); + 2*StateMachine.jointsSmoothingTime(10); + 3*StateMachine.jointsSmoothingTime(10); + 4*StateMachine.jointsSmoothingTime(10); + 5*StateMachine.jointsSmoothingTime(10); + 6*StateMachine.jointsSmoothingTime(10); + 7*StateMachine.jointsSmoothingTime(10); + 8*StateMachine.jointsSmoothingTime(10); + 9*StateMachine.jointsSmoothingTime(10); + 10*StateMachine.jointsSmoothingTime(10); + 11*StateMachine.jointsSmoothingTime(10); + 12*StateMachine.jointsSmoothingTime(10); + 13*StateMachine.jointsSmoothingTime(10); + 14*StateMachine.jointsSmoothingTime(10); + 15*StateMachine.jointsSmoothingTime(10); + 16*StateMachine.jointsSmoothingTime(10); + 17*StateMachine.jointsSmoothingTime(10); + 18*StateMachine.jointsSmoothingTime(10); + 19*StateMachine.jointsSmoothingTime(10); + 20*StateMachine.jointsSmoothingTime(10); + 21*StateMachine.jointsSmoothingTime(10); + 22*StateMachine.jointsSmoothingTime(10); + 23*StateMachine.jointsSmoothingTime(10); + 24*StateMachine.jointsSmoothingTime(10); + 25*StateMachine.jointsSmoothingTime(10)]; + +% if the demo is not "yogaExtended", stop at the 8th move +if ~StateMachine.yogaExtended + + StateMachine.joints_leftYogaRef = StateMachine.joints_leftYogaRef(1:8,:); + StateMachine.joints_rightYogaRef = StateMachine.joints_rightYogaRef(1:8,:); + StateMachine.joints_leftYogaRef(8,1) = 15*StateMachine.jointsSmoothingTime(4); + StateMachine.joints_rightYogaRef(8,1) = 15*StateMachine.jointsSmoothingTime(10); +end + +% MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA +for i = 1:size(StateMachine.joints_rightYogaRef,1) + + StateMachine.joints_rightYogaRef(i,2:4) = [StateMachine.joints_rightYogaRef(i,2) -StateMachine.joints_rightYogaRef(i,3) -StateMachine.joints_rightYogaRef(i,4)]; + rightArm = StateMachine.joints_rightYogaRef(i,end-15:end-12); + StateMachine.joints_rightYogaRef(i,end-15:end-12) = StateMachine.joints_rightYogaRef(i,end-19:end-16); + StateMachine.joints_rightYogaRef(i,end-19:end-16) = rightArm; + rightLeg = StateMachine.joints_rightYogaRef(i,end-5:end); + StateMachine.joints_rightYogaRef(i,end-5:end) = StateMachine.joints_rightYogaRef(i,end-11:end-6); + StateMachine.joints_rightYogaRef(i,end-11:end-6) = rightLeg; +end + +%% References for CoM trajectory (COORDINATOR DEMO ONLY) + +% that the robot waits before starting the left-and-right +Config.noOscillationTime = 0; +Config.directionOfOscillation = [0;1;0]; +Config.amplitudeOfOscillation = 0.02; % [m] +Config.frequencyOfOscillation = 0.2; % [Hz] \ No newline at end of file diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/configRobot.m b/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova02/configRobot.m similarity index 70% rename from torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/configRobot.m rename to controllers/floating-base-balancing-torque-control/app/robots/iCubGenova02/configRobot.m index cde2821..5d39ceb 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/configRobot.m +++ b/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova02/configRobot.m @@ -1,11 +1,10 @@ % CONFIGROBOT initializes parameters specific of a particular robot % (e.g., icuGazeboSim) -% %% --- Initialization --- Config.ON_GAZEBO = false; ROBOT_DOF = 23; -ROBOT_DOF_FOR_SIMULINK = eye(ROBOT_DOF); +Config.GRAV_ACC = 9.81; % Robot configuration for WBToolbox WBTConfigRobot = WBToolbox.Configuration; @@ -33,11 +32,11 @@ end % Frames list -Frames.BASE = 'root_link'; -Frames.IMU = 'imu_frame'; -Frames.LEFT_FOOT = 'l_sole'; -Frames.RIGHT_FOOT = 'r_sole'; -Frames.COM = 'com'; +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 @@ -66,34 +65,35 @@ % 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 and Config.PITCH_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 de to the IMU drift in estimating this angle. Hence, -% when either of the flags Config.YAW_IMU_FILTER or Config.PITCH_IMU_FILTER -% is set to true, then the yaw and/or pitch angles of the contact foot are -% ignored and kept equal to the initial values. -Config.FILTER_IMU_YAW = true; -Config.FILTER_IMU_PITCH = true; - -% Config.CORRECT_NECK_IMU: when set equal to true, the kineamtics from the +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). -Config.CORRECT_NECK_IMU = true; +% 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; +Config.USE_QP_SOLVER = true; % Ports name list -Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; -Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; -Ports.WRENCH_LEFT_FOOT = '/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o'; -Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o'; - -% Ports dimensions. It is necessary to set this variable in case the same -% port has different dimensions for different robots -Ports.NECK_POS_PORT_SIZE = 6; \ No newline at end of file +Ports.WRENCH_LEFT_FOOT = '/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o'; +Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o'; +Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; +Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; + +% Ports dimensions +Ports.NECK_POS_PORT_SIZE = 6; +Ports.IMU_PORT_SIZE = 12; +Ports.IMU_PORT_ORIENTATION_INDEX = [1,2,3]; +Ports.WRENCH_PORT_SIZE = 6; \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova02/configStateMachine.m b/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova02/configStateMachine.m new file mode 100644 index 0000000..7107ed3 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova02/configStateMachine.m @@ -0,0 +1,160 @@ +% CONFIGSTATEMACHINE configures the state machine (type of demo, velocity +% of the demo, repeat movements, and so on). + +%% --- Initialization --- + +% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) +Config.LEFT_RIGHT_MOVEMENTS = false; + +% If equal to one, the desired values of the center of mass are smoothed internally +Config.SMOOTH_COM_DES = true; + +% If equal to one, the desired values of the postural tasks are smoothed internally +Config.SMOOTH_JOINT_DES = true; + +% Joint torques saturation [Nm] +Sat.torque = 60; + +% Joint torques rate of change saturation +Sat.uDotMax = 300; + +% max unsigned difference between two consecutive (measured) joint positions, +% i.e. delta_qj = abs(qj(k) - qj(k-1)) +Sat.maxJointsPositionDelta = 15*pi/180; % [rad] + +%% Regularization parameters +Reg.pinvDamp_baseVel = 1e-7; +Reg.pinvDamp = 0.07; +Reg.pinvTol = 1e-5; +Reg.KP_postural = 0.1; +Reg.KD_postural = 0; +Reg.HessianQP = 1e-7; + +%% State Machine configuration + +% time between two yoga positions +StateMachine.joints_pauseBetweenYogaMoves = 5; + +% contact forces threshold +StateMachine.wrench_thresholdContactOn = 50; +StateMachine.wrench_thresholdContactOff = 100; + +% threshold on CoM and joints error +StateMachine.CoM_threshold = 0.01; +StateMachine.joints_thresholdNotInContact = 5; +StateMachine.joints_thresholdInContact = 50; + +% initial state for state machine +StateMachine.initialState = 1; + +% other configuration parameters for state machine +StateMachine.tBalancing = 1; +StateMachine.tBalancingBeforeYoga = 1; +StateMachine.yogaExtended = true; +StateMachine.skipYoga = false; +StateMachine.demoOnlyBalancing = false; +StateMachine.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first +StateMachine.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot (not available for now) + +%%%% List of possible "Yoga in loop" %%%% + +% the robot will repeat the FULL DEMO (two feet balancing, yoga on left +% foot, back on two feet, yoga right foot, back on two feet). The demo is +% repeated until the user stops the Simulink model. This option is ignored +% if Sm.demoStartsOnRightSupport = true. +StateMachine.twoFeetYogaInLoop = false; + +% the robot will repeat the ONE FOOT yoga for the number of times the user +% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two +% feet balancing in between to consecutive yoga. WARNING: if the option +% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga +% on left foot for the number of times the user specifies in the Sm.yogaCounter, +% and then it will repeat the yoga on the right foot for the same number of times. +StateMachine.oneFootYogaInLoop = false; +StateMachine.yogaCounter = 5; + +%% Parameters for motors reflected inertia + +% transmission ratio (1/N) +Config.Gamma = 0.01*eye(ROBOT_DOF); + +% modify the value of the transmission ratio for the hip pitch. +% TODO: avoid to hard-code the joint numbering +Config.Gamma(end-5, end-5) = 0.0067; +Config.Gamma(end-11,end-11) = 0.0067; + +% motors inertia (Kg*m^2) +legsMotors_I_m = 0.0827*1e-4; +torsoPitchRollMotors_I_m = 0.0827*1e-4; +torsoYawMotors_I_m = 0.0585*1e-4; +armsMotors_I_m = 0.0585*1e-4; + +% add harmonic drives reflected inertia +if Config.INCLUDE_HARMONIC_DRIVE_INERTIA + + legsMotors_I_m = legsMotors_I_m + 0.054*1e-4; + torsoPitchRollMotors_I_m = torsoPitchRollMotors_I_m + 0.054*1e-4; + torsoYawMotors_I_m = torsoYawMotors_I_m + 0.021*1e-4; + armsMotors_I_m = armsMotors_I_m + 0.021*1e-4; +end + +Config.I_m = diag([torsoPitchRollMotors_I_m*ones(2,1); + torsoYawMotors_I_m; + armsMotors_I_m*ones(8,1); + legsMotors_I_m*ones(12,1)]); + +% parameters for coupling matrices. Updated according to the wiki: +% +% http://wiki.icub.org/wiki/ICub_coupled_joints +% +% and corrected according to https://github.com/robotology/robots-configuration/issues/39 +t = 0.615; +r = 0.022; +R = 0.04; + +% coupling matrices +T_LShoulder = [-1 0 0; + -1 -t 0; + 0 t -t]; + +T_RShoulder = [ 1 0 0; + 1 t 0; + 0 -t t]; + +T_torso = [ 0.5 -0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; + +if Config.INCLUDE_COUPLING + + Config.T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12)); +else + Config.T = eye(ROBOT_DOF); +end + +% gain for feedforward term in joint torques calculation. Valid range: a +% value between 0 and 1 +Config.K_ff = 0; + +% Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA if true, the desired joints +% accelerations are used for computing the feedforward term in joint +% torques calculations. Not effective if Config.K_ff = 0. +Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA = false; + +%% Constraints for QP for balancing + +% The friction cone is approximated by using linear interpolation of the circle. +% So, numberOfPoints defines the number of points used to interpolate the circle +% in each cicle's quadrant +numberOfPoints = 4; +forceFrictionCoefficient = 1/3; +torsionalFrictionCoefficient = 1/75; +fZmin = 10; + +% physical size of the foot +feet_size = [-0.07 0.12 ; % xMin, xMax + -0.045 0.05 ]; % yMin, yMax + +% Compute contact constraints (friction cone, unilateral constraints) +[ConstraintsMatrix, bVectorConstraints] = wbc.computeRigidContactConstraints ... + (forceFrictionCoefficient, numberOfPoints, torsionalFrictionCoefficient, feet_size, fZmin); \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova02/gainsAndReferences.m b/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova02/gainsAndReferences.m new file mode 100644 index 0000000..6131896 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova02/gainsAndReferences.m @@ -0,0 +1,346 @@ +% GAINSANDREFERENCES compute gains matrices, references and other control +% related quantities for each state of the state machine. + +%% --- Initialization --- + +% CoM gains +Gain.KP_CoM = [50 100 5 % state == 1 TWO FEET BALANCING + 50 100 5 % state == 2 COM TRANSITION TO LEFT + 50 100 5 % state == 3 LEFT FOOT BALANCING + 50 100 5 % state == 4 YOGA LEFT FOOT + 50 100 5 % state == 5 PREPARING FOR SWITCHING + 50 100 5 % state == 6 LOOKING FOR CONTACT + 50 100 5 % state == 7 TRANSITION TO INITIAL POSITION + 50 100 5 % state == 8 COM TRANSITION TO RIGHT FOOT + 50 100 5 % state == 9 RIGHT FOOT BALANCING + 50 100 5 % state == 10 YOGA RIGHT FOOT + 50 100 5 % state == 11 PREPARING FOR SWITCHING + 50 100 5 % state == 12 LOOKING FOR CONTACT + 50 100 5];% state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_CoM = 2*sqrt(Gain.KP_CoM)/20; + +% Angular momentum gains +Gain.KI_AngularMomentum = 3; +Gain.KP_AngularMomentum = 2*sqrt(Gain.KI_AngularMomentum)/5; + +% Postural task gains +% % TORSO %% LEFT ARM %% RIGHT ARM %% LEFT LEG %% RIGHT LEG %% +Gain.KP_postural = [10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 1 TWO FEET BALANCING + 10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 2 COM TRANSITION TO LEFT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 100 100, 30 30 20 20 100 100 % state == 3 LEFT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10,100 200 100 400 100 100,100 50 30 100 100 100 % state == 4 YOGA LEFT FOOT + 30 30 30, 5 5 10 10, 10 10 20 10,200 250 20 20 10 10,220 550 220 200 65 300 % state == 5 PREPARING FOR SWITCHING + 30 30 30, 10 10 20 10, 10 10 20 10,100 350 20 200 10 100,220 550 220 200 65 300 % state == 6 LOOKING FOR CONTACT + 30 30 30, 10 10 10 8, 10 10 10 8, 30 50 60 30 5 5, 30 30 30 20 5 5 % state == 7 TRANSITION TO INITIAL POSITION + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 100 100, 30 30 30 20 100 100 % state == 8 COM TRANSITION TO RIGHT FOOT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 100 100, 30 30 20 20 100 100 % state == 9 RIGHT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10,100 50 30 100 100 100,100 200 100 100 10 10 % state == 10 YOGA RIGHT FOOT + 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,200 250 20 20 10 10 % state == 11 PREPARING FOR SWITCHING + 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100 % state == 12 LOOKING FOR CONTACT + 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100];% state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_postural = 2*sqrt(Gain.KP_postural(1,:))/20; + +% symmetric gains +Gain.KP_postural(10,18:23) = Gain.KP_postural(10,18:23)*1.5; +Gain.KP_postural(10,1:3) = Gain.KP_postural(10,1:3)*2; +Gain.KP_postural(4,12:17) = Gain.KP_postural(4,12:17)*1.5; +Gain.KP_postural(4,1:3) = Gain.KP_postural(4,1:3)*2; +Gain.KP_postural(4,4:11) = Gain.KP_postural(4,4:11)*1.5; +Gain.KP_postural(7,2:3) = Gain.KP_postural(7,2:3)*1.5; + +%% Smoothing times + +% Smoothing time gain scheduling +Config.SmoothingTimeGainScheduling = 2; + +% Smoothing time CoM references +StateMachine.CoMSmoothingTime = [1; %% state == 1 TWO FEET BALANCING + 1; %% state == 2 COM TRANSITION TO LEFT FOOT + 1; %% state == 3 LEFT FOOT BALANCING + 0.65; %% state == 4 YOGA LEFT FOOT + 2; %% state == 5 PREPARING FOR SWITCHING + 2; %% state == 6 LOOKING FOR CONTACT + 1; %% state == 7 TRANSITION INIT POSITION + 1; %% state == 8 COM TRANSITION TO RIGHT FOOT + 1; %% state == 9 RIGHT FOOT BALANCING + 0.65; %% state == 10 YOGA RIGHT FOOT + 2; %% state == 11 PREPARING FOR SWITCHING + 5; %% state == 12 LOOKING FOR CONTACT + 10]; %% state == 13 TRANSITION INIT POSITION + +% Smoothing time for joints references +StateMachine.jointsSmoothingTime = [1; %% state == 1 TWO FEET BALANCING + 1; %% state == 2 COM TRANSITION TO LEFT FOOT + 1; %% state == 3 LEFT FOOT BALANCING + 0.65; %% state == 4 YOGA LEFT FOOT + 2; %% state == 5 PREPARING FOR SWITCHING + 2; %% state == 6 LOOKING FOR CONTACT + 1; %% state == 7 TRANSITION INIT POSITION + 1; %% state == 8 COM TRANSITION TO RIGHT FOOT + 1; %% state == 9 RIGHT FOOT BALANCING + 0.65; %% state == 10 YOGA RIGHT FOOT + 2; %% state == 11 PREPARING FOR SWITCHING + 5; %% state == 12 LOOKING FOR CONTACT + 10]; %% state == 13 TRANSITION INIT POSITION + +% scale factor smoothing time multiplies the smoothing factor during the +% Yoga (state 4 and 10). The purpose is to reduce the time necessary for +% the reference to converge to the next position, but without changing also +% the valuse stored in Sm.joints_leftYogaRef/Sm.joints_rightYogaRef +StateMachine.scaleFactorSmoothingTime = 0.9; + +%% CoM delta + +% To be summed to the reference CoM position +StateMachine.CoM_delta = [% THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE LEFT FOOT + 0.0, 0.00, 0.0; %% NOT USED + 0.0, 0.00, 0.0; %% state == 2 COM TRANSITION TO LEFT FOOT + 0.0, 0.005, 0.0; %% state == 3 LEFT FOOT BALANCING + 0.0, 0.005, 0.0; %% state == 4 YOGA LEFT FOOT + 0.0, 0.00, 0.0; %% state == 5 PREPARING FOR SWITCHING + 0.02, -0.08, 0.0; %% state == 6 LOOKING FOR CONTACT + 0.0, 0.00, 0.0; %% state == 7 TWO FEET BALANCING + % THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE RIGHT FOOT + 0.0, 0.00, 0.0; %% state == 8 COM TRANSITION TO RIGHT FOOT + 0.0, 0.00, 0.0; %% state == 9 RIGHT FOOT BALANCING + 0.0, -0.01, 0.0; %% state == 10 YOGA RIGHT FOOT + 0.0, 0.00, 0.0; %% state == 11 PREPARING FOR SWITCHING + 0.0, 0.09, 0.0; %% state == 12 LOOKING FOR CONTACT + 0.0, 0.00, 0.0]; %% NOT USED + +%% Joint references +StateMachine.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 2 COM TRANSITION TO LEFT + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % + [ 0.0864,0.0258,0.0152, ... %% state == 3 LEFT FOOT BALANCING + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,-0.1060,-0.1151]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 5 PREPARING FOR SWITCHING + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % + [ 0.0864,0.0258,0.0152, ... %% state == 6 LOOKING FOR CONTACT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [ 0.0864,0.0258,0.0152, ... %% state == 8 COM TRANSITION TO RIGHT FOOT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % + [ 0.0864,0.0258,0.0152, ... %% state == 9 RIGHT FOOT BALANCING + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 11 PREPARING FOR SWITCHING + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % + [ 0.0864,0.0258,0.0152, ... %% state == 12 LOOKING FOR CONTACT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369]; % + zeros(1,ROBOT_DOF)]; %% THIS REFERENCE IS IGNORED + + +% YOGA MOVESET (joint references during state 4 and 10) +q1 = [-0.0790,0.2279, 0.4519, ... + -1.1621,0.6663, 0.4919, 0.9947, ... + -1.0717,1.2904,-0.2447, 1.0948, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3484,0.4008,-0.0004,-0.3672,-0.1060,-0.0875]; + +q2 = [-0.0790,0.1279, 0.4519, ... + -1.1621,0.6663, 0.4965, 0.9947, ... + -1.0717,1.2904,-0.2493, 1.0948, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... + 0.3714,0.9599, 1.3253,-1.6594,-0.1060,-0.0614]; + +q3 = [-0.0852,-0.3273,0.0821,... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... + 0.3714,0.9599, 1.3253,-1.6594, 0.2443,-0.0614]; + +q4 = [-0.0852,-0.4273,0.0821,... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.3473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; + +q5 = [-0.0790,-0.2273, 0.4519, ... + -1.1621,0.6663, 0.4965, 0.9947, ... + -1.0717,1.2904,-0.2493, 1.0948, ... + 0.2092, 0.4473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; + +q6 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; + +q7 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253, -1.6217, 0.2443,-0.0614]; + +q8 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; + +q9 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 0.0107,1.3253,-0.0189, 0.2443,-0.0614]; + +q10 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; + +q11 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; + +q12 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 0.3107,1.3253,-0.0189, 0.2443,-0.0614]; + +q13 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; + +q14 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 0.0107,1.3253,-0.0189, 0.2443,-0.0614]; + +q15 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 1.5514, 0.3107,1.3253,-0.0189, 0.2443,-0.0614]; + +q16 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.2514, 0.0107,1.3253,-0.0189, 0.2443,-0.0614]; + +q17 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + -0.3514, 0.3107,1.3253,-0.0189, 0.2443,-0.0614]; + +StateMachine.joints_leftYogaRef = [ 0, q1; + 1*StateMachine.jointsSmoothingTime(4),q2; + 2*StateMachine.jointsSmoothingTime(4),q3; + 3*StateMachine.jointsSmoothingTime(4),q4; + 4*StateMachine.jointsSmoothingTime(4),q5; + 5*StateMachine.jointsSmoothingTime(4),q6; + 6*StateMachine.jointsSmoothingTime(4),q7; + 7*StateMachine.jointsSmoothingTime(4),q8; + 8*StateMachine.jointsSmoothingTime(4),q9; + 9*StateMachine.jointsSmoothingTime(4),q10; + 10*StateMachine.jointsSmoothingTime(4),q11; + 11*StateMachine.jointsSmoothingTime(4),q12; + 12*StateMachine.jointsSmoothingTime(4),q13; + 13*StateMachine.jointsSmoothingTime(4),q14; + 14*StateMachine.jointsSmoothingTime(4),q15; + 15*StateMachine.jointsSmoothingTime(4),q16; + 16*StateMachine.jointsSmoothingTime(4),q17; + 17*StateMachine.jointsSmoothingTime(4),q10; + 18*StateMachine.jointsSmoothingTime(4),q11; + 19*StateMachine.jointsSmoothingTime(4),q12; + 20*StateMachine.jointsSmoothingTime(4),q13; + 21*StateMachine.jointsSmoothingTime(4),q14; + 22*StateMachine.jointsSmoothingTime(4),q15; + 23*StateMachine.jointsSmoothingTime(4),q16; + 24*StateMachine.jointsSmoothingTime(4),q17; + 25*StateMachine.jointsSmoothingTime(4),q8]; + +StateMachine.joints_rightYogaRef = StateMachine.joints_leftYogaRef; +StateMachine.joints_rightYogaRef(:,1) = [0; + 1*StateMachine.jointsSmoothingTime(10); + 2*StateMachine.jointsSmoothingTime(10); + 3*StateMachine.jointsSmoothingTime(10); + 4*StateMachine.jointsSmoothingTime(10); + 5*StateMachine.jointsSmoothingTime(10); + 6*StateMachine.jointsSmoothingTime(10); + 7*StateMachine.jointsSmoothingTime(10); + 8*StateMachine.jointsSmoothingTime(10); + 9*StateMachine.jointsSmoothingTime(10); + 10*StateMachine.jointsSmoothingTime(10); + 11*StateMachine.jointsSmoothingTime(10); + 12*StateMachine.jointsSmoothingTime(10); + 13*StateMachine.jointsSmoothingTime(10); + 14*StateMachine.jointsSmoothingTime(10); + 15*StateMachine.jointsSmoothingTime(10); + 16*StateMachine.jointsSmoothingTime(10); + 17*StateMachine.jointsSmoothingTime(10); + 18*StateMachine.jointsSmoothingTime(10); + 19*StateMachine.jointsSmoothingTime(10); + 20*StateMachine.jointsSmoothingTime(10); + 21*StateMachine.jointsSmoothingTime(10); + 22*StateMachine.jointsSmoothingTime(10); + 23*StateMachine.jointsSmoothingTime(10); + 24*StateMachine.jointsSmoothingTime(10); + 25*StateMachine.jointsSmoothingTime(10)]; + +% if the demo is not "yogaExtended", stop at the 8th move +if ~StateMachine.yogaExtended + + StateMachine.joints_leftYogaRef = StateMachine.joints_leftYogaRef(1:8,:); + StateMachine.joints_rightYogaRef = StateMachine.joints_rightYogaRef(1:8,:); + StateMachine.joints_leftYogaRef(8,1) = 15*StateMachine.jointsSmoothingTime(4); + StateMachine.joints_rightYogaRef(8,1) = 15*StateMachine.jointsSmoothingTime(10); +end + +% MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA +for i = 1:size(StateMachine.joints_rightYogaRef,1) + + StateMachine.joints_rightYogaRef(i,2:4) = [StateMachine.joints_rightYogaRef(i,2) -StateMachine.joints_rightYogaRef(i,3) -StateMachine.joints_rightYogaRef(i,4)]; + rightArm = StateMachine.joints_rightYogaRef(i,end-15:end-12); + StateMachine.joints_rightYogaRef(i,end-15:end-12) = StateMachine.joints_rightYogaRef(i,end-19:end-16); + StateMachine.joints_rightYogaRef(i,end-19:end-16) = rightArm; + rightLeg = StateMachine.joints_rightYogaRef(i,end-5:end); + StateMachine.joints_rightYogaRef(i,end-5:end) = StateMachine.joints_rightYogaRef(i,end-11:end-6); + StateMachine.joints_rightYogaRef(i,end-11:end-6) = rightLeg; +end + +%% References for CoM trajectory (COORDINATOR DEMO ONLY) + +% that the robot waits before starting the left-and-right +Config.noOscillationTime = 0; +Config.directionOfOscillation = [0;1;0]; +Config.amplitudeOfOscillation = 0.02; % [m] +Config.frequencyOfOscillation = 0.2; % [Hz] \ No newline at end of file diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/configRobot.m b/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova04/configRobot.m similarity index 70% rename from torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/configRobot.m rename to controllers/floating-base-balancing-torque-control/app/robots/iCubGenova04/configRobot.m index 6189885..677c848 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/configRobot.m +++ b/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova04/configRobot.m @@ -1,11 +1,10 @@ % CONFIGROBOT initializes parameters specific of a particular robot % (e.g., icuGazeboSim) -% %% --- Initialization --- Config.ON_GAZEBO = false; ROBOT_DOF = 23; -ROBOT_DOF_FOR_SIMULINK = eye(ROBOT_DOF); +Config.GRAV_ACC = 9.81; % Robot configuration for WBToolbox WBTConfigRobot = WBToolbox.Configuration; @@ -33,16 +32,16 @@ end % Frames list -Frames.BASE = 'root_link'; -Frames.IMU = 'imu_frame'; -Frames.LEFT_FOOT = 'l_sole'; -Frames.RIGHT_FOOT = 'r_sole'; -Frames.COM = 'com'; +Frames.BASE = 'root_link'; +Frames.IMU = 'imu_frame'; +Frames.LEFT_FOOT = 'l_sole'; +Frames.RIGHT_FOOT = 'r_sole'; +Frames.COM = 'com'; % Config.SATURATE_TORQUE_DERIVATIVE: if true, the derivative of the control % input is saturated. In this way, it is possible to reduce high frequency % oscillations and discontinuities in the control input. -Config.SATURATE_TORQUE_DERIVATIVE = true; +Config.SATURATE_TORQUE_DERIVATIVE = true; % if TRUE, the controller will STOP if the joints hit the joints limits % and/or if the (unsigned) difference between two consecutive joints @@ -68,32 +67,33 @@ % left foot do not move. Config.USE_IMU4EST_BASE = false; -% Config.YAW_IMU_FILTER and Config.PITCH_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 de to the IMU drift in estimating this angle. Hence, -% when either of the flags Config.YAW_IMU_FILTER or Config.PITCH_IMU_FILTER -% is set to true, then the yaw and/or pitch angles of the contact foot are -% ignored and kept equal to the initial values. -Config.FILTER_IMU_YAW = true; -Config.FILTER_IMU_PITCH = true; - -% Config.CORRECT_NECK_IMU: when set equal to true, the kineamtics from the +% 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). -Config.CORRECT_NECK_IMU = true; +% 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; +Config.USE_QP_SOLVER = true; % Ports name list -Ports.WRENCH_LEFT_FOOT = '/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o'; -Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o'; -Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; -Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; - -% Ports dimensions. It is necessary to set this variable in case the same -% port has different dimensions for different robots -Ports.NECK_POS_PORT_SIZE = 6; \ No newline at end of file +Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; +Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; +Ports.WRENCH_LEFT_FOOT = '/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o'; +Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o'; + +% Ports dimensions +Ports.NECK_POS_PORT_SIZE = 6; +Ports.IMU_PORT_SIZE = 12; +Ports.IMU_PORT_ORIENTATION_INDEX = [1,2,3]; +Ports.WRENCH_PORT_SIZE = 6; \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova04/configStateMachine.m b/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova04/configStateMachine.m new file mode 100644 index 0000000..6c1d178 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova04/configStateMachine.m @@ -0,0 +1,160 @@ +% CONFIGSTATEMACHINE configures the state machine (type of demo, velocity +% of the demo, repeat movements, and so on). + +%% --- Initialization --- + +% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) +Config.LEFT_RIGHT_MOVEMENTS = false; + +% If equal to one, the desired values of the center of mass are smoothed internally +Config.SMOOTH_COM_DES = true; + +% If equal to one, the desired values of the postural tasks are smoothed internally +Config.SMOOTH_JOINT_DES = true; + +% Joint torques saturation [Nm] +Sat.torque = 60; + +% Joint torques rate of change saturation +Sat.uDotMax = 300; + +% max unsigned difference between two consecutive (measured) joint positions, +% i.e. delta_qj = abs(qj(k) - qj(k-1)) +Sat.maxJointsPositionDelta = 15*pi/180; % [rad] + +%% Regularization parameters +Reg.pinvDamp_baseVel = 1e-7; +Reg.pinvDamp = 0.07; +Reg.pinvTol = 1e-5; +Reg.KP_postural = 0.1; +Reg.KD_postural = 0; +Reg.HessianQP = 1e-7; + +%% State Machine configuration + +% time between two yoga positions +StateMachine.joints_pauseBetweenYogaMoves = 5; + +% contact forces threshold +StateMachine.wrench_thresholdContactOn = 50; +StateMachine.wrench_thresholdContactOff = 100; + +% threshold on CoM and joints error +StateMachine.CoM_threshold = 0.02; +StateMachine.joints_thresholdNotInContact = 15; +StateMachine.joints_thresholdInContact = 50; + +% initial state for state machine +StateMachine.initialState = 1; + +% other configuration parameters for state machine +StateMachine.tBalancing = 1; +StateMachine.tBalancingBeforeYoga = 1; +StateMachine.yogaExtended = true; +StateMachine.skipYoga = false; +StateMachine.demoOnlyBalancing = false; +StateMachine.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first +StateMachine.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot (not available for now) + +%%%% List of possible "Yoga in loop" %%%% + +% the robot will repeat the FULL DEMO (two feet balancing, yoga on left +% foot, back on two feet, yoga right foot, back on two feet). The demo is +% repeated until the user stops the Simulink model. This option is ignored +% if Sm.demoStartsOnRightSupport = true. +StateMachine.twoFeetYogaInLoop = false; + +% the robot will repeat the ONE FOOT yoga for the number of times the user +% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two +% feet balancing in between to consecutive yoga. WARNING: if the option +% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga +% on left foot for the number of times the user specifies in the Sm.yogaCounter, +% and then it will repeat the yoga on the right foot for the same number of times. +StateMachine.oneFootYogaInLoop = false; +StateMachine.yogaCounter = 5; + +%% Parameters for motors reflected inertia + +% transmission ratio (1/N) +Config.Gamma = 0.01*eye(ROBOT_DOF); + +% modify the value of the transmission ratio for the hip pitch. +% TODO: avoid to hard-code the joint numbering +Config.Gamma(end-5, end-5) = 0.0067; +Config.Gamma(end-11,end-11) = 0.0067; + +% motors inertia (Kg*m^2) +legsMotors_I_m = 0.0827*1e-4; +torsoPitchRollMotors_I_m = 0.0827*1e-4; +torsoYawMotors_I_m = 0.0585*1e-4; +armsMotors_I_m = 0.0585*1e-4; + +% add harmonic drives reflected inertia +if Config.INCLUDE_HARMONIC_DRIVE_INERTIA + + legsMotors_I_m = legsMotors_I_m + 0.054*1e-4; + torsoPitchRollMotors_I_m = torsoPitchRollMotors_I_m + 0.054*1e-4; + torsoYawMotors_I_m = torsoYawMotors_I_m + 0.021*1e-4; + armsMotors_I_m = armsMotors_I_m + 0.021*1e-4; +end + +Config.I_m = diag([torsoPitchRollMotors_I_m*ones(2,1); + torsoYawMotors_I_m; + armsMotors_I_m*ones(8,1); + legsMotors_I_m*ones(12,1)]); + +% parameters for coupling matrices. Updated according to the wiki: +% +% http://wiki.icub.org/wiki/ICub_coupled_joints +% +% and corrected according to https://github.com/robotology/robots-configuration/issues/39 +t = 0.615; +r = 0.022; +R = 0.04; + +% coupling matrices +T_LShoulder = [-1 0 0; + -1 -t 0; + 0 t -t]; + +T_RShoulder = [ 1 0 0; + 1 t 0; + 0 -t t]; + +T_torso = [ 0.5 -0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; + +if Config.INCLUDE_COUPLING + + Config.T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12)); +else + Config.T = eye(ROBOT_DOF); +end + +% gain for feedforward term in joint torques calculation. Valid range: a +% value between 0 and 1 +Config.K_ff = 1; + +% Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA if true, the desired joints +% accelerations are used for computing the feedforward term in joint +% torques calculations. Not effective if Config.K_ff = 0. +Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA = true; + +%% Constraints for QP for balancing + +% The friction cone is approximated by using linear interpolation of the circle. +% So, numberOfPoints defines the number of points used to interpolate the circle +% in each cicle's quadrant +numberOfPoints = 4; +forceFrictionCoefficient = 1/3; +torsionalFrictionCoefficient = 1/75; +fZmin = 10; + +% physical size of the foot +feet_size = [-0.07 0.12 ; % xMin, xMax + -0.045 0.05 ]; % yMin, yMax + +% Compute contact constraints (friction cone, unilateral constraints) +[ConstraintsMatrix, bVectorConstraints] = wbc.computeRigidContactConstraints ... + (forceFrictionCoefficient, numberOfPoints, torsionalFrictionCoefficient, feet_size, fZmin); \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova04/gainsAndReferences.m b/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova04/gainsAndReferences.m new file mode 100644 index 0000000..87758ee --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/app/robots/iCubGenova04/gainsAndReferences.m @@ -0,0 +1,351 @@ +% GAINSANDREFERENCES compute gains matrices, references and other control +% related quantities for each state of the state machine. + +%% --- Initialization --- + +% CoM gains +Gain.KP_CoM = [50 100 5 % state == 1 TWO FEET BALANCING + 50 100 5 % state == 2 COM TRANSITION TO LEFT + 50 100 5 % state == 3 LEFT FOOT BALANCING + 50 100 5 % state == 4 YOGA LEFT FOOT + 50 100 5 % state == 5 PREPARING FOR SWITCHING + 50 100 5 % state == 6 LOOKING FOR CONTACT + 50 100 5 % state == 7 TRANSITION TO INITIAL POSITION + 50 150 5 % state == 8 COM TRANSITION TO RIGHT FOOT + 50 100 5 % state == 9 RIGHT FOOT BALANCING + 50 100 5 % state == 10 YOGA RIGHT FOOT + 50 100 5 % state == 11 PREPARING FOR SWITCHING + 50 100 5 % state == 12 LOOKING FOR CONTACT + 50 100 5];% state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_CoM = 2*sqrt(Gain.KP_CoM)/15; + +% Angular momentum gains +Gain.KI_AngularMomentum = 3 ; +Gain.KP_AngularMomentum = 2*sqrt(Gain.KI_AngularMomentum)/5; + +% Postural task gains +% % TORSO %% LEFT ARM %% RIGHT ARM %% LEFT LEG %% RIGHT LEG %% +Gain.KP_postural = [10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 1 TWO FEET BALANCING + 10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 2 COM TRANSITION TO LEFT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 100 100, 30 30 20 20 100 100 % state == 3 LEFT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10,100 200 100 400 100 100,100 50 30 50 100 100 % state == 4 YOGA LEFT FOOT + 30 30 30, 5 5 10 10, 10 10 20 10,200 250 20 20 10 10,220 550 220 200 65 300 % state == 5 PREPARING FOR SWITCHING + 30 30 30, 10 10 20 10, 10 10 20 10,100 350 20 200 10 100,220 550 220 200 65 300 % state == 6 LOOKING FOR CONTACT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 5 5, 30 30 30 20 5 5 % state == 7 TRANSITION TO INITIAL POSITION + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 100 100, 30 30 30 20 100 100 % state == 8 COM TRANSITION TO RIGHT FOOT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 100 100, 30 30 20 20 100 100 % state == 9 RIGHT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10,100 50 30 50 100 100,100 200 100 100 10 10 % state == 10 YOGA RIGHT FOOT + 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,200 250 20 20 10 10 % state == 11 PREPARING FOR SWITCHING + 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100 % state == 12 LOOKING FOR CONTACT + 30 40 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100];% state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_postural = 0*sqrt(Gain.KP_postural(1,:))/20; + +% symmetric gains +Gain.KP_postural(4,1:3) = Gain.KP_postural(4,1:3)*3; +Gain.KP_postural(6,18:23) = Gain.KP_postural(6,18:23)*2; +Gain.KP_postural(10,1:3) = Gain.KP_postural(10,1:3)*3; +Gain.KP_postural(13,1:3) = Gain.KP_postural(13,1:3)*3; + +%% Smoothing times + +% Smoothing time gain scheduling +Config.SmoothingTimeGainScheduling = 2; + +% Smoothing time CoM references +StateMachine.CoMSmoothingTime = [1; %% state == 1 TWO FEET BALANCING + 1; %% state == 2 COM TRANSITION TO LEFT FOOT + 1; %% state == 3 LEFT FOOT BALANCING + 0.9; %% state == 4 YOGA LEFT FOOT + 2; %% state == 5 PREPARING FOR SWITCHING + 2; %% state == 6 LOOKING FOR CONTACT + 1; %% state == 7 TRANSITION INIT POSITION + 1; %% state == 8 COM TRANSITION TO RIGHT FOOT + 1; %% state == 9 RIGHT FOOT BALANCING + 0.9; %% state == 10 YOGA RIGHT FOOT + 2; %% state == 11 PREPARING FOR SWITCHING + 2; %% state == 12 LOOKING FOR CONTACT + 5]; %% state == 13 TRANSITION INIT POSITION + + +% Smoothing time for joints references +StateMachine.jointsSmoothingTime = [1; %% state == 1 TWO FEET BALANCING + 1; %% state == 2 COM TRANSITION TO LEFT FOOT + 1; %% state == 3 LEFT FOOT BALANCING + 0.9; %% state == 4 YOGA LEFT FOOT + 2; %% state == 5 PREPARING FOR SWITCHING + 2; %% state == 6 LOOKING FOR CONTACT + 1; %% state == 7 TRANSITION INIT POSITION + 1; %% state == 8 COM TRANSITION TO RIGHT FOOT + 1; %% state == 9 RIGHT FOOT BALANCING + 0.9; %% state == 10 YOGA RIGHT FOOT + 2; %% state == 11 PREPARING FOR SWITCHING + 2; %% state == 12 LOOKING FOR CONTACT + 5]; %% state == 13 TRANSITION INIT POSITION + + +% scale factor smoothing time multiplies the smoothing factor during the +% Yoga (state 4 and 10). The purpose is to reduce the time necessary for +% the reference to converge to the next position, but without changing also +% the valuse stored in Sm.joints_leftYogaRef/Sm.joints_rightYogaRef +StateMachine.scaleFactorSmoothingTime = 0.9; + +%% CoM delta + +% To be summed to the reference CoM position +StateMachine.CoM_delta = [% THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE LEFT FOOT + 0.0, 0.00, 0.0; %% NOT USED + 0.0, 0.00, 0.0; %% state == 2 COM TRANSITION TO LEFT FOOT + 0.0, 0.005, 0.0; %% state == 3 LEFT FOOT BALANCING + 0.0, 0.005, 0.0; %% state == 4 YOGA LEFT FOOT + 0.0, 0.005, 0.0; %% state == 5 PREPARING FOR SWITCHING + 0.02,-0.09, 0.0; %% state == 6 LOOKING FOR CONTACT + 0.0, 0.00, 0.0; %% state == 7 TWO FEET BALANCING + % THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE RIGHT FOOT + 0.0, 0.00, 0.0; %% state == 8 COM TRANSITION TO RIGHT FOOT + 0.0, -0.005, 0.0; %% state == 9 RIGHT FOOT BALANCING + 0.0, -0.005, 0.0; %% state == 10 YOGA RIGHT FOOT + 0.0, -0.015, 0.0; %% state == 11 PREPARING FOR SWITCHING + 0.02, 0.02, 0.0; %% state == 12 LOOKING FOR CONTACT + 0.0, 0.00, 0.0]; %% NOT USED + +%% Joint references +StateMachine.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 2 COM TRANSITION TO LEFT + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % + [ 0.0864,0.0258,0.0152, ... %% state == 3 LEFT FOOT BALANCING + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,-0.1060,-0.1151]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 5 PREPARING FOR SWITCHING + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % + [ 0.0864,0.0258,0.0152, ... %% state == 6 LOOKING FOR CONTACT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [ 0.0864,0.0258,0.0152, ... %% state == 8 COM TRANSITION TO RIGHT FOOT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % + [ 0.0864,0.0258,0.0152, ... %% state == 9 RIGHT FOOT BALANCING + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 11 PREPARING FOR SWITCHING + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % + [ 0.0864,0.0258,0.0152, ... %% state == 12 LOOKING FOR CONTACT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369]; % + zeros(1,ROBOT_DOF)]; %% THIS REFERENCE IS IGNORED + +% YOGA MOVESET (joint references during state 4 and 10) +q1 = [-0.0790,0.2279, 0.4519, ... + -1.1621,0.6663, 0.4919, 0.9947, ... + -1.0717,1.2904,-0.2447, 1.0948, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3484,0.4008,-0.0004,-0.3672,-0.1060,-0.0875]; + +q2 = [-0.0790,0.1279, 0.4519, ... + -1.1621,0.6663, 0.4965, 0.9947, ... + -1.0717,1.2904,-0.2493, 1.0948, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... + 0.3714,0.9599, 1.3253,-1.6594,-0.1060,-0.0614]; + +q3 = [-0.0852,-0.3273,0.0821,... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... + 0.3714,0.9599, 1.3253,-1.6594, 0.5000,-0.0614]; + +q4 = [-0.0852,-0.4273,0.0821,... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.3473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q5 = [-0.0790,-0.2273, 0.4519, ... + -1.1621,0.6663, 0.4965, 0.9947, ... + -1.0717,1.2904,-0.2493, 1.0948, ... + 0.2092, 0.4473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q6 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q7 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253, -1.6217, 0.5000,-0.0614]; + +q8 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q9 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614]; + +q10 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q11 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q12 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q13 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q14 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.8514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614]; + +q15 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 1.5514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q16 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.2514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614]; + +q17 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + -0.3514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q18 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +StateMachine.joints_leftYogaRef = [ 0, q1; + 1*StateMachine.jointsSmoothingTime(4),q2; + 2*StateMachine.jointsSmoothingTime(4),q3; + 3*StateMachine.jointsSmoothingTime(4),q4; + 4*StateMachine.jointsSmoothingTime(4),q5; + 5*StateMachine.jointsSmoothingTime(4),q6; + 6*StateMachine.jointsSmoothingTime(4),q7; + 7*StateMachine.jointsSmoothingTime(4),q8; + 8*StateMachine.jointsSmoothingTime(4),q9; + 9*StateMachine.jointsSmoothingTime(4),q10; + 10*StateMachine.jointsSmoothingTime(4),q11; + 11*StateMachine.jointsSmoothingTime(4),q12; + 12*StateMachine.jointsSmoothingTime(4),q13; + 13*StateMachine.jointsSmoothingTime(4),q14; + 14*StateMachine.jointsSmoothingTime(4),q15; + 15*StateMachine.jointsSmoothingTime(4),q16; + 16*StateMachine.jointsSmoothingTime(4),q17; + 17*StateMachine.jointsSmoothingTime(4),q10; + 18*StateMachine.jointsSmoothingTime(4),q11; + 19*StateMachine.jointsSmoothingTime(4),q12; + 20*StateMachine.jointsSmoothingTime(4),q13; + 21*StateMachine.jointsSmoothingTime(4),q14; + 22*StateMachine.jointsSmoothingTime(4),q15; + 23*StateMachine.jointsSmoothingTime(4),q16; + 24*StateMachine.jointsSmoothingTime(4),q17; + 25*StateMachine.jointsSmoothingTime(4),q18]; + +StateMachine.joints_rightYogaRef = StateMachine.joints_leftYogaRef; +StateMachine.joints_rightYogaRef(:,1) = [0; + 1*StateMachine.jointsSmoothingTime(10); + 2*StateMachine.jointsSmoothingTime(10); + 3*StateMachine.jointsSmoothingTime(10); + 4*StateMachine.jointsSmoothingTime(10); + 5*StateMachine.jointsSmoothingTime(10); + 6*StateMachine.jointsSmoothingTime(10); + 7*StateMachine.jointsSmoothingTime(10); + 8*StateMachine.jointsSmoothingTime(10); + 9*StateMachine.jointsSmoothingTime(10); + 10*StateMachine.jointsSmoothingTime(10); + 11*StateMachine.jointsSmoothingTime(10); + 12*StateMachine.jointsSmoothingTime(10); + 13*StateMachine.jointsSmoothingTime(10); + 14*StateMachine.jointsSmoothingTime(10); + 15*StateMachine.jointsSmoothingTime(10); + 16*StateMachine.jointsSmoothingTime(10); + 17*StateMachine.jointsSmoothingTime(10); + 18*StateMachine.jointsSmoothingTime(10); + 19*StateMachine.jointsSmoothingTime(10); + 20*StateMachine.jointsSmoothingTime(10); + 21*StateMachine.jointsSmoothingTime(10); + 22*StateMachine.jointsSmoothingTime(10); + 23*StateMachine.jointsSmoothingTime(10); + 24*StateMachine.jointsSmoothingTime(10); + 25*StateMachine.jointsSmoothingTime(10)]; + +% if the demo is not "yogaExtended", stop at the 8th move +if ~StateMachine.yogaExtended + + StateMachine.joints_leftYogaRef = StateMachine.joints_leftYogaRef(1:8,:); + StateMachine.joints_rightYogaRef = StateMachine.joints_rightYogaRef(1:8,:); + StateMachine.joints_leftYogaRef(8,1) = 15*StateMachine.jointsSmoothingTime(4); + StateMachine.joints_rightYogaRef(8,1) = 15*StateMachine.jointsSmoothingTime(10); +end + +% MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA +for i = 1:size(StateMachine.joints_rightYogaRef,1) + + StateMachine.joints_rightYogaRef(i,2:4) = [StateMachine.joints_rightYogaRef(i,2) -StateMachine.joints_rightYogaRef(i,3) -StateMachine.joints_rightYogaRef(i,4)]; + rightArm = StateMachine.joints_rightYogaRef(i,end-15:end-12); + StateMachine.joints_rightYogaRef(i,end-15:end-12) = StateMachine.joints_rightYogaRef(i,end-19:end-16); + StateMachine.joints_rightYogaRef(i,end-19:end-16) = rightArm; + rightLeg = StateMachine.joints_rightYogaRef(i,end-5:end); + StateMachine.joints_rightYogaRef(i,end-5:end) = StateMachine.joints_rightYogaRef(i,end-11:end-6); + StateMachine.joints_rightYogaRef(i,end-11:end-6) = rightLeg; +end + +%% References for CoM trajectory (COORDINATOR DEMO ONLY) + +% that the robot waits before starting the left-and-right +Config.noOscillationTime = 0; +Config.directionOfOscillation = [0;1;0]; +Config.amplitudeOfOscillation = 0.02; % [m] +Config.frequencyOfOscillation = 0.2; % [Hz] \ No newline at end of file diff --git a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/configRobot.m b/controllers/floating-base-balancing-torque-control/app/robots/icubGazeboSim/configRobot.m similarity index 70% rename from torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/configRobot.m rename to controllers/floating-base-balancing-torque-control/app/robots/icubGazeboSim/configRobot.m index 4dc212f..81647ff 100644 --- a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/configRobot.m +++ b/controllers/floating-base-balancing-torque-control/app/robots/icubGazeboSim/configRobot.m @@ -1,11 +1,10 @@ % CONFIGROBOT initializes parameters specific of a particular robot % (e.g., icuGazeboSim) -% %% --- Initialization --- Config.ON_GAZEBO = true; ROBOT_DOF = 23; -ROBOT_DOF_FOR_SIMULINK = eye(ROBOT_DOF); +Config.GRAV_ACC = 9.81; % Robot configuration for WBToolbox WBTConfigRobot = WBToolbox.Configuration; @@ -33,11 +32,11 @@ end % Frames list -Frames.BASE = 'root_link'; -Frames.IMU = 'imu_frame'; -Frames.LEFT_FOOT = 'l_sole'; -Frames.RIGHT_FOOT = 'r_sole'; -Frames.COM = 'com'; +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 @@ -66,34 +65,35 @@ % 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 and Config.PITCH_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 de to the IMU drift in estimating this angle. Hence, -% when either of the flags Config.YAW_IMU_FILTER or Config.PITCH_IMU_FILTER -% is set to true, then the yaw and/or pitch angles of the contact foot are -% ignored and kept equal to the initial values. -Config.FILTER_IMU_YAW = true; -Config.FILTER_IMU_PITCH = true; - -% Config.CORRECT_NECK_IMU: when set equal to true, the kineamtics from the +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). -Config.CORRECT_NECK_IMU = true; +% 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; +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. It is necessary to set this variable in case the same -% port has different dimensions for different robots -Ports.NECK_POS_PORT_SIZE = 6; \ No newline at end of file +Ports.WRENCH_LEFT_FOOT = '/wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o'; +Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o'; +Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; +Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; + +% Ports dimensions +Ports.NECK_POS_PORT_SIZE = 6; +Ports.IMU_PORT_SIZE = 12; +Ports.IMU_PORT_ORIENTATION_INDEX = [1,2,3]; +Ports.WRENCH_PORT_SIZE = 6; \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control/app/robots/icubGazeboSim/configStateMachine.m b/controllers/floating-base-balancing-torque-control/app/robots/icubGazeboSim/configStateMachine.m new file mode 100644 index 0000000..edc45cc --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/app/robots/icubGazeboSim/configStateMachine.m @@ -0,0 +1,159 @@ +% CONFIGSTATEMACHINE configures the state machine (type of demo, velocity +% of the demo, repeat movements, and so on). + +%% --- Initialization --- + +% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) +Config.LEFT_RIGHT_MOVEMENTS = false; + +% If equal to one, the desired values of the center of mass are smoothed internally +Config.SMOOTH_COM_DES = true; + +% If equal to one, the desired values of the postural tasks are smoothed internally +Config.SMOOTH_JOINT_DES = true; + +% Joint torques saturation [Nm] +Sat.torque = 60; + +% Joint torques rate of change saturation +Sat.uDotMax = 300; + +% max unsigned difference between two consecutive (measured) joint positions, +% i.e. delta_qj = abs(qj(k) - qj(k-1)) +Sat.maxJointsPositionDelta = 15*pi/180; % [rad] + +%% Regularization parameters +Reg.pinvDamp_baseVel = 1e-7; +Reg.pinvDamp = 1; +Reg.pinvTol = 1e-5; +Reg.KP_postural = 0.1; +Reg.KD_postural = 0; +Reg.HessianQP = 1e-7; + +%% State Machine configuration + +% time between two yoga positions +StateMachine.joints_pauseBetweenYogaMoves = 3; + +% contact forces threshold +StateMachine.wrench_thresholdContactOn = 25; +StateMachine.wrench_thresholdContactOff = 85; + +% threshold on CoM and joints error +StateMachine.CoM_threshold = 0.01; +StateMachine.joints_thresholdNotInContact = 5; +StateMachine.joints_thresholdInContact = 50; + +% initial state for state machine +StateMachine.initialState = 1; + +% other configuration parameters for state machine +StateMachine.tBalancing = 1; +StateMachine.tBalancingBeforeYoga = 1; +StateMachine.skipYoga = false; +StateMachine.demoOnlyBalancing = false; +StateMachine.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first +StateMachine.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot (not available for now) + +%%%% List of possible "Yoga in loop" %%%% + +% the robot will repeat the FULL DEMO (two feet balancing, yoga on left +% foot, back on two feet, yoga right foot, back on two feet). The demo is +% repeated until the user stops the Simulink model. This option is ignored +% if Sm.demoStartsOnRightSupport = true. +StateMachine.twoFeetYogaInLoop = false; + +% the robot will repeat the ONE FOOT yoga for the number of times the user +% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two +% feet balancing in between to consecutive yoga. WARNING: if the option +% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga +% on left foot for the number of times the user specifies in the Sm.yogaCounter, +% and then it will repeat the yoga on the right foot for the same number of times. +StateMachine.oneFootYogaInLoop = false; +StateMachine.yogaCounter = 5; + +%% Parameters for motors reflected inertia + +% transmission ratio (1/N) +Config.Gamma = 0.01*eye(ROBOT_DOF); + +% modify the value of the transmission ratio for the hip pitch. +% TODO: avoid to hard-code the joint numbering +Config.Gamma(end-5, end-5) = 0.0067; +Config.Gamma(end-11,end-11) = 0.0067; + +% motors inertia (Kg*m^2) +legsMotors_I_m = 0.0827*1e-4; +torsoPitchRollMotors_I_m = 0.0827*1e-4; +torsoYawMotors_I_m = 0.0585*1e-4; +armsMotors_I_m = 0.0585*1e-4; + +% add harmonic drives reflected inertia +if Config.INCLUDE_HARMONIC_DRIVE_INERTIA + + legsMotors_I_m = legsMotors_I_m + 0.054*1e-4; + torsoPitchRollMotors_I_m = torsoPitchRollMotors_I_m + 0.054*1e-4; + torsoYawMotors_I_m = torsoYawMotors_I_m + 0.021*1e-4; + armsMotors_I_m = armsMotors_I_m + 0.021*1e-4; +end + +Config.I_m = diag([torsoPitchRollMotors_I_m*ones(2,1); + torsoYawMotors_I_m; + armsMotors_I_m*ones(8,1); + legsMotors_I_m*ones(12,1)]); + +% parameters for coupling matrices. Updated according to the wiki: +% +% http://wiki.icub.org/wiki/ICub_coupled_joints +% +% and corrected according to https://github.com/robotology/robots-configuration/issues/39 +t = 0.615; +r = 0.022; +R = 0.04; + +% coupling matrices +T_LShoulder = [-1 0 0; + -1 -t 0; + 0 t -t]; + +T_RShoulder = [ 1 0 0; + 1 t 0; + 0 -t t]; + +T_torso = [ 0.5 -0.5 0; + 0.5 0.5 0; + r/(2*R) r/(2*R) r/R]; + +if Config.INCLUDE_COUPLING + + Config.T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12)); +else + Config.T = eye(ROBOT_DOF); +end + +% gain for feedforward term in joint torques calculation. Valid range: a +% value between 0 and 1 +Config.K_ff = 0; + +% Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA if true, the desired joints +% accelerations are used for computing the feedforward term in joint +% torques calculations. Not effective if Config.K_ff = 0. +Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA = false; + +%% Constraints for QP for balancing + +% The friction cone is approximated by using linear interpolation of the circle. +% So, numberOfPoints defines the number of points used to interpolate the circle +% in each cicle's quadrant +numberOfPoints = 4; +forceFrictionCoefficient = 1/3; +torsionalFrictionCoefficient = 1/75; +fZmin = 10; + +% physical size of the foot +feet_size = [-0.05 0.10; % xMin, xMax + -0.025 0.025]; % yMin, yMax + +% Compute contact constraints (friction cone, unilateral constraints) +[ConstraintsMatrix, bVectorConstraints] = wbc.computeRigidContactConstraints ... + (forceFrictionCoefficient, numberOfPoints, torsionalFrictionCoefficient, feet_size, fZmin); \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control/app/robots/icubGazeboSim/gainsAndReferences.m b/controllers/floating-base-balancing-torque-control/app/robots/icubGazeboSim/gainsAndReferences.m new file mode 100644 index 0000000..3f85427 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/app/robots/icubGazeboSim/gainsAndReferences.m @@ -0,0 +1,238 @@ +% GAINSANDREFERENCES compute gains matrices, references and other control +% related quantities for each state of the state machine. + +%% --- Initialization --- + +% CoM gains +Gain.KP_CoM = [10 50 10 % state == 1 TWO FEET BALANCING + 10 50 10 % state == 2 COM TRANSITION TO LEFT + 10 50 10 % state == 3 LEFT FOOT BALANCING + 10 50 10 % state == 4 YOGA LEFT FOOT + 10 50 10 % state == 5 PREPARING FOR SWITCHING + 10 50 10 % state == 6 LOOKING FOR CONTACT + 10 50 10 % state == 7 TRANSITION TO INITIAL POSITION + 10 50 10 % state == 8 COM TRANSITION TO RIGHT FOOT + 10 50 10 % state == 9 RIGHT FOOT BALANCING + 10 50 10 % state == 10 YOGA RIGHT FOOT + 10 50 10 % state == 11 PREPARING FOR SWITCHING + 10 50 10 % state == 12 LOOKING FOR CONTACT + 10 50 10]; % state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_CoM = 2*sqrt(Gain.KP_CoM); + +% Angular momentum gains +Gain.KI_AngularMomentum = 0.25 ; +Gain.KP_AngularMomentum = 2*sqrt(Gain.KI_AngularMomentum); + +% Postural task gains +% % TORSO %% LEFT ARM %% RIGHT ARM %% LEFT LEG %% RIGHT LEG %% +Gain.KP_postural = [10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 1 TWO FEET BALANCING + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 2 COM TRANSITION TO LEFT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 3 LEFT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10, 50 50 250 200 50 50, 50 50 50 50 50 50 % state == 4 YOGA LEFT FOOT + 30 30 30, 10 10 10 10, 10 10 10 10, 30 50 300 60 50 50, 30 50 30 60 50 50 % state == 5 PREPARING FOR SWITCHING + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 6 LOOKING FOR CONTACT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 7 TRANSITION TO INITIAL POSITION + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 8 COM TRANSITION TO RIGHT FOOT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 9 RIGHT FOOT BALANCING + 30 30 30, 10 10 10 10, 10 10 10 10, 50 50 50 50 50 50, 50 50 250 200 50 50 % state == 10 YOGA RIGHT FOOT + 30 30 30, 10 10 10 10, 10 10 10 10, 30 50 30 60 50 50, 30 50 300 60 50 50 % state == 11 PREPARING FOR SWITCHING + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 12 LOOKING FOR CONTACT + 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50];% state == 13 TRANSITION TO INITIAL POSITION + +Gain.KD_postural = 0*sqrt(Gain.KP_postural(1,:)); + +%% Smoothing times + +% Smoothing time gain scheduling +Config.SmoothingTimeGainScheduling = 2; + +% Smoothing time CoM references +StateMachine.CoMSmoothingTime = [5; %% state == 1 TWO FEET BALANCING + 5; %% state == 2 COM TRANSITION TO LEFT FOOT + 3; %% state == 3 LEFT FOOT BALANCING + 4; %% state == 4 YOGA LEFT FOOT + 5; %% state == 5 PREPARING FOR SWITCHING + 5; %% state == 6 LOOKING FOR CONTACT + 4; %% state == 7 TRANSITION INIT POSITION + 5; %% state == 8 COM TRANSITION TO RIGHT FOOT + 3; %% state == 9 RIGHT FOOT BALANCING + 4; %% state == 10 YOGA RIGHT FOOT + 5; %% state == 11 PREPARING FOR SWITCHING + 5; %% state == 12 LOOKING FOR CONTACT + 4]; %% state == 13 TRANSITION INIT POSITION + +% Smoothing time for joints references +StateMachine.jointsSmoothingTime = [5; %% state == 1 TWO FEET BALANCING + 5; %% state == 2 COM TRANSITION TO LEFT FOOT + 3; %% state == 3 LEFT FOOT BALANCING + 4; %% state == 4 YOGA LEFT FOOT + 5; %% state == 5 PREPARING FOR SWITCHING + 5; %% state == 6 LOOKING FOR CONTACT + 4; %% state == 7 TRANSITION INIT POSITION + 5; %% state == 8 COM TRANSITION TO RIGHT FOOT + 3; %% state == 9 RIGHT FOOT BALANCING + 4; %% state == 10 YOGA RIGHT FOOT + 5; %% state == 11 PREPARING FOR SWITCHING + 5; %% state == 12 LOOKING FOR CONTACT + 4]; %% state == 13 TRANSITION INIT POSITION + +% scale factor smoothing time multiplies the smoothing factor during the +% Yoga (state 4 and 10). The purpose is to reduce the time necessary for +% the reference to converge to the next position, but without changing also +% the valuse stored in Sm.joints_leftYogaRef/Sm.joints_rightYogaRef +StateMachine.scaleFactorSmoothingTime = 0.9; + +%% CoM delta + +% To be summed to the reference CoM position +StateMachine.CoM_delta = [% THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE LEFT FOOT + 0.0, 0.00, 0.0; %% NOT USED + 0.0, 0.01, 0.0; %% state == 2 COM TRANSITION TO LEFT FOOT + 0.0, 0.00, 0.0; %% state == 3 LEFT FOOT BALANCING + 0.0, 0.01, 0.0; %% state == 4 YOGA LEFT FOOT + 0.0, 0.00, 0.0; %% state == 5 PREPARING FOR SWITCHING + 0.0, -0.09, 0.0; %% state == 6 LOOKING FOR CONTACT + 0.0, 0.00, 0.0; %% state == 7 TWO FEET BALANCING + % THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE RIGHT FOOT + 0.0, -0.01, 0.0; %% state == 8 COM TRANSITION TO RIGHT FOOT + 0.0, 0.00, 0.0; %% state == 9 RIGHT FOOT BALANCING + 0.0, -0.00, 0.0; %% state == 10 YOGA RIGHT FOOT + 0.0, 0.00, 0.0; %% state == 11 PREPARING FOR SWITCHING + 0.0, 0.09, 0.0; %% state == 12 LOOKING FOR CONTACT + 0.0, 0.00, 0.0]; %% NOT USED + +%% Joint references +StateMachine.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 2 COM TRANSITION TO LEFT + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % + [ 0.0864,0.0258,0.0152, ... %% state == 3 LEFT FOOT BALANCING + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 5 PREPARING FOR SWITCHING + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % + [ 0.0864,0.0258,0.0152, ... %% state == 6 LOOKING FOR CONTACT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [ 0.0864,0.0258,0.0152, ... %% state == 8 COM TRANSITION TO RIGHT FOOT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % + [ 0.0864,0.0258,0.0152, ... %% state == 9 RIGHT FOOT BALANCING + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % + zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED + [-0.0348,0.0779,0.0429, ... %% state == 11 PREPARING FOR SWITCHING + -0.1493,0.8580,0.2437,0.8710, ... % + -0.1493,0.8580,0.2437,0.8710, ... % + 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % + -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % + [ 0.0864,0.0258,0.0152, ... %% state == 12 LOOKING FOR CONTACT + 0.1253,0.8135,0.3051,0.7928, ... % + 0.0563,0.6789,0.3340,0.6214, ... % + -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277, ... % + 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369]; % + zeros(1,ROBOT_DOF)]; %% THIS REFERENCE IS IGNORED + +% YOGA MOVESET (joint references during state 4 and 10) +q1 = [-0.0790,0.2279, 0.4519, ... + -1.1621,0.6663, 0.4919, 0.9947, ... + -1.0717,1.2904,-0.2447, 1.0948, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3484,0.4008,-0.0004,-0.3672,-0.1060,-0.0875]; + +q2 = [-0.0790,0.2279, 0.4519, ... + -1.1621,0.6663, 0.4965, 0.9947, ... + -1.0717,1.2904,-0.2493, 1.0948, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... + 0.3714,0.9599, 1.3253,-1.6594,-0.1060,-0.0614]; + +q3 = [-0.0852,-0.4273,0.0821,... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... + 0.3714,0.9599, 1.3253,-1.6594, 0.5000,-0.0614]; + +q4 = [-0.0852,-0.4273,0.0821,... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.3473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q5 = [-0.0790,-0.2273, 0.4519, ... + -1.1621,0.6663, 0.4965, 0.9947, ... + -1.0717,1.2904,-0.2493, 1.0948, ... + 0.2092, 0.4473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q6 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +q7 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253, -1.6217, 0.5000,-0.0614]; + +q8 = [-0.0852,-0.4273,0.0821, ... + 0.1391, 1.4585,0.2464, 0.3042, ... + -0.4181, 1.6800,0.7373, 0.3031, ... + 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... + 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; + +StateMachine.joints_leftYogaRef = [ 0, q1; + 1*StateMachine.jointsSmoothingTime(4),q2; + 2*StateMachine.jointsSmoothingTime(4),q3; + 3*StateMachine.jointsSmoothingTime(4),q4; + 4*StateMachine.jointsSmoothingTime(4),q5; + 5*StateMachine.jointsSmoothingTime(4),q6; + 6*StateMachine.jointsSmoothingTime(4),q7; + 7*StateMachine.jointsSmoothingTime(4),q8]; + +StateMachine.joints_rightYogaRef = StateMachine.joints_leftYogaRef; +StateMachine.joints_rightYogaRef(:,1) = [0; + 1*StateMachine.jointsSmoothingTime(10); + 2*StateMachine.jointsSmoothingTime(10); + 3*StateMachine.jointsSmoothingTime(10); + 4*StateMachine.jointsSmoothingTime(10); + 5*StateMachine.jointsSmoothingTime(10); + 6*StateMachine.jointsSmoothingTime(10); + 7*StateMachine.jointsSmoothingTime(10)]; + +% MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA +for i = 1:size(StateMachine.joints_rightYogaRef,1) + + StateMachine.joints_rightYogaRef(i,2:4) = [StateMachine.joints_rightYogaRef(i,2) -StateMachine.joints_rightYogaRef(i,3) -StateMachine.joints_rightYogaRef(i,4)]; + rightArm = StateMachine.joints_rightYogaRef(i,end-15:end-12); + StateMachine.joints_rightYogaRef(i,end-15:end-12) = StateMachine.joints_rightYogaRef(i,end-19:end-16); + StateMachine.joints_rightYogaRef(i,end-19:end-16) = rightArm; + rightLeg = StateMachine.joints_rightYogaRef(i,end-5:end); + StateMachine.joints_rightYogaRef(i,end-5:end) = StateMachine.joints_rightYogaRef(i,end-11:end-6); + StateMachine.joints_rightYogaRef(i,end-11:end-6) = rightLeg; +end + +%% References for CoM trajectory (COORDINATOR DEMO ONLY) + +% that the robot waits before starting the left-and-right +Config.noOscillationTime = 0; +Config.directionOfOscillation = [0;1;0]; +Config.amplitudeOfOscillation = 0.02; % [m] +Config.frequencyOfOscillation = 0.2; % [Hz] \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control/initTorqueControlBalancing.m b/controllers/floating-base-balancing-torque-control/initTorqueControlBalancing.m new file mode 100644 index 0000000..aa9da74 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/initTorqueControlBalancing.m @@ -0,0 +1,83 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% /** +% * Copyright (C) 2016 CoDyCo +% * @author: Daniele Pucci, Gabriele Nava +% * Permission is granted to copy, distribute, and/or modify this program +% * under the terms of the GNU General Public License, version 2 or any +% * later version published by the Free Software Foundation. +% * +% * A copy of the license can be found at +% * http://www.robotcub.org/icub/license/gpl.txt +% * +% * This program is distributed in the hope that it will be useful, but +% * WITHOUT ANY WARRANTY; without even the implied warranty of +% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General +% * Public License for more details +% */ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% NOTE: THIS SCRIPT IS RUN AUTOMATICALLY WHEN THE USER STARTS THE ASSOCIATED +% SIMULINK MODEL. NO NEED TO RUN THIS SCRIPT EVERY TIME. +clearvars -except sl_synch_handles simulinkStaticGUI +clc + +% Add path to local source code +addpath('./src/') + +%% GENERAL SIMULATION INFO +% +% If you are simulating the robot with Gazebo, remember that it is required +% to launch Gazebo as follows: +% +% gazebo -slibgazebo_yarp_clock.so +% +% and properly set the environmental variable YARP_ROBOT_NAME in the .bashrc. + +% Simulation time in seconds. For long simulations, put an high number +% (not inf) for allowing automatic code generation +Config.SIMULATION_TIME = 600000; + +% Controller period [s] +Config.tStep = 0.01; + +%% PRELIMINARY CONFIGURATION +% +% DEMO_TYPE: defines the kind of demo that will be performed. +% +% 'YOGA': the robot will perform the YOGA++ demo (highly dynamic movements +% while balancing on one foot and two feet) +% +% 'COORDINATOR': the robot can either balance on two feet or move from left +% to right follwing a desired center-of-mass trajectory. +% +DEMO_TYPE = 'YOGA'; + +% Config.SCOPES: debugging scopes activation +Config.SCOPES_WRENCHES = true; +Config.SCOPES_GAIN_SCHE_INFO = true; +Config.SCOPES_MAIN = true; +Config.SCOPES_QP = true; + +% DATA PROCESSING +% +% Save the Matlab workspace after stopping the simulation +Config.SAVE_WORKSPACE = false; + +% Verify that the integration time has been respected during the simulation +Config.CHECK_INTEGRATION_TIME = true; +Config.PLOT_INTEGRATION_TIME = false; + +% Run robot-specific configuration parameters +run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configRobot.m')); +run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configStateMachine.m')); +run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/gainsAndReferences.m')); + +% Deactivate/activate the internal coordinator +if strcmpi(DEMO_TYPE, 'COORDINATOR') + + Config.COORDINATOR_DEMO = true; + +elseif strcmpi(DEMO_TYPE, 'YOGA') + + Config.COORDINATOR_DEMO = false; +end \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control/src-static-gui/closeModel.m b/controllers/floating-base-balancing-torque-control/src-static-gui/closeModel.m new file mode 100644 index 0000000..496418a --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/src-static-gui/closeModel.m @@ -0,0 +1,20 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Save and close the Simulink model through Matlab command line. +% It also closes the associate static GUI + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('[closeModel]: closing the Simulink model...') + +% save and close the Simulink model +save_system('torqueControlBalancing.mdl'); +close_system('torqueControlBalancing.mdl'); + +% close all figures +close all + +% remove paths (optional) +rmpath('../../library/matlab-gui'); +rmpath('./src-static-gui'); + +disp('[closeModel]: done.') \ No newline at end of file diff --git a/torque-controllers/momentum-based-yoga/src-gui/compileModel.m b/controllers/floating-base-balancing-torque-control/src-static-gui/compileModel.m similarity index 70% rename from torque-controllers/momentum-based-yoga/src-gui/compileModel.m rename to controllers/floating-base-balancing-torque-control/src-static-gui/compileModel.m index ccb4af4..e6e31a1 100644 --- a/torque-controllers/momentum-based-yoga/src-gui/compileModel.m +++ b/controllers/floating-base-balancing-torque-control/src-static-gui/compileModel.m @@ -3,14 +3,13 @@ % Compile the Simulink model through Matlab command line. %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -disp('Compiling the model...') +disp('Compiling the Simulink model...') pause(1.5) try - torqueBalancingYoga([], [], [], 'compile') - torqueBalancingYoga([], [], [], 'term') + torqueControlBalancing([], [], [], 'compile') + torqueControlBalancing([], [], [], 'term') catch ME @@ -20,4 +19,6 @@ clc disp('Compilation done.') + +% warning about Simulink timing warning('The model will anyways start with FEW SECONDS OF DELAY after pressing the ''Start Model'' button.') \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control/src/momentumBasedController.m b/controllers/floating-base-balancing-torque-control/src/momentumBasedController.m new file mode 100644 index 0000000..3456004 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/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/floating-base-balancing-torque-control/src/processOutputQP_oneFoot.m b/controllers/floating-base-balancing-torque-control/src/processOutputQP_oneFoot.m new file mode 100644 index 0000000..27a00f8 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/src/processOutputQP_oneFoot.m @@ -0,0 +1,57 @@ +function f_star = processOutputQP_oneFoot(analyticalSolution,primalSolution,QPStatus,feetContactStatus,Config) + + % PROCESSOUTPUTQP_ONEFOOT evaluates the output of the WBToolbox QP block. + % In case the QP block exited with an error, a + % "default", user defined solution to the QP problem + % is provided instead of the one coming from the QP block. + % + % FORMAT: f_star = processOutputQP_oneFoot(analyticalSolution,primalSolution,QPStatus,feetContactStatus,Config) + % + % INPUT: - analyticalSolution = the alternative user defined solution to the QP + % problem to be used when the QP block fails; + % - primalSolution = the solution to the QP problem provided by the + % WBToolbox QP block; + % - QPStatus = the status check returned by the QP block. + % + % - feetContactStatus = [2 x 1] a vector describing the feet contact + % status. Format: [leftFoot; rightFoot]; + % - Config = a structure containing the robot-related configuration + % parameters; + % + % OUTPUT: - f_star = the final solution to the QP problem that will be used in + % the controller. + % + % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava + % + % all authors are with the Italian Istitute of Technology (IIT) + % email: name.surname@iit.it + % + % Genoa, Dec 2017 + % + + %% --- Initialization --- + + CONTACT_THRESHOLD = 0.1; + QP_STATUS_THRESHOLD = 0.01; + + % if the robot is balancing on one foot, extend the QP solution to + % have the correct dimension of f_star for matrix multiplication + if feetContactStatus(1) > (1 - CONTACT_THRESHOLD) + + % left foot balancing + updated_primalSolution = [primalSolution; zeros(6,1)]; + updated_analyticalSolution = [analyticalSolution; zeros(6,1)]; + + else + % right foot balancing + updated_primalSolution = [zeros(6,1); primalSolution]; + updated_analyticalSolution = [zeros(6,1); analyticalSolution]; + end + + if Config.USE_QP_SOLVER && abs(QPStatus)< QP_STATUS_THRESHOLD + + f_star = updated_primalSolution; + else + f_star = updated_analyticalSolution; + end +end \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control/src/processOutputQP_twoFeet.m b/controllers/floating-base-balancing-torque-control/src/processOutputQP_twoFeet.m new file mode 100644 index 0000000..7b32870 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/src/processOutputQP_twoFeet.m @@ -0,0 +1,41 @@ +function f_star = processOutputQP_twoFeet(analyticalSolution,primalSolution,QPStatus,Config) + + % PROCESSOUTPUTQP_TWOFEET evaluates the output of the WBToolbox QP block. + % In case the QP block exited with an error, a + % "default", user defined solution to the QP problem + % is provided instead of the one coming from the QP block. + % + % FORMAT: f_star = processOutputQP_twoFeet(analyticalSolution,primalSolution,QPStatus,feetContactStatus,Config) + % + % INPUT: - analyticalSolution = the alternative user defined solution to the QP + % problem to be used when the QP block fails; + % - primalSolution = the solution to the QP problem provided by the + % WBToolbox QP block; + % - QPStatus = the status check returned by the QP block. + % + % - feetContactStatus = [2 x 1] a vector describing the feet contact + % status. Format: [leftFoot; rightFoot]; + % - Config = a structure containing the robot-related configuration + % parameters; + % + % OUTPUT: - f_star = the final solution to the QP problem that will be used in + % the controller. + % + % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava + % + % all authors are with the Italian Istitute of Technology (IIT) + % email: name.surname@iit.it + % + % Genoa, Dec 2017 + % + + %% --- Initialization --- + QP_STATUS_THRESHOLD = 0.01; + + if Config.USE_QP_SOLVER && abs(QPStatus)< QP_STATUS_THRESHOLD + + f_star = primalSolution; + else + f_star = analyticalSolution; + end +end \ No newline at end of file diff --git a/controllers/floating-base-balancing-torque-control/src/stateMachineMomentumControl.m b/controllers/floating-base-balancing-torque-control/src/stateMachineMomentumControl.m new file mode 100644 index 0000000..7e3078d --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/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/torque-controllers/momentum-based-standup/startStandupWithoutSimulinkGui.m b/controllers/floating-base-balancing-torque-control/startModelWithStaticGui.m similarity index 83% rename from torque-controllers/momentum-based-standup/startStandupWithoutSimulinkGui.m rename to controllers/floating-base-balancing-torque-control/startModelWithStaticGui.m index 1e07a88..89496c2 100644 --- a/torque-controllers/momentum-based-standup/startStandupWithoutSimulinkGui.m +++ b/controllers/floating-base-balancing-torque-control/startModelWithStaticGui.m @@ -1,6 +1,6 @@ %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% RUN THIS SCRIPT TO USE SIMULINK WITHOUT OPENING THE SIMULINK GUI +% RUN THIS SCRIPT TO USE SIMULINK WITH THE STATIC SIMULINK GUI %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear variables @@ -8,19 +8,19 @@ % add the path to the static gui and to some utility functions addpath('../../library/matlab-gui'); -addpath('./src-gui'); +addpath('./src-static-gui'); disp('[startModel]: loading the model...') % open the model -open_system('torqueBalancingStandup.mdl','loadonly'); +open_system('torqueControlBalancing.mdl','loadonly'); % add message to tell the user that the model has been opened correctly disp('[startModel]: model loaded correctly') disp('[startModel]: the "Start Model" button is enabled only after compiling the model.') % add warning to warn the user NOT to close the GUI -warning('DO NOT CLOSE the GUI. The model won''t be closed! Use "Close Model" button instead.') +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') diff --git a/torque-controllers/momentum-based-standup/stopTorqueBalancingStandup.m b/controllers/floating-base-balancing-torque-control/stopTorqueControlBalancing.m similarity index 85% rename from torque-controllers/momentum-based-standup/stopTorqueBalancingStandup.m rename to controllers/floating-base-balancing-torque-control/stopTorqueControlBalancing.m index 5f63bc2..3a5a225 100644 --- a/torque-controllers/momentum-based-standup/stopTorqueBalancingStandup.m +++ b/controllers/floating-base-balancing-torque-control/stopTorqueControlBalancing.m @@ -47,14 +47,14 @@ % number of times the real time step was bigger than twice the % desired time step value - numOfTimeStepViolations = sum(diff(yarp_time0) > 2*Config.Ts); + numOfTimeStepViolations = sum(diff(yarp_time0) > 2*Config.tStep); if numOfTimeStepViolations > 1 && numOfTimeStepViolations <= 50 - warning(['The time step tolerance of ', num2str(Config.Ts), '[s] has been violated at least once.']) + 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.Ts), '[s] has been violated more than 50 times.']) + 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 diff --git a/controllers/floating-base-balancing-torque-control/torqueControlBalancing.mdl b/controllers/floating-base-balancing-torque-control/torqueControlBalancing.mdl new file mode 100644 index 0000000..c0acb05 --- /dev/null +++ b/controllers/floating-base-balancing-torque-control/torqueControlBalancing.mdl @@ -0,0 +1,22965 @@ +Model { + Name "torqueControlBalancing" + Version 9.0 + SavedCharacterEncoding "UTF-8" + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.3486" + NumModelReferences 0 + NumTestPointedSignals 0 + NumProvidedFunctions 0 + NumRequiredFunctions 0 + NumResetEvents 0 + HasInitializeEvent 0 + HasTerminateEvent 0 + IsExportFunctionModel 0 + NumParameterArguments 0 + NumExternalFileReferences 55 + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Configuration" + Path "torqueControlBalancing/Configuration" + SID "4537" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "torqueControlBalancing/Dump and visualize/Visualizer/GetMeasurement1" + SID "4543" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "torqueControlBalancing/GetMeasurement" + SID "4539" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "torqueControlBalancing/GetMeasurement1" + SID "4538" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "torqueControlBalancing/GetMeasurement2" + SID "4541" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "torqueControlBalancing/IMU_meas" + SID "2630" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MatchSignalSizes" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torque" + "s/QPSolver/QP One Foot/MatchSignalSizes" + SID "4691" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/QP" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torque" + "s/QPSolver/QP One Foot/QP One Foot" + SID "4693" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MatchSignalSizes" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torque" + "s/QPSolver/QP Two Feet/MatchSignalSizes" + SID "4679" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/QP" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torque" + "s/QPSolver/QP Two Feet/QP Two Feet" + SID "4681" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" + "um integral error/CentroidalMomentum" + SID "3718" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" + "um integral error/Compute base to fixed link transform/l_sole to root_link relative transform" + SID "4450" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" + "um integral error/Compute base to fixed link transform/r_sole to root_link relative transform" + SID "4475" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" + "um integral error/Jacobian" + SID "3719" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular moment" + "um integral error/Jacobian1" + SID "3720" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/CentroidalMom" + "entum" + SID "2345" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/GetBiasForces" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/GetBiasForces" + SID "2348" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/MassMatrix" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/MassMatrix" + SID "2349" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/CoM" + SID "4363" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/DotJNu" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/DotJNu l_so" + "le " + SID "2375" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/DotJNu" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/DotJNu r_so" + "le " + SID "2376" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Jacobian com" + SID "2378" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Jacobian l_" + "sole" + SID "2379" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/Jacobian r_" + "sole" + SID "2380" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/l_sole" + SID "2405" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Kinematics/r_sole" + SID "2406" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/holder " + SID "4559" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute State Veloc" + "ity/Feet Jacobians/Jacobian l_sole" + SID "4219" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Jacobians/Jacobian" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute State Veloc" + "ity/Feet Jacobians/Jacobian r_sole" + SID "4220" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/LFoot to base link transform /Fixed base to imu transform" + SID "4250" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/LFoot to base link transform /Fixed base to root link transform" + SID "4251" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/LFoot to base link transform /Neck Position" + SID "4254" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/LFoot to base link transform /holder 1" + SID "4258" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/LFoot to base link transform /holder 2" + SID "4259" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/RFoot to base link transform/Fixed base to imu transform" + SID "4855" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/RFoot to base link transform/Fixed base to root link transform" + SID "4856" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/RFoot to base link transform/Neck Position" + SID "4860" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/RFoot to base link transform/holder 1" + SID "4865" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/RFoot to base link transform/holder 2" + SID "4866" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/Relative transform l_sole_H_base" + SID "4269" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fix" + "ed link transform/Relative transform r_sole_H_base" + SID "4306" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/Minim" + "umJerkTrajectoryGenerator" + SID "2176" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/holde" + "r 1" + SID "2187" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/holder\n" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/holde" + "r 2" + SID "2188" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/xCom/" + "CoM" + SID "4229" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/xCom2" + "/CoM" + SID "4735" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and Re" + "ferences/Smooth reference CoM/MinimumJerkTrajectoryGenerator2" + SID "2153" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/MinimumJerkTrajectoryGenerator" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and Re" + "ferences/Smooth reference joint position/MinimumJerkTrajectoryGenerator1" + SID "2152" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/States/GetLimits" + Path "torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF JOINTS HI" + "T THE LIMITS/GetLimits" + SID "2432" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Actuators/SetReferences" + Path "torqueControlBalancing/SetReferences" + SID "2354" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Simulator Synchronizer" + Path "torqueControlBalancing/synchronizer/GAZEBO_SYNCHRONIZER/Simulator Synchronizer" + SID "2426" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Real Time Synchronizer" + Path "torqueControlBalancing/synchronizer/REAL_TIME_SYNC/Real Time Synchronizer" + SID "2431" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/Yarp Clock" + Path "torqueControlBalancing/synchronizer/Yarp Clock" + SID "4629" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "torqueControlBalancing/wrench_leftFoot" + SID "2206" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "torqueControlBalancing/wrench_rightFoot" + SID "2218" + Type "LIBRARY_BLOCK" + } + OrderedModelArguments 1 + } + DiagnosticSuppressor "on" + SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" + LogicAnalyzerGraphicalSettings "" + LogicAnalyzerPlugin "on" + LogicAnalyzerSignalOrdering "" + CustomCodeFunctionData "" + SLCCPlugin "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)));\ninitTorqueControlBalancing;\n\n%% Try to ed" + "it the GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n set(sl_synch_handles.startB" + "utton,'Backgroundcolor',[0.0,1.0,0.0]);\n set(sl_synch_handles.startButton,'Enable','on');\n\nend" + StartFcn "%% Try to edit the GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n se" + "t(sl_synch_handles.startButton,'Backgroundcolor',[0.8,0.8,0.8]);\n set(sl_synch_handles.startButton,'Enable','off" + "');\n\n %% Update Compile Button\n set(sl_synch_handles.compileButton,'Backgroundcolor',[0.8,0.8,0.8]);\n s" + "et(sl_synch_handles.compileButton,'Enable','off');\n\n %% Update Exit Button\n set(sl_synch_handles.exitButton" + ",'Backgroundcolor',[0.8,0.8,0.8]);\n set(sl_synch_handles.exitButton,'Enable','off');\n\nend\n\n" + StopFcn "%% stop the simulation\ncd(fileparts(which(bdroot)));\nstopTorqueControlBalancing;\n\n%% Try to edit the" + " GUI (the user may have already closed it)\ntry\n\n %% Update 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" + 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] + } + 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] + } + Object { + $PropName "DockComponentsInfo" + $ObjectID 6 + $ClassName "Simulink.DockComponentInfo" + Type "GLUE2:PropertyInspector" + ID "Property Inspector" + Visible [0] + CreateCallback "" + UserData "" + Floating [0] + DockPosition "Right" + Width [640] + Height [480] + } + WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + "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" + } + } + HideAutomaticNames on + Created "Tue Feb 18 10:13:36 2014" + Creator "daniele" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "gnava" + ModifiedDateFormat "%" + LastModifiedDate "Fri Mar 08 16:25:41 2019" + RTWModifiedTimeStamp 473963141 + 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" + VisualizeLoggedSignalsWhenLoggingToFile off + 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 7 + $ClassName "Simulink.SimulationData.ModelLoggingInfo" + model_ "torqueControlBalancing" + signals_ [] + overrideMode_ [0.0] + Array { + Type "Cell" + Dimension 1 + Cell "torqueControlBalancing" + PropName "logAsSpecifiedByModels_" + } + Array { + Type "Cell" + Dimension 1 + Cell [] + PropName "logAsSpecifiedByModelsSSIDs_" + } + } + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + OrderedModelArguments on + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 8 + Version "1.17.1" + DisabledProps [] + Description "" + Array { + Type "Handle" + Dimension 9 + Simulink.SolverCC { + $ObjectID 9 + Version "1.17.1" + DisabledProps [] + Description "" + 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 10 + Version "1.17.1" + DisabledProps [] + Description "" + 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 11 + Version "1.17.1" + Array { + Type "Cell" + Dimension 8 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + Cell "UseSpecifiedMinMax" + PropName "DisabledProps" + } + Description "" + 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 12 + Version "1.17.1" + Array { + Type "Cell" + Dimension 1 + Cell "UseOnlyExistingSharedCode" + PropName "DisabledProps" + } + Description "" + 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 13 + Version "1.17.1" + DisabledProps [] + Description "" + 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 14 + Version "1.17.1" + DisabledProps [] + Description "" + 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 15 + Version "1.17.1" + DisabledProps [] + Description "" + 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 16 + 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" + } + SystemTargetFile "grt.tlc" + HardwareBoard "None" + TLCOptions "" + GenCodeOnly on + MakeCommand "make_rtw" + GenerateMakefile on + PackageGeneratedCodeAndArtifacts off + PackageName "" + TemplateMakefile "grt_default_tmf" + PostCodeGenCommand "" + Description "" + GenerateReport off + SaveLog 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 17 + 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 "" + 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 18 + Version "1.17.1" + Array { + Type "Cell" + Dimension 17 + 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" + PropName "DisabledProps" + } + Description "" + Array { + Type "Handle" + Dimension 1 + Simulink.CPPComponent { + $ObjectID 19 + Version "1.18.0" + Array { + Type "Cell" + Dimension 10 + Cell "Description" + Cell "Components" + Cell "Name" + Cell "GenerateDestructor" + Cell "GenerateExternalIOAccessMethods" + Cell "ParameterMemberVisibility" + Cell "InternalMemberVisibility" + Cell "GenerateParameterAccessMethods" + Cell "GenerateInternalMemberAccessMethods" + Cell "UseOperatorNewForModelRefRegistration" + PropName "DisabledProps" + } + Description "" + 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 20 + Version "1.17.1" + DisabledProps [] + Description "Simulink Coverage Configuration Component" + 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" + ExtraOptions "-aBlockCommentType=\"BlockPathComment\" -aIgnoreTestpoints=0 " + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 730, 390, 2252, 1260 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 8 + } + Object { + $PropName "DataTransfer" + $ObjectID 21 + $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 { + 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 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 off + } + 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 + IsObserver 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 "Inherit from input (this choice will be removed - see release notes)" + FixptAsFi off + NumInputs "1" + SampleTime "0" + } + } + System { + Name "torqueControlBalancing" + 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 "361" + ReportName "simulink-default.rpt" + SIDHighWatermark "4885" + Block { + BlockType Reference + Name "Configuration" + SID "4537" + Ports [] + Position [1180, 545, 1245, 565] + ZOrder 553 + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Configuration" + SourceType "" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + ConfigSource "Workspace" + ConfigObject "'WBTConfigRobot'" + RobotName "'icubSim'" + UrdfFile "'model.urdf'" + ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" + ControlBoardsNames "{'left_arm','right_arm','torso'}" + LocalName "'WBT'" + GravityVector "[0,0,-9.81]" + } + Block { + BlockType SubSystem + Name "Dump and visualize" + SID "2275" + Ports [] + Position [1155, 381, 1275, 409] + ZOrder 547 + ForegroundColor "blue" + BackgroundColor "[0.333333, 1.000000, 1.000000]" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 22 + $ClassName "Simulink.Mask" + Display "disp('VISUALIZER')" + } + System { + Name "Dump and visualize" + 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 "415" + Block { + BlockType SubSystem + Name "Desired and Measured Forces" + SID "3241" + Ports [4, 0, 1] + Position [260, 313, 435, 387] + ZOrder 899 + RequestExecContextInheritance off + System { + Name "Desired and Measured Forces" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "240" + 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 [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 "466" + 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" + } + Block { + BlockType Outport + Name "des forces l_sole" + SID "4812" + Position [75, -112, 105, -98] + ZOrder 702 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "measured forces r_sole" + SID "4813" + Position [-365, 33, -335, 47] + ZOrder 703 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "des forces r_sole" + SID "4816" + Position [75, 18, 105, 32] + ZOrder 706 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "measured moments l_sole" + SID "4811" + Position [-365, -67, -335, -53] + ZOrder 701 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "des moments l_sole" + SID "4814" + Position [75, -67, 105, -53] + ZOrder 704 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "measured moments r_sole" + SID "4815" + Position [-365, 98, -335, 112] + ZOrder 705 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "des moments r_sole" + SID "4817" + Position [75, 63, 105, 77] + ZOrder 707 + Port "8" + IconDisplay "Port number" + } + 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','2018a','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','2018a','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" + "','2018a','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','2018a','Location',[401 474 1711 10" + "60])" + NumInputPorts "4" + Floating off + } + Line { + ZOrder 38 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock "Demux Forces nd Moments" + DstPort 1 + } + Line { + ZOrder 39 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock "Demux Forces nd Moments" + DstPort 2 + } + Line { + ZOrder 40 + SrcBlock "f_star" + SrcPort 1 + DstBlock "Demux Forces nd Moments" + DstPort 3 + } + Line { + Name "measured forces l_sole" + ZOrder 41 + SrcBlock "Demux Forces nd Moments" + SrcPort 1 + Points [43, 0; 0, -270; 142, 0] + Branch { + ZOrder 96 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 1 + } + Branch { + ZOrder 45 + DstBlock "Sum" + DstPort 1 + } + } + Line { + Name "desired forces l_sole" + ZOrder 42 + SrcBlock "Demux Forces nd Moments" + SrcPort 2 + Points [66, 0; 0, -255] + Branch { + ZOrder 82 + DstBlock "Sum" + DstPort 2 + } + Branch { + ZOrder 46 + Labels [-1, 0] + Points [0, -25] + DstBlock "left Foot forces" + DstPort 2 + } + } + Line { + Name "error forces l_sole" + ZOrder 43 + Labels [-1, 0] + SrcBlock "Sum" + SrcPort 1 + DstBlock "left Foot forces" + DstPort 3 + } + Line { + Name "thresholdContactOn" + ZOrder 50 + SrcBlock "Constant" + SrcPort 1 + Points [259, 0] + Branch { + ZOrder 91 + Labels [-1, 0] + Points [0, 185] + DstBlock "right Foot forces" + DstPort 5 + } + Branch { + ZOrder 90 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 5 + } + } + Line { + Name "desired forces r_sole" + ZOrder 83 + SrcBlock "Demux Forces nd Moments" + SrcPort 4 + Points [116, 0; 0, -140] + Branch { + ZOrder 86 + DstBlock "Sum1" + DstPort 2 + } + Branch { + ZOrder 84 + Labels [-1, 0] + Points [0, -25] + DstBlock "right Foot forces" + DstPort 2 + } + } + Line { + Name "measured forces r_sole" + ZOrder 81 + SrcBlock "Demux Forces nd Moments" + SrcPort 3 + Points [90, 0; 0, -155; 95, 0] + Branch { + ZOrder 97 + Labels [-1, 0] + DstBlock "right Foot forces" + DstPort 1 + } + Branch { + ZOrder 64 + DstBlock "Sum1" + DstPort 1 + } + } + Line { + Name "error forces r_sole" + ZOrder 74 + Labels [-1, 0] + SrcBlock "Sum1" + SrcPort 1 + DstBlock "right Foot forces" + DstPort 3 + } + Line { + Name "thresholdContactOff" + ZOrder 49 + SrcBlock "Constant2" + SrcPort 1 + Points [280, 0] + Branch { + ZOrder 89 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 4 + } + Branch { + ZOrder 88 + Labels [-1, 0] + Points [0, 185] + DstBlock "right Foot forces" + DstPort 4 + } + } + Line { + Name "measured moments r_sole" + ZOrder 127 + SrcBlock "Demux Forces nd Moments" + SrcPort 7 + Points [68, 0; 0, 110; 117, 0] + Branch { + ZOrder 131 + DstBlock "Sum3" + DstPort 1 + } + Branch { + ZOrder 130 + Labels [0, 0] + DstBlock "right Foot moments" + DstPort 1 + } + } + Line { + Name "state" + ZOrder 51 + SrcBlock "state" + SrcPort 1 + Points [321, 0] + Branch { + ZOrder 95 + Points [0, 185] + Branch { + ZOrder 123 + Points [0, 205; 1, 0] + Branch { + ZOrder 126 + Labels [-1, 0] + Points [0, 180] + DstBlock "right Foot moments" + DstPort 4 + } + Branch { + ZOrder 125 + DstBlock "left Foot moments" + DstPort 4 + } + } + Branch { + ZOrder 122 + Labels [-1, 0] + DstBlock "right Foot forces" + DstPort 6 + } + } + Branch { + ZOrder 94 + Labels [-1, 0] + DstBlock "left Foot forces" + DstPort 6 + } + } + Line { + Name "desired moments r_sole" + ZOrder 128 + SrcBlock "Demux Forces nd Moments" + SrcPort 8 + Points [40, 0; 0, 110] + Branch { + ZOrder 134 + Labels [-1, 0] + DstBlock "right Foot moments" + DstPort 2 + } + Branch { + ZOrder 133 + Points [0, 35] + DstBlock "Sum3" + DstPort 2 + } + } + Line { + Name "error moments r_sole" + ZOrder 129 + Labels [-1, 0] + SrcBlock "Sum3" + SrcPort 1 + DstBlock "right Foot moments" + DstPort 3 + } + Line { + Name "error moments l_sole" + ZOrder 116 + Labels [-1, 0] + SrcBlock "Sum2" + SrcPort 1 + DstBlock "left Foot moments" + DstPort 3 + } + Line { + Name "desired moments l_sole" + ZOrder 115 + SrcBlock "Demux Forces nd Moments" + SrcPort 6 + Points [121, 0] + Branch { + ZOrder 120 + Points [0, 35] + DstBlock "Sum2" + DstPort 2 + } + Branch { + ZOrder 119 + Labels [-1, 0] + DstBlock "left Foot moments" + DstPort 2 + } + } + Line { + Name "measured moments l_sole" + ZOrder 114 + SrcBlock "Demux Forces nd Moments" + SrcPort 5 + Points [185, 0] + Branch { + ZOrder 118 + DstBlock "Sum2" + DstPort 1 + } + Branch { + ZOrder 117 + Labels [-1, 0] + DstBlock "left Foot moments" + DstPort 1 + } + } + } + } + Block { + BlockType SubSystem + Name "Feet Status and Gains" + SID "4104" + Ports [3, 0, 1] + Position [255, 10, 440, 90] + ZOrder 903 + RequestExecContextInheritance off + System { + Name "Feet Status and Gains" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "472" + 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','2018a','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','2018a','Location',[898 828 2209 1" + "517])" + NumInputPorts "6" + Floating off + } + Line { + ZOrder 1 + SrcBlock "state" + SrcPort 1 + Points [85, 0] + Branch { + ZOrder 31 + DstBlock "Feet Contact Activation" + DstPort 2 + } + Branch { + ZOrder 30 + Points [0, 195] + DstBlock "Gain Scheduling Postural" + DstPort 6 + } + } + Line { + ZOrder 8 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock "Feet Contact Activation" + DstPort 1 + } + Line { + ZOrder 9 + SrcBlock "KP_postural_diag" + SrcPort 1 + DstBlock "Demux1" + DstPort 1 + } + Line { + Name "left_leg" + ZOrder 11 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 4 + DstBlock "Gain Scheduling Postural" + DstPort 4 + } + Line { + Name "right_leg" + ZOrder 12 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 5 + DstBlock "Gain Scheduling Postural" + DstPort 5 + } + Line { + Name "right_arm" + ZOrder 13 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 3 + DstBlock "Gain Scheduling Postural" + DstPort 3 + } + Line { + Name "torso" + ZOrder 14 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 1 + DstBlock "Gain Scheduling Postural" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 15 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 2 + DstBlock "Gain Scheduling Postural" + DstPort 2 + } + } + } + Block { + BlockType From + Name "From" + SID "4725" + Position [490, 275, 585, 295] + ZOrder 898 + ShowName off + HideAutomaticName off + GotoTag "tau_star_afterSat" + TagVisibility "global" + } + Block { + BlockType From + Name "From1" + SID "4479" + Position [80, 198, 155, 212] + ZOrder 709 + ShowName off + HideAutomaticName off + GotoTag "state" + TagVisibility "global" + } + Block { + BlockType From + Name "From10" + SID "4793" + Position [45, 331, 140, 349] + ZOrder 945 + ShowName off + GotoTag "wrench_rightFoot" + TagVisibility "global" + } + Block { + BlockType From + Name "From11" + SID "4791" + Position [45, 311, 140, 329] + ZOrder 943 + ShowName off + GotoTag "wrench_leftFoot" + TagVisibility "global" + } + Block { + BlockType From + Name "From12" + SID "4795" + Position [500, 196, 580, 214] + ZOrder 947 + ShowName off + GotoTag "pos_CoM_des" + TagVisibility "global" + } + Block { + BlockType From + Name "From13" + SID "4798" + Position [515, 317, 565, 333] + ZOrder 950 + ShowName off + HideAutomaticName off + GotoTag "nu" + TagVisibility "global" + } + Block { + BlockType From + Name "From14" + SID "4828" + Position [510, 357, 565, 373] + ZOrder 953 + ShowName off + HideAutomaticName off + GotoTag "jointPos" + TagVisibility "global" + } + Block { + BlockType From + Name "From2" + SID "4720" + Position [60, 353, 120, 367] + ZOrder 864 + ShowName off + GotoTag "f_star" + TagVisibility "global" + } + Block { + BlockType From + Name "From3" + SID "4796" + Position [500, 156, 580, 174] + ZOrder 948 + ShowName off + GotoTag "pos_CoM" + TagVisibility "global" + } + Block { + BlockType From + Name "From4" + SID "4786" + Position [65, 16, 165, 34] + ZOrder 938 + ShowName off + HideAutomaticName off + GotoTag "feetContactStatus" + TagVisibility "global" + } + Block { + BlockType From + Name "From5" + SID "4721" + Position [505, 36, 580, 54] + ZOrder 865 + ShowName off + GotoTag "tau_star" + TagVisibility "global" + } + Block { + BlockType From + Name "From6" + SID "4781" + Position [500, 115, 580, 135] + ZOrder 937 + ShowName off + GotoTag "jointPos_des" + TagVisibility "global" + } + Block { + BlockType From + Name "From7" + SID "4485" + Position [505, 76, 580, 94] + ZOrder 715 + ShowName off + HideAutomaticName off + GotoTag "w_H_b" + TagVisibility "global" + } + Block { + BlockType From + Name "From8" + SID "4787" + Position [65, 41, 170, 59] + ZOrder 939 + ShowName off + HideAutomaticName off + GotoTag "KP_postural_diag" + TagVisibility "global" + } + Block { + BlockType From + Name "From9" + SID "4709" + Position [80, 166, 155, 184] + ZOrder 721 + ShowName off + HideAutomaticName off + GotoTag "QPStatus" + TagVisibility "global" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n1" + SID "3269" + Position [30, 279, 180, 291] + ZOrder 902 + ShowName off + Value "Config.SCOPES_WRENCHES" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n2" + SID "4593" + Position [60, 124, 175, 136] + ZOrder 720 + ShowName off + Value "Config.SCOPES_QP" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n4" + SID "3275" + Position [485, -26, 600, -14] + ZOrder 706 + ShowName off + Value "Config.SCOPES_MAIN" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n6" + SID "4103" + Position [25, -26, 205, -14] + ZOrder 906 + ShowName off + Value "Config.SCOPES_GAIN_SCHE_INFO" + } + Block { + BlockType SubSystem + Name "Visualize eventual QP failures" + SID "4620" + Ports [2, 0, 1] + Position [290, 161, 405, 219] + ZOrder 717 + RequestExecContextInheritance off + System { + Name "Visualize eventual QP failures" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + 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 "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','2018a','Location',[313 476 1506 1203])" + NumInputPorts "2" + Floating off + } + Line { + ZOrder 1 + SrcBlock "QPStatus" + SrcPort 1 + DstBlock "QP status\n(0: no failure)" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "state" + SrcPort 1 + DstBlock "QP status\n(0: no failure)" + DstPort 2 + } + } + } + Block { + BlockType SubSystem + Name "Visualizer" + SID "4504" + Ports [9, 0, 1] + Position [635, 25, 775, 385] + ZOrder 707 + RequestExecContextInheritance off + System { + Name "Visualizer" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "185" + 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.16773','MaxYLimReal','0.72406','YLabelReal','','MinYLimMag','0.00000','MaxYLimMa" + "g','0.72406','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTi" + "ckColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098" + "039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.074509" + "8039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]" + ",'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marke" + "r','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" + "),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0]" + ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement" + "',1),struct('MinYLimReal','-1.25','MaxYLimReal','1.25','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Lege" + "ndVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274" + "509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529" + "411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764" + "7058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" + "','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[" + "0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','" + "LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker'," + "'none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" + ",'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinYL" + "imReal','0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on" + "','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627" + "4509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4" + "11764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745" + "09803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProp" + "ertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" + ",3)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0." + "686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;" + "1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0" + ".274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Lin" + "ePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" + "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'Line" + "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0" + ".5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" + "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" + "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','400'," + "'TimeRangeFrames','400','DisplayLayoutDimensions',[3 1],'DisplayContentCache',[]),extmgr.Configuration('Tools'," + "'Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements'" + ",true,'Version','2018a')),'Version','2018a','Location',[630 446 1920 1048])" + NumInputPorts "3" + Floating off + } + Block { + BlockType Scope + Name "Base linear and angular velocity" + SID "4121" + Ports [3] + Position [1195, 319, 1280, 441] + ZOrder 891 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','baseVel_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecim" + "ation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplay" + "s',{struct('MinYLimReal','-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLimMag','10'," + "'LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6" + "86274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6" + "23529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0." + "717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vi" + "sible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" + "or',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'," + "'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('" + "MinYLimReal','-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisib" + "ility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098039" + "22 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294117647" + "06 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823" + "529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'" + ",'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" + ",struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1]," + "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWid" + "th',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none'," + "'Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserD" + "efinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinYLimReal'" + ",'-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on'," + "'XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745" + "09803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411" + "764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509" + "803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','LinePropertiesCache',{{s" + "truct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'L" + "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth" + "',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" + "lor',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'" + "NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3)},'DisplayPropertyDefaults',struct('MinYLimReal" + "','-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','AxesColor',[0 0 0],'Ax" + "esTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074" + "5098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07" + "45098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156" + "863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'M" + "arker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," + "'on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 " + "0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Place" + "ment',1),'DisplayLayoutDimensions',[3 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',fals" + "e),extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2018a','Location',[135 478 2" + "065 1412])" + NumInputPorts "3" + Floating off + } + Block { + BlockType Scope + Name "CoM Position" + SID "2286" + Ports [4] + Position [540, 245, 645, 380] + ZOrder 255 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','pos_CoM_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecim" + "ation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplay" + "s',{struct('MinYLimReal','-0.15575','MaxYLimReal','0.70056','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag'" + ",'0.70056','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTick" + "Color',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803" + "9215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980" + "39215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'" + "Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker'" + ",'none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')," + "struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'" + "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidt" + "h',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'," + "1),struct('MinYLimReal','-0.14783','MaxYLimReal','0.63035','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" + "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68" + "6274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62" + "3529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7" + "17647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" + "ible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','" + "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Mark" + "er','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('M" + "inYLimReal','-0.08636','MaxYLimReal','0.08883','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibi" + "lity','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980392" + "2 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941176470" + "6 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588235" + "29 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," + "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')," + "struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'" + "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidt" + "h',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDe" + "finedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal'," + "'0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid" + "',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098039" + "22 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705" + "882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921" + "569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCa" + "che',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1" + " 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','L" + "ineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','" + "none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" + "ruct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames" + "',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4)},'DisplayPropertyDefaults',struct('YLa" + "belReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorO" + "rder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392" + "156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;" + "1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0]" + ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" + "'Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineName" + "s',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','400','TimeRangeFrames','400','DisplayLayoutDime" + "nsions',[4 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY')" + ",extmgr.Configuration('Tools','Measurements',true,'Version','2018a')),'Version','2018a','Location',[135 239 265" + "0 1508])" + NumInputPorts "4" + Floating off + } + Block { + BlockType Selector + Name "CoM6D -> \nCoMXYZ1" + SID "3703" + Ports [1, 1] + Position [915, -462, 955, -438] + ZOrder 583 + ShowName off + NumberOfDimensions "2" + InputPortWidth "7" + IndexOptions "Index vector (dialog),Index vector (dialog)" + Indices "[1 2 3],[1 2 3]" + OutputSizes "1,1" + Port { + PortNumber 1 + Name "baseOrientation" + } + } + Block { + BlockType Selector + Name "CoM6D -> \nCoMXYZ2" + SID "3699" + Ports [1, 1] + Position [915, -502, 955, -478] + ZOrder 581 + ShowName off + NumberOfDimensions "2" + InputPortWidth "7" + IndexOptions "Index vector (dialog),Index vector (dialog)" + Indices "[1 2 3],[4]" + OutputSizes "1,1" + Port { + PortNumber 1 + Name "basePosition" + } + } + Block { + BlockType Demux + Name "Demux" + SID "4111" + Ports [1, 2] + Position [945, 320, 950, 400] + ZOrder 890 + ShowName off + Outputs "2" + 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','2018a','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','2018a','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" + ContentPreviewEnabled off + measuredType "Joints Torque" + } + Block { + BlockType Scope + Name "Joint Position Desired" + SID "2334" + Ports [6] + Position [1195, -198, 1280, -82] + ZOrder 501 + HideAutomaticName off + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','jointPos_Desired_DATA','DataLoggingSaveFormat','StructureWithTime','DataLog" + "gingDecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'Serializ" + "edDisplays',{struct('MinYLimReal','-8.36237','MaxYLimReal','29.68131','YLabelReal','','MinYLimMag','0.00000','M" + "axYLimMag','29.68131','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0" + "],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;" + "0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608" + " 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6509803" + "92156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0" + ".5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" + "ble','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" + "',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marke" + "r','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'" + "Placement',1),struct('MinYLimReal','-81.97965','MaxYLimReal','72.42484','YLabelReal','','MinYLimMag','0','MaxYL" + "imMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTic" + "kColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980" + "39215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098" + "039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]," + "'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker" + "','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" + ",struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0]," + "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWid" + "th',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none'," + "'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" + ",2),struct('MinYLimReal','-78.54318','MaxYLimReal','93.23402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10" + "','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0" + ".686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0" + ".623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;" + "0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%" + "','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" + "olor',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle" + "','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'M" + "arker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," + "'on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct" + "('MinYLimReal','-12.71731','MaxYLimReal','14.72789','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendV" + "isibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509" + "803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411" + "764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705" + "8823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" + "on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" + " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','Lin" + "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" + "ne','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'U" + "serDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimR" + "eal','-113.68313','MaxYLimReal','94.48842','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility" + "','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0." + "686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;" + "1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0" + ".274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Lin" + "ePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" + "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'Line" + "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0" + ".5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" + "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" + "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','0.6" + "25','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',tr" + "ue,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0" + ".686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058823" + "53 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " + "1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','LinePropertiesCache',{{struct('Col" + "or',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle'," + "'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 " + "1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'," + "0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesC" + "olor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.06666" + "66666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831" + "372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07450980392156" + "86 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','" + "LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker'," + "'none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" + "truct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'L" + "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" + "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowCon" + "tent',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmg" + "r.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuratio" + "n('Tools','Measurements',true,'Version','2018a')),'Version','2018a','Location',[135 204 1853 1049])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Scope + Name "Joint Position Measured" + SID "2333" + Ports [6] + Position [1195, -348, 1280, -232] + ZOrder 312 + HideAutomaticName off + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','jointPos_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDeci" + "mation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispla" + "ys',{struct('MinYLimReal','-2.41385','MaxYLimReal','5.58471','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag" + "','5.58471','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTic" + "kColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980" + "39215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098" + "039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]," + "'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker" + "','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" + ",struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0]," + "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWid" + "th',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none'," + "'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'" + ",1),struct('MinYLimReal','-39.61854','MaxYLimReal','58.9684','YLabelReal','','MinYLimMag','0','MaxYLimMag','10'" + ",'LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0." + "686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0." + "623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0" + ".717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%<" + "SignalLabel>','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" + "lor',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'" + ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Ma" + "rker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" + "on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct(" + "'MinYLimReal','-39.58369','MaxYLimReal','58.65801','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" + "sibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098" + "03922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294117" + "64706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058" + "823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 " + "1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','Line" + "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Us" + "erDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimRe" + "al','-8.16303','MaxYLimReal','11.36009','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','" + "on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686" + "274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0" + ".411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27" + "4509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePr" + "opertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" + "'Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" + "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5," + "'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" + "','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedCh" + "annelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-11.45" + "092','MaxYLimReal','7.90757','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid'" + ",true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980392" + "2 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" + "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039215" + "69 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCac" + "he',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 " + "0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" + "uct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames'" + ",{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','0.75','MaxYLimRea" + "l','3.25','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true," + "'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922" + "],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" + "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411" + "7647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color" + "',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marke" + "r','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" + "),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1]" + ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0," + "'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesCol" + "or',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666" + "666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137" + "2549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" + " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" + "uct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'Lin" + "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth'," + "0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowConte" + "nt',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr." + "Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration(" + "'Tools','Measurements',true,'Version','2018a')),'Version','2018a','Location',[135 184 3841 2132])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Scope + Name "Joint Positon Error" + SID "2335" + Ports [6] + Position [1195, -40, 1280, 80] + ZOrder 504 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','jointPos_Error_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggi" + "ngDecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'Serialized" + "Displays',{struct('MinYLimReal','-10.85715','MaxYLimReal','13.2441','YLabelReal','','MinYLimMag','0.00000','Max" + "YLimMag','13.2441','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'" + "AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0" + "745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0." + "0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6509803921" + "56863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5," + "'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" + "','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[" + "1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','" + "LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker'," + "'none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Pla" + "cement',1),struct('MinYLimReal','-24.65677','MaxYLimReal','13.37484','YLabelReal','','MinYLimMag','0','MaxYLimM" + "ag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickCo" + "lor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980392" + "15686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039" + "215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Ti" + "tle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','" + "none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" + "ruct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'Li" + "neStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth'" + ",0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vi" + "sible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2)" + ",struct('MinYLimReal','-37.11648','MaxYLimReal','33.25109','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" + "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68" + "6274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62" + "3529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7" + "17647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" + "ible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" + "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','" + "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Mark" + "er','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" + "')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('M" + "inYLimReal','-28.71162','MaxYLimReal','40.05399','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisi" + "bility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803" + "922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764" + "706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882" + "3529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%" + "','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" + "),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1]" + ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" + "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none'" + ",'Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'User" + "DefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal" + "','-16.66332','MaxYLimReal','13.39274','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','o" + "n','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862" + "74509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0." + "411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274" + "509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePro" + "pertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" + "Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'" + "Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'" + ",'on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedCha" + "nnelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-0.375'" + ",'MaxYLimReal','13.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true" + ",'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6" + "86274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353" + " 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;" + "0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','LinePropertiesCache',{{struct('Color" + "',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marke" + "r','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" + "),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1]" + ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0," + "'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesCol" + "or',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666" + "666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137" + "2549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" + " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" + "uct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'Lin" + "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth'," + "0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowConte" + "nt',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr." + "Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration(" + "'Tools','Measurements',true,'Version','2018a')),'Version','2018a','Location',[135 169 2055 1097])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Scope + Name "Joint Velocity" + SID "4122" + Ports [6] + Position [1195, 148, 1280, 272] + ZOrder 886 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','jointVel_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingDeci" + "mation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispla" + "ys',{struct('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','1" + "0','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[" + "0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 " + "0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686" + ";0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','" + "%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none'," + "'Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" + "Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'" + "Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'" + ",'on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struc" + "t('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legend" + "Visibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450" + "9803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" + "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470" + "58823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," + "'on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " + "1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'" + "UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinYLim" + "Real','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility" + "','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0." + "686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;" + "1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0" + ".274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Lin" + "ePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" + "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'Line" + "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0" + ".5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" + "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" + "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-20" + ".50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XG" + "rid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098" + "03922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764" + "705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803" + "921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertie" + "sCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'" + ",[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-'" + ",'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker" + "','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" + ",struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNa" + "mes',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-20.50153','M" + "axYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true," + "'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68" + "6274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 " + "0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0" + ".0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{" + "struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'" + "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidt" + "h',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" + "Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" + "olor',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}}," + "'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-20.50153','MaxYLimReal" + "','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',tr" + "ue,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803" + "922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431" + "372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529" + "4117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Co" + "lor',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle'" + ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Ma" + "rker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" + "on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0" + " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'" + ",0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('MinYLimReal','-20.50" + "153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','AxesColor',[0 0 0],'AxesTickC" + "olor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039" + "215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.074509803" + "9215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'T" + "itle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker'," + "'none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" + "truct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'L" + "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth" + "',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1" + "),'TimeRangeSamples','60','TimeRangeFrames','60','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools'," + "'Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2018a'))" + ",'Version','2018a','Location',[379 354 2309 1288])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Scope + Name "Measured Torques" + SID "2338" + Ports [6] + Position [540, 73, 645, 187] + ZOrder 495 + ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(ext" + "mgr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039" + "]),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggi" + "ng',true,'DataLoggingVariableName','tauMeasured_DATA','DataLoggingSaveFormat','StructureWithTime','DataLoggingD" + "ecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDis" + "plays',{struct('MinYLimReal','-13.53306','MaxYLimReal','13.58663','YLabelReal','','MinYLimMag','0.00000','MaxYL" + "imMag','13.58663','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'A" + "xesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07" + "45098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0" + "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215" + "6863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'" + "Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'" + ",'on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1" + " 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','L" + "ineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','" + "none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Plac" + "ement',1),struct('MinYLimReal','-4.19072','MaxYLimReal','4.87188','YLabelReal','','MinYLimMag','0','MaxYLimMag'" + ",'10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor" + "',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980392156" + "86 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215" + "686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title" + "','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" + "t('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineS" + "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0." + "5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" + "le','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),st" + "ruct('MinYLimReal','-4.21765','MaxYLimReal','4.8762','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legend" + "Visibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450" + "9803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" + "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470" + "58823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," + "'on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " + "1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','Li" + "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" + "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'" + "UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLim" + "Real','-45.10013','MaxYLimReal','35.90119','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility" + "','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0." + "686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;" + "1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0" + ".274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Lin" + "ePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" + "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'Line" + "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0" + ".5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" + "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" + "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-44" + ".89002','MaxYLimReal','34.01019','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XG" + "rid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098" + "03922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764" + "705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803" + "921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertie" + "sCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'" + ",[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-'" + ",'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker" + "','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" + ",struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNa" + "mes',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-0.5','MaxYLi" + "mReal','14.5','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',t" + "rue,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980" + "3922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843" + "1372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352" + "94117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','LinePropertiesCache',{{struct('Color',[1 1 0]," + "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWid" + "th',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'," + "'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" + "Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames" + "',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0" + "],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;" + "0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608" + " 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6509803" + "92156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0" + ".5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" + "ble','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" + "',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-" + "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marke" + "r','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'" + "Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configurat" + "ion('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','M" + "easurements',true,'Version','2018a')),'Version','2018a','Location',[1266 568 2566 1252])" + NumInputPorts "6" + Floating off + } + Block { + BlockType Selector + Name "Selector1" + SID "4118" + Ports [1, 1] + Position [845, 188, 905, 212] + ZOrder 889 + ShowName off + InputPortWidth "6+ROBOT_DOF" + IndexOptions "Index vector (dialog)" + Indices "[7:6+ROBOT_DOF]" + OutputSizes "1" + } + Block { + BlockType Selector + Name "Selector2" + SID "4119" + Ports [1, 1] + Position [845, 348, 905, 372] + ZOrder 888 + ShowName off + InputPortWidth "6+ROBOT_DOF" + IndexOptions "Index vector (dialog)" + Indices "[1:6]" + OutputSizes "1" + } + Block { + BlockType Sum + Name "Sum" + SID "2295" + Ports [2, 1] + Position [210, -45, 230, -25] + ZOrder 493 + ShowName off + 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','-23.63249','MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimM" + "ag','27.95301','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'Axes" + "TickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450" + "98039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745" + "098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" + "3],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Mar" + "ker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 " + "0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','Line" + "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placeme" + "nt',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabelReal','','MinYLimMag','0','MaxYLimMag','1" + "0','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[" + "0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 " + "0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686" + ";0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','" + "%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none'," + "'Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" + "Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyl" + "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'" + "Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'" + ",'on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struc" + "t('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" + "sibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098" + "03922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294117" + "64706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058" + "823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" + "n'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 " + "1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','Line" + "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" + "e','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Us" + "erDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimRe" + "al','-23.22756','MaxYLimReal','25.22297','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility'," + "'on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68" + "6274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 " + "0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2" + "74509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineP" + "ropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct" + "('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" + "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5" + ",'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" + "e','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedC" + "hannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-66.4" + "4849','MaxYLimReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGri" + "d',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803" + "922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.41176470" + "5882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450980392" + "1569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesC" + "ache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[" + "1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','" + "LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker'," + "'none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" + "truct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" + "s',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','0.25','MaxYLimR" + "eal','7.75','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',tru" + "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745098039" + "22],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784313" + "72549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294" + "117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','LinePropertiesCache',{{struct('Color',[1 1 0],'L" + "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth" + "',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" + "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" + "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle'" + ",'-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames'," + "{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0]," + "'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0." + "0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0" + ".0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392" + "156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5" + ",'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" + "e','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," + "[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-'," + "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker'" + ",'none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Pl" + "acement',1),'TimeRangeSamples','90','TimeRangeFrames','90','DisplayLayoutDimensions',[6 1]),extmgr.Configuratio" + "n('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Mea" + "surements',true,'Version','2018a')),'Version','2018a','Location',[135 169 1990 1170])" + NumInputPorts "6" + Floating off + } + Line { + Name "CoM_Measured" + ZOrder 391 + SrcBlock "pos_CoM" + SrcPort 1 + Points [120, 0] + Branch { + ZOrder 835 + DstBlock "Sum1" + DstPort 1 + } + Branch { + ZOrder 834 + Labels [-1, 0] + DstBlock "CoM Position" + DstPort 1 + } + } + Line { + ZOrder 702 + SrcBlock "Gain" + SrcPort 1 + DstBlock "Demux1" + DstPort 1 + } + Line { + ZOrder 839 + SrcBlock "jointPos" + SrcPort 1 + Points [80, 0] + Branch { + ZOrder 840 + DstBlock "Gain" + DstPort 1 + } + Branch { + ZOrder 7 + DstBlock "Sum3" + DstPort 1 + } + } + Line { + Name "right_leg" + ZOrder 700 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 5 + DstBlock "Joint Position Measured" + DstPort 5 + } + Line { + Name "torso" + ZOrder 698 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 1 + DstBlock "Joint Position Measured" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 697 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 2 + DstBlock "Joint Position Measured" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 701 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 4 + DstBlock "Joint Position Measured" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 699 + Labels [-1, 0] + SrcBlock "Demux1" + SrcPort 3 + DstBlock "Joint Position Measured" + DstPort 3 + } + Line { + ZOrder 480 + SrcBlock "GetMeasurement1" + SrcPort 1 + Points [100, 0] + Branch { + ZOrder 831 + DstBlock "Sum" + DstPort 2 + } + Branch { + ZOrder 686 + DstBlock "Demux9" + DstPort 1 + } + } + Line { + Name "right_leg" + ZOrder 666 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 5 + DstBlock "Desired Torques" + DstPort 5 + } + Line { + Name "torso" + ZOrder 668 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 1 + DstBlock "Desired Torques" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 667 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 2 + DstBlock "Desired Torques" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 665 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 4 + DstBlock "Desired Torques" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 669 + Labels [-1, 0] + SrcBlock "Demux8" + SrcPort 3 + DstBlock "Desired Torques" + DstPort 3 + } + Line { + Name "right_leg" + ZOrder 656 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 5 + DstBlock "Torques Error" + DstPort 5 + } + Line { + Name "torso" + ZOrder 659 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 1 + DstBlock "Torques Error" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 658 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 2 + DstBlock "Torques Error" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 657 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 4 + DstBlock "Torques Error" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 655 + Labels [-1, 0] + SrcBlock "Demux10" + SrcPort 3 + DstBlock "Torques Error" + DstPort 3 + } + Line { + ZOrder 685 + SrcBlock "Sum" + SrcPort 1 + DstBlock "Demux10" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 660 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 5 + DstBlock "Measured Torques" + DstPort 5 + } + Line { + Name "torso" + ZOrder 661 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 1 + DstBlock "Measured Torques" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 662 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 2 + DstBlock "Measured Torques" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 663 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 4 + DstBlock "Measured Torques" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 664 + Labels [-1, 0] + SrcBlock "Demux9" + SrcPort 3 + DstBlock "Measured Torques" + DstPort 3 + } + Line { + ZOrder 390 + SrcBlock "jointPos_des" + SrcPort 1 + Points [32, 0] + Branch { + ZOrder 52 + Points [0, 160] + DstBlock "Sum3" + DstPort 2 + } + Branch { + ZOrder 53 + DstBlock "Gain3" + DstPort 1 + } + } + Line { + ZOrder 703 + SrcBlock "Gain3" + SrcPort 1 + DstBlock "Demux2" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 694 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 5 + DstBlock "Joint Position Desired" + DstPort 5 + } + Line { + Name "torso" + ZOrder 693 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 1 + DstBlock "Joint Position Desired" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 695 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 2 + DstBlock "Joint Position Desired" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 692 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 4 + DstBlock "Joint Position Desired" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 696 + Labels [-1, 0] + SrcBlock "Demux2" + SrcPort 3 + DstBlock "Joint Position Desired" + DstPort 3 + } + Line { + ZOrder 704 + SrcBlock "Gain4" + SrcPort 1 + DstBlock "Demux5" + DstPort 1 + } + Line { + Name "right_leg" + ZOrder 690 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 5 + DstBlock "Joint Positon Error" + DstPort 5 + } + Line { + Name "torso" + ZOrder 688 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 1 + DstBlock "Joint Positon Error" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 689 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 2 + DstBlock "Joint Positon Error" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 691 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 4 + DstBlock "Joint Positon Error" + DstPort 4 + } + Line { + Name "right_arm" + ZOrder 687 + Labels [-1, 0] + SrcBlock "Demux5" + SrcPort 3 + DstBlock "Joint Positon Error" + DstPort 3 + } + Line { + ZOrder 70 + SrcBlock "Sum3" + SrcPort 1 + DstBlock "Gain4" + DstPort 1 + } + Line { + Name "CoM_Desired" + ZOrder 832 + SrcBlock "pos_CoM_des" + SrcPort 1 + Points [67, 0] + Branch { + ZOrder 837 + Points [0, 35] + DstBlock "Sum1" + DstPort 2 + } + Branch { + ZOrder 836 + Labels [-1, 0] + DstBlock "CoM Position" + DstPort 2 + } + } + Line { + ZOrder 388 + SrcBlock "w_H_b" + SrcPort 1 + Points [77, 0] + Branch { + ZOrder 869 + Points [0, -40] + DstBlock "CoM6D -> \nCoMXYZ2" + DstPort 1 + } + Branch { + ZOrder 868 + DstBlock "CoM6D -> \nCoMXYZ1" + DstPort 1 + } + } + Line { + Name "basePosition" + ZOrder 103 + Labels [-1, 0] + SrcBlock "CoM6D -> \nCoMXYZ2" + SrcPort 1 + DstBlock "Base Pose" + DstPort 1 + } + Line { + Name "baseOrientation" + ZOrder 104 + Labels [-1, 0] + SrcBlock "CoM6D -> \nCoMXYZ1" + SrcPort 1 + DstBlock "Base Pose" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 670 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 4 + DstBlock "Desired Torques After Saturation" + DstPort 4 + } + Line { + Name "right_leg" + ZOrder 673 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 5 + DstBlock "Desired Torques After Saturation" + DstPort 5 + } + Line { + Name "left_arm" + ZOrder 671 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 2 + DstBlock "Desired Torques After Saturation" + DstPort 2 + } + Line { + Name "torso" + ZOrder 674 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 1 + DstBlock "Desired Torques After Saturation" + DstPort 1 + } + Line { + Name "right_arm" + ZOrder 672 + Labels [-1, 0] + SrcBlock "Demux7" + SrcPort 3 + DstBlock "Desired Torques After Saturation" + DstPort 3 + } + Line { + ZOrder 581 + SrcBlock "tau_star_afterSat" + SrcPort 1 + DstBlock "Demux7" + DstPort 1 + } + Line { + ZOrder 387 + SrcBlock "tau_star" + SrcPort 1 + Points [41, 0] + Branch { + ZOrder 821 + Points [0, 150] + DstBlock "Sum" + DstPort 1 + } + Branch { + ZOrder 684 + DstBlock "Demux8" + DstPort 1 + } + } + Line { + ZOrder 806 + SrcBlock "nu" + SrcPort 1 + Points [26, 0] + Branch { + ZOrder 852 + DstBlock "Selector1" + DstPort 1 + } + Branch { + ZOrder 805 + Points [0, 160] + DstBlock "Selector2" + DstPort 1 + } + } + Line { + ZOrder 807 + SrcBlock "Selector2" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + ZOrder 808 + SrcBlock "Gain1" + SrcPort 1 + DstBlock "Demux3" + DstPort 1 + } + Line { + Name "base linear velocity" + ZOrder 809 + Labels [-1, 0] + SrcBlock "Demux" + SrcPort 1 + DstBlock "Base linear and angular velocity" + DstPort 1 + } + Line { + Name "base angular velocity" + ZOrder 810 + Labels [-1, 0] + SrcBlock "Gain2" + SrcPort 1 + DstBlock "Base linear and angular velocity" + DstPort 2 + } + Line { + Name "right_arm" + ZOrder 811 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 3 + DstBlock "Joint Velocity" + DstPort 3 + } + Line { + Name "right_leg" + ZOrder 812 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 5 + DstBlock "Joint Velocity" + DstPort 5 + } + Line { + ZOrder 813 + SrcBlock "Selector1" + SrcPort 1 + DstBlock "Gain1" + DstPort 1 + } + Line { + Name "torso" + ZOrder 817 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 1 + DstBlock "Joint Velocity" + DstPort 1 + } + Line { + Name "left_arm" + ZOrder 818 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 2 + DstBlock "Joint Velocity" + DstPort 2 + } + Line { + Name "left_leg" + ZOrder 819 + Labels [-1, 0] + SrcBlock "Demux3" + SrcPort 4 + DstBlock "Joint Velocity" + DstPort 4 + } + Line { + ZOrder 820 + SrcBlock "Demux" + SrcPort 2 + DstBlock "Gain2" + DstPort 1 + } + Line { + Name "CoM_Error" + ZOrder 838 + Labels [-1, 0] + SrcBlock "Sum1" + SrcPort 1 + DstBlock "CoM Position" + DstPort 3 + } + Line { + Name "state" + ZOrder 389 + SrcBlock "state" + SrcPort 1 + Points [234, 0] + Branch { + ZOrder 827 + Points [0, 145] + Branch { + ZOrder 825 + Points [0, 150] + Branch { + ZOrder 829 + Points [0, 155] + Branch { + ZOrder 828 + Points [0, 185] + Branch { + ZOrder 844 + Points [0, 55; 742, 0] + Branch { + ZOrder 862 + Labels [-1, 0] + DstBlock "Base linear and angular velocity" + DstPort 3 + } + Branch { + ZOrder 861 + Points [0, -160] + Branch { + ZOrder 859 + Labels [-1, 0] + DstBlock "Joint Velocity" + DstPort 6 + } + Branch { + ZOrder 853 + Points [0, -190] + Branch { + ZOrder 858 + Labels [-1, 0] + DstBlock "Joint Positon Error" + DstPort 6 + } + Branch { + ZOrder 848 + Points [0, -160] + Branch { + ZOrder 857 + Labels [-1, 0] + DstBlock "Joint Position Desired" + DstPort 6 + } + Branch { + ZOrder 850 + Points [0, -150; 1, 0] + Branch { + ZOrder 866 + Labels [-1, 0] + Points [0, -170] + DstBlock "Base Pose" + DstPort 3 + } + Branch { + ZOrder 865 + DstBlock "Joint Position Measured" + DstPort 6 + } + } + } + } + } + } + Branch { + ZOrder 843 + Labels [-1, 0] + DstBlock "CoM Position" + DstPort 4 + } + } + Branch { + ZOrder 89 + Labels [-1, 0] + DstBlock "Measured Torques" + DstPort 6 + } + } + Branch { + ZOrder 90 + Labels [-1, 0] + DstBlock "Torques Error" + DstPort 6 + } + } + Branch { + ZOrder 75 + Labels [-1, 0] + DstBlock "Desired Torques" + DstPort 6 + } + } + Branch { + ZOrder 823 + Labels [-1, 0] + DstBlock "Desired Torques After Saturation" + DstPort 6 + } + } + } + } + Line { + ZOrder 423 + SrcBlock "ON_GAZEBO\n4" + SrcPort 1 + Points [100, 0] + DstBlock "Visualizer" + DstPort enable + } + Line { + ZOrder 390 + SrcBlock "From1" + SrcPort 1 + Points [52, 0] + Branch { + ZOrder 450 + DstBlock "Visualize eventual QP failures" + DstPort 2 + } + Branch { + ZOrder 448 + Points [0, 40] + Branch { + ZOrder 474 + Points [0, 135] + DstBlock "Desired and Measured Forces" + DstPort 4 + } + Branch { + ZOrder 455 + DstBlock "Visualizer" + DstPort 6 + } + } + Branch { + ZOrder 449 + Points [0, -130] + DstBlock "Feet Status and Gains" + DstPort 3 + } + } + Line { + ZOrder 395 + SrcBlock "From7" + SrcPort 1 + DstBlock "Visualizer" + DstPort 2 + } + Line { + ZOrder 424 + SrcBlock "From5" + SrcPort 1 + DstBlock "Visualizer" + DstPort 1 + } + Line { + ZOrder 463 + SrcBlock "From12" + SrcPort 1 + DstBlock "Visualizer" + DstPort 5 + } + Line { + ZOrder 415 + SrcBlock "ON_GAZEBO\n2" + SrcPort 1 + Points [165, 0] + DstBlock "Visualize eventual QP failures" + DstPort enable + } + Line { + ZOrder 416 + SrcBlock "From9" + SrcPort 1 + DstBlock "Visualize eventual QP failures" + DstPort 1 + } + Line { + ZOrder 451 + SrcBlock "ON_GAZEBO\n1" + SrcPort 1 + Points [160, 0] + DstBlock "Desired and Measured Forces" + DstPort enable + } + Line { + ZOrder 445 + SrcBlock "ON_GAZEBO\n6" + SrcPort 1 + Points [135, 0] + DstBlock "Feet Status and Gains" + DstPort enable + } + Line { + ZOrder 446 + SrcBlock "From4" + SrcPort 1 + DstBlock "Feet Status and Gains" + DstPort 1 + } + Line { + ZOrder 447 + SrcBlock "From8" + SrcPort 1 + DstBlock "Feet Status and Gains" + DstPort 2 + } + Line { + ZOrder 452 + SrcBlock "From2" + SrcPort 1 + DstBlock "Desired and Measured Forces" + DstPort 3 + } + Line { + ZOrder 453 + SrcBlock "From11" + SrcPort 1 + DstBlock "Desired and Measured Forces" + DstPort 1 + } + Line { + ZOrder 454 + SrcBlock "From10" + SrcPort 1 + DstBlock "Desired and Measured Forces" + DstPort 2 + } + Line { + ZOrder 462 + SrcBlock "From6" + SrcPort 1 + DstBlock "Visualizer" + DstPort 3 + } + Line { + ZOrder 464 + SrcBlock "From3" + SrcPort 1 + DstBlock "Visualizer" + DstPort 4 + } + Line { + ZOrder 465 + SrcBlock "From" + SrcPort 1 + DstBlock "Visualizer" + DstPort 7 + } + Line { + ZOrder 466 + SrcBlock "From13" + SrcPort 1 + DstBlock "Visualizer" + DstPort 8 + } + Line { + ZOrder 475 + SrcBlock "From14" + SrcPort 1 + DstBlock "Visualizer" + DstPort 9 + } + } + } + Block { + BlockType Reference + Name "GetMeasurement" + SID "4539" + Ports [0, 1] + Position [780, 388, 865, 402] + ZOrder 905 + BackgroundColor "[0.925500, 0.870600, 0.133300]" + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetMeasurement" + SourceType "GetMeasurement" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + measuredType "Joints Acceleration" + } + Block { + BlockType Reference + Name "GetMeasurement1" + SID "4538" + Ports [0, 1] + Position [785, 432, 860, 448] + ZOrder 958 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetMeasurement" + SourceType "GetMeasurement" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + measuredType "Joints Position" + } + Block { + BlockType Reference + Name "GetMeasurement2" + SID "4541" + Ports [0, 1] + Position [785, 462, 860, 478] + ZOrder 960 + BackgroundColor "[0.513700, 0.674500, 1.000000]" + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetMeasurement" + SourceType "GetMeasurement" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + measuredType "Joints Velocity" + } + Block { + BlockType Goto + Name "GoTo Motors Inertia" + SID "4831" + Position [955, 387, 1040, 403] + ZOrder 906 + HideAutomaticName off + GotoTag "jointAcc" + TagVisibility "global" + } + Block { + BlockType Reference + Name "IMU_meas" + SID "2630" + Ports [0, 1] + Position [800, 494, 840, 506] + ZOrder 959 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" + SourceType "YARP Read" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + portName "Ports.IMU" + signalSize "Ports.IMU_PORT_SIZE" + timeout "0.5" + blocking off + timestamp off + autoconnect on + errorOnConnection on + } + Block { + BlockType SubSystem + Name "MOMENTUM BASED TORQUE CONTROL" + SID "4836" + Ports [5, 1] + Position [955, 427, 1105, 573] + ZOrder 961 + RequestExecContextInheritance off + System { + Name "MOMENTUM BASED TORQUE CONTROL" + Location [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 "278" + 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_rightFoot" + SID "4880" + Position [160, 538, 190, 552] + ZOrder 963 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "wrench_leftFoot" + SID "4881" + Position [160, 598, 190, 612] + ZOrder 964 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "Balancing Controller QP" + SID "2355" + Ports [20, 1] + Position [800, 31, 955, 609] + ZOrder 542 + BackgroundColor "lightBlue" + RequestExecContextInheritance off + System { + Name "Balancing Controller QP" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + 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 [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 "284" + 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 [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 "343" + 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 "66" + 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::65" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 56 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4590::64" + Tag "Stateflow S-Function torqueControlBalancing 15" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 55 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "onOneFoot" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4590::66" + Position [460, 241, 480, 259] + ZOrder 57 + } + Block { + BlockType Outport + Name "onOneFoot" + SID "4590::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 53 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "onOneFoot" + ZOrder 54 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "onOneFoot" + 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 "QP One Foot" + SID "4696" + Ports [5, 2, 1] + Position [25, -332, 120, -178] + ZOrder 736 + NamePlacement "alternate" + TreatAsAtomicUnit on + RequestExecContextInheritance off + System { + Name "QP One Foot" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "341" + 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 "67" + 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::66" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 57 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4690::65" + Tag "Stateflow S-Function torqueControlBalancing 26" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 56 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "analyticalSolution" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4690::67" + Position [460, 241, 480, 259] + ZOrder 58 + } + Block { + BlockType Outport + Name "analyticalSolution" + SID "4690::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 16 + SrcBlock "H" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 17 + SrcBlock "g" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "analyticalSolution" + ZOrder 18 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "analyticalSolution" + 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 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" + 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 "75" + 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::74" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 59 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4692::73" + Tag "Stateflow S-Function torqueControlBalancing 27" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 58 + 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::75" + Position [460, 241, 480, 259] + ZOrder 60 + } + Block { + BlockType Outport + Name "f_star" + SID "4692::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 28 + SrcBlock "analyticalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 29 + SrcBlock "primalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 30 + SrcBlock "QPStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 31 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "f_star" + ZOrder 32 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "f_star" + DstPort 1 + } + Line { + ZOrder 33 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 34 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "QP One Foot" + SID "4693" + Ports [4, 2] + Position [1145, -242, 1250, -128] + ZOrder 751 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/QP" + SourceType "QP" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + lbA off + ubA on + lb off + ub off + computeObjVal off + stopIfFails off + } + Block { + BlockType Outport + Name "f_star_oneFoot" + SID "4695" + Position [1790, -172, 1820, -158] + ZOrder 760 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "QPStatus" + SID "4694" + Position [1360, -162, 1390, -148] + ZOrder 755 + Port "2" + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "MatchSignalSizes" + SrcPort 1 + DstBlock "Process QP output" + DstPort 2 + } + Line { + ZOrder 3 + SrcBlock "A" + SrcPort 1 + DstBlock "QP One Foot" + DstPort 3 + } + Line { + ZOrder 4 + SrcBlock "H" + SrcPort 1 + Points [31, 0] + Branch { + ZOrder 34 + Points [0, -150] + DstBlock "Analytical Solution QP One Foot (unconstrained)" + DstPort 1 + } + Branch { + ZOrder 6 + DstBlock "QP One Foot" + DstPort 1 + } + } + Line { + ZOrder 7 + SrcBlock "ubA" + SrcPort 1 + DstBlock "QP One Foot" + DstPort 4 + } + Line { + ZOrder 8 + SrcBlock "g" + SrcPort 1 + Points [51, 0] + Branch { + ZOrder 33 + Points [0, -55] + Branch { + ZOrder 40 + Points [0, -75] + DstBlock "Analytical Solution QP One Foot (unconstrained)" + DstPort 2 + } + Branch { + ZOrder 11 + DstBlock "MatchSignalSizes" + DstPort 1 + } + } + Branch { + ZOrder 12 + DstBlock "QP One Foot" + DstPort 2 + } + } + Line { + ZOrder 13 + SrcBlock "Analytical Solution QP One Foot (unconstrained)" + SrcPort 1 + DstBlock "Process QP output" + DstPort 1 + } + Line { + ZOrder 14 + SrcBlock "QP One Foot" + SrcPort 2 + Points [34, 0] + Branch { + ZOrder 41 + Points [0, 50] + DstBlock "Process QP output" + DstPort 3 + } + Branch { + ZOrder 26 + DstBlock "QPStatus" + DstPort 1 + } + } + Line { + ZOrder 17 + SrcBlock "QP One Foot" + SrcPort 1 + DstBlock "MatchSignalSizes" + DstPort 2 + } + Line { + ZOrder 42 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock "Process QP output" + DstPort 4 + } + Line { + ZOrder 43 + SrcBlock "Process QP output" + SrcPort 1 + DstBlock "f_star_oneFoot" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "QP Two Feet" + SID "4684" + Ports [4, 2, 1] + Position [20, -5, 120, 155] + ZOrder 734 + TreatAsAtomicUnit on + RequestExecContextInheritance off + System { + Name "QP Two Feet" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "426" + 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 "67" + 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::66" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 57 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4678::65" + Tag "Stateflow S-Function torqueControlBalancing 24" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 56 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "analyticalSolution" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4678::67" + Position [460, 241, 480, 259] + ZOrder 58 + } + Block { + BlockType Outport + Name "analyticalSolution" + SID "4678::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 16 + SrcBlock "H" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 17 + SrcBlock "g" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "analyticalSolution" + ZOrder 18 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "analyticalSolution" + 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 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" + 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 "75" + 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::74" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 58 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4680::73" + Tag "Stateflow S-Function torqueControlBalancing 25" + Ports [3, 2] + Position [180, 100, 230, 180] + ZOrder 57 + 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::75" + Position [460, 241, 480, 259] + ZOrder 59 + } + Block { + BlockType Outport + Name "f_star" + SID "4680::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 38 + SrcBlock "analyticalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 39 + SrcBlock "primalSolution" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 40 + SrcBlock "QPStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + Name "f_star" + ZOrder 41 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "f_star" + DstPort 1 + } + Line { + ZOrder 42 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 43 + SrcBlock " SFunction " + SrcPort 1 + Points [20, 0] + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "QP Two Feet" + SID "4681" + Ports [4, 2] + Position [1575, 1016, 1680, 1134] + ZOrder 752 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/QP" + SourceType "QP" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + lbA off + ubA on + lb off + ub off + computeObjVal off + stopIfFails off + } + Block { + BlockType Outport + Name "QPStatus" + SID "4683" + Position [1765, 1098, 1795, 1112] + ZOrder 756 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "f0_twoFeet" + SID "4682" + Position [2155, 1018, 2185, 1032] + ZOrder 760 + Port "2" + IconDisplay "Port number" + } + 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" + } + Block { + BlockType Outport + Name "onOneFoot" + SID "4717" + Position [-10, -137, 20, -123] + ZOrder 738 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "f_0_twoFeet" + SID "4627" + Position [240, 108, 270, 122] + ZOrder 728 + Port "3" + IconDisplay "Port number" + } + 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" + } + Block { + BlockType Outport + Name "f_star" + SID "4409" + Position [1440, 733, 1470, 747] + ZOrder 639 + Port "2" + IconDisplay "Port number" + } + Line { + ZOrder 293 + SrcBlock "Na" + SrcPort 1 + DstBlock "Product2" + DstPort 1 + } + Line { + ZOrder 54 + SrcBlock "Product1" + SrcPort 1 + DstBlock "Add" + DstPort 2 + } + Line { + ZOrder 291 + SrcBlock "tauModel" + SrcPort 1 + DstBlock "Add" + DstPort 1 + } + Line { + ZOrder 304 + SrcBlock "Add" + SrcPort 1 + DstBlock "tau_QP" + DstPort 1 + } + Line { + ZOrder 57 + SrcBlock "Add1" + SrcPort 1 + Points [253, 0] + Branch { + ZOrder 407 + Points [0, 60] + DstBlock "f_star" + DstPort 1 + } + Branch { + ZOrder 383 + DstBlock "Product1" + DstPort 2 + } + } + Line { + ZOrder 406 + SrcBlock "f_LDot is f_star while on One Foot" + SrcPort 1 + DstBlock "Add1" + DstPort 1 + } + Line { + ZOrder 66 + SrcBlock "Product2" + SrcPort 1 + DstBlock "Add1" + DstPort 2 + } + Line { + ZOrder 292 + SrcBlock "Sigma" + SrcPort 1 + DstBlock "Product1" + DstPort 1 + } + Line { + ZOrder 355 + SrcBlock "QPSolver" + SrcPort 3 + DstBlock "Product2" + DstPort 2 + } + Line { + ZOrder 346 + SrcBlock "gradientOneFoot" + SrcPort 1 + DstBlock "QPSolver" + DstPort 3 + } + Line { + ZOrder 351 + SrcBlock "ConstraintsMatrixTwoFeet" + SrcPort 1 + DstBlock "QPSolver" + DstPort 8 + } + Line { + ZOrder 344 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock "QPSolver" + DstPort 1 + } + Line { + ZOrder 348 + SrcBlock "bVectorConstraintsOneFoot" + SrcPort 1 + DstBlock "QPSolver" + DstPort 5 + } + Line { + ZOrder 345 + SrcBlock "HessianMatrixOneFoot" + SrcPort 1 + DstBlock "QPSolver" + DstPort 2 + } + Line { + ZOrder 347 + SrcBlock "ConstraintsMatrixOneFoot" + SrcPort 1 + DstBlock "QPSolver" + DstPort 4 + } + Line { + ZOrder 350 + SrcBlock "gradientTwoFeet" + SrcPort 1 + DstBlock "QPSolver" + DstPort 7 + } + Line { + ZOrder 349 + SrcBlock "HessianMatrixTwoFeet" + SrcPort 1 + DstBlock "QPSolver" + DstPort 6 + } + Line { + ZOrder 352 + SrcBlock "bVectorConstraintsTwoFeet" + SrcPort 1 + DstBlock "QPSolver" + DstPort 9 + } + Line { + ZOrder 376 + SrcBlock "QPSolver" + SrcPort 2 + Points [27, 0; 0, -60] + DstBlock "f_LDot is f_star while on One Foot" + DstPort 2 + } + Line { + ZOrder 378 + SrcBlock "f_LDot" + SrcPort 1 + DstBlock "f_LDot is f_star while on One Foot" + DstPort 3 + } + Line { + ZOrder 405 + SrcBlock "QPSolver" + SrcPort 1 + DstBlock "f_LDot is f_star while on One Foot" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Compute angular momentum integral error" + SID "3714" + Ports [4, 1] + Position [1250, 289, 1420, 331] + ZOrder 835 + RequestExecContextInheritance off + System { + Name "Compute angular momentum integral error" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "208" + 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" + ContentPreviewEnabled off + } + Block { + BlockType SubSystem + Name "Compute base to fixed link transform" + SID "4422" + Ports [1, 2] + Position [-120, -229, 5, -51] + ZOrder 901 + RequestExecContextInheritance off + System { + Name "Compute base to fixed link transform" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "723" + 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" + 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" + 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" + } + Block { + BlockType Outport + Name "fixed_r_sole_H_b" + SID "4477" + Position [495, 93, 525, 107] + ZOrder 912 + Port "2" + IconDisplay "Port number" + } + 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 "3783" + 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::3782" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 107 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "3721::3781" + Tag "Stateflow S-Function torqueControlBalancing 20" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 106 + 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::3783" + Position [460, 241, 480, 259] + ZOrder 108 + } + Block { + BlockType Outport + Name "baseVel_equivalent" + SID "3721::24" + Position [460, 101, 480, 119] + ZOrder 10 + IconDisplay "Port number" + } + Line { + ZOrder 169 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 170 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 171 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 172 + SrcBlock "jointPos_err" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "baseVel_equivalent" + ZOrder 173 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "baseVel_equivalent" + DstPort 1 + } + Line { + ZOrder 174 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 175 + 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" + 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" + 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 "96" + 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::95" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 86 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4027::94" + Tag "Stateflow S-Function torqueControlBalancing 19" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 85 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "booleanState" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4027::96" + Position [460, 241, 480, 259] + ZOrder 87 + } + Block { + BlockType Outport + Name "booleanState" + SID "4027::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 97 + SrcBlock "state" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "booleanState" + ZOrder 98 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "booleanState" + 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 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" + } + Line { + ZOrder 1 + SrcBlock "jointPos_des" + SrcPort 1 + Points [37, 0] + Branch { + ZOrder 121 + Points [0, 115] + Branch { + ZOrder 136 + Points [0, 65] + Branch { + ZOrder 131 + Points [0, 35] + Branch { + ZOrder 133 + Points [0, 50] + DstBlock "Add" + DstPort 1 + } + Branch { + ZOrder 126 + DstBlock "Jacobian1" + DstPort 2 + } + } + Branch { + ZOrder 124 + DstBlock "Jacobian" + DstPort 2 + } + } + Branch { + ZOrder 123 + DstBlock "CentroidalMomentum" + DstPort 2 + } + } + Branch { + ZOrder 90 + DstBlock "Compute base to fixed link transform" + DstPort 1 + } + } + Line { + ZOrder 127 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "Add" + DstPort 2 + } + Line { + ZOrder 129 + SrcBlock "Add" + SrcPort 1 + Points [757, 0] + Branch { + ZOrder 142 + DstBlock "Get Equivalent Base Velocity" + DstPort 4 + } + Branch { + ZOrder 141 + Points [0, 65] + DstBlock "CentroidalMomentum" + DstPort 4 + } + } + Line { + ZOrder 16 + SrcBlock "Get Equivalent Base Velocity" + SrcPort 1 + DstBlock "CentroidalMomentum" + DstPort 3 + } + Line { + ZOrder 18 + SrcBlock "CentroidalMomentum" + SrcPort 1 + DstBlock "Selector" + DstPort 1 + } + Line { + ZOrder 19 + SrcBlock "Selector" + SrcPort 1 + DstBlock "intL_angMomError" + DstPort 1 + } + Line { + ZOrder 25 + SrcBlock "state" + SrcPort 1 + DstBlock "Select base to world transform" + DstPort 1 + } + Line { + ZOrder 27 + SrcBlock "Select base to world transform" + SrcPort 1 + DstBlock "Switch" + DstPort 2 + } + Line { + ZOrder 28 + SrcBlock "Switch" + SrcPort 1 + Points [30, 0] + Branch { + ZOrder 138 + DstBlock "CentroidalMomentum" + DstPort 1 + } + Branch { + ZOrder 135 + Points [0, 170] + Branch { + ZOrder 139 + DstBlock "Jacobian" + DstPort 1 + } + Branch { + ZOrder 130 + Points [0, 35] + DstBlock "Jacobian1" + DstPort 1 + } + } + } + Line { + ZOrder 82 + SrcBlock "Compute base to fixed link transform" + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + Line { + ZOrder 83 + SrcBlock "Compute base to fixed link transform" + SrcPort 2 + DstBlock "Switch" + DstPort 3 + } + Line { + ZOrder 132 + SrcBlock "Jacobian" + SrcPort 1 + DstBlock "Get Equivalent Base Velocity" + DstPort 1 + } + Line { + ZOrder 134 + SrcBlock "Jacobian1" + SrcPort 1 + DstBlock "Get Equivalent Base Velocity" + DstPort 2 + } + Line { + ZOrder 140 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock "Get Equivalent Base Velocity" + DstPort 3 + } + } + } + Block { + BlockType Constant + Name "Constant" + SID "2371" + Position [1020, -63, 1140, -37] + ZOrder 383 + ShowName off + Value "ConstraintsMatrix" + } + Block { + BlockType Constant + Name "Constant1" + SID "2372" + Position [1020, -18, 1140, 8] + ZOrder 384 + ShowName off + Value "bVectorConstraints" + } + Block { + BlockType SubSystem + Name "From tau_QP to Joint Torques (motor reflected inertia)" + SID "4519" + Ports [1, 1] + Position [2065, 51, 2220, 109] + ZOrder 862 + RequestExecContextInheritance off + System { + Name "From tau_QP to Joint Torques (motor reflected inertia)" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "338" + 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 "102" + Block { + BlockType Demux + Name " Demux " + SID "4551::100" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 91 + Outputs "1" + } + Block { + BlockType Ground + Name " Ground " + SID "4551::102" + Position [20, 121, 40, 139] + ZOrder 93 + } + Block { + BlockType S-Function + Name " SFunction " + SID "4551::99" + Tag "Stateflow S-Function torqueControlBalancing 9" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 90 + 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::101" + Position [460, 241, 480, 259] + ZOrder 92 + } + Block { + BlockType Outport + Name "reflectedInertia" + SID "4551::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + Name "reflectedInertia" + ZOrder 81 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "reflectedInertia" + DstPort 1 + } + Line { + ZOrder 82 + SrcBlock " Ground " + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 83 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 84 + 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 From + Name "From" + SID "4654" + Position [-460, -158, -375, -142] + ZOrder 902 + ShowName off + HideAutomaticName off + GotoTag "jointAcc_des" + TagVisibility "global" + } + Block { + BlockType From + Name "From1" + SID "4830" + Position [-460, -108, -380, -92] + ZOrder 903 + ShowName off + HideAutomaticName off + GotoTag "jointAcc" + TagVisibility "global" + } + Block { + BlockType Gain + Name "Gain" + SID "4525" + Position [-60, -175, 30, -145] + ZOrder 596 + ShowName off + HideAutomaticName off + Gain "Config.K_ff" + 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" + } + Line { + ZOrder 6 + SrcBlock "Product" + SrcPort 1 + DstBlock "Gain" + DstPort 1 + } + Line { + ZOrder 16 + SrcBlock "Gain" + SrcPort 1 + DstBlock "Add" + DstPort 1 + } + Line { + ZOrder 23 + SrcBlock "Add" + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + Line { + ZOrder 24 + SrcBlock " " + SrcPort 1 + DstBlock "Switch" + DstPort 2 + } + Line { + ZOrder 41 + SrcBlock "Switch" + SrcPort 1 + DstBlock "tau_joints" + DstPort 1 + } + Line { + ZOrder 20 + SrcBlock "tau_QP" + SrcPort 1 + Points [24, 0] + Branch { + ZOrder 62 + Points [0, -75] + DstBlock "Add" + DstPort 2 + } + Branch { + ZOrder 93 + DstBlock "Switch" + DstPort 3 + } + } + Line { + ZOrder 52 + SrcBlock "(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" + SrcPort 1 + DstBlock "Product" + DstPort 1 + } + Line { + ZOrder 108 + SrcBlock " 1" + SrcPort 1 + DstBlock "Switch1" + DstPort 2 + } + Line { + ZOrder 109 + SrcBlock "Switch1" + SrcPort 1 + DstBlock "Product" + DstPort 2 + } + Line { + ZOrder 114 + SrcBlock "From" + SrcPort 1 + DstBlock "Switch1" + DstPort 1 + } + Line { + ZOrder 115 + SrcBlock "From1" + SrcPort 1 + DstBlock "Switch1" + DstPort 3 + } + } + } + Block { + BlockType Goto + Name "Goto1" + SID "4495" + Position [2320, 16, 2390, 34] + ZOrder 856 + ShowName off + HideAutomaticName off + GotoTag "tau_star" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto5" + SID "4499" + Position [2035, 596, 2095, 614] + ZOrder 860 + ShowName off + HideAutomaticName off + GotoTag "f_star" + TagVisibility "global" + } + Block { + BlockType SubSystem + Name "Momentum Based Balancing Controller\n" + SID "2407" + Ports [22, 12] + Position [1465, -117, 1740, 872] + ZOrder 278 + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "Momentum Based Balancing Controller\n" + Location [219, 337, 814, 775] + Open off + PortBlocksUseCompactNotation off + 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 "3782" + 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::3781" + Ports [1, 1] + Position [270, 570, 320, 610] + ZOrder 184 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "2407::3780" + Tag "Stateflow S-Function torqueControlBalancing 17" + Ports [22, 13] + Position [180, 70, 230, 530] + ZOrder 183 + 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::3782" + Position [460, 581, 480, 599] + ZOrder 185 + } + Block { + BlockType Outport + Name "HessianMatrixOneFoot" + SID "2407::824" + Position [460, 101, 480, 119] + ZOrder 72 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "gradientOneFoot" + SID "2407::822" + Position [460, 136, 480, 154] + ZOrder 70 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "ConstraintsMatrixOneFoot" + SID "2407::5" + Position [460, 171, 480, 189] + ZOrder -5 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "bVectorConstraintsOneFoot" + SID "2407::810" + Position [460, 206, 480, 224] + ZOrder 58 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "HessianMatrixTwoFeet" + SID "2407::827" + Position [460, 246, 480, 264] + ZOrder 75 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "gradientTwoFeet" + SID "2407::828" + Position [460, 281, 480, 299] + ZOrder 76 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "ConstraintsMatrixTwoFeet" + SID "2407::811" + Position [460, 316, 480, 334] + ZOrder 59 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "bVectorConstraintsTwoFeet" + SID "2407::812" + Position [460, 351, 480, 369] + ZOrder 60 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "tauModel" + SID "2407::815" + Position [460, 386, 480, 404] + ZOrder 63 + Port "9" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "Sigma" + SID "2407::816" + Position [460, 426, 480, 444] + ZOrder 64 + Port "10" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "Na" + SID "2407::820" + Position [460, 461, 480, 479] + ZOrder 68 + Port "11" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "f_LDot" + SID "2407::821" + Position [460, 496, 480, 514] + ZOrder 69 + Port "12" + IconDisplay "Port number" + } + Line { + ZOrder 2078 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 2079 + SrcBlock "ConstraintsMatrix" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 2080 + SrcBlock "bVectorConstraints" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 2081 + SrcBlock "jointPos" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 2082 + SrcBlock "jointPos_des" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 2083 + SrcBlock "nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + ZOrder 2084 + SrcBlock "M" + SrcPort 1 + DstBlock " SFunction " + DstPort 7 + } + Line { + ZOrder 2085 + SrcBlock "h" + SrcPort 1 + DstBlock " SFunction " + DstPort 8 + } + Line { + ZOrder 2086 + SrcBlock "L" + SrcPort 1 + DstBlock " SFunction " + DstPort 9 + } + Line { + ZOrder 2087 + SrcBlock "intL_angMomError" + SrcPort 1 + DstBlock " SFunction " + DstPort 10 + } + Line { + ZOrder 2088 + SrcBlock "w_H_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 11 + } + Line { + ZOrder 2089 + SrcBlock "w_H_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 12 + } + Line { + ZOrder 2090 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 13 + } + Line { + ZOrder 2091 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 14 + } + Line { + ZOrder 2092 + SrcBlock "JDot_l_sole_nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 15 + } + Line { + ZOrder 2093 + SrcBlock "JDot_r_sole_nu" + SrcPort 1 + DstBlock " SFunction " + DstPort 16 + } + Line { + ZOrder 2094 + SrcBlock "pos_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 17 + } + Line { + ZOrder 2095 + SrcBlock "J_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 18 + } + Line { + ZOrder 2096 + SrcBlock "desired_pos_vel_acc_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 19 + } + Line { + ZOrder 2097 + SrcBlock "KP_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 20 + } + Line { + ZOrder 2098 + SrcBlock "KD_CoM" + SrcPort 1 + DstBlock " SFunction " + DstPort 21 + } + Line { + ZOrder 2099 + SrcBlock "KP_postural" + SrcPort 1 + DstBlock " SFunction " + DstPort 22 + } + Line { + Name "HessianMatrixOneFoot" + ZOrder 2100 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "HessianMatrixOneFoot" + DstPort 1 + } + Line { + Name "gradientOneFoot" + ZOrder 2101 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "gradientOneFoot" + DstPort 1 + } + Line { + Name "ConstraintsMatrixOneFoot" + ZOrder 2102 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "ConstraintsMatrixOneFoot" + DstPort 1 + } + Line { + Name "bVectorConstraintsOneFoot" + ZOrder 2103 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "bVectorConstraintsOneFoot" + DstPort 1 + } + Line { + Name "HessianMatrixTwoFeet" + ZOrder 2104 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "HessianMatrixTwoFeet" + DstPort 1 + } + Line { + Name "gradientTwoFeet" + ZOrder 2105 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 7 + DstBlock "gradientTwoFeet" + DstPort 1 + } + Line { + Name "ConstraintsMatrixTwoFeet" + ZOrder 2106 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 8 + DstBlock "ConstraintsMatrixTwoFeet" + DstPort 1 + } + Line { + Name "bVectorConstraintsTwoFeet" + ZOrder 2107 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 9 + DstBlock "bVectorConstraintsTwoFeet" + DstPort 1 + } + Line { + Name "tauModel" + ZOrder 2108 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 10 + DstBlock "tauModel" + DstPort 1 + } + Line { + Name "Sigma" + ZOrder 2109 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 11 + DstBlock "Sigma" + DstPort 1 + } + Line { + Name "Na" + ZOrder 2110 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 12 + DstBlock "Na" + DstPort 1 + } + Line { + Name "f_LDot" + ZOrder 2111 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 13 + DstBlock "f_LDot" + DstPort 1 + } + Line { + ZOrder 2112 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 2113 + 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" + } + 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 [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 "312" + 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 [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 "522" + 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 [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 "634" + 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 "82" + 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::81" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 71 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4518::80" + Tag "Stateflow S-Function torqueControlBalancing 6" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 70 + 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::82" + Position [460, 241, 480, 259] + ZOrder 72 + } + Block { + BlockType Outport + Name "M_with_inertia" + SID "4518::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 94 + SrcBlock "M" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "M_with_inertia" + ZOrder 95 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "M_with_inertia" + DstPort 1 + } + Line { + ZOrder 96 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 97 + 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" + } + 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" + 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" + 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" + ContentPreviewEnabled off + } + Block { + BlockType Outport + Name "M" + SID "4357" + Position [750, 48, 780, 62] + ZOrder 587 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "h" + SID "4358" + Position [625, 178, 655, 192] + ZOrder 588 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "L" + SID "4359" + Position [625, 308, 655, 322] + ZOrder 589 + Port "3" + IconDisplay "Port number" + } + 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 [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 "365" + 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" + 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\n" + SID "2375" + Ports [4, 1] + Position [60, 382, 235, 443] + ZOrder 829 + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Jacobians/DotJNu" + SourceType "DotJNu" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + frameName "Frames.LEFT_FOOT" + } + Block { + BlockType Reference + Name "DotJNu r_sole\n " + SID "2376" + Ports [4, 1] + Position [65, 464, 235, 521] + ZOrder 831 + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Jacobians/DotJNu" + SourceType "DotJNu" + SourceProductName "WholeBodyToolbox" + 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" + 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" + 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" + 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" + 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" + 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" + } + Block { + BlockType Outport + Name "w_H_r_sole" + SID "4367" + Position [380, 108, 410, 122] + ZOrder 874 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "J_l_sole" + SID "4368" + Position [380, 173, 410, 187] + ZOrder 875 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "J_r_sole" + SID "4369" + Position [380, 223, 410, 237] + ZOrder 876 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "J_CoM" + SID "4370" + Position [380, 273, 410, 287] + ZOrder 877 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "pos_CoM" + SID "4371" + Position [380, 333, 410, 347] + ZOrder 878 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "JDot_l_sole_nu" + SID "4372" + Position [380, 408, 410, 422] + ZOrder 879 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "JDot_r_sole_nu" + SID "4374" + Position [380, 488, 410, 502] + ZOrder 881 + Port "8" + IconDisplay "Port number" + } + Line { + ZOrder 37 + SrcBlock "CoM6D -> \nCoMXYZ3" + SrcPort 1 + Points [57, 0] + Branch { + ZOrder 203 + DstBlock "DotJNu r_sole\n " + DstPort 4 + } + Branch { + ZOrder 193 + Points [0, -80] + DstBlock "DotJNu l_sole\n" + DstPort 4 + } + } + Line { + ZOrder 62 + SrcBlock "CoM6D -> \nCoMXYZ4" + SrcPort 1 + Points [80, 0] + Branch { + ZOrder 206 + DstBlock "DotJNu r_sole\n " + DstPort 3 + } + Branch { + ZOrder 204 + Points [0, -80] + DstBlock "DotJNu l_sole\n" + 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\n" + SrcPort 1 + DstBlock "JDot_l_sole_nu" + DstPort 1 + } + Line { + ZOrder 218 + SrcBlock "DotJNu r_sole\n " + 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\n " + DstPort 2 + } + Branch { + ZOrder 214 + DstBlock "DotJNu l_sole\n" + 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\n " + DstPort 1 + } + Branch { + ZOrder 220 + DstBlock "DotJNu l_sole\n" + 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" + } + Block { + BlockType Outport + Name "h" + SID "2351" + Position [-475, -97, -445, -83] + ZOrder 217 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "L" + SID "2352" + Position [-475, -27, -445, -13] + ZOrder 216 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "w_H_l_sole" + SID "4375" + Position [-185, 43, -155, 57] + ZOrder 882 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "w_H_r_sole" + SID "4376" + Position [-185, 83, -155, 97] + ZOrder 883 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "J_l_sole" + SID "4377" + Position [-185, 123, -155, 137] + ZOrder 884 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "J_r_sole" + SID "4378" + Position [-185, 163, -155, 177] + ZOrder 885 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "J_CoM" + SID "4379" + Position [-185, 203, -155, 217] + ZOrder 886 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "pos_CoM" + SID "4380" + Position [-185, 243, -155, 257] + ZOrder 887 + Port "9" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "JDot_l_sole_nu" + SID "4381" + Position [-185, 283, -155, 297] + ZOrder 888 + Port "10" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "JDot_r_sole_nu" + SID "4382" + Position [-185, 323, -155, 337] + ZOrder 889 + Port "11" + IconDisplay "Port number" + } + Line { + ZOrder 109 + SrcBlock "w_H_b" + SrcPort 1 + Points [25, 0] + Branch { + ZOrder 137 + Points [0, 460] + DstBlock "Kinematics" + DstPort 3 + } + Branch { + ZOrder 135 + DstBlock "Dynamics" + DstPort 1 + } + } + Line { + ZOrder 110 + SrcBlock "jointPos" + SrcPort 1 + Points [62, 0] + Branch { + ZOrder 158 + DstBlock "Dynamics" + DstPort 2 + } + Branch { + ZOrder 138 + Points [0, 280] + DstBlock "Kinematics" + DstPort 2 + } + } + Line { + ZOrder 111 + SrcBlock "nu" + SrcPort 1 + Points [106, 0] + Branch { + ZOrder 159 + DstBlock "Dynamics" + DstPort 3 + } + Branch { + ZOrder 139 + Points [0, 100] + DstBlock "Kinematics" + DstPort 1 + } + } + Line { + ZOrder 112 + SrcBlock "Dynamics" + SrcPort 1 + DstBlock "M" + DstPort 1 + } + Line { + ZOrder 113 + SrcBlock "Dynamics" + SrcPort 2 + DstBlock "h" + DstPort 1 + } + Line { + ZOrder 114 + SrcBlock "Dynamics" + SrcPort 3 + DstBlock "L" + DstPort 1 + } + Line { + ZOrder 129 + SrcBlock "Kinematics" + SrcPort 7 + DstBlock "JDot_l_sole_nu" + DstPort 1 + } + Line { + ZOrder 130 + SrcBlock "Kinematics" + SrcPort 8 + DstBlock "JDot_r_sole_nu" + DstPort 1 + } + Line { + ZOrder 126 + SrcBlock "Kinematics" + SrcPort 4 + DstBlock "J_r_sole" + DstPort 1 + } + Line { + ZOrder 123 + SrcBlock "Kinematics" + SrcPort 1 + DstBlock "w_H_l_sole" + DstPort 1 + } + Line { + ZOrder 125 + SrcBlock "Kinematics" + SrcPort 3 + DstBlock "J_l_sole" + DstPort 1 + } + Line { + ZOrder 127 + SrcBlock "Kinematics" + SrcPort 5 + DstBlock "J_CoM" + DstPort 1 + } + Line { + ZOrder 128 + SrcBlock "Kinematics" + SrcPort 6 + DstBlock "pos_CoM" + DstPort 1 + } + Line { + ZOrder 124 + SrcBlock "Kinematics" + SrcPort 2 + DstBlock "w_H_r_sole" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Joint Torque Saturation" + SID "4553" + Ports [1, 1] + Position [995, 301, 1105, 339] + ZOrder 555 + BackgroundColor "orange" + RequestExecContextInheritance off + System { + Name "Joint Torque Saturation" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "502" + 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 "77" + 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::76" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 65 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4556::75" + Tag "Stateflow S-Function torqueControlBalancing 11" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 64 + FunctionName "sf_sfun" + PortCounts "[4 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "uSat" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4556::77" + Position [460, 241, 480, 259] + ZOrder 66 + } + Block { + BlockType Outport + Name "uSat" + SID "4556::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 133 + SrcBlock "u" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 134 + SrcBlock "u_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 135 + SrcBlock "tStep" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 136 + SrcBlock "uDotMax" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "uSat" + ZOrder 137 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "uSat" + DstPort 1 + } + Line { + ZOrder 138 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 139 + 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" + ContentPreviewEnabled off + } + Block { + BlockType Outport + Name "tau_star_sat" + SID "4555" + Position [545, 88, 575, 102] + ZOrder -2 + IconDisplay "Port number" + } + 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 "3806" + 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::3805" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 174 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4215::3804" + Tag "Stateflow S-Function torqueControlBalancing 7" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 173 + 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::3806" + Position [460, 241, 480, 259] + ZOrder 175 + } + Block { + BlockType Outport + Name "nu_b" + SID "4215::5" + Position [460, 101, 480, 119] + ZOrder -6 + IconDisplay "Port number" + } + Line { + ZOrder 22 + SrcBlock "J_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 23 + SrcBlock "J_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 24 + SrcBlock "feetContactStatus" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 25 + SrcBlock "jointVel" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "nu_b" + ZOrder 26 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "nu_b" + DstPort 1 + } + Line { + ZOrder 27 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 28 + 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" + 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" + 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" + } + Block { + BlockType Outport + Name "J_r_sole" + SID "4222" + Position [935, 588, 965, 602] + ZOrder 677 + Port "2" + IconDisplay "Port number" + } + 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" + } + 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 [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 "248" + 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" + 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" + 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 "3797" + 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::3796" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 196 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4257::3795" + Tag "Stateflow S-Function torqueControlBalancing 8" + Ports [6, 2] + Position [180, 102, 230, 243] + ZOrder 195 + 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::3797" + Position [460, 241, 480, 259] + ZOrder 197 + } + Block { + BlockType Outport + Name "w_H_b" + SID "4257::106" + Position [460, 101, 480, 119] + ZOrder 80 + IconDisplay "Port number" + } + Line { + ZOrder 28 + SrcBlock "imu_H_fixedLink" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 29 + SrcBlock "imu_H_fixedLink_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 30 + SrcBlock "fixedLink_H_base" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 31 + SrcBlock "rpyFromIMU_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 32 + SrcBlock "rpyFromIMU" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 33 + SrcBlock "neck_pos" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + Name "w_H_b" + ZOrder 34 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "w_H_b" + 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 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" + MultiThreadCoSim "auto" + portName "Ports.NECK_POS" + signalSize "Ports.NECK_POS_PORT_SIZE" + timeout "0.5" + blocking off + timestamp off + autoconnect on + errorOnConnection off + } + Block { + BlockType Selector + Name "Selector1" + SID "4849" + Ports [1, 1] + Position [1250, 260, 1275, 280] + ZOrder 749 + ShowName off + InputPortWidth "Ports.IMU_PORT_SIZE" + IndexOptions "Index vector (dialog)" + Indices "Ports.IMU_PORT_ORIENTATION_INDEX" + OutputSizes "1" + } + Block { + BlockType Selector + Name "Selector2" + SID "4848" + Ports [1, 1] + Position [1250, 210, 1275, 230] + ZOrder 747 + ShowName off + InputPortWidth "Ports.IMU_PORT_SIZE" + IndexOptions "Index vector (dialog)" + Indices "Ports.IMU_PORT_ORIENTATION_INDEX" + OutputSizes "1" + } + Block { + BlockType Switch + Name "Switch6" + SID "4255" + Position [1760, 151, 1840, 419] + ZOrder 734 + ShowName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Constant + Name "USE_IMU4EST_BASE" + SID "4256" + Position [1570, 275, 1725, 295] + ZOrder 735 + ShowName off + Value "Config.USE_IMU4EST_BASE" + } + Block { + BlockType Reference + Name "holder\n1" + SID "4258" + Ports [1, 1] + Position [1175, 113, 1225, 127] + ZOrder 739 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + SourceProductName "WholeBodyToolbox" + 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" + ContentPreviewEnabled off + } + Block { + BlockType SubSystem + Name "neck transform" + SID "4260" + Ports [1, 1] + Position [1135, 305, 1260, 335] + ZOrder 743 + RequestExecContextInheritance off + System { + Name "neck transform" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "843" + 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" + } + 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" + } + Line { + ZOrder 1 + SrcBlock "Switch6" + SrcPort 1 + DstBlock "link_H_b" + DstPort 1 + } + Line { + Name "link_H_root" + ZOrder 2 + Labels [0, 1] + SrcBlock "Inv_2" + SrcPort 1 + Points [65, 0] + Branch { + ZOrder 3 + Points [0, 165] + DstBlock "Switch6" + DstPort 3 + } + Branch { + ZOrder 4 + Points [0, -40] + DstBlock "Get Base Rotation From IMU" + DstPort 3 + } + } + Line { + ZOrder 5 + SrcBlock "USE_IMU4EST_BASE" + SrcPort 1 + DstBlock "Switch6" + DstPort 2 + } + Line { + Name "imu_H_link" + ZOrder 6 + Labels [0, 0] + SrcBlock "Inv" + SrcPort 1 + Points [65, 0] + Branch { + ZOrder 7 + DstBlock "Get Base Rotation From IMU" + DstPort 1 + } + Branch { + ZOrder 8 + Points [0, 50] + DstBlock "holder\n1" + DstPort 1 + } + } + Line { + ZOrder 9 + SrcBlock "holder\n2" + SrcPort 1 + DstBlock "Selector2" + DstPort 1 + } + Line { + ZOrder 10 + SrcBlock "Fixed base to imu transform" + SrcPort 1 + DstBlock "Inv" + DstPort 1 + } + Line { + ZOrder 12 + SrcBlock "jointPos" + SrcPort 1 + Points [42, 0] + Branch { + ZOrder 13 + Points [0, -165] + DstBlock "Fixed base to imu transform" + DstPort 2 + } + Branch { + ZOrder 14 + DstBlock "Fixed base to root link transform" + DstPort 2 + } + } + Line { + ZOrder 15 + SrcBlock "Get Base Rotation From IMU" + SrcPort 1 + DstBlock "Switch6" + DstPort 1 + } + Line { + ZOrder 16 + SrcBlock "Fixed base to root link transform" + SrcPort 1 + DstBlock "Inv_2" + DstPort 2 + } + Line { + ZOrder 17 + SrcBlock "inertial" + SrcPort 1 + Points [96, 0] + Branch { + ZOrder 18 + DstBlock "holder\n2" + DstPort 1 + } + Branch { + ZOrder 19 + Points [0, 50] + DstBlock "Selector1" + DstPort 1 + } + } + Line { + ZOrder 20 + SrcBlock "Neck Position" + SrcPort 1 + DstBlock "neck transform" + DstPort 1 + } + Line { + ZOrder 21 + SrcBlock "neck transform" + SrcPort 1 + DstBlock "Get Base Rotation From IMU" + DstPort 6 + } + Line { + ZOrder 22 + SrcBlock "w_H_link" + SrcPort 1 + Points [98, 0] + Branch { + ZOrder 23 + Points [0, 55] + DstBlock "Inv_2" + DstPort 1 + } + Branch { + ZOrder 24 + Points [0, -60] + DstBlock "Inv" + DstPort 2 + } + } + Line { + ZOrder 25 + SrcBlock "w_H_base_fixed" + SrcPort 1 + Points [57, 0] + Branch { + ZOrder 26 + Points [0, 165] + DstBlock "Fixed base to root link transform" + DstPort 1 + } + Branch { + ZOrder 27 + DstBlock "Fixed base to imu transform" + DstPort 1 + } + } + Line { + ZOrder 28 + SrcBlock "Selector2" + SrcPort 1 + DstBlock "Get Base Rotation From IMU" + DstPort 4 + } + Line { + ZOrder 30 + SrcBlock "holder\n1" + SrcPort 1 + DstBlock "Get Base Rotation From IMU" + DstPort 2 + } + Line { + ZOrder 31 + SrcBlock "Selector1" + SrcPort 1 + DstBlock "Get Base Rotation From IMU" + DstPort 5 + } + } + } + Block { + BlockType SubSystem + Name "RFoot to base link transform" + SID "4850" + Ports [4, 1] + Position [455, 100, 600, 140] + ZOrder 956 + RequestExecContextInheritance off + System { + Name "RFoot to base link transform" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "248" + 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" + 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" + 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 "3797" + 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::3796" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 196 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4857::3795" + Tag "Stateflow S-Function torqueControlBalancing 3" + Ports [6, 2] + Position [180, 102, 230, 243] + ZOrder 195 + 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::3797" + Position [460, 241, 480, 259] + ZOrder 197 + } + Block { + BlockType Outport + Name "w_H_b" + SID "4857::106" + Position [460, 101, 480, 119] + ZOrder 80 + IconDisplay "Port number" + } + Line { + ZOrder 28 + SrcBlock "imu_H_fixedLink" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 29 + SrcBlock "imu_H_fixedLink_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 30 + SrcBlock "fixedLink_H_base" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 31 + SrcBlock "rpyFromIMU_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 32 + SrcBlock "rpyFromIMU" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 33 + SrcBlock "neck_pos" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + Name "w_H_b" + ZOrder 34 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "w_H_b" + 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 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" + MultiThreadCoSim "auto" + portName "Ports.NECK_POS" + signalSize "Ports.NECK_POS_PORT_SIZE" + timeout "0.5" + blocking off + timestamp off + autoconnect on + errorOnConnection off + } + Block { + BlockType Selector + Name "Selector1" + SID "4861" + Ports [1, 1] + Position [1250, 260, 1275, 280] + ZOrder 749 + ShowName off + InputPortWidth "Ports.IMU_PORT_SIZE" + IndexOptions "Index vector (dialog)" + Indices "Ports.IMU_PORT_ORIENTATION_INDEX" + OutputSizes "1" + } + Block { + BlockType Selector + Name "Selector2" + SID "4862" + Ports [1, 1] + Position [1250, 210, 1275, 230] + ZOrder 747 + ShowName off + InputPortWidth "Ports.IMU_PORT_SIZE" + IndexOptions "Index vector (dialog)" + Indices "Ports.IMU_PORT_ORIENTATION_INDEX" + OutputSizes "1" + } + Block { + BlockType Switch + Name "Switch6" + SID "4863" + Position [1760, 151, 1840, 419] + ZOrder 734 + ShowName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Constant + Name "USE_IMU4EST_BASE" + SID "4864" + Position [1570, 275, 1725, 295] + ZOrder 735 + ShowName off + Value "Config.USE_IMU4EST_BASE" + } + Block { + BlockType Reference + Name "holder\n1" + SID "4865" + Ports [1, 1] + Position [1175, 113, 1225, 127] + ZOrder 739 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/holder\n" + SourceType "Holder" + SourceProductName "WholeBodyToolbox" + 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" + ContentPreviewEnabled off + } + Block { + BlockType SubSystem + Name "neck transform" + SID "4867" + Ports [1, 1] + Position [1135, 305, 1260, 335] + ZOrder 743 + RequestExecContextInheritance off + System { + Name "neck transform" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "843" + 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" + } + 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" + } + 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" + 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" + 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" + } + Block { + BlockType Outport + Name "fixed_r_sole_H_b" + SID "4320" + Position [650, 113, 680, 127] + ZOrder 912 + Port "2" + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "Constant7" + SrcPort 1 + Points [10, 0] + Branch { + ZOrder 2 + Points [0, -22; 255, 0; 0, 42] + Branch { + ZOrder 34 + Points [0, 85] + DstBlock "RFoot to base link transform" + DstPort 4 + } + Branch { + ZOrder 4 + DstBlock "LFoot to base link transform " + DstPort 4 + } + } + Branch { + ZOrder 5 + Points [0, 85] + DstBlock "Relative transform r_sole_H_base" + DstPort 1 + } + Branch { + ZOrder 6 + DstBlock "Relative transform l_sole_H_base" + DstPort 1 + } + } + Line { + ZOrder 35 + SrcBlock "RFoot to base link transform" + SrcPort 1 + DstBlock "fixed_r_sole_H_b" + DstPort 1 + } + Line { + ZOrder 8 + SrcBlock "LFoot to base link transform " + SrcPort 1 + DstBlock "fixed_l_sole_H_b" + DstPort 1 + } + Line { + ZOrder 9 + SrcBlock "Relative transform l_sole_H_base" + SrcPort 1 + DstBlock "LFoot to base link transform " + DstPort 3 + } + Line { + ZOrder 33 + SrcBlock "Relative transform r_sole_H_base" + SrcPort 1 + DstBlock "RFoot to base link transform" + DstPort 3 + } + Line { + ZOrder 11 + SrcBlock "jointPos" + SrcPort 1 + Points [51, 0] + Branch { + ZOrder 12 + Points [0, 85] + Branch { + ZOrder 13 + Points [0, 37] + Branch { + ZOrder 22 + Points [0, 23] + DstBlock "Goto" + DstPort 1 + } + Branch { + ZOrder 21 + Points [274, 0; 0, -62] + Branch { + ZOrder 31 + DstBlock "RFoot to base link transform" + DstPort 1 + } + Branch { + ZOrder 14 + Points [0, -85] + DstBlock "LFoot to base link transform " + DstPort 1 + } + } + } + Branch { + ZOrder 16 + DstBlock "Relative transform r_sole_H_base" + DstPort 2 + } + } + Branch { + ZOrder 17 + DstBlock "Relative transform l_sole_H_base" + DstPort 2 + } + } + Line { + ZOrder 18 + SrcBlock "IMU_meas" + SrcPort 1 + Points [104, 0; 0, -75] + Branch { + ZOrder 32 + DstBlock "RFoot to base link transform" + DstPort 2 + } + Branch { + ZOrder 19 + Points [0, -85] + DstBlock "LFoot to base link transform " + DstPort 2 + } + } + } + } + Block { + BlockType Goto + Name "Goto3" + SID "4497" + Position [10, 2913, 65, 2927] + ZOrder 864 + ShowName off + HideAutomaticName off + GotoTag "state" + TagVisibility "global" + } + Block { + BlockType Goto + Name "Goto5" + SID "4799" + Position [330, 2497, 380, 2513] + ZOrder 956 + ShowName off + GotoTag "nu" + TagVisibility "global" + } + Block { + BlockType SubSystem + Name "State Machine" + SID "2163" + Ports [5, 10] + Position [-275, 2597, -65, 2883] + ZOrder 506 + ForegroundColor "red" + Priority "-100" + RequestExecContextInheritance off + System { + Name "State Machine" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "292" + 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" + MultiThreadCoSim "auto" + externalSettlingTime off + settlingTime "Config.SmoothingTimeGainScheduling" + sampleTime "Config.tStep" + resetOnSettlingTime off + firstDerivatives off + secondDerivatives off + explicitInitialValue off + } + Block { + BlockType Mux + Name "Mux" + SID "3689" + Ports [3, 1] + Position [305, 250, 310, 360] + ZOrder 694 + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType SubSystem + Name "STATE MACHINE" + SID "2220" + Ports [10, 10] + Position [-65, 107, 250, 468] + ZOrder 497 + ForegroundColor "red" + LibraryVersion "1.30" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + System { + Name "STATE MACHINE" + Location [219, 337, 814, 775] + Open off + PortBlocksUseCompactNotation off + 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 "3785" + 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::3784" + Ports [1, 1] + Position [270, 495, 320, 535] + ZOrder 182 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "2220::3783" + Tag "Stateflow S-Function torqueControlBalancing 13" + Ports [10, 11] + Position [180, 100, 230, 340] + ZOrder 181 + 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::3785" + Position [460, 506, 480, 524] + ZOrder 183 + } + Block { + BlockType Outport + Name "w_H_b" + SID "2220::106" + Position [460, 101, 480, 119] + ZOrder 80 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "pos_CoM_des" + SID "2220::77" + Position [460, 136, 480, 154] + ZOrder 53 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "jointPos_des" + SID "2220::67" + Position [460, 171, 480, 189] + ZOrder 43 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "feetContactStatus" + SID "2220::64" + Position [460, 206, 480, 224] + ZOrder 40 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "KP_postural_diag" + SID "2220::94" + Position [460, 246, 480, 264] + ZOrder 70 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "KP_CoM_diag" + SID "2220::112" + Position [460, 281, 480, 299] + ZOrder 84 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "KD_CoM_diag" + SID "2220::113" + Position [460, 316, 480, 334] + ZOrder 85 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "state" + SID "2220::96" + Position [460, 351, 480, 369] + ZOrder 71 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "smoothingTimeJoints" + SID "2220::111" + Position [460, 386, 480, 404] + ZOrder 83 + Port "9" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "smoothingTimeCoM" + SID "2220::3776" + Position [460, 426, 480, 444] + ZOrder 174 + Port "10" + IconDisplay "Port number" + } + Line { + ZOrder 860 + SrcBlock "pos_CoM_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 861 + SrcBlock "jointPos_0" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 862 + SrcBlock "pos_CoM_fixed_l_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 863 + SrcBlock "pos_CoM_fixed_r_sole" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + ZOrder 864 + SrcBlock "jointPos" + SrcPort 1 + DstBlock " SFunction " + DstPort 5 + } + Line { + ZOrder 865 + SrcBlock "time" + SrcPort 1 + DstBlock " SFunction " + DstPort 6 + } + Line { + ZOrder 866 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock " SFunction " + DstPort 7 + } + Line { + ZOrder 867 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock " SFunction " + DstPort 8 + } + Line { + ZOrder 868 + SrcBlock "l_sole_H_b" + SrcPort 1 + DstBlock " SFunction " + DstPort 9 + } + Line { + ZOrder 869 + SrcBlock "r_sole_H_b" + SrcPort 1 + DstBlock " SFunction " + DstPort 10 + } + Line { + Name "w_H_b" + ZOrder 870 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "w_H_b" + DstPort 1 + } + Line { + Name "pos_CoM_des" + ZOrder 871 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "pos_CoM_des" + DstPort 1 + } + Line { + Name "jointPos_des" + ZOrder 872 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "jointPos_des" + DstPort 1 + } + Line { + Name "feetContactStatus" + ZOrder 873 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "feetContactStatus" + DstPort 1 + } + Line { + Name "KP_postural_diag" + ZOrder 874 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "KP_postural_diag" + DstPort 1 + } + Line { + Name "KP_CoM_diag" + ZOrder 875 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 7 + DstBlock "KP_CoM_diag" + DstPort 1 + } + Line { + Name "KD_CoM_diag" + ZOrder 876 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 8 + DstBlock "KD_CoM_diag" + DstPort 1 + } + Line { + Name "state" + ZOrder 877 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 9 + DstBlock "state" + DstPort 1 + } + Line { + Name "smoothingTimeJoints" + ZOrder 878 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 10 + DstBlock "smoothingTimeJoints" + DstPort 1 + } + Line { + Name "smoothingTimeCoM" + ZOrder 879 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 11 + DstBlock "smoothingTimeCoM" + DstPort 1 + } + Line { + ZOrder 880 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 881 + 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" + 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" + ContentPreviewEnabled off + } + Block { + BlockType SubSystem + Name "xCom" + SID "4226" + Ports [2, 1] + Position [-385, 184, -235, 211] + ZOrder 890 + ShowName off + RequestExecContextInheritance off + System { + Name "xCom" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "824" + 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" + 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" + } + Line { + ZOrder 1 + SrcBlock "jointPos" + SrcPort 1 + DstBlock "CoM" + DstPort 2 + } + Line { + ZOrder 2 + SrcBlock "CoM6D -> \nCoMXYZ2" + SrcPort 1 + DstBlock "pos_CoM_fixed_l_sole " + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "CoM" + SrcPort 1 + DstBlock "CoM6D -> \nCoMXYZ2" + DstPort 1 + } + Line { + ZOrder 4 + SrcBlock "w_H_b" + SrcPort 1 + DstBlock "CoM" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "xCom2" + SID "4732" + Ports [2, 1] + Position [-385, 219, -235, 246] + ZOrder 904 + ShowName off + RequestExecContextInheritance off + System { + Name "xCom2" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "823" + 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" + 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" + } + 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" + } + Block { + BlockType Outport + Name "feetContactStatus" + SID "2236" + Position [580, 229, 610, 241] + ZOrder 480 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "pos_CoM_des" + SID "2234" + Position [365, 158, 395, 172] + ZOrder 478 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "jointPos_des" + SID "2235" + Position [365, 193, 395, 207] + ZOrder 479 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "KP_postural_diag" + SID "2237" + Position [580, 257, 615, 273] + ZOrder 545 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "smoothingTime_Joints" + SID "3181" + Position [365, 403, 395, 417] + ZOrder 680 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "KP_CoM_diag" + SID "3316" + Position [580, 297, 615, 313] + ZOrder 689 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "KD_CoM_diag" + SID "3318" + Position [580, 335, 615, 355] + ZOrder 691 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "smoothingTime_CoM" + SID "4738" + Position [365, 438, 395, 452] + ZOrder 905 + Port "9" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "state" + SID "2238" + Position [365, 367, 395, 383] + ZOrder 550 + Port "10" + IconDisplay "Port number" + } + Line { + ZOrder 173 + SrcBlock "xCom" + SrcPort 1 + Points [13, 0] + Branch { + ZOrder 416 + Points [0, -70] + DstBlock "holder\n1" + DstPort 1 + } + Branch { + ZOrder 415 + DstBlock "STATE MACHINE" + DstPort 3 + } + } + Line { + ZOrder 10 + SrcBlock "STATE MACHINE" + SrcPort 3 + DstBlock "jointPos_des" + DstPort 1 + } + Line { + ZOrder 377 + SrcBlock "STATE MACHINE" + SrcPort 2 + DstBlock "pos_CoM_des" + DstPort 1 + } + Line { + ZOrder 14 + SrcBlock "STATE MACHINE" + SrcPort 8 + DstBlock "state" + DstPort 1 + } + Line { + ZOrder 15 + SrcBlock "STATE MACHINE" + SrcPort 4 + Points [218, 0] + Branch { + ZOrder 459 + DstBlock "feetContactStatus" + DstPort 1 + } + Branch { + ZOrder 458 + Points [0, -80] + DstBlock "Goto" + DstPort 1 + } + } + Line { + ZOrder 53 + SrcBlock "STATE MACHINE" + SrcPort 9 + DstBlock "smoothingTime_Joints" + DstPort 1 + } + Line { + ZOrder 60 + SrcBlock "STATE MACHINE" + SrcPort 6 + DstBlock "Mux" + DstPort 2 + } + Line { + ZOrder 61 + SrcBlock "Demux1" + SrcPort 3 + DstBlock "KD_CoM_diag" + DstPort 1 + } + Line { + ZOrder 62 + SrcBlock "STATE MACHINE" + SrcPort 7 + DstBlock "Mux" + DstPort 3 + } + Line { + ZOrder 63 + SrcBlock "STATE MACHINE" + SrcPort 5 + DstBlock "Mux" + DstPort 1 + } + Line { + ZOrder 64 + SrcBlock "Mux" + SrcPort 1 + DstBlock "MinimumJerkTrajectoryGenerator" + DstPort 1 + } + Line { + ZOrder 65 + SrcBlock "MinimumJerkTrajectoryGenerator" + SrcPort 1 + DstBlock "Demux1" + DstPort 1 + } + Line { + ZOrder 66 + SrcBlock "Demux1" + SrcPort 1 + Points [17, 0] + Branch { + ZOrder 456 + Points [0, -75] + DstBlock "Goto1" + DstPort 1 + } + Branch { + ZOrder 455 + DstBlock "KP_postural_diag" + DstPort 1 + } + } + Line { + ZOrder 67 + SrcBlock "Demux1" + SrcPort 2 + DstBlock "KP_CoM_diag" + DstPort 1 + } + Line { + ZOrder 32 + SrcBlock "jointPos" + SrcPort 1 + Points [54, 0] + Branch { + ZOrder 420 + DstBlock "STATE MACHINE" + DstPort 5 + } + Branch { + ZOrder 419 + Points [0, -30] + Branch { + ZOrder 413 + Points [0, -35] + Branch { + ZOrder 262 + DstBlock "xCom" + DstPort 2 + } + Branch { + ZOrder 205 + Points [0, -40] + DstBlock "holder\n2" + DstPort 1 + } + } + Branch { + ZOrder 256 + DstBlock "xCom2" + DstPort 2 + } + } + } + Line { + ZOrder 368 + SrcBlock "fixed_l_sole_H_b" + SrcPort 1 + Points [83, 0] + Branch { + ZOrder 426 + DstBlock "STATE MACHINE" + DstPort 9 + } + Branch { + ZOrder 424 + Points [0, -220] + DstBlock "xCom" + DstPort 1 + } + } + Line { + ZOrder 369 + SrcBlock "fixed_r_sole_H_b" + SrcPort 1 + Points [105, 0] + Branch { + ZOrder 429 + Points [0, -220] + DstBlock "xCom2" + DstPort 1 + } + Branch { + ZOrder 428 + DstBlock "STATE MACHINE" + DstPort 10 + } + } + Line { + ZOrder 408 + SrcBlock "Clock1" + SrcPort 1 + DstBlock "STATE MACHINE" + DstPort 6 + } + Line { + ZOrder 500 + SrcBlock "wrench_leftFoot" + SrcPort 1 + Points [35, 0] + Branch { + ZOrder 465 + Points [0, -20] + DstBlock "Goto3" + DstPort 1 + } + Branch { + ZOrder 464 + DstBlock "STATE MACHINE" + DstPort 8 + } + } + Line { + ZOrder 411 + SrcBlock "holder\n1" + SrcPort 1 + DstBlock "STATE MACHINE" + DstPort 1 + } + Line { + ZOrder 412 + SrcBlock "holder\n2" + SrcPort 1 + DstBlock "STATE MACHINE" + DstPort 2 + } + Line { + ZOrder 418 + SrcBlock "xCom2" + SrcPort 1 + DstBlock "STATE MACHINE" + DstPort 4 + } + Line { + ZOrder 430 + SrcBlock "STATE MACHINE" + SrcPort 10 + DstBlock "smoothingTime_CoM" + DstPort 1 + } + Line { + ZOrder 376 + SrcBlock "STATE MACHINE" + SrcPort 1 + Points [42, 0] + Branch { + ZOrder 454 + Points [0, -45] + DstBlock "Goto2" + DstPort 1 + } + Branch { + ZOrder 453 + DstBlock "w_H_b" + DstPort 1 + } + } + Line { + ZOrder 499 + SrcBlock "wrench_rightFoot" + SrcPort 1 + Points [37, 0] + Branch { + ZOrder 463 + Points [0, -35] + DstBlock "Goto4" + DstPort 1 + } + Branch { + ZOrder 462 + DstBlock "STATE MACHINE" + DstPort 7 + } + } + } + } + Block { + BlockType SubSystem + Name "Update Gains and References" + SID "4030" + Ports [7, 5] + Position [80, 2650, 285, 2860] + ZOrder 702 + RequestExecContextInheritance off + System { + Name "Update Gains and References" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "498" + 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 [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 "777" + 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 "30" + 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::29" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 20 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4782::28" + Tag "Stateflow S-Function torqueControlBalancing 2" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 19 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "D" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4782::30" + Position [460, 241, 480, 259] + ZOrder 21 + } + Block { + BlockType Outport + Name "D" + SID "4782::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 17 + SrcBlock "d" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "D" + ZOrder 18 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "D" + 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 "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 "30" + 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::29" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 20 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4783::28" + Tag "Stateflow S-Function torqueControlBalancing 1" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 19 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "D" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4783::30" + Position [460, 241, 480, 259] + ZOrder 21 + } + Block { + BlockType Outport + Name "D" + SID "4783::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 13 + SrcBlock "d" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "D" + ZOrder 14 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "D" + 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 "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 "30" + 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::29" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 20 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4784::28" + Tag "Stateflow S-Function torqueControlBalancing 4" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 19 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "D" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4784::30" + Position [460, 241, 480, 259] + ZOrder 21 + } + Block { + BlockType Outport + Name "D" + SID "4784::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 13 + SrcBlock "d" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "D" + ZOrder 14 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "D" + 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 Outport + Name "KP_postural" + SID "4778" + Position [420, -82, 450, -68] + ZOrder 918 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "KP_CoM" + SID "4779" + Position [420, -12, 450, 2] + ZOrder 919 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "KD_CoM" + SID "4780" + Position [420, 63, 450, 77] + ZOrder 920 + Port "3" + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "KP_postural_diag" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "KP_CoM_diag" + SrcPort 1 + DstBlock "MATLAB Function1" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "KD_CoM_diag" + SrcPort 1 + DstBlock "MATLAB Function2" + DstPort 1 + } + Line { + ZOrder 4 + SrcBlock "MATLAB Function" + SrcPort 1 + DstBlock "KP_postural" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "MATLAB Function1" + SrcPort 1 + DstBlock "KP_CoM" + DstPort 1 + } + Line { + ZOrder 6 + SrcBlock "MATLAB Function2" + SrcPort 1 + DstBlock "KD_CoM" + DstPort 1 + } + } + } + Block { + BlockType Selector + Name "Selector1" + SID "4502" + Ports [1, 1] + Position [750, -164, 775, -146] + ZOrder 935 + ShowName off + HideAutomaticName off + NumberOfDimensions "2" + InputPortWidth "3" + IndexOptions "Index vector (dialog),Index vector (dialog)" + Indices "[1 2 3],[1]" + OutputSizes "1,1" + } + Block { + BlockType SubSystem + Name "Smooth reference CoM" + SID "4767" + Ports [2, 1] + Position [455, -147, 660, -78] + ZOrder 932 + RequestExecContextInheritance off + System { + Name "Smooth reference CoM" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + 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 "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" + MultiThreadCoSim "auto" + externalSettlingTime on + settlingTime "0.01" + sampleTime "Config.tStep" + resetOnSettlingTime off + firstDerivatives on + secondDerivatives on + explicitInitialValue off + } + Block { + BlockType Mux + Name "Mux" + SID "2154" + Ports [3, 1] + Position [445, -279, 450, -171] + ZOrder 486 + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType Mux + Name "Mux1" + SID "4883" + Ports [2, 1] + Position [445, -142, 450, -33] + ZOrder 917 + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Reshape + Name "Reshape" + SID "2159" + Ports [1, 1] + Position [845, -175, 890, -135] + ZOrder 456 + ShowName off + OutputDimensionality "Customize" + OutputDimensions "[3,3]" + } + Block { + BlockType Constant + Name "SMOOTH_DES_COM" + SID "2160" + Position [515, -165, 655, -145] + ZOrder 488 + ShowName off + Value "Config.SMOOTH_COM_DES" + } + Block { + BlockType Switch + Name "Switch3" + SID "2246" + Position [695, -263, 785, -47] + ZOrder 487 + ShowName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "desired_pos_vel_acc_CoM" + SID "4773" + Position [965, -162, 995, -148] + ZOrder 915 + IconDisplay "Port number" + } + Line { + ZOrder 615 + SrcBlock "smoothingTime_CoM" + SrcPort 1 + DstBlock "MinimumJerkTrajectoryGenerator2" + DstPort 2 + } + Line { + ZOrder 11 + SrcBlock "Switch3" + SrcPort 1 + DstBlock "Reshape" + DstPort 1 + } + Line { + ZOrder 614 + SrcBlock "pos_CoM_des" + SrcPort 1 + Points [85, 0] + Branch { + ZOrder 629 + Points [0, 135] + DstBlock "Mux1" + DstPort 1 + } + Branch { + ZOrder 628 + DstBlock "MinimumJerkTrajectoryGenerator2" + DstPort 1 + } + } + Line { + ZOrder 12 + SrcBlock "SMOOTH_DES_COM" + SrcPort 1 + DstBlock "Switch3" + DstPort 2 + } + Line { + ZOrder 10 + SrcBlock "MinimumJerkTrajectoryGenerator2" + SrcPort 3 + DstBlock "Mux" + DstPort 3 + } + Line { + ZOrder 83 + SrcBlock "Mux" + SrcPort 1 + DstBlock "Switch3" + DstPort 1 + } + Line { + ZOrder 9 + SrcBlock "MinimumJerkTrajectoryGenerator2" + SrcPort 2 + DstBlock "Mux" + DstPort 2 + } + Line { + ZOrder 613 + SrcBlock "Reshape" + SrcPort 1 + DstBlock "desired_pos_vel_acc_CoM" + DstPort 1 + } + Line { + ZOrder 8 + SrcBlock "MinimumJerkTrajectoryGenerator2" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + ZOrder 626 + SrcBlock "Mux1" + SrcPort 1 + DstBlock "Switch3" + DstPort 3 + } + Line { + ZOrder 627 + SrcBlock "Constant" + SrcPort 1 + DstBlock "Mux1" + DstPort 2 + } + } + } + Block { + BlockType SubSystem + Name "Smooth reference joint position" + SID "4761" + Ports [2, 1] + Position [455, -262, 660, -198] + ZOrder 931 + HideAutomaticName off + RequestExecContextInheritance off + System { + Name "Smooth reference joint position" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "426" + 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" + MultiThreadCoSim "auto" + externalSettlingTime on + settlingTime "0.01" + sampleTime "Config.tStep" + resetOnSettlingTime off + firstDerivatives on + secondDerivatives on + explicitInitialValue off + } + Block { + BlockType Constant + Name "SMOOTH_DES_COM2" + SID "2161" + Position [380, -78, 520, -62] + ZOrder 492 + ShowName off + Value "Config.SMOOTH_JOINT_DES" + } + Block { + BlockType Switch + Name "Switch5" + SID "2248" + Position [575, -113, 630, -27] + ZOrder 491 + ShowName off + Criteria "u2 > Threshold" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Terminator + Name "Terminator" + SID "4656" + Position [310, -71, 325, -59] + ZOrder 901 + ShowName off + HideAutomaticName off + } + Block { + BlockType Outport + Name "jointPos_des_smoothed" + SID "4766" + Position [685, -77, 715, -63] + ZOrder 909 + IconDisplay "Port number" + } + 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" + } + Block { + BlockType Outport + Name "KP_CoM" + SID "4035" + Position [745, 13, 775, 27] + ZOrder 706 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "KD_CoM" + SID "4036" + Position [745, 43, 775, 57] + ZOrder 707 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "desired_pos_vel_acc_CoM" + SID "4041" + Position [745, -117, 775, -103] + ZOrder 712 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "jointPos_des_smoothed" + SID "4042" + Position [745, -237, 775, -223] + ZOrder 713 + Port "5" + IconDisplay "Port number" + } + 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" + } + Block { + BlockType Outport + Name "nu" + SID "2271" + Position [340, 2539, 370, 2551] + ZOrder 577 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "state" + SID "3201" + Position [25, 2870, 55, 2880] + ZOrder 647 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "feetContactStatus" + SID "2272" + Position [80, 2565, 110, 2575] + ZOrder 410 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "KP_postural" + SID "2268" + Position [340, 2670, 370, 2680] + ZOrder 517 + Port "5" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "KP_CoM" + SID "3330" + Position [340, 2710, 370, 2720] + ZOrder 692 + Port "6" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "KD_CoM" + SID "3331" + Position [340, 2750, 370, 2760] + ZOrder 693 + Port "7" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "desired_pos_vel_acc_CoM" + SID "2274" + Position [340, 2790, 370, 2800] + ZOrder 323 + Port "8" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "jointPos_des" + SID "2273" + Position [340, 2830, 370, 2840] + ZOrder 418 + Port "9" + IconDisplay "Port number" + } + Line { + ZOrder 545 + SrcBlock "Compute base to fixed link transform" + SrcPort 1 + DstBlock "State Machine" + DstPort 2 + } + Line { + ZOrder 546 + SrcBlock "Compute base to fixed link transform" + SrcPort 2 + DstBlock "State Machine" + DstPort 3 + } + Line { + ZOrder 558 + SrcBlock "State Machine" + SrcPort 1 + Points [67, 0; 0, -45] + Branch { + ZOrder 689 + DstBlock "Compute State Velocity" + DstPort 3 + } + Branch { + ZOrder 687 + Points [0, -30] + DstBlock "w_H_b" + DstPort 1 + } + } + Line { + ZOrder 559 + SrcBlock "State Machine" + SrcPort 2 + Points [94, 0; 0, -45] + Branch { + ZOrder 686 + DstBlock "Compute State Velocity" + DstPort 4 + } + Branch { + ZOrder 685 + Points [0, -20] + DstBlock "feetContactStatus" + DstPort 1 + } + } + Line { + ZOrder 627 + SrcBlock "State Machine" + SrcPort 3 + DstBlock "Update Gains and References" + DstPort 1 + } + Line { + ZOrder 628 + SrcBlock "State Machine" + SrcPort 4 + DstBlock "Update Gains and References" + DstPort 2 + } + Line { + ZOrder 629 + SrcBlock "State Machine" + SrcPort 5 + DstBlock "Update Gains and References" + DstPort 3 + } + Line { + ZOrder 630 + SrcBlock "State Machine" + SrcPort 6 + DstBlock "Update Gains and References" + DstPort 4 + } + Line { + ZOrder 631 + SrcBlock "State Machine" + SrcPort 7 + DstBlock "Update Gains and References" + DstPort 5 + } + Line { + ZOrder 632 + SrcBlock "State Machine" + SrcPort 8 + DstBlock "Update Gains and References" + DstPort 6 + } + Line { + ZOrder 633 + SrcBlock "State Machine" + SrcPort 9 + DstBlock "Update Gains and References" + DstPort 7 + } + Line { + ZOrder 634 + SrcBlock "State Machine" + SrcPort 10 + Points [42, 0] + Branch { + ZOrder 636 + Points [0, 45] + DstBlock "Goto3" + DstPort 1 + } + Branch { + ZOrder 635 + DstBlock "state" + DstPort 1 + } + } + Line { + ZOrder 639 + SrcBlock "Compute State Velocity" + SrcPort 1 + Points [22, 0] + Branch { + ZOrder 655 + Points [0, -40] + DstBlock "Goto5" + DstPort 1 + } + Branch { + ZOrder 654 + DstBlock "nu" + DstPort 1 + } + } + Line { + ZOrder 640 + SrcBlock "Update Gains and References" + SrcPort 1 + DstBlock "KP_postural" + DstPort 1 + } + Line { + ZOrder 641 + SrcBlock "Update Gains and References" + SrcPort 2 + DstBlock "KP_CoM" + DstPort 1 + } + Line { + ZOrder 642 + SrcBlock "Update Gains and References" + SrcPort 3 + DstBlock "KD_CoM" + DstPort 1 + } + Line { + ZOrder 652 + SrcBlock "Update Gains and References" + SrcPort 4 + DstBlock "desired_pos_vel_acc_CoM" + DstPort 1 + } + Line { + ZOrder 653 + SrcBlock "Update Gains and References" + SrcPort 5 + DstBlock "jointPos_des" + DstPort 1 + } + Line { + ZOrder 671 + SrcBlock "IMU_meas" + SrcPort 1 + DstBlock "Compute base to fixed link transform" + DstPort 2 + } + Line { + ZOrder 672 + SrcBlock "jointPos" + SrcPort 1 + Points [31, 0] + Branch { + ZOrder 674 + Points [0, -55] + Branch { + ZOrder 692 + Points [0, -130] + DstBlock "Compute State Velocity" + DstPort 1 + } + Branch { + ZOrder 691 + DstBlock "State Machine" + DstPort 1 + } + } + Branch { + ZOrder 673 + DstBlock "Compute base to fixed link transform" + DstPort 1 + } + } + Line { + ZOrder 677 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock "State Machine" + DstPort 5 + } + Line { + ZOrder 680 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock "State Machine" + DstPort 4 + } + Line { + ZOrder 690 + SrcBlock "jointVel" + SrcPort 1 + DstBlock "Compute State Velocity" + DstPort 2 + } + } + } + Block { + BlockType SubSystem + Name "emergency stop: joint limits" + SID "2416" + Ports [1] + Position [300, 246, 425, 274] + ZOrder 908 + BackgroundColor "red" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 23 + $ClassName "Simulink.Mask" + Display "disp('EMERGENCY STOP')" + RunInitForIconRedraw "off" + } + System { + Name "emergency stop: joint limits" + 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 "904" + Block { + BlockType Inport + Name "jointPos" + SID "2419" + Position [150, 238, 180, 252] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n1" + SID "4660" + 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 "4663" + 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 "4661" + 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] + 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 "898" + Block { + BlockType Inport + Name "jointPos" + SID "4662" + Position [60, 103, 90, 117] + ZOrder 552 + IconDisplay "Port number" + } + Block { + BlockType EnablePort + Name "Enable" + SID "3207" + Ports [] + Position [227, -35, 247, -15] + ZOrder 553 + ShowName off + HideAutomaticName off + } + Block { + BlockType Assertion + Name "Assertion" + SID "2420" + Position [340, 72, 400, 118] + ZOrder 207 + HideAutomaticName off + } + Block { + BlockType Reference + Name "GetLimits" + SID "2432" + Ports [0, 2] + Position [15, 23, 135, 92] + ZOrder 551 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetLimits" + SourceType "GetLimits" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + limitsSource "ControlBoard" + limitsType "Position" + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "2421" + Ports [4, 1] + 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 "3782" + Block { + BlockType Inport + Name "umin" + SID "2421::18" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "umax" + SID "2421::19" + Position [20, 136, 40, 154] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "u" + SID "2421::1" + Position [20, 171, 40, 189] + ZOrder -3 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tol" + SID "2421::20" + Position [20, 206, 40, 224] + ZOrder -4 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "2421::3781" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 131 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "2421::3780" + Tag "Stateflow S-Function torqueControlBalancing 18" + Ports [4, 2] + Position [180, 102, 230, 203] + ZOrder 130 + FunctionName "sf_sfun" + PortCounts "[4 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "inRange" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "2421::3782" + Position [460, 241, 480, 259] + ZOrder 132 + } + Block { + BlockType Outport + Name "inRange" + SID "2421::5" + Position [460, 101, 480, 119] + ZOrder -8 + IconDisplay "Port number" + } + Line { + ZOrder 22 + SrcBlock "umin" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 23 + SrcBlock "umax" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 24 + SrcBlock "u" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + ZOrder 25 + SrcBlock "tol" + SrcPort 1 + DstBlock " SFunction " + DstPort 4 + } + Line { + Name "inRange" + ZOrder 26 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "inRange" + DstPort 1 + } + Line { + ZOrder 27 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 28 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Constant + Name "index1" + SID "2422" + Position [65, 140, 90, 150] + 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 "Assertion" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "GetLimits" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 4 + 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" + 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] + 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 "827" + Block { + BlockType Inport + Name "jointPos" + SID "4665" + Position [15, 53, 45, 67] + ZOrder 552 + IconDisplay "Port number" + } + Block { + BlockType EnablePort + Name "Enable" + SID "4666" + Ports [] + Position [217, -20, 237, 0] + ZOrder 553 + ShowName off + HideAutomaticName off + } + Block { + BlockType Assertion + Name "Assertion" + SID "4667" + Position [340, 72, 400, 118] + ZOrder 207 + HideAutomaticName off + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "4669" + Ports [2, 1] + Position [165, 24, 285, 166] + 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 "3782" + Block { + BlockType Inport + Name "u" + SID "4669::1" + Position [20, 101, 40, 119] + ZOrder -3 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "delta_u_max" + SID "4669::18" + Position [20, 136, 40, 154] + ZOrder -1 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "4669::3781" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 131 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "4669::3780" + Tag "Stateflow S-Function torqueControlBalancing 14" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 130 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "noSpikes" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "4669::3782" + Position [460, 241, 480, 259] + ZOrder 132 + } + Block { + BlockType Outport + Name "noSpikes" + SID "4669::5" + Position [460, 101, 480, 119] + ZOrder -8 + IconDisplay "Port number" + } + Line { + ZOrder 16 + SrcBlock "u" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 17 + SrcBlock "delta_u_max" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "noSpikes" + ZOrder 18 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "noSpikes" + 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 Constant + Name "index1" + SID "4671" + Position [-40, 124, 100, 136] + 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 "Assertion" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "index1" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 2 + } + } + } + Line { + ZOrder 1 + SrcBlock "ON_GAZEBO\n1" + SrcPort 1 + DstBlock "STOP IF JOINTS HIT THE LIMITS" + DstPort enable + } + Line { + ZOrder 2 + SrcBlock "jointPos" + SrcPort 1 + Points [20, 0] + Branch { + ZOrder 3 + Points [0, 110] + DstBlock "STOP IF THERE ARE SPIKES IN THE ENCODERS" + DstPort 1 + } + Branch { + ZOrder 4 + DstBlock "STOP IF JOINTS HIT THE LIMITS" + DstPort 1 + } + } + Line { + ZOrder 5 + SrcBlock "ON_GAZEBO\n2" + SrcPort 1 + DstBlock "STOP IF THERE ARE SPIKES IN THE ENCODERS" + DstPort enable + } + } + } + Block { + BlockType Outport + Name "jointTorques_star" + SID "4841" + Position [1160, 313, 1190, 327] + ZOrder 560 + IconDisplay "Port number" + } + Line { + ZOrder 94 + SrcBlock "Dynamics and Kinematics" + SrcPort 4 + DstBlock "Balancing Controller QP" + DstPort 4 + } + Line { + ZOrder 102 + SrcBlock "Dynamics and Kinematics" + SrcPort 11 + DstBlock "Balancing Controller QP" + DstPort 11 + } + Line { + ZOrder 100 + SrcBlock "Dynamics and Kinematics" + SrcPort 9 + DstBlock "Balancing Controller QP" + DstPort 9 + } + Line { + ZOrder 91 + SrcBlock "Dynamics and Kinematics" + SrcPort 2 + DstBlock "Balancing Controller QP" + DstPort 2 + } + Line { + ZOrder 99 + SrcBlock "Dynamics and Kinematics" + SrcPort 8 + DstBlock "Balancing Controller QP" + DstPort 8 + } + Line { + ZOrder 95 + SrcBlock "Dynamics and Kinematics" + SrcPort 5 + DstBlock "Balancing Controller QP" + DstPort 5 + } + Line { + ZOrder 121 + SrcBlock "Robot State and References" + SrcPort 2 + Points [72, 0] + Branch { + ZOrder 286 + DstBlock "Balancing Controller QP" + DstPort 13 + } + Branch { + ZOrder 284 + Points [0, -210] + DstBlock "Dynamics and Kinematics" + DstPort 2 + } + } + Line { + ZOrder 134 + SrcBlock "Robot State and References" + SrcPort 7 + DstBlock "Balancing Controller QP" + DstPort 18 + } + Line { + ZOrder 279 + SrcBlock "jointVel" + SrcPort 1 + DstBlock "Robot State and References" + DstPort 2 + } + Line { + ZOrder 282 + SrcBlock "Joint Torque Saturation" + SrcPort 1 + DstBlock "jointTorques_star" + DstPort 1 + } + Line { + ZOrder 98 + SrcBlock "Dynamics and Kinematics" + SrcPort 7 + DstBlock "Balancing Controller QP" + DstPort 7 + } + Line { + ZOrder 129 + SrcBlock "Robot State and References" + SrcPort 3 + DstBlock "Balancing Controller QP" + DstPort 14 + } + Line { + ZOrder 204 + SrcBlock "Balancing Controller QP" + SrcPort 1 + DstBlock "Joint Torque Saturation" + DstPort 1 + } + Line { + ZOrder 97 + SrcBlock "Dynamics and Kinematics" + SrcPort 6 + DstBlock "Balancing Controller QP" + DstPort 6 + } + Line { + ZOrder 90 + SrcBlock "Dynamics and Kinematics" + SrcPort 1 + DstBlock "Balancing Controller QP" + DstPort 1 + } + Line { + ZOrder 132 + SrcBlock "Robot State and References" + SrcPort 6 + DstBlock "Balancing Controller QP" + DstPort 17 + } + Line { + ZOrder 92 + SrcBlock "Dynamics and Kinematics" + SrcPort 3 + DstBlock "Balancing Controller QP" + DstPort 3 + } + Line { + ZOrder 136 + SrcBlock "Robot State and References" + SrcPort 9 + DstBlock "Balancing Controller QP" + DstPort 20 + } + Line { + ZOrder 130 + SrcBlock "Robot State and References" + SrcPort 4 + DstBlock "Balancing Controller QP" + DstPort 15 + } + Line { + ZOrder 281 + SrcBlock "IMU_meas" + SrcPort 1 + DstBlock "Robot State and References" + DstPort 3 + } + Line { + ZOrder 119 + SrcBlock "Robot State and References" + SrcPort 1 + Points [39, 0; 0, -290] + DstBlock "Dynamics and Kinematics" + DstPort 1 + } + Line { + ZOrder 131 + SrcBlock "Robot State and References" + SrcPort 5 + DstBlock "Balancing Controller QP" + DstPort 16 + } + Line { + ZOrder 101 + SrcBlock "Dynamics and Kinematics" + SrcPort 10 + DstBlock "Balancing Controller QP" + DstPort 10 + } + Line { + ZOrder 135 + SrcBlock "Robot State and References" + SrcPort 8 + DstBlock "Balancing Controller QP" + DstPort 19 + } + Line { + ZOrder 278 + SrcBlock "jointPos" + SrcPort 1 + Points [36, 0] + Branch { + ZOrder 294 + Points [0, 70] + DstBlock "Robot State and References" + DstPort 1 + } + Branch { + ZOrder 297 + Points [0, -35] + DstBlock "emergency stop: joint limits" + DstPort 1 + } + Branch { + ZOrder 293 + Points [324, 0] + Branch { + ZOrder 289 + DstBlock "Dynamics and Kinematics" + DstPort 3 + } + Branch { + ZOrder 283 + Points [0, 70] + DstBlock "Balancing Controller QP" + DstPort 12 + } + } + } + Line { + ZOrder 313 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock "Robot State and References" + DstPort 4 + } + Line { + ZOrder 314 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock "Robot State and References" + DstPort 5 + } + } + } + Block { + BlockType Reference + Name "SetReferences" + SID "2354" + Ports [1] + Position [1185, 487, 1245, 513] + ZOrder 549 + BackgroundColor "[0.827500, 0.576500, 0.603900]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Actuators/SetReferences" + SourceType "SetReferences" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + controlType "Torque" + refSpeed "50" + refAcceleration "1000000*(pi/180)" + } + Block { + BlockType SubSystem + Name "synchronizer" + SID "2423" + Ports [] + Position [1157, 423, 1272, 453] + ZOrder 546 + ForegroundColor "red" + BackgroundColor "green" + ShowName off + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 24 + $ClassName "Simulink.Mask" + Display "disp('SYNCHRONIZER')" + } + System { + Name "synchronizer" + 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 "947" + Block { + BlockType SubSystem + Name "GAZEBO_SYNCHRONIZER" + SID "2424" + Ports [0, 0, 1] + Position [5, 15, 130, 75] + ZOrder -7 + RequestExecContextInheritance off + System { + Name "GAZEBO_SYNCHRONIZER" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + 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 EnablePort + Name "Enable" + SID "2425" + Ports [] + Position [177, 85, 197, 105] + ZOrder 538 + } + Block { + BlockType Reference + Name "Simulator Synchronizer" + SID "2426" + Ports [] + Position [120, 24, 250, 61] + ZOrder 539 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" + SourceType "Simulator Synchronizer" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + period "Config.tStep" + serverPortName "'/clock/rpc'" + clientPortName "'/WBT_synchronizer/rpc:o'" + } + } + } + Block { + BlockType Logic + Name "Logical\nOperator" + SID "2427" + Ports [1, 1] + Position [-30, -34, -5, -16] + ZOrder 307 + BlockMirror on + NamePlacement "alternate" + ShowName off + Operator "NOT" + AllPortsSameDT off + OutDataTypeStr "boolean" + } + Block { + BlockType Constant + Name "ON_GAZEBO\n" + SID "2428" + Position [95, -35, 210, -15] + ZOrder 304 + BlockMirror on + NamePlacement "alternate" + ShowName off + Value "Config.ON_GAZEBO" + } + Block { + BlockType SubSystem + Name "REAL_TIME_SYNC" + SID "2429" + Ports [0, 0, 1] + Position [-160, 17, -35, 77] + ZOrder -9 + RequestExecContextInheritance off + System { + Name "REAL_TIME_SYNC" + Location [134, 55, 3840, 2160] + Open off + PortBlocksUseCompactNotation off + 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 EnablePort + Name "Enable" + SID "2430" + Ports [] + Position [167, 90, 187, 110] + ZOrder 538 + } + Block { + BlockType Reference + Name "Real Time Synchronizer" + SID "2431" + Ports [] + Position [115, 34, 240, 71] + ZOrder 539 + ForegroundColor "[0.916667, 0.916667, 0.916667]" + BackgroundColor "gray" + ShowName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" + SourceType "Real Time Synchronizer" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + period "Config.tStep" + } + } + } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "4628" + Ports [1] + Position [125, -95, 185, -75] + ZOrder 699 + ShowName off + HideAutomaticName off + VariableName "yarp_time" + MaxDataPoints "inf" + SaveFormat "Structure With Time" + Save2DSignal "3-D array (concatenate along third dimension)" + FixptAsFi on + SampleTime "-1" + } + Block { + BlockType Reference + Name "Yarp Clock" + SID "4629" + Ports [0, 1] + Position [-50, -94, 15, -76] + ZOrder 698 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + ShowName off + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/Yarp Clock" + SourceType "YARP Clock" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + } + Line { + ZOrder 1 + SrcBlock "Logical\nOperator" + SrcPort 1 + Points [-60, 0] + DstBlock "REAL_TIME_SYNC" + DstPort enable + } + Line { + ZOrder 2 + SrcBlock "ON_GAZEBO\n" + SrcPort 1 + Points [-25, 0] + Branch { + ZOrder 6 + DstBlock "GAZEBO_SYNCHRONIZER" + DstPort enable + } + Branch { + ZOrder 4 + DstBlock "Logical\nOperator" + DstPort 1 + } + } + Line { + ZOrder 8 + SrcBlock "Yarp Clock" + SrcPort 1 + DstBlock "To Workspace" + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "wrench_leftFoot" + SID "2206" + Ports [0, 1] + Position [800, 553, 840, 567] + ZOrder 962 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" + SourceType "YARP Read" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + portName "Ports.WRENCH_LEFT_FOOT" + signalSize "Ports.WRENCH_PORT_SIZE" + timeout "0.5" + blocking off + timestamp off + autoconnect on + errorOnConnection on + } + Block { + BlockType Reference + Name "wrench_rightFoot" + SID "2218" + Ports [0, 1] + Position [800, 523, 840, 537] + ZOrder 963 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + HideAutomaticName off + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" + SourceType "YARP Read" + SourceProductName "WholeBodyToolbox" + MultiThreadCoSim "auto" + portName "Ports.WRENCH_RIGHT_FOOT" + signalSize "Ports.WRENCH_PORT_SIZE" + timeout "0.5" + blocking off + timestamp off + autoconnect on + errorOnConnection on + } + Line { + ZOrder 278 + SrcBlock "MOMENTUM BASED TORQUE CONTROL" + SrcPort 1 + DstBlock "SetReferences" + DstPort 1 + } + Line { + ZOrder 279 + SrcBlock "GetMeasurement" + SrcPort 1 + DstBlock "GoTo Motors Inertia" + DstPort 1 + } + Line { + ZOrder 282 + SrcBlock "GetMeasurement2" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 2 + } + Line { + ZOrder 283 + SrcBlock "GetMeasurement1" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 1 + } + Line { + ZOrder 284 + SrcBlock "IMU_meas" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 3 + } + Line { + ZOrder 286 + SrcBlock "wrench_rightFoot" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 4 + } + Line { + ZOrder 287 + SrcBlock "wrench_leftFoot" + SrcPort 1 + DstBlock "MOMENTUM BASED TORQUE CONTROL" + DstPort 5 + } + } +} +#Finite State Machines +# +# Stateflow 80000014 +# +# +Stateflow { + machine { + id 1 + name "torqueControlBalancing" + created "18-Feb-2014 10:14:40" + isLibrary 0 + sfVersion 80000012 + firstTarget 235 + } + chart { + id 2 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and References/Reshape Gains Matr" + "ices/MATLAB Function1" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 3 0 0] + viewObj 2 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 1 + disableImplicitCasting 1 + eml { + name "diagonalMatrixFromVectorFCN" + } + firstData 4 + firstTransition 7 + firstJunction 6 + } + state { + id 3 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 2 + treeNode [2 0 0 0] + superState SUBCHART + subviewer 2 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function D = diagonalMatrixFromVectorFCN(d)\n\n D = wbc.diagonalMatrixFromVector(d);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 4 + ssIdNumber 4 + name "d" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [2 0 5] + } + data { + id 5 + ssIdNumber 5 + name "D" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [2 4 0] + } + junction { + id 6 + position [23.5747 49.5747 7] + chart 2 + subviewer 2 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [2 0 0] + } + transition { + id 7 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 6 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 2 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 2 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [2 0 0] + } + instance { + id 8 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and References/Reshape Gains Matr" + "ices/MATLAB Function1" + chart 2 + } + chart { + id 9 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and References/Reshape Gains Matr" + "ices/MATLAB Function" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 10 0 0] + viewObj 9 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 2 + disableImplicitCasting 1 + eml { + name "diagonalMatrixFromVectorFCN" + } + firstData 11 + firstTransition 14 + firstJunction 13 + } + state { + id 10 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 9 + treeNode [9 0 0 0] + superState SUBCHART + subviewer 9 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function D = diagonalMatrixFromVectorFCN(d)\n\n D = wbc.diagonalMatrixFromVector(d);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 11 + ssIdNumber 4 + name "d" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [9 0 12] + } + data { + id 12 + ssIdNumber 5 + name "D" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [9 11 0] + } + junction { + id 13 + position [23.5747 49.5747 7] + chart 9 + subviewer 9 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [9 0 0] + } + transition { + id 14 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 13 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 9 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 9 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [9 0 0] + } + instance { + id 15 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and References/Reshape Gains Matr" + "ices/MATLAB Function" + chart 9 + } + chart { + id 16 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fixed link transform/RFoot to " + "base link transform/Get Base Rotation From IMU" + windowPosition [326.89 491.339 161 328] + viewLimits [0 156.75 0 153.75] + screen [1 1 1280 1024 1.000677131425054] + treeNode [0 17 0 0] + viewObj 16 + ssIdHighWaterMark 88 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 3 + disableImplicitCasting 1 + eml { + name "getWorldToBaseTransformFCN" + } + firstData 18 + firstTransition 27 + firstJunction 26 + } + state { + id 17 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 16 + treeNode [16 0 0 0] + superState SUBCHART + subviewer 16 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function w_H_b = getWorldToBaseTransformFCN(imu_H_fixedLink, imu_H_fixedLink_0, fixedLink_H_base," + " rpyFromIMU_0, rpyFromIMU, neck_pos, Config)\n\n FILTER_IMU_YAW = Config.FILTER_IMU_YAW;\n \n [w_H_b, w" + "Imu_H_base, wImu_H_fixedLink_0] = wbc.worldToBaseTransformWithIMU(imu_H_fixedLink, imu_H_fixedLink_0, fixedLink_" + "H_base, rpyFromIMU_0, rpyFromIMU, FILTER_IMU_YAW);\n \n % Correct head IMU with neck positions (HEAD IMU o" + "nly)\n if Config.CORRECT_NECK_IMU\n \n wImu_H_wImuAssumingNeckToZero = wbc.correctIMUWithNeckPo" + "s(neck_pos);\n wImu_H_base = wImu_H_wImuAssumingNeckToZero * wImu_H_base;\n w_H_" + "b = wImu_H_fixedLink_0\\wImu_H_base; \n end\nend\n" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 18 + ssIdNumber 82 + name "imu_H_fixedLink" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 0 19] + } + data { + id 19 + ssIdNumber 83 + name "imu_H_fixedLink_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 18 20] + } + data { + id 20 + ssIdNumber 81 + name "fixedLink_H_base" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 19 21] + } + data { + id 21 + ssIdNumber 85 + name "rpyFromIMU_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 20 22] + } + data { + id 22 + ssIdNumber 65 + name "rpyFromIMU" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 21 23] + } + data { + id 23 + ssIdNumber 88 + name "neck_pos" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 22 24] + } + data { + id 24 + ssIdNumber 71 + name "w_H_b" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 23 25] + } + data { + id 25 + ssIdNumber 87 + name "Config" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [16 24 0] + } + junction { + id 26 + position [23.5747 49.5747 7] + chart 16 + subviewer 16 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [16 0 0] + } + transition { + id 27 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 26 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 16 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 16 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [16 0 0] + } + instance { + id 28 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fixed link transform/RFoot to " + "base link transform/Get Base Rotation From IMU" + chart 16 + } + chart { + id 29 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and References/Reshape Gains Matr" + "ices/MATLAB Function2" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 30 0 0] + viewObj 29 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 4 + disableImplicitCasting 1 + eml { + name "diagonalMatrixFromVectorFCN" + } + firstData 31 + firstTransition 34 + firstJunction 33 + } + state { + id 30 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 29 + treeNode [29 0 0 0] + superState SUBCHART + subviewer 29 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function D = diagonalMatrixFromVectorFCN(d)\n\n D = wbc.diagonalMatrixFromVector(d);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 31 + ssIdNumber 4 + name "d" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [29 0 32] + } + data { + id 32 + ssIdNumber 5 + name "D" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [29 31 0] + } + junction { + id 33 + position [23.5747 49.5747 7] + chart 29 + subviewer 29 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [29 0 0] + } + transition { + id 34 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 33 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 29 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 29 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [29 0 0] + } + instance { + id 35 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Update Gains and References/Reshape Gains Matr" + "ices/MATLAB Function2" + chart 29 + } + chart { + id 36 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/Add motors reflected inertia/Add motor r" + "eflected inertias" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 37 0 0] + viewObj 36 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 6 + disableImplicitCasting 1 + eml { + name "addMotorsInertiaFCN" + } + firstData 38 + firstTransition 42 + firstJunction 41 + } + state { + id 37 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 36 + treeNode [36 0 0 0] + superState SUBCHART + subviewer 36 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function M_with_inertia = addMotorsInertiaFCN(M,Config)\n\n Gamma = Config.Gamma;\n T =" + " Config.T;\n I_m = Config.I_m;\n \n M_with_inertia = wbc.addMotorsReflectedInertia(M,Gamma,T,I_m);\ne" + "nd" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 38 + ssIdNumber 4 + name "M" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [36 0 39] + } + data { + id 39 + ssIdNumber 5 + name "M_with_inertia" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [36 38 40] + } + data { + id 40 + ssIdNumber 6 + name "Config" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [36 39 0] + } + junction { + id 41 + position [23.5747 49.5747 7] + chart 36 + subviewer 36 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [36 0 0] + } + transition { + id 42 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 41 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 36 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 36 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [36 0 0] + } + instance { + id 43 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Dynamics and Kinematics/Dynamics/Add motors reflected inertia/Add motor r" + "eflected inertias" + chart 36 + } + chart { + id 44 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute State Velocity/Compute Base Velocity" + windowPosition [420.256 -293.125 200 760] + viewLimits [0 156.75 0 153.75] + screen [1 1 1680 1050 1.25] + treeNode [0 45 0 0] + viewObj 44 + ssIdHighWaterMark 10 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 7 + disableImplicitCasting 1 + eml { + name "computeBaseVelocityFCN" + } + firstData 46 + firstTransition 53 + firstJunction 52 + } + state { + id 45 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 44 + treeNode [44 0 0 0] + superState SUBCHART + subviewer 44 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function nu_b = computeBaseVelocityFCN(J_l_sole, J_r_sole, feetContactStatus, jointVel, Reg)\n\n " + " pinvDamp_baseVel = Reg.pinvDamp_baseVel;\n \n nu_b = wbc.computeBaseVelocityWithFeetContact(J_l_sole, J" + "_r_sole, feetContactStatus, jointVel, pinvDamp_baseVel);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 46 + ssIdNumber 4 + name "J_l_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [44 0 47] + } + data { + id 47 + ssIdNumber 8 + name "J_r_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [44 46 48] + } + data { + id 48 + ssIdNumber 9 + name "feetContactStatus" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [44 47 49] + } + data { + id 49 + ssIdNumber 5 + name "nu_b" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [44 48 50] + } + data { + id 50 + ssIdNumber 6 + name "jointVel" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [44 49 51] + } + data { + id 51 + ssIdNumber 10 + name "Reg" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [44 50 0] + } + junction { + id 52 + position [23.5747 49.5747 7] + chart 44 + subviewer 44 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [44 0 0] + } + transition { + id 53 + labelString "{eML_blk_kernel();}" + labelPosition [48.125 43.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 52 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 44 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 44 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [44 0 0] + } + instance { + id 54 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute State Velocity/Compute Base Velocity" + chart 44 + } + chart { + id 55 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fixed link transform/LFoot to " + "base link transform /Get Base Rotation From IMU" + windowPosition [326.89 491.339 161 328] + viewLimits [0 156.75 0 153.75] + screen [1 1 1280 1024 1.000677131425054] + treeNode [0 56 0 0] + viewObj 55 + ssIdHighWaterMark 88 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 8 + disableImplicitCasting 1 + eml { + name "getWorldToBaseTransformFCN" + } + firstData 57 + firstTransition 66 + firstJunction 65 + } + state { + id 56 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 55 + treeNode [55 0 0 0] + superState SUBCHART + subviewer 55 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function w_H_b = getWorldToBaseTransformFCN(imu_H_fixedLink, imu_H_fixedLink_0, fixedLink_H_base," + " rpyFromIMU_0, rpyFromIMU, neck_pos, Config)\n\n FILTER_IMU_YAW = Config.FILTER_IMU_YAW;\n \n [w_H_b, w" + "Imu_H_base, wImu_H_fixedLink_0] = wbc.worldToBaseTransformWithIMU(imu_H_fixedLink, imu_H_fixedLink_0, fixedLink_" + "H_base, rpyFromIMU_0, rpyFromIMU, FILTER_IMU_YAW);\n \n % Correct head IMU with neck positions (HEAD IMU o" + "nly)\n if Config.CORRECT_NECK_IMU\n \n wImu_H_wImuAssumingNeckToZero = wbc.correctIMUWithNeckPo" + "s(neck_pos);\n wImu_H_base = wImu_H_wImuAssumingNeckToZero * wImu_H_base;\n w_H_" + "b = wImu_H_fixedLink_0\\wImu_H_base; \n end\nend\n" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 57 + ssIdNumber 82 + name "imu_H_fixedLink" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 0 58] + } + data { + id 58 + ssIdNumber 83 + name "imu_H_fixedLink_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 57 59] + } + data { + id 59 + ssIdNumber 81 + name "fixedLink_H_base" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 58 60] + } + data { + id 60 + ssIdNumber 85 + name "rpyFromIMU_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 59 61] + } + data { + id 61 + ssIdNumber 65 + name "rpyFromIMU" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 60 62] + } + data { + id 62 + ssIdNumber 88 + name "neck_pos" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 61 63] + } + data { + id 63 + ssIdNumber 71 + name "w_H_b" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 62 64] + } + data { + id 64 + ssIdNumber 87 + name "Config" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 63 0] + } + junction { + id 65 + position [23.5747 49.5747 7] + chart 55 + subviewer 55 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [55 0 0] + } + transition { + id 66 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 65 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 55 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 55 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [55 0 0] + } + instance { + id 67 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fixed link transform/LFoot to " + "base link transform /Get Base Rotation From IMU" + chart 55 + } + chart { + id 68 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/From tau_QP to Joint Torques (motor reflected ine" + "rtia)/(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 69 0 0] + viewObj 68 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 9 + disableImplicitCasting 1 + eml { + name "computeMotorsInertiaFCN" + } + firstData 70 + firstTransition 73 + firstJunction 72 + } + state { + id 69 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 68 + treeNode [68 0 0 0] + superState SUBCHART + subviewer 68 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function reflectedInertia = computeMotorsInertiaFCN(Config)\n \n Gamma = Config.Gamma;\n " + " T = Config.T;\n I_m = Config.I_m;\n \n reflectedInertia = wbc.computeMotorsReflectedInertia(Gam" + "ma,T,I_m);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 70 + ssIdNumber 5 + name "reflectedInertia" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [68 0 71] + } + data { + id 71 + ssIdNumber 4 + name "Config" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [68 70 0] + } + junction { + id 72 + position [23.5747 49.5747 7] + chart 68 + subviewer 68 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [68 0 0] + } + transition { + id 73 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 72 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 68 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 68 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [68 0 0] + } + instance { + id 74 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/From tau_QP to Joint Torques (motor reflected ine" + "rtia)/(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" + chart 68 + } + chart { + id 75 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/Saturate Torque Derivative" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 76 0 0] + viewObj 75 + ssIdHighWaterMark 8 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 11 + disableImplicitCasting 1 + eml { + name "saturateInputDerivativeFCN" + } + firstData 77 + firstTransition 83 + firstJunction 82 + } + state { + id 76 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 75 + treeNode [75 0 0 0] + superState SUBCHART + subviewer 75 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function uSat = saturateInputDerivativeFCN(u, u_0, tStep, uDotMax)\n\n uSat = wbc.saturateInpu" + "tDerivative(u, u_0, tStep, uDotMax);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 77 + ssIdNumber 4 + name "u" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [75 0 78] + } + data { + id 78 + ssIdNumber 5 + name "uSat" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [75 77 79] + } + data { + id 79 + ssIdNumber 6 + name "u_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [75 78 80] + } + data { + id 80 + ssIdNumber 7 + name "tStep" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [75 79 81] + } + data { + id 81 + ssIdNumber 8 + name "uDotMax" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [75 80 0] + } + junction { + id 82 + position [23.5747 49.5747 7] + chart 75 + subviewer 75 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [75 0 0] + } + transition { + id 83 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 82 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 75 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 75 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [75 0 0] + } + instance { + id 84 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Joint Torque Saturation/Saturate Torque Derivative" + chart 75 + } + chart { + id 85 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/STATE MACHINE" + windowPosition [326.89 491.339 161 328] + viewLimits [0 156.75 0 153.75] + screen [1 1 1280 1024 1.000677131425054] + treeNode [0 86 0 0] + viewObj 85 + ssIdHighWaterMark 86 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 13 + disableImplicitCasting 1 + eml { + name "stateMachineMomentumControlFCN" + } + firstData 87 + firstTransition 111 + firstJunction 110 + } + state { + id 86 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 85 + treeNode [85 0 0 0] + superState SUBCHART + subviewer 85 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function [w_H_b, pos_CoM_des, jointPos_des, feetContactStatus, KP_postural_diag, KP_CoM_diag, KD_" + "CoM_diag, state, smoothingTimeJoints, smoothingTimeCoM] = ...\n stateMachineMomentumControlFCN(pos_" + "CoM_0, jointPos_0, pos_CoM_fixed_l_sole, pos_CoM_fixed_r_sole, jointPos, ...\n " + " time, wrench_rightFoot, wrench_leftFoot, l_sole_H_b, r_sole_H_b, StateMachine, Gain, Config)\n \n " + "[w_H_b, pos_CoM_des, jointPos_des, feetContactStatus, KP_postural_diag, KP_CoM_diag, KD_CoM_diag, state, smoothi" + "ngTimeJoints, smoothingTimeCoM] = ...\n stateMachineMomentumControl(pos_CoM_0, jointPos_0, pos_CoM_fixed_" + "l_sole, pos_CoM_fixed_r_sole, jointPos, ...\n time, wrench_rightFoot, wrench_" + "leftFoot, l_sole_H_b, r_sole_H_b, StateMachine, Gain, Config);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 87 + ssIdNumber 37 + name "pos_CoM_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 0 88] + } + data { + id 88 + ssIdNumber 54 + name "jointPos_0" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 87 89] + } + data { + id 89 + ssIdNumber 64 + name "pos_CoM_fixed_l_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 88 90] + } + data { + id 90 + ssIdNumber 77 + name "pos_CoM_fixed_r_sole" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 89 91] + } + data { + id 91 + ssIdNumber 69 + name "jointPos" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 90 92] + } + data { + id 92 + ssIdNumber 56 + name "time" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 91 93] + } + data { + id 93 + ssIdNumber 65 + name "wrench_rightFoot" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 92 94] + } + data { + id 94 + ssIdNumber 74 + name "wrench_leftFoot" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 93 95] + } + data { + id 95 + ssIdNumber 71 + name "w_H_b" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 94 96] + } + data { + id 96 + ssIdNumber 52 + name "pos_CoM_des" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 95 97] + } + data { + id 97 + ssIdNumber 46 + name "jointPos_des" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 96 98] + } + data { + id 98 + ssIdNumber 43 + name "feetContactStatus" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 97 99] + } + data { + id 99 + ssIdNumber 72 + name "l_sole_H_b" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 98 100] + } + data { + id 100 + ssIdNumber 78 + name "r_sole_H_b" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 99 101] + } + data { + id 101 + ssIdNumber 55 + name "StateMachine" + scope PARAMETER_DATA + paramIndexForInitFromWorkspace 2 + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 100 102] + } + data { + id 102 + ssIdNumber 66 + name "KP_postural_diag" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 101 103] + } + data { + id 103 + ssIdNumber 82 + name "KP_CoM_diag" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 102 104] + } + data { + id 104 + ssIdNumber 83 + name "KD_CoM_diag" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 103 105] + } + data { + id 105 + ssIdNumber 67 + name "Gain" + scope PARAMETER_DATA + paramIndexForInitFromWorkspace 1 + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 104 106] + } + data { + id 106 + ssIdNumber 68 + name "state" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 105 107] + } + data { + id 107 + ssIdNumber 81 + name "smoothingTimeJoints" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 106 108] + } + data { + id 108 + ssIdNumber 86 + name "smoothingTimeCoM" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 107 109] + } + data { + id 109 + ssIdNumber 85 + name "Config" + scope PARAMETER_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [85 108 0] + } + junction { + id 110 + position [23.5747 49.5747 7] + chart 85 + subviewer 85 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [85 0 0] + } + transition { + id 111 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 110 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 85 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 85 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [85 0 0] + } + instance { + id 112 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Robot State and References/State Machine/STATE MACHINE" + chart 85 + } + chart { + id 113 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/MAT" + "LAB Function" + windowPosition [369.958 -65.92 200 534.4] + viewLimits [0 156.75 0 153.75] + screen [1 1 1366 768 1.25] + treeNode [0 114 0 0] + viewObj 113 + ssIdHighWaterMark 8 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 14 + disableImplicitCasting 1 + eml { + name "checkSpikes" + } + supportVariableSizing 0 + firstData 115 + firstTransition 119 + firstJunction 118 + } + state { + id 114 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 113 + treeNode [113 0 0 0] + superState SUBCHART + subviewer 113 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function noSpikes = checkSpikes(u, delta_u_max)\n\n noSpikes = wbc.checkSpikes(u, delta_u_max" + ");\nend" + editorLayout "100 M4x1[10 5 700 500]" + fimathString "fimath(...\n'RoundMode', 'floor',...\n'OverflowMode', 'wrap',...\n'ProductMode', 'KeepLSB', " + "'ProductWordLength', 32,...\n'SumMode', 'KeepLSB', 'SumWordLength', 32,...\n'CastBeforeSum', true)" + fimathForFiConstructors FimathMatlabFactoryDefault + emlDefaultFimath FimathUserSpecified + } + } + data { + id 115 + ssIdNumber 6 + name "u" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [113 0 116] + } + data { + id 116 + ssIdNumber 4 + name "delta_u_max" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [113 115 117] + } + data { + id 117 + ssIdNumber 7 + name "noSpikes" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [113 116 0] + } + junction { + id 118 + position [23.5747 49.5747 7] + chart 113 + subviewer 113 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [113 0 0] + } + transition { + id 119 + 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 118 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 113 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 113 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [113 0 0] + } + instance { + id 120 + 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 + 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 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 15 + disableImplicitCasting 1 + eml { + name "robotIsOnSingleSupportQP_FCN" + } + firstData 123 + firstTransition 126 + firstJunction 125 + } + state { + id 122 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 121 + treeNode [121 0 0 0] + superState SUBCHART + subviewer 121 + 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 123 + 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 [121 0 124] + } + data { + id 124 + 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 [121 123 0] + } + junction { + id 125 + position [23.5747 49.5747 7] + chart 121 + subviewer 121 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [121 0 0] + } + transition { + id 126 + 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 125 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 121 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 121 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [121 0 0] + } + instance { + id 127 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/One Foot Two Fee" + "t QP Selector" + chart 121 + } + chart { + id 128 + 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 + ssIdHighWaterMark 82 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 17 + disableImplicitCasting 1 + eml { + name "momentumBasedControllerFCN" + } + firstData 130 + firstTransition 168 + firstJunction 167 + } + state { + id 129 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 128 + treeNode [128 0 0 0] + superState SUBCHART + subviewer 128 + 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 130 + 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 [128 0 131] + } + data { + id 131 + 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 [128 130 132] + } + data { + id 132 + 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 [128 131 133] + } + data { + id 133 + 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 [128 132 134] + } + data { + id 134 + 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 [128 133 135] + } + data { + id 135 + 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 [128 134 136] + } + data { + id 136 + 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 [128 135 137] + } + data { + id 137 + 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 [128 136 138] + } + data { + id 138 + 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 [128 137 139] + } + data { + id 139 + 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 [128 138 140] + } + data { + id 140 + 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 [128 139 141] + } + data { + id 141 + 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 [128 140 142] + } + data { + id 142 + 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 [128 141 143] + } + data { + id 143 + 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 [128 142 144] + } + data { + id 144 + 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 [128 143 145] + } + data { + id 145 + 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 [128 144 146] + } + data { + id 146 + 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 [128 145 147] + } + data { + id 147 + 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 [128 146 148] + } + data { + id 148 + 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 [128 147 149] + } + data { + id 149 + 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 [128 148 150] + } + data { + id 150 + 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 [128 149 151] + } + data { + id 151 + 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 [128 150 152] + } + data { + id 152 + 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 [128 151 153] + } + data { + id 153 + 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 [128 152 154] + } + data { + id 154 + 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 [128 153 155] + } + data { + id 155 + 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 [128 154 156] + } + data { + id 156 + 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 [128 155 157] + } + data { + id 157 + 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 [128 156 158] + } + data { + id 158 + 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 [128 157 159] + } + data { + id 159 + 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 [128 158 160] + } + data { + id 160 + 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 [128 159 161] + } + data { + id 161 + 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 [128 160 162] + } + data { + id 162 + 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 [128 161 163] + } + data { + id 163 + 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 [128 162 164] + } + data { + id 164 + 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 [128 163 165] + } + data { + id 165 + 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 [128 164 166] + } + data { + id 166 + 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 [128 165 0] + } + junction { + id 167 + position [23.5747 49.5747 7] + chart 128 + subviewer 128 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [128 0 0] + } + transition { + id 168 + 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 167 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 128 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 128 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [128 0 0] + } + instance { + id 169 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Momentum Based Balancing Controller\n" + chart 128 + } + chart { + id 170 + 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 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 18 + disableImplicitCasting 1 + eml { + name "checkRangeFCN" + } + supportVariableSizing 0 + firstData 172 + firstTransition 178 + firstJunction 177 + } + state { + id 171 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 170 + treeNode [170 0 0 0] + superState SUBCHART + subviewer 170 + 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" + 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 172 + 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 [170 0 173] + } + data { + id 173 + 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 [170 172 174] + } + data { + id 174 + 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 [170 173 175] + } + data { + id 175 + 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 [170 174 176] + } + data { + id 176 + 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 [170 175 0] + } + junction { + id 177 + position [23.5747 49.5747 7] + chart 170 + subviewer 170 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [170 0 0] + } + transition { + id 178 + 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 177 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 170 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 170 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [170 0 0] + } + instance { + id 179 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" + chart 170 + } + chart { + id 180 + 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 + ssIdHighWaterMark 5 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 19 + disableImplicitCasting 1 + eml { + name "footOnGround" + } + firstData 182 + firstTransition 185 + firstJunction 184 + } + state { + id 181 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 180 + treeNode [180 0 0 0] + superState SUBCHART + subviewer 180 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function 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 182 + 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 [180 0 183] + } + data { + id 183 + 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 [180 182 0] + } + junction { + id 184 + position [23.5747 49.5747 7] + chart 180 + subviewer 180 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [180 0 0] + } + transition { + id 185 + 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 184 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 180 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 180 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [180 0 0] + } + instance { + id 186 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Select ba" + "se to world transform" + chart 180 + } + chart { + id 187 + 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 + ssIdHighWaterMark 10 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 20 + disableImplicitCasting 1 + eml { + name "getEquivalentBaseVel_FCN" + } + firstData 189 + firstTransition 196 + firstJunction 195 + } + state { + id 188 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 187 + treeNode [187 0 0 0] + superState SUBCHART + subviewer 187 + 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 189 + 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 [187 0 190] + } + data { + id 190 + 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 [187 189 191] + } + data { + id 191 + 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 [187 190 192] + } + data { + id 192 + 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 [187 191 193] + } + data { + id 193 + 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 [187 192 194] + } + data { + id 194 + 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 [187 193 0] + } + junction { + id 195 + position [23.5747 49.5747 7] + chart 187 + subviewer 187 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [187 0 0] + } + transition { + id 196 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 195 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 187 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 187 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [187 0 0] + } + instance { + id 197 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute angular momentum integral error/Get Equiv" + "alent Base Velocity" + chart 187 + } + chart { + id 198 + 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 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 24 + disableImplicitCasting 1 + eml { + name "analyticalSolutionQP_FCN" + } + firstData 200 + firstTransition 204 + firstJunction 203 + } + state { + id 199 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 198 + treeNode [198 0 0 0] + superState SUBCHART + subviewer 198 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function analyticalSolution = analyticalSolutionQP_FCN(H,g)\n\n analyticalSolution = wbc.analy" + "ticalSolutionQP(H,g);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 200 + 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 [198 0 201] + } + data { + id 201 + 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 [198 200 202] + } + data { + id 202 + 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 [198 201 0] + } + junction { + id 203 + position [23.5747 49.5747 7] + chart 198 + subviewer 198 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [198 0 0] + } + transition { + id 204 + 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 203 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 198 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 198 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [198 0 0] + } + instance { + id 205 + 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 { + id 206 + 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 + ssIdHighWaterMark 14 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 25 + disableImplicitCasting 1 + eml { + name "processOutputQP_twoFeetFCN" + } + firstData 208 + firstTransition 214 + firstJunction 213 + } + state { + id 207 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 206 + treeNode [206 0 0 0] + superState SUBCHART + subviewer 206 + 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 208 + 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 [206 0 209] + } + data { + id 209 + 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 [206 208 210] + } + data { + id 210 + 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 [206 209 211] + } + data { + id 211 + 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 [206 210 212] + } + data { + id 212 + 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 [206 211 0] + } + junction { + id 213 + position [23.5747 49.5747 7] + chart 206 + subviewer 206 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [206 0 0] + } + transition { + id 214 + 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 213 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 206 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 206 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [206 0 0] + } + instance { + id 215 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP Two Feet/Proc" + "ess QP output" + chart 206 + } + chart { + id 216 + 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 + ssIdHighWaterMark 6 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 26 + disableImplicitCasting 1 + eml { + name "analyticalSolutionQP_FCN" + } + firstData 218 + firstTransition 222 + firstJunction 221 + } + state { + id 217 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 216 + treeNode [216 0 0 0] + superState SUBCHART + subviewer 216 + 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 218 + 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 [216 0 219] + } + data { + id 219 + 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 [216 218 220] + } + data { + id 220 + 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 [216 219 0] + } + junction { + id 221 + position [23.5747 49.5747 7] + chart 216 + subviewer 216 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [216 0 0] + } + transition { + id 222 + 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 221 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 216 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 216 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [216 0 0] + } + instance { + id 223 + 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 { + id 224 + 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 + ssIdHighWaterMark 14 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 27 + disableImplicitCasting 1 + eml { + name "processOutputQP_oneFoot" + } + firstData 226 + firstTransition 233 + firstJunction 232 + } + state { + id 225 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 224 + treeNode [224 0 0 0] + superState SUBCHART + subviewer 224 + 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 226 + 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 [224 0 227] + } + data { + id 227 + 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 [224 226 228] + } + data { + id 228 + 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 [224 227 229] + } + data { + id 229 + 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 [224 228 230] + } + data { + id 230 + 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 [224 229 231] + } + data { + id 231 + 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 [224 230 0] + } + junction { + id 232 + position [23.5747 49.5747 7] + chart 224 + subviewer 224 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [224 0 0] + } + transition { + id 233 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 232 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 224 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 224 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [224 0 0] + } + instance { + id 234 + machine 1 + name "MOMENTUM BASED TORQUE CONTROL/Balancing Controller QP/Compute Desired Torques/QPSolver/QP One Foot/Proc" + "ess QP output" + chart 224 + } + target { + id 235 + machine 1 + name "sfun" + codeFlags "" + linkNode [1 0 236] + } + target { + id 236 + machine 1 + name "rtw" + linkNode [1 235 0] + } +} diff --git a/doc/How-to-run-torqueBalancing-on-real-iCub.md b/doc/How-to-run-torqueBalancing-on-real-iCub.md index 9dab17c..ea96fcd 100644 --- a/doc/How-to-run-torqueBalancing-on-real-iCub.md +++ b/doc/How-to-run-torqueBalancing-on-real-iCub.md @@ -1,13 +1,14 @@ - ## HOW TO RUN BALANCING WITH TORQUE CONTROL ON ICUB -The procedure to run the torque balancing module is still quite elaborate. Users willing to use the module should follow this list. +#### Preliminary procedures: -- Set the environmental variable `YARP_ROBOT_NAME` in the `.bashrc` according to the robot one wants to use (e.g. icubGazeboSim for simulations, or iCubGenova04, etc. for experiments). Please **note** that for the time being, the same environmental variable must be set also in the Matlab environment, by properly choosing the `YARP_ROBOT_NAME` in the controllers' [initialization script](https://github.com/robotology/whole-body-controllers/blob/master/torque-controllers/momentum-based-yoga/initTorqueBalancingYoga.m). +- 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. `$ 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. 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. @@ -15,29 +16,4 @@ The procedure to run the torque balancing module is still quite elaborate. Users #### After putting the robot on the ground: -- Open the simulink model and run the module. - -#### Citing this contribution -In case you want to cite the content of this module please refer to [iCub whole-body control through force regulation on rigid non-coplanar contacts](http://journal.frontiersin.org/article/10.3389/frobt.2015.00006/abstract) and use the following bibtex entry: - -``` -@INPROCEEDINGS{Nava_etal2016, -author={G. Nava and F. Romano and F. Nori and D. Pucci}, -booktitle={2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, -title={Stability analysis and design of momentum-based controllers for humanoid robots}, -year={2016}, -pages={680-687}, -keywords={Lyapunov methods;asymptotic stability;closed loop systems;control system synthesis;humanoid robots;legged locomotion;linearisation techniques;momentum;robust control;Lyapunov analysis;balancing controller design;closed loop system asymptotic stability;iCub humanoid robot;linearized system joint space;momentum-based controller design;robust controllers;stability analysis;unstable zero dynamics;walking controller design;Acceleration;Asymptotic stability;Humanoid robots;Legged locomotion;Robot kinematics;Stability analysis}, -doi={10.1109/IROS.2016.7759126}, -month={Oct},} -``` - -``` - @article{Nori_etal2015, - author="Nori, F. and Traversaro, S. and Eljaik, J. and Romano, F. and Del Prete, A. and Pucci, D.", - title="iCub whole-body control through force regulation on rigid non-coplanar contacts", - year="2015", - journal="Frontiers in {R}obotics and {A}{I}", - volume="1" - } -``` +- Open the simulink model and run the module. \ No newline at end of file diff --git a/doc/How-to-run-torqueBalancing-simulations-with-iCub.md b/doc/How-to-run-torqueBalancing-simulations-with-iCub.md index 7025cb6..3a7ca63 100644 --- a/doc/How-to-run-torqueBalancing-simulations-with-iCub.md +++ b/doc/How-to-run-torqueBalancing-simulations-with-iCub.md @@ -1,43 +1,26 @@ - ## HOW TO RUN A SIMULATION WITH TORQUE CONTROL ON ICUB The procedure to run the torque balancing module 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, or `iCubGenova04`, etc. for experiments). Please **note** that for the time being, the same environmental variable must be set also in the Matlab environment, by properly choosing the `YARP_ROBOT_NAME` in the controllers' [initialization script](https://github.com/robotology/whole-body-controllers/blob/master/torque-controllers/momentum-based-yoga/initTorqueBalancingYoga.m). +- 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). -- Launch the `yarpserver` (with `--write` option if necessary). +- 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: -- Launch gazebo. If you want to use the synchronization between the controller and the simulator to avoid real-time factor related problems, launch gazebo as follows: `gazebo -slibgazebo_yarp_clock.so`. - -- Bring the robot in a suitable home position (e.g. `$ yarpmotorgui --from homePoseBalancing.ini` and then select a custom position by clicking on `Global Joints Commands/Custom postions`. +``` +yarp resource --find model.urdf -- For `icubGazeboSim`, 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 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. + then, check that the path and the model name are correct. + +- Launch the `yarpserver` (with `--write` option if necessary). -#### Citing this contribution -In case you want to cite the content of this module please refer to [iCub whole-body control through force regulation on rigid non-coplanar contacts](http://journal.frontiersin.org/article/10.3389/frobt.2015.00006/abstract) and use the following bibtex entry: +- 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`). -``` -@INPROCEEDINGS{Nava_etal2016, -author={G. Nava and F. Romano and F. Nori and D. Pucci}, -booktitle={2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, -title={Stability analysis and design of momentum-based controllers for humanoid robots}, -year={2016}, -pages={680-687}, -keywords={Lyapunov methods;asymptotic stability;closed loop systems;control system synthesis;humanoid robots;legged locomotion;linearisation techniques;momentum;robust control;Lyapunov analysis;balancing controller design;closed loop system asymptotic stability;iCub humanoid robot;linearized system joint space;momentum-based controller design;robust controllers;stability analysis;unstable zero dynamics;walking controller design;Acceleration;Asymptotic stability;Humanoid robots;Legged locomotion;Robot kinematics;Stability analysis}, -doi={10.1109/IROS.2016.7759126}, -month={Oct},} -``` +- 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). -``` - @article{Nori_etal2015, - author="Nori, F. and Traversaro, S. and Eljaik, J. and Romano, F. and Del Prete, A. and Pucci, D.", - title="iCub whole-body control through force regulation on rigid non-coplanar contacts", - year="2015", - journal="Frontiers in {R}obotics and {A}{I}", - volume="1" - } -``` +- (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. + +- 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-the-robot-for-wbc-experiments.md index b2be775..3e8ab64 100644 --- a/doc/How-to-setup-the-robot-for-wbc-experiments.md +++ b/doc/How-to-setup-the-robot-for-wbc-experiments.md @@ -1,34 +1,35 @@ -## How to setup the iCub robot for whole-body torque control experiments +## HOW TO SETUP ICUB FOR WHOLE BODY CONTROL EXPERIMENTS -**Disclaimer:** this documentation is not a complete guide on how to work with iCub, and is intended to be used by users that already have an idea of what they are doing. If you are looking for a detailed guide, check the [icub-wiki](http://wiki.icub.org/wiki/Main_Page). +**Disclaimer:** this documentation is not a complete guide on how to work with iCub, and it is intended to be used by users that already have an idea of what they are doing. If you are looking for a more detailed guide, check the [icub-wiki](http://wiki.icub.org/wiki/Main_Page). ### Preliminary checks #### Update yarp, icub-main and robotology-superbuild -First, it is necessary to update the `yarp`, `icub-main` and `robotology-superbuild` directories on the iCub laptop, as well as on the computer used for launching the Simulink controllers (if different). You can do this by opening a terminal in the source folder of each repository (e.g. the `yarp` folder) and running the command: + +First, it is necessary to update the `yarp`, `icub-main` and `robotology-superbuild` directories on the iCub laptop, as well as on the computer used for launching the Simulink controllers (if different). You can do this by opening a terminal, and by pointing the **source folder** of each repository (e.g. the `yarp` folder). Then, type the command: `git pull` -You don't have to repeat this operation on computers which share sources with the icub laptop. +You don't have to repeat this operation on computers which share sources folders with the iCub laptop. -Then, if there are some updates, compile them by running the commands: +Then, if there are some updates, you can compile them by running the commands: `cd build` -`ccmake ../` +`cmake .` `make` -You also need to update all the repositories downloaded by means of the `robotology-superbuild`. Go into the `robotology-superbuild` folder and run: +You may also need to update all the repositories downloaded through the `robotology-superbuild`. Go into the `robotology-superbuild` folder and run: `cd build` `make update-all` -**IMPORTANT** before running `make-update all`, be sure all repositories downloaded with the superbuild are in `master` branch (they are inside the `robotology-superbuild/robotology` folder. Go into the source directory of each repo (e.g. `iDyntree`) and check the current github branch by running the `git status` command. If they are not in `master` branch, then be sure the `YCM_DEVEL_MODE` option is activated for the corresponding repository. You can check this in the `build` folder, by running: +**IMPORTANT** before running `make-update all`, be sure all repositories downloaded with the superbuild are in `master` branch (repositories are inside the `robotology-superbuild/robotology` folder. Go into the source directory of each repo (e.g. `iDyntree`) and check the current github branch by running the `git status` command. If they are not in `master` branch, then be sure the `YCM_DEVEL_MODE` option is activated for the corresponding repository. You can check this in the `robotology-superbuild/build` folder, by running: `ccmake ./` and by looking at the status of the options of the format `YCM_DEVEL_MODE_name_of_the_repo`. -To update the iCub on-board PC (pc104 or icub-head), first connect to it using the command +To update the iCub on-board PC (`pc104` or `icub-head`), first connect to it using the command `ssh pc104` or @@ -36,33 +37,35 @@ or and if necessary add -X option to redirect the graphic output to your local machine. Then follow the same procedure presented before. -## Firmware update +### Firmware update + +**iCub software version anterior to 1.8.0**: -*iCub software version anterior to 1.8.0*: - It is not recommended to perform the firmware update without the support from IIT. -*Latest iCub software version*: +**Latest iCub software version**: + - Look [here](https://github.com/robotology/QA/issues/240) for instructions on how to perform an update using `Firmware-updater`. - You can find information about CAN-bus numbers and CAN-bus device drivers [here](http://wiki.icub.org/wiki/Can_addresses_and_associated_firmware#Can_Networks). -## Calibration of the robot +### Calibration of the robot -### Joint encoders calibration +#### Joint encoders calibration On iCub it is sometimes required to re-calibrate the “zero position” associated to the joints of the robot. The robot should be fixed on the pole for calibration. Fine calibration can be done with the help of a level tool, as described in the [wiki](http://wiki.icub.org/wiki/Manual#Three._Calibration). Remember to calibrate the `torso` joints before the `neck` and `arms` joints. For example: - On a terminal run the `yarpmotorgui`, then switch to the tab associated to the body part you want to calibrate. Then, click the `idle` button for the joints to calibrate, allowing you to move them freely. With the help of a level tool, move the joint such that you can measure that it is level. Read the joint encoder value corresponding to the "level" position. -- 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: +- 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 +#### IMU calibration For IMU calibration, the robot has to be on the pole, in home position (i.e. all joints to 0). Actually, we calibrate the neck `pitch` and `roll` such that the IMU `linear accelerations` readings are, as expected, [0 0 9.81] along the [x, y, z] axis of an inertial reference frame with the `z` axis pointing against the gravity. @@ -70,14 +73,16 @@ You need to receive the acceleration values of the head to be able to calibrate In `yarpmotorgui`, switch to the tab associated to the head. Then, click the `idle` button for the joints to calibrate: `neck_pitch` and `neck_roll`, allowing you to move them freely. Then, move the head joints until signals are near their desired values. Note the encoder values and proceed as before. -## Launch the iCub +### Launch the iCub + Once everything is correctly updated and calibrated: 1. Switch on the power supplies (wait a little for the power to come to the iCub). 2. Launch `yarpmanager` on the icub external PC. Properly configure the `cluster`. Then run the application `icub_startup_wbd.xml`. -## Some little things to know +### Some little things to know - 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. + + - How to hide DEBUG information: in the cmake information, put the variable DEBUG from DEBUG to RELEASE. \ No newline at end of file diff --git a/doc/README.md b/doc/README.md index 8842765..db304bf 100644 --- a/doc/README.md +++ b/doc/README.md @@ -8,4 +8,4 @@ Guidelines on how to create/use Simulink models for control. - [How-to-run-torqueBalancing-on-real-iCub](How-to-run-torqueBalancing-on-real-iCub.md) -- [How-to-setup-the-robot-for-wbc-experiments](How-to-setup-the-robot-for-wbc-experiments.md) +- [How-to-setup-the-robot-for-wbc-experiments](How-to-setup-the-robot-for-wbc-experiments.md) \ No newline at end of file diff --git a/library/README.md b/library/README.md index fae559f..fcfb1f2 100644 --- a/library/README.md +++ b/library/README.md @@ -2,11 +2,11 @@ ## matlab-wbc -A collection of utlity functions/scripts used by the controllers. +A package of utility Matlab functions used by all the controllers. ### How to install the folder -Add the `matlab-wbc` folder to the Matlab path, or run the `startup.m` script in the `config` folder. If you chose to run the `startup.m` script, remember to start matlab from the folder where the `pathdef.m` file is (usually `~/Documents/MATLAB`). +Add the `matlab-wbc` folder to the Matlab path, or run the `startup_WBC.m` script in the `config` folder. If you chose to run the `startup_WBC.m` script, remember to **always** start matlab from the folder where the `pathdef.m` file is (usually `~/Documents/MATLAB`). ### How to use the +wbc library @@ -14,6 +14,8 @@ To use any function inside the package [matlab-wbc/+wbc](matlab-wbc/+wbc), add t ## matlab-gui -A static GUI for starting/closing the Simulink models without the need of opening them. +A static GUI for starting/closing the Simulink models without the need of opening the Simulink default GUI. +## simulink-library +A library of Simulink models used by all the controllers. \ No newline at end of file diff --git a/library/matlab-gui/closeModel_template.m b/library/matlab-gui/closeModel_template.m new file mode 100644 index 0000000..741a87e --- /dev/null +++ b/library/matlab-gui/closeModel_template.m @@ -0,0 +1,21 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% TEMPLATE FOR THE CLOSEMODEL FCN (TO BE USED IN THE STATIC GUI) + +% 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(MODEL_NAME_STRING); +close_system(MODEL_NAME_STRING); + +% close all figures +close all + +% remove paths (optional) +rmpath(PATH_TO_BE_REMOVED); + +disp('[closeModel]: done.') \ No newline at end of file diff --git a/library/matlab-gui/compileModel_template.m b/library/matlab-gui/compileModel_template.m new file mode 100644 index 0000000..8e716eb --- /dev/null +++ b/library/matlab-gui/compileModel_template.m @@ -0,0 +1,26 @@ +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% TEMPLATE FOR THE COMPILEMODEL FCN (TO BE USED WITH THE STATIC GUI) + +% Compile the Simulink model through Matlab command line. + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Compiling the Simulink model...') + +pause(1.5) + +try + SIMULINK_MODEL_NAME([], [], [], 'compile') + SIMULINK_MODEL_NAME([], [], [], '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/library/matlab-gui/simulinkStaticGUI.fig b/library/matlab-gui/simulinkStaticGUI.fig new file mode 100644 index 0000000000000000000000000000000000000000..d09bcbf8d62585b44e72c2cd39114ccef27c82ff GIT binary patch literal 8388 zcma)>bx<2l@aT~icXugXDDFiHEycBXaf%gp3xO7j6?gYiiWj#Kprp7IZHq&22@oKJ zK=S&1zc+8@{r7h6vwL@c-0bYm-R#`zsu=62sB%aNi*o3y7(a7%^KcU4Fz~PsaQ61| zl;=>_(KA<(mK5T6>E~o0;Pj5e+e@Cq?4Q-O_v7G@<`9#Xmy(bd7vm5Y6_exm{}dP@ z*8d2$rY`xv*$e>|R!Gr_#gsRT6b-^l+HEf5DXm;p{L8c`Zlj?pKuvboD74ZIC#Jqf zQ)RpPdNM3n*ZC*cO6wRM=O8QhL4u!ZJTBj(+#(d8PqiwJ@AivD!LJT8AvZssPo1d$ zHK5V9fx&M<5kMdsgm9k0#j4(TWcMnmvcg4WA(pn-?a)Q7$&?5JnfP1*IriHu3IkdM z*dWrW3_sb7B@mQSu^XmP;s((^=Sfr$n*h9XJctpT{tSF9c|FQ=AXf4E4=;`N&+MoQ zJ#;$P4d+T;0sZNE{ylv)v^1>o`SW2^iBW(F;3O=3p%r zRcdIxBR&X!jVx9-wUVfUS@*5FCvoT?(MeVoH&+q474a|DNvYx>J03T=t`7|T#T)|U z5EVX;9=kU&eF{o1$R@aY4*x7ZGg9R+QfR{q5^Wke&W+d@l+H3*;)o-ayDZZ2X)7&y zdq=#-RkMTGd)ol+#k6?LcbO! zcvT6Zd1 NV2e1zpkj<)SgKezyTlUB+}NBJDKUzx0(FUERKF&8$j(>YM)IbU>WN z;pNwZjDelWW{5pwmU#>@e!~L2Imh%LqG4;Ve5sqdVHILJfkk?cZC@=Gb@Nr9W;M&?NVR-Mxe5Shcx*ogOi?OpT*du}9oZ;pv*)~z8Me!f; z*;m2SR5B6z3aPhHXX55qEpcc}*8TRku5y%xWf3b?PUsc?HuKb#bCYaS2%+RopCq09 z=KE~+=2}{xbrLDiQ7-L9i+;X$mwvz34SgrCZHsl-;D`#$z6mvX-@4pMVPlV}F0ZA?C^990E-NLlx2gVKF1JmvZ0Cy$~Gs+C7 zq+mXp2>q+Ta$Wc2s%7oRdd!sk6~~_0@is>F1RmcEwqAazaj-NAp;or_ERd)W_{h?* zauH**d~hAR&^b%xhMIB#d{pfIwNw9g4w~fd#|_f5vW>ZLvj~v09@m^xPwk4{O%*9< zm2$7XSxXAV%z`3s5iN{*h4fRvhJ9F{6?dTc-4Ob~!nyZHVct zV!)lB1mMH_2+aMGAk$OVsR#pZ_dv=wjyk%}JGK-?t3r) zr#$=Gvnvlw-sw%bZ6n{znvmx)+qi%8s%NNY!j)1lOQOG{x1;W55Bv5D@KtEQOx&>tikcU!dnl*sy4<)_Btk!8FBTX-PSxb8!BGoaVCI8UB z0OxUZ#5G@o^5 z14wNU<%Z?6Fe{;^+OL+i&}sPcyN@lYS+b_oS&d9xX_4W{Y3lVhTUl`iDp^Yl zC2~!Ygbi`B-5Z2cOdBr8{pnHgG1$ z!|F0p6*K42Y&4yk2dRa7xO#a(lTO<}8J?z4aDpmSnugmwPc)uVLR zrHqz?gFhcX!<(@lLSi>?cAu0Gv(!XsW)H@?WQgtrMy`i5D8+eEQcUN)-AK?$D;(lI2!9~3V7`Ea2 zlwI;2_O#Q1GwLI}@&{&=!YVcPWq2>@l{6vzN=@WN3*mQbTk-XsulKc~-%+lAtX;j9 z)*Wqyx5%FY0vE~Y9=oY!HG^u66^9}_;12dSH5o@8oHGG+$S?jG+R5<;e-5my9yyj4 z+HaI$upElo-ztcMqy@cVJrtww;T(Z8h6U133r(hGhPwN%=TtQIF9OEN=9!a$Q!@d% z%6?1v-wy4Mhu}VfX`cF+XBQu?lGZF9xKH^qM=Ea-)w`?2Q1sGFCO6vyO5LYjN%W}~ z^PVC`(hH^V(-Xt&6(05i>N(`eRNcM~I@#)gEk~^k`2Or7{WmsW`!JjGNd5H8=|3Lu ztTs#XU76bK;7(A-pgjjXEntj~JKgSZF@j_g|CKdq{|G7CakR7Bw0mb~hx_Zr0G-fRF$#Or61^p(sOvi^Xxn9`m1HQ*{6NHEdy#%WOs|oXk3Mks~{-&GEq_=2`p; z4#G5uL_yNCjC{(<@1*wmZI;1RH*Gz;XE};Q+Bn=NglWwTR2oBM;E(DyTM7OlTYTr# zHAjte&)S>b-3iF=mo%aR*Lq9>>-x`1gg{H0Bl?g~TkXRwk z(m#7o-DkCYNkn#Og8v+Z1Th`gOE0ZB?AwZ1JXf~>OLw151-bzlMzkmj;0ZD;1@GKH zFWVX(p8_HG$~?*%7#LzQ1vRcXe@C2jI(VGt7o|0`Q}KY0b$2j(wg!nnhW=vcP){#? z?Ax}8sPJ;_5FQ!RlfMGs3i^tlx8Uc1CZkP@@T%IGf=NmGT%n(Qq={N2@&8&Bg(z}q zL5drAZzDu5?kBkgJn~Mnhne%NYPO$+HKv0vn}B-HIi6 z53XC?zTmuUSnZD8M>MeO#^8vMlt`@sRJ@u==k6C`O(a-~tn*S)*qC$LcLAx9-Uiv!FKdX{YL#mhpxe1l zQG1;4YtfO7AJPZ-K>0D%2T87ES+NH0pKN0|nM*6YW7J3X;?vU1Vzn@5=(Ev}rtyn^ zQhzjRjxPzqIvB}I#dCG|dkfCO_CH=on2W0{yp=8=JAWqwvMG7o=Wu17TQmAe=ZVv! zXgw`mBPGNg$8XY#FhP~5iaPyg+Z9n9Lr1>Zy2XOEjYJ_SIdvWTHM#FYQ}zP#pF7@0 zI&QF#C)alq@oSXPo)n0B=2s?Mk7~mAkioF;g~q`boKr`LvG(TKT5BvhyUfk&|srkUE<=;tENT;%j~u?MbAC z4rO{3Sk7KB`xj(Olmw7^bvdHS#g;-i;-Z%IXCHg&?;Mu|=t%=9wLSB`4fJyeoYPm6 zh^gRBy54wyEPBzyCXJMBZgB)}VrP{35l}Bhyxg)_XF7C?z@;iGF6&NHbkZW?kDYaF zb=~)Q-nUl~3Nktk8WYYM`SId9ZduN3M4yW#E7j>ecx|$b{tV=oeU^V`2g)pf8kOAL zEO>mU=3`WCj%9uN)qg$Y`6xb~Hg)OoiFoTK1Kt6bLVB($(C$-wAe*=M%Zu|-c0Cn_ zcRN;1tDPRlxzyKxZSDr247vfuXDfBM=UqQ7dx)wOR5 zzM%FO?}=qgOyLabReQO!qR7?1=TDj&3FFjc~p z%g&?wVyAD@hR9n%E@cO5gWiu&oiT+pz%1XtaOd`1AMC~PXF{1GBj&utJ3^VZeCBlP z4NN0jiM}lPlyT=ea%5ZiOvhR*L(EdmR)&t&-#Po0Wq%-g_ioW?mMGER`?Gu&)i$>@ z6ARSgTByV83Ux)*YoOL?_b8%9KZmAKu53_#L3x5G@}uw37urvy&PD5<)6F`HH;g%B zoqXB-SKB}A!qd02Y`8I{Mt5~uLD&2srih5Mnv}l1%k{TsLV9~ACyL=Ykyv7KxEm9DMuV7wnMBSzFLs)HQVM!zLLLxeqi)1 zDx@44FOp?5a{t2jZRvlr3}r2LKt6FahTA&iA0a1%-Y9y1@B0~^t`m6tUFZ6beg4tB zOa4(SaYs;cMrZcw8$WgrU3?;N8I+YLPAd{?9>&{x8xyu`2~bspZc`ue(U;B!r<{-N z4_p%NjzajdM7F5MbI%EP>xfDX^sE7@8zv^VG5wI*b3zcl@#&A5m+eXbrqu|Ijr2yP zD$AVz2SBW+ak68t0}F`J_sV6MvPM?8MmETpr99DbK)UvF%twz!SgD&JN1v)0@B|iD zpnvdS@bB=$Px=SUwzu!-g-T~T`_IQ(?T}Exf7G4dToQtUOO23JN=PQFLj8U+FQrhj zRlp@_GvHG9AGrL}x>q7CI{RDO`RpHe^K%C-NgkM+h-U5yRu%jo*IrDDlrzoToid`w#pHiAbz}#B}Dm z9C+h?HSUjbK|fmF%U6yrbV1{o!6z~Q3K0zE#6sJHS&e~hM6=+Lr4^G_nrw^nU;M=wMDs7Koa$z~H=eH*Y~9-DH0D*Q$b zmaU)4>yZ~=_vb0BFHHi~@2-^ZF%lSMIP7Fnevl!YPLxyws|1(j720HgbhHWQ<_U)B zg(^C5fOD^t?M1;kwJRLYjt)tV4H~W{M*;^}%7}NL|3Y&OP%aQ7`7ab---g~l_%D2s zW^jo*5K#s1W7AVV1hM#IH%A|csG#V5l3swRuSIZ#c^pqw9|Q(CF27>ua$>0LWG+Q; zh1Jp7((sU0h{F2&w2>9k%aAzeh49#pV#WxHJC(dS^#fk$>LN#Ks?4Y(`$h39ls=XW z7e-){4yyd}ZTbYL28H9perbzyrj8bTfro7J6&j4hHAutIl|8|*zIcfy;h3nsuA^2` zzg5^GuD4vq`&G(u|71(o!It~Z6}XXk59B~TXk4Napa_XE?_?-~cno$THx0wQcQ`SA zQ`h`(Mt9oe@e6$v571@pWsONxv`}!5a22sUpL`521Zm-Qnn>)YJHYS7L4h3DHr-za7yzr@6Gm_^dwXJS11wEQ!VAENs}#;x^i9r z&gQdjHB5S9WWjCl@&)V(S|F`!cV+~+y*%Bccp=*criTtbd<{O03@|NG3O$`1b2-Zq zd`&DfR$M#d$q;O13+y#Ga(}%r47vTOR>=m@$$=r2;Wkm#;l zxx*d87*k{7xX`LOM13{dv3uV##-t#VG~F)UDODHs!hfuGg=Iqf70yC<)=U+DOj0$p z8E;lAHC}pzTJ7Yx`_C_Us*95@nx3>68(Y0G9D$c`%Dw(2LF#@{Q`Z$6G;%+=UX<=q z_;&dn{~7mAx78HtYsrQl-L%=Ll6AtY8TC?2 z8uUHfR#VpY*(`D*di?ax-kk3H;vLtlq?VF$u^#U+Sh;QPylWpD90 zn8DQjCqydzp?AY}Y-#)JjD;lnAOr+_Tu%(ne&_CshfreNwEg5cZH)<57ZK>#_A7kc zvdd&@px6u!&DmpG9jBoYbU!NWfsuXpu_m&(WLW|K(T@9?!df8D!P+gRDE7_68LSy| zr)`Wzd5-A626s0u?TK`OXEKoldvV>n8Pl#4jLzoYhfmeMR=Hx{=aO{VnKRnw2?%(7 zu(wK;8soba5y~^;eBOOLcd21-s#xxRmWSpLvNiS1{#5`Y<_Zt^%|sT`Nc=gp(4nBV z;@KHv-37};cx;(UX=SBQyuKHeZ6vo6ryyPDxZ0UqO={zc^8Y+U=3?ISFKB=XE2!^mMJ<_ z6g)p}mN4bJ&C?Fi#%VP@CXRdJ<5C6!erxn{e>cbzb7ry`Z0cfBjFkCe?ovsy^oMxb z#-^F?Y^QAhnjogV)-g_DxX08Qay)QS9v{N$v>r@*bx;bmKAOSaUAWxrlhIS@ga34L z-H@pj2^%3<8Mu4a9&24Jv{Kz3)9~(rj#-xirBwugUV|T4J_N{<^FpSA26s7DSFqFr zCmIe4GRWA}HlHuTh{PJc@wC<+ZTWN0pz7ityp=03fh~fBL44$==ptN|8c=ExNVI9_ z{y4{0^O%g|fCxk1WJgxc*j);`5x9xB7Q__*4@vWnr*GC1M^UO}fnew7wo7W1d zPlmIP^iC}qX^#>Nzz zUu<-0!ZB1u9yaXKgR`*u=@xl-?wk5n-&=496Q&HO|azUQrdH4 z#Po&qR(u3_L+Sys)T;^&p6cpDUkg6n*dLbLxn9GF>|B2UWlkK|sakzor*1-6zBF=( zv*t!Vyf7C<3+kB= z$s3PepWVa-E*B*ga{ePyC9G{+gKHd@Dz@<9lJCKanWyMN(r2fZm(vQVk=~`Hk+9?q zV-9zCi-=37e(mMg3RFvfNzQ}6qPNNoHIvL&r|`!2+Ll4qEUzkd!h~pU`+Xj`uO8Xk z8*VtLY!o>~e}^SGv77IdovyTA<@>JmQ|I(&wls~RM_?@vmzAwcUiBTAM4&di(*2-j zQ!usq;Tls#r_pThZ2MV5#%((dm@xXm=J5*5#Lj*%6+`Tm5%HXajU6wzfAr>b-(+83 z>U<_8Q_pW&?#HbZxjJ$&6|>*AHoxtzYpZ@?_H4h2NpwTxlYi`{o{ADMd-8E2zZt+|{wHW1n8D=q+IBM$K#pcHo?*Z5Sxt(U(*XrrPT7t$wA#O{d% z+r+tbC^?Q)bv0mj5nbIZZDz6XkZScEZWeRiLU3%xaxEw*6U~AwsNe8Upn`jlu#2?& zWqt34vf%DI#NiF3?E&QZI;&qjr(a8@lvU6fExAor7CbF7fpBTRIOz$T!S+cT@$$m_Aak+-f{_r_y~buU1vnAd}HOr2(|Gm+l!kvac;LsAqIM6UrDf!!a% zg_JfaFMBEx@viB!Ce&*L33RT#K&BQp@TM7$Kc-UGr6CY>@$6<8&uik;{mF+VIYKI} zp=f3t!fk6%pr_~Jgwe|zzhDtAE#}eZ=4-Mz>%aBmdzjjfk1Ka8GasX}l2qw}aB_SU zZFt%Bxz;zLZTj?wb}X!w>ts@3MRIqjAR{!Gyh-1B0|v;_{`h~h`1S+6*)lsH48Oy| zy2&yHpM9Xdv2^h|^j_v1V3RM=HdRO7x{_rUJ8|^3xK?|59NpNy*Oro3Dzs7Ko!(;9 zb$NKL!Rm%ghgM!I7c%Q(+@7(Ir$F7hlh;gdjLRaZ?sR6HJXuDnz8zOFyh-X3C~%k@ z3tl543T4|Xk|f}%z*T94UIy4_ny?hbplAx4^80=$zP31_ZZCqS0Nta?R*I|&}1^2yG1>h^; z1Z74!s-C8+d>YOyS|z=^=T*HZ zQn`5(E+j)r#XeS3YIn@%84faX#?xaDQpN$8p#~HTy317fkEeMCuhz6)eAvld-V(z9 z%f^qX2UOjcTw?QknnM(K+3O5f+-ULf>z+f7YtLL#dQuyD0_$DJe*H-S2b_~Xvl-!} zjenydY(T_AsXZ-C+$C@vdNdlmFdF=bV-3Ba4*qn^xN6`ey#roT_jK;>N!i%7g3GqZ zvUGph+_bP%lN-(Q`Q&fHZiPYw^mP9$# zD}Akgb*E-@etJ*gb-kg!R3E0k)SvL5%)0NIZ}~Z%G@|v+3#b+ zg0C`Q@jsql?T#*#>=Lg|+IJqazRJau@$+%qO^xg}?N@3V5x&kVidb(y)wV{-AbxeA zbwOgHF!r^(liKjJSx{Y| + + % This function has no output args, see OutputFcn. + % + % hObject handle to figure + % eventdata reserved - to be defined in a future version of MATLAB + % handles structure with handles and user data (see GUIDATA) + % varargin command line arguments to simulinkStaticGUI (see VARARGIN) + + % Choose default command line output for simulinkStaticGUI + handles.output = hObject; + + % Update handles structure + guidata(hObject, handles); + + % UIWAIT makes simulinkStaticGUI wait for user response (see UIRESUME) + % uiwait(handles.figure1); + + % synchronization of the push button status with Simulink GUI + assignin('base','sl_synch_handles',handles) +end + +% --- Outputs from this function are returned to the command line. +function varargout = simulinkStaticGUI_OutputFcn(hObject, eventdata, handles) %#ok + + % varargout cell array for returning output args (see VARARGOUT); + % hObject handle to figure + % eventdata reserved - to be defined in a future version of MATLAB + % handles structure with handles and user data (see GUIDATA) + + % Get default command line output from handles structure + varargout{1} = handles.output; +end + +% --- Executes on button press in compileButton. +function compileButton_Callback(hObject, eventdata, handles) %#ok + + % COMPILE MODEL BUTTON + % + % hObject handle to compileButton (see GCBO) + % eventdata reserved - to be defined in a future version of MATLAB + % handles structure with handles and user data (see GUIDATA) + + % compile the model + compileModel; + + if ~exist('errorMessages','var') + + % enable the start/stop button + set(handles.startButton,'Enable','on') + set(handles.startButton,'Backgroundcolor','g'); + else + error(errorMessages.message) + end +end + +% --- Executes on button press in startButton. +function startButton_Callback(hObject, eventdata, handles) %#ok + + % START MODEL BUTTON + % + % hObject handle to startButton (see GCBO) + % eventdata reserved - to be defined in a future version of MATLAB + % handles structure with handles and user data (see GUIDATA) + + status = get_param(bdroot,'simulationstatus'); + + if strcmp(status,'stopped') + + set_param(bdroot,'simulationcommand','start') + + % Deactivate all buttons, including the start button itself + set(handles.startButton,'Backgroundcolor',[0.8,0.8,0.8]); + set(handles.startButton,'Enable','off') + set(handles.compileButton,'Backgroundcolor',[0.8,0.8,0.8]); + set(handles.compileButton,'Enable','off') + set(handles.exitButton,'Backgroundcolor',[0.8,0.8,0.8]); + set(handles.exitButton,'Enable','off') + + else + error('Model is already running') + end + + % Assign handles and the startstop object to the base workspace + assignin('base','sl_synch_handles',handles) +end + +% --- Executes on button press in stopModelButton. +function stopButton_Callback(hObject, eventdata, handles) %#ok + + % STOP MODEL BUTTON + % + % hObject handle to stopModelButton (see GCBO) + % eventdata reserved - to be defined in a future version of MATLAB + % handles structure with handles and user data (see GUIDATA) + + status = get_param(bdroot,'simulationstatus'); + + % 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') + + % Enable compile and exit buttons + set(handles.compileButton,'Backgroundcolor',[1.0,0.6,0.0]); + set(handles.compileButton,'Enable','on') + set(handles.exitButton,'Backgroundcolor',[0.0,1.0,1.0]); + set(handles.exitButton,'Enable','on') + + set_param(bdroot, 'SimulationCommand', 'Stop') + else + error('Model is not running') + end + + % Assign handles and the startstop object to the base workspace + assignin('base','sl_synch_handles',handles) +end + +% --- Executes on button press in exitButton. +function exitButton_Callback(hObject, eventdata, handles) %#ok + + % EXIT MODEL BUTTON + % + % hObject handle to exitButton (see GCBO) + % eventdata reserved - to be defined in a future version of MATLAB + % handles structure with handles and user data (see GUIDATA) + + closeModel; + + % Assign handles and the startstop object to the base workspace + assignin('base','sl_synch_handles',handles) +end \ No newline at end of file diff --git a/library/matlab-gui/torqueBalancingGui.fig b/library/matlab-gui/torqueBalancingGui.fig deleted file mode 100644 index c6e79114f490c819cbbf4f8af347afdbf5503beb..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 7378 zcma)=^;Z*)_y6fG2|YprmnKFjQ|~hhP_Hc0XS^c4PMtc2yTAb}#_G6KvNBOos^Z3H2_DIW+y3*KPkb8~bSqt$u9iYZjW%oTE6Kai* zVe>z|Hd(1#dH0djFMBGl1`-6$8T~1&U*6C~s#c|q#4|%jD_h)-FlSUbYfIng>9uVq zF40|J2r}vv<;sGyF^2P82>xj4JG*DOrSZD8r&?2J{&2KL9u}5@MATCq6sx3*_Q~pqC_*l&*_x>5 z)UV&n{)*i!U_Sj#<%)Qn3lWHxE?V7aO&}y@6?PTy&dYFmTj@4|7-GpHIk4>3A$1W8 zG!;{3>xpFS@8C6HXk@OHc)v_^5zDWV2o zr8`n7#gNfA@beyb4PH=}%DlVR!^3)fw43s!6yGBo2-i;4^+#hS-j3fX3E#CLej!ot z@hs|S%x2MLQBVM;cktSZ?ZuzsxN5yUM$%gs`KwX9ke&~x>aoCAnZS_(o@nzs%=)$+ zIkI9KlTirY&HPILwSG!yxA`6Z#{qo_1Z+K-k7BNCRyDz_#rQ%KoS{0Jwlw@F6F zj2Ymkh1?p2yN>l8k2k8tHLe}u zMO2ssoLb|TqHo%%U@T`93Ep?`^DfUN?Wwh8&?)hh(y)?kyShdtvHljp9gyo!?K6)Yw+Rl|C!J`d zIQjz5{g0xHP_Wsa38crX^yT}<&3k6uHE%}BwwbktWuWQxS(u#p&5v!`N;l31N=IP@ zIT^^C`ZL_$S?|1n-;b*HvhWsWqh_!!aF@8v&*i(=7x_=TX&EPoYO}r)bb*As7+CFn zcoda1){eH-Rak=DzIBHKi}vxC%u1z?rJ8I+840{xzrFVjHZBg;Z0B;){W8@f2x7Qw zN>SRu1dc)<{idOJpOc~sweea2Yax(S6U~5C>^`u zlVkJG%tBM8-w$oh-+k1+vGV)<+b*GN6YC&jP0b^mw2cimy9-!V(4HDzrpM-E3b zfR`PsZFdWui9SxsjxQOJ|9;ym5xPZ%&uyh^#-!shxHIX4{T84UgjMwJOoWr+fW*{C9KcivZ*c zui8-!P3BP<@()1bjuW3LAi*6V)gm%G+hbcWQXukPm|X5ulSkdQ9%3j|+#&i@?F)m8 zqM>9Y8zT%j2Gfgua1#QmwXIc_LGKEcu;LNQ$)Sl+8Iji?Q{hr<4MSYu6lRo;8*jhD z6(9FW1YQaN#q|eTdM0GLm(S?H%8aUEkEQBYJ}nyJc?Hd5!17wthecmGT)d$ow1tuJJY6S!XPu1Kh% zgV&g|S6NnOx*oetjHS2@IdMvQ$2W_XVT#K-$}Dt)a<%sFyXZMVb@z;aJH57Jux~ku zWwkaNKJ#&-KUZS0!|8%@frKCbyX0<8bKHB9GUqksj-o+5Y(rdwjsy;-90d}fz=`|_ zIG~4=lEBR9x%R}4@KFxPsk@$D5b!G++Woa)l<07xu65`AYliYeB$K6;=17z@;JITh z+xu(*fgin;EYPcO8sTmH*=ZGtPY(B9d|#*FKhzT6MEv>TBK4l>F9St^4gQ;c~rj8!`Crw7K1jR|Q|OJ=mZf-(5oKDfov! zDfB9o3r@8AF9p8t69uWg5Q8(b@+!n^U%ndqDL1u-aT_e(93b`=PIO65#QDCwj{Gxv ztbRPVbz7nfH7i)dWL#JLcX1e*3C$V-3$BkxYx4t}?j7#qX4#t#6L$)|12=VkeU|O( zbw?&ur3JDvVMwZJN<+cpOVk1PWU@erIcmNSGj2s&A@E}E&*6bwV;$$M$TP~Z=hVTt zZ!#xa=FXoio$1b7f7iF$u;5yGinOC&>jzWg}@O<78~Vd{4KP?}0c_-wu=7q&M;C!JC2*Pk)4(Vx^i zliyfl_!s7&`RUkQ@DH(TEVdTqfP|Dqu&LZp#$Gu5_}`|I_K$GrmXp2xhP|`BJ^s|o zu>l<18Ur9F?GD~%QZOPMKM0>arqaoQCT1kg9aTIKn>4oVi2YVGyPfVx1T0)6BYJle zYjFAGk0g0q{rBkS@2SPbWg5A7#WY1(J(Fx*Z8bd&Z5>l=EnOomUG=H}xrFSOifLoJ zYC%fO>32^G-;k1AiqNIAkkbU=i{Y@Ju*LmP7YW_}bPbpiWCkURC} zoiXm*5y$MeSm7 zta-m}?d3i#chd8&?6Z?F2n}i~{W`ItXu`=2s2cX-KH=LVc4boaAKDZ8{nqp`udCZ0j>B^o-LFzidJ4Hqi{xeq9TZ_42zh8_(af5iYHr~?Pv(uvUWj-{$&MttPD4ma6>4)DSf%8jiQ9HM=QM}hh>Ce?Xu1?>4oRUh0S)3 z($M`TK`cOUghHQau+N**)3L<9lU=k`?x@YAsYt?BYwX-pV!XXMviId&C6G^qR0M9} zMdL;wffZ?0)=$`%C2yHYn%z>B`pgu?H6u{lVw}z^+2aiLm~{$94D(qw$VhV5jN7t= z7#rDkUHCGeZ)k=Xs)d@(S$NqQI1urEkzP|{sH7-3QlI?&J;u#BH6$^``j@kjdRnlN zXZl4@{q=QVO*Vvi&uBq-==6gpN?5f7EP6VpY3Ep+Bf;tc&Hr>V&!E zZ4=5Gm!4yCR$f3WPr$?8Rx6RRQ5Mg~*r+;H*aCQ+pCXqizUcqMFRS!}4sE{oK#NZa z=**eMH|%q`yIdWiHq*Wijhc_@N2K>MvElTC&sakr>cZGP2Y;DJb46kkKY_fSR|Xpeyi5a zU6{*FRF{5s6PIhif9L(wiJxxYmWhIcm1sMwTjJo{UR4&Rp|fNASkh~WDEBEtM6F<) zVm{~-Vjz!N!`&!tGAZ!-;Kt;>KI2v;@r=~=M#mw*ITl70^tYK#VEcUAn}#w#oeG^c z{d_9U`ht<+vClU-TKU?h_~!$=|5dik7e6)N(fN;G%Hwfi1}^omS6lmFn}vue!#SIJ!{Qg1^d(0N`j`!(kB``GwfwPWyaC>MsgNazsR} zw`gmt4|sQskHYv+_fds!7MZ7HY&Kd`MIuY3s3QC?x~Ax;Zve=FS1^#`dD>}hX*lxi zCfm*K1BQoM&kJ#sm@xlikdC@GAdk3QYF%K5enA9?rG(4(Dk@o#&%^RDe5khXV4WHe zFpT8R=jc`AD*xG5(rY05T|pShy>BajYZSqQ2(tVhX0>tZ07Z9a`Fucuy zw(+k&U7Ex-&kf%qCS(Wip0ChvmGte34Ml(CWC$NZ(hy_N zEiLxfyJ529J>Pr{FvD&xhm*)JgQ&MrJ9mVTm<4%iwPUM$n!D2-MIi{&s^1l-V|~d# zLGZ=0j`l>K4~MR;!`R2IK1n7KNA7;j4V%ERPjZNC2S0r_#t&-9${? zlSU=>6L1=)k{38GeLcXtSO!}(S=L|o!&KN+K=j`$GNPYek7>TurmXl*eLY>=qc5fh zZs9tcNt!|Rh_76M&tzBVXXyH{nyvfPB_?lI0Nf(2Ab!UG;nmm8?`?N#1OW1%g$yMk znsYC&+5WynA}j(gtrZ1 z!pr#Yxq%vhTK~`aFM2>K86cGuz)O%y*xNskUguW;O2xw~pC|xmP|LQZkcKWTjD3`U z4mfd?$0_wv&iS<+f%j{>=B2WAlBF`heuM6H{bJcI!ZGille{i8s&Bpz#(w1EFU3mQ zDeBF6WR9k`zsQSUZ{$^l)r=cV*yy@hZY+G3*tpsjl2u$H==OyYmosO)u4eYBC}YkD zEATZ)soGO#tJ){R>{KrMHS2wWM3LQN)LV`YWp0yQ?~WxlZjZlj+$KZXmLO5N3l$?J zuRkT^_BnZxrPggs(J3@<1Sp>^?nqS~{Fl?)U(P)H+Ek{ZyvBDC0W*>@^6yYaB$>+W z{hT@=!l<`)1du{~k78z$AIYwTKd!6woeaAr{Uln=T#!(GIK<@?ac>D_M_4tMQ=TQzuuikW=v&hz>NsVVd6sSnmhJR_S0wYZ-=H2Y@qYJ>S_h2NY zKNzX|58;3K_=7`lqPNfAr~#C+`?liw6>RAhY-trBeBt*RXK1l{LlaSmbXfMnpEQ12XTXW!hWPOH`UU#$*?bar60v}=}5dcM$@6A8y-?g(G}D{j=i=QwRFZM zSq%d)g@h(MNT{09a(K7)anwLq))^QNxEbe1dnN|r_)ne zB`L$zxqijcPknU03HC=E+aKdB&VYEPw5sW`s#AB>2kvH6y0xr^TO`Sdjkii~ruN1V zD{SCWhxgz^!evL9*%yII440OfG6ON7zoD(8pgd(4VB`pR^~}>p-C-Q-oNp@}z08mG z!TlS-^7-rRlym?#|DnwxWUYj7?_vT9-vmouh51rZ3esiW-Vz%K z@9I{J*^=-9FU`oQ0$0svPIgWSvQEh^|NN#d9K<8yaFOilbV|vdF~u#Fj@p&Xok`+) zt@D`XH}1*84C_!_X@pDDmlp`)pFwSN#IkBz;qw+S$>NIg3rNwOV{dCI~bPqeEkkg?HhXZ^JG{>?C6E*1c0U8;z{XB zBjTsiR2rAkG&+vYdta)saLDK`JYS$n?B7$0!JpoEH-LBZCXcHz+9>*+m#(gi`%tGGA!P z8{LcRj>fi`A7M|lBsG3?v;{Sm_P;jRur=}jh`Sm)a1{^_oVx8OFgal;Md0~a^^X3B zuH5|FzMw{k&zZ2MyjW9LLdkw!7%M{?8&)A`E4&u?HFD4;DV@9ro=Df7k9Qh*o_D;3 zn7DYdAaWiQW8@gZ?oaj0W{_MSA-gx8H)%#9;T?O%UGksx*u1xAEr(Y__}`ARK%7ry zZZg!xb0t;SlOS~Q<_%<^CGXTB3-{~lZKz7yDw*`P^N|DFUBiRT(5x7bna5nuanL}1 zw5XqQB2~bz`>2u)SG*sdK6enVDPEl5TbIS#`^=9!-IgF_Y#i3i-NhS`ci8R@Nzr7z zn0rZ4@bb z%A`?Ol^GslSb*jqD16*)c6k>-xS#jBl+9b4xsE@SEyR_GLRv{3|ACbCkyb9{f!xLI z^Gk_D2rB4FTZ+_gck`Oc{R|~@fY|T)1jxhRSNHODXyS~+hls7QC4IK`&PcNE&hBv~ z{lc)z{;heKw!Qy&{GWe```QU*nugq_c9vtvPvheh*gjWaOA7m`(<>IBt6QCn)0RGb z@{8~~8MYj+yRS%#Ww-oX`vO+{XhCYWx|1lak^cDLxwqLl{M7mp;q(AoibZm8jkivW+vKn9~+XLo&8Zwqkv>$4;pj>DGSJPIN_-vRHa{x=7Y z&2^1dp3|G6De0L^S(h2sLZ;H1oIqs;?Bqm5TG6J)p&90J9HmVxPpKvymYl8#zMe#; zufOFq{=mh-EEDQ)xy+yY2+4SOw;naUb;HeF2v33v-Xh`e?q7>veu9#T4&N-*2)exM~T9UpPz9&W3nMO-JP7u(VWT& z4BpwcCVWzYtSbvHa6`MHH1A|^ndTl=Gi8vnJ>MZLQ7I>mDq%ze6USx|XXfJ;g_z*$ z>CGsT&D}EpuVe%KTq_l%oE%gy4v+fvV3^#beO)|_DDXNFlQ5gjefd^H|J--TYG#|* zJ2Skv?=1@P@}EErN&r}1{cvaC1WtBH^=)f`8B%BWYnO1|57^jh-FOAdnqsA2x56ID zn0t819uwx`{jD}p^cT8Yys#;c!UD3zzUJXE%Xu~E7SZ&3&RBc*7}aO6hL=@nK8nT! z(#MLEzK($y5_B33^{tx&TYPpa32l`s-z+&;oypIUAr+Jz%~Rl&PljX)HFN@1@_+N? z^X6o3i(2=%9!!p+`f(F>ZF6p>U2>dPi_qWB0D!EC`6odRrj%9fEyiQ|27*JPjNA3} z+lU%sO&F_)QgRj&djwO;}dC3v_;0nHrBf@IzB|vs{1tI z9D`1ME=n#P&#)d%RSed(O1LDQtWXSSRBM%VH6*21g>ps696vaOJ<5K0iG z8j{#}uM+z9X+P=6%7f_u(-Q*LoKYr~MPLoQYxsk*UJaEJ0OfP*%S4*Fi>kk2(svpO zUkPb{k2%Mz2e)|CtKA1g45Mr;Z3Lc1Pr+}NEdToH$fk63;X*M9GL&$^pqlU3sW2CR9!Np-*Ks@{Qm%AGNKd! diff --git a/library/matlab-gui/torqueBalancingGui.m b/library/matlab-gui/torqueBalancingGui.m deleted file mode 100644 index 4d2a878..0000000 --- a/library/matlab-gui/torqueBalancingGui.m +++ /dev/null @@ -1,164 +0,0 @@ -function varargout = torqueBalancingGui(varargin) - - % TORQUEBALANCINGGUI MATLAB code for torqueBalancingGui.fig - % - % TORQUEBALANCINGGUI, by itself, creates a new TORQUEBALANCINGGUI or raises the existing - % singleton*. - % - % H = TORQUEBALANCINGGUI returns the handle to a new TORQUEBALANCINGGUI or the handle to - % the existing singleton*. - % - % TORQUEBALANCINGGUI('CALLBACK',hObject,eventData,handles,...) calls the local - % function named CALLBACK in TORQUEBALANCINGGUI.M with the given input arguments. - % - % TORQUEBALANCINGGUI('Property','Value',...) creates a new TORQUEBALANCINGGUI or raises the - % existing singleton*. Starting from the left, property value pairs are - % applied to the GUI before torqueBalancingGui_OpeningFcn gets called. An - % unrecognized property name or invalid value makes property application - % stop. All inputs are passed to torqueBalancingGui_OpeningFcn via varargin. - % - % *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one - % instance to run (singleton)". - % - % See also: GUIDE, GUIDATA, GUIHANDLES - - % Edit the above text to modify the response to help torqueBalancingGui - - % Last Modified by GUIDE v2.5 13-Dec-2018 14:51:36 - - % Begin initialization code - DO NOT EDIT - gui_Singleton = 1; - gui_State = struct('gui_Name', mfilename, ... - 'gui_Singleton', gui_Singleton, ... - 'gui_OpeningFcn', @torqueBalancingGui_OpeningFcn, ... - 'gui_OutputFcn', @torqueBalancingGui_OutputFcn, ... - 'gui_LayoutFcn', [] , ... - 'gui_Callback', []); - - if nargin && ischar(varargin{1}) - gui_State.gui_Callback = str2func(varargin{1}); - end - - if nargout - [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); - else - gui_mainfcn(gui_State, varargin{:}); - end - % End initialization code - DO NOT EDIT -end - -% --- Executes just before torqueBalancingGui is made visible. -function torqueBalancingGui_OpeningFcn(hObject, eventdata, handles, varargin) %#ok - - % This function has no output args, see OutputFcn. - % - % hObject handle to figure - % eventdata reserved - to be defined in a future version of MATLAB - % handles structure with handles and user data (see GUIDATA) - % varargin command line arguments to torqueBalancingGui (see VARARGIN) - - % Choose default command line output for torqueBalancingGui - handles.output = hObject; - - % Update handles structure - guidata(hObject, handles); - - % UIWAIT makes torqueBalancingGui wait for user response (see UIRESUME) - % uiwait(handles.figure1); - - % synchronization of the push button status with Simulink GUI - assignin('base','sl_synch_handles',handles) -end - -% --- Outputs from this function are returned to the command line. -function varargout = torqueBalancingGui_OutputFcn(hObject, eventdata, handles) %#ok - - % varargout cell array for returning output args (see VARARGOUT); - % hObject handle to figure - % eventdata reserved - to be defined in a future version of MATLAB - % handles structure with handles and user data (see GUIDATA) - - % Get default command line output from handles structure - varargout{1} = handles.output; -end - -% --- Executes on button press in pushbutton1. -function pushbutton1_Callback(hObject, eventdata, handles) %#ok - - % hObject handle to pushbutton1 (see GCBO) - % eventdata reserved - to be defined in a future version of MATLAB - % handles structure with handles and user data (see GUIDATA) - - % PUSHBUTTON1 = COMPILE MODEL - - % compile the model - compileModel; - - if ~exist('errorMessages','var') - - % enable the start/stop button - set(handles.pushbutton5,'Enable','on') - set(handles.pushbutton5,'Backgroundcolor','g'); - else - error(errorMessages.message) - end -end - -% --- Executes on button press in pushbutton4. -function pushbutton4_Callback(hObject, eventdata, handles) %#ok - - % hObject handle to pushbutton5 (see GCBO) - % eventdata reserved - to be defined in a future version of MATLAB - % handles structure with handles and user data (see GUIDATA) - - % PUSHBUTTON4 = CLOSE MODEL - - closeModel; - - % Assign handles and the startstop object to the base workspace - assignin('base','sl_synch_handles',handles) -end - -% --- Executes on button press in pushbutton5. -function pushbutton5_Callback(hObject, eventdata, handles) %#ok - - % hObject handle to pushbutton4 (see GCBO) - % eventdata reserved - to be defined in a future version of MATLAB - % handles structure with handles and user data (see GUIDATA) - - % PUSHBUTTON5 = START/STOP MODEL - - mystring = get(hObject,'String'); - status = get_param(bdroot,'simulationstatus'); - - if strcmp(mystring,'Start Model') - - % Check the status of the simulation and start if it's stopped - if strcmp(status,'stopped') - set_param(bdroot,'simulationcommand','start') - end - - % Update the string on the pushbutton - set(handles.pushbutton5,'String','Stop Model') - set(handles.pushbutton5,'Backgroundcolor','r'); - - elseif strcmp(mystring,'Stop Model') - - % Check the status of the simulation and stop it if it's running - if strcmp(status,'running') - set_param(bdroot, 'SimulationCommand', 'Stop') - end - - % Update the string on the pushbutton - set(handles.pushbutton5,'String','Start Model') - set(handles.pushbutton5,'Backgroundcolor',[0.8,0.8,0.8]); - - % disable the button (simulation terminated) - set(handles.pushbutton5,'Enable','inactive'); - else - warning('Unrecognized string for pushbutton4') %#ok - end - - % Assign handles and the startstop object to the base workspace - assignin('base','sl_synch_handles',handles) -end diff --git a/library/matlab-wbc/+wbc/addMotorsInertia.m b/library/matlab-wbc/+wbc/addMotorsInertia.m deleted file mode 100644 index fae3567..0000000 --- a/library/matlab-wbc/+wbc/addMotorsInertia.m +++ /dev/null @@ -1,70 +0,0 @@ -function M_with_inertia = addMotorsInertia(M,Config) - - % ADDMOTORSINERTIA adds the motors reflected inertias to the joint space - % mass matrix. - % - % PROCEDURE: the assume the floating base + joint + motors dynamics can be - % described by the following system of equations: - % - % M_b*nuDot_b + M_bj*qjDDot + h_b = J_b^T*f (1) - % - % M_j*qjDDot + M_jb*nuDot_b + h_j = J_j^T*f + tau_j (2) - % - % I_m*thetaDDot = tau_m - transpose(T*Gamma)*tau_j (3) - % - % Assuming that the joint is rigidly attached to the motor, one has that: - % - % thetaDDot = (T*Gamma)^-1*qjDDot (4) - % - % where Gamma is a diagonal matrix containing the transmission ratio for - % all joints, while T is a block diagonal matrix taking into account the - % fact that some joint movements are obtained by combining the effect of - % different motors (see also http://wiki.icub.org/wiki/ICub_coupled_joints). - % By substituting (4) into (3), multiplying (3) by transpose(T*Gamma)^-1 and - % finally summing up (3) and (2), one has: - % - % M_b*nuDot_b + M_bj*qjDDot + h_b = J_b^T*f (1) - % - % (M_j+transpose(T*Gamma)^-1*I_m*(T*Gamma)^-1)*qjDDot + M_jb*nuDot_b + h_j = J_j^T*f + u (2b) - % - % where u = transpose(T*Gamma)^-1*tau_m. The input joint toruqes can be calculated from - % (3) as follows: - % - % tau_j = u - K_ff*transpose(T*Gamma)^-1*I_m*(T*Gamma)^-1*qjDDot - % - % with K_ff belonging to [0,1]. - % - % FORMAT: M_with_inertia = addMotorsInertia(M,Config) - % - % INPUT: -M = [6+n x 6+n] mass matrix - % -Config = user defined configuration parameters - % - % OUTPUT: -M_with_inertia = [6+n x 6+n] mass matrix with motor reflected - % inertia - % - % Authors: Gabriele Nava - % - % all authors are with the Italian Istitute of Technology (IIT) - % email: name.surname@iit.it - % - % Genoa, Dec 2017 - % - - %% --- Initialization --- - - % parameters - nDof = size(M,1)-6; - - % add motors reflected inertias - if Config.USE_MOTOR_REFLECTED_INERTIA - - reflectedInertia = wbc.computeMotorsInertia(Config); - M_reflected_inertia = [zeros(6,6+nDof); - zeros(nDof,6) reflectedInertia]; - else - - M_reflected_inertia = zeros(size(M)); - end - - M_with_inertia = M +M_reflected_inertia; -end diff --git a/library/matlab-wbc/+wbc/addMotorsReflectedInertia.m b/library/matlab-wbc/+wbc/addMotorsReflectedInertia.m new file mode 100644 index 0000000..1e4c91a --- /dev/null +++ b/library/matlab-wbc/+wbc/addMotorsReflectedInertia.m @@ -0,0 +1,69 @@ +function M_with_inertia = addMotorsReflectedInertia(M,Gamma,T,I_m) + + % ADDMOTORSREFLECTEDINERTIA adds the motors reflected inertias to the + % joint space mass matrix. + % + % PROCEDURE: the assume the floating base + joint + motors dynamics can be + % described by the following system of equations: + % + % M_b*nuDot_b + M_bs*sDDot + h_b = J_b^T*f (1) + % + % M_s*sDDot + M_sb*nuDot_b + h_s = J_s^T*f + tau_s (2) + % + % I_m*thetaDDot = tau_m - transpose(T*Gamma)*tau_s (3) + % + % Assuming that the joints are rigidly attached to the motors, one has that: + % + % thetaDDot = (T*Gamma)^-1*sDDot (4) + % + % where Gamma is a diagonal matrix containing the transmission ratio for + % all joints, while T is a block diagonal matrix taking into account the + % fact that some joint movements are obtained by combining the effect of + % different motors (see also http://wiki.icub.org/wiki/ICub_coupled_joints). + % By substituting (4) into (3), multiplying (3) by transpose(T*Gamma)^-1 and + % finally summing up (3) and (2), one has: + % + % M_b*nuDot_b + M_bs*sDDot + h_b = J_b^T*f (1) + % + % (M_s+transpose(T*Gamma)^-1*I_m*(T*Gamma)^-1)*sDDot + M_sb*nuDot_b + h_s = J_s^T*f + u (2b) + % + % where u = transpose(T*Gamma)^-1*tau_m. The input joint torques can be + % calculated from (3) as follows: + % + % tau_s = u - K_ff*transpose(T*Gamma)^-1*I_m*(T*Gamma)^-1*sDDot + % + % with K_ff belonging to [0,1]. + % + % FORMAT: M_with_inertia = addMotorsReflectedInertia(M,Gamma,T,I_m) + % + % INPUT: - M = [6+n x 6+n] mass matrix; + % - Gamma = [n x n] diagonal matrix that accounts for the transmission + % ratio of the joints in the mechanism; + % - T = [n x n] matrix that accounts for the coupling between + % different joints; + % - I_m = [n x n] diagonal matrix that contains the motors + % inertia (not reflected). + % + % OUTPUT: - M_with_inertia = [6+n x 6+n] mass matrix with motor reflected + % inertia. + % + % Authors: Gabriele Nava + % + % all authors are with the Italian Istitute of Technology (IIT) + % email: name.surname@iit.it + % + % Genoa, Dec 2017 + % + + %% --- Initialization --- + + % parameters + NDOF = size(M,1)-6; + + reflectedInertia = wbc.computeMotorsReflectedInertia(Gamma,T,I_m); + + M_reflected_inertia = [zeros(6,6+NDOF); + zeros(NDOF,6) reflectedInertia]; + + M_with_inertia = M + M_reflected_inertia; +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/analyticalSolutionQP.m b/library/matlab-wbc/+wbc/analyticalSolutionQP.m index 6388cae..62951d3 100644 --- a/library/matlab-wbc/+wbc/analyticalSolutionQP.m +++ b/library/matlab-wbc/+wbc/analyticalSolutionQP.m @@ -1,15 +1,15 @@ function analyticalSolution = analyticalSolutionQP(HessianMatrixQP,gradientQP) - % ANALYTICALSOLUTION resolves the unconstrained solution to a QP problem - % (to be used as possible alternative when the WBToolbox - % QP block fails to find a solution) + % ANALYTICALSOLUTIONQP provides the unconstrained solution of a QP + % problem. To be used as possible alternative when + % the WBToolbox "QP block" fails to find a solution. % % FORMAT: analyticalSolution = analyticalSolutionQP(HessianMatrixQP,gradientQP) % % INPUT: - HessianMatrixQP = hessian matrix of the QP problem; - % - gradientQP = gradient of the QP problem. + % - gradientQP = gradient of the QP problem. % - % OUTPUT: - analyticalSolution = the analytical solution to the QP problem. + % OUTPUT: - analyticalSolution = the analytical solution of the QP problem. % % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava % @@ -22,4 +22,4 @@ %% --- Initialization --- analyticalSolution = -inv(HessianMatrixQP)*gradientQP; -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/basePoseDerivative.m b/library/matlab-wbc/+wbc/basePoseDerivative.m index ed5ef5a..44f5ab1 100644 --- a/library/matlab-wbc/+wbc/basePoseDerivative.m +++ b/library/matlab-wbc/+wbc/basePoseDerivative.m @@ -1,11 +1,12 @@ function pose_bDot = basePoseDerivative(nu_b, pose_b) % BASEPOSEDERIVATIVE calculates the derivative of the floating base pose in - % terms of quaternion derivative. It is composed by the - % linear base velocities, and the derivative of the base + % terms of position + quaternion derivative. The + % resulting term is composed by the linear base + % velocities, and the derivative of the base % orientation expressed in quaternions. % - % FORMAT: basePoseDot = basePoseDerivative(nu_b, pose_b) + % FORMAT: pose_bDot = basePoseDerivative(nu_b, pose_b) % % INPUT: - nu_b = [6 * 1] base velocity % - pose_b = [7 * 1] base pose in quaternions @@ -24,19 +25,19 @@ %% --- Initialization --- % base quaternion - qt_b = pose_b(4:end); + qt_b = pose_b(4:end); % base rotation matrix - w_R_b = wbc.rotationFromQuaternion(qt_b); + w_R_b = wbc.rotationFromQuaternion(qt_b); - % transform base velocity into the world frame - omega_b = nu_b(4:end); - omega_w = transpose(w_R_b)*omega_b; + % express base velocity w.r.t. the base frame + w_omega_b = nu_b(4:end); + b_omega_b = transpose(w_R_b)*w_omega_b; % calculate the quaternion derivative - k = 1; - qt_bDot = wbc.quaternionDerivative(qt_b, omega_w, k); + k = 1; + qt_bDot = wbc.quaternionDerivative(qt_b, b_omega_b, k); % compute the base pose derivative - pose_bDot = [nu_b(1:3); qt_bDot]; -end + pose_bDot = [nu_b(1:3); qt_bDot]; +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/checkRange.m b/library/matlab-wbc/+wbc/checkRange.m index e29bc21..a5ab6c5 100644 --- a/library/matlab-wbc/+wbc/checkRange.m +++ b/library/matlab-wbc/+wbc/checkRange.m @@ -1,13 +1,13 @@ function inRange = checkRange(umin, umax, u, tol) - % CHECKRANGE checks if the current joint position is inside the limits. + % CHECKRANGE checks if the current input value is inside the limits. % % FORMAT: inRange = checkRange(umin, umax, u, tol) % % INPUT: - umin = [n * 1] min values % - umax = [n * 1] max values - % - u = [n * 1] values - % - tol = tolerance + % - u = [n * 1] current values + % - tol = tolerance % % OUTPUT: - inRange = boolean for checking if u is inside the limits % @@ -29,4 +29,4 @@ else inRange = 0; end -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/checkSpikes.m b/library/matlab-wbc/+wbc/checkSpikes.m index 3162c74..916db42 100644 --- a/library/matlab-wbc/+wbc/checkSpikes.m +++ b/library/matlab-wbc/+wbc/checkSpikes.m @@ -35,6 +35,7 @@ res = sum(res); if abs(res) < 0.1 + noSpikes = 1; else noSpikes = 0; diff --git a/library/matlab-wbc/+wbc/computeBaseVelocity.m b/library/matlab-wbc/+wbc/computeBaseVelocity.m deleted file mode 100644 index 0560a65..0000000 --- a/library/matlab-wbc/+wbc/computeBaseVelocity.m +++ /dev/null @@ -1,35 +0,0 @@ -function nu_b = computeBaseVelocity(J_LeftFoot, J_RightFoot, feetInContact, sDot, Sat) - - % COMPUTEBASEVELOCITY computes the floating base velocity assuming - % left/right foot is in contact with the ground. - % - % FORMAT: nu_b = computeBaseVelocity(J_LeftFoot,J_RightFoot, feetInContact, sDot, Sat) - % - % INPUT: - J_LeftFoot = [6 * ROBOT_DOF +6] left foot Jacobian - % - J_RightFoot = [6 * ROBOT_DOF +6] right foot Jacobian - % - feetInContact = [2 * 1] feet in contact - % - sDot = [ROBOT_DOF * 1] joint velocity - % - Sat = saturation parameters - % - % OUTPUT: - nu_b = [6 * 1] floating base velocity - % - % 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 --- - - % Compute Jacobian - Jc = [feetInContact(1)*J_LeftFoot; - feetInContact(2)*J_RightFoot]; - - % Compute multiplier of nu_b - pinvJb = (transpose(Jc(:,1:6))*Jc(:,1:6) +Sat.pinvDamp_nu_b*eye(6))\transpose(Jc(:,1:6)); - - % Base velocity - nu_b = -pinvJb*Jc(:,7:end)*sDot; -end diff --git a/library/matlab-wbc/+wbc/computeBaseVelocityWithFeetContact.m b/library/matlab-wbc/+wbc/computeBaseVelocityWithFeetContact.m new file mode 100644 index 0000000..6e8726c --- /dev/null +++ b/library/matlab-wbc/+wbc/computeBaseVelocityWithFeetContact.m @@ -0,0 +1,37 @@ +function nu_b = computeBaseVelocityWithFeetContact(J_l_sole, J_r_sole, feetContactStatus, jointVel, pinvDampTolerance) + + % COMPUTEBASEVELOCITYWITHFEETCONTACT computes the floating base velocity assuming the + % left and/or right foot is in contact with the ground. + % + % FORMAT: nu_b = computeBaseVelocityWithFeetContact(J_l_sole, J_r_sole, feetContactStatus, jointVel, pinvDampTolerance) + % + % INPUT: - J_l_sole = [6 * ROBOT_DOF +6] left foot Jacobian + % - J_r_sole = [6 * ROBOT_DOF +6] right foot Jacobian + % - feetContactStatus = [2 * 1] booleans describing the status of + % the contacts (1 contact is active, 0 contact + % is deactivated); [left; right] + % - jointVel = [ROBOT_DOF * 1] joint velocity + % - pinvDampTolerance = tolerance for matrix pseudoinverse. + % + % OUTPUT: - nu_b = [6 * 1] floating base velocity + % + % 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 --- + + % Compute full contacts Jacobian + Jc = [feetContactStatus(1)*J_l_sole; + feetContactStatus(2)*J_r_sole]; + + % Compute multiplier of nu_b + pinvJb = wbc.pinvDamped(Jc(:,1:6), pinvDampTolerance); + + % Base velocity + nu_b = -pinvJb*Jc(:,7:end)*jointVel; +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/computeMotorsInertia.m b/library/matlab-wbc/+wbc/computeMotorsInertia.m deleted file mode 100644 index b6d97a9..0000000 --- a/library/matlab-wbc/+wbc/computeMotorsInertia.m +++ /dev/null @@ -1,28 +0,0 @@ -function reflectedInertia = computeMotorsInertia(Config) - - % COMPUTEMOTORSINERTIA compute the motors reflected inertia. - % - % FORMAT: reflectedInertia = computeMotorsInertia(Config) - % - % INPUT: -Config = user defined configuration parameters - % - % OUTPUT: -reflectedInertia = [n x n] matrix of motors reflected inertia - % - % Authors: Gabriele Nava - % - % all authors are with the Italian Istitute of Technology (IIT) - % email: name.surname@iit.it - % - % Genoa, Dec 2017 - % - - %% --- Initialization --- - - % compute motors reflected inertia - Gamma = Config.Gamma; - T = Config.T; - I_m = Config.I_m; - invTGamma = eye(size(Gamma))/(T*Gamma); - invTGamma_t = eye(size(Gamma))/(transpose(T*Gamma)); - reflectedInertia = invTGamma_t*I_m*invTGamma; -end diff --git a/library/matlab-wbc/+wbc/computeMotorsReflectedInertia.m b/library/matlab-wbc/+wbc/computeMotorsReflectedInertia.m new file mode 100644 index 0000000..60749f7 --- /dev/null +++ b/library/matlab-wbc/+wbc/computeMotorsReflectedInertia.m @@ -0,0 +1,32 @@ +function reflectedInertia = computeMotorsReflectedInertia(Gamma,T,I_m) + + % COMPUTEMOTORSREFLECTEDINERTIA compute the motors reflected inertia for + % a mechanical system composed of motors, + % links and RIGID transmissions. + % + % FORMAT: reflectedInertia = computeMotorsReflectedInertia(Gamma,T,I_m) + % + % INPUT: - Gamma: [n * n] diagonal matrix that accounts for the transmission + % ratio of the joints in the mechanism; + % - T: [n * n] matrix that accounts for the coupling between + % different joints; + % - I_m: [n * n] diagonal matrix that contains the motors + % inertia (not reflected). + % + % OUTPUT: - reflectedInertia: [n * n] matrix of motors reflected inertia + % + % Authors: Gabriele Nava + % + % all authors are with the Italian Istitute of Technology (IIT) + % email: name.surname@iit.it + % + % Genoa, Dec 2017 + % + + %% --- Initialization --- + + % compute motors reflected inertia + invTGamma = eye(size(Gamma))/(T*Gamma); + invTGamma_t = eye(size(Gamma))/(transpose(T*Gamma)); + reflectedInertia = invTGamma_t*I_m*invTGamma; +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/computeRigidContactConstraints.m b/library/matlab-wbc/+wbc/computeRigidContactConstraints.m new file mode 100644 index 0000000..23b6338 --- /dev/null +++ b/library/matlab-wbc/+wbc/computeRigidContactConstraints.m @@ -0,0 +1,105 @@ +function [ConstraintsMatrix, bVectorConstraints] = computeRigidContactConstraints(staticFrictionCoefficient, numberOfPoints, torsionalFrictionCoefficient, contactAreaSize, fZmin) + + % COMPUTERIGIDCONTACTCONSTRAINTS computes the constraint matrix and bias vector + % for applying friction cones, unilateral + % constraints and local CoP constraints at + % contact locations (assuming rigid contacts). + % The output are the matrix C and the + % bias vector b such that: + % + % C*f < b + % + % f are the contact forces and moments + % w.r.t. a reference frames attached to + % the contact location. + % + % FORMAT: [ConstraintsMatrix, bVectorConstraints] = computeRigidContactConstraints ... + % (staticFrictionCoefficient, numberOfPoints, torsionalFrictionCoefficient, footSize, fZmin) + % + % INPUT: - staticFrictionCoefficient = static linear coefficient of friction + % - numberOfPoints = number of points in each quadrants for + % linearizing friction cone + % - torsionalFrictionCoefficient = torsional coefficient of friction + % - contactAreaSize = physical size of the contact area + % - fZmin = minimal positive vertical force at contact + % + % OUTPUT: - ConstraintsMatrix = constraint matrix + % - bVectorConstraints = bias vector constraints + % + % 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 --- + + % Compute friction cones contraints approximation with straight lines + + % split the pi/2 angle into numberOfPoints -1 + segmentAngle = pi/2 / (numberOfPoints - 1); + + % define angle + angle = 0:segmentAngle:(2*pi -segmentAngle); + points = [cos(angle); sin(angle)]; + numberOfEquations = size(points, 2); + assert(size(points, 2) == (4 * (numberOfPoints - 2) + 4)); + + % A_ineq*x <= b, with b all zeros + A_ineq = zeros(numberOfEquations, 6); + + % define equations + for i = 1 : numberOfEquations + + firstPoint = points(:, i); + secondPoint = points(:, rem(i, numberOfEquations) + 1); + + % define line passing through the above points + angularCoefficients = (secondPoint(2) -firstPoint(2)) / (secondPoint(1) -firstPoint(1)); + + offsets = firstPoint(2) -angularCoefficients*firstPoint(1); + + inequalityFactor = + 1; + + % if any of the two points are between pi and 2pi, then the inequality is + % in the form of y >= m*x + q, and is needed to change the sign of it. + if (angle(i) > pi || angle(rem(i, numberOfEquations) + 1) > pi) + + inequalityFactor = -1; + end + + % a wrench is 6 dimensional f = [fx, fy, fz, mux, muy, muz]' + % there are constraints on fy and fz, and the offset will be multiplied + % by mu * fx + % + A_ineq(i,:) = inequalityFactor.* [-angularCoefficients, 1, (-offsets*staticFrictionCoefficient), 0, 0, 0]; + end + + %% POSITIVITY OF VERTICAL FORCE, AND COP + % + % Positivity of vertical force: F_z > fZmin + % Center of pressure (CoP): dimMinArea_y < torque_x/F_z < dimMaxArea_y + % dimMinArea_x < -torque_y/F_z < dimMaxArea_x + % + % footSize = [dimMiArea_x, dimMaxArea_x + % dimMinArea_y, dimMaxArea_y] + % + % F_x F_y F_z torque_x torque_y torque_z + ConstraintsMatrix = [ 0, 0, -torsionalFrictionCoefficient, 0, 0, 1; % torque_z -torsionalFrictionCoefficient*F_z < 0 + 0, 0, -torsionalFrictionCoefficient, 0, 0, -1; % -torque_z -torsionalFrictionCoefficient*F_z < 0 + 0, 0, -1, 0, 0, 0; % -F_z < -fZmin + 0, 0, contactAreaSize(1,1), 0, 1, 0; % torque_y +dimMinArea_x*F_z < 0 + 0, 0, -contactAreaSize(1,2), 0, -1, 0; % -torque_y -dimMaxArea_x*F_z < 0 + 0, 0, contactAreaSize(2,1), -1, 0, 0; % -torque_x +dimMinArea_y*F_z < 0 + 0, 0, -contactAreaSize(2,2), 1, 0, 0]; % torque_x -dimMaxArea_y*F_z < 0 + + % add inequality constraints + ConstraintsMatrix = [A_ineq; + ConstraintsMatrix ]; + + bVectorConstraints = [zeros(size(A_ineq,1), 1); zeros(7,1)]; + + bVectorConstraints(3 + size(A_ineq,1)) = -fZmin; +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/constraints.m b/library/matlab-wbc/+wbc/constraints.m deleted file mode 100644 index 678943f..0000000 --- a/library/matlab-wbc/+wbc/constraints.m +++ /dev/null @@ -1,94 +0,0 @@ -function [ConstraintsMatrix, bVectorConstraints] = constraints(staticFrictionCoefficient, numberOfPoints, torsionalFrictionCoefficient, footSize, fZmin) - - % CONSTRAINTS computes the constraint matrix and bias vector for applying - % friction cones and unilateral constraints at contact locations. - % - % FORMAT: [ConstraintsMatrix, bVectorConstraints] = constraints ... - % (staticFrictionCoefficient, numberOfPoints, torsionalFrictionCoefficient, footSize, fZmin) - % - % INPUT: - staticFrictionCoefficient = static linear coefficient of friction - % - numberOfPoints = number of points in each quadrants for - % linearizing friction cone - % - torsionalFrictionCoefficient = torsional coefficient of friction - % - footSize = physical size of the foot - % - fZmin = minimal positive vertical force at contact - % - % OUTPUT: - ConstraintsMatrix = constraint matrix - % - bVectorConstraints = bias vector constraints - % - % 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 --- - - % Compute friction cones contraints approximation with straight lines - - % split the pi/2 angle into numberOfPoints -1 - segmentAngle = pi/2 / (numberOfPoints - 1); - - % define angle - angle = 0:segmentAngle:(2*pi -segmentAngle); - points = [cos(angle); sin(angle)]; - numberOfEquations = size(points, 2); - assert(size(points, 2) == (4 * (numberOfPoints - 2) + 4)); - - % A_ineq*x <= b, with b all zeros - A_ineq = zeros(numberOfEquations, 6); - - % define equations - for i = 1 : numberOfEquations - - firstPoint = points(:, i); - secondPoint = points(:, rem(i, numberOfEquations) + 1); - - % define line passing through the above points - angularCoefficients = (secondPoint(2) -firstPoint(2)) / (secondPoint(1) -firstPoint(1)); - - offsets = firstPoint(2) -angularCoefficients*firstPoint(1); - - inequalityFactor = +1; - - % if any of the two points are between pi and 2pi, then the inequality is - % in the form of y >= m*x + q, and is needed to change the sign of it. - if (angle(i) > pi || angle(rem(i, numberOfEquations) + 1) > pi) - inequalityFactor = -1; - end - - % a wrench is 6 dimensional f = [fx, fy, fz, mux, muy, muz]' - % there are constraints on fy and fz, and the offset will be multiplied - % by mu * fx - % - A_ineq(i,:) = inequalityFactor.* [-angularCoefficients, 1, (-offsets*staticFrictionCoefficient), 0, 0, 0]; - end - - %% POSITIVITY OF VERTICAL FORCE, AND COP - % - % Positivity of vertical force: F_z > fZmin - % Center of pressure (CoP): dimMinFoot_y < torque_x/F_z < dimMaxFoot_y - % dimMinFoot_x < -torque_y/F_z < dimMaxFoot_x - % - % footSize = [ dimMinFoot_x, dimMaxFoot_x - % dimMinFoot_y, dimMaxFoot_y ] - % - % F_x F_y F_z torque_x torque_y torque_z - ConstraintsMatrix = [ 0, 0, -torsionalFrictionCoefficient, 0, 0, 1; % torque_z -torsionalFrictionCoefficient*F_z < 0 - 0, 0, -torsionalFrictionCoefficient, 0, 0, -1; % -torque_z -torsionalFrictionCoefficient*F_z < 0 - 0, 0, -1, 0, 0, 0; % -F_z < -fZmin - 0, 0, footSize(1,1), 0, 1, 0; % torque_y +dimMinFoot_x*F_z < 0 - 0, 0, -footSize(1,2), 0, -1, 0; % -torque_y -dimMaxFoot_x*F_z < 0 - 0, 0, footSize(2,1), -1, 0, 0; % -torque_x +dimMinFoot_y*F_z < 0 - 0, 0, -footSize(2,2), 1, 0, 0]; % torque_x -dimMaxFoot_y*F_z < 0 - - % add inequality constraints - ConstraintsMatrix = [A_ineq; - ConstraintsMatrix ]; - - bVectorConstraints = [zeros(size(A_ineq,1), 1); zeros(7,1)]; - - bVectorConstraints(3 + size(A_ineq,1)) = -fZmin; -end diff --git a/library/matlab-wbc/+wbc/contactsTransitionQP.m b/library/matlab-wbc/+wbc/contactsTransitionQP.m deleted file mode 100644 index d89dec5..0000000 --- a/library/matlab-wbc/+wbc/contactsTransitionQP.m +++ /dev/null @@ -1,38 +0,0 @@ -function onOneFoot = contactsTransitionQP(LR_FootInContact) - - % CONTACTSTRANSITIONQP detects a contact activation (from left foot to - % right foot balancing) and provides this information - % to the WBtoolbox QP block's utilities. - % - % FORMAT: onOneFoot = contactsTransitionQP(LR_FootInContact) - % - % INPUT: - LR_FootInContact = a vector describing the feet contact status; - % - % OUTPUT: - onOneFoot = true if the robot is balancing on one foot, false - % otherwise. - % - % 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; - - if sum(LR_FootInContact) > (2 - CONTACT_THRESHOLD) - % 2 Contacts - onOneFoot = false; - return; - elseif sum(LR_FootInContact) > (1 - CONTACT_THRESHOLD) - % 1 Contact - onOneFoot = true; - return; - else - % TODO - onOneFoot = false; - return; -end diff --git a/library/matlab-wbc/+wbc/correctIMUWithNeckPos.m b/library/matlab-wbc/+wbc/correctIMUWithNeckPos.m new file mode 100644 index 0000000..c9d2ddd --- /dev/null +++ b/library/matlab-wbc/+wbc/correctIMUWithNeckPos.m @@ -0,0 +1,62 @@ +function imu_H_imuAssumingNeckToZero = correctIMUWithNeckPos(neckJointsPositions) + + % CORRECTIMUWITHNECKPOS to be used with the iCub HEAD IMU. It corrects + % the IMU transformation matrix computed assuming + % the neck joints position equals to 0, and returns + % the imu_H_imuAssumingNeckToZero transform. + % + % FORMAT: imu_H_imuAssumingNeckToZero = correctIMUWithNeckPos(neckJointsPositions) + % + % INPUT: - neckJointsPositions = [3 * 1] vector of joints (neck_pitch, neck_roll, + % neck_yaw) expressed in radians + % + % OUTPUT: - imu_H_imuAssumingNeckToZero = [4 * 4] imu to corrected_imu transform + % + % 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 --- + + % Compute the imuAssumingNeckToZero_H_neckBase transform + H_34 = evalDHMatrix( 9.5*1e-3, 0, pi/2, +pi/2); + H_45 = evalDHMatrix( 0, 0, -pi/2, -pi/2); + H_56 = evalDHMatrix(18.5*1e-3, 110.8*1e-3, -pi/2, +pi/2); + H_61 = evalDHMatrix( 0, 6.6*1e-3, pi/2, 0); + + imuAssumingNeckToZero_H_neckBase = H_34*H_45*H_56*H_61; + + % Compute the imu_H_neckBase transform + H_34 = evalDHMatrix( 9.5*1e-3, 0, pi/2, neckJointsPositions(1) +pi/2); + H_45 = evalDHMatrix( 0, 0, -pi/2, neckJointsPositions(2) -pi/2); + H_56 = evalDHMatrix(18.5*1e-3, 110.8*1e-3, -pi/2, neckJointsPositions(3) +pi/2); + H_61 = evalDHMatrix( 0, 6.6*1e-3, pi/2, 0); + + imu_H_neckBase = H_34*H_45*H_56*H_61; + + imu_H_imuAssumingNeckToZero = imu_H_neckBase/(imuAssumingNeckToZero_H_neckBase); +end + +function H = evalDHMatrix(a, d, alpha, theta) + + % EVALDHMATRIX calculates the DH matrix from angles and parameters. + % + % FORMAT: H = evalDHMatrix(a, d, alpha, theta) + % + % INPUT: - a = dimension (m) + % - d = dimension (m) + % - alpha = angle (rad) + % - theta = angle (rad) + % + % OUTPUT: - H = [4 * 4] DH matrix + % + + H = [ cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), cos(theta)*a + sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), sin(theta)*a + 0, sin(alpha), cos(alpha), d + 0, 0, 0, 1]; +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/correctImuWithNeckPos.m b/library/matlab-wbc/+wbc/correctImuWithNeckPos.m deleted file mode 100644 index adb23ab..0000000 --- a/library/matlab-wbc/+wbc/correctImuWithNeckPos.m +++ /dev/null @@ -1,42 +0,0 @@ -function imu_H_imuAssumingNeckToZero = correctImuWithNeckPos(neckJoints) - - % CORRECTIMUWITHNECKPOS corrects the IMU transform computed assuming - % the neck joint positions equal to 0, and returns - % the imu_H_imuAssumingNeckToZero transform. - % - % FORMAT: imu_H_imuAssumingNeckToZero = correctImuWithNeckPos(neckJoints) - % - % INPUT: - neckJoints = [3 * 1] vector of joints (neck_pitch, neck_roll, - % neck_yaw) expressed in radians (while on the port - % they are published in degrees) - % - % OUTPUT: - imu_H_imuAssumingNeckToZero= [4 * 4] imu to corrected_imu transform - % - % 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 --- - - % Compute the imuAssumingNeckToZero_H_neckBase transform - H_34 = wbc.evalDHMatrix( 9.5*1e-3, 0, pi/2, +pi/2); - H_45 = wbc.evalDHMatrix( 0, 0, -pi/2, -pi/2); - H_56 = wbc.evalDHMatrix(18.5*1e-3, 110.8*1e-3, -pi/2, +pi/2); - H_61 = wbc.evalDHMatrix( 0, 6.6*1e-3, pi/2, 0); - - imuAssumingNeckToZero_H_neckBase = H_34*H_45*H_56*H_61; - - % Compute the imu_H_neckBase transform - H_34 = wbc.evalDHMatrix( 9.5*1e-3, 0, pi/2, neckJoints(1) +pi/2); - H_45 = wbc.evalDHMatrix( 0, 0, -pi/2, neckJoints(2) -pi/2); - H_56 = wbc.evalDHMatrix(18.5*1e-3, 110.8*1e-3, -pi/2, neckJoints(3) +pi/2); - H_61 = wbc.evalDHMatrix( 0, 6.6*1e-3, pi/2, 0); - - imu_H_neckBase = H_34*H_45*H_56*H_61; - - imu_H_imuAssumingNeckToZero = imu_H_neckBase/(imuAssumingNeckToZero_H_neckBase); -end diff --git a/library/matlab-wbc/+wbc/diagonalMatrixFromVector.m b/library/matlab-wbc/+wbc/diagonalMatrixFromVector.m new file mode 100644 index 0000000..5f9fd21 --- /dev/null +++ b/library/matlab-wbc/+wbc/diagonalMatrixFromVector.m @@ -0,0 +1,22 @@ +function D = diagonalMatrixFromVector(d) + + % DIAGONALMATRIXFROMVECTOR coverts a vector into a diagonal matrix. + % + % FORMAT: D = diagonalMatrixFromVector(d) + % + % INPUT: - d = [n * 1] vector; + % + % OUTPUT: - D = [n * n] diagonal matrix. + % + % 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 --- + + D = diag(d); +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/evalDHMatrix.m b/library/matlab-wbc/+wbc/evalDHMatrix.m deleted file mode 100644 index ec6e993..0000000 --- a/library/matlab-wbc/+wbc/evalDHMatrix.m +++ /dev/null @@ -1,28 +0,0 @@ -function H = evalDHMatrix(a, d, alpha, theta) - - % EVALDHMATRIX calculates the DH matrix from angles and parameters. - % - % FORMAT: H = evalDHMatrix(a, d, alpha, theta) - % - % INPUT: - a = dimension (m) - % - d = dimension (m) - % - alpha = angle (rad) - % - theta = angle (rad) - % - % OUTPUT: - H = [4 * 4] DH matrix - % - % 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 --- - - H = [ cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), cos(theta)*a - sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), sin(theta)*a - 0, sin(alpha), cos(alpha), d - 0, 0, 0, 1]; -end diff --git a/library/matlab-wbc/+wbc/fromPosQuatToTransfMatr.m b/library/matlab-wbc/+wbc/fromPosQuatToTransfMatr.m index 4a87e5d..f228f84 100644 --- a/library/matlab-wbc/+wbc/fromPosQuatToTransfMatr.m +++ b/library/matlab-wbc/+wbc/fromPosQuatToTransfMatr.m @@ -1,8 +1,8 @@ -function H = fromPosQuatToTransfMatr(q) +function H = fromPosQuatToTransfMatr(q) % FROMPOSQUATTOTRANSFMATRIX takes as input a pose (position + orientation) % with orientation expressed in quaternions, and - % outputs the same pose in terms of trnsformation + % outputs the same pose in terms of transformation % matrix. % % FORMAT: H = fromPosQuatToTransfMatr(q) @@ -30,4 +30,4 @@ % compose the transformation matrix H = [R, x; [0 0 0 1]]; -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/fromPosRpyToTransfMatrix.m b/library/matlab-wbc/+wbc/fromPosRpyToTransfMatrix.m index 0518b1c..24bf6a1 100644 --- a/library/matlab-wbc/+wbc/fromPosRpyToTransfMatrix.m +++ b/library/matlab-wbc/+wbc/fromPosRpyToTransfMatrix.m @@ -7,9 +7,9 @@ % % FORMAT: H = fromPosRpyToTransfMatrix(pos_rpy) % - % INPUT: - pos_rpy = [6 * 1] pose (position + roll-pitch-yaw) + % INPUT: - pos_rpy = [6 * 1] pose (position + roll-pitch-yaw) % - % OUTPUT: - H = [4 * 4] pose (transformation matrix) + % OUTPUT: - H = [4 * 4] pose (transformation matrix) % % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava % @@ -31,4 +31,4 @@ % compose the transformation matrix H = [R, pos; [0 0 0 1]]; -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/linearPID.m b/library/matlab-wbc/+wbc/linearPID.m index 7968c89..999df08 100644 --- a/library/matlab-wbc/+wbc/linearPID.m +++ b/library/matlab-wbc/+wbc/linearPID.m @@ -27,4 +27,4 @@ % classical PID controller acc_star = acc_des -Kp*(pos-pos_des) -Kd*(vel-vel_des); -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/pinvDamped.m b/library/matlab-wbc/+wbc/pinvDamped.m index 84263c1..90ebd85 100644 --- a/library/matlab-wbc/+wbc/pinvDamped.m +++ b/library/matlab-wbc/+wbc/pinvDamped.m @@ -4,7 +4,7 @@ % % FORMAT: pinvDampA = pinvDamped(A,regDamp) % - % INPUT: - A = [n * m] rotation matrix + % INPUT: - A = [n * m] rotation matrix % - regDamp = regularization parameter % % OUTPUT: - pinvDampA = [m * n] matrix pseudoinverse of A @@ -20,4 +20,4 @@ %% --- Initialization --- pinvDampA = transpose(A)/(A*transpose(A) +regDamp*eye(size(A,1))); -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/posRotToTransfMatrix.m b/library/matlab-wbc/+wbc/posRotToTransfMatrix.m index b5cd1d5..717fa9b 100644 --- a/library/matlab-wbc/+wbc/posRotToTransfMatrix.m +++ b/library/matlab-wbc/+wbc/posRotToTransfMatrix.m @@ -1,4 +1,4 @@ -function H = posRotToTransfMatrix(pos,R) +function H = posRotToTransfMatrix(pos,R) % POSROTTOTRANSFMATR computes the transformation matrix given as input a % position vector and a rotation matrix @@ -9,9 +9,9 @@ % FORMAT: H = posRotToTransfMatrix(pos, R) % % INPUT: - pos = [3 * 1] position vector - % - R = [3 * 3] rotation matrix + % - R = [3 * 3] rotation matrix % - % OUTPUT: - H = [4 * 4] transformation matrix + % OUTPUT: - H = [4 * 4] transformation matrix % % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava % @@ -26,4 +26,4 @@ H = eye(4); H(1:3,1:3) = R; H(1:3, 4) = pos; -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/processOneFootOutputQP.m b/library/matlab-wbc/+wbc/processOneFootOutputQP.m deleted file mode 100644 index bbe7990..0000000 --- a/library/matlab-wbc/+wbc/processOneFootOutputQP.m +++ /dev/null @@ -1,33 +0,0 @@ -function f0_oneFoot = processOneFootOutputQP(primalSolution,LR_FootInContact) - - % PROCESSONEFOOTOUTPUTQP evaluates the output of the WBToolbox QP block, and - % associates this output with the left foot contact - % forces or with the right foot contact forces (or - % both) according to the contacts status: if the - % contact is not active, a vector of zeros is - % assigned as contact forces and moments instead of - % the solution provided by the QP. - % - % FORMAT: f0_oneFoot = processOneFootOutputQP(primalSolution,LR_FootInContact) - % - % INPUT: - primalSolution = the solution to the QP problem provided by the - % WBToolbox QP block; - % - LR_FootInContact = a vector describing the feet contact status; - % - % OUTPUT: - f0_oneFoot = 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 --- - - r_inContact = LR_FootInContact(1); - l_inContact = LR_FootInContact(2); - f0_oneFoot = [primalSolution * r_inContact ; primalSolution * l_inContact]*abs(r_inContact - l_inContact); -end diff --git a/library/matlab-wbc/+wbc/processOutputQP.m b/library/matlab-wbc/+wbc/processOutputQP.m deleted file mode 100644 index 5fdbd2a..0000000 --- a/library/matlab-wbc/+wbc/processOutputQP.m +++ /dev/null @@ -1,39 +0,0 @@ -function f0 = processOutputQP(analyticalSolution,primalSolution,QPStatus,Config) - - % PROCESSOUTPUTQP 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: f0 = processOutputQP(analyticalSolution,primalSolution,QPStatus,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. - % - Config = a structure containing the robot-related configuration - % parameters; - % - % OUTPUT: - f0 = 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 --- - - tollQPStatus = 0.01; - - if Config.USE_QP_SOLVER && abs(QPStatus)< tollQPStatus - - f0 = primalSolution; - else - f0 = analyticalSolution; - end -end - diff --git a/library/matlab-wbc/+wbc/quaternionDerivative.m b/library/matlab-wbc/+wbc/quaternionDerivative.m index 1022812..e3756fe 100644 --- a/library/matlab-wbc/+wbc/quaternionDerivative.m +++ b/library/matlab-wbc/+wbc/quaternionDerivative.m @@ -1,12 +1,12 @@ -function qtDot = quaternionDerivative(qt, omega, k) +function qtDot = quaternionDerivative(qt,omega,k) % QUATERNIONDERIVATIVE computes the time derivative of a quaternion. % - % FORMAT: qtDot = quaternionDerivative(qt, omega, k) + % FORMAT: qtDot = quaternionDerivative(qt,omega,k) % - % INPUT: - qt = [4 * 1] quaternion + % INPUT: - qt = [4 * 1] quaternion % - omega = [3 * 1] angular velocity - % - k = gain for regularization terms + % - k = gain for regularization terms % % OUTPUT: - qtDot = [4 * 1] quaternion derivative % @@ -21,6 +21,6 @@ %% --- Initialization --- % TO BE VERIFIED: the expected angular velocity is in BODY frame, i.e. b_omega_b - qtDot = 0.5 *[0 -transpose(omega); ... - omega -wbc.skew(omega)]*qt +k*(1 -transpose(qt)*qt)*qt; -end + qtDot = 0.5 *[0, -transpose(omega); ... + omega, -wbc.skew(omega)]*qt +k*(1 -transpose(qt)*qt)*qt; +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/referenceGeneratorCoM.m b/library/matlab-wbc/+wbc/referenceGeneratorCoM.m deleted file mode 100644 index 1ec904f..0000000 --- a/library/matlab-wbc/+wbc/referenceGeneratorCoM.m +++ /dev/null @@ -1,48 +0,0 @@ -function references_CoM = referenceGeneratorCoM(pos_CoM_0, t, Config) - - % REFERENCEGENERATORCOM computes a CoM reference trajectory. Default trajectory is - % a sine function. - % - % USAGE: please note that this function has been designed for being inserted - % in a Simulink model. - % - % FORMAT: references_CoM = referenceGeneratorCoM(pos_CoM_0, t, Config) - % - % INPUT: - pos_CoM_0 = [3 * 1] initial CoM position - % - t = simulation time - % - Config = user defined configuration - % - % - % OUTPUT: - references_CoM = [9 * 1] desired CoM position, velocity and acceleration - % - % 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 --- - - pos_CoM_des = pos_CoM_0; - vel_CoM_des = zeros(3,1); - acc_CoM_des = zeros(3,1); - - if Config.amplitudeOfOscillation ~= 0 - - if t > Config.noOscillationTime - - Amplitude = Config.amplitudeOfOscillation; - else - Amplitude = 0; - end - - frequency = Config.frequencyOfOscillation; - pos_CoM_des = pos_CoM_0 +Amplitude*sin(2*pi*frequency*t)*Config.directionOfOscillation; - vel_CoM_des = Amplitude*2*pi*frequency*cos(2*pi*frequency*t)*Config.directionOfOscillation; - acc_CoM_des = -Amplitude*(2*pi*frequency)^2*sin(2*pi*frequency*t)*Config.directionOfOscillation; - end - - references_CoM = [pos_CoM_des; vel_CoM_des; acc_CoM_des]; -end diff --git a/library/matlab-wbc/+wbc/robotIsOnSingleSupportQP.m b/library/matlab-wbc/+wbc/robotIsOnSingleSupportQP.m new file mode 100644 index 0000000..c904588 --- /dev/null +++ b/library/matlab-wbc/+wbc/robotIsOnSingleSupportQP.m @@ -0,0 +1,42 @@ +function onOneFoot = robotIsOnSingleSupportQP(feetContactStatus) + + % ROBOTISONSINGLESUPPORTQP detects if the robot is balancing on single + % or double support. + % + % FORMAT: onOneFoot = robotIsOnSingleSupportQP(feetContactStatus) + % + % INPUT: - feetContactStatus = [2 x 1] a vector of booleans describing + % the feet contact status. + % Format: [leftFoot; rightFoot]; + % + % OUTPUT: - onOneFoot = true if the robot is balancing on one foot, false + % otherwise. + % + % 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; + + if sum(feetContactStatus) > (2 - CONTACT_THRESHOLD) + + % two feet balancing + onOneFoot = false; + return; + + elseif sum(feetContactStatus) > (1 - CONTACT_THRESHOLD) + + % one foot balancing + onOneFoot = true; + return; + else + onOneFoot = false; + return; + end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/rotationDerivativeFromAngVel.m b/library/matlab-wbc/+wbc/rotationDerivativeFromAngVel.m index 44f1881..2ee723d 100644 --- a/library/matlab-wbc/+wbc/rotationDerivativeFromAngVel.m +++ b/library/matlab-wbc/+wbc/rotationDerivativeFromAngVel.m @@ -3,10 +3,10 @@ % ROTATIONDERIVATIVEFROMANGVEL computes the derivative of a rotation matrix % given the angular velocity. It makes use of % the following convention: if the rotation of - % a body b is expressed in the world frame w + % a body b is expressed in the WORLD FRAME w % (i.e., R = w_R_b), then this function is % expecting the angular velocity of the body - % to be expressed in the world frame, i.e. + % to be expressed in the WORLD FRAME, i.e. % omega = w_omega. % % USAGE: please note that this function has been designed for being inserted @@ -34,4 +34,4 @@ % the inertial frame, i.e. w_omega. kCorr = 1; RDot = wbc.skew(omega)*R +kCorr*(eye(3)-R*transpose(R))*R; -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/rotationFromQuaternion.m b/library/matlab-wbc/+wbc/rotationFromQuaternion.m index 32af795..8010fce 100644 --- a/library/matlab-wbc/+wbc/rotationFromQuaternion.m +++ b/library/matlab-wbc/+wbc/rotationFromQuaternion.m @@ -3,7 +3,7 @@ % ROTATIONFROMQUATERNION computes the rotation matrix from a quaternion by % applying Rodrigues's formula: % - % R = I_3 + 2*s*skew(r) + 2*skew(r)^2 + % R = I_3 + 2 * s * skew(r) + 2 * skew(r)^2 % % FORMAT: R = rotationFromQuaternion(q) % @@ -23,4 +23,4 @@ % rotation matrix R = eye(3) +2*q(1)*wbc.skew(q(2:4)) +2*wbc.skew(q(2:4))^2; -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/rotationFromRollPitchYaw.m b/library/matlab-wbc/+wbc/rotationFromRollPitchYaw.m index 2dfaab8..64bad44 100644 --- a/library/matlab-wbc/+wbc/rotationFromRollPitchYaw.m +++ b/library/matlab-wbc/+wbc/rotationFromRollPitchYaw.m @@ -5,9 +5,9 @@ % % FORMAT: R = rotationFromRollPitchYaw(rpy) % - % INPUTS: - rpy = [3 x 1] roll-pitch-yaw vector; + % INPUTS: - rpy = [3 * 1] roll-pitch-yaw vector; % - % OUTPUTS: - R = [3 x 3] rotation matrix. + % OUTPUTS: - R = [3 * 3] rotation matrix. % % Author : Gabriele Nava (gabriele.nava@iit.it) % Genova, Dec 2018 @@ -19,4 +19,4 @@ % http://wiki.icub.org/codyco/dox/html/idyntree/html/classiDynTree_1_1Rotation.html#a600352007d9250f7f227f21db85611f2 % R = wbc.rotz(rpy(3))*wbc.roty(rpy(2))*wbc.rotx(rpy(1)); -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/rotationalPID_acceleration.m b/library/matlab-wbc/+wbc/rotationalPID_acceleration.m index e3e6bb7..3bcdaf0 100644 --- a/library/matlab-wbc/+wbc/rotationalPID_acceleration.m +++ b/library/matlab-wbc/+wbc/rotationalPID_acceleration.m @@ -44,7 +44,7 @@ %% --- Initialization --- - % Modified gains to use the control with the real robot + % WARNING: modified gains to use the control on the real robot c0 = 0.001; c1 = Kd; c2 = Kp; @@ -56,4 +56,4 @@ w_omegaDot_star = omegaE_Dot -c1*(w_omega-w_omega_des) -c2*skv; -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/rotationalPID_velocity.m b/library/matlab-wbc/+wbc/rotationalPID_velocity.m index a8df398..63899d0 100644 --- a/library/matlab-wbc/+wbc/rotationalPID_velocity.m +++ b/library/matlab-wbc/+wbc/rotationalPID_velocity.m @@ -26,4 +26,4 @@ skv = wbc.skewVee(w_R_b*transpose(w_R_b_des)); w_omega = w_omega_des -Kp*skv; -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/rotx.m b/library/matlab-wbc/+wbc/rotx.m index 1fa3d97..61b8e7c 100644 --- a/library/matlab-wbc/+wbc/rotx.m +++ b/library/matlab-wbc/+wbc/rotx.m @@ -25,4 +25,4 @@ R(3,2) = sin(alpha); R(3,3) = cos(alpha); -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/roty.m b/library/matlab-wbc/+wbc/roty.m index ea9cf4e..3a3b6e7 100644 --- a/library/matlab-wbc/+wbc/roty.m +++ b/library/matlab-wbc/+wbc/roty.m @@ -25,4 +25,4 @@ R(3,1) = -sin(alpha); R(3,3) = cos(alpha); -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/rotz.m b/library/matlab-wbc/+wbc/rotz.m index 79e28df..ef5d4d2 100644 --- a/library/matlab-wbc/+wbc/rotz.m +++ b/library/matlab-wbc/+wbc/rotz.m @@ -25,4 +25,4 @@ R(2,1) = sin(alpha); R(2,2) = cos(alpha); -end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/saturateInputDerivative.m b/library/matlab-wbc/+wbc/saturateInputDerivative.m new file mode 100644 index 0000000..baa5de3 --- /dev/null +++ b/library/matlab-wbc/+wbc/saturateInputDerivative.m @@ -0,0 +1,73 @@ +function uSat = saturateInputDerivative(u, u_0, tStep, uDeltaMax) + + % SATURATEINPUTDERIVATIVE saturates the input u such that the absolute + % value of its numerical derivative: + % + % uDelta = (uPrev-u)/tStep + % + % cannot be greater than a predefined value. + % + % FORMAT: uSat = saturateInputDerivative(u, u_0, Config, Sat) + % + % INPUTS: u = input signal; + % u_0 = input at t = 0; + % tStep = time step for finite difference formula; + % uDeltaMax = max input derivative (absolute value). + % + % OUTPUTS: uSat = saturated input signal. + % + % 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 --- + persistent uPrev; + + % initialize the input value at the previous step + if isempty(uPrev) + + uPrev = u_0; + end + + % evaluate the max and min allowed input + delta_u_max = uDeltaMax * tStep; + delta_u_min = -uDeltaMax * tStep; + delta_u_Sat = saturateInput(u-uPrev, delta_u_min, delta_u_max); + + % update u at previous time + uSat = uPrev + delta_u_Sat; + uPrev = uSat; +end + +% utility function: saturates the input value +function y = saturateInput(u, min, max) + + assert(isequal(size(min), size(max)), 'Min and max must be same size') + + if length(min) == 1 + + y = u; + y(y > max) = max; + y(y < min) = min; + else + + assert(length(min) == length(u), 'input and saturation must have same size'); + y = u; + + for i = 1:length(min) + + if y(i) > max(i) + + y(i) = max(i); + + elseif y(i) < min(i) + + y(i) = min(i); + end + end + end +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/skew.m b/library/matlab-wbc/+wbc/skew.m index b246aa2..83f5882 100644 --- a/library/matlab-wbc/+wbc/skew.m +++ b/library/matlab-wbc/+wbc/skew.m @@ -22,5 +22,4 @@ X = [ 0 -x(3) x(2); x(3) 0 -x(1); -x(2) x(1) 0 ]; -end - +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/skewVee.m b/library/matlab-wbc/+wbc/skewVee.m index e651d2d..ea61c35 100644 --- a/library/matlab-wbc/+wbc/skewVee.m +++ b/library/matlab-wbc/+wbc/skewVee.m @@ -22,11 +22,10 @@ %% --- Initialization --- % skew symmetric part of X - X_skew = 0.5*(X -transpose(X)); + X_skew = 0.5*(X - transpose(X)); % vector composing the matrix x = [-X_skew(2,3) X_skew(1,3) -X_skew(1,2)]; -end - +end \ No newline at end of file diff --git a/library/matlab-wbc/+wbc/worldToBaseTransformWithIMU.m b/library/matlab-wbc/+wbc/worldToBaseTransformWithIMU.m index a098d7d..f660e20 100644 --- a/library/matlab-wbc/+wbc/worldToBaseTransformWithIMU.m +++ b/library/matlab-wbc/+wbc/worldToBaseTransformWithIMU.m @@ -1,19 +1,23 @@ -function w_H_b = worldToBaseTransformWithIMU(imu_H_link,imu_H_link_0,link_H_base,inertial_0,inertial,neck_pos,Config) +function [w_H_b, wImu_H_base, wImu_H_fixedLink_0] = worldToBaseTransformWithIMU(imu_H_fixedLink, imu_H_fixedLink_0, fixedLink_H_base, rpyFromIMU_0, rpyFromIMU, FILTER_IMU_YAW) - % WORLDTOBASETRANSFORMWITHIMU calculates the world-to-base transformation - % matrix using IMU orientation. + % WORLDTOBASETRANSFORMWITHIMU calculates the world-to-base frame transformation + % matrix. The orientation is updated by + % using informations from IMU. % - % FORMAT: w_H_b = worldToBaseTransformWithIMU(imu_H_link,imu_H_link_0,link_H_base,inertial_0,inertial,neck_pos,Config) + % FORMAT: [w_H_b, wImu_H_base, wImu_H_fixedLink_0] = worldToBaseTransformWithIMU(imu_H_fixedLink, imu_H_fixedLink_0, fixedLink_H_base, rpyFromIMU_0, rpyFormIMU, FILTER_IMU_YAW) % - % INPUT: - imu_H_link = [4 * 4] imu to fixed link transform - % - imu_H_link_0 = [4 * 4] imu to fixed link transform at 0 - % - link_H_base = [4 * 4] fixed link to base transform - % - inertial_0 = IMU orientation, velocity, acceleration at 0 - % - inertial = IMU orientation, velocity, acceleration - % - neck_pos = [3 * 1] neck position - % - Config = user defined parameters + % INPUT: - imu_H_fixedLink = [4 * 4] imu to fixed link transform + % - imu_H_fixedLink_0 = [4 * 4] imu to fixed link transform at t = 0 + % - fixedLink_H_base = [4 * 4] fixed link to base transform + % - rpyFromIMU_0 = IMU orientation at t = 0 + % - rpyFromIMU = IMU orientation + % - FILTER_IMU_YAW = boolean % - % OUTPUT: - w_H_b = [4 * 4] world to base transform + % OUTPUT: - w_H_b = [4 * 4] world to base frame transformation matrix + % - wImu_H_base = [4 * 4] IMU inertial frame to base frame + % transformation matrix + % - wImu_H_fixedLink_0 = [4 * 4] IMU inertial frame to fixed frame + % transformation matrix at t = 0. % % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava % @@ -25,52 +29,47 @@ %% --- Initialization --- - % Converting the inertial values from grad into rad - inertial = (inertial * pi)/180; - inertial_0 = (inertial_0 * pi)/180; + % WARNING!!! Converting the inertial values from grad into rad + rpyFromIMU = (rpyFromIMU * pi)/180; + rpyFromIMU_0 = (rpyFromIMU_0 * pi)/180; % Composing the rotation matrix: % See http://wiki.icub.org/images/8/82/XsensMtx.pdf page 12 - wImu_R_imu = wbc.rotz(inertial(3))*wbc.roty(inertial(2))*wbc.rotx(inertial(1)); - wImu_R_imu_0 = wbc.rotz(inertial_0(3))*wbc.roty(inertial_0(2))*wbc.rotx(inertial_0(1)); + wImu_R_imu = wbc.rotz(rpyFromIMU(3))*wbc.roty(rpyFromIMU(2))*wbc.rotx(rpyFromIMU(1)); + wImu_R_imu_0 = wbc.rotz(rpyFromIMU_0(3))*wbc.roty(rpyFromIMU_0(2))*wbc.rotx(rpyFromIMU_0(1)); % Rotation between the IMU and the fixed link - imu_R_link = imu_H_link(1:3,1:3); - imu_R_link_0 = imu_H_link_0(1:3,1:3); + imu_R_fixedLink = imu_H_fixedLink(1:3,1:3); + imu_R_fixedLink_0 = imu_H_fixedLink_0(1:3,1:3); % Rotation between the IMU inertial frame and the fixed link - wImu_R_link = wImu_R_imu * imu_R_link; - wImu_R_link_0 = wImu_R_imu_0 * imu_R_link_0; + wImu_R_fixedLink = wImu_R_imu * imu_R_fixedLink; + wImu_R_fixedLink_0 = wImu_R_imu_0 * imu_R_fixedLink_0; % Convert into roll-pitch-yaw - rollPitchYaw_link_0 = wbc.rollPitchYawFromRotation(wImu_R_link_0); - rollPitchYaw_link = wbc.rollPitchYawFromRotation(wImu_R_link); + rollPitchYaw_fixedLink_0 = wbc.rollPitchYawFromRotation(wImu_R_fixedLink_0); + rollPitchYaw_fixedLink = wbc.rollPitchYawFromRotation(wImu_R_fixedLink); - rollPitchYawFiltered_link = rollPitchYaw_link; + % Filter the Yaw angle (may be measured wrong by the IMU) + rollPitchYaw_filtered = rollPitchYaw_fixedLink; - if Config.FILTER_IMU_YAW - rollPitchYawFiltered_link(3) = rollPitchYaw_link_0(3); - end - if Config.FILTER_IMU_PITCH - rollPitchYawFiltered_link(2) = rollPitchYaw_link_0(2); + if FILTER_IMU_YAW + + rollPitchYaw_filtered(3) = rollPitchYaw_fixedLink_0(3); end - wImu_R_link = wbc.rotz(rollPitchYawFiltered_link(3))*wbc.roty(rollPitchYawFiltered_link(2))*wbc.rotx(rollPitchYawFiltered_link(1)); + wImu_R_fixedLink = wbc.rotz(rollPitchYaw_filtered(3))*wbc.roty(rollPitchYaw_filtered(2))*wbc.rotx(rollPitchYaw_filtered(1)); % IMU inertial frame to fixed link transform - wImu_H_link = [wImu_R_link, zeros(3,1) - zeros(1,3), 1 ]; + wImu_H_fixedLink = [wImu_R_fixedLink, zeros(3,1) + zeros(1,3), 1 ]; - wImu_H_link_0 = [wImu_R_link_0, zeros(3,1) - zeros(1,3), 1 ]; + wImu_H_fixedLink_0 = [wImu_R_fixedLink_0, zeros(3,1) + zeros(1,3), 1 ]; % IMU inertial frame to base link transform - wImu_H_base = wImu_H_link * link_H_base; - - %% Correct IMU with neck position - wImu_H_wImuAssumingNeckToZero = wbc.correctImuWithNeckPos(neck_pos); + wImu_H_base = wImu_H_fixedLink * fixedLink_H_base; - wImu_H_base = wImu_H_wImuAssumingNeckToZero * wImu_H_base; - w_H_b = wImu_H_link_0\wImu_H_base; - -end + %% World to base frame transformation matrix + w_H_b = wImu_H_fixedLink_0\wImu_H_base; +end \ No newline at end of file diff --git a/library/simulink-library/CMM_iCub_23_25_DoFs.mdl b/library/simulink-library/CMM_iCub_23_25_DoFs.mdl new file mode 100644 index 0000000..de9bdde --- /dev/null +++ b/library/simulink-library/CMM_iCub_23_25_DoFs.mdl @@ -0,0 +1,7280 @@ +Model { + Name "CMM_iCub_23_25_DoFs" + Version 9.0 + SavedCharacterEncoding "UTF-8" + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.19" + NumModelReferences 0 + NumTestPointedSignals 0 + NumProvidedFunctions 0 + NumRequiredFunctions 0 + NumResetEvents 0 + HasInitializeEvent 0 + HasTerminateEvent 0 + IsExportFunctionModel 0 + NumParameterArguments 0 + NumExternalFileReferences 31 + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum21" + SID "75" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum22" + SID "76" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum23" + SID "77" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum24" + SID "78" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum25" + SID "79" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_base/CentroidalMomentum31" + SID "80" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Centro" + "idalMomentum1" + SID "293" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Centro" + "idalMomentum16" + SID "294" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Centro" + "idalMomentum2" + SID "295" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Centro" + "idalMomentum3" + SID "296" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/Centro" + "idalMomentum4" + SID "297" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Centro" + "idalMomentum1" + SID "431" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Centro" + "idalMomentum16" + SID "432" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Centro" + "idalMomentum2" + SID "433" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Centro" + "idalMomentum3" + SID "434" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/Centro" + "idalMomentum4" + SID "435" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Centro" + "idalMomentum1" + SID "446" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Centro" + "idalMomentum16" + SID "447" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Centro" + "idalMomentum2" + SID "448" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Centro" + "idalMomentum3" + SID "449" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/Centro" + "idalMomentum4" + SID "450" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Centro" + "idalMomentum1" + SID "461" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Centro" + "idalMomentum16" + SID "462" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Centro" + "idalMomentum2" + SID "463" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Centro" + "idalMomentum3" + SID "464" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/Centro" + "idalMomentum4" + SID "465" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5 (SPECI" + "AL)/CentroidalMomentum1" + SID "476" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5 (SPECI" + "AL)/CentroidalMomentum16" + SID "477" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5 (SPECI" + "AL)/CentroidalMomentum2" + SID "478" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5 (SPECI" + "AL)/CentroidalMomentum3" + SID "479" + Type "LIBRARY_BLOCK" + } + ExternalFileReference { + Reference "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + Path "CMM_iCub_23_25_DoFs/CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5 (SPECI" + "AL)/CentroidalMomentum4" + SID "480" + 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" + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + FPTRunName "Run 1" + MaxMDLFileLineLength 120 + LastSavedArchitecture "glnxa64" + Object { + $PropName "BdWindowsInfo" + $ObjectID 1 + $ClassName "Simulink.BDWindowsInfo" + Object { + $PropName "WindowsInfo" + $ObjectID 2 + $ClassName "Simulink.WindowInfo" + IsActive [1] + Location [134.0, 55.0, 3706.0, 2105.0] + Object { + $PropName "ModelBrowserInfo" + $ObjectID 3 + $ClassName "Simulink.ModelBrowserInfo" + Visible [0] + DockPosition "Left" + Width [50] + Height [50] + Filter [9] + } + Object { + $PropName "ExplorerBarInfo" + $ObjectID 4 + $ClassName "Simulink.ExplorerBarInfo" + Visible [1] + } + Object { + $PropName "EditorsInfo" + $ObjectID 5 + $ClassName "Simulink.EditorInfo" + IsActive [1] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [3668.0, 1809.0] + ZoomFactor [10.0] + Offset [1396.6, 632.55] + } + Object { + $PropName "DockComponentsInfo" + $ObjectID 6 + $ClassName "Simulink.DockComponentInfo" + Type "GLUE2:PropertyInspector" + ID "Property Inspector" + Visible [0] + CreateCallback "" + UserData "" + Floating [0] + DockPosition "Right" + Width [640] + Height [480] + } + WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAC6AP///wAAAAEAAAAAAAAAAPwCA" + "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" + "QBjAHQAbwByAAAAAAD/////AAAAOgD///8AAA56AAAHbQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" + "////wEAAACe/////wAAAAAAAAAA/////wEAAAEM/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGq/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAASj/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" + } + } + HideAutomaticNames on + Created "Thu Feb 28 16:10:03 2019" + Creator "gnava" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "gnava" + ModifiedDateFormat "%" + LastModifiedDate "Fri Mar 08 16:25:42 2019" + RTWModifiedTimeStamp 473963142 + ModelVersionFormat "1.%" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "disabled" + WideLines off + ShowLineDimensions off + 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 off + VariantCondition off + ExecutionContextIcon off + ShowLinearizationAnnotations on + ShowVisualizeInsertedRTB on + ShowMarkup on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + BlockVariantConditionDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + FunctionConnectors off + BrowserLookUnderMasks off + SimulationMode "normal" + VisualizeLoggedSignalsWhenLoggingToFile off + 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 7 + $ClassName "Simulink.SimulationData.ModelLoggingInfo" + model_ "CMM_iCub_23_25_DoFs" + overrideMode_ [0.0] + Array { + Type "Cell" + Dimension 1 + Cell "CMM_iCub_23_25_DoFs" + 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 8 + Version "1.17.1" + DisabledProps [] + Description "" + Array { + Type "Handle" + Dimension 9 + Simulink.SolverCC { + $ObjectID 9 + Version "1.17.1" + DisabledProps [] + Description "" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + EnableMultiTasking off + ConcurrentTasks off + Solver "VariableStepAuto" + SolverName "VariableStepAuto" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverInfoToggleStatus off + IsAutoAppliedInSIP off + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + SampleTimeProperty [] + DecoupledContinuousIntegration off + } + Simulink.DataIOCC { + $ObjectID 10 + Version "1.17.1" + DisabledProps [] + Description "" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Dataset" + 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 11 + Version "1.17.1" + Array { + Type "Cell" + Dimension 8 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + Cell "UseSpecifiedMinMax" + PropName "DisabledProps" + } + Description "" + 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 128 + LocalBlockOutputs on + RollThreshold 5 + StateBitsets off + DataBitsets off + ActiveStateOutputEnumStorageType "Native Integer" + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + LifeSpan "auto" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "off" + AccelVerboseBuild off + OptimizeBlockOrder "off" + OptimizeDataStoreBuffers on + BusAssignmentInplaceUpdate on + DifferentSizesBufferReuse off + } + Simulink.DebuggingCC { + $ObjectID 12 + Version "1.17.1" + Array { + Type "Cell" + Dimension 1 + Cell "UseOnlyExistingSharedCode" + PropName "DisabledProps" + } + Description "" + 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 "Simplified" + MergeDetectMultiDrivingBlocksExec "error" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "none" + 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 "error" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + UseOnlyExistingSharedCode "error" + SFcnCompatibilityMsg "none" + FrameProcessingCompatibilityMsg "error" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + AllowSymbolicDim on + RowMajorDimensionSupport off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "none" + 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 "error" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "error" + SFTransitionOutsideNaturalParentDiag "warning" + SFUnreachableExecutionPathDiag "warning" + SFUndirectedBroadcastEventsDiag "warning" + SFTransitionActionBeforeConditionDiag "warning" + SFOutputUsedAsStateInMooreChartDiag "error" + SFTemporalDelaySmallerThanSampleTimeDiag "warning" + SFSelfTransitionDiag "warning" + SFExecutionAtInitializationDiag "warning" + SFMachineParentedDataDiag "warning" + IntegerSaturationMsg "warning" + AllowedUnitSystems "all" + UnitsInconsistencyMsg "warning" + AllowAutomaticUnitConversions on + RCSCRenamedMsg "warning" + RCSCObservableMsg "warning" + ForceCombineOutputUpdateInSim off + UnitDatabase "" + } + Simulink.HardwareCC { + $ObjectID 13 + Version "1.17.1" + DisabledProps [] + Description "" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerLongLong 64 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 64 + ProdBitPerSizeT 64 + ProdBitPerPtrDiffT 64 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "Float" + ProdIntDivRoundTo "Zero" + ProdEndianess "LittleEndian" + ProdWordSize 64 + ProdShiftRightIntArith on + ProdLongLongMode off + ProdHWDeviceType "Intel->x86-64 (Windows64)" + 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 14 + Version "1.17.1" + DisabledProps [] + Description "" + 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 on + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 15 + Version "1.17.1" + DisabledProps [] + Description "" + SimCustomSourceCode "" + SimCustomHeaderCode "" + SimCustomInitializer "" + SimCustomTerminator "" + SimReservedNameArray [] + SimUserSources "" + SimUserIncludeDirs "" + SimUserLibraries "" + SimUserDefines "" + SimCustomCompilerFlags "" + SimCustomLinkerFlags "" + SFSimEcho on + SimCtrlC on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + SimGenImportedTypeDefs off + ModelFunctionsGlobalVisibility "on" + CompileTimeRecursionLimit 50 + EnableRuntimeRecursion on + MATLABDynamicMemAlloc on + MATLABDynamicMemAllocThreshold 65536 + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 16 + 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 "GenerateMissedCodeReplacementReport" + Cell "GenerateErtSFunction" + Cell "CreateSILPILBlock" + Cell "CodeExecutionProfiling" + Cell "CodeProfilingSaveOptions" + Cell "CodeProfilingInstrumentation" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + HardwareBoard "None" + TLCOptions "" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + PackageGeneratedCodeAndArtifacts off + PackageName "" + TemplateMakefile "grt_default_tmf" + PostCodeGenCommand "" + Description "" + GenerateReport off + SaveLog 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 17 + Version "1.17.1" + Array { + Type "Cell" + Dimension 28 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "BlockCommentType" + 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 "CustomSymbolStrModelFcn" + Cell "CustomSymbolStrFcnArg" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + Cell "CustomSymbolStrUtil" + Cell "CustomSymbolStrEmxType" + Cell "CustomSymbolStrEmxFcn" + Cell "CustomUserTokenString" + Cell "ReqsInCode" + PropName "DisabledProps" + } + Description "" + 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 off + MATLABSourceComments off + EnableCustomComments off + InternalIdentifierFile "" + InternalIdentifier "Shortened" + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + ReservedNameArray [] + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 18 + Version "1.17.1" + Array { + Type "Cell" + Dimension 15 + Cell "IncludeMdlTerminateFcn" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "ExistingSharedCode" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "GenerateAllocFcn" + Cell "PurelyIntegerCode" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + Cell "RemoveDisableFunc" + Cell "RemoveResetFunc" + PropName "DisabledProps" + } + Description "" + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + GenFloatMathFcnCalls "NOT IN USE" + TargetLangStandard "C99 (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 "Use local settings" + CombineOutputUpdateFcns on + CombineSignalStateStructs off + SuppressErrorStatus off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + CodeInterfacePackaging "Nonreusable function" + SupportNonFinite on + 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 19 + Version "1.17.1" + DisabledProps [] + Description "Simulink Coverage Configuration Component" + Name "Simulink Coverage" + CovEnable off + CovScope "EntireSystem" + CovIncludeTopModel on + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovCompData "" + CovMetricSettings "dwe" + CovFilter "" + CovHTMLOptions "" + CovNameIncrementing off + CovHtmlReporting off + CovForceBlockReductionOff on + CovEnableCumulative on + CovSaveCumulativeToWorkspaceVar off + CovSaveSingleToWorkspaceVar off + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovSaveOutputData on + CovOutputDir "slcov_output/$ModelName$" + CovDataFileName "$ModelName$_cvdata" + CovShowResultsExplorer on + CovReportOnPause on + CovModelRefEnable "off" + CovModelRefExcluded "" + CovExternalEMLEnable on + CovSFcnEnable on + CovBoundaryAbsTol 1e-05 + CovBoundaryRelTol 0.01 + CovUseTimeInterval off + CovStartTime 0 + CovStopTime 0 + CovMcdcMode "Masking" + } + PropName "Components" + } + Name "Configuration" + ExtraOptions "" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 145, 177, 1135, 847 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 8 + } + Object { + $PropName "DataTransfer" + $ObjectID 20 + $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 { + 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 "left" + VerticalAlignment "top" + 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 Concatenate + NumInputs "2" + Mode "Vector" + ConcatenateDimension "1" + } + 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 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 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 off + } + Block { + BlockType S-Function + FunctionName "system" + SFunctionModules "''" + PortCounts "[]" + } + 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 + IsObserver off + } + 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 + } + } + System { + Name "CMM_iCub_23_25_DoFs" + 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" + ReportName "simulink-default.rpt" + SIDHighWatermark "489" + Block { + BlockType SubSystem + Name "CentroidalMomentum Matrix iCub 23/25 DoFs" + SID "1" + Ports [3, 1] + Position [1505, 678, 1655, 752] + ZOrder 1 + RequestExecContextInheritance off + System { + Name "CentroidalMomentum Matrix iCub 23/25 DoFs" + 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 "461" + Block { + BlockType Inport + Name "w_H_b" + SID "2" + Position [270, 150, 300, 165] + ZOrder 701 + BlockRotation 270 + BlockMirror on + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "3" + Position [185, 150, 215, 165] + ZOrder 702 + BlockRotation 270 + BlockMirror on + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "105" + Position [-95, 150, -65, 165] + ZOrder 723 + BlockRotation 270 + BlockMirror on + Port "3" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "CMM_base" + SID "72" + Ports [3, 1] + Position [360, 181, 490, 249] + ZOrder 704 + RequestExecContextInheritance off + System { + Name "CMM_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 "252" + Block { + BlockType Inport + Name "w_H_b" + SID "73" + Position [55, 133, 85, 147] + ZOrder 693 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "74" + Position [55, 168, 85, 182] + ZOrder 694 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "vectorOfZeros" + SID "106" + Position [55, 203, 85, 217] + ZOrder 696 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Reference + Name "CentroidalMomentum21" + SID "75" + Ports [4, 1] + Position [575, 223, 770, 292] + ZOrder 627 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum22" + SID "76" + Ports [4, 1] + Position [575, 328, 770, 397] + ZOrder 628 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum23" + SID "77" + Ports [4, 1] + Position [575, 433, 770, 502] + ZOrder 629 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum24" + SID "78" + Ports [4, 1] + Position [575, 538, 770, 607] + ZOrder 630 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum25" + SID "79" + Ports [4, 1] + Position [575, 643, 770, 712] + ZOrder 631 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum31" + SID "80" + Ports [4, 1] + Position [575, 748, 770, 817] + ZOrder 646 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Constant + Name "Constant" + SID "81" + Position [40, 250, 100, 280] + ZOrder 638 + ShowName off + Value "[1,0,0,0,0,0]" + } + Block { + BlockType Constant + Name "Constant1" + SID "82" + Position [40, 355, 100, 385] + ZOrder 640 + ShowName off + Value "[0,1,0,0,0,0]" + } + Block { + BlockType Constant + Name "Constant3" + SID "83" + Position [40, 565, 100, 595] + ZOrder 642 + ShowName off + Value "[0,0,0,1,0,0]" + } + Block { + BlockType Constant + Name "Constant37" + SID "84" + Position [40, 460, 100, 490] + ZOrder 691 + ShowName off + Value "[0,0,1,0,0,0]" + } + Block { + BlockType Constant + Name "Constant4" + SID "85" + Position [40, 670, 100, 700] + ZOrder 643 + ShowName off + Value "[0,0,0,0,1,0]" + } + Block { + BlockType Constant + Name "Constant6" + SID "87" + Position [40, 775, 100, 805] + ZOrder 645 + ShowName off + Value "[0,0,0,0,0,1]" + } + Block { + BlockType Concatenate + Name "Matrix\nConcatenate" + SID "88" + Ports [6, 1] + Position [830, 207, 885, 838] + ZOrder 692 + ShowName off + NumInputs "6" + Mode "Multidimensional array" + ConcatenateDimension "2" + } + Block { + BlockType Outport + Name "CMM_base" + SID "89" + Position [955, 518, 985, 532] + ZOrder 695 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "w_H_b" + SrcPort 1 + Points [370, 0; 0, 95] + Branch { + ZOrder 2 + Points [0, 105] + Branch { + ZOrder 3 + Points [0, 105] + Branch { + ZOrder 4 + Points [0, 105] + Branch { + ZOrder 5 + Points [0, 105] + Branch { + ZOrder 6 + Points [0, 105] + DstBlock "CentroidalMomentum31" + DstPort 1 + } + Branch { + ZOrder 7 + DstBlock "CentroidalMomentum25" + DstPort 1 + } + } + Branch { + ZOrder 8 + DstBlock "CentroidalMomentum24" + DstPort 1 + } + } + Branch { + ZOrder 9 + DstBlock "CentroidalMomentum23" + DstPort 1 + } + } + Branch { + ZOrder 10 + DstBlock "CentroidalMomentum22" + DstPort 1 + } + } + Branch { + ZOrder 11 + DstBlock "CentroidalMomentum21" + DstPort 1 + } + } + Line { + ZOrder 12 + SrcBlock "jointPos" + SrcPort 1 + Points [320, 0; 0, 75] + Branch { + ZOrder 13 + Points [0, 105] + Branch { + ZOrder 14 + Points [0, 105] + Branch { + ZOrder 15 + Points [0, 105] + Branch { + ZOrder 16 + Points [0, 105] + Branch { + ZOrder 17 + Points [0, 105] + DstBlock "CentroidalMomentum31" + DstPort 2 + } + Branch { + ZOrder 18 + DstBlock "CentroidalMomentum25" + DstPort 2 + } + } + Branch { + ZOrder 19 + DstBlock "CentroidalMomentum24" + DstPort 2 + } + } + Branch { + ZOrder 20 + DstBlock "CentroidalMomentum23" + DstPort 2 + } + } + Branch { + ZOrder 21 + DstBlock "CentroidalMomentum22" + DstPort 2 + } + } + Branch { + ZOrder 22 + DstBlock "CentroidalMomentum21" + DstPort 2 + } + } + Line { + ZOrder 23 + SrcBlock "CentroidalMomentum25" + SrcPort 1 + DstBlock "Matrix\nConcatenate" + DstPort 5 + } + Line { + ZOrder 24 + SrcBlock "Matrix\nConcatenate" + SrcPort 1 + DstBlock "CMM_base" + DstPort 1 + } + Line { + ZOrder 25 + SrcBlock "CentroidalMomentum23" + SrcPort 1 + DstBlock "Matrix\nConcatenate" + DstPort 3 + } + Line { + ZOrder 26 + SrcBlock "CentroidalMomentum24" + SrcPort 1 + DstBlock "Matrix\nConcatenate" + DstPort 4 + } + Line { + ZOrder 27 + SrcBlock "CentroidalMomentum31" + SrcPort 1 + DstBlock "Matrix\nConcatenate" + DstPort 6 + } + Line { + ZOrder 28 + SrcBlock "Constant6" + SrcPort 1 + DstBlock "CentroidalMomentum31" + DstPort 3 + } + Line { + ZOrder 29 + SrcBlock "Constant" + SrcPort 1 + DstBlock "CentroidalMomentum21" + DstPort 3 + } + Line { + ZOrder 30 + SrcBlock "Constant1" + SrcPort 1 + DstBlock "CentroidalMomentum22" + DstPort 3 + } + Line { + ZOrder 31 + SrcBlock "Constant37" + SrcPort 1 + DstBlock "CentroidalMomentum23" + DstPort 3 + } + Line { + ZOrder 51 + SrcBlock "vectorOfZeros" + SrcPort 1 + Points [249, 0; 0, 70] + Branch { + ZOrder 50 + DstBlock "CentroidalMomentum21" + DstPort 4 + } + Branch { + ZOrder 33 + Points [0, 105] + Branch { + ZOrder 34 + Points [0, 105] + Branch { + ZOrder 35 + Points [0, 105] + Branch { + ZOrder 36 + Points [0, 105] + Branch { + ZOrder 37 + Points [0, 105] + DstBlock "CentroidalMomentum31" + DstPort 4 + } + Branch { + ZOrder 38 + DstBlock "CentroidalMomentum25" + DstPort 4 + } + } + Branch { + ZOrder 39 + DstBlock "CentroidalMomentum24" + DstPort 4 + } + } + Branch { + ZOrder 40 + DstBlock "CentroidalMomentum23" + DstPort 4 + } + } + Branch { + ZOrder 41 + DstBlock "CentroidalMomentum22" + DstPort 4 + } + } + } + Line { + ZOrder 43 + SrcBlock "Constant4" + SrcPort 1 + DstBlock "CentroidalMomentum25" + DstPort 3 + } + Line { + ZOrder 44 + SrcBlock "Constant3" + SrcPort 1 + DstBlock "CentroidalMomentum24" + DstPort 3 + } + Line { + ZOrder 45 + SrcBlock "CentroidalMomentum22" + SrcPort 1 + DstBlock "Matrix\nConcatenate" + DstPort 2 + } + Line { + ZOrder 46 + SrcBlock "CentroidalMomentum21" + SrcPort 1 + DstBlock "Matrix\nConcatenate" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "CMM_joints" + SID "126" + Ports [4, 1] + Position [360, 280, 490, 400] + ZOrder 726 + RequestExecContextInheritance off + System { + Name "CMM_joints" + 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 "432" + Block { + BlockType Inport + Name "w_H_b" + SID "322" + Position [1160, -77, 1190, -63] + ZOrder 767 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "323" + Position [1160, 13, 1190, 27] + ZOrder 768 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "324" + Position [1160, 273, 1190, 287] + ZOrder 769 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "SelMatrix" + SID "325" + Position [1160, 103, 1190, 117] + ZOrder 770 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "CMM Joints Group 1" + SID "292" + Ports [4, 1] + Position [1290, -76, 1440, -34] + ZOrder 762 + RequestExecContextInheritance off + System { + Name "CMM Joints Group 1" + 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 "325" + Block { + BlockType Inport + Name "w_H_b" + SID "337" + Position [-250, 28, -220, 42] + ZOrder 775 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "338" + Position [-250, 118, -220, 132] + ZOrder 776 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "SelMatrix" + SID "339" + Position [-615, 128, -585, 142] + ZOrder 777 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "425" + Position [-615, 218, -585, 232] + ZOrder 788 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Reference + Name "CentroidalMomentum1" + SID "293" + Ports [4, 1] + Position [0, 98, 195, 167] + ZOrder 733 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum16" + SID "294" + Ports [4, 1] + Position [0, 23, 195, 92] + ZOrder 732 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum2" + SID "295" + Ports [4, 1] + Position [0, 173, 195, 242] + ZOrder 734 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum3" + SID "296" + Ports [4, 1] + Position [0, 248, 195, 317] + ZOrder 735 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum4" + SID "297" + Ports [4, 1] + Position [0, 323, 195, 392] + ZOrder 736 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Constant + Name "Constant" + SID "381" + Position [-260, 207, -210, 223] + ZOrder 780 + Value "zeros(6,1)" + } + Block { + BlockType Constant + Name "Constant1" + SID "424" + Position [-615, 172, -585, 188] + ZOrder 787 + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "423" + Ports [3, 5] + Position [-535, 111, -365, 249] + ZOrder 786 + 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 "36" + Block { + BlockType Inport + Name "SelMatrix" + SID "423::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "groupNumber" + SID "423::22" + Position [20, 136, 40, 154] + ZOrder 13 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "423::27" + Position [20, 171, 40, 189] + ZOrder 18 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "423::35" + Ports [1, 1] + Position [270, 315, 320, 355] + ZOrder 26 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "423::34" + Tag "Stateflow S-Function CMM_iCub_23_25_DoFs 3" + Ports [3, 6] + Position [180, 102, 230, 243] + ZOrder 25 + FunctionName "sf_sfun" + PortCounts "[3 6]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "v1" + } + Port { + PortNumber 3 + Name "v2" + } + Port { + PortNumber 4 + Name "v3" + } + Port { + PortNumber 5 + Name "v4" + } + Port { + PortNumber 6 + Name "v5" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "423::36" + Position [460, 326, 480, 344] + ZOrder 27 + } + Block { + BlockType Outport + Name "v1" + SID "423::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v2" + SID "423::23" + Position [460, 136, 480, 154] + ZOrder 14 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v3" + SID "423::24" + Position [460, 171, 480, 189] + ZOrder 15 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v4" + SID "423::25" + Position [460, 206, 480, 224] + ZOrder 16 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v5" + SID "423::26" + Position [460, 246, 480, 264] + ZOrder 17 + Port "5" + IconDisplay "Port number" + } + Line { + ZOrder 74 + SrcBlock "SelMatrix" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 75 + SrcBlock "groupNumber" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 76 + SrcBlock "ROBOT_DOF" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + Name "v1" + ZOrder 77 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "v1" + DstPort 1 + } + Line { + Name "v2" + ZOrder 78 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "v2" + DstPort 1 + } + Line { + Name "v3" + ZOrder 79 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "v3" + DstPort 1 + } + Line { + Name "v4" + ZOrder 80 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "v4" + DstPort 1 + } + Line { + Name "v5" + ZOrder 81 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "v5" + DstPort 1 + } + Line { + ZOrder 82 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 83 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Concatenate + Name "Matrix\nConcatenate6" + SID "349" + Ports [5, 1] + Position [290, 21, 365, 399] + ZOrder 779 + ShowName off + NumInputs "5" + Mode "Multidimensional array" + ConcatenateDimension "2" + } + Block { + BlockType Outport + Name "CMM_group1" + SID "343" + Position [440, 203, 470, 217] + ZOrder 778 + IconDisplay "Port number" + } + Line { + ZOrder 57 + SrcBlock "Constant" + SrcPort 1 + Points [160, 0] + Branch { + ZOrder 63 + Points [0, 75] + Branch { + ZOrder 65 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 3 + } + Branch { + ZOrder 64 + DstBlock "CentroidalMomentum3" + DstPort 3 + } + } + Branch { + ZOrder 59 + Points [0, -75] + Branch { + ZOrder 62 + DstBlock "CentroidalMomentum1" + DstPort 3 + } + Branch { + ZOrder 61 + Points [0, -75] + DstBlock "CentroidalMomentum16" + DstPort 3 + } + } + Branch { + ZOrder 58 + DstBlock "CentroidalMomentum2" + DstPort 3 + } + } + Line { + ZOrder 28 + SrcBlock "CentroidalMomentum2" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 3 + } + Line { + ZOrder 27 + SrcBlock "CentroidalMomentum1" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 2 + } + Line { + ZOrder 45 + SrcBlock "jointPos" + SrcPort 1 + Points [103, 0] + Branch { + ZOrder 51 + Points [0, 75] + Branch { + ZOrder 53 + Points [0, 75] + Branch { + ZOrder 56 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 2 + } + Branch { + ZOrder 55 + DstBlock "CentroidalMomentum3" + DstPort 2 + } + } + Branch { + ZOrder 52 + DstBlock "CentroidalMomentum2" + DstPort 2 + } + } + Branch { + ZOrder 50 + Points [0, -75] + DstBlock "CentroidalMomentum16" + DstPort 2 + } + Branch { + ZOrder 49 + DstBlock "CentroidalMomentum1" + DstPort 2 + } + } + Line { + ZOrder 29 + SrcBlock "CentroidalMomentum3" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 4 + } + Line { + ZOrder 26 + SrcBlock "CentroidalMomentum16" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 1 + } + Line { + ZOrder 32 + SrcBlock "w_H_b" + SrcPort 1 + Points [135, 0] + Branch { + ZOrder 34 + Points [0, 75] + Branch { + ZOrder 42 + DstBlock "CentroidalMomentum1" + DstPort 1 + } + Branch { + ZOrder 36 + Points [0, 75] + Branch { + ZOrder 43 + DstBlock "CentroidalMomentum2" + DstPort 1 + } + Branch { + ZOrder 38 + Points [0, 75] + Branch { + ZOrder 44 + DstBlock "CentroidalMomentum3" + DstPort 1 + } + Branch { + ZOrder 40 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 1 + } + } + } + } + Branch { + ZOrder 33 + DstBlock "CentroidalMomentum16" + DstPort 1 + } + } + Line { + ZOrder 30 + SrcBlock "CentroidalMomentum4" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 5 + } + Line { + ZOrder 31 + SrcBlock "Matrix\nConcatenate6" + SrcPort 1 + DstBlock "CMM_group1" + DstPort 1 + } + Line { + ZOrder 68 + SrcBlock "MATLAB Function" + SrcPort 1 + Points [84, 0; 0, -50] + DstBlock "CentroidalMomentum16" + DstPort 4 + } + Line { + ZOrder 69 + SrcBlock "MATLAB Function" + SrcPort 2 + DstBlock "CentroidalMomentum1" + DstPort 4 + } + Line { + ZOrder 70 + SrcBlock "MATLAB Function" + SrcPort 3 + Points [80, 0; 0, 50] + DstBlock "CentroidalMomentum2" + DstPort 4 + } + Line { + ZOrder 71 + SrcBlock "MATLAB Function" + SrcPort 4 + Points [55, 0; 0, 100] + DstBlock "CentroidalMomentum3" + DstPort 4 + } + Line { + ZOrder 72 + SrcBlock "MATLAB Function" + SrcPort 5 + Points [29, 0; 0, 150] + DstBlock "CentroidalMomentum4" + DstPort 4 + } + Line { + ZOrder 73 + SrcBlock "SelMatrix" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 74 + SrcBlock "Constant1" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 2 + } + Line { + ZOrder 75 + SrcBlock "ROBOT_DOF" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 3 + } + } + } + Block { + BlockType SubSystem + Name "CMM Joints Group 2" + SID "426" + Ports [4, 1] + Position [1290, 4, 1440, 46] + ZOrder 776 + RequestExecContextInheritance off + System { + Name "CMM Joints Group 2" + 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 "325" + Block { + BlockType Inport + Name "w_H_b" + SID "427" + Position [-250, 28, -220, 42] + ZOrder 775 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "428" + Position [-250, 118, -220, 132] + ZOrder 776 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "SelMatrix" + SID "429" + Position [-615, 128, -585, 142] + ZOrder 777 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "430" + Position [-615, 218, -585, 232] + ZOrder 788 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Reference + Name "CentroidalMomentum1" + SID "431" + Ports [4, 1] + Position [0, 98, 195, 167] + ZOrder 733 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum16" + SID "432" + Ports [4, 1] + Position [0, 23, 195, 92] + ZOrder 732 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum2" + SID "433" + Ports [4, 1] + Position [0, 173, 195, 242] + ZOrder 734 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum3" + SID "434" + Ports [4, 1] + Position [0, 248, 195, 317] + ZOrder 735 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum4" + SID "435" + Ports [4, 1] + Position [0, 323, 195, 392] + ZOrder 736 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Constant + Name "Constant" + SID "436" + Position [-260, 207, -210, 223] + ZOrder 780 + Value "zeros(6,1)" + } + Block { + BlockType Constant + Name "Constant1" + SID "437" + Position [-615, 172, -585, 188] + ZOrder 787 + Value "2" + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "438" + Ports [3, 5] + Position [-535, 111, -365, 249] + ZOrder 786 + 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 "36" + Block { + BlockType Inport + Name "SelMatrix" + SID "438::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "groupNumber" + SID "438::22" + Position [20, 136, 40, 154] + ZOrder 13 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "438::27" + Position [20, 171, 40, 189] + ZOrder 18 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "438::35" + Ports [1, 1] + Position [270, 315, 320, 355] + ZOrder 26 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "438::34" + Tag "Stateflow S-Function CMM_iCub_23_25_DoFs 4" + Ports [3, 6] + Position [180, 102, 230, 243] + ZOrder 25 + FunctionName "sf_sfun" + PortCounts "[3 6]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "v1" + } + Port { + PortNumber 3 + Name "v2" + } + Port { + PortNumber 4 + Name "v3" + } + Port { + PortNumber 5 + Name "v4" + } + Port { + PortNumber 6 + Name "v5" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "438::36" + Position [460, 326, 480, 344] + ZOrder 27 + } + Block { + BlockType Outport + Name "v1" + SID "438::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v2" + SID "438::23" + Position [460, 136, 480, 154] + ZOrder 14 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v3" + SID "438::24" + Position [460, 171, 480, 189] + ZOrder 15 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v4" + SID "438::25" + Position [460, 206, 480, 224] + ZOrder 16 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v5" + SID "438::26" + Position [460, 246, 480, 264] + ZOrder 17 + Port "5" + IconDisplay "Port number" + } + Line { + ZOrder 31 + SrcBlock "SelMatrix" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 32 + SrcBlock "groupNumber" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 33 + SrcBlock "ROBOT_DOF" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + Name "v1" + ZOrder 34 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "v1" + DstPort 1 + } + Line { + Name "v2" + ZOrder 35 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "v2" + DstPort 1 + } + Line { + Name "v3" + ZOrder 36 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "v3" + DstPort 1 + } + Line { + Name "v4" + ZOrder 37 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "v4" + DstPort 1 + } + Line { + Name "v5" + ZOrder 38 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "v5" + 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 Concatenate + Name "Matrix\nConcatenate6" + SID "439" + Ports [5, 1] + Position [290, 21, 365, 399] + ZOrder 779 + ShowName off + NumInputs "5" + Mode "Multidimensional array" + ConcatenateDimension "2" + } + Block { + BlockType Outport + Name "CMM_group2" + SID "440" + Position [440, 203, 470, 217] + ZOrder 778 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "Constant" + SrcPort 1 + Points [160, 0] + Branch { + ZOrder 2 + Points [0, 75] + Branch { + ZOrder 3 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 3 + } + Branch { + ZOrder 4 + DstBlock "CentroidalMomentum3" + DstPort 3 + } + } + Branch { + ZOrder 5 + Points [0, -75] + Branch { + ZOrder 6 + DstBlock "CentroidalMomentum1" + DstPort 3 + } + Branch { + ZOrder 7 + Points [0, -75] + DstBlock "CentroidalMomentum16" + DstPort 3 + } + } + Branch { + ZOrder 8 + DstBlock "CentroidalMomentum2" + DstPort 3 + } + } + Line { + ZOrder 9 + SrcBlock "CentroidalMomentum2" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 3 + } + Line { + ZOrder 10 + SrcBlock "CentroidalMomentum1" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 2 + } + Line { + ZOrder 11 + SrcBlock "jointPos" + SrcPort 1 + Points [103, 0] + Branch { + ZOrder 12 + Points [0, 75] + Branch { + ZOrder 13 + Points [0, 75] + Branch { + ZOrder 14 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 2 + } + Branch { + ZOrder 15 + DstBlock "CentroidalMomentum3" + DstPort 2 + } + } + Branch { + ZOrder 16 + DstBlock "CentroidalMomentum2" + DstPort 2 + } + } + Branch { + ZOrder 17 + Points [0, -75] + DstBlock "CentroidalMomentum16" + DstPort 2 + } + Branch { + ZOrder 18 + DstBlock "CentroidalMomentum1" + DstPort 2 + } + } + Line { + ZOrder 19 + SrcBlock "CentroidalMomentum3" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 4 + } + Line { + ZOrder 20 + SrcBlock "CentroidalMomentum16" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 1 + } + Line { + ZOrder 21 + SrcBlock "w_H_b" + SrcPort 1 + Points [135, 0] + Branch { + ZOrder 22 + Points [0, 75] + Branch { + ZOrder 23 + DstBlock "CentroidalMomentum1" + DstPort 1 + } + Branch { + ZOrder 24 + Points [0, 75] + Branch { + ZOrder 25 + DstBlock "CentroidalMomentum2" + DstPort 1 + } + Branch { + ZOrder 26 + Points [0, 75] + Branch { + ZOrder 27 + DstBlock "CentroidalMomentum3" + DstPort 1 + } + Branch { + ZOrder 28 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 1 + } + } + } + } + Branch { + ZOrder 29 + DstBlock "CentroidalMomentum16" + DstPort 1 + } + } + Line { + ZOrder 30 + SrcBlock "CentroidalMomentum4" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 5 + } + Line { + ZOrder 31 + SrcBlock "Matrix\nConcatenate6" + SrcPort 1 + DstBlock "CMM_group2" + DstPort 1 + } + Line { + ZOrder 32 + SrcBlock "MATLAB Function" + SrcPort 1 + Points [84, 0; 0, -50] + DstBlock "CentroidalMomentum16" + DstPort 4 + } + Line { + ZOrder 33 + SrcBlock "MATLAB Function" + SrcPort 2 + DstBlock "CentroidalMomentum1" + DstPort 4 + } + Line { + ZOrder 34 + SrcBlock "MATLAB Function" + SrcPort 3 + Points [80, 0; 0, 50] + DstBlock "CentroidalMomentum2" + DstPort 4 + } + Line { + ZOrder 35 + SrcBlock "MATLAB Function" + SrcPort 4 + Points [55, 0; 0, 100] + DstBlock "CentroidalMomentum3" + DstPort 4 + } + Line { + ZOrder 36 + SrcBlock "MATLAB Function" + SrcPort 5 + Points [29, 0; 0, 150] + DstBlock "CentroidalMomentum4" + DstPort 4 + } + Line { + ZOrder 37 + SrcBlock "SelMatrix" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 38 + SrcBlock "Constant1" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 2 + } + Line { + ZOrder 39 + SrcBlock "ROBOT_DOF" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 3 + } + } + } + Block { + BlockType SubSystem + Name "CMM Joints Group 3" + SID "441" + Ports [4, 1] + Position [1290, 84, 1440, 126] + ZOrder 777 + RequestExecContextInheritance off + System { + Name "CMM Joints Group 3" + 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 "325" + Block { + BlockType Inport + Name "w_H_b" + SID "442" + Position [-250, 28, -220, 42] + ZOrder 775 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "443" + Position [-250, 118, -220, 132] + ZOrder 776 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "SelMatrix" + SID "444" + Position [-615, 128, -585, 142] + ZOrder 777 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "445" + Position [-615, 218, -585, 232] + ZOrder 788 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Reference + Name "CentroidalMomentum1" + SID "446" + Ports [4, 1] + Position [0, 98, 195, 167] + ZOrder 733 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum16" + SID "447" + Ports [4, 1] + Position [0, 23, 195, 92] + ZOrder 732 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum2" + SID "448" + Ports [4, 1] + Position [0, 173, 195, 242] + ZOrder 734 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum3" + SID "449" + Ports [4, 1] + Position [0, 248, 195, 317] + ZOrder 735 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum4" + SID "450" + Ports [4, 1] + Position [0, 323, 195, 392] + ZOrder 736 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Constant + Name "Constant" + SID "451" + Position [-260, 207, -210, 223] + ZOrder 780 + Value "zeros(6,1)" + } + Block { + BlockType Constant + Name "Constant1" + SID "452" + Position [-615, 172, -585, 188] + ZOrder 787 + Value "3" + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "453" + Ports [3, 5] + Position [-535, 111, -365, 249] + ZOrder 786 + 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 "36" + Block { + BlockType Inport + Name "SelMatrix" + SID "453::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "groupNumber" + SID "453::22" + Position [20, 136, 40, 154] + ZOrder 13 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "453::27" + Position [20, 171, 40, 189] + ZOrder 18 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "453::35" + Ports [1, 1] + Position [270, 315, 320, 355] + ZOrder 26 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "453::34" + Tag "Stateflow S-Function CMM_iCub_23_25_DoFs 5" + Ports [3, 6] + Position [180, 102, 230, 243] + ZOrder 25 + FunctionName "sf_sfun" + PortCounts "[3 6]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "v1" + } + Port { + PortNumber 3 + Name "v2" + } + Port { + PortNumber 4 + Name "v3" + } + Port { + PortNumber 5 + Name "v4" + } + Port { + PortNumber 6 + Name "v5" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "453::36" + Position [460, 326, 480, 344] + ZOrder 27 + } + Block { + BlockType Outport + Name "v1" + SID "453::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v2" + SID "453::23" + Position [460, 136, 480, 154] + ZOrder 14 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v3" + SID "453::24" + Position [460, 171, 480, 189] + ZOrder 15 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v4" + SID "453::25" + Position [460, 206, 480, 224] + ZOrder 16 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v5" + SID "453::26" + Position [460, 246, 480, 264] + ZOrder 17 + Port "5" + IconDisplay "Port number" + } + Line { + ZOrder 31 + SrcBlock "SelMatrix" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 32 + SrcBlock "groupNumber" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 33 + SrcBlock "ROBOT_DOF" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + Name "v1" + ZOrder 34 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "v1" + DstPort 1 + } + Line { + Name "v2" + ZOrder 35 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "v2" + DstPort 1 + } + Line { + Name "v3" + ZOrder 36 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "v3" + DstPort 1 + } + Line { + Name "v4" + ZOrder 37 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "v4" + DstPort 1 + } + Line { + Name "v5" + ZOrder 38 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "v5" + 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 Concatenate + Name "Matrix\nConcatenate6" + SID "454" + Ports [5, 1] + Position [290, 21, 365, 399] + ZOrder 779 + ShowName off + NumInputs "5" + Mode "Multidimensional array" + ConcatenateDimension "2" + } + Block { + BlockType Outport + Name "CMM_group3" + SID "455" + Position [440, 203, 470, 217] + ZOrder 778 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "Constant" + SrcPort 1 + Points [160, 0] + Branch { + ZOrder 2 + Points [0, 75] + Branch { + ZOrder 3 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 3 + } + Branch { + ZOrder 4 + DstBlock "CentroidalMomentum3" + DstPort 3 + } + } + Branch { + ZOrder 5 + Points [0, -75] + Branch { + ZOrder 6 + DstBlock "CentroidalMomentum1" + DstPort 3 + } + Branch { + ZOrder 7 + Points [0, -75] + DstBlock "CentroidalMomentum16" + DstPort 3 + } + } + Branch { + ZOrder 8 + DstBlock "CentroidalMomentum2" + DstPort 3 + } + } + Line { + ZOrder 9 + SrcBlock "CentroidalMomentum2" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 3 + } + Line { + ZOrder 10 + SrcBlock "CentroidalMomentum1" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 2 + } + Line { + ZOrder 11 + SrcBlock "jointPos" + SrcPort 1 + Points [103, 0] + Branch { + ZOrder 12 + Points [0, 75] + Branch { + ZOrder 13 + Points [0, 75] + Branch { + ZOrder 14 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 2 + } + Branch { + ZOrder 15 + DstBlock "CentroidalMomentum3" + DstPort 2 + } + } + Branch { + ZOrder 16 + DstBlock "CentroidalMomentum2" + DstPort 2 + } + } + Branch { + ZOrder 17 + Points [0, -75] + DstBlock "CentroidalMomentum16" + DstPort 2 + } + Branch { + ZOrder 18 + DstBlock "CentroidalMomentum1" + DstPort 2 + } + } + Line { + ZOrder 19 + SrcBlock "CentroidalMomentum3" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 4 + } + Line { + ZOrder 20 + SrcBlock "CentroidalMomentum16" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 1 + } + Line { + ZOrder 21 + SrcBlock "w_H_b" + SrcPort 1 + Points [135, 0] + Branch { + ZOrder 22 + Points [0, 75] + Branch { + ZOrder 23 + DstBlock "CentroidalMomentum1" + DstPort 1 + } + Branch { + ZOrder 24 + Points [0, 75] + Branch { + ZOrder 25 + DstBlock "CentroidalMomentum2" + DstPort 1 + } + Branch { + ZOrder 26 + Points [0, 75] + Branch { + ZOrder 27 + DstBlock "CentroidalMomentum3" + DstPort 1 + } + Branch { + ZOrder 28 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 1 + } + } + } + } + Branch { + ZOrder 29 + DstBlock "CentroidalMomentum16" + DstPort 1 + } + } + Line { + ZOrder 30 + SrcBlock "CentroidalMomentum4" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 5 + } + Line { + ZOrder 31 + SrcBlock "Matrix\nConcatenate6" + SrcPort 1 + DstBlock "CMM_group3" + DstPort 1 + } + Line { + ZOrder 32 + SrcBlock "MATLAB Function" + SrcPort 1 + Points [84, 0; 0, -50] + DstBlock "CentroidalMomentum16" + DstPort 4 + } + Line { + ZOrder 33 + SrcBlock "MATLAB Function" + SrcPort 2 + DstBlock "CentroidalMomentum1" + DstPort 4 + } + Line { + ZOrder 34 + SrcBlock "MATLAB Function" + SrcPort 3 + Points [80, 0; 0, 50] + DstBlock "CentroidalMomentum2" + DstPort 4 + } + Line { + ZOrder 35 + SrcBlock "MATLAB Function" + SrcPort 4 + Points [55, 0; 0, 100] + DstBlock "CentroidalMomentum3" + DstPort 4 + } + Line { + ZOrder 36 + SrcBlock "MATLAB Function" + SrcPort 5 + Points [29, 0; 0, 150] + DstBlock "CentroidalMomentum4" + DstPort 4 + } + Line { + ZOrder 37 + SrcBlock "SelMatrix" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 38 + SrcBlock "Constant1" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 2 + } + Line { + ZOrder 39 + SrcBlock "ROBOT_DOF" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 3 + } + } + } + Block { + BlockType SubSystem + Name "CMM Joints Group 4" + SID "456" + Ports [4, 1] + Position [1290, 164, 1440, 206] + ZOrder 778 + RequestExecContextInheritance off + System { + Name "CMM Joints Group 4" + 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 "325" + Block { + BlockType Inport + Name "w_H_b" + SID "457" + Position [-250, 28, -220, 42] + ZOrder 775 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "458" + Position [-250, 118, -220, 132] + ZOrder 776 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "SelMatrix" + SID "459" + Position [-615, 128, -585, 142] + ZOrder 777 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "460" + Position [-615, 218, -585, 232] + ZOrder 788 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Reference + Name "CentroidalMomentum1" + SID "461" + Ports [4, 1] + Position [0, 98, 195, 167] + ZOrder 733 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum16" + SID "462" + Ports [4, 1] + Position [0, 23, 195, 92] + ZOrder 732 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum2" + SID "463" + Ports [4, 1] + Position [0, 173, 195, 242] + ZOrder 734 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum3" + SID "464" + Ports [4, 1] + Position [0, 248, 195, 317] + ZOrder 735 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum4" + SID "465" + Ports [4, 1] + Position [0, 323, 195, 392] + ZOrder 736 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Constant + Name "Constant" + SID "466" + Position [-260, 207, -210, 223] + ZOrder 780 + Value "zeros(6,1)" + } + Block { + BlockType Constant + Name "Constant1" + SID "467" + Position [-615, 172, -585, 188] + ZOrder 787 + Value "4" + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "468" + Ports [3, 5] + Position [-535, 111, -365, 249] + ZOrder 786 + 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 "36" + Block { + BlockType Inport + Name "SelMatrix" + SID "468::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "groupNumber" + SID "468::22" + Position [20, 136, 40, 154] + ZOrder 13 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "468::27" + Position [20, 171, 40, 189] + ZOrder 18 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "468::35" + Ports [1, 1] + Position [270, 315, 320, 355] + ZOrder 26 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "468::34" + Tag "Stateflow S-Function CMM_iCub_23_25_DoFs 6" + Ports [3, 6] + Position [180, 102, 230, 243] + ZOrder 25 + FunctionName "sf_sfun" + PortCounts "[3 6]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "v1" + } + Port { + PortNumber 3 + Name "v2" + } + Port { + PortNumber 4 + Name "v3" + } + Port { + PortNumber 5 + Name "v4" + } + Port { + PortNumber 6 + Name "v5" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "468::36" + Position [460, 326, 480, 344] + ZOrder 27 + } + Block { + BlockType Outport + Name "v1" + SID "468::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v2" + SID "468::23" + Position [460, 136, 480, 154] + ZOrder 14 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v3" + SID "468::24" + Position [460, 171, 480, 189] + ZOrder 15 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v4" + SID "468::25" + Position [460, 206, 480, 224] + ZOrder 16 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v5" + SID "468::26" + Position [460, 246, 480, 264] + ZOrder 17 + Port "5" + IconDisplay "Port number" + } + Line { + ZOrder 31 + SrcBlock "SelMatrix" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 32 + SrcBlock "groupNumber" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 33 + SrcBlock "ROBOT_DOF" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + Name "v1" + ZOrder 34 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "v1" + DstPort 1 + } + Line { + Name "v2" + ZOrder 35 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "v2" + DstPort 1 + } + Line { + Name "v3" + ZOrder 36 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "v3" + DstPort 1 + } + Line { + Name "v4" + ZOrder 37 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "v4" + DstPort 1 + } + Line { + Name "v5" + ZOrder 38 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "v5" + 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 Concatenate + Name "Matrix\nConcatenate6" + SID "469" + Ports [5, 1] + Position [290, 21, 365, 399] + ZOrder 779 + ShowName off + NumInputs "5" + Mode "Multidimensional array" + ConcatenateDimension "2" + } + Block { + BlockType Outport + Name "CMM_group4" + SID "470" + Position [440, 203, 470, 217] + ZOrder 778 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "Constant" + SrcPort 1 + Points [160, 0] + Branch { + ZOrder 2 + Points [0, 75] + Branch { + ZOrder 3 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 3 + } + Branch { + ZOrder 4 + DstBlock "CentroidalMomentum3" + DstPort 3 + } + } + Branch { + ZOrder 5 + Points [0, -75] + Branch { + ZOrder 6 + DstBlock "CentroidalMomentum1" + DstPort 3 + } + Branch { + ZOrder 7 + Points [0, -75] + DstBlock "CentroidalMomentum16" + DstPort 3 + } + } + Branch { + ZOrder 8 + DstBlock "CentroidalMomentum2" + DstPort 3 + } + } + Line { + ZOrder 9 + SrcBlock "CentroidalMomentum2" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 3 + } + Line { + ZOrder 10 + SrcBlock "CentroidalMomentum1" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 2 + } + Line { + ZOrder 11 + SrcBlock "jointPos" + SrcPort 1 + Points [103, 0] + Branch { + ZOrder 12 + Points [0, 75] + Branch { + ZOrder 13 + Points [0, 75] + Branch { + ZOrder 14 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 2 + } + Branch { + ZOrder 15 + DstBlock "CentroidalMomentum3" + DstPort 2 + } + } + Branch { + ZOrder 16 + DstBlock "CentroidalMomentum2" + DstPort 2 + } + } + Branch { + ZOrder 17 + Points [0, -75] + DstBlock "CentroidalMomentum16" + DstPort 2 + } + Branch { + ZOrder 18 + DstBlock "CentroidalMomentum1" + DstPort 2 + } + } + Line { + ZOrder 19 + SrcBlock "CentroidalMomentum3" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 4 + } + Line { + ZOrder 20 + SrcBlock "CentroidalMomentum16" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 1 + } + Line { + ZOrder 21 + SrcBlock "w_H_b" + SrcPort 1 + Points [135, 0] + Branch { + ZOrder 22 + Points [0, 75] + Branch { + ZOrder 23 + DstBlock "CentroidalMomentum1" + DstPort 1 + } + Branch { + ZOrder 24 + Points [0, 75] + Branch { + ZOrder 25 + DstBlock "CentroidalMomentum2" + DstPort 1 + } + Branch { + ZOrder 26 + Points [0, 75] + Branch { + ZOrder 27 + DstBlock "CentroidalMomentum3" + DstPort 1 + } + Branch { + ZOrder 28 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 1 + } + } + } + } + Branch { + ZOrder 29 + DstBlock "CentroidalMomentum16" + DstPort 1 + } + } + Line { + ZOrder 30 + SrcBlock "CentroidalMomentum4" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 5 + } + Line { + ZOrder 31 + SrcBlock "Matrix\nConcatenate6" + SrcPort 1 + DstBlock "CMM_group4" + DstPort 1 + } + Line { + ZOrder 32 + SrcBlock "MATLAB Function" + SrcPort 1 + Points [84, 0; 0, -50] + DstBlock "CentroidalMomentum16" + DstPort 4 + } + Line { + ZOrder 33 + SrcBlock "MATLAB Function" + SrcPort 2 + DstBlock "CentroidalMomentum1" + DstPort 4 + } + Line { + ZOrder 34 + SrcBlock "MATLAB Function" + SrcPort 3 + Points [80, 0; 0, 50] + DstBlock "CentroidalMomentum2" + DstPort 4 + } + Line { + ZOrder 35 + SrcBlock "MATLAB Function" + SrcPort 4 + Points [55, 0; 0, 100] + DstBlock "CentroidalMomentum3" + DstPort 4 + } + Line { + ZOrder 36 + SrcBlock "MATLAB Function" + SrcPort 5 + Points [29, 0; 0, 150] + DstBlock "CentroidalMomentum4" + DstPort 4 + } + Line { + ZOrder 37 + SrcBlock "SelMatrix" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 38 + SrcBlock "Constant1" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 2 + } + Line { + ZOrder 39 + SrcBlock "ROBOT_DOF" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 3 + } + } + } + Block { + BlockType SubSystem + Name "CMM Joints Group 5 (SPECIAL)" + SID "471" + Ports [4, 1] + Position [1290, 244, 1440, 286] + ZOrder 779 + RequestExecContextInheritance off + System { + Name "CMM Joints Group 5 (SPECIAL)" + 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 "258" + Block { + BlockType Inport + Name "w_H_b" + SID "472" + Position [-250, 28, -220, 42] + ZOrder 775 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "jointPos" + SID "473" + Position [-250, 118, -220, 132] + ZOrder 776 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "SelMatrix" + SID "474" + Position [-615, 128, -585, 142] + ZOrder 777 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "475" + Position [-615, 218, -585, 232] + ZOrder 788 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Reference + Name "CentroidalMomentum1" + SID "476" + Ports [4, 1] + Position [0, 98, 195, 167] + ZOrder 733 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum16" + SID "477" + Ports [4, 1] + Position [0, 23, 195, 92] + ZOrder 732 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum2" + SID "478" + Ports [4, 1] + Position [0, 173, 195, 242] + ZOrder 734 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum3" + SID "479" + Ports [4, 1] + Position [0, 248, 195, 317] + ZOrder 735 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Reference + Name "CentroidalMomentum4" + SID "480" + Ports [4, 1] + Position [0, 323, 195, 392] + ZOrder 736 + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Dynamics/CentroidalMomentum" + SourceType "CentroidalMomentum" + SourceProductName "WholeBodyToolbox" + ContentPreviewEnabled off + } + Block { + BlockType Constant + Name "Constant" + SID "481" + Position [-260, 207, -210, 223] + ZOrder 780 + Value "zeros(6,1)" + } + Block { + BlockType Constant + Name "Constant1" + SID "482" + Position [-615, 172, -585, 188] + ZOrder 787 + Value "5" + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "483" + Ports [3, 5] + Position [-535, 111, -365, 249] + ZOrder 786 + 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 "36" + Block { + BlockType Inport + Name "SelMatrix" + SID "483::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "groupNumber" + SID "483::22" + Position [20, 136, 40, 154] + ZOrder 13 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "483::27" + Position [20, 171, 40, 189] + ZOrder 18 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "483::35" + Ports [1, 1] + Position [270, 315, 320, 355] + ZOrder 26 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "483::34" + Tag "Stateflow S-Function CMM_iCub_23_25_DoFs 7" + Ports [3, 6] + Position [180, 102, 230, 243] + ZOrder 25 + FunctionName "sf_sfun" + PortCounts "[3 6]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "v1" + } + Port { + PortNumber 3 + Name "v2" + } + Port { + PortNumber 4 + Name "v3" + } + Port { + PortNumber 5 + Name "v4" + } + Port { + PortNumber 6 + Name "v5" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "483::36" + Position [460, 326, 480, 344] + ZOrder 27 + } + Block { + BlockType Outport + Name "v1" + SID "483::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v2" + SID "483::23" + Position [460, 136, 480, 154] + ZOrder 14 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v3" + SID "483::24" + Position [460, 171, 480, 189] + ZOrder 15 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v4" + SID "483::25" + Position [460, 206, 480, 224] + ZOrder 16 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v5" + SID "483::26" + Position [460, 246, 480, 264] + ZOrder 17 + Port "5" + IconDisplay "Port number" + } + Line { + ZOrder 31 + SrcBlock "SelMatrix" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 32 + SrcBlock "groupNumber" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + ZOrder 33 + SrcBlock "ROBOT_DOF" + SrcPort 1 + DstBlock " SFunction " + DstPort 3 + } + Line { + Name "v1" + ZOrder 34 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "v1" + DstPort 1 + } + Line { + Name "v2" + ZOrder 35 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 3 + DstBlock "v2" + DstPort 1 + } + Line { + Name "v3" + ZOrder 36 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 4 + DstBlock "v3" + DstPort 1 + } + Line { + Name "v4" + ZOrder 37 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 5 + DstBlock "v4" + DstPort 1 + } + Line { + Name "v5" + ZOrder 38 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 6 + DstBlock "v5" + 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 Concatenate + Name "Matrix\nConcatenate2" + SID "487" + Ports [3, 1] + Position [460, 110, 530, 260] + ZOrder 790 + ShowName off + NumInputs "3" + Mode "Multidimensional array" + ConcatenateDimension "2" + } + Block { + BlockType Concatenate + Name "Matrix\nConcatenate6" + SID "484" + Ports [3, 1] + Position [265, 23, 335, 247] + ZOrder 779 + ShowName off + NumInputs "3" + Mode "Multidimensional array" + ConcatenateDimension "2" + } + Block { + BlockType Switch + Name "Switch" + SID "489" + Position [620, 156, 680, 324] + ZOrder 792 + Criteria "u2 > Threshold" + Threshold "23" + InputSameDT off + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "CMM_group5" + SID "485" + Position [735, 233, 765, 247] + ZOrder 778 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "Constant" + SrcPort 1 + Points [160, 0] + Branch { + ZOrder 2 + Points [0, 75] + Branch { + ZOrder 3 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 3 + } + Branch { + ZOrder 4 + DstBlock "CentroidalMomentum3" + DstPort 3 + } + } + Branch { + ZOrder 5 + Points [0, -75] + Branch { + ZOrder 6 + DstBlock "CentroidalMomentum1" + DstPort 3 + } + Branch { + ZOrder 7 + Points [0, -75] + DstBlock "CentroidalMomentum16" + DstPort 3 + } + } + Branch { + ZOrder 8 + DstBlock "CentroidalMomentum2" + DstPort 3 + } + } + Line { + ZOrder 9 + SrcBlock "CentroidalMomentum2" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 3 + } + Line { + ZOrder 10 + SrcBlock "CentroidalMomentum1" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 2 + } + Line { + ZOrder 11 + SrcBlock "jointPos" + SrcPort 1 + Points [103, 0] + Branch { + ZOrder 12 + Points [0, 75] + Branch { + ZOrder 13 + Points [0, 75] + Branch { + ZOrder 14 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 2 + } + Branch { + ZOrder 15 + DstBlock "CentroidalMomentum3" + DstPort 2 + } + } + Branch { + ZOrder 16 + DstBlock "CentroidalMomentum2" + DstPort 2 + } + } + Branch { + ZOrder 17 + Points [0, -75] + DstBlock "CentroidalMomentum16" + DstPort 2 + } + Branch { + ZOrder 18 + DstBlock "CentroidalMomentum1" + DstPort 2 + } + } + Line { + ZOrder 43 + SrcBlock "CentroidalMomentum3" + SrcPort 1 + Points [162, 0; 0, -100] + DstBlock "Matrix\nConcatenate2" + DstPort 2 + } + Line { + ZOrder 20 + SrcBlock "CentroidalMomentum16" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 1 + } + Line { + ZOrder 21 + SrcBlock "w_H_b" + SrcPort 1 + Points [135, 0] + Branch { + ZOrder 22 + Points [0, 75] + Branch { + ZOrder 23 + DstBlock "CentroidalMomentum1" + DstPort 1 + } + Branch { + ZOrder 24 + Points [0, 75] + Branch { + ZOrder 25 + DstBlock "CentroidalMomentum2" + DstPort 1 + } + Branch { + ZOrder 26 + Points [0, 75] + Branch { + ZOrder 27 + DstBlock "CentroidalMomentum3" + DstPort 1 + } + Branch { + ZOrder 28 + Points [0, 75] + DstBlock "CentroidalMomentum4" + DstPort 1 + } + } + } + } + Branch { + ZOrder 29 + DstBlock "CentroidalMomentum16" + DstPort 1 + } + } + Line { + ZOrder 44 + SrcBlock "CentroidalMomentum4" + SrcPort 1 + Points [188, 0; 0, -125] + DstBlock "Matrix\nConcatenate2" + DstPort 3 + } + Line { + ZOrder 32 + SrcBlock "MATLAB Function" + SrcPort 1 + Points [84, 0; 0, -50] + DstBlock "CentroidalMomentum16" + DstPort 4 + } + Line { + ZOrder 33 + SrcBlock "MATLAB Function" + SrcPort 2 + DstBlock "CentroidalMomentum1" + DstPort 4 + } + Line { + ZOrder 34 + SrcBlock "MATLAB Function" + SrcPort 3 + Points [80, 0; 0, 50] + DstBlock "CentroidalMomentum2" + DstPort 4 + } + Line { + ZOrder 35 + SrcBlock "MATLAB Function" + SrcPort 4 + Points [55, 0; 0, 100] + DstBlock "CentroidalMomentum3" + DstPort 4 + } + Line { + ZOrder 36 + SrcBlock "MATLAB Function" + SrcPort 5 + Points [29, 0; 0, 150] + DstBlock "CentroidalMomentum4" + DstPort 4 + } + Line { + ZOrder 37 + SrcBlock "SelMatrix" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 38 + SrcBlock "Constant1" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 2 + } + Line { + ZOrder 39 + SrcBlock "ROBOT_DOF" + SrcPort 1 + Points [21, 0] + Branch { + ZOrder 53 + Points [0, 181; 1123, 0; 0, -166] + DstBlock "Switch" + DstPort 2 + } + Branch { + ZOrder 52 + DstBlock "MATLAB Function" + DstPort 3 + } + } + Line { + ZOrder 42 + SrcBlock "Matrix\nConcatenate6" + SrcPort 1 + Points [81, 0] + Branch { + ZOrder 50 + Points [0, 160] + DstBlock "Switch" + DstPort 3 + } + Branch { + ZOrder 49 + DstBlock "Matrix\nConcatenate2" + DstPort 1 + } + } + Line { + ZOrder 45 + SrcBlock "Matrix\nConcatenate2" + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + Line { + ZOrder 51 + SrcBlock "Switch" + SrcPort 1 + DstBlock "CMM_group5" + DstPort 1 + } + } + } + Block { + BlockType Concatenate + Name "Matrix\nConcatenate6" + SID "348" + Ports [5, 1] + Position [1510, -101, 1580, 311] + ZOrder 772 + ShowName off + NumInputs "5" + Mode "Multidimensional array" + ConcatenateDimension "2" + } + Block { + BlockType Outport + Name "CMM_joints" + SID "326" + Position [1630, 98, 1660, 112] + ZOrder 771 + IconDisplay "Port number" + } + Line { + ZOrder 317 + SrcBlock "Matrix\nConcatenate6" + SrcPort 1 + DstBlock "CMM_joints" + DstPort 1 + } + Line { + ZOrder 260 + SrcBlock "w_H_b" + SrcPort 1 + Points [15, 0] + Branch { + ZOrder 347 + DstBlock "CMM Joints Group 1" + DstPort 1 + } + Branch { + ZOrder 282 + Points [0, 80] + Branch { + ZOrder 348 + DstBlock "CMM Joints Group 2" + DstPort 1 + } + Branch { + ZOrder 334 + Points [0, 80] + Branch { + ZOrder 349 + DstBlock "CMM Joints Group 3" + DstPort 1 + } + Branch { + ZOrder 336 + Points [0, 80] + Branch { + ZOrder 350 + DstBlock "CMM Joints Group 4" + DstPort 1 + } + Branch { + ZOrder 340 + Points [0, 80] + DstBlock "CMM Joints Group 5 (SPECIAL)" + DstPort 1 + } + } + } + } + } + Line { + ZOrder 261 + SrcBlock "jointPos" + SrcPort 1 + Points [27, 0] + Branch { + ZOrder 284 + Points [0, 80] + Branch { + ZOrder 355 + DstBlock "CMM Joints Group 3" + DstPort 2 + } + Branch { + ZOrder 354 + Points [0, 80] + Branch { + ZOrder 357 + DstBlock "CMM Joints Group 4" + DstPort 2 + } + Branch { + ZOrder 356 + Points [0, 80] + DstBlock "CMM Joints Group 5 (SPECIAL)" + DstPort 2 + } + } + } + Branch { + ZOrder 353 + DstBlock "CMM Joints Group 2" + DstPort 2 + } + Branch { + ZOrder 352 + Points [0, -80] + DstBlock "CMM Joints Group 1" + DstPort 2 + } + } + Line { + ZOrder 262 + SrcBlock "SelMatrix" + SrcPort 1 + Points [41, 0] + Branch { + ZOrder 288 + Points [0, 80] + Branch { + ZOrder 360 + DstBlock "CMM Joints Group 4" + DstPort 3 + } + Branch { + ZOrder 359 + Points [0, 80] + DstBlock "CMM Joints Group 5 (SPECIAL)" + DstPort 3 + } + } + Branch { + ZOrder 362 + DstBlock "CMM Joints Group 3" + DstPort 3 + } + Branch { + ZOrder 361 + Points [0, -80] + Branch { + ZOrder 364 + DstBlock "CMM Joints Group 2" + DstPort 3 + } + Branch { + ZOrder 363 + Points [0, -80] + DstBlock "CMM Joints Group 1" + DstPort 3 + } + } + } + Line { + ZOrder 312 + SrcBlock "CMM Joints Group 1" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 1 + } + Line { + ZOrder 331 + SrcBlock "CMM Joints Group 2" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 2 + } + Line { + ZOrder 332 + SrcBlock "CMM Joints Group 3" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 3 + } + Line { + ZOrder 333 + SrcBlock "CMM Joints Group 4" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 4 + } + Line { + ZOrder 330 + SrcBlock "CMM Joints Group 5 (SPECIAL)" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 5 + } + Line { + ZOrder 342 + SrcBlock "ROBOT_DOF" + SrcPort 1 + Points [52, 0] + Branch { + ZOrder 365 + DstBlock "CMM Joints Group 5 (SPECIAL)" + DstPort 4 + } + Branch { + ZOrder 346 + Points [0, -80] + Branch { + ZOrder 367 + Points [0, -80] + Branch { + ZOrder 370 + DstBlock "CMM Joints Group 3" + DstPort 4 + } + Branch { + ZOrder 369 + Points [0, -80] + Branch { + ZOrder 373 + Points [0, -80] + DstBlock "CMM Joints Group 1" + DstPort 4 + } + Branch { + ZOrder 372 + DstBlock "CMM Joints Group 2" + DstPort 4 + } + } + } + Branch { + ZOrder 366 + DstBlock "CMM Joints Group 4" + DstPort 4 + } + } + } + } + } + Block { + BlockType SubSystem + Name "Identity Matrix" + SID "113" + Ports [1, 1] + Position [-30, 366, 135, 404] + ZOrder 724 + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + ContentPreviewEnabled on + System { + Name "Identity 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 "30" + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "113::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "113::29" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 20 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "113::28" + Tag "Stateflow S-Function CMM_iCub_23_25_DoFs 1" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 19 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "SelMatrix" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "113::30" + Position [460, 241, 480, 259] + ZOrder 21 + } + Block { + BlockType Outport + Name "SelMatrix" + SID "113::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 17 + SrcBlock "ROBOT_DOF" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "SelMatrix" + ZOrder 18 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "SelMatrix" + 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 Concatenate + Name "Matrix\nConcatenate6" + SID "102" + Ports [2, 1] + Position [560, 151, 615, 404] + ZOrder 700 + ShowName off + Mode "Multidimensional array" + ConcatenateDimension "2" + } + Block { + BlockType SubSystem + Name "Vector of Zeros" + SID "112" + Ports [1, 1] + Position [-30, 217, 135, 253] + ZOrder 725 + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + ContentPreviewEnabled on + System { + Name "Vector of Zeros" + 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 "30" + Block { + BlockType Inport + Name "ROBOT_DOF" + SID "112::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "112::29" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 20 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "112::28" + Tag "Stateflow S-Function CMM_iCub_23_25_DoFs 2" + Ports [1, 2] + Position [180, 100, 230, 160] + ZOrder 19 + FunctionName "sf_sfun" + PortCounts "[1 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "v_0" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "112::30" + Position [460, 241, 480, 259] + ZOrder 21 + } + Block { + BlockType Outport + Name "v_0" + SID "112::5" + Position [460, 101, 480, 119] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 13 + SrcBlock "ROBOT_DOF" + SrcPort 1 + DstBlock " SFunction " + DstPort 1 + } + Line { + Name "v_0" + ZOrder 14 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "v_0" + 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 Outport + Name "CMM" + SID "103" + Position [665, 273, 695, 287] + ZOrder 703 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "Matrix\nConcatenate6" + SrcPort 1 + DstBlock "CMM" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "CMM_base" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "jointPos" + SrcPort 1 + Points [0, 45] + Branch { + ZOrder 167 + Points [0, 110] + DstBlock "CMM_joints" + DstPort 2 + } + Branch { + ZOrder 166 + DstBlock "CMM_base" + DstPort 2 + } + } + Line { + ZOrder 16 + SrcBlock "w_H_b" + SrcPort 1 + Points [0, 25] + Branch { + ZOrder 165 + Points [0, 100] + DstBlock "CMM_joints" + DstPort 1 + } + Branch { + ZOrder 164 + DstBlock "CMM_base" + DstPort 1 + } + } + Line { + ZOrder 33 + SrcBlock "ROBOT_DOF" + SrcPort 1 + Points [0, 65] + Branch { + ZOrder 172 + Points [0, 120] + Branch { + ZOrder 170 + DstBlock "CMM_joints" + DstPort 3 + } + Branch { + ZOrder 169 + Points [0, 30] + DstBlock "Identity Matrix" + DstPort 1 + } + } + Branch { + ZOrder 103 + DstBlock "Vector of Zeros" + DstPort 1 + } + } + Line { + ZOrder 105 + SrcBlock "Vector of Zeros" + SrcPort 1 + DstBlock "CMM_base" + DstPort 3 + } + Line { + ZOrder 162 + SrcBlock "CMM_joints" + SrcPort 1 + DstBlock "Matrix\nConcatenate6" + DstPort 2 + } + Line { + ZOrder 163 + SrcBlock "Identity Matrix" + SrcPort 1 + DstBlock "CMM_joints" + DstPort 4 + } + } + } + } +} +#Finite State Machines +# +# Stateflow 80000014 +# +# +Stateflow { + machine { + id 1 + name "CMM_iCub_23_25_DoFs" + created "28-Feb-2019 16:30:20" + isLibrary 0 + sfVersion 80000012 + firstTarget 81 + } + chart { + id 2 + machine 1 + name "CentroidalMomentum Matrix iCub 23//25 DoFs/Identity Matrix" + 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 "generateSelectorMatrix" + } + 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 SelMatrix = generateSelectorMatrix(ROBOT_DOF)\n\n if ROBOT_DOF > 23\n \n " + " SelMatrix = eye(ROBOT_DOF);\n else\n SelMatrix = blkdiag(eye(ROBOT_DOF), zeros(2));\n end\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 4 + ssIdNumber 4 + name "ROBOT_DOF" + 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 "SelMatrix" + 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 "CentroidalMomentum Matrix iCub 23//25 DoFs/Identity Matrix" + chart 2 + } + chart { + id 9 + machine 1 + name "CentroidalMomentum Matrix iCub 23//25 DoFs/Vector of Zeros" + 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 "generateVectorOfZeros" + } + 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 v_0 = generateVectorOfZeros(ROBOT_DOF)\n\n v_0 = zeros(ROBOT_DOF,1);\nend" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 11 + ssIdNumber 4 + name "ROBOT_DOF" + 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 "v_0" + 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 "CentroidalMomentum Matrix iCub 23//25 DoFs/Vector of Zeros" + chart 9 + } + chart { + id 16 + machine 1 + name "CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/MATLAB Function" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 17 0 0] + viewObj 16 + ssIdHighWaterMark 11 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 3 + disableImplicitCasting 1 + eml { + name "selectFiveMatrixColumns" + } + 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 [v1,v2,v3,v4,v5] = selectFiveMatrixColumns(SelMatrix,groupNumber,ROBOT_DOF)\n\n v1 = " + "SelMatrix((groupNumber-1)*5 + 1,1:ROBOT_DOF);\n v2 = SelMatrix((groupNumber-1)*5 + 2,1:ROBOT_DOF);\n v3 = " + "SelMatrix((groupNumber-1)*5 + 3,1:ROBOT_DOF);\n v4 = SelMatrix((groupNumber-1)*5 + 4,1:ROBOT_DOF);\n v5 = " + "SelMatrix((groupNumber-1)*5 + 5,1:ROBOT_DOF);\nend\n" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 18 + ssIdNumber 4 + name "SelMatrix" + 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 [16 0 19] + } + data { + id 19 + ssIdNumber 5 + name "v1" + 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 [16 18 20] + } + data { + id 20 + ssIdNumber 6 + name "groupNumber" + 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 7 + name "v2" + 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 20 22] + } + data { + id 22 + ssIdNumber 8 + name "v3" + 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 21 23] + } + data { + id 23 + ssIdNumber 9 + name "v4" + 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 22 24] + } + data { + id 24 + ssIdNumber 10 + name "v5" + 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 11 + name "ROBOT_DOF" + 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 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 "CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 1/MATLAB Function" + chart 16 + } + chart { + id 29 + machine 1 + name "CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/MATLAB Function" + 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 11 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 4 + disableImplicitCasting 1 + eml { + name "selectFiveMatrixColumns" + } + firstData 31 + firstTransition 40 + firstJunction 39 + } + 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 [v1,v2,v3,v4,v5] = selectFiveMatrixColumns(SelMatrix,groupNumber,ROBOT_DOF)\n\n v1 = " + "SelMatrix((groupNumber-1)*5 + 1,1:ROBOT_DOF);\n v2 = SelMatrix((groupNumber-1)*5 + 2,1:ROBOT_DOF);\n v3 = " + "SelMatrix((groupNumber-1)*5 + 3,1:ROBOT_DOF);\n v4 = SelMatrix((groupNumber-1)*5 + 4,1:ROBOT_DOF);\n v5 = " + "SelMatrix((groupNumber-1)*5 + 5,1:ROBOT_DOF);\nend\n" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 31 + ssIdNumber 4 + name "SelMatrix" + 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 "v1" + 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 33] + } + data { + id 33 + ssIdNumber 6 + name "groupNumber" + 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 [29 32 34] + } + data { + id 34 + ssIdNumber 7 + name "v2" + 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 [29 33 35] + } + data { + id 35 + ssIdNumber 8 + name "v3" + 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 [29 34 36] + } + data { + id 36 + ssIdNumber 9 + name "v4" + 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 [29 35 37] + } + data { + id 37 + ssIdNumber 10 + name "v5" + 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 [29 36 38] + } + data { + id 38 + ssIdNumber 11 + name "ROBOT_DOF" + 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 [29 37 0] + } + junction { + id 39 + position [23.5747 49.5747 7] + chart 29 + subviewer 29 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [29 0 0] + } + transition { + id 40 + 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 39 + 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 41 + machine 1 + name "CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 2/MATLAB Function" + chart 29 + } + chart { + id 42 + machine 1 + name "CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/MATLAB Function" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 43 0 0] + viewObj 42 + ssIdHighWaterMark 11 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 5 + disableImplicitCasting 1 + eml { + name "selectFiveMatrixColumns" + } + firstData 44 + firstTransition 53 + firstJunction 52 + } + state { + id 43 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 42 + treeNode [42 0 0 0] + superState SUBCHART + subviewer 42 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function [v1,v2,v3,v4,v5] = selectFiveMatrixColumns(SelMatrix,groupNumber,ROBOT_DOF)\n\n v1 = " + "SelMatrix((groupNumber-1)*5 + 1,1:ROBOT_DOF);\n v2 = SelMatrix((groupNumber-1)*5 + 2,1:ROBOT_DOF);\n v3 = " + "SelMatrix((groupNumber-1)*5 + 3,1:ROBOT_DOF);\n v4 = SelMatrix((groupNumber-1)*5 + 4,1:ROBOT_DOF);\n v5 = " + "SelMatrix((groupNumber-1)*5 + 5,1:ROBOT_DOF);\nend\n" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 44 + ssIdNumber 4 + name "SelMatrix" + 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 [42 0 45] + } + data { + id 45 + ssIdNumber 5 + name "v1" + 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 [42 44 46] + } + data { + id 46 + ssIdNumber 6 + name "groupNumber" + 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 [42 45 47] + } + data { + id 47 + ssIdNumber 7 + name "v2" + 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 [42 46 48] + } + data { + id 48 + ssIdNumber 8 + name "v3" + 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 [42 47 49] + } + data { + id 49 + ssIdNumber 9 + name "v4" + 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 [42 48 50] + } + data { + id 50 + ssIdNumber 10 + name "v5" + 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 [42 49 51] + } + data { + id 51 + ssIdNumber 11 + name "ROBOT_DOF" + 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 [42 50 0] + } + junction { + id 52 + position [23.5747 49.5747 7] + chart 42 + subviewer 42 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [42 0 0] + } + transition { + id 53 + 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 52 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 42 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 42 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [42 0 0] + } + instance { + id 54 + machine 1 + name "CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 3/MATLAB Function" + chart 42 + } + chart { + id 55 + machine 1 + name "CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/MATLAB Function" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 56 0 0] + viewObj 55 + ssIdHighWaterMark 11 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 6 + disableImplicitCasting 1 + eml { + name "selectFiveMatrixColumns" + } + firstData 57 + firstTransition 66 + firstJunction 65 + } + state { + id 56 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 55 + treeNode [55 0 0 0] + superState SUBCHART + subviewer 55 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function [v1,v2,v3,v4,v5] = selectFiveMatrixColumns(SelMatrix,groupNumber,ROBOT_DOF)\n\n v1 = " + "SelMatrix((groupNumber-1)*5 + 1,1:ROBOT_DOF);\n v2 = SelMatrix((groupNumber-1)*5 + 2,1:ROBOT_DOF);\n v3 = " + "SelMatrix((groupNumber-1)*5 + 3,1:ROBOT_DOF);\n v4 = SelMatrix((groupNumber-1)*5 + 4,1:ROBOT_DOF);\n v5 = " + "SelMatrix((groupNumber-1)*5 + 5,1:ROBOT_DOF);\nend\n" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 57 + ssIdNumber 4 + name "SelMatrix" + 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 [55 0 58] + } + data { + id 58 + ssIdNumber 5 + name "v1" + 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 [55 57 59] + } + data { + id 59 + ssIdNumber 6 + name "groupNumber" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 58 60] + } + data { + id 60 + ssIdNumber 7 + name "v2" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 59 61] + } + data { + id 61 + ssIdNumber 8 + name "v3" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 60 62] + } + data { + id 62 + ssIdNumber 9 + name "v4" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 61 63] + } + data { + id 63 + ssIdNumber 10 + name "v5" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 62 64] + } + data { + id 64 + ssIdNumber 11 + name "ROBOT_DOF" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [55 63 0] + } + junction { + id 65 + position [23.5747 49.5747 7] + chart 55 + subviewer 55 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [55 0 0] + } + transition { + id 66 + labelString "{eML_blk_kernel();}" + labelPosition [28.125 13.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 65 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 55 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 55 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [55 0 0] + } + instance { + id 67 + machine 1 + name "CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 4/MATLAB Function" + chart 55 + } + chart { + id 68 + machine 1 + name "CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5 (SPECIAL)/MATLAB Function" + windowPosition [422 539.941 189 413] + viewLimits [0 156.75 0 153.75] + screen [1 1 3600 1200 1.180555555555556] + treeNode [0 69 0 0] + viewObj 68 + ssIdHighWaterMark 11 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 7 + disableImplicitCasting 1 + eml { + name "selectFiveMatrixColumns" + } + firstData 70 + firstTransition 79 + firstJunction 78 + } + state { + id 69 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 68 + treeNode [68 0 0 0] + superState SUBCHART + subviewer 68 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function [v1,v2,v3,v4,v5] = selectFiveMatrixColumns(SelMatrix,groupNumber,ROBOT_DOF)\n\n v1 = " + "SelMatrix((groupNumber-1)*5 + 1,1:ROBOT_DOF);\n v2 = SelMatrix((groupNumber-1)*5 + 2,1:ROBOT_DOF);\n v3 = " + "SelMatrix((groupNumber-1)*5 + 3,1:ROBOT_DOF);\n v4 = SelMatrix((groupNumber-1)*5 + 4,1:ROBOT_DOF);\n v5 = " + "SelMatrix((groupNumber-1)*5 + 5,1:ROBOT_DOF);\nend\n" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 70 + ssIdNumber 4 + name "SelMatrix" + 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 [68 0 71] + } + data { + id 71 + ssIdNumber 5 + name "v1" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [68 70 72] + } + data { + id 72 + ssIdNumber 6 + name "groupNumber" + 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 [68 71 73] + } + data { + id 73 + ssIdNumber 7 + name "v2" + 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 [68 72 74] + } + data { + id 74 + ssIdNumber 8 + name "v3" + 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 [68 73 75] + } + data { + id 75 + ssIdNumber 9 + name "v4" + 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 [68 74 76] + } + data { + id 76 + ssIdNumber 10 + name "v5" + 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 [68 75 77] + } + data { + id 77 + ssIdNumber 11 + name "ROBOT_DOF" + 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 [68 76 0] + } + junction { + id 78 + position [23.5747 49.5747 7] + chart 68 + subviewer 68 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [68 0 0] + } + transition { + id 79 + 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 78 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 68 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 68 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [68 0 0] + } + instance { + id 80 + machine 1 + name "CentroidalMomentum Matrix iCub 23//25 DoFs/CMM_joints/CMM Joints Group 5 (SPECIAL)/MATLAB Function" + chart 68 + } + target { + id 81 + machine 1 + name "sfun" + description "Default Simulink S-Function Target." + linkNode [1 0 0] + } +} diff --git a/torque-controllers/CMakeLists.txt b/torque-controllers/CMakeLists.txt deleted file mode 100644 index 1bc1398..0000000 --- a/torque-controllers/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -add_subdirectory(momentum-based-yoga) diff --git a/torque-controllers/README.md b/torque-controllers/README.md deleted file mode 100644 index 67b1e33..0000000 --- a/torque-controllers/README.md +++ /dev/null @@ -1,14 +0,0 @@ -# Controllers - -Simulink torque controllers for balancing and walking of humanoid robots. - -## List of available controllers: - -- **impedance-control** [[README]](impedance-control/README.md) -- **momentum-based-standup** [[README]](momentum-based-standup/README.md) -- **momentum-based-yoga** [[README]](momentum-based-yoga/README.md) - -## Usage - -See corresponding READMEs, and check the [documentation](https://github.com/robotology-playground/whole-body-controllers/tree/master/doc) of the repo. - diff --git a/torque-controllers/impedance-control/README.md b/torque-controllers/impedance-control/README.md deleted file mode 100644 index e79f1b2..0000000 --- a/torque-controllers/impedance-control/README.md +++ /dev/null @@ -1,23 +0,0 @@ -## 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. - -### Compatibility - -The repository contains the Simulink model `impedanceControl.mdl`, which is generated by using Matlab R2016b. - -### Supported robots - -Currently, supported robots are: `iCubGenova04`, `iCunGenova02`, `icubGazeboSim`, `iCubGazeboV2_5`. - -## Module details - -### Configuration file - -At start, the module calls the initialization file initImpedanceControl.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/torque-controllers/impedance-control/impedanceControl.mdl b/torque-controllers/impedance-control/impedanceControl.mdl deleted file mode 100644 index f65ce88..0000000 --- a/torque-controllers/impedance-control/impedanceControl.mdl +++ /dev/null @@ -1,4751 +0,0 @@ -Model { - Name "impedanceControl" - Version 9.0 - SavedCharacterEncoding "UTF-8" - GraphicalInterface { - NumRootInports 0 - NumRootOutports 0 - ParameterArgumentNames "" - ComputedModelVersion "1.315" - NumModelReferences 0 - NumTestPointedSignals 0 - NumProvidedFunctions 0 - NumRequiredFunctions 0 - NumResetEvents 0 - HasInitializeEvent 0 - HasTerminateEvent 0 - IsExportFunctionModel 0 - NumParameterArguments 0 - NumExternalFileReferences 12 - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Configuration" - Path "impedanceControl/Configuration" - SID "432" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "impedanceControl/Get Measurement" - SID "431" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Get Gravity Forces" - Path "impedanceControl/Gravity compensation/Get Gravity Forces" - SID "430" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "impedanceControl/PID feedback controller/Get Measurement" - SID "433" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Mass Matrix" - Path "impedanceControl/PID feedback controller/references/Mass Matrix1" - SID "391" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "impedanceControl/PID feedback controller/references/Visualization/Get Measurement" - SID "450" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "impedanceControl/PID feedback controller/references/holder" - SID "398" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyActuators/Set References" - Path "impedanceControl/Set References" - SID "137" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "impedanceControl/Visualization/Get Measurement" - SID "434" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Limits" - Path "impedanceControl/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/Get Limits" - SID "478" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Simulator Synchronizer" - Path "impedanceControl/synchronizer/GAZEBO_SYNCHRONIZER/Simulator Synchronizer" - SID "304" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Real Time Synchronizer" - Path "impedanceControl/synchronizer/REAL_TIME_SYNC/Real Time Synchronizer" - SID "309" - 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" - ScopeRefreshTime 0.035000 - OverrideScopeRefreshTime on - DisableAllScopes off - DataTypeOverride "UseLocalSettings" - DataTypeOverrideAppliesTo "AllNumericTypes" - MinMaxOverflowLogging "UseLocalSettings" - MinMaxOverflowArchiveMode "Overwrite" - FPTRunName "Run 1" - MaxMDLFileLineLength 120 - InitFcn "cd(fileparts(which(bdroot)));\ninitImpedanceControl;" - StopFcn "cd(fileparts(which(bdroot)));\nstopImpedanceControl;" - LastSavedArchitecture "glnxa64" - Object { - $PropName "BdWindowsInfo" - $ObjectID 1 - $ClassName "Simulink.BDWindowsInfo" - Object { - $PropName "WindowsInfo" - $ObjectID 2 - $ClassName "Simulink.WindowInfo" - IsActive [1] - Location [663.0, 393.0, 2495.0, 1444.0] - Object { - $PropName "ModelBrowserInfo" - $ObjectID 3 - $ClassName "Simulink.ModelBrowserInfo" - Visible [0] - DockPosition "Left" - Width [50] - Height [50] - Filter [9] - } - Object { - $PropName "ExplorerBarInfo" - $ObjectID 4 - $ClassName "Simulink.ExplorerBarInfo" - Visible [1] - } - Object { - $PropName "EditorsInfo" - $ObjectID 5 - $ClassName "Simulink.EditorInfo" - IsActive [1] - ViewObjType "SimulinkSubsys" - LoadSaveID "481" - Extents [2457.0, 1144.0] - ZoomFactor [4.0] - Offset [-127.25, -72.25] - } - Object { - $PropName "DockComponentsInfo" - $ObjectID 6 - $ClassName "Simulink.DockComponentInfo" - Type "GLUE2:PropertyInspector" - ID "Property Inspector" - Visible [0] - CreateCallback "" - UserData "" - Floating [0] - DockPosition "Right" - Width [640] - Height [480] - } - WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" - "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAC6AP///wAAAAEAAAAAAAAAAPwCA" - "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAAOgD///8AAAm/AAAE1AAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACe/////wAAAAAAAAAA/////wEAAAEM/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGq/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAASj/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" - } - } - HideAutomaticNames on - Created "Wed Nov 19 19:38:03 2014" - Creator "daniele" - UpdateHistory "UpdateHistoryNever" - ModifiedByFormat "%" - LastModifiedBy "gnava" - ModifiedDateFormat "%" - LastModifiedDate "Mon Dec 17 19:11:17 2018" - RTWModifiedTimeStamp 466974677 - ModelVersionFormat "1.%" - SampleTimeColors off - SampleTimeAnnotations off - LibraryLinkDisplay "disabled" - WideLines off - ShowLineDimensions off - 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 off - FunctionConnectors off - BrowserLookUnderMasks off - SimulationMode "normal" - VisualizeLoggedSignalsWhenLoggingToFile off - 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 7 - $ClassName "Simulink.SimulationData.ModelLoggingInfo" - model_ "impedanceControl" - overrideMode_ [0.0] - Array { - Type "Cell" - Dimension 1 - Cell "impedanceControl" - 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 8 - Version "1.17.1" - DisabledProps [] - Description "" - Array { - Type "Handle" - Dimension 9 - Simulink.SolverCC { - $ObjectID 9 - Version "1.17.1" - DisabledProps [] - Description "" - StartTime "0.0" - StopTime "Config.SIMULATION_TIME" - AbsTol "auto" - FixedStep "Config.Ts" - 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 10 - Version "1.17.1" - DisabledProps [] - Description "" - Decimation "1" - ExternalInput "[t, u]" - FinalStateName "xFinal" - InitialState "xInitial" - LimitDataPoints on - MaxDataPoints "1000" - LoadExternalInput off - LoadInitialState off - SaveFinalState off - SaveCompleteFinalSimState off - SaveFormat "Array" - SignalLoggingSaveFormat "ModelDataLogs" - 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 11 - Version "1.17.1" - Array { - Type "Cell" - Dimension 8 - Cell "BooleansAsBitfields" - Cell "PassReuseOutputArgsAs" - Cell "PassReuseOutputArgsThreshold" - Cell "ZeroExternalMemoryAtStartup" - Cell "ZeroInternalMemoryAtStartup" - Cell "OptimizeModelRefInitCode" - Cell "NoFixptDivByZeroProtection" - Cell "UseSpecifiedMinMax" - PropName "DisabledProps" - } - Description "" - 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 128 - 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 12 - Version "1.17.1" - Array { - Type "Cell" - Dimension 1 - Cell "UseOnlyExistingSharedCode" - PropName "DisabledProps" - } - Description "" - 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 "Simplified" - MergeDetectMultiDrivingBlocksExec "error" - CheckExecutionContextPreStartOutputMsg off - CheckExecutionContextRuntimeOutputMsg off - SignalResolutionControl "UseLocalSettings" - BlockPriorityViolationMsg "warning" - MinStepSizeMsg "warning" - TimeAdjustmentMsg "none" - MaxConsecutiveZCsMsg "error" - MaskedZcDiagnostic "warning" - IgnoredZcDiagnostic "warning" - SolverPrmCheckMsg "warning" - InheritedTsInSrcMsg "warning" - MultiTaskDSMMsg "warning" - MultiTaskCondExecSysMsg "none" - 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 "error" - 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 "none" - SimStateInterfaceChecksumMismatchMsg "warning" - SimStateOlderReleaseMsg "error" - ChecksumConsistencyForSSReuse "none" - InitInArrayFormatMsg "warning" - StrictBusMsg "ErrorOnBusTreatedAsVector" - 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 13 - Version "1.17.1" - DisabledProps [] - Description "" - 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 14 - Version "1.17.1" - DisabledProps [] - Description "" - 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 15 - Version "1.17.1" - DisabledProps [] - Description "" - SimCustomSourceCode "" - SimCustomHeaderCode "" - SimCustomInitializer "" - SimCustomTerminator "" - SimReservedNameArray [] - SimUserSources "" - SimUserIncludeDirs "" - SimUserLibraries "" - SimUserDefines "" - SimCustomCompilerFlags "" - SimCustomLinkerFlags "" - SFSimEcho on - SimCtrlC on - SimIntegrity on - SimUseLocalCustomCode off - SimParseCustomCode on - SimBuildMode "sf_incremental_build" - SimGenImportedTypeDefs off - ModelFunctionsGlobalVisibility "on" - CompileTimeRecursionLimit 50 - EnableRuntimeRecursion on - MATLABDynamicMemAlloc on - MATLABDynamicMemAllocThreshold 65536 - } - Simulink.RTWCC { - $BackupClass "Simulink.RTWCC" - $ObjectID 16 - 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" - } - SystemTargetFile "grt.tlc" - HardwareBoard "None" - TLCOptions "" - GenCodeOnly off - MakeCommand "make_rtw" - GenerateMakefile on - PackageGeneratedCodeAndArtifacts off - PackageName "" - TemplateMakefile "grt_default_tmf" - PostCodeGenCommand "" - Description "" - GenerateReport off - SaveLog 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 17 - 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 "CustomSymbolStrUtil" - Cell "ReqsInCode" - Cell "CustomSymbolStrModelFcn" - Cell "CustomUserTokenString" - Cell "CustomSymbolStrEmxType" - Cell "CustomSymbolStrEmxFcn" - Cell "BlockCommentType" - PropName "DisabledProps" - } - Description "" - 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 18 - Version "1.17.1" - Array { - Type "Cell" - Dimension 16 - Cell "GeneratePreprocessorConditionals" - Cell "IncludeMdlTerminateFcn" - Cell "SupportNonInlinedSFcns" - Cell "SuppressErrorStatus" - Cell "ERTCustomFileBanners" - Cell "GenerateSampleERTMain" - Cell "GenerateTestInterfaces" - Cell "ModelStepFunctionPrototypeControlCompliant" - Cell "GenerateAllocFcn" - Cell "PurelyIntegerCode" - Cell "SupportComplex" - Cell "SupportAbsoluteTime" - Cell "SupportContinuousTime" - Cell "ExistingSharedCode" - Cell "RemoveDisableFunc" - Cell "RemoveResetFunc" - PropName "DisabledProps" - } - Description "" - TargetFcnLib "ansi_tfl_table_tmw.mat" - TargetLibSuffix "" - TargetPreCompLibLocation "" - GenFloatMathFcnCalls "NOT IN USE" - TargetLangStandard "C89/C90 (ANSI)" - 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 on - MultiInstanceERTCode off - CodeInterfacePackaging "Nonreusable function" - SupportNonFinite on - 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 19 - Version "1.17.1" - DisabledProps [] - Description "Simulink Coverage Configuration Component" - 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" - ExtraOptions "" - CurrentDlgPage "Solver" - ConfigPrmDlgPosition [ 396, 258, 1456, 1010 ] - } - PropName "ConfigurationSets" - } - Simulink.ConfigSet { - $PropName "ActiveConfigurationSet" - $ObjectID 8 - } - Object { - $PropName "DataTransfer" - $ObjectID 20 - $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 { - 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 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 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 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 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 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 off - } - 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 S-Function - FunctionName "system" - SFunctionModules "''" - PortCounts "[]" - } - Block { - BlockType Scope - DefaultConfigurationName "Simulink.scopes.TimeScopeBlockCfg" - } - Block { - BlockType Selector - NumberOfDimensions "1" - IndexMode "One-based" - InputPortWidth "-1" - SampleTime "-1" - } - Block { - BlockType Sin - SineType "Time based" - TimeSource "Use simulation time" - Amplitude "1" - Bias "0" - Frequency "1" - Phase "0" - Samples "10" - Offset "0" - SampleTime "-1" - VectorParams1D on - } - 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 - IsObserver 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 Terminator - } - } - System { - Name "impedanceControl" - Location [663, 393, 3158, 1837] - 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 "293" - ReportName "simulink-default.rpt" - SIDHighWatermark "486" - Block { - BlockType Reference - Name "Configuration" - SID "432" - Ports [] - Position [245, 240, 320, 270] - ZOrder 635 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Configuration" - SourceType "" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - ConfigSource "Workspace" - ConfigObject "'WBTConfigRobot'" - RobotName "'icubSim'" - UrdfFile "'model.urdf'" - ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" - ControlBoardsNames "{'left_arm','right_arm','torso'}" - LocalName "'WBT'" - GravityVector "[0,0,-9.81]" - } - Block { - BlockType Reference - Name "Get Measurement" - SID "431" - Ports [0, 1] - Position [245, 303, 320, 327] - ZOrder 634 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Position" - } - Block { - BlockType SubSystem - Name "Gravity compensation" - SID "226" - Ports [1, 1] - Position [415, 380, 570, 440] - ZOrder -36 - RequestExecContextInheritance off - Port { - PortNumber 1 - Name "gravityTorques" - } - System { - Name "Gravity compensation" - Location [65, 24, 2560, 1440] - 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 "231" - Block { - BlockType Inport - Name "joint angles" - SID "423" - Position [115, 172, 160, 188] - ZOrder 639 - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant4" - SID "425" - Position [105, 122, 170, 138] - ZOrder 637 - ShowName off - Value "eye(4)" - } - Block { - BlockType Reference - Name "Get Gravity Forces" - SID "430" - Ports [2, 1] - Position [210, 105, 350, 205] - ZOrder 641 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Get Gravity Forces" - SourceType "Gravity bias" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - Port { - PortNumber 1 - Name "gravityTorques floating base" - } - } - Block { - BlockType Selector - Name "Selector2" - SID "428" - Ports [1, 1] - Position [500, 136, 540, 174] - ZOrder 634 - ShowName off - InputPortWidth "ROBOT_DOF+6" - IndexOptions "Index vector (dialog)" - Indices "[7:ROBOT_DOF+6]" - OutputSizes "1" - } - Block { - BlockType Outport - Name "gravity torques" - SID "429" - Position [590, 148, 620, 162] - ZOrder 640 - IconDisplay "Port number" - } - Line { - ZOrder 77 - SrcBlock "Selector2" - SrcPort 1 - DstBlock "gravity torques" - DstPort 1 - } - Line { - ZOrder 84 - SrcBlock "Constant4" - SrcPort 1 - DstBlock "Get Gravity Forces" - DstPort 1 - } - Line { - ZOrder 85 - SrcBlock "joint angles" - SrcPort 1 - DstBlock "Get Gravity Forces" - DstPort 2 - } - Line { - Name "gravityTorques floating base" - ZOrder 83 - Labels [1, 1] - SrcBlock "Get Gravity Forces" - SrcPort 1 - DstBlock "Selector2" - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "PID feedback controller" - SID "310" - Ports [1, 1] - Position [410, 286, 570, 344] - ZOrder -41 - RequestExecContextInheritance off - Port { - PortNumber 1 - Name "PD control" - PropagatedSignals "torqueDesired" - } - System { - Name "PID feedback controller" - Location [663, 393, 3158, 1837] - 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 "232" - Block { - BlockType Inport - Name "joint angles" - SID "311" - Position [50, 72, 80, 88] - ZOrder 386 - IconDisplay "Port number" - } - Block { - BlockType Gain - Name "Gain1" - SID "14" - Position [705, 70, 760, 100] - ZOrder 22 - ShowName off - Gain "Kp" - Multiplication "Matrix(K*u)" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - Port { - PortNumber 1 - Name "Kp*(qj_des - qj)" - } - } - Block { - BlockType Gain - Name "Gain2" - SID "20" - Position [705, 140, 760, 170] - ZOrder 26 - ShowName off - Gain "Kd" - Multiplication "Matrix(K*u)" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Reference - Name "Get Measurement" - SID "433" - Ports [0, 1] - Position [335, 158, 405, 172] - ZOrder 635 - BackgroundColor "[0.513700, 0.674500, 1.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Velocity" - } - Block { - BlockType Sum - Name "Sum" - SID "13" - Ports [2, 1] - Position [475, 69, 500, 111] - ZOrder 21 - ShowName off - Inputs "+-" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Sum - Name "Sum1" - SID "19" - Ports [3, 1] - Position [880, -22, 925, 192] - ZOrder 25 - ShowName off - Inputs "+++" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - Port { - PortNumber 1 - Name "torqueDesired" - } - } - Block { - BlockType Sum - Name "Sum3" - SID "97" - Ports [2, 1] - Position [475, 134, 500, 176] - ZOrder 174 - ShowName off - Inputs "+-" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType SubSystem - Name "references" - SID "88" - Ports [1, 3] - Position [170, -18, 265, 178] - ZOrder 173 - ShowName off - RequestExecContextInheritance off - Port { - PortNumber 1 - Name "Mj*qjDD_des" - } - System { - Name "references" - Location [663, 393, 3158, 1837] - 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 "200" - Block { - BlockType Inport - Name "qj" - SID "384" - Position [-200, -212, -170, -198] - ZOrder 666 - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant1" - SID "385" - Position [10, -255, 40, -225] - ZOrder 674 - NamePlacement "alternate" - Value "eye(4)" - } - Block { - BlockType Gain - Name "Gain1" - SID "420" - Position [155, -425, 210, -395] - ZOrder 675 - ShowName off - Gain "pi/180" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Gain - Name "Gain2" - SID "421" - Position [155, -485, 210, -455] - ZOrder 676 - ShowName off - Gain "pi/180" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Gain - Name "Gain5" - SID "386" - Position [155, -315, 210, -285] - ZOrder 669 - ShowName off - Gain "pi/180" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Reference - Name "Mass Matrix1" - SID "391" - Ports [2, 1] - Position [95, -256, 235, -189] - ZOrder 673 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Mass Matrix" - SourceType "Mass Matrix" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Product - Name "Product1" - SID "392" - Ports [2, 1] - Position [370, -228, 430, -197] - ZOrder 665 - ShowName off - Multiplication "Matrix(*)" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - RndMeth "Floor" - SaturateOnIntegerOverflow off - } - Block { - BlockType Selector - Name "Selector" - SID "422" - Ports [1, 1] - Position [270, -239, 310, -201] - ZOrder 677 - NumberOfDimensions "2" - InputPortWidth "3" - IndexOptions "Index vector (dialog),Index vector (dialog)" - Indices "[7:ROBOT_DOF+6],[7:ROBOT_DOF+6]" - OutputSizes "1,1" - } - Block { - BlockType Sum - Name "Sum1" - SID "396" - Ports [2, 1] - Position [230, -545, 250, -525] - ZOrder 670 - ShowName off - IconShape "round" - Inputs "|++" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType SubSystem - Name "Visualization" - SID "443" - Ports [1] - Position [450, -503, 505, -467] - ZOrder 678 - RequestExecContextInheritance off - System { - Name "Visualization" - Location [663, 393, 3158, 1837] - 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 "299" - Block { - BlockType Inport - Name "qjDesired" - SID "444" - Position [130, 447, 160, 463] - ZOrder 629 - IconDisplay "Port number" - Port { - PortNumber 1 - Name "qjDesired" - } - } - Block { - BlockType Demux - Name "Demux1" - SID "467" - Ports [1, 5] - Position [550, 121, 555, 219] - ZOrder 665 - 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 "468" - Ports [1, 5] - Position [550, 256, 555, 354] - ZOrder 666 - 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 "469" - Ports [1, 5] - Position [550, 406, 555, 504] - ZOrder 667 - 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 Gain - Name "Gain" - SID "456" - Position [340, 155, 385, 185] - ZOrder 662 - Gain "180/pi" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Gain - Name "Gain1" - SID "457" - Position [340, 290, 385, 320] - ZOrder 663 - Gain "180/pi" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Gain - Name "Gain2" - SID "458" - Position [340, 440, 385, 470] - ZOrder 664 - Gain "180/pi" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Reference - Name "Get Measurement" - SID "450" - Ports [0, 1] - Position [115, 158, 180, 182] - ZOrder 659 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Position" - Port { - PortNumber 1 - Name "qjMeasured" - } - } - Block { - BlockType Sum - Name "Sum2" - SID "451" - Ports [2, 1] - Position [245, 295, 265, 315] - ZOrder 147 - ShowName off - IconShape "round" - Inputs "+|-" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - Port { - PortNumber 1 - Name "qjError" - } - } - Block { - BlockType Scope - Name "qjDes" - SID "453" - Ports [5] - Position [655, 400, 760, 510] - ZOrder 655 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," - "'DataLoggingVariableName','jointDesiredData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','" - "1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct(" - "'MinYLimReal','-19.45498','MaxYLimReal','10.88562','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','19.45498'," - "'LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862" - "74509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294" - "11764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058" - "823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'" - ",'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" - "ruct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'" - "Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNa" - "mes',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-64.64255','MaxY" - "LimReal','67.27378','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid" - "',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980" - "3922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843137" - "2549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352941176" - "47 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 " - "1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'" - "ShowContent',true,'Placement',2),struct('MinYLimReal','-62.0429','MaxYLimReal','65.68488','YLabelReal','','MinYLim" - "Mag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0" - " 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0" - ".0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0" - "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker" - "','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" - "ruct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'" - "Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('Min" - "YLimReal','-10.72907','MaxYLimReal','20.79257','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibilit" - "y','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.4" - "11764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098" - "03921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperties" - "Cache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 " - "0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineW" - "idth',0.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('Colo" - "r',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLi" - "nes',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-10.51827','MaxYLimReal','18.840" - "43','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagP" - "hase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrde" - "r',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862" - "745098 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],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" - "truct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',tru" - "e,'Placement',5),struct('MinYLimReal','1.00000','MaxYLimReal','3.00000','YLabelReal','','MinYLimMag','0','MaxYLimM" - "ag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor" - "',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 " - "0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0." - "717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','Lin" - "ePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" - "truct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames'," - "{{}},'NumLines',0,'LineNames',{{}},'ShowContent',false,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelRea" - "l','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 " - "1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098" - " 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07450980392" - "15686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','" - "LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Pla" - "cement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('T" - "ools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measuremen" - "ts',true,'Version','2017b')),'Version','2018a','Location',[353 355 1643 1076])" - NumInputPorts "5" - Floating off - } - Block { - BlockType Scope - Name "qjError" - SID "454" - Ports [5] - Position [655, 250, 760, 360] - ZOrder 653 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," - "'DataLoggingVariableName','jointErrorData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1'" - ",'DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('M" - "inYLimReal','-2.58811','MaxYLimReal','2.46722','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','2.58811','Lege" - "ndVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509" - "803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764" - "706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882352" - "9 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Lin" - "ePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" - "truct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames'," - "{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-2.05965','MaxYLimRea" - "l','4.6546','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'" - "PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'C" - "olorOrder',[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],'Li" - "neStyle','-','LineWidth',0.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','-','LineWid" - "th',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowCont" - "ent',true,'Placement',2),struct('MinYLimReal','-1.80349','MaxYLimReal','4.43083','YLabelReal','','MinYLimMag','0'," - "'MaxYLimMag','10','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.07450980" - "39215686 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],'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('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,'Marker','" - "none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Us" - "erDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal'" - ",'-9.48486','MaxYLimReal','9.48486','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XG" - "rid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098039" - "22 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882" - "353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;" - "0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{st" - "ruct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'" - "Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1]," - "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'Lin" - "eNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-9.67523','MaxYLimReal','9.4403','YLabelRea" - "l','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'A" - "xesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666" - "666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137" - "2549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0." - "650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidt" - "h',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" - "ible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," - "[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','Li" - "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none" - "','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'," - "5),struct('MinYLimReal','1.00000','MaxYLimReal','3.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Lege" - "ndVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509" - "803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764" - "706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882352" - "9 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','','LinePropertiesCac" - "he',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidt" - "h',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" - "ible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," - "[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines" - "',0,'LineNames',{{}},'ShowContent',false,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesCol" - "or',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666" - "666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137254901" - "9608 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','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','Vis" - "ible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'Ti" - "meRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Na" - "vigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Vers" - "ion','2017b')),'Version','2018a','Location',[373 384 1613 955])" - NumInputPorts "5" - Floating off - } - Block { - BlockType Scope - Name "qjMeas" - SID "455" - Ports [5] - Position [655, 115, 760, 225] - ZOrder 651 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," - "'DataLoggingVariableName','jointData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','Dat" - "aLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLi" - "mReal','-0.13911','MaxYLimReal','0.15522','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','0.15522','LegendVis" - "ibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980392" - "2 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.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','%','LineProp" - "ertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[1 0 1],'LineStyle','-','LineWidth',0.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','n" - "one','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct" - "('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}}," - "'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-1.04571','MaxYLimReal','1" - ".32633','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'Plot" - "MagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Color" - "Order',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.39215" - "6862745098 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],'LineSt" - "yle','-','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','-','LineWidth'," - "0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent'" - ",true,'Placement',2),struct('MinYLimReal','-1.03984','MaxYLimReal','1.33197','YLabelReal','','MinYLimMag','0','Max" - "YLimMag','10','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.074509803921" - "5686 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','Vis" - "ible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.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','none" - "','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDe" - "finedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-0" - ".10994','MaxYLimReal','0.25318','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.05" - "88235294117647 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,'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],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNam" - "es',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-0.10165','MaxYLimReal','0.23223','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.0666666" - "666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137254" - "9019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650" - "980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth'," - "0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" - "e','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 " - "0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5)," - "struct('MinYLimReal','1.00000','MaxYLimReal','3.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendV" - "isibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803" - "922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.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','Visibl" - "e','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " - "0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0" - ",'LineNames',{{}},'ShowContent',false,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor'" - ",[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666" - "667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137254901960" - "8 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392" - "156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'" - "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth'," - "0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" - "e','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeR" - "angeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navig" - "ation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version" - "','2017b')),'Version','2018a','Location',[76 213 1356 904])" - NumInputPorts "5" - Floating off - } - Line { - Name "qjMeasured" - ZOrder 2 - SrcBlock "Get Measurement" - SrcPort 1 - Points [70, 0] - Branch { - ZOrder 3 - DstBlock "Sum2" - DstPort 1 - } - Branch { - ZOrder 4 - Labels [1, 1] - DstBlock "Gain" - DstPort 1 - } - } - Line { - Name "torso" - ZOrder 48 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 1 - DstBlock "qjMeas" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 47 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 2 - DstBlock "qjMeas" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 45 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 4 - DstBlock "qjMeas" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 44 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 3 - DstBlock "qjMeas" - DstPort 3 - } - Line { - Name "right_leg" - ZOrder 46 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 5 - DstBlock "qjMeas" - DstPort 5 - } - Line { - Name "torso" - ZOrder 40 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 1 - DstBlock "qjError" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 37 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 2 - DstBlock "qjError" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 38 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 4 - DstBlock "qjError" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 39 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 3 - DstBlock "qjError" - DstPort 3 - } - Line { - Name "right_leg" - ZOrder 41 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 5 - DstBlock "qjError" - DstPort 5 - } - Line { - Name "qjError" - ZOrder 15 - Labels [1, 1] - SrcBlock "Sum2" - SrcPort 1 - DstBlock "Gain1" - DstPort 1 - } - Line { - Name "torso" - ZOrder 36 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 1 - DstBlock "qjDes" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 35 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 2 - DstBlock "qjDes" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 34 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 4 - DstBlock "qjDes" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 32 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 3 - DstBlock "qjDes" - DstPort 3 - } - Line { - Name "qjDesired" - ZOrder 20 - SrcBlock "qjDesired" - SrcPort 1 - Points [90, 0] - Branch { - ZOrder 21 - Labels [1, 1] - DstBlock "Gain2" - DstPort 1 - } - Branch { - ZOrder 22 - DstBlock "Sum2" - DstPort 2 - } - } - Line { - Name "right_leg" - ZOrder 33 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 5 - DstBlock "qjDes" - DstPort 5 - } - Line { - ZOrder 49 - SrcBlock "Gain" - SrcPort 1 - DstBlock "Demux1" - DstPort 1 - } - Line { - ZOrder 50 - SrcBlock "Gain1" - SrcPort 1 - DstBlock "Demux2" - DstPort 1 - } - Line { - ZOrder 51 - SrcBlock "Gain2" - SrcPort 1 - DstBlock "Demux3" - DstPort 1 - } - } - } - Block { - BlockType Scope - Name "Visualize References" - SID "399" - Ports [3] - Position [380, -403, 420, -347] - ZOrder 668 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(ext" - "mgr.Configuration('Core','General UI',true),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration(" - "'Sources','WiredSimulink',true,'DataLoggingVariableName','references','DataLoggingSaveFormat','StructureWithTim" - "e','DataLoggingDecimation','1','DataLoggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals" - "','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-61.68503','MaxYLimReal','61.68503','YLabelRea" - "l','','MinYLimMag','0.00000','MaxYLimMag','61.68503','LegendVisibility','Off','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',{{}},'UserDefinedChann" - "elNames',{{}},'NumLines',69,'LineNames',{{'qj Ref:1','qj Ref:2','qj Ref:3','qj Ref:4','qj Ref:5','qj Ref:6','qj" - " Ref:7','qj Ref:8','qj Ref:9','qj Ref:10','qj Ref:11','qj Ref:12','qj Ref:13','qj Ref:14','qj Ref:15','qj Ref:1" - "6','qj Ref:17','qj Ref:18','qj Ref:19','qj Ref:20','qj Ref:21','qj Ref:22','qj Ref:23','qjDot Ref:1','qjDot Ref" - ":2','qjDot Ref:3','qjDot Ref:4','qjDot Ref:5','qjDot Ref:6','qjDot Ref:7','qjDot Ref:8','qjDot Ref:9','qjDot Re" - "f:10','qjDot Ref:11','qjDot Ref:12','qjDot Ref:13','qjDot Ref:14','qjDot Ref:15','qjDot Ref:16','qjDot Ref:17'," - "'qjDot Ref:18','qjDot Ref:19','qjDot Ref:20','qjDot Ref:21','qjDot Ref:22','qjDot Ref:23','qjDDot Ref:1','qjDDo" - "t Ref:2','qjDDot Ref:3','qjDDot Ref:4','qjDDot Ref:5','qjDDot Ref:6','qjDDot Ref:7','qjDDot Ref:8','qjDDot Ref:" - "9','qjDDot Ref:10','qjDDot Ref:11','qjDDot Ref:12','qjDDot Ref:13','qjDDot Ref:14','qjDDot Ref:15','qjDDot Ref:" - "16','qjDDot Ref:17','qjDDot Ref:18','qjDDot Ref:19','qjDDot Ref:20','qjDDot Ref:21','qjDDot Ref:22','qjDDot Ref" - ":23'}},'ShowContent',true,'Placement',1)},'DisplayPropertyDefaults',struct('MinYLimReal','-61.68503','MaxYLimRe" - "al','61.68503','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','61.68503','LegendVisibility','Off','XGrid'," - "true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922" - " 0.686274509803922],'ColorOrder',[0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;1" - " 1 0.0666666666666667;0.717647058823529 0.274509803921569 1;0.392156862745098 0.831372549019608 0.0745098039215" - "686;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863])),extmgr.Configuration('Tools','Plot Navigat" - "ion',true,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2016b')),'Versi" - "on','2018a','Position',[675 241 570 450])" - NumInputPorts "3" - Floating off - } - Block { - BlockType Reference - Name "holder" - SID "398" - Ports [1, 1] - Position [-65, -546, -10, -524] - ZOrder 672 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Sin - Name "qj Ref" - SID "393" - Ports [0, 1] - Position [-95, -485, 15, -455] - ZOrder 656 - Amplitude "AMPLS" - Frequency "FREQS*2*pi" - SampleTime "0" - } - Block { - BlockType Sin - Name "qjDDot Ref" - SID "395" - Ports [0, 1] - Position [-95, -370, 15, -340] - ZOrder 662 - Amplitude "-AMPLS.*(FREQS*2*pi).^2" - Frequency "FREQS*2*pi" - SampleTime "0" - } - Block { - BlockType Sin - Name "qjDot Ref" - SID "394" - Ports [0, 1] - Position [-95, -425, 15, -395] - ZOrder 659 - Amplitude "AMPLS.*FREQS*2*pi" - Frequency "FREQS*2*pi" - Phase "pi/2" - SampleTime "0" - } - Block { - BlockType Outport - Name "Mj*qjDDot_des" - SID "400" - Position [480, -217, 510, -203] - ZOrder 663 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name " qj_des" - SID "402" - Position [465, -542, 495, -528] - ZOrder 657 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qjDot_des" - SID "401" - Position [300, -417, 330, -403] - ZOrder 660 - Port "3" - IconDisplay "Port number" - } - Line { - ZOrder 50 - SrcBlock "qj Ref" - SrcPort 1 - Points [10, 0] - Branch { - ZOrder 62 - Points [0, 32; 319, 0; 0, 43] - DstBlock "Visualize References" - DstPort 1 - } - Branch { - ZOrder 61 - DstBlock "Gain2" - DstPort 1 - } - } - Line { - ZOrder 51 - SrcBlock "qjDot Ref" - SrcPort 1 - Points [11, 0] - Branch { - ZOrder 57 - DstBlock "Gain1" - DstPort 1 - } - Branch { - ZOrder 53 - Points [0, 35] - DstBlock "Visualize References" - DstPort 2 - } - } - Line { - ZOrder 36 - SrcBlock "Product1" - SrcPort 1 - DstBlock "Mj*qjDDot_des" - DstPort 1 - } - Line { - ZOrder 67 - SrcBlock "Mass Matrix1" - SrcPort 1 - DstBlock "Selector" - DstPort 1 - } - Line { - ZOrder 68 - SrcBlock "Selector" - SrcPort 1 - DstBlock "Product1" - DstPort 1 - } - Line { - ZOrder 52 - SrcBlock "qjDDot Ref" - SrcPort 1 - Points [11, 0] - Branch { - ZOrder 56 - Points [0, 55] - DstBlock "Gain5" - DstPort 1 - } - Branch { - ZOrder 55 - DstBlock "Visualize References" - DstPort 3 - } - } - Line { - ZOrder 42 - SrcBlock "Gain5" - SrcPort 1 - Points [114, 0; 0, 95] - DstBlock "Product1" - DstPort 2 - } - Line { - ZOrder 45 - SrcBlock "qj" - SrcPort 1 - Points [30, 0] - Branch { - ZOrder 91 - Points [0, -330] - DstBlock "holder" - DstPort 1 - } - Branch { - ZOrder 66 - DstBlock "Mass Matrix1" - DstPort 2 - } - } - Line { - ZOrder 59 - SrcBlock "holder" - SrcPort 1 - DstBlock "Sum1" - DstPort 1 - } - Line { - ZOrder 49 - SrcBlock "Constant1" - SrcPort 1 - DstBlock "Mass Matrix1" - DstPort 1 - } - Line { - ZOrder 58 - SrcBlock "Gain1" - SrcPort 1 - DstBlock "qjDot_des" - DstPort 1 - } - Line { - ZOrder 60 - SrcBlock "Gain2" - SrcPort 1 - Points [25, 0] - DstBlock "Sum1" - DstPort 2 - } - Line { - ZOrder 46 - SrcBlock "Sum1" - SrcPort 1 - Points [123, 0] - Branch { - ZOrder 89 - Points [0, 50] - DstBlock "Visualization" - DstPort 1 - } - Branch { - ZOrder 88 - DstBlock " qj_des" - DstPort 1 - } - } - } - } - Block { - BlockType Outport - Name "tau_fb" - SID "313" - Position [1055, 78, 1085, 92] - ZOrder 388 - IconDisplay "Port number" - } - Line { - ZOrder 341 - SrcBlock "Get Measurement" - SrcPort 1 - DstBlock "Sum3" - DstPort 2 - } - Line { - ZOrder 6 - SrcBlock "Sum" - SrcPort 1 - Points [98, 0; 0, -5] - DstBlock "Gain1" - DstPort 1 - } - Line { - ZOrder 291 - SrcBlock "joint angles" - SrcPort 1 - Points [52, 0] - Branch { - ZOrder 370 - Points [0, 125; 158, 0; 0, -105] - DstBlock "Sum" - DstPort 2 - } - Branch { - ZOrder 331 - DstBlock "references" - DstPort 1 - } - } - Line { - Name "Mj*qjDD_des" - ZOrder 322 - Labels [-1, 0] - SrcBlock "references" - SrcPort 1 - DstBlock "Sum1" - DstPort 1 - } - Line { - ZOrder 48 - SrcBlock "Sum3" - SrcPort 1 - DstBlock "Gain2" - DstPort 1 - } - Line { - ZOrder 323 - SrcBlock "references" - SrcPort 3 - DstBlock "Sum3" - DstPort 1 - } - Line { - Name "torqueDesired" - ZOrder 295 - Labels [1, 0] - SrcBlock "Sum1" - SrcPort 1 - DstBlock "tau_fb" - DstPort 1 - } - Line { - ZOrder 325 - SrcBlock "references" - SrcPort 2 - DstBlock "Sum" - DstPort 1 - } - Line { - Name "Kp*(qj_des - qj)" - ZOrder 369 - Labels [-1, 0] - SrcBlock "Gain1" - SrcPort 1 - DstBlock "Sum1" - DstPort 2 - } - Line { - ZOrder 390 - SrcBlock "Gain2" - SrcPort 1 - DstBlock "Sum1" - DstPort 3 - } - } - } - Block { - BlockType Reference - Name "Set References" - SID "137" - Ports [1] - Position [985, 340, 1060, 390] - ZOrder 383 - BackgroundColor "[0.827500, 0.576500, 0.603900]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyActuators/Set References" - SourceType "Set References" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - controlType "Torque" - refSpeed "50" - refAcceleration "1000000*(pi/180)" - } - Block { - BlockType Sum - Name "Sum1" - SID "314" - Ports [2, 1] - Position [710, 269, 795, 456] - ZOrder 633 - ShowName off - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - Port { - PortNumber 1 - Name "control input" - } - } - Block { - BlockType SubSystem - Name "Visualization" - SID "315" - Ports [2] - Position [970, 443, 1075, 492] - ZOrder -43 - RequestExecContextInheritance off - System { - Name "Visualization" - Location [65, 24, 2560, 1440] - 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 "220" - Block { - BlockType Inport - Name "tauDesired" - SID "318" - Position [130, 447, 160, 463] - ZOrder 629 - IconDisplay "Port number" - Port { - PortNumber 1 - Name "torqueDesired" - PropagatedSignals "control input" - } - } - Block { - BlockType Inport - Name "gravityTorques" - SID "319" - Position [130, 598, 160, 612] - ZOrder 630 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name "Demux1" - SID "463" - Ports [1, 5] - Position [550, 121, 555, 219] - ZOrder 662 - 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 "464" - Ports [1, 5] - Position [550, 256, 555, 354] - ZOrder 663 - 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 "465" - Ports [1, 5] - Position [550, 406, 555, 504] - ZOrder 664 - 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 "Demux4" - SID "466" - Ports [1, 5] - Position [550, 556, 555, 654] - ZOrder 665 - 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 Reference - Name "Get Measurement" - SID "434" - Ports [0, 1] - Position [115, 158, 180, 182] - ZOrder 659 - BackgroundColor "[0.827500, 0.576500, 0.603900]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Torque" - Port { - PortNumber 1 - Name "torqueMeasured" - } - } - Block { - BlockType Sum - Name "Sum2" - SID "49" - Ports [2, 1] - Position [245, 295, 265, 315] - ZOrder 147 - ShowName off - IconShape "round" - Inputs "+|-" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - Port { - PortNumber 1 - Name "torqueError" - } - } - Block { - BlockType Scope - Name "gravTorques" - SID "419" - Ports [5] - Position [655, 550, 760, 660] - ZOrder 657 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr." - "Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),extmg" - "r.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariableNa" - "me','gravTorque','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateData'," - "true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-2.62369','MaxY" - "LimReal','22.48453','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','22.48453','LegendVisibility','On','XGrid'," - "true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6" - "86274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1" - "6078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823" - "5294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Col" - "or',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" - "Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','" - "-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}}" - ",'ShowContent',true,'Placement',1),struct('MinYLimReal','-1.31451','MaxYLimReal','0.36849','YLabelReal','','MinYLim" - "Mag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 " - "0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0" - "745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745" - "098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'" - "Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" - "Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','" - "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker'," - "'none','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('MinYLimReal'," - "'-1.35483','MaxYLimReal','0.37657','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGri" - "d',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 " - "0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 " - "0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058" - "8235294117647 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'),struc" - "t('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('MinYLimReal','-80.67711','MaxYLimReal','54.64406','YLabelReal','','Mi" - "nYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor'," - "[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.066666666666666" - "7;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0" - ".0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6509803921568" - "63],'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'),str" - "uct('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,'Mar" - "ker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}" - "},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimR" - "eal','-80.58848','MaxYLimReal','54.86216','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on" - "','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509" - "803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705" - "882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " - "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],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'Li" - "neStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNam" - "es',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','1.00000','MaxYLimReal','3.00000','YLabelReal',''" - ",'MinYLimMag','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.06666666666" - "66667;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.650980392" - "156863],'Title','','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','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')}},'UserDef" - "inedChannelNames',{{}},'NumLines',0,'LineNames',{{}},'ShowContent',false,'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.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'" - "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0" - ".5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',t" - "rue,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configura" - "tion('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Meas" - "urements',true,'Version','2017b')),'Version','2018a','Location',[76 210 1356 901])" - NumInputPorts "5" - Floating off - } - Block { - BlockType Scope - Name "tauDes" - SID "417" - Ports [5] - Position [655, 400, 760, 510] - ZOrder 655 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr." - "Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),extmg" - "r.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true,'Da" - "taLoggingVariableName','tauDesiredData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','Da" - "taLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLi" - "mReal','-2.62369','MaxYLimReal','22.48453','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','22.48453','LegendVi" - "sibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980392" - "2 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.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','%','LinePropert" - "iesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[" - "1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','Line" - "Width',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" - "',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLine" - "s',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-1.31451','MaxYLimReal','0.36849','" - "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.07450980392156" - "86 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','Line" - "Width',0.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,'Placement',2" - "),struct('MinYLimReal','-1.35483','MaxYLimReal','0.37657','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legen" - "dVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980" - "3922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.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','%','LineProp" - "ertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.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'),struct('Co" - "lor',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumL" - "ines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-80.67711','MaxYLimReal','54.644" - "06','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPh" - "ase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder'" - ",[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745" - "098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509803" - "9215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" - "Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','" - "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker'," - "'none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placeme" - "nt',4),struct('MinYLimReal','-80.58848','MaxYLimReal','54.86216','YLabelReal','','MinYLimMag','0','MaxYLimMag','10'" - ",'LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862" - "74509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" - "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882" - "3529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','L" - "inePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.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'),str" - "uct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}" - "},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','1.00000','MaxYLimReal','3" - ".00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotM" - "agPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOr" - "der',[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.07450" - "98039215686 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','-','LineWi" - "dth',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',false,'Placement',6)},'Di" - "splayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803" - "922 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',{{stru" - "ct('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,'Mark" - "er','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" - "truct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames'" - ",{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions'," - "[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Conf" - "iguration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[76 185 1366 906])" - NumInputPorts "5" - Floating off - } - Block { - BlockType Scope - Name "tauError" - SID "415" - Ports [5] - Position [655, 250, 760, 360] - ZOrder 653 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr." - "Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),extmg" - "r.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true,'Da" - "taLoggingVariableName','tauErrorData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','Data" - "LoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimR" - "eal','-2.62369','MaxYLimReal','22.48453','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','22.48453','LegendVisi" - "bility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 " - "0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 " - "0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450" - "9803921569 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','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vi" - "sible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," - "[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'" - ",0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-1.31451','MaxYLimReal','0.36849','YL" - "abelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',fa" - "lse,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0" - ".0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.8" - "31372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" - " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vi" - "sible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," - "[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none'," - "'Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2)," - "struct('MinYLimReal','-1.35483','MaxYLimReal','0.37657','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendV" - "isibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098039" - "22 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1" - ";1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27" - "4509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProper" - "tiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," - "[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none'," - "'Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLin" - "es',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-80.67711','MaxYLimReal','54.64406" - "','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhas" - "e',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.07450980392" - "15686 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'),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,'Marker','n" - "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement" - "',4),struct('MinYLimReal','-80.58848','MaxYLimReal','54.86216','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" - "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274" - "509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294117" - "64706 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','%','Lin" - "ePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" - "Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','" - "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker'," - "'none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}}," - "'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','1.00000','MaxYLimReal','3.0" - "0000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMag" - "Phase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrde" - "r',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3921568627" - "45098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.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,'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','Visi" - "ble','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{}},'ShowContent',false,'Placement',6)},'Disp" - "layPropertyDefaults',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.41176470588235" - "3 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',{{struct" - "('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker" - "','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" - "uct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{" - "{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6" - " 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Config" - "uration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[387 264 1667 955])" - NumInputPorts "5" - Floating off - } - Block { - BlockType Scope - Name "tauMeas" - SID "413" - Ports [5] - Position [655, 115, 760, 225] - ZOrder 651 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr." - "Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),extmg" - "r.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true,'Da" - "taLoggingVariableName','tauMeasuredData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','D" - "ataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYL" - "imReal','-2.62369','MaxYLimReal','22.48453','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','22.48453','LegendV" - "isibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098039" - "22 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1" - ";1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27" - "4509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProper" - "tiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," - "[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none'," - "'Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLin" - "es',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-1.31451','MaxYLimReal','0.36849'," - "'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.0745098039215" - "686 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','none'," - "'Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','" - "LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'," - "2),struct('MinYLimReal','-1.35483','MaxYLimReal','0.37657','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Lege" - "ndVisibility','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.62352941176470" - "6 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0" - ".274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePro" - "pertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[1 0 1],'LineStyle','-','LineWidth',0.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','non" - "e','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',{{}},'Num" - "Lines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-80.67711','MaxYLimReal','54.64" - "406','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagP" - "hase',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.39215686274" - "5098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07450980" - "39215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-'" - ",'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle'," - "'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker'" - ",'none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placem" - "ent',4),struct('MinYLimReal','-80.58848','MaxYLimReal','54.86216','YLabelReal','','MinYLimMag','0','MaxYLimMag','10" - "','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686" - "274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294" - "11764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" - "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','" - "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle" - "','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" - "ruct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{" - "}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','1.00000','MaxYLimReal','" - "3.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'Plot" - "MagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorO" - "rder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.3921568" - "62745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745" - "098039215686 0.650980392156863],'Title','','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidt" - "h',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','-','LineW" - "idth',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',false,'Placement',6)},'D" - "isplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" - "3922 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.274509803921569 1;" - "0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{str" - "uct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')," - "struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames" - "',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions'" - ",[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Con" - "figuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[71 188 1371 939])" - NumInputPorts "5" - Floating off - } - Line { - ZOrder 575 - SrcBlock "gravityTorques" - SrcPort 1 - DstBlock "Demux4" - DstPort 1 - } - Line { - Name "torqueMeasured" - ZOrder 468 - SrcBlock "Get Measurement" - SrcPort 1 - Points [70, 0] - Branch { - ZOrder 570 - Labels [1, 1] - DstBlock "Demux1" - DstPort 1 - } - Branch { - ZOrder 418 - DstBlock "Sum2" - DstPort 1 - } - } - Line { - Name "torso" - ZOrder 569 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 1 - DstBlock "tauMeas" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 565 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 2 - DstBlock "tauMeas" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 568 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 4 - DstBlock "tauMeas" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 567 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 3 - DstBlock "tauMeas" - DstPort 3 - } - Line { - Name "right_leg" - ZOrder 566 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 5 - DstBlock "tauMeas" - DstPort 5 - } - Line { - Name "torso" - ZOrder 560 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 1 - DstBlock "tauError" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 561 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 2 - DstBlock "tauError" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 564 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 4 - DstBlock "tauError" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 562 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 3 - DstBlock "tauError" - DstPort 3 - } - Line { - Name "right_leg" - ZOrder 563 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 5 - DstBlock "tauError" - DstPort 5 - } - Line { - Name "torqueError" - ZOrder 571 - Labels [1, 1] - SrcBlock "Sum2" - SrcPort 1 - DstBlock "Demux2" - DstPort 1 - } - Line { - Name "torso" - ZOrder 555 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 1 - DstBlock "tauDes" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 559 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 2 - DstBlock "tauDes" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 558 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 4 - DstBlock "tauDes" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 557 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 3 - DstBlock "tauDes" - DstPort 3 - } - Line { - Name "torqueDesired" - ZOrder 431 - SrcBlock "tauDesired" - SrcPort 1 - Points [90, 0] - Branch { - ZOrder 572 - Labels [1, 1] - DstBlock "Demux3" - DstPort 1 - } - Branch { - ZOrder 433 - DstBlock "Sum2" - DstPort 2 - } - } - Line { - Name "right_leg" - ZOrder 556 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 5 - DstBlock "tauDes" - DstPort 5 - } - Line { - Name "torso" - ZOrder 551 - Labels [-1, 0] - SrcBlock "Demux4" - SrcPort 1 - DstBlock "gravTorques" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 553 - Labels [-1, 0] - SrcBlock "Demux4" - SrcPort 2 - DstBlock "gravTorques" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 552 - Labels [-1, 0] - SrcBlock "Demux4" - SrcPort 4 - DstBlock "gravTorques" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 550 - Labels [-1, 0] - SrcBlock "Demux4" - SrcPort 3 - DstBlock "gravTorques" - DstPort 3 - } - Line { - Name "right_leg" - ZOrder 554 - Labels [-1, 0] - SrcBlock "Demux4" - SrcPort 5 - DstBlock "gravTorques" - DstPort 5 - } - } - } - Block { - BlockType SubSystem - Name "emergency stop: joint limits" - SID "470" - Ports [1] - Position [430, 481, 555, 509] - ZOrder 636 - BackgroundColor "red" - ShowName off - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 21 - $ClassName "Simulink.Mask" - Display "disp('EMERGENCY STOP')" - RunInitForIconRedraw "off" - } - System { - Name "emergency stop: joint limits" - Location [663, 393, 3158, 1837] - 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 "400" - Block { - BlockType Inport - Name "qj" - SID "471" - Position [150, 238, 180, 252] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n1" - SID "472" - 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 "473" - 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 "474" - Ports [1, 0, 1] - Position [285, 229, 440, 261] - ZOrder 554 - RequestExecContextInheritance off - System { - Name "STOP IF JOINTS HIT THE LIMITS" - Location [663, 393, 3158, 1837] - 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 "300" - Block { - BlockType Inport - Name "qj" - SID "475" - Position [60, 103, 90, 117] - ZOrder 552 - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "476" - Ports [] - Position [227, -45, 247, -25] - ZOrder 553 - } - Block { - BlockType Assertion - Name "Assertion" - SID "477" - Position [340, 72, 400, 118] - ZOrder 207 - } - Block { - BlockType Reference - Name "Get Limits" - SID "478" - Ports [0, 2] - Position [20, 23, 140, 92] - ZOrder 551 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Limits" - SourceType "Get Limits" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - limitsSource "ControlBoard" - limitsType "Position" - } - Block { - BlockType SubSystem - Name "MATLAB Function" - SID "479" - Ports [4, 1] - 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 "3770" - Block { - BlockType Inport - Name "umin" - SID "479::18" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "umax" - SID "479::19" - Position [20, 136, 40, 154] - ZOrder -2 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "u" - SID "479::1" - Position [20, 171, 40, 189] - ZOrder -3 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "tol" - SID "479::20" - Position [20, 206, 40, 224] - ZOrder -4 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "479::3769" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 119 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "479::3768" - Tag "Stateflow S-Function impedanceControl 18" - Ports [4, 2] - Position [180, 102, 230, 203] - ZOrder 118 - FunctionName "sf_sfun" - PortCounts "[4 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "inRange" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "479::3770" - Position [460, 241, 480, 259] - ZOrder 120 - } - Block { - BlockType Outport - Name "inRange" - SID "479::5" - Position [460, 101, 480, 119] - ZOrder -8 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "umin" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "umax" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 3 - SrcBlock "u" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 4 - SrcBlock "tol" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - Name "inRange" - ZOrder 5 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "inRange" - DstPort 1 - } - Line { - ZOrder 6 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 7 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Constant - Name "index1" - SID "480" - Position [70, 140, 95, 150] - ZOrder 204 - ShowName off - Value "0.01" - VectorParams1D off - } - Line { - ZOrder 1 - SrcBlock "qj" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 3 - } - Line { - ZOrder 2 - SrcBlock "MATLAB Function" - SrcPort 1 - DstBlock "Assertion" - DstPort 1 - } - Line { - ZOrder 3 - SrcBlock "Get Limits" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 1 - } - Line { - ZOrder 4 - SrcBlock "Get Limits" - 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 "481" - Ports [1, 0, 1] - Position [285, 339, 440, 371] - ZOrder 556 - RequestExecContextInheritance off - System { - Name "STOP IF THERE ARE SPIKES IN THE ENCODERS" - Location [663, 393, 3158, 1837] - 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 "400" - Block { - BlockType Inport - Name "qj" - SID "482" - Position [15, 53, 45, 67] - ZOrder 552 - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "483" - Ports [] - Position [217, -25, 237, -5] - ZOrder 553 - } - Block { - BlockType Assertion - Name "Assertion" - SID "484" - Position [340, 72, 400, 118] - ZOrder 207 - } - Block { - BlockType SubSystem - Name "MATLAB Function" - SID "485" - Ports [2, 1] - Position [165, 24, 285, 166] - 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 "3770" - Block { - BlockType Inport - Name "u" - SID "485::1" - Position [20, 101, 40, 119] - ZOrder -3 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "delta_u_max" - SID "485::18" - Position [20, 136, 40, 154] - ZOrder -1 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "485::3769" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 119 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "485::3768" - Tag "Stateflow S-Function impedanceControl 14" - Ports [2, 2] - Position [180, 105, 230, 165] - ZOrder 118 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "noSpikes" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "485::3770" - Position [460, 241, 480, 259] - ZOrder 120 - } - Block { - BlockType Outport - Name "noSpikes" - SID "485::5" - Position [460, 101, 480, 119] - ZOrder -8 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "u" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "delta_u_max" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "noSpikes" - ZOrder 3 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "noSpikes" - DstPort 1 - } - Line { - ZOrder 4 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 5 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Constant - Name "index1" - SID "486" - Position [-40, 124, 100, 136] - ZOrder 554 - ShowName off - Value "Sat.maxJointsPositionDelta" - VectorParams1D off - } - Line { - ZOrder 1 - SrcBlock "qj" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "MATLAB Function" - SrcPort 1 - DstBlock "Assertion" - DstPort 1 - } - Line { - ZOrder 3 - SrcBlock "index1" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 2 - } - } - } - Line { - ZOrder 1 - SrcBlock "ON_GAZEBO\n1" - SrcPort 1 - DstBlock "STOP IF JOINTS HIT THE LIMITS" - DstPort enable - } - Line { - ZOrder 2 - SrcBlock "qj" - 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 SubSystem - Name "synchronizer" - SID "301" - Ports [] - Position [437, 213, 552, 248] - ZOrder 631 - ForegroundColor "red" - BackgroundColor "green" - ShowName off - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 22 - $ClassName "Simulink.Mask" - Display "disp('SYNCHRONIZER')" - } - System { - Name "synchronizer" - Location [65, 24, 1366, 768] - 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 "309" - Block { - BlockType SubSystem - Name "GAZEBO_SYNCHRONIZER" - SID "302" - Ports [0, 0, 1] - Position [15, 15, 140, 75] - ZOrder -7 - RequestExecContextInheritance off - System { - Name "GAZEBO_SYNCHRONIZER" - Location [65, 24, 1366, 768] - 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 "659" - Block { - BlockType EnablePort - Name "Enable" - SID "303" - Ports [] - Position [177, 90, 197, 110] - ZOrder 538 - } - Block { - BlockType Reference - Name "Simulator Synchronizer" - SID "304" - Ports [] - Position [120, 24, 250, 61] - ZOrder 539 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" - SourceType "Simulator Synchronizer" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - period "Config.Ts" - serverPortName "'/clock/rpc'" - clientPortName "'/WBT_synchronizer/rpc:o'" - } - } - } - Block { - BlockType Logic - Name "Logical\nOperator" - SID "305" - Ports [1, 1] - Position [-25, -41, 5, -9] - ZOrder 307 - BlockMirror on - NamePlacement "alternate" - ShowName off - Operator "NOT" - AllPortsSameDT off - OutDataTypeStr "boolean" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n" - SID "306" - Position [115, -35, 230, -15] - ZOrder 304 - BlockMirror on - NamePlacement "alternate" - ShowName off - Value "Config.ON_GAZEBO" - } - Block { - BlockType SubSystem - Name "REAL_TIME_SYNC" - SID "307" - Ports [0, 0, 1] - Position [-170, 12, -45, 72] - ZOrder -9 - RequestExecContextInheritance off - System { - Name "REAL_TIME_SYNC" - Location [65, 24, 1366, 768] - 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 "700" - Block { - BlockType EnablePort - Name "Enable" - SID "308" - Ports [] - Position [167, 95, 187, 115] - ZOrder 538 - } - Block { - BlockType Reference - Name "Real Time Synchronizer" - SID "309" - Ports [] - Position [115, 34, 240, 71] - ZOrder 539 - ForegroundColor "[0.916667, 0.916667, 0.916667]" - BackgroundColor "gray" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" - SourceType "Real Time Synchronizer" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - period "Config.Ts" - } - } - } - Line { - ZOrder 1 - SrcBlock "Logical\nOperator" - SrcPort 1 - Points [-75, 0] - DstBlock "REAL_TIME_SYNC" - DstPort enable - } - Line { - ZOrder 2 - SrcBlock "ON_GAZEBO\n" - SrcPort 1 - Points [-35, 0] - Branch { - ZOrder 3 - DstBlock "GAZEBO_SYNCHRONIZER" - DstPort enable - } - Branch { - ZOrder 4 - DstBlock "Logical\nOperator" - DstPort 1 - } - } - } - } - Line { - Name "gravityTorques" - ZOrder 337 - SrcBlock "Gravity compensation" - SrcPort 1 - Points [41, 0] - Branch { - ZOrder 366 - Labels [-1, 0] - DstBlock "Sum1" - DstPort 2 - } - Branch { - ZOrder 330 - Points [0, 70] - DstBlock "Visualization" - DstPort 2 - } - } - Line { - Name "control input" - ZOrder 321 - SrcBlock "Sum1" - SrcPort 1 - Points [46, 0] - Branch { - ZOrder 367 - Labels [-1, 0] - DstBlock "Set References" - DstPort 1 - } - Branch { - ZOrder 328 - Points [0, 90] - DstBlock "Visualization" - DstPort 1 - } - } - Line { - Name "PD control" - ZOrder 365 - Labels [-1, 0] - SrcBlock "PID feedback controller" - SrcPort 1 - DstBlock "Sum1" - DstPort 1 - } - Line { - ZOrder 347 - SrcBlock "Get Measurement" - SrcPort 1 - Points [22, 0] - Branch { - ZOrder 338 - Points [0, 95] - Branch { - ZOrder 374 - Points [0, 85] - DstBlock "emergency stop: joint limits" - DstPort 1 - } - Branch { - ZOrder 373 - DstBlock "Gravity compensation" - DstPort 1 - } - } - Branch { - ZOrder 307 - DstBlock "PID feedback controller" - DstPort 1 - } - } - } -} -#Finite State Machines -# -# Stateflow 80000014 -# -# -Stateflow { - machine { - id 1 - name "impedanceControl" - created "12-Mar-2015 18:23:09" - isLibrary 0 - sfVersion 80000012 - firstTarget 20 - } - chart { - id 2 - machine 1 - name "emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/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 3 0 0] - viewObj 2 - ssIdHighWaterMark 8 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 14 - disableImplicitCasting 1 - eml { - name "checkSpikes" - } - supportVariableSizing 0 - firstData 4 - firstTransition 8 - firstJunction 7 - } - 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 noSpikes = checkSpikes(u, delta_u_max)\n\n noSpikes = wbc.checkSpikes(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 4 - 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 [2 0 5] - } - data { - id 5 - 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 [2 4 6] - } - data { - id 6 - 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 [2 5 0] - } - junction { - id 7 - position [23.5747 49.5747 7] - chart 2 - subviewer 2 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [2 0 0] - } - transition { - id 8 - 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 7 - 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 9 - machine 1 - name "emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/MATLAB Function" - chart 2 - } - chart { - id 10 - 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 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 18 - disableImplicitCasting 1 - eml { - name "checkRangeFCN" - } - supportVariableSizing 0 - firstData 12 - firstTransition 18 - firstJunction 17 - } - state { - id 11 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 10 - treeNode [10 0 0 0] - superState SUBCHART - subviewer 10 - 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" - 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 12 - 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 [10 0 13] - } - data { - id 13 - 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 [10 12 14] - } - data { - id 14 - 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 [10 13 15] - } - data { - id 15 - 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 [10 14 16] - } - data { - id 16 - 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 [10 15 0] - } - junction { - id 17 - position [23.5747 49.5747 7] - chart 10 - subviewer 10 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [10 0 0] - } - transition { - id 18 - 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 17 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 10 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 10 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [10 0 0] - } - instance { - id 19 - machine 1 - name "emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" - chart 10 - } - target { - id 20 - machine 1 - name "sfun" - description "Default Simulink S-Function Target." - linkNode [1 0 0] - } -} diff --git a/torque-controllers/impedance-control/stopImpedanceControl.m b/torque-controllers/impedance-control/stopImpedanceControl.m deleted file mode 100644 index 31f91a5..0000000 --- a/torque-controllers/impedance-control/stopImpedanceControl.m +++ /dev/null @@ -1,22 +0,0 @@ -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% 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. - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% 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 \ No newline at end of file diff --git a/torque-controllers/momentum-based-standup/README.md b/torque-controllers/momentum-based-standup/README.md deleted file mode 100644 index 83e3d4e..0000000 --- a/torque-controllers/momentum-based-standup/README.md +++ /dev/null @@ -1,51 +0,0 @@ -## Module description - -This module implements a torque control balancing strategy. It computes the interaction forces at the feet/legs in order to stabilise a desired centroidal 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 [iCub whole-body control through force regulation on rigid non-coplanar contacts](http://journal.frontiersin.org/article/10.3389/frobt.2015.00006/abstract) - -### Dependencies - -In order to run the simulation of iCub standing up from a chair, make sure you have installed on your pc the following dependencies: - - - Both iCub and the chair models are stored into [icub-gazebo-wholebody](https://github.com/robotology-playground/icub-gazebo-wholebody) repository. - - It is suggested to install the above mentioned repository using [robotology-superbuild](https://github.com/robotology/robotology-superbuild) (enable `ROBOTOLOGY_ENABLE_DYNAMICS`, `ROBOTOLOGY_USES_GAZEBO` and `ROBOTOLOGY_USES_MATLAB` options). - -### Installation - -1) In your `.bashrc` file, add the following line to your `GAZEBO_RESOURCE_PATH`: - - ``` - source /usr/share/gazebo/setup.sh - export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:/PATH/WHERE/YOUR/WORLD/IS - - ``` - - Instead of opening Gazebo as usual, on a terminal type: - - `gazebo -slibgazebo_yarp_clock.so nameOfYourWorld` where for iCub standup demo `nameOfYourWorld = icub_standup_world`. - - and it will load the correct model. - -### Simulations - -To run iCub stand up demo, do not run the `yarpmotorgui`: iCub is already in its home position! - -### Compatibility - -The repository contains the Simulink model `torqueBalancingStandup.mdl`, which is generated by using Matlab R2016b. - -### Supported robots - -Currently, supported robots are: `iCubGenova02`, `icubGazeboSim`. - -## Module details - -### Configuration file - -At start, the module calls the initialization file initTorqueBalancingStandup.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/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/configRobot.m b/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/configRobot.m deleted file mode 100644 index c35862f..0000000 --- a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/configRobot.m +++ /dev/null @@ -1,104 +0,0 @@ -% CONFIGROBOT initializes parameters specific of a particular robot -% (e.g., icuGazeboSim) -% - -%% --- Initialization --- -Config.ON_GAZEBO = false; -ROBOT_DOF = 23; -ROBOT_DOF_FOR_SIMULINK = eye(ROBOT_DOF); - -% Robot configuration for WBToolbox -WBTConfigRobot = WBToolbox.Configuration; -WBTConfigRobot.RobotName = 'icub'; -WBTConfigRobot.UrdfFile = 'model.urdf'; -WBTConfigRobot.LocalName = 'WBT'; - -% Controlboards and joints list. Each joint is associated to the corresponding controlboard -WBTConfigRobot.ControlBoardsNames = {'torso','left_arm','right_arm','left_leg','right_leg'}; -WBTConfigRobot.ControlledJoints = []; -Config.numOfJointsForEachControlboard = []; - -ControlBoards = struct(); -ControlBoards.(WBTConfigRobot.ControlBoardsNames{1}) = {'torso_pitch','torso_roll','torso_yaw'}; -ControlBoards.(WBTConfigRobot.ControlBoardsNames{2}) = {'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow'}; -ControlBoards.(WBTConfigRobot.ControlBoardsNames{3}) = {'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow'}; -ControlBoards.(WBTConfigRobot.ControlBoardsNames{4}) = {'l_hip_pitch','l_hip_roll','l_hip_yaw','l_knee','l_ankle_pitch','l_ankle_roll'}; -ControlBoards.(WBTConfigRobot.ControlBoardsNames{5}) = {'r_hip_pitch','r_hip_roll','r_hip_yaw','r_knee','r_ankle_pitch','r_ankle_roll'}; - -for n = 1:length(WBTConfigRobot.ControlBoardsNames) - - WBTConfigRobot.ControlledJoints = [WBTConfigRobot.ControlledJoints, ... - ControlBoards.(WBTConfigRobot.ControlBoardsNames{n})]; - Config.numOfJointsForEachControlboard = [Config.numOfJointsForEachControlboard; length(ControlBoards.(WBTConfigRobot.ControlBoardsNames{n}))]; -end - -% Frames list -Frames.BASE = 'root_link'; -Frames.IMU = 'imu_frame'; -Frames.LEFT_FOOT = 'l_sole'; -Frames.RIGHT_FOOT = 'r_sole'; -Frames.COM = 'com'; -Frames.LEFT_LEG = 'l_upper_leg_contact'; -Frames.RIGHT_LEG = 'r_upper_leg_contact'; -Frames.LEFT_HAND = 'l_hand_dh_frame'; -Frames.RIGHT_HAND = 'r_hand_dh_frame'; - -%% iCub STANDUP demo parameters -% when Config.STANDUP_WITH_HUMAN is setted to TRUE, the robot will be aware -% of the external forces at the arms provided by the human and it will use -% also them for lifting up. -Config.STANDUP_WITH_HUMAN = true; - -%% Other parameters - -% Config.USE_MOTOR_REFLECTED_INERTIA: if set to true, motors reflected -% inertias are included in the system mass matrix. If -% Config.INCLUDE_COUPLING is true, then the coupling effect (some joints -% motion is the result of more than one motor motion) is taken into account. -% Config.INCLUDE_HARMONIC_DRIVE_INERTIA is true, then the harmonic drive -% reflected inertia is also considered -Config.USE_MOTOR_REFLECTED_INERTIA = true; -Config.INCLUDE_COUPLING = true; -Config.INCLUDE_HARMONIC_DRIVE_INERTIA = true; - -% if TRUE, the controller will STOP if the joints hit the joints limits -% and/or if the (unsigned) difference between two consecutive joints -% encoders measurements is greater than a given threshold. -Config.EMERGENCY_STOP_WITH_JOINTS_LIMITS = false; -Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES = true; - -% Config.USE_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 and Config.PITCH_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 de to the IMU drift in estimating this angle. Hence, -% when either of the flags Config.YAW_IMU_FILTER or Config.PITCH_IMU_FILTER -% is set to true, then the yaw and/or pitch angles of the contact foot are -% ignored and kept equal to the initial values. -Config.FILTER_IMU_YAW = true; -Config.FILTER_IMU_PITCH = true; - -% Config.CORRECT_NECK_IMU: when set equal to true, the kineamtics 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). -Config.CORRECT_NECK_IMU = true; - -% 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.WRENCH_LEFT_ARM = '/wholeBodyDynamics/left_arm/endEffectorWrench:o'; -Ports.WRENCH_RIGHT_ARM = '/wholeBodyDynamics/right_arm/endEffectorWrench:o'; -Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; -Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; \ No newline at end of file diff --git a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m b/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m deleted file mode 100644 index 0b142db..0000000 --- a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m +++ /dev/null @@ -1,215 +0,0 @@ -% INITCOORDINATOR initializes the robot configuration for running -% 'COORDINATOR' demo. -% - -%% --- Initialization --- - -% Feet in contact (COORDINATOR DEMO ONLY) -Config.LEFT_RIGHT_FOOT_IN_CONTACT = [1 1]; - -% Initial foot on ground. If false, right foot is used as default contact -% frame (this does not means that the other foot cannot be in contact too). -% (COORDINATOR DEMO ONLY) -Config.LEFT_FOOT_IN_CONTACT_AT_0 = true; - -% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) -Config.DEMO_MOVEMENTS = true; - -% If equal to true, the desired streamed values of the postural tasks are -% smoothed internally -Config.SMOOTH_JOINT_DES = false; - -% torque saturation -Sat.torque = 34; - -% 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] - -%% Control gains - -% PARAMETERS FOR TWO FEET BALANCING -if sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 2 - - Gain.KP_COM = diag([50 50 50]); - Gain.KD_COM = 2*sqrt(Gain.KP_COM)*0; - Gain.KP_AngularMomentum = 5; - Gain.KD_AngularMomentum = 2*sqrt(Gain.KP_AngularMomentum); - - % Impedances acting in the null space of the desired contact forces - impTorso = [10 10 20]; - - impArms = [10 10 10 8]; - - impLeftLeg = [30 30 30 60 10 10]; - - impRightLeg = [30 30 30 60 10 10]; -end - -% PARAMETERS FOR ONE FOOT BALANCING -if sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 1 - - Gain.KP_COM = diag([50 100 50]); - Gain.KD_COM = diag([0 0 0]); - Gain.KP_AngularMomentum = 1 ; - Gain.KD_AngularMomentum = 1 ; - - % Impedances acting in the null space of the desired contact forces - impTorso = [20 20 30]; - - impArms = [15 15 15 8]; - - impLeftLeg = [30 30 30 120 10 10]; - - impRightLeg = [30 30 30 60 10 10]; -end - -Gain.impedances = [impTorso(1,:),impArms(1,:),impArms(1,:),impLeftLeg(1,:),impRightLeg(1,:)]; -Gain.dampings = 0*sqrt(Gain.impedances); - -if (size(Gain.impedances,2) ~= ROBOT_DOF) - error('Dimension mismatch between ROBOT_DOF and dimension of the variable impedences. Check these variables in the file gains.m'); -end - -% Smoothing time gain scheduling (STANDUP DEMO ONLY) -Gain.SmoothingTimeGainScheduling = 0.02; - -%% References for CoM trajectory (COORDINATOR DEMO ONLY) - -% that the robot waits before starting the left-and-right -Config.noOscillationTime = 0; - -if Config.DEMO_MOVEMENTS && sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 2 - - Config.directionOfOscillation = [0;1;0]; - % amplitude of oscillations in meters - Config.amplitudeOfOscillation = 0.02; - % frequency of oscillations in hertz - Config.frequencyOfOscillation = 0.2; -else - Config.directionOfOscillation = [0;0;0]; - Config.amplitudeOfOscillation = 0.0; - Config.frequencyOfOscillation = 0.0; -end - -%% Parameters for motors reflected inertia - -% transmission ratio -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; - -%% State machine parameters - -% smoothing time for joints and CoM -Sm.smoothingTimeCoM_Joints = 1; - -% if Sm.smoothingTimeCoM_Joints = 0, this will allow to smooth anyway the -% CoM reference trajectory -Sm.smoothingTimeCoM = 1; - -% contact forces threshold (STANDUP DEMO ONLY) -Sm.wrench_thresholdContactLFoot = 1; -Sm.wrench_thresholdContactRFoot = 1; -Sm.wrench_thresholdContactLHand = 1; -Sm.wrench_thresholdContactRHand = 1; - -% initial state for state machine (STANDUP DEMO ONLY) -Sm.stateAt0 = 1; - -% delta to be summed to the reference CoM position (STANDUP DEMO ONLY) -Sm.CoM_delta = [0; 0; 0]; - -% joint references (STANDUP DEMO ONLY) -Sm.joints_standUpPositions = zeros(1,9); - -% configuration parameters for state machine (STANDUP DEMO ONLY) -Sm.tBalancing = 1; - -%% 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/5; -torsionalFrictionCoefficient = 1/150; - -% physical size of the foot -feet_size = [-0.07 0.12; % xMin, xMax - -0.045 0.05]; % yMin, yMax - -leg_size = [-0.025 0.025 ; % xMin, xMax - -0.005 0.005]; % yMin, yMax - -fZmin = 10; - -%% Regularization parameters -Reg.pinvDamp_nu_b = 1e-7; -Reg.pinvDamp = 0.01; -Reg.pinvTol = 1e-5; -Reg.impedances = 0.1; -Reg.dampings = 0; -Reg.HessianQP = 1e-7; -Reg.norm_tolerance = 1e-4; \ No newline at end of file diff --git a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m b/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m deleted file mode 100644 index 5ee0b0f..0000000 --- a/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initStateMachineStandup.m +++ /dev/null @@ -1,209 +0,0 @@ -% INITSTATEMACHINESTANDUP initializes the robot configuration for running -% 'STANDUP' demo. -% - -%% --- Initialization --- - -% Feet in contact (COORDINATOR DEMO ONLY) -Config.LEFT_RIGHT_FOOT_IN_CONTACT = [1 1]; - -% Initial foot on ground. If false, right foot is used as default contact -% frame (this does not means that the other foot cannot be in contact too). -% (COORDINATOR DEMO ONLY) -Config.LEFT_FOOT_IN_CONTACT_AT_0 = true; - -% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) -Config.DEMO_MOVEMENTS = false; - -% If equal to one, the desired streamed values of the postural tasks are -% smoothed internally -Config.SMOOTH_JOINT_DES = true; - -% torque saturation -Sat.torque = 34; - -% 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_nu_b = 1e-3; -Reg.pinvDamp = 0.07; -Reg.pinvTol = 1e-5; -Reg.impedances = 0.1; -Reg.dampings = 0; -Reg.HessianQP = 1e-2; -Reg.norm_tolerance = 1e-4; - -%% COM AND JOINT GAINS -Gain.KP_COM = [50 50 50; % state == 1 BALANCING ON THE LEGS - 50 50 50; % state == 2 MOVE COM FORWARD - 50 50 50; % state == 3 TWO FEET BALANCING - 50 50 50]; % state == 4 LIFTING UP - -Gain.KD_COM = 2*sqrt(Gain.KP_COM)*0; - -Gain.KP_AngularMomentum = 2; -Gain.KD_AngularMomentum = 2*sqrt(Gain.KP_AngularMomentum); - -% % TORSO %% LEFT ARM %% RIGHT ARM %% LEFT LEG %% RIGHT LEG %% -Gain.impedances = [10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50; % state == 1 BALANCING ON THE LEGS - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50; % state == 2 MOVE COM FORWARD - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50; % state == 3 TWO FEET BALANCING - 10 30 20, 20 20 10 8, 20 20 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50]; % state == 4 LIFTING UP - -Gain.impedances(3,:) = Gain.impedances(3,:)./2; -Gain.dampings = 0*sqrt(Gain.impedances(1,:)); - -% Smoothing time gain scheduling (STANDUP DEMO ONLY) -Gain.SmoothingTimeGainScheduling = 2; - -%% STATE MACHINE PARMETERS - -% smoothing time for joints and CoM -Sm.smoothingTimeCoM_Joints = [1; % state == 1 BALANCING ON THE LEGS - 0.5; % state == 2 MOVE COM FORWARD - 0; % state == 3 TWO FEET BALANCING - 2]; % state == 4 LIFTING UP - -% if Sm.smoothingTimeCoM_Joints = 0, this will allow to smooth anyway the -% CoM reference trajectory -Sm.smoothingTimeCoM = 0.5; - -% contact forces threshold (YOGA DEMO ONLY) -Sm.wrench_thresholdContactLFoot = [0; % NOT USED - 90; % state == 2 MOVE COM FORWARD - 140; % state == 3 TWO FEET BALANCING - 0]; % NOT USED - -Sm.wrench_thresholdContactRFoot = [0 % NOT USED - 90; % state == 2 MOVE COM FORWARD - 140; % state == 3 TWO FEET BALANCING - 0]; % NOT USED - -% external forces at arms threshold -Sm.wrench_thresholdContactRHand = [7.5 % state == 1 BALANCING ON THE LEGS - 0; % NOT USED - 0; % NOT USED - 0]; % NOT USED - - -Sm.wrench_thresholdContactLHand = [7.5 % state == 1 BALANCING ON THE LEGS - 0; % NOT USED - 0; % NOT USED - 0]; % NOT USED - -% initial state for state machine (STANDUP DEMO ONLY) -Sm.stateAt0 = 1; - -% delta to be summed to the reference CoM position (STANDUP DEMO ONLY) - -Sm.CoM_delta = [% THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE LEFT LEG - 0.0 0.0 0.0; % NOT USED - 0.0867 0.0 0.0; % state == 2 MOVE COM FORWARD - 0.005 0.0 0.0; % state == 3 TWO FEET BALANCING - 0.03 0.0 0.22]; % state == 4 LIFTING UP - -% configuration parameters for state machine (STANDUP DEMO ONLY) -Sm.tBalancing = 3; - -%% Joint references (STANDUP DEMO ONLY) - - %Hip pitch %Hip roll %Knee %Ankle pitch %Shoulder pitch %Shoulder roll %Shoulder yaw %Elbow %Torso pitch -Sm.joints_standUpPositions = [0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000; % NOT USED - 1.5402 0.1594 -1.7365 -0.2814 -1.6455 0.4363 0.5862 0.2473 0.4363; % state == 2 MOVE COM FORWARD - 1.1097 0.0122 -0.8365 -0.0714 -1.4615 0.4363 0.1545 0.2018 0.0611; % state == 3 TWO FEET BALANCING - 0.2094 0.1047 -0.1745 -0.0349 -1.4615 0.4363 0.5862 0.2473 0.0000]; % state == 4 LIFTING UP - -%% References for CoM trajectory (COORDINATOR DEMO ONLY) - -% that the robot waits before starting the left-and-right -Config.noOscillationTime = 0; -Config.directionOfOscillation = [0;0;0]; -Config.amplitudeOfOscillation = 0.0; -Config.frequencyOfOscillation = 0.0; - -%% Parameters for motors reflected inertia - -% transmission ratio -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/5; -torsionalFrictionCoefficient = 1/150; -fZmin = 10; - -% physical size of the foot -feet_size = [-0.05 0.05; % xMin, xMax - -0.045 0.05]; % yMin, yMax - -leg_size = [-0.025 0.05 ; % xMin, xMax - -0.025 0.025]; % yMin, yMax \ No newline at end of file diff --git a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/configRobot.m b/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/configRobot.m deleted file mode 100644 index 100ae5b..0000000 --- a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/configRobot.m +++ /dev/null @@ -1,104 +0,0 @@ -% CONFIGROBOT initializes parameters specific of a particular robot -% (e.g., icuGazeboSim) -% - -%% --- Initialization --- -Config.ON_GAZEBO = true; -ROBOT_DOF = 23; -ROBOT_DOF_FOR_SIMULINK = eye(ROBOT_DOF); - -% 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'; -Frames.LEFT_LEG = 'l_upper_leg_contact'; -Frames.RIGHT_LEG = 'r_upper_leg_contact'; -Frames.LEFT_HAND = 'l_hand_dh_frame'; -Frames.RIGHT_HAND = 'r_hand_dh_frame'; - -%% iCub STANDUP demo parameters -% when Config.STANDUP_WITH_HUMAN is setted to TRUE, the robot will be aware -% of the external forces at the arms provided by the human and it will use -% also them for lifting up. -Config.STANDUP_WITH_HUMAN = false; - -%% Other parameters - -% 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; - -% if TRUE, the controller will STOP if the joints hit the joints limits -% and/or if the (unsigned) difference between two consecutive joints -% encoders measurements is greater than a given threshold. -Config.EMERGENCY_STOP_WITH_JOINTS_LIMITS = false; -Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES = true; - -% Config.USE_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 and Config.PITCH_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 de to the IMU drift in estimating this angle. Hence, -% when either of the flags Config.YAW_IMU_FILTER or Config.PITCH_IMU_FILTER -% is set to true, then the yaw and/or pitch angles of the contact foot are -% ignored and kept equal to the initial values. -Config.FILTER_IMU_YAW = true; -Config.FILTER_IMU_PITCH = true; - -% Config.CORRECT_NECK_IMU: when set equal to true, the kineamtics 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). -Config.CORRECT_NECK_IMU = true; - -% 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.WRENCH_LEFT_ARM = '/wholeBodyDynamics/left_arm/endEffectorWrench:o'; -Ports.WRENCH_RIGHT_ARM = '/wholeBodyDynamics/right_arm/endEffectorWrench:o'; -Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial']; -Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o']; \ No newline at end of file diff --git a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m b/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m deleted file mode 100644 index a1b8cc5..0000000 --- a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initCoordinator.m +++ /dev/null @@ -1,215 +0,0 @@ -% INITCOORDINATOR initializes the robot configuration for running -% 'COORDINATOR' demo. -% - -%% --- Initialization --- - -% Feet in contact (COORDINATOR DEMO ONLY) -Config.LEFT_RIGHT_FOOT_IN_CONTACT = [1 1]; - -% Initial foot on ground. If false, right foot is used as default contact -% frame (this does not means that the other foot cannot be in contact too). -% (COORDINATOR DEMO ONLY) -Config.LEFT_FOOT_IN_CONTACT_AT_0 = true; - -% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) -Config.DEMO_MOVEMENTS = true; - -% If equal to true, the desired streamed values of the postural tasks are -% smoothed internally -Config.SMOOTH_JOINT_DES = false; - -% torque saturation -Sat.torque = 34; - -% 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] - -%% Control gains - -% PARAMETERS FOR TWO FEET BALANCING -if sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 2 - - Gain.KP_COM = diag([50 50 50]); - Gain.KD_COM = 2*sqrt(Gain.KP_COM)*0; - Gain.KP_AngularMomentum = 10 ; - Gain.KD_AngularMomentum = 2*sqrt(Gain.KP_AngularMomentum); - - % Impedances acting in the null space of the desired contact forces - impTorso = [10 10 20]; - - impArms = [10 10 10 8]; - - impLeftLeg = [30 30 30 60 10 10]; - - impRightLeg = [30 30 30 60 10 10]; -end - -% PARAMETERS FOR ONE FOOT BALANCING -if sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 1 - - Gain.KP_COM = diag([50 100 50]); - Gain.KD_COM = diag([0 0 0]); - Gain.KP_AngularMomentum = 1 ; - Gain.KD_AngularMomentum = 1 ; - - % Impedances acting in the null space of the desired contact forces - impTorso = [20 20 30]; - - impArms = [15 15 15 8]; - - impLeftLeg = [30 30 30 120 10 10]; - - impRightLeg = [30 30 30 60 10 10]; -end - -Gain.impedances = [impTorso(1,:),impArms(1,:),impArms(1,:),impLeftLeg(1,:),impRightLeg(1,:)]; -Gain.dampings = 0*sqrt(Gain.impedances); - -if (size(Gain.impedances,2) ~= ROBOT_DOF) - error('Dimension mismatch between ROBOT_DOF and dimension of the variable impedences. Check these variables in the file gains.m'); -end - -% Smoothing time gain scheduling (STANDUP DEMO ONLY) -Gain.SmoothingTimeGainScheduling = 0.02; - -%% References for CoM trajectory (COORDINATOR DEMO ONLY) - -% that the robot waits before starting the left-and-right -Config.noOscillationTime = 0; - -if Config.DEMO_MOVEMENTS && sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 2 - - Config.directionOfOscillation = [0;1;0]; - % amplitude of oscillations in meters - Config.amplitudeOfOscillation = 0.02; - % frequency of oscillations in hertz - Config.frequencyOfOscillation = 0.2; -else - Config.directionOfOscillation = [0;0;0]; - Config.amplitudeOfOscillation = 0.0; - Config.frequencyOfOscillation = 0.0; -end - -%% Parameters for motors reflected inertia - -% transmission ratio -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; - -%% State machine parameters - -% smoothing time for joints and CoM -Sm.smoothingTimeCoM_Joints = 1; - -% if Sm.smoothingTimeCoM_Joints = 0, this will allow to smooth anyway the -% CoM reference trajectory -Sm.smoothingTimeCoM = 1; - -% contact forces threshold (STANDUP DEMO ONLY) -Sm.wrench_thresholdContactLFoot = 1; -Sm.wrench_thresholdContactRFoot = 1; -Sm.wrench_thresholdContactLHand = 1; -Sm.wrench_thresholdContactRHand = 1; - -% initial state for state machine (STANDUP DEMO ONLY) -Sm.stateAt0 = 1; - -% delta to be summed to the reference CoM position (STANDUP DEMO ONLY) -Sm.CoM_delta = [0; 0; 0]; - -% joint references (STANDUP DEMO ONLY) -Sm.joints_standUpPositions = zeros(1,9); - -% configuration parameters for state machine (STANDUP DEMO ONLY) -Sm.tBalancing = 1; - -%% 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; -torsionalFrictionCoefficient = 1/75; - -% physical size of the foot -feet_size = [-0.07 0.07; % xMin, xMax - -0.03 0.03]; % yMin, yMax - -leg_size = [-0.025 0.05 ; % xMin, xMax - -0.025 0.025]; % yMin, yMax - -fZmin = 10; - -%% Regularization parameters -Reg.pinvDamp_nu_b = 1e-7; -Reg.pinvDamp = 0.01; -Reg.pinvTol = 1e-5; -Reg.impedances = 0.1; -Reg.dampings = 0; -Reg.HessianQP = 1e-5; -Reg.norm_tolerance = 1e-4; \ No newline at end of file diff --git a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m b/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m deleted file mode 100644 index 7c58f17..0000000 --- a/torque-controllers/momentum-based-standup/app/robots/icubGazeboSim/initStateMachineStandup.m +++ /dev/null @@ -1,209 +0,0 @@ -% INITSTATEMACHINESTANDUP initializes the robot configuration for running -% 'STANDUP' demo. -% - -%% --- Initialization --- - -% Feet in contact (COORDINATOR DEMO ONLY) -Config.LEFT_RIGHT_FOOT_IN_CONTACT = [1 1]; - -% Initial foot on ground. If false, right foot is used as default contact -% frame (this does not means that the other foot cannot be in contact too). -% (COORDINATOR DEMO ONLY) -Config.LEFT_FOOT_IN_CONTACT_AT_0 = true; - -% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) -Config.DEMO_MOVEMENTS = false; - -% If equal to one, the desired streamed values of the postural tasks are -% smoothed internally -Config.SMOOTH_JOINT_DES = true; - -% torque saturation -Sat.torque = 60; - -% 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_nu_b = 1e-3; -Reg.pinvDamp = 1; -Reg.pinvTol = 1e-5; -Reg.impedances = 0.1; -Reg.dampings = 0; -Reg.HessianQP = 1e-2; -Reg.norm_tolerance = 1e-4; - -%% COM AND JOINT GAINS -Gain.KP_COM = [250 250 50; % state == 1 BALANCING ON THE LEGS - 250 250 50; % state == 2 MOVE COM FORWARD - 250 250 50; % state == 3 TWO FEET BALANCING - 250 250 50]; % state == 4 LIFTING UP - -Gain.KD_COM = 2*sqrt(Gain.KP_COM)/10; - -Gain.KP_AngularMomentum = 1; -Gain.KD_AngularMomentum = 2*sqrt(Gain.KP_AngularMomentum); - -% % TORSO %% LEFT ARM %% RIGHT ARM %% LEFT LEG %% RIGHT LEG %% -Gain.impedances = [10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50; % state == 1 BALANCING ON THE LEGS - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50; % state == 2 MOVE COM FORWARD - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50; % state == 3 TWO FEET BALANCING - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50]; % state == 4 LIFTING UP - -Gain.impedances(3,:) = Gain.impedances(3,:)./2; -Gain.dampings = 0*sqrt(Gain.impedances(1,:)); - -% Smoothing time gain scheduling (STANDUP DEMO ONLY) -Gain.SmoothingTimeGainScheduling = 2; - -%% STATE MACHINE PARMETERS - -% smoothing time for joints and CoM -Sm.smoothingTimeCoM_Joints = [1; % state == 1 BALANCING ON THE LEGS - 2.5; % state == 2 MOVE COM FORWARD - 2; % state == 3 TWO FEET BALANCING - 4]; % state == 4 LIFTING UP - -% if Sm.smoothingTimeCoM_Joints = 0, this will allow to smooth anyway the -% CoM reference trajectory -Sm.smoothingTimeCoM = 0.5; - -% contact forces threshold (YOGA DEMO ONLY) -Sm.wrench_thresholdContactLFoot = [0; % NOT USED - 75; % state == 2 MOVE COM FORWARD - 95; % state == 3 TWO FEET BALANCING - 0]; % NOT USED - -Sm.wrench_thresholdContactRFoot = [0 % NOT USED - 75; % state == 2 MOVE COM FORWARD - 95; % state == 3 TWO FEET BALANCING - 0]; % NOT USED - -% external forces at arms threshold -Sm.wrench_thresholdContactRHand = [10 % state == 1 BALANCING ON THE LEGS - 0; % NOT USED - 0; % NOT USED - 0]; % NOT USED - - -Sm.wrench_thresholdContactLHand = [10 % state == 1 BALANCING ON THE LEGS - 0; % NOT USED - 0; % NOT USED - 0]; % NOT USED - -% initial state for state machine (STANDUP DEMO ONLY) -Sm.stateAt0 = 1; - -% delta to be summed to the reference CoM position (STANDUP DEMO ONLY) - -Sm.CoM_delta = [% THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE LEFT LEG - 0.0 0.0 0.0; % NOT USED - 0.1277 -0.0295 0.0; % state == 2 MOVE COM FORWARD - 0.04 -0.0 0.0; % state == 3 TWO FEET BALANCING - 0.0 0.0 0.20]; % state == 4 LIFTING UP - -% configuration parameters for state machine (STANDUP DEMO ONLY) -Sm.tBalancing = 3; - -%% Joint references (STANDUP DEMO ONLY) - - %Hip pitch %Hip roll %Knee %Ankle pitch %Shoulder pitch %Shoulder roll %Shoulder yaw %Elbow %Torso pitch -Sm.joints_standUpPositions = [0.0000 0.0000 0.0000 0.0000 0.0000 0.4363 0.0000 0.0000 0.0000; % state == 1 THIS REFERENCE IS NOT USED - 1.5402 0.1594 -1.7365 -0.2814 -1.6455 0.4363 0.5862 0.2473 0.4363; % state == 2 MOVE COM FORWARD - 1.1097 0.0122 -0.8365 -0.0714 -1.4615 0.4363 0.1545 0.2018 0.0611; % state == 3 TWO FEET BALANCING - 0.2094 0.1047 -0.1745 -0.0349 -1.6455 0.4363 0.5862 0.2473 0.0000]; % state == 4 LIFTING UP - -%% References for CoM trajectory (COORDINATOR DEMO ONLY) - -% that the robot waits before starting the left-and-right -Config.noOscillationTime = 0; -Config.directionOfOscillation = [0;0;0]; -Config.amplitudeOfOscillation = 0.0; -Config.frequencyOfOscillation = 0.0; - -%% Parameters for motors reflected inertia - -% transmission ratio -Config.Gamma = 0.01*eye(ROBOT_DOF); - -% modify the value of the transmission ratio for the hip pitch. -% TODO: avoid to hard-code the joint numbering -Config.Gamma(end-5, end-5) = 0.0067; -Config.Gamma(end-11,end-11) = 0.0067; - -% motors inertia (Kg*m^2) -legsMotors_I_m = 0.0827*1e-4; -torsoPitchRollMotors_I_m = 0.0827*1e-4; -torsoYawMotors_I_m = 0.0585*1e-4; -armsMotors_I_m = 0.0585*1e-4; - -% add harmonic drives reflected inertia -if Config.INCLUDE_HARMONIC_DRIVE_INERTIA - - legsMotors_I_m = legsMotors_I_m + 0.054*1e-4; - torsoPitchRollMotors_I_m = torsoPitchRollMotors_I_m + 0.054*1e-4; - torsoYawMotors_I_m = torsoYawMotors_I_m + 0.021*1e-4; - armsMotors_I_m = armsMotors_I_m + 0.021*1e-4; -end - -Config.I_m = diag([torsoPitchRollMotors_I_m*ones(2,1); - torsoYawMotors_I_m; - armsMotors_I_m*ones(8,1); - legsMotors_I_m*ones(12,1)]); - -% parameters for coupling matrices. Updated according to the wiki: -% -% http://wiki.icub.org/wiki/ICub_coupled_joints -% -% and corrected according to https://github.com/robotology/robots-configuration/issues/39 -t = 0.615; -r = 0.022; -R = 0.04; - -% coupling matrices -T_LShoulder = [-1 0 0; - -1 -t 0; - 0 t -t]; - -T_RShoulder = [ 1 0 0; - 1 t 0; - 0 -t t]; - -T_torso = [ 0.5 -0.5 0; - 0.5 0.5 0; - r/(2*R) r/(2*R) r/R]; - -if Config.INCLUDE_COUPLING - - Config.T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12)); -else - Config.T = eye(ROBOT_DOF); -end - -% gain for feedforward term in joint torques calculation. Valid range: a -% value between 0 and 1 -Config.K_ff = 0; - -% Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA if true, the desired joints -% accelerations are used for computing the feedforward term in joint -% torques calculations. Not effective if Config.K_ff = 0. -Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA = false; - -%% Constraints for QP for balancing - -% The friction cone is approximated by using linear interpolation of the circle. -% So, numberOfPoints defines the number of points used to interpolate the circle -% in each cicle's quadrant -numberOfPoints = 4; -forceFrictionCoefficient = 1/3; -torsionalFrictionCoefficient = 1/75; -fZmin = 10; - -% physical size of the foot -feet_size = [-0.05 0.10; % xMin, xMax - -0.025 0.025]; % yMin, yMax - -leg_size = [-0.025 0.05 ; % xMin, xMax - -0.025 0.025]*2; % yMin, yMax \ No newline at end of file diff --git a/torque-controllers/momentum-based-standup/initTorqueBalancingStandup.m b/torque-controllers/momentum-based-standup/initTorqueBalancingStandup.m deleted file mode 100644 index 4302695..0000000 --- a/torque-controllers/momentum-based-standup/initTorqueBalancingStandup.m +++ /dev/null @@ -1,110 +0,0 @@ -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% /** -% * 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 -% */ -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% In the Simulink model, this script is run every time the user presses -% the 'start' button. -clearvars -except sl_synch_handles torqueBalGUI -clc - -% Import +wbc scope and add path to "src" folder -import wbc.* -addpath('./src/') - -%% GENERAL SIMULATION INFO -% If you are simulating the robot with Gazebo, -% remember that you have to launch Gazebo as follow: -% -% gazebo -slibgazebo_yarp_clock.so -% -% and set the environmental variable YARP_ROBOT_NAME in the .bashrc file. - -% Simulation time in seconds -Config.SIMULATION_TIME = 600000; % high number (not inf) - -%% PRELIMINARY CONFIGURATIONS -% Sm.SM_TYPE: defines the kind of state machines that can be chosen. -% -% 'STANDUP': the robot will stand up from a chair. The associated -% configuration parameters can be found under the folder -% -% robots/YARP_ROBOT_NAME/initStateMachineStandup.m -% -% 'COORDINATOR': the robot can either stay still, or follow a desired -% center-of-mass trajectory. The associated configuration -% parameters can be found under the folder: -% -% app/robots/YARP_ROBOT_NAME/initCoordinator.m -% -SM_TYPE = 'STANDUP'; - -% Config.SCOPES: if set to true, all visualizers for debugging are active -Config.SCOPES_ALL = true; - -% You can also activate only some specific debugging scopes -Config.SCOPES_EXT_WRENCHES = false; -Config.SCOPES_GAIN_SCHE_INFO = false; -Config.SCOPES_MAIN = false; -Config.SCOPES_QP = false; -Config.SCOPES_INERTIA = true; - -% If Config.SAVE_WORKSPACE = True, every time the simulink model is run the -% workspace is saved after stopping the simulation -Config.SAVE_WORKSPACE = false; - -% If CHECK_INTEGRATION_TIME = True, after stopping the simulation the -% Simulink time is compared with the Yarp time to check if the desired -% integration time step is respected -Config.CHECK_INTEGRATION_TIME = true; -Config.PLOT_INTEGRATION_TIME = false; - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% CONFIGURATIONS COMPLETED: loading gains and parameters for the specific robot -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Controller period [s] -Config.Ts = 0.01; - -% Run robot-specific configuration parameters -run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configRobot.m')); - -% Run demo-specific configuration parameters -Sm.SM_MASK_COORDINATOR = bin2dec('001'); -Sm.SM_MASK_STANDUP = bin2dec('010'); - -if strcmpi(SM_TYPE, 'COORDINATOR') - - Sm.SM_TYPE_BIN = Sm.SM_MASK_COORDINATOR; - demoSpecificParameters = fullfile('app/robots',getenv('YARP_ROBOT_NAME'),'initCoordinator.m'); - run(demoSpecificParameters); - % If true, the robot will perform the STANDUP demo - Config.iCubStandUp = false; - -elseif strcmpi(SM_TYPE, 'STANDUP') - - Sm.SM_TYPE_BIN = Sm.SM_MASK_STANDUP; - demoSpecificParameters = fullfile('app/robots',getenv('YARP_ROBOT_NAME'),'initStateMachineStandup.m'); - run(demoSpecificParameters); - % If true, the robot will perform the STANDUP demo - Config.iCubStandUp = true; -end - -%% Contact constraints: legs and feet -% feet constraints -[ConstraintsMatrixFeet,bVectorConstraintsFeet] = constraints(forceFrictionCoefficient,numberOfPoints,torsionalFrictionCoefficient,feet_size,fZmin); -% legs constraints -[ConstraintsMatrixLegs,bVectorConstraintsLegs] = constraints(forceFrictionCoefficient,numberOfPoints,torsionalFrictionCoefficient,leg_size,fZmin); \ No newline at end of file diff --git a/torque-controllers/momentum-based-standup/src-gui/closeModel.m b/torque-controllers/momentum-based-standup/src-gui/closeModel.m deleted file mode 100644 index 78885f3..0000000 --- a/torque-controllers/momentum-based-standup/src-gui/closeModel.m +++ /dev/null @@ -1,17 +0,0 @@ -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Save and close the Simulink model through Matlab command line. -% It also closes the associate GUI - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -disp('[closeModel]: closing the model...') - -save_system('torqueBalancingStandup.mdl'); -close_system('torqueBalancingStandup.mdl'); -close all - -% remove paths -rmpath('../../library/matlab-gui'); -rmpath('./src-gui'); - -disp('[closeModel]: done') \ No newline at end of file diff --git a/torque-controllers/momentum-based-standup/src/balancingControllerStandup.m b/torque-controllers/momentum-based-standup/src/balancingControllerStandup.m deleted file mode 100644 index c73a135..0000000 --- a/torque-controllers/momentum-based-standup/src/balancingControllerStandup.m +++ /dev/null @@ -1,275 +0,0 @@ -function [tauModel, Sigma, NA, f_LDot, ... - HessianMatrixQP1Foot, gradientQP1Foot, ConstraintsMatrixQP1Foot, bVectorConstraintsQp1Foot, ... - HessianMatrixQP2FeetOrLegs, gradientQP2FeetOrLegs, ConstraintsMatrixQP2FeetOrLegs, bVectorConstraintsQp2FeetOrLegs, ... - errorCoM, f_noQP, correctionFromSupportForce, L_error, V] = ... - balancingControllerStandup(constraints, ROBOT_DOF_FOR_SIMULINK, ConstraintsMatrix, bVectorConstraints, ... - qj, qjDes, nu, M, h, L, intLw, w_H_l_contact, w_H_r_contact, JL, JR, dJL_nu, dJR_nu, xCoM, J_CoM, desired_x_dx_ddx_CoM, ... - gainsPCOM, gainsDCOM, impedances, Reg, Gain, w_H_lArm, w_H_rArm, LArmWrench, RArmWrench, STANDUP_WITH_HUMAN, state) - - % BALANCINGCONTROLLERSTANDUP momentum-based balancing controller. - % - - %% --- Initialization --- - - %% DEFINITION OF CONTROL AND DYNAMIC VARIABLES - pos_leftContact = w_H_l_contact(1:3,4); - w_R_l_contact = w_H_l_contact(1:3,1:3); - pos_rightContact = w_H_r_contact(1:3,4); - w_R_r_contact = w_H_r_contact(1:3,1:3); - - % arms position - pos_leftArm = w_H_lArm(1:3,4); - pos_rightArm = w_H_rArm(1:3,4); - - dampings = Gain.dampings; - ROBOT_DOF = size(ROBOT_DOF_FOR_SIMULINK,1); - gravAcc = 9.81; - - % Mass of the robot - m = M(1,1); - - % The mass matrix is partitioned as: - % - % M = [ Mb, Mbj - % Mbj', Mj ]; - % - % where: Mb \in R^{6x6} - % Mbj \in R^{6x6+nDof} - % Mj \in R^{nDofxnDof} - % - Mb = M(1:6,1:6); - Mbj = M(1:6,7:end); - Mj = M(7:end,7:end); - - St = [zeros(6,ROBOT_DOF); - eye(ROBOT_DOF,ROBOT_DOF)]; - gravityWrench = [zeros(2,1); - -m*gravAcc; - zeros(3,1)]; - - % Velocity of the center of mass - xCoM_dot = J_CoM(1:3,:)*nu; - - % Joint velocity - qjDot = nu(7:end); - - % Joint position error - qjTilde = qj-qjDes; - - % Desired acceleration for the center of mass - xDDcomStar = desired_x_dx_ddx_CoM(:,3) -gainsPCOM.*(xCoM - desired_x_dx_ddx_CoM(:,1)) -gainsDCOM.*(xCoM_dot - desired_x_dx_ddx_CoM(:,2)); - - % Application point of the contact force on the right contact link w.r.t. CoM - Pr = pos_rightContact -xCoM; - - % Application point of the contact force on the left contact link w.r.t. CoM - Pl = pos_leftContact -xCoM; - - % Arms location w.r.t. CoM - PlArm = pos_leftArm -xCoM; - PrArm = pos_rightArm -xCoM; - - % The following variables serve for determening the rate-of-change of - % the robot's momentum. In particular, when balancing on two feet, one has: - % - % dot(L) = gravityWrench + AL*f_L + AR*f_R - % = gravityWrench + [AL,AR]*f - % - % where f_L and f_R are the contact wrenches acting on the left and - % right foot, respectively, and f = [f_L;f_R]. - % - AL = [ eye(3),zeros(3); - wbc.skew(Pl), eye(3)]; - AR = [ eye(3), zeros(3); - wbc.skew(Pr), eye(3)]; - - % dot(L) = mg + A*f - A = [AL, AR]; - - % matrix multipliyng the forces at arms - A_lArm = [ eye(3), zeros(3); - wbc.skew(PlArm), eye(3)]; - A_rArm = [ eye(3), zeros(3); - wbc.skew(PrArm), eye(3)]; - - A_arms = [A_lArm, A_rArm]; - - pinvA = pinv(A, Reg.pinvTol)*constraints(1)*constraints(2) ... - + [inv(AL);zeros(6)]*constraints(1)*(1-constraints(2)) ... - + [zeros(6);inv(AR)]*constraints(2)*(1-constraints(1)); - - % Null space of the matrix A - NA = (eye(12,12)-pinvA*A)*constraints(1)*constraints(2); - - % Total wrench applied at arms. External force is decomposed into two components - % along the direction of momentum error. Then, the parallel component is considered - fArms = [LArmWrench; - RArmWrench]; - - % support force - fsupport = A_arms * fArms; - - % Time varying contact jacobian - Jc = [JL*constraints(1); - JR*constraints(2)]; - - % Time varying dot(J)*nu - Jc_nuDot = [dJL_nu*constraints(1) ; - dJR_nu*constraints(2)]; - - JcMinv = Jc/M; - JcMinvSt = JcMinv*St; - JcMinvJct = JcMinv*transpose(Jc); - - % multiplier of f in tau - JBar = transpose(Jc(:,7:end)) -Mbj'/Mb*transpose(Jc(:,1:6)); - - Pinv_JcMinvSt = wbc.pinvDamped(JcMinvSt,Reg.pinvDamp); - - % nullJcMinvSt --> null space of Pinv_JcMinvSt - nullJcMinvSt = eye(ROBOT_DOF) - Pinv_JcMinvSt*JcMinvSt; - - % Mbar is the mass matrix associated with the joint dynamics, i.e. - Mbar = Mj-Mbj'/Mb*Mbj; - NLMbar = nullJcMinvSt*Mbar; - - % Adaptation of control gains for back compatibility with older - % versions of the controller - impedances = diag(impedances)*pinv(NLMbar,Reg.pinvTol) + Reg.impedances*eye(ROBOT_DOF); - dampings = diag(dampings)*pinv(NLMbar,Reg.pinvTol) + Reg.dampings*eye(ROBOT_DOF); - - % desired robot momentum - L_desired = [m.*desired_x_dx_ddx_CoM(:,2); - zeros(3,1)]; - - % momentum error - L_error = L -L_desired; - - % projector of contact forces into the direction parallel to momentum - % error - alpha = (transpose(L_error)*fsupport)/(norm(L_error)+Reg.norm_tolerance); - L_errParallel = L_error/(norm(L_error)+Reg.norm_tolerance); - - if STANDUP_WITH_HUMAN && alpha <= 0 && state < 4 - - correctionFromSupportForce = alpha*L_errParallel; - else - correctionFromSupportForce = zeros(6,1); - end - - %% QP PARAMETERS FOR TWO FEET STANDING - % In the case the robot stands on two feet/legs, the control objective is - % the minimization of the joint torques through the redundancy of the - % contact forces. By direct calculations one shows that the joint - % torques take the following form: - % - % 0) tau = tauModel + Sigma*f_LDot + SigmaNA*f0 - % - % where f0 is the redundancy of the contact wrenches. Then, the problem - % is defined as follows: - % - % 1) f0 = argmin |tau(f0)|^2 - % s.t. - % ConstraintsMatrixQP2Contacts*f0 < bVectorConstraintsQp2Contacts - % - % Update constraint matrices. The constraint matrix for the inequality - % constraints in the problem 1) is built up starting from the constraint - % matrix associated with each single contact. More precisely, the contact - % wrench associated with the left contact (resp. right contact) is subject to - % the following constraint: - % - % constraintMatrixLeftContact*l_sole_f_L < bVectorConstraints - % - % In this case, however, f_L is expressed w.r.t. the frame l_contact, - % which is solidal to the left contact. Since the controller uses contact - % wrenches expressed w.r.t. a frame whose orientation is that of the - % inertial frame, we have to update the constraint matrix according to - % the transformation w_R_l_contact, i.e. - % - % constraintMatrixLeftContact = ConstraintsMatrix*w_R_l_contact - % - % The same hold for the right foot - constraintMatrixLeftContact = ConstraintsMatrix * blkdiag(w_R_l_contact',w_R_l_contact'); - constraintMatrixRightContact = ConstraintsMatrix * blkdiag(w_R_r_contact',w_R_r_contact'); - ConstraintsMatrix2FeetOrLegs = blkdiag(constraintMatrixLeftContact,constraintMatrixRightContact); - bVectorConstraints2FeetOrLegs = [bVectorConstraints;bVectorConstraints]; - - % Terms used in Eq. 0) - tauModel = Pinv_JcMinvSt*(JcMinv*h - Jc_nuDot) + nullJcMinvSt*(h(7:end) - Mbj'/Mb*h(1:6) ... - -impedances*NLMbar*qjTilde -dampings*NLMbar*qjDot); - - Sigma = -(Pinv_JcMinvSt*JcMinvJct + nullJcMinvSt*JBar); - - % Desired rate-of-change of the robot momentum - LDotDes = [m*xDDcomStar ; - -Gain.KD_AngularMomentum*L(4:end)-Gain.KP_AngularMomentum*intLw] +correctionFromSupportForce; - - % computing Lyapunov function - int_L_tilde_times_gain = [m * gainsPCOM .* (xCoM - desired_x_dx_ddx_CoM(:,1)); - Gain.KP_AngularMomentum .* intLw]; - - V = 0.5 * transpose(L_error) * L_error; - V = V + 0.5 * transpose(int_L_tilde_times_gain) * int_L_tilde_times_gain; - - % Contact wrenches realizing the desired rate-of-change of the robot - % momentum LDotDes when standing on two feet. Note that f_LDot is - % different from zero only when both foot are in contact, i.e. - % constraints(1) = constraints(2) = 1. This because when the robot - % stands on one foot, the f_LDot is evaluated directly from the - % optimizer (see next section). - f_LDot = pinvA*(LDotDes - gravityWrench)*constraints(1)*constraints(2); - SigmaNA = Sigma*NA; - - % The optimization problem 1) 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 - % - % ConstraintsMatrix2Feet*f < bVectorConstraints, - % - % which in terms of f0 is: - % - % ConstraintsMatrix2Feet*NA*f0 < bVectorConstraints - ConstraintsMatrix2Feet*f_LDot - ConstraintsMatrixQP2FeetOrLegs = ConstraintsMatrix2FeetOrLegs*NA; - bVectorConstraintsQp2FeetOrLegs = bVectorConstraints2FeetOrLegs-ConstraintsMatrix2FeetOrLegs*f_LDot; - - % Evaluation of Hessian matrix and gradient vector for solving the - % optimization problem 1). - HessianMatrixQP2FeetOrLegs = SigmaNA'*SigmaNA + eye(size(SigmaNA,2))*Reg.HessianQP; - gradientQP2FeetOrLegs = SigmaNA'*(tauModel + Sigma*f_LDot); - - %% QP PARAMETERS FOR ONE FOOT STANDING - % In the case the robot stands on one foot, there is no redundancy of - % the contact wrenches. Hence, we cannot use this redundancy for - % minimizing the joint torques. For this reason, the minimization - % problem is modified as follows: - % - % 2) f = argmin|dot(L)(f) - dot(L)_des|^2 - % s.t. - % ConstraintsMatrixQP1Foot*f < bVectorConstraintsQp1Foot - % - % where f is the contact wrench either of the left or on the right - % foot. - % - ConstraintsMatrixQP1Foot = constraints(1) * (1 - constraints(2)) * ConstraintsMatrix + ... - constraints(2) * (1 - constraints(1)) * ConstraintsMatrix; - bVectorConstraintsQp1Foot = bVectorConstraints; - - A1Foot = AL*constraints(1)*(1-constraints(2)) + AR*constraints(2)*(1-constraints(1)); - HessianMatrixQP1Foot = A1Foot'*A1Foot + eye(size(A1Foot,2))*Reg.HessianQP; - gradientQP1Foot = -A1Foot'*(LDotDes - gravityWrench); - - %% DEBUG DIAGNOSTICS - - % Unconstrained solution for the problem 1) - f0 = -wbc.pinvDamped(SigmaNA, Reg.pinvDamp*1e-5)*(tauModel + Sigma*f_LDot); - - % Unconstrained contact wrenches - f_noQP = pinvA*(LDotDes - gravityWrench) + NA*f0*constraints(1)*constraints(2); - - % Error on the center of mass - errorCoM = xCoM - desired_x_dx_ddx_CoM(:,1); -end diff --git a/torque-controllers/momentum-based-standup/src/legsContactDetector.m b/torque-controllers/momentum-based-standup/src/legsContactDetector.m deleted file mode 100644 index 6fb7663..0000000 --- a/torque-controllers/momentum-based-standup/src/legsContactDetector.m +++ /dev/null @@ -1,15 +0,0 @@ -function legsInContact = legsContactDetector(icubStandup,state) - - % LEGSCONTACTDETECTOR activates the contacts at legs if the current state - % of the state machine is lower than state 3 (two feet balancing) - % - - %% --- Initialization --- - - legsInContact = 0; - - if (state < 3) && icubStandup - - legsInContact = 1; - end -end diff --git a/torque-controllers/momentum-based-standup/src/stateMachineStandup.m b/torque-controllers/momentum-based-standup/src/stateMachineStandup.m deleted file mode 100644 index e4bc304..0000000 --- a/torque-controllers/momentum-based-standup/src/stateMachineStandup.m +++ /dev/null @@ -1,118 +0,0 @@ -function [w_H_b, constraints, CoM_des, qj_des, impedances, KPCoM, KDCoM, currentState, jointsAndCoMSmoothingTime] = ... - stateMachineStandup(wrench_rightFoot, wrench_leftFoot, wrench_leftHand, wrench_rightHand, xCoM_0, qj_0, xCoM, l_sole_H_b, l_upper_leg_contact_H_b, t, STANDUP_WITH_HUMAN, Sm, Gain) - - % SATEMACHINESTANDUP state machine for performing standup movements. - % - - %% --- Initialization --- - - persistent state; - persistent tSwitch; - persistent w_H_fixedLink; - persistent CoMprevious; - - if isempty(state) || isempty(tSwitch) || isempty(w_H_fixedLink) || isempty(CoMprevious) - state = Sm.stateAt0; - tSwitch = 0; - w_H_fixedLink = l_sole_H_b/l_upper_leg_contact_H_b; - CoMprevious = xCoM_0; - end - - constraints = [1;1]; - w_H_b = eye(4); - qj_des = qj_0; - CoM_des = xCoM_0; - jointsAndCoMSmoothingTime = Sm.smoothingTimeCoM_Joints(state); - - %% BALANCING ON THE LEGS - if state == 1 - - w_H_b = w_H_fixedLink * l_upper_leg_contact_H_b; - jointsAndCoMSmoothingTime = Sm.smoothingTimeCoM_Joints(state); - - % after tBalancing time, start moving CoM forward. If - % Config.STANDUP_WITH_HUMAN is enbabled, the robot waits for external - % help before lifting up. - if STANDUP_WITH_HUMAN - - if t > Sm.tBalancing && wrench_rightHand(1) > Sm.wrench_thresholdContactRHand(state) && wrench_leftHand(1) > Sm.wrench_thresholdContactLHand(state) - state = 2; - tSwitch = t; - end - else - if t > Sm.tBalancing - state = 2; - tSwitch = t; - end - end - end - - %% MOVE COM FORWARD - if state == 2 - - w_H_b = w_H_fixedLink * l_upper_leg_contact_H_b; - - % setup new desired position for some joints: remapper - qj_des([18 19 21 22]) = Sm.joints_standUpPositions(state,[1 2 3 4]); - qj_des([12 13 15 16]) = Sm.joints_standUpPositions(state,[1 2 3 4]); - qj_des([8 9 10 11]) = Sm.joints_standUpPositions(state,[5 6 7 8]); - qj_des([4 5 6 7]) = Sm.joints_standUpPositions(state,[5 6 7 8]); - qj_des(1) = Sm.joints_standUpPositions(state,9); - - tDelta = t-tSwitch; - CoM_des = xCoM_0 + transpose(Sm.CoM_delta(state,:)); - jointsAndCoMSmoothingTime = Sm.smoothingTimeCoM_Joints(state); - - if (wrench_rightFoot(3)+wrench_leftFoot(3)) > (Sm.wrench_thresholdContactLFoot(state) + Sm.wrench_thresholdContactRFoot(state)) && tDelta > 1.5 - state = 3; - w_H_fixedLink = w_H_fixedLink * l_upper_leg_contact_H_b/l_sole_H_b; - tSwitch = t; - CoMprevious = xCoM; - end - end - - %% TWO FEET BALANCING - if state == 3 - - w_H_b = w_H_fixedLink * l_sole_H_b; - - % setup new desired position for some joints: remapper - qj_des([18 19 21 22]) = Sm.joints_standUpPositions(state,[1 2 3 4]); - qj_des([12 13 15 16]) = Sm.joints_standUpPositions(state,[1 2 3 4]); - qj_des([8 9 10 11]) = Sm.joints_standUpPositions(state,[5 6 7 8]); - qj_des([4 5 6 7]) = Sm.joints_standUpPositions(state,[5 6 7 8]); - qj_des(1) = Sm.joints_standUpPositions(state,9); - - CoM_des = CoMprevious + transpose(Sm.CoM_delta(state,:)); - tDelta = t-tSwitch; - jointsAndCoMSmoothingTime = Sm.smoothingTimeCoM_Joints(state); - - if (wrench_leftFoot(3) > Sm.wrench_thresholdContactLFoot(state)) && (wrench_rightFoot(3) > Sm.wrench_thresholdContactRFoot(state)) && tDelta > 0.5 - state = 4; - tSwitch = t; - CoMprevious = xCoM; - end - end - - %% LIFTING UP - if state == 4 - - w_H_b = w_H_fixedLink * l_sole_H_b; - - % setup new desired position for some joints: remapper - qj_des([18 19 21 22]) = Sm.joints_standUpPositions(state,[1 2 3 4]); - qj_des([12 13 15 16]) = Sm.joints_standUpPositions(state,[1 2 3 4]); - qj_des([8 9 10 11]) = Sm.joints_standUpPositions(state,[5 6 7 8]); - qj_des([4 5 6 7]) = Sm.joints_standUpPositions(state,[5 6 7 8]); - qj_des(1) = Sm.joints_standUpPositions(state,9); - - CoM_des = CoMprevious + transpose(Sm.CoM_delta(state,:)); - jointsAndCoMSmoothingTime = Sm.smoothingTimeCoM_Joints(state); - end - - currentState = state; - impedances = Gain.impedances(state,:); - KPCoM = Gain.KP_COM(state,:); - KDCoM = Gain.KD_COM(state,:); - -end diff --git a/torque-controllers/momentum-based-standup/torqueBalancingStandup.mdl b/torque-controllers/momentum-based-standup/torqueBalancingStandup.mdl deleted file mode 100644 index a0f78fc..0000000 --- a/torque-controllers/momentum-based-standup/torqueBalancingStandup.mdl +++ /dev/null @@ -1,30531 +0,0 @@ -Model { - Name "torqueBalancingStandup" - Version 9.0 - SavedCharacterEncoding "UTF-8" - GraphicalInterface { - NumRootInports 0 - NumRootOutports 0 - ParameterArgumentNames "" - ComputedModelVersion "1.3311" - NumModelReferences 0 - NumTestPointedSignals 0 - NumProvidedFunctions 0 - NumRequiredFunctions 0 - NumResetEvents 0 - HasInitializeEvent 0 - HasTerminateEvent 0 - IsExportFunctionModel 0 - NumParameterArguments 0 - NumExternalFileReferences 99 - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Configuration" - Path "torqueBalancingStandup/Configuration" - SID "4999" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingStandup/Dump and visualize/Robot data/Get Measurement" - SID "5000" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingStandup/Dump and visualize/Robot data/Get Measurement1" - SID "5001" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" - Path "torqueBalancingStandup/Dynamics and Kinematics/Dynamics/Centroidal Momentum" - SID "2345" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Get Bias Forces" - Path "torqueBalancingStandup/Dynamics and Kinematics/Dynamics/Get Bias Forces" - SID "2348" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Mass Matrix" - Path "torqueBalancingStandup/Dynamics and Kinematics/Dynamics/Mass Matrix" - SID "2349" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Arm External Forces/r_sole1" - SID "4838" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Arm External Forces/r_sole2" - SID "4839" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Contacts kinematics/ " - SID "4851" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Contacts kinematics/ " - SID "4852" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Contacts kinematics/DotJ Nu l_sole " - SID "4853" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Contacts kinematics/DotJ Nu l_sole 1" - SID "4854" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Contacts kinematics/Jacobian l_sole" - SID "4855" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Contacts kinematics/Jacobian l_sole1" - SID "4856" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Contacts kinematics/Jacobian r_sole" - SID "4857" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Contacts kinematics/Jacobian r_sole1" - SID "4858" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Contacts kinematics/l_sole" - SID "4865" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Contacts kinematics/l_sole1" - SID "4866" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Contacts kinematics/r_sole" - SID "4867" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Contacts kinematics/r_sole1" - SID "4868" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/Jacobian com" - SID "2378" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Dynamics and Kinematics/Kinematics/xCom/CoM" - SID "4363" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "simulink/Logic and Bit\nOperations/Compare\nTo Zero" - Path "torqueBalancingStandup/Robot State and References/Compare To Zero" - SID "2916" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "simulink/Logic and Bit\nOperations/Compare\nTo Zero" - Path "torqueBalancingStandup/Robot State and References/Compare To Zero1" - SID "2917" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "simulink/Logic and Bit\nOperations/Bitwise\nOperator" - Path "torqueBalancingStandup/Robot State and References/Coordinator" - SID "2908" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingStandup/Robot State and References/Get Measurement" - SID "5012" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to " - "world transform (fixed base)" - SID "4071" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/Base to fixed_link/RFoot to " - "world transform (fixed base)" - SID "4084" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot" - " to base link transform /Fixed base to imu transform" - SID "4052" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot" - " to base link transform /Fixed base to root link transform" - SID "4053" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot" - " to base link transform /Neck Position" - SID "4056" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot" - " to base link transform /holder 1" - SID "4060" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot" - " to base link transform /holder 2" - SID "4061" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/Compute State Velocity/Feet " - "Jacobians/Jacobian LFoot" - SID "4094" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/Compute State Velocity/Feet " - "Jacobians/Jacobian RFoot" - SID "4095" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/Compute State Velocity/Get M" - "easurement" - SID "5013" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/IMU measurements" - SID "3222" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/holder " - SID "2109" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/holder 1" - SID "2110" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Robot State and References/Internal Coordinator/xCom/CoM" - SID "2141" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - Path "torqueBalancingStandup/Robot State and References/Select State and References/Minimum Jerk Trajecto" - "ry Generator1" - SID "5122" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - Path "torqueBalancingStandup/Robot State and References/Select State and References/Minimum Jerk Trajecto" - "ry Generator2" - SID "2153" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute State Velocity/Fixe" - "d Links Jacobians/ " - SID "4797" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute State Velocity/Fixe" - "d Links Jacobians/ " - SID "4798" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute State Velocity/Fixe" - "d Links Jacobians/ " - SID "4799" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute State Velocity/Fixe" - "d Links Jacobians/ " - SID "4800" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute State Velocity/Get " - "Measurement" - SID "5014" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute base to fixed link " - "transform/LFoot to base link transform /Fixed base to imu transform" - SID "4250" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/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/Yarp Read" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/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 "torqueBalancingStandup/Robot State and References/State Machine Standup/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 "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute base to fixed link " - "transform/LFoot to base link transform /holder 2" - SID "4259" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute base to fixed link " - "transform/LFoot to world transform (fixed base)" - SID "4269" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute base to fixed link " - "transform/LLeg to base link transform/Fixed base to imu transform" - SID "4275" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute base to fixed link " - "transform/LLeg to base link transform/Fixed base to root link transform" - SID "4276" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute base to fixed link " - "transform/LLeg to base link transform/Neck Position" - SID "4279" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute base to fixed link " - "transform/LLeg to base link transform/holder 1" - SID "4283" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute base to fixed link " - "transform/LLeg to base link transform/holder 2" - SID "4284" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Compute base to fixed link " - "transform/LLeg to world transform (fixed base)" - SID "4306" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/Minimum Jerk Trajectory Gen" - "erator" - SID "2176" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/holder 1" - SID "2187" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/holder 2" - SID "2188" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/inertial" - SID "2630" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/left_foot_wrench1" - SID "4547" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/left_foot_wrench2" - SID "4548" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/left_foot_wrench3" - SID "4549" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/right_foot_wrench1" - SID "4567" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/Robot State and References/State Machine Standup/xCom/CoM" - SID "4229" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "simulink/Logic and Bit\nOperations/Bitwise\nOperator" - Path "torqueBalancingStandup/Robot State and References/Yoga" - SID "2909" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyActuators/Set References" - Path "torqueBalancingStandup/Set References" - SID "2354" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Centroidal Momentum" - SID "3718" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Compute base to fixed link t" - "ransform/LFoot to base link transform /Fixed base to imu transform" - SID "4935" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Compute base to fixed link t" - "ransform/LFoot to base link transform /Fixed base to root link transform" - SID "4936" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Compute base to fixed link t" - "ransform/LFoot to base link transform /Neck Position" - SID "4939" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Compute base to fixed link t" - "ransform/LFoot to base link transform /holder 1" - SID "4943" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Compute base to fixed link t" - "ransform/LFoot to base link transform /holder 2" - SID "4944" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Compute base to fixed link t" - "ransform/LFoot to world transform (fixed base)" - SID "4954" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Compute base to fixed link t" - "ransform/LLeg to base link transform/Fixed base to imu transform" - SID "4960" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Compute base to fixed link t" - "ransform/LLeg to base link transform/Fixed base to root link transform" - SID "4961" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Compute base to fixed link t" - "ransform/LLeg to base link transform/Neck Position" - SID "4964" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Compute base to fixed link t" - "ransform/LLeg to base link transform/holder 1" - SID "4968" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Compute base to fixed link t" - "ransform/LLeg to base link transform/holder 2" - SID "4969" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Compute base to fixed link t" - "ransform/LLeg to world transform (fixed base)" - SID "4979" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Fixed Links Jacobians/ " - SID "4915" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Fixed Links Jacobians/ " - SID "4916" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Fixed Links Jacobians/ " - SID "4917" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/Fixed Links Jacobians/ " - SID "4918" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingStandup/controller_QP/Compute angular momentum integral/inertial" - SID "3724" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Match Signal Sizes" - Path "torqueBalancingStandup/controller_QP/Compute joint torques/QPSolver/One Foot/Match Signal Sizes1" - SID "5047" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/QP" - Path "torqueBalancingStandup/controller_QP/Compute joint torques/QPSolver/One Foot/QP Two Feet" - SID "5048" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Match Signal Sizes" - Path "torqueBalancingStandup/controller_QP/Compute joint torques/QPSolver/Two Feet/Match Signal Sizes" - SID "5060" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/QP" - Path "torqueBalancingStandup/controller_QP/Compute joint torques/QPSolver/Two Feet/QP Two Feet" - SID "5061" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingStandup/controller_QP/Compute joint torques (motor reflected inertia)/Get Measurement" - SID "5116" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingStandup/controller_QP/left_foot_wrench1" - SID "4833" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingStandup/controller_QP/left_foot_wrench2" - SID "4834" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Limits" - Path "torqueBalancingStandup/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/Get Limits" - SID "5135" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Simulator Synchronizer" - Path "torqueBalancingStandup/synchronizer/GAZEBO_SYNCHRONIZER/Simulator Synchronizer" - SID "2426" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Real Time Synchronizer" - Path "torqueBalancingStandup/synchronizer/REAL_TIME_SYNC/Real Time Synchronizer" - SID "2431" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Clock" - Path "torqueBalancingStandup/synchronizer/Yarp Clock" - SID "4828" - 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" - PostLoadFcn "%% Try to open the static GUI and try to adjust it \ntry\n\n torqueBalGUI = torqueBalancingGui;\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(torqueBalGUI)\n\nend" - InitFcn "%% initialize the simulation\ncd(fileparts(which(bdroot)));\ninitTorqueBalancingStandup;" - StartFcn "%% Try to edit the GUI (the user may have already closed it)\ntry\n\n %% Get the current status of t" - "he push button\n pushButtonString = sl_synch_handles.pushbutton5.String;\n\n %% If the string is 'Start Model'" - ", the start was not driven by the GUI.\n %% Therefore it is necessary to update the button message to 'Stop Model" - "' manually.\n\n if strcmp(pushButtonString, 'Start Model')\n\n set(sl_synch_handles.pushbutton5,'String','" - "Stop Model')\n set(sl_synch_handles.pushbutton5,'Backgroundcolor','r');\n set(sl_synch_handles.pushbut" - "ton5,'Enable','on')\n\n end \nend" - StopFcn "%% stop the simulation\ncd(fileparts(which(bdroot)));\nstopTorqueBalancingStandup;\n\n%% Try to edit the" - " GUI (the user may have already closed it)\ntry\n\n %% Get the current status of the push button\n pushButtonS" - "tring = sl_synch_handles.pushbutton5.String;\n\n %% If the string is 'Stop Model', the stop was not driven by the" - " GUI.\n %% Therefore it is necessary to update the button message to 'Start Model' manually.\n\n if strcmp(pus" - "hButtonString, 'Stop Model')\n\n set(sl_synch_handles.pushbutton5,'String','Start Model')\n set(sl_syn" - "ch_handles.pushbutton5,'Backgroundcolor',[0.8,0.8,0.8]);\n\n %% disable the button (only after compiling it w" - "ill be enables again)\n set(sl_synch_handles.pushbutton5,'Enable','inactive')\n end \nend" - LastSavedArchitecture "glnxa64" - Object { - $PropName "BdWindowsInfo" - $ObjectID 1 - $ClassName "Simulink.BDWindowsInfo" - Object { - $PropName "WindowsInfo" - $ObjectID 2 - $ClassName "Simulink.WindowInfo" - IsActive [1] - Location [134.0, 55.0, 3706.0, 2105.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] - } - Object { - $PropName "EditorsInfo" - $ObjectID 5 - $ClassName "Simulink.EditorInfo" - IsActive [1] - ViewObjType "SimulinkTopLevel" - LoadSaveID "0" - Extents [3668.0, 1809.0] - ZoomFactor [3.0706896551724134] - Offset [717.74003368893875, 346.44076361594608] - } - Object { - $PropName "DockComponentsInfo" - $ObjectID 6 - $ClassName "Simulink.DockComponentInfo" - Type "GLUE2:PropertyInspector" - ID "Property Inspector" - Visible [0] - CreateCallback "" - UserData "" - Floating [0] - DockPosition "Right" - Width [640] - Height [480] - } - WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" - "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAC6AP///wAAAAEAAAAAAAAAAPwCA" - "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAAOgD///8AAA56AAAHbQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACe/////wAAAAAAAAAA/////wEAAAEM/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGq/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAASj/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" - } - } - HideAutomaticNames on - Created "Tue Feb 18 10:13:36 2014" - Creator "daniele" - UpdateHistory "UpdateHistoryNever" - ModifiedByFormat "%" - LastModifiedBy "gnava" - ModifiedDateFormat "%" - LastModifiedDate "Mon Dec 17 19:11:20 2018" - RTWModifiedTimeStamp 466974680 - 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" - VisualizeLoggedSignalsWhenLoggingToFile off - PauseTimes "5" - NumberOfSteps 100 - 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 7 - $ClassName "Simulink.SimulationData.ModelLoggingInfo" - model_ "torqueBalancingStandup" - signals_ [] - overrideMode_ [0.0] - Array { - Type "Cell" - Dimension 1 - Cell "torqueBalancingStandup" - 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 8 - Version "1.17.1" - DisabledProps [] - Description "" - Array { - Type "Handle" - Dimension 9 - Simulink.SolverCC { - $ObjectID 9 - Version "1.17.1" - DisabledProps [] - Description "" - StartTime "0.0" - StopTime "Config.SIMULATION_TIME " - AbsTol "auto" - FixedStep "Config.Ts" - 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 10 - Version "1.17.1" - DisabledProps [] - Description "" - 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 11 - Version "1.17.1" - Array { - Type "Cell" - Dimension 8 - Cell "BooleansAsBitfields" - Cell "PassReuseOutputArgsAs" - Cell "PassReuseOutputArgsThreshold" - Cell "ZeroExternalMemoryAtStartup" - Cell "ZeroInternalMemoryAtStartup" - Cell "OptimizeModelRefInitCode" - Cell "NoFixptDivByZeroProtection" - Cell "UseSpecifiedMinMax" - PropName "DisabledProps" - } - Description "" - 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 12 - Version "1.17.1" - Array { - Type "Cell" - Dimension 1 - Cell "UseOnlyExistingSharedCode" - PropName "DisabledProps" - } - Description "" - 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 13 - Version "1.17.1" - DisabledProps [] - Description "" - 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 14 - Version "1.17.1" - DisabledProps [] - Description "" - 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 15 - Version "1.17.1" - DisabledProps [] - Description "" - 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 16 - 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" - } - SystemTargetFile "grt.tlc" - HardwareBoard "None" - TLCOptions "" - GenCodeOnly off - MakeCommand "make_rtw" - GenerateMakefile on - PackageGeneratedCodeAndArtifacts off - PackageName "" - TemplateMakefile "grt_default_tmf" - PostCodeGenCommand "" - Description "" - GenerateReport off - SaveLog 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 17 - 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 "" - 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 18 - Version "1.17.1" - Array { - Type "Cell" - Dimension 17 - 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" - PropName "DisabledProps" - } - Description "" - TargetFcnLib "ansi_tfl_table_tmw.mat" - TargetLibSuffix "" - TargetPreCompLibLocation "" - GenFloatMathFcnCalls "NOT IN USE" - TargetLangStandard "C89/C90 (ANSI)" - 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 off - CombineSignalStateStructs off - SuppressErrorStatus off - IncludeFileDelimiter "Auto" - ERTCustomFileBanners off - SupportAbsoluteTime on - LogVarNameModifier "rt_" - MatFileLogging on - MultiInstanceERTCode off - CodeInterfacePackaging "Nonreusable function" - SupportNonFinite on - 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 on - 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 19 - Version "1.17.1" - DisabledProps [] - Description "Simulink Coverage Configuration Component" - 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" - ExtraOptions "" - CurrentDlgPage "Solver" - ConfigPrmDlgPosition [ 75, 71, 1155, 671 ] - } - PropName "ConfigurationSets" - } - Simulink.ConfigSet { - $PropName "ActiveConfigurationSet" - $ObjectID 8 - } - Object { - $PropName "DataTransfer" - $ObjectID 20 - $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 { - 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 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 MultiPortSwitch - DataPortOrder "One-based contiguous" - Inputs "3" - DataPortIndices "{1,2,3}" - DataPortForDefault "Last data port" - DiagnosticForDefault "Error" - zeroidx off - InputSameDT on - OutMin "[]" - OutMax "[]" - OutDataTypeStr "Inherit: Inherit via internal rule" - LockScale off - RndMeth "Floor" - SaturateOnIntegerOverflow on - SampleTime "-1" - AllowDiffInputSizes off - } - 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 off - } - 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 - IsObserver 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 "Inherit from input (this choice will be removed - see release notes)" - FixptAsFi off - NumInputs "1" - SampleTime "0" - } - } - System { - Name "torqueBalancingStandup" - 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 "307" - ReportName "simulink-default.rpt" - SIDHighWatermark "5143" - Block { - BlockType Reference - Name "Configuration" - SID "4999" - Ports [] - Position [845, 600, 920, 630] - ZOrder 553 - HideAutomaticName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Configuration" - SourceType "" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - ConfigSource "Workspace" - ConfigObject "'WBTConfigRobot'" - RobotName "'icubSim'" - UrdfFile "'model.urdf'" - ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" - ControlBoardsNames "{'left_arm','right_arm','torso'}" - LocalName "'WBT'" - GravityVector "[0,0,-9.81]" - } - Block { - BlockType SubSystem - Name "Dump and visualize" - SID "2275" - Ports [] - Position [1700, 456, 1830, 489] - ZOrder 547 - ForegroundColor "blue" - BackgroundColor "[0.333333, 1.000000, 1.000000]" - ShowName off - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 21 - $ClassName "Simulink.Mask" - Display "disp('VISUALIZER')" - } - System { - Name "Dump and visualize" - 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 "349" - Block { - BlockType From - Name "From" - SID "4478" - Position [585, 161, 665, 179] - ZOrder 708 - GotoTag "tau_des" - TagVisibility "global" - } - Block { - BlockType From - Name "From1" - SID "4479" - Position [585, 251, 665, 269] - ZOrder 709 - GotoTag "state" - TagVisibility "global" - } - Block { - BlockType From - Name "From2" - SID "4480" - Position [585, 341, 665, 359] - ZOrder 710 - GotoTag "CoM_error" - TagVisibility "global" - } - Block { - BlockType From - Name "From3" - SID "4481" - Position [585, 431, 665, 449] - ZOrder 711 - GotoTag "CoM_des" - TagVisibility "global" - } - Block { - BlockType From - Name "From4" - SID "4482" - Position [585, 296, 665, 314] - ZOrder 712 - GotoTag "qj_des" - TagVisibility "global" - } - Block { - BlockType From - Name "From5" - SID "4483" - Position [585, 386, 665, 404] - ZOrder 713 - GotoTag "f_QP" - TagVisibility "global" - } - Block { - BlockType From - Name "From6" - SID "4484" - Position [585, 476, 665, 494] - ZOrder 714 - GotoTag "f_noQP" - TagVisibility "global" - } - Block { - BlockType From - Name "From7" - SID "4485" - Position [585, 206, 665, 224] - ZOrder 715 - GotoTag "w_H_b" - TagVisibility "global" - } - Block { - BlockType SubSystem - Name "Human assistance" - SID "4909" - Ports [1] - Position [715, 529, 850, 591] - ZOrder -14 - RequestExecContextInheritance off - System { - Name "Human assistance" - 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 "718" - Block { - BlockType Inport - Name "state" - SID "4910" - Position [295, 237, 325, 253] - ZOrder 890 - IconDisplay "Port number" - } - Block { - BlockType From - Name "From10" - SID "4908" - Position [280, 181, 360, 199] - ZOrder 889 - GotoTag "V" - TagVisibility "global" - } - Block { - BlockType From - Name "From8" - SID "4906" - Position [265, 21, 370, 39] - ZOrder 887 - GotoTag "corrFromSupportF" - TagVisibility "global" - } - Block { - BlockType From - Name "From9" - SID "4907" - Position [280, 101, 360, 119] - ZOrder 888 - GotoTag "L_error" - TagVisibility "global" - } - Block { - BlockType Scope - Name "Ltilde" - SID "4829" - Ports [2] - Position [505, 101, 535, 134] - ZOrder 885 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(ext" - "mgr.Configuration('Core','General UI',true),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration(" - "'Sources','WiredSimulink',true,'DataLoggingVariableName','ScopeData1','DataLoggingLimitDataPoints',true,'DataLo" - "ggingSaveFormat','Array','DataLoggingDecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visu" - "als','Time Domain',true),extmgr.Configuration('Tools','Plot Navigation',true),extmgr.Configuration('Tools','Mea" - "surements',true,'Version','2017b')),'Version','2018a','Position',[1000 454 560 420])" - NumInputPorts "2" - Floating off - } - Block { - BlockType Scope - Name "V_lyap" - SID "4830" - Ports [2] - Position [505, 181, 535, 214] - ZOrder 886 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(ext" - "mgr.Configuration('Core','General UI',true),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration(" - "'Sources','WiredSimulink',true,'DataLoggingVariableName','ScopeData2','DataLoggingLimitDataPoints',true,'DataLo" - "ggingSaveFormat','Array','DataLoggingDecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visu" - "als','Time Domain',true),extmgr.Configuration('Tools','Plot Navigation',true),extmgr.Configuration('Tools','Mea" - "surements',true,'Version','2017b')),'Version','2018a')" - NumInputPorts "2" - Floating off - } - Block { - BlockType Scope - Name "humanSupport6DForce" - SID "4831" - Ports [2] - Position [505, 21, 535, 54] - ZOrder 884 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(ext" - "mgr.Configuration('Core','General UI',true),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration(" - "'Sources','WiredSimulink',true,'DataLoggingVariableName','humanSupport6DForceData','DataLoggingSaveFormat','Str" - "uctureWithTime','DataLoggingDecimation','1','DataLoggingDecimateData',true,'DataLogging',true),extmgr.Configura" - "tion('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-10','MaxYLimReal','10','YLabelRe" - "al','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','Off','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',{{}},'UserDefinedChannelNames',{{" - "}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1)},'DisplayPropertyDefaults',struct('MinYLim" - "Real','-10','MaxYLimReal','10','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','Off','XGr" - "id',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" - "3922 0.686274509803922],'ColorOrder',[0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784313725" - "49;1 1 0.0666666666666667;0.717647058823529 0.274509803921569 1;0.392156862745098 0.831372549019608 0.074509803" - "9215686;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863])),extmgr.Configuration('Tools','Plot Nav" - "igation',true),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Position" - "',[670 236 580 480])" - NumInputPorts "2" - Floating off - } - Line { - ZOrder 427 - SrcBlock "From9" - SrcPort 1 - DstBlock "Ltilde" - DstPort 1 - } - Line { - ZOrder 429 - SrcBlock "state" - SrcPort 1 - Points [116, 0; 0, -40] - Branch { - ZOrder 425 - DstBlock "V_lyap" - DstPort 2 - } - Branch { - ZOrder 424 - Points [0, -80] - Branch { - ZOrder 422 - DstBlock "Ltilde" - DstPort 2 - } - Branch { - ZOrder 421 - Points [0, -80] - DstBlock "humanSupport6DForce" - DstPort 2 - } - } - } - Line { - ZOrder 426 - SrcBlock "From8" - SrcPort 1 - DstBlock "humanSupport6DForce" - DstPort 1 - } - Line { - ZOrder 428 - SrcBlock "From10" - SrcPort 1 - DstBlock "V_lyap" - DstPort 1 - } - } - } - Block { - BlockType Logic - Name "Logical\nOperator1" - SID "3273" - Ports [2, 1] - Position [715, 97, 745, 128] - ZOrder 705 - ShowName off - Operator "OR" - AllPortsSameDT off - OutDataTypeStr "boolean" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n3" - SID "3274" - Position [575, 99, 680, 111] - ZOrder 704 - ShowName off - Value "Config.SCOPES_ALL" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n4" - SID "3275" - Position [570, 114, 685, 126] - ZOrder 706 - ShowName off - Value "Config.SCOPES_MAIN" - } - Block { - BlockType SubSystem - Name "Robot data" - SID "4504" - Ports [8, 0, 1] - Position [715, 149, 855, 506] - ZOrder 707 - RequestExecContextInheritance off - System { - Name "Robot data" - 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 "217" - Block { - BlockType Inport - Name "tau_des" - SID "4505" - Position [65, 63, 95, 77] - ZOrder 591 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_b" - SID "4506" - Position [1380, 78, 1410, 92] - ZOrder 592 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4507" - Position [225, 143, 255, 157] - ZOrder 593 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj_des" - SID "4508" - Position [730, 218, 760, 232] - ZOrder 594 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "CoM_Error" - SID "4509" - Position [65, 473, 95, 487] - ZOrder 595 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "fDesQP" - SID "4510" - Position [815, 493, 845, 507] - ZOrder 596 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "CoM_des" - SID "4511" - Position [65, 508, 95, 522] - ZOrder 597 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "fDesNoQP" - SID "4512" - Position [815, 613, 845, 627] - ZOrder 598 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "4513" - Ports [] - Position [72, 290, 92, 310] - ZOrder 599 - } - Block { - BlockType Scope - Name "CoM" - SID "2286" - Ports [4] - Position [540, 465, 645, 600] - 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','comData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation" - "','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{s" - "truct('MinYLimReal','-0.18788','MaxYLimReal','0.45595','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','0.4" - "5595','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor" - "',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980392156" - "86 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215" - "686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title" - "','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0." - "5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" - "le','on')}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'CoM_Measured:1','CoM_Measured:2','CoM_Me" - "asured:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.1834','MaxYLimReal','0.4156','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.07450980" - "39215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" - " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'CoM_De" - "sired:1','CoM_Desired:2','CoM_Desired:3'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-0.11878','M" - "axYLimReal','0.07929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'" - "YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686" - "274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0" - ".16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 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',3,'LineNames',{{'CoM_Error:1','CoM_Error:2','CoM_Error:3'}},'ShowContent',true,'Placement',3),struct(" - "'MinYLimReal','0.75','MaxYLimReal','3.25','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',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',4)},'DisplayPropertyDefa" - "ults',struct('YLabelReal','','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor'" - ",[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666" - "666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137254" - "9019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0." - "650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none" - "','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct" - "('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineSt" - "yle','-','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','400','TimeRangeFrames','400','DisplayLayoutDimensions',[4 1]),extmgr.C" - "onfiguration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('" - "Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[76 190 1931 1191])" - NumInputPorts "4" - Floating off - } - Block { - BlockType Selector - Name "CoM6D -> \nCoMXYZ1" - SID "3703" - Ports [1, 1] - Position [1485, 161, 1525, 199] - 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 [1485, 66, 1525, 104] - 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 "3311" - Ports [1, 2] - Position [990, 468, 995, 532] - ZOrder 543 - ShowName off - Outputs "2" - DisplayOption "bar" - Port { - PortNumber 1 - Name "wL_QP" - } - Port { - PortNumber 2 - Name "wR_QP" - } - } - Block { - BlockType Demux - Name "Demux1" - SID "5018" - Ports [1, 5] - Position [440, 21, 445, 119] - ZOrder 662 - 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 "5019" - Ports [1, 5] - Position [440, 176, 445, 274] - ZOrder 663 - 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 "3312" - Ports [1, 2] - Position [1070, 531, 1075, 589] - ZOrder 544 - ShowName off - Outputs "2" - DisplayOption "bar" - Port { - PortNumber 1 - Name "error_L" - } - Port { - PortNumber 2 - Name "error_R" - } - } - Block { - BlockType Demux - Name "Demux4" - SID "3313" - Ports [1, 2] - Position [1070, 591, 1075, 649] - ZOrder 545 - ShowName off - Outputs "2" - DisplayOption "bar" - Port { - PortNumber 1 - Name "wL_NO_QP" - } - Port { - PortNumber 2 - Name "wR_NO_QP" - } - } - Block { - BlockType Demux - Name "Demux5" - SID "5020" - Ports [1, 5] - Position [440, 311, 445, 409] - ZOrder 664 - 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 "Demux6" - SID "5021" - Ports [1, 5] - Position [1085, 41, 1090, 139] - ZOrder 665 - 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 "5022" - Ports [1, 5] - Position [1085, 176, 1090, 274] - ZOrder 666 - 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 "5023" - Ports [1, 5] - Position [1085, 311, 1090, 409] - ZOrder 667 - 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 Gain - Name "Gain" - SID "2289" - Position [880, 72, 930, 108] - 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 "Gain3" - SID "2290" - Position [880, 207, 930, 243] - 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 [880, 342, 930, 378] - 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 "Get Measurement" - SID "5000" - Ports [0, 1] - Position [50, 346, 115, 374] - ZOrder 600 - BackgroundColor "[0.827500, 0.576500, 0.603900]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Torque" - } - Block { - BlockType Reference - Name "Get Measurement1" - SID "5001" - Ports [0, 1] - Position [710, 76, 780, 104] - ZOrder 601 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Position" - Port { - PortNumber 1 - Name "qj" - } - } - Block { - BlockType Selector - Name "Selector" - SID "2294" - Ports [1, 1] - Position [135, 506, 190, 524] - ZOrder 507 - ShowName off - NumberOfDimensions "2" - IndexOptions "Index vector (dialog),Index vector (dialog)" - Indices "[1:3],1" - OutputSizes "1,1" - Port { - PortNumber 1 - Name "CoM_Desired" - } - } - Block { - BlockType Sum - Name "Sum" - SID "2295" - Ports [2, 1] - Position [210, 215, 230, 235] - ZOrder 493 - ShowName off - IconShape "round" - Inputs "|+-" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - Port { - PortNumber 1 - Name "torqueError" - } - } - Block { - BlockType Sum - Name "Sum1" - SID "2296" - Ports [2, 1] - Position [255, 470, 275, 490] - ZOrder 276 - ShowName off - IconShape "round" - Inputs "|++" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - Port { - PortNumber 1 - Name "CoM_Measured" - } - } - Block { - BlockType Sum - Name "Sum2" - SID "2297" - Ports [2, 1] - Position [985, 550, 1005, 570] - ZOrder 484 - ShowName off - IconShape "round" - Inputs "|+-" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Sum - Name "Sum3" - SID "2298" - Ports [2, 1] - Position [815, 350, 835, 370] - ZOrder 506 - ShowName off - IconShape "round" - Inputs "-+|" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Scope - Name "base Pose" - SID "3704" - Ports [2] - Position [1620, 38, 1725, 227] - 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','basePoseData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecim" - "ation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplay" - "s',{struct('MinYLimReal','-0.26969','MaxYLimReal','0.44828','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag'" - ",'0.44828','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',{{'basePosition:1','basePosition:2','b" - "asePosition:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-1.22546','MaxYLimReal','1.24727','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',9,'LineNames',{{" - "'baseOrientation:1','baseOrientation:2','baseOrientation:3','baseOrientation:4','baseOrientation:5','baseOrient" - "ation:6','baseOrientation:7','baseOrientation:8','baseOrientation:9'}},'ShowContent',true,'Placement',2)},'Disp" - "layPropertyDefaults',struct('YLabelReal','','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',fa" - "lse,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1" - " 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.39215686274" - "5098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745" - "098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5" - ",'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" - "e','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," - "[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}" - "},'ShowContent',true,'Placement',1),'TimeRangeSamples','400','TimeRangeFrames','400','DisplayLayoutDimensions'," - "[2 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr." - "Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[635 427 1915 1075])" - NumInputPorts "2" - Floating off - } - Block { - BlockType Scope - Name "f" - SID "2318" - Ports [7] - Position [1230, 475, 1345, 675] - ZOrder 483 - 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','feetForcesData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataL" - "oggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispl" - "ays',{struct('MinYLimReal','-38.5356','MaxYLimReal','184.29767','YLabelReal','','MinYLimMag','0.00000','MaxYLim" - "Mag','184.29767','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',6,'LineNames',{{'wL_QP:1','wL_QP:2','wL_QP:3'" - ",'wL_QP:4','wL_QP:5','wL_QP:6'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-37.48755','MaxYLimRea" - "l','185.45061','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',true,'YGrid'" - ",true,'PlotMagPhase',false,'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',6,'LineNames',{{'wR_QP:1','wR_QP:2','wR_QP:3','wR_QP:4','wR_QP:5','wR_QP:6'}},'ShowContent',true,'Placement" - "',2),struct('MinYLimReal','-7.6662','MaxYLimReal','4.87037','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','%<" - "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',{{'error_L:1','error_L:2','error_L:3','error_L:4" - "','error_L:5','error_L:6'}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-4.87037','MaxYLimReal','7." - "6662','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',true,'YGrid',true,'Pl" - "otMagPhase',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.058823529411764" - "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','" - "LineWidth',0.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'),s" - "truct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'L" - "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',6,'Li" - "neNames',{{'error_R:1','error_R:2','error_R:3','error_R:4','error_R:5','error_R:6'}},'ShowContent',true,'Placem" - "ent',4),struct('MinYLimReal','-40.03612','MaxYLimReal','183.9086','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',6,'LineNames',{{'wL_NO_QP:1','wL_NO_QP:2','wL_NO_QP:3','" - "wL_NO_QP:4','wL_NO_QP:5','wL_NO_QP:6'}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-36.16374','Max" - "YLimReal','187.25001','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','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',6,'LineNames',{{'wR_NO_QP:1','wR_NO_QP:2','wR_NO_QP:3','wR_NO_QP:4','wR_NO_QP:5','wR_NO_QP:6'}},'Sho" - "wContent',true,'Placement',6),struct('MinYLimReal','0.875','MaxYLimReal','2.125','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.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',1,'LineNames',{{'state'}},'ShowContent',t" - "rue,'Placement',7)},'DisplayPropertyDefaults',struct('YLabelReal','','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',1),'TimeRange" - "Samples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[7 1]),extmgr.Configuration('Tools','Plot Naviga" - "tion',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Versi" - "on','2017b')),'Version','2018a','Location',[66 78 2561 1439])" - NumInputPorts "7" - Floating off - } - Block { - BlockType Scope - Name "qj" - SID "2333" - Ports [6] - Position [1240, 42, 1330, 158] - ZOrder 312 - ShowName 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','jointData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimati" - "on','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays'," - "{struct('MinYLimReal','-27.84492','MaxYLimReal','29.50386','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" - "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68" - "6274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62" - "3529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7" - "17647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" - "ible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','" - "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Mark" - "er','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "')}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'torso:1','torso:2','torso:3'}},'ShowContent',tr" - "ue,'Placement',1),struct('MinYLimReal','-27.84492','MaxYLimReal','29.50386','YLabelReal','','MinYLimMag','0','M" - "axYLimMag','10','LegendVisibility','on','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',4,'LineNames',{{'left_arm:1','left_arm:2','left" - "_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-27.84492','MaxYLimReal','29.503" - "86','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotM" - "agPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Col" - "orOrder',[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','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" - "ct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineN" - "ames',{{'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3),struct('Min" - "YLimReal','-27.84492','MaxYLimReal','29.50386','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibi" - "lity','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980392" - "2 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941176470" - "6 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588235" - "29 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," - "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')," - "struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'" - "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidt" - "h',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDe" - "finedChannelNames',{{}},'NumLines',6,'LineNames',{{'left_leg:1','left_leg:2','left_leg:3','left_leg:4','left_le" - "g:5','left_leg:6'}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-27.84492','MaxYLimReal','29.50386'" - ",'YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagP" - "hase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorO" - "rder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392" - "156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;" - "1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',6,'LineName" - "s',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right_leg:6'}},'ShowContent',true,'" - "Placement',5),struct('MinYLimReal','-27.84492','MaxYLimReal','29.50386','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',1,'LineNames',{{'state'}},'ShowContent',true,'Place" - "ment',6)},'DisplayPropertyDefaults',struct('MinYLimReal','-27.84492','MaxYLimReal','29.50386','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',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','2017b')),'Version','2018a','Location',[76 144 1387 833])" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "qjDes" - SID "2334" - Ports [6] - Position [1245, 177, 1330, 293] - ZOrder 501 - ShowName 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','jointDesiredData','DataLoggingSaveFormat','StructureWithTime','DataLoggingD" - "ecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDis" - "plays',{struct('MinYLimReal','-30.77436','MaxYLimReal','32.14439','YLabelReal','','MinYLimMag','0','MaxYLimMag'" - ",'10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor" - "',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980392156" - "86 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215" - "686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title" - "','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0." - "5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" - "le','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),st" - "ruct('MinYLimReal','-30.77436','MaxYLimReal','32.14439','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Leg" - "endVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" - "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352" - "9411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176" - "47058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" - "e','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'," - "[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker'" - ",'none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}" - "},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinY" - "LimReal','-30.77436','MaxYLimReal','32.14439','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibil" - "ity','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922" - " 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706" - " 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882352" - "9 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','" - "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" - "truct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'L" - "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDef" - "inedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','" - "-30.77436','MaxYLimReal','32.14439','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','%','LineProper" - "tiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" - "or',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle'," - "'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChanne" - "lNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-30.77436'" - ",'MaxYLimReal','32.14439','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',tr" - "ue,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0" - ".686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058823" - "53 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " - "1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache'" - ",{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none" - "','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct" - "('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{" - "}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-30.77436','MaxYLimR" - "eal','32.14439','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" - ",true,'PlotMagPhase',false,'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],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none" - "','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct" - "('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNam" - "es',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('MinYLimReal','-30.77436','MaxYL" - "imReal','32.14439','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',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','D" - "isplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),extmgr.C" - "onfiguration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[71 126 1789 1047])" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "qj_err" - SID "2335" - Ports [6] - Position [1245, 310, 1330, 430] - 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','jointErrorData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDec" - "imation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispl" - "ays',{struct('MinYLimReal','-35.04513','MaxYLimReal','5.70055','YLabelReal','','MinYLimMag','0.00000','MaxYLimM" - "ag','35.04513','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'Axes" - "TickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450" - "98039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745" - "098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 " - "0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','Line" - "Width',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placeme" - "nt',1),struct('MinYLimReal','-17.018','MaxYLimReal','13.97901','YLabelReal','','MinYLimMag','0','MaxYLimMag','1" - "0','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[" - "0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 " - "0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686" - ";0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','" - "%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none'," - "'Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" - "Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyl" - "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'" - "Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'" - ",'on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struc" - "t('MinYLimReal','-14.7238','MaxYLimReal','13.09087','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',3),struct('MinYLimR" - "eal','-49.66887','MaxYLimReal','60.79298','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',4),struct('MinYLimReal','-49." - "19464','MaxYLimReal','53.42299','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGr" - "id',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" - "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647" - "05882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039" - "21569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperties" - "Cache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.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')}},'UserDefinedChannelNam" - "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','0.625','MaxYLi" - "mReal','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.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],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineName" - "s',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','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',1),'TimeRangeSamples','40','" - "TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'Onc" - "eAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'" - "Version','2018a','Location',[314 388 1594 1036])" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "tauDes" - SID "2336" - Ports [6] - Position [540, 25, 645, 135] - 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','tauDesiredData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDec" - "imation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispl" - "ays',{struct('MinYLimReal','-23.10101','MaxYLimReal','3.19276','YLabelReal','','MinYLimMag','0.00000','MaxYLimM" - "ag','23.10101','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','-4.42598','MaxYLimReal','1.27717','YLabelReal','','MinY" - "LimMag','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.066666666" - "6666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.8313725" - "49019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0" - ".650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','Line" - "Width',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineS" - "tyle','-','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','lef" - "t_arm:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-4.41697','MaxYLim" - "Real','1.28037','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" - ",true,'PlotMagPhase',false,'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',4,'LineNames',{{'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3)" - ",struct('MinYLimReal','-17.19705','MaxYLimReal','3.90482','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','L" - "egendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686" - "274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.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',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','-17.13564','MaxYLimReal'" - ",'3.8831','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true," - "'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922" - "],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" - "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411" - "7647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color" - "',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',6," - "'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right_leg:6'}},'ShowConten" - "t',true,'Placement',5),struct('MinYLimReal','0.875','MaxYLimReal','2.125','YLabelReal','','MinYLimMag','0','Max" - "YLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesT" - "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509" - "8039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450" - "98039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863" - "],'Title','','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','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',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',6)},'D" - "isplayPropertyDefaults',struct('YLabelReal','','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',0,'LineNames',{{" - "[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions'" - ",[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr" - ".Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[371 497 1661 1218]" - ")" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "tauError" - SID "2337" - Ports [6] - Position [540, 178, 645, 292] - 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','tauErrorData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecim" - "ation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplay" - "s',{struct('MinYLimReal','-2.36328','MaxYLimReal','1.89167','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag'" - ",'2.36328','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',{{'torso:1','torso:2','torso:3'}},'Sho" - "wContent',true,'Placement',1),struct('MinYLimReal','-5.02419','MaxYLimReal','0.81261','YLabelReal','','MinYLimM" - "ag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0" - " 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666" - "667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137254901" - "9608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650" - "980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidt" - "h',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" - "olor',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle" - "','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'left_arm:1','left_ar" - "m:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-4.91628','MaxYLimReal" - "','0.78262','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',tru" - "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745098039" - "22],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784313" - "72549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294" - "117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Col" - "or',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle'," - "'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 " - "1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'," - "4,'LineNames',{{'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3),str" - "uct('MinYLimReal','-16.25383','MaxYLimReal','5.0836','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',6,'LineNames',{{'left_leg:1','left_leg:2','left_leg:3','left_leg:4','l" - "eft_leg:5','left_leg:6'}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-13.81087','MaxYLimReal','4.3" - "7989','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'Plo" - "tMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'C" - "olorOrder',[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','-','L" - "ineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','" - "none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" - "ruct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'Li" - "neStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',6,'Lin" - "eNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right_leg:6'}},'ShowContent',t" - "rue,'Placement',5),struct('MinYLimReal','0.875','MaxYLimReal','2.125','YLabelReal','','MinYLimMag','0','MaxYLim" - "Mag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickC" - "olor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.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','Visibl" - "e','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.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',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',6)},'Displ" - "ayPropertyDefaults',struct('YLabelReal','','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',0,'LineNames',{{[]}}" - ",'ShowContent',true,'Placement',1),'TimeRangeSamples','90','TimeRangeFrames','90','DisplayLayoutDimensions',[6 " - "1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Con" - "figuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[76 190 1356 823])" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "tauMeasured" - SID "2338" - Ports [6] - Position [540, 313, 645, 427] - 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','tauMeasuredData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDe" - "cimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisp" - "lays',{struct('MinYLimReal','-23.10101','MaxYLimReal','3.19276','YLabelReal','','MinYLimMag','0.00000','MaxYLim" - "Mag','23.10101','LegendVisibility','on','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',3,'LineNames',{{'torso:1','torso:2','torso:3'}}" - ",'ShowContent',true,'Placement',1),struct('MinYLimReal','-4.42598','MaxYLimReal','1.27717','YLabelReal','','Min" - "YLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'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',4,'LineNames',{{'left_arm:1','le" - "ft_arm:2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-4.41287','MaxYLi" - "mReal','1.27992','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',4,'LineNames',{{'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3" - "),struct('MinYLimReal','-18.64731','MaxYLimReal','16.95716','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',6,'LineNames',{{'left_leg:1','left_leg:2','left_leg:3','left_le" - "g:4','left_leg:5','left_leg:6'}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-18.32313','MaxYLimRea" - "l','14.57051','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('C" - "olor',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle" - "','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " - "0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines" - "',6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right_leg:6'}},'ShowCo" - "ntent',true,'Placement',5),struct('MinYLimReal','0.875','MaxYLimReal','2.125','YLabelReal','','MinYLimMag','0'," - "'MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'A" - "xesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.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],'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',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',6)" - "},'DisplayPropertyDefaults',struct('YLabelReal','','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',0,'LineNames" - "',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensi" - "ons',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),ex" - "tmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[76 179 1356 87" - "9])" - NumInputPorts "6" - Floating off - } - Line { - ZOrder 391 - SrcBlock "CoM_Error" - SrcPort 1 - Points [114, 0] - Branch { - ZOrder 346 - DstBlock "Sum1" - DstPort 1 - } - Branch { - ZOrder 3 - Points [0, 70] - DstBlock "CoM" - DstPort 3 - } - } - Line { - ZOrder 393 - SrcBlock "CoM_des" - SrcPort 1 - DstBlock "Selector" - DstPort 1 - } - Line { - ZOrder 589 - SrcBlock "Gain" - SrcPort 1 - DstBlock "Demux6" - DstPort 1 - } - Line { - Name "CoM_Measured" - ZOrder 5 - Labels [-1, 0] - SrcBlock "Sum1" - SrcPort 1 - DstBlock "CoM" - DstPort 1 - } - Line { - Name "qj" - ZOrder 480 - Labels [1, 1] - SrcBlock "Get Measurement1" - SrcPort 1 - Points [40, 0] - Branch { - ZOrder 364 - DstBlock "Gain" - DstPort 1 - } - Branch { - ZOrder 7 - DstBlock "Sum3" - DstPort 1 - } - } - Line { - Name "right_leg" - ZOrder 588 - Labels [-1, 0] - SrcBlock "Demux6" - SrcPort 5 - DstBlock "qj" - DstPort 5 - } - Line { - Name "torso" - ZOrder 584 - Labels [-1, 0] - SrcBlock "Demux6" - SrcPort 1 - DstBlock "qj" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 587 - Labels [-1, 0] - SrcBlock "Demux6" - SrcPort 2 - DstBlock "qj" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 586 - Labels [-1, 0] - SrcBlock "Demux6" - SrcPort 4 - DstBlock "qj" - DstPort 4 - } - Line { - ZOrder 392 - SrcBlock "fDesQP" - SrcPort 1 - Points [44, 0] - Branch { - ZOrder 377 - Points [0, 60] - DstBlock "Sum2" - DstPort 1 - } - Branch { - ZOrder 376 - DstBlock "Demux" - DstPort 1 - } - } - Line { - Name "right_arm" - ZOrder 585 - Labels [-1, 0] - SrcBlock "Demux6" - SrcPort 3 - DstBlock "qj" - DstPort 3 - } - Line { - ZOrder 25 - SrcBlock "Sum2" - SrcPort 1 - DstBlock "Demux3" - DstPort 1 - } - Line { - ZOrder 394 - SrcBlock "fDesNoQP" - SrcPort 1 - Points [145, 0] - Branch { - ZOrder 385 - DstBlock "Sum2" - DstPort 2 - } - Branch { - ZOrder 380 - DstBlock "Demux4" - DstPort 1 - } - } - Line { - ZOrder 387 - SrcBlock "tau_des" - SrcPort 1 - Points [41, 0] - Branch { - ZOrder 602 - DstBlock "Demux1" - DstPort 1 - } - Branch { - ZOrder 31 - Points [0, 155] - DstBlock "Sum" - DstPort 1 - } - } - Line { - ZOrder 479 - SrcBlock "Get Measurement" - SrcPort 1 - Points [100, 0] - Branch { - ZOrder 609 - DstBlock "Demux5" - DstPort 1 - } - Branch { - ZOrder 332 - DstBlock "Sum" - DstPort 2 - } - } - Line { - Name "right_leg" - ZOrder 598 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 5 - DstBlock "tauDes" - DstPort 5 - } - Line { - Name "torso" - ZOrder 601 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 1 - DstBlock "tauDes" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 599 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 2 - DstBlock "tauDes" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 597 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 4 - DstBlock "tauDes" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 600 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 3 - DstBlock "tauDes" - DstPort 3 - } - Line { - Name "right_leg" - ZOrder 595 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 5 - DstBlock "tauError" - DstPort 5 - } - Line { - Name "torso" - ZOrder 594 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 1 - DstBlock "tauError" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 592 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 2 - DstBlock "tauError" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 596 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 4 - DstBlock "tauError" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 593 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 3 - DstBlock "tauError" - DstPort 3 - } - Line { - Name "torqueError" - ZOrder 603 - Labels [0, 0] - SrcBlock "Sum" - SrcPort 1 - DstBlock "Demux2" - DstPort 1 - } - Line { - Name "right_leg" - ZOrder 606 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 5 - DstBlock "tauMeasured" - DstPort 5 - } - Line { - Name "torso" - ZOrder 604 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 1 - DstBlock "tauMeasured" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 605 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 2 - DstBlock "tauMeasured" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 607 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 4 - DstBlock "tauMeasured" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 608 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 3 - DstBlock "tauMeasured" - DstPort 3 - } - Line { - ZOrder 390 - SrcBlock "qj_des" - SrcPort 1 - Points [28, 0] - Branch { - ZOrder 52 - Points [0, 135] - DstBlock "Sum3" - DstPort 2 - } - Branch { - ZOrder 53 - DstBlock "Gain3" - DstPort 1 - } - } - Line { - ZOrder 590 - SrcBlock "Gain3" - SrcPort 1 - DstBlock "Demux7" - DstPort 1 - } - Line { - Name "right_leg" - ZOrder 581 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 5 - DstBlock "qjDes" - DstPort 5 - } - Line { - Name "torso" - ZOrder 579 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 1 - DstBlock "qjDes" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 580 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 2 - DstBlock "qjDes" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 582 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 4 - DstBlock "qjDes" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 583 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 3 - DstBlock "qjDes" - DstPort 3 - } - Line { - ZOrder 591 - SrcBlock "Gain4" - SrcPort 1 - DstBlock "Demux8" - DstPort 1 - } - Line { - Name "right_leg" - ZOrder 578 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 5 - DstBlock "qj_err" - DstPort 5 - } - Line { - Name "torso" - ZOrder 575 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 1 - DstBlock "qj_err" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 577 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 2 - DstBlock "qj_err" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 576 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 4 - DstBlock "qj_err" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 574 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 3 - DstBlock "qj_err" - DstPort 3 - } - Line { - ZOrder 70 - SrcBlock "Sum3" - SrcPort 1 - DstBlock "Gain4" - DstPort 1 - } - Line { - Name "CoM_Desired" - ZOrder 71 - SrcBlock "Selector" - SrcPort 1 - Points [70, 0] - Branch { - ZOrder 353 - DstBlock "Sum1" - DstPort 2 - } - Branch { - ZOrder 348 - Labels [-1, 0] - DstBlock "CoM" - DstPort 2 - } - } - Line { - ZOrder 389 - SrcBlock "state" - SrcPort 1 - Points [74, 0] - Branch { - ZOrder 75 - Points [76, 0] - Branch { - ZOrder 329 - Points [554, 0] - Branch { - ZOrder 78 - Points [0, 135] - Branch { - ZOrder 79 - Points [0, 135] - DstBlock "qj_err" - DstPort 6 - } - Branch { - ZOrder 82 - DstBlock "qjDes" - DstPort 6 - } - } - Branch { - ZOrder 83 - DstBlock "qj" - DstPort 6 - } - } - Branch { - ZOrder 76 - Points [0, -20] - DstBlock "tauDes" - DstPort 6 - } - } - Branch { - ZOrder 84 - Points [0, 135] - Branch { - ZOrder 85 - Points [0, 135] - Branch { - ZOrder 86 - Points [0, 165] - Branch { - ZOrder 350 - Points [0, 80] - DstBlock "f" - DstPort 7 - } - Branch { - ZOrder 88 - DstBlock "CoM" - DstPort 4 - } - } - Branch { - ZOrder 89 - DstBlock "tauMeasured" - DstPort 6 - } - } - Branch { - ZOrder 90 - DstBlock "tauError" - DstPort 6 - } - } - } - Line { - Name "wL_QP" - ZOrder 91 - Labels [-1, 0] - SrcBlock "Demux" - SrcPort 1 - DstBlock "f" - DstPort 1 - } - Line { - Name "wR_QP" - ZOrder 92 - Labels [-1, 0] - SrcBlock "Demux" - SrcPort 2 - DstBlock "f" - DstPort 2 - } - Line { - Name "error_L" - ZOrder 93 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 1 - DstBlock "f" - DstPort 3 - } - Line { - Name "error_R" - ZOrder 94 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 2 - DstBlock "f" - DstPort 4 - } - Line { - Name "wL_NO_QP" - ZOrder 95 - Labels [-1, 0] - SrcBlock "Demux4" - SrcPort 1 - DstBlock "f" - DstPort 5 - } - Line { - Name "wR_NO_QP" - ZOrder 96 - Labels [-1, 0] - SrcBlock "Demux4" - SrcPort 2 - DstBlock "f" - DstPort 6 - } - Line { - ZOrder 388 - SrcBlock "w_H_b" - SrcPort 1 - Points [24, 0] - Branch { - ZOrder 102 - Points [0, 95] - DstBlock "CoM6D -> \nCoMXYZ1" - DstPort 1 - } - Branch { - ZOrder 101 - DstBlock "CoM6D -> \nCoMXYZ2" - DstPort 1 - } - } - Line { - Name "basePosition" - ZOrder 103 - Labels [1, 1] - SrcBlock "CoM6D -> \nCoMXYZ2" - SrcPort 1 - DstBlock "base Pose" - DstPort 1 - } - Line { - Name "baseOrientation" - ZOrder 104 - Labels [1, 1] - SrcBlock "CoM6D -> \nCoMXYZ1" - SrcPort 1 - DstBlock "base Pose" - DstPort 2 - } - } - } - Line { - ZOrder 387 - SrcBlock "ON_GAZEBO\n3" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 1 - } - Line { - ZOrder 388 - SrcBlock "ON_GAZEBO\n4" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 2 - } - Line { - ZOrder 398 - SrcBlock "Logical\nOperator1" - SrcPort 1 - Points [35, 0] - DstBlock "Robot data" - DstPort enable - } - Line { - ZOrder 391 - SrcBlock "From2" - SrcPort 1 - DstBlock "Robot data" - DstPort 5 - } - Line { - ZOrder 392 - SrcBlock "From5" - SrcPort 1 - DstBlock "Robot data" - DstPort 6 - } - Line { - ZOrder 393 - SrcBlock "From6" - SrcPort 1 - DstBlock "Robot data" - DstPort 8 - } - Line { - ZOrder 394 - SrcBlock "From4" - SrcPort 1 - DstBlock "Robot data" - DstPort 4 - } - Line { - ZOrder 395 - SrcBlock "From7" - SrcPort 1 - DstBlock "Robot data" - DstPort 2 - } - Line { - ZOrder 396 - SrcBlock "From" - SrcPort 1 - DstBlock "Robot data" - DstPort 1 - } - Line { - ZOrder 397 - SrcBlock "From3" - SrcPort 1 - DstBlock "Robot data" - DstPort 7 - } - Line { - ZOrder 390 - SrcBlock "From1" - SrcPort 1 - Points [9, 0] - Branch { - ZOrder 429 - Points [0, 300] - DstBlock "Human assistance" - DstPort 1 - } - Branch { - ZOrder 419 - DstBlock "Robot data" - DstPort 3 - } - } - } - } - Block { - BlockType SubSystem - Name "Dynamics and Kinematics" - SID "2341" - Ports [4, 14] - Position [1195, 354, 1345, 681] - ZOrder 541 - BackgroundColor "[0.000000, 1.000000, 0.498039]" - RequestExecContextInheritance off - System { - Name "Dynamics and Kinematics" - 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 "293" - Block { - BlockType Inport - Name "w_H_b" - SID "2342" - Position [-850, -167, -820, -153] - ZOrder 209 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "2343" - Position [-850, -97, -820, -83] - ZOrder 224 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "nu" - SID "2344" - Position [-850, -27, -820, -13] - ZOrder 582 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state " - SID "4982" - Position [-465, 338, -435, 352] - ZOrder 901 - Port "4" - 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 [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 "492" - Block { - BlockType Inport - Name "w_H_b" - SID "4354" - Position [180, 28, 210, 42] - ZOrder 584 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "4355" - Position [180, 63, 210, 77] - ZOrder 585 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "nu" - SID "4356" - Position [20, 188, 50, 202] - ZOrder 586 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "Add motors reflected inertias" - SID "4983" - Ports [1, 1] - Position [575, 34, 700, 76] - ZOrder 590 - RequestExecContextInheritance off - System { - Name "Add motors reflected inertias" - 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 "891" - Block { - BlockType Inport - Name "M" - SID "4984" - Position [40, 138, 70, 152] - ZOrder 591 - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "Add motor reflected inertias" - SID "4985" - Ports [1, 1] - Position [140, 110, 305, 180] - 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 "70" - Block { - BlockType Inport - Name "M" - SID "4985::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4985::69" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 59 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4985::68" - Tag "Stateflow S-Function torqueBalancingStandup 6" - Ports [1, 2] - Position [180, 100, 230, 160] - ZOrder 58 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[1 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "M_with_inertia" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4985::70" - Position [460, 241, 480, 259] - ZOrder 60 - } - Block { - BlockType Outport - Name "M_with_inertia" - SID "4985::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 65 - SrcBlock "M" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - Name "M_with_inertia" - ZOrder 66 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "M_with_inertia" - DstPort 1 - } - Line { - ZOrder 67 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 68 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Outport - Name "M_with_motor_inertia" - SID "4986" - Position [385, 138, 415, 152] - ZOrder 592 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "M" - SrcPort 1 - DstBlock "Add motor reflected inertias" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "Add motor reflected inertias" - SrcPort 1 - DstBlock "M_with_motor_inertia" - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "Centroidal Momentum" - SID "2345" - Ports [4, 1] - Position [370, 281, 555, 344] - ZOrder 223 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" - SourceType "Centroidal Momentum" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Selector - Name "CoM6D -> \nCoMXYZ1" - SID "2346" - Ports [1, 1] - Position [200, 211, 240, 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 [200, 186, 240, 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 "Get Bias Forces" - SID "2348" - Ports [4, 1] - Position [395, 132, 535, 233] - ZOrder 222 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Get Bias Forces" - SourceType "Get Generalized Bias Forces" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "Mass Matrix" - SID "2349" - Ports [2, 1] - Position [395, 19, 535, 86] - ZOrder 221 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Mass Matrix" - SourceType "Mass Matrix" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Outport - Name "M(q)" - SID "4357" - Position [730, 48, 760, 62] - ZOrder 587 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "biasTorques" - SID "4358" - Position [625, 178, 655, 192] - ZOrder 588 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "L(q,dq)" - SID "4359" - Position [625, 308, 655, 322] - ZOrder 589 - Port "3" - IconDisplay "Port number" - } - Line { - ZOrder 81 - SrcBlock "Mass Matrix" - SrcPort 1 - DstBlock "Add motors reflected inertias" - DstPort 1 - } - Line { - ZOrder 83 - SrcBlock "Centroidal Momentum" - SrcPort 1 - DstBlock "L(q,dq)" - DstPort 1 - } - Line { - ZOrder 82 - SrcBlock "Get Bias Forces" - SrcPort 1 - DstBlock "biasTorques" - DstPort 1 - } - Line { - ZOrder 78 - SrcBlock "w_H_b" - SrcPort 1 - Points [108, 0] - Branch { - ZOrder 5 - Points [0, 110] - Branch { - ZOrder 6 - Points [0, 145] - DstBlock "Centroidal Momentum" - DstPort 1 - } - Branch { - ZOrder 7 - DstBlock "Get Bias Forces" - DstPort 1 - } - } - Branch { - ZOrder 8 - DstBlock "Mass Matrix" - DstPort 1 - } - } - Line { - ZOrder 79 - SrcBlock "qj" - SrcPort 1 - Points [97, 0] - Branch { - ZOrder 10 - Points [0, 100] - Branch { - ZOrder 11 - Points [0, 135] - DstBlock "Centroidal Momentum" - DstPort 2 - } - Branch { - ZOrder 12 - DstBlock "Get Bias Forces" - DstPort 2 - } - } - Branch { - ZOrder 13 - DstBlock "Mass Matrix" - DstPort 2 - } - } - Line { - ZOrder 14 - SrcBlock "CoM6D -> \nCoMXYZ2" - SrcPort 1 - Points [56, 0] - Branch { - ZOrder 75 - Points [0, 125] - DstBlock "Centroidal Momentum" - DstPort 3 - } - Branch { - ZOrder 73 - DstBlock "Get Bias Forces" - DstPort 3 - } - } - Line { - ZOrder 17 - SrcBlock "CoM6D -> \nCoMXYZ1" - SrcPort 1 - Points [35, 0] - Branch { - ZOrder 77 - Points [0, 115] - DstBlock "Centroidal Momentum" - DstPort 4 - } - Branch { - ZOrder 76 - DstBlock "Get Bias Forces" - 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 104 - SrcBlock "Add motors reflected inertias" - SrcPort 1 - DstBlock "M(q)" - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Kinematics" - SID "4321" - Ports [4, 11] - Position [-370, 25, -155, 395] - ZOrder 837 - RequestExecContextInheritance off - System { - Name "Kinematics" - 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 "310" - Block { - BlockType Inport - Name "nu" - SID "4342" - Position [-270, 493, -240, 507] - ZOrder 857 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "4339" - Position [-185, 233, -155, 247] - ZOrder 868 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_b" - SID "4336" - Position [-185, 283, -155, 297] - ZOrder 870 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state " - SID "4842" - Position [-275, 623, -245, 637] - ZOrder 883 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "Arm External Forces" - SID "4835" - Ports [2, 2] - Position [115, 113, 260, 177] - ZOrder 882 - RequestExecContextInheritance off - System { - Name "Arm External Forces" - 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 "841" - Block { - BlockType Inport - Name "w_H_b" - SID "4836" - Position [1065, 593, 1095, 607] - ZOrder 829 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "4837" - Position [1065, 668, 1095, 682] - ZOrder 830 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "r_sole1" - SID "4838" - Ports [2, 1] - Position [1260, 645, 1420, 685] - ZOrder 827 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_HAND" - } - Block { - BlockType Reference - Name "r_sole2" - SID "4839" - Ports [2, 1] - Position [1260, 590, 1420, 630] - ZOrder 828 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_HAND" - } - Block { - BlockType Outport - Name "w_H_LArm" - SID "4840" - Position [1455, 603, 1485, 617] - ZOrder 839 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_RArm" - SID "4841" - Position [1455, 658, 1485, 672] - ZOrder 840 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "qj" - SrcPort 1 - Points [85, 0] - Branch { - ZOrder 2 - Points [0, -55] - DstBlock "r_sole2" - DstPort 2 - } - Branch { - ZOrder 3 - DstBlock "r_sole1" - DstPort 2 - } - } - Line { - ZOrder 4 - SrcBlock "r_sole1" - SrcPort 1 - DstBlock "w_H_RArm" - DstPort 1 - } - Line { - ZOrder 5 - SrcBlock "w_H_b" - SrcPort 1 - Points [116, 0] - Branch { - ZOrder 6 - DstBlock "r_sole2" - DstPort 1 - } - Branch { - ZOrder 7 - Points [0, 55] - DstBlock "r_sole1" - DstPort 1 - } - } - Line { - ZOrder 8 - SrcBlock "r_sole2" - SrcPort 1 - DstBlock "w_H_LArm" - DstPort 1 - } - } - } - Block { - BlockType Selector - Name "CoM6D -> \nCoMXYZ3" - SID "2369" - Ports [1, 1] - Position [-95, 550, -55, 560] - 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 [-95, 495, -55, 505] - ZOrder 832 - ShowName off - InputPortWidth "6+ROBOT_DOF" - IndexOptions "Index vector (dialog)" - Indices "[1 2 3 4 5 6]" - OutputSizes "1" - } - Block { - BlockType Constant - Name "Constant" - SID "4843" - Position [-310, 582, -205, 598] - ZOrder 884 - Value "Config.iCubStandUp" - } - Block { - BlockType SubSystem - Name "Contacts kinematics" - SID "4845" - Ports [5, 6] - Position [100, 361, 275, 634] - ZOrder 886 - RequestExecContextInheritance off - System { - Name "Contacts kinematics" - 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 "337" - Block { - BlockType Inport - Name "w_H_b" - SID "4846" - Position [20, 88, 50, 102] - ZOrder 548 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "4847" - Position [20, 223, 50, 237] - ZOrder 553 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "nu_b" - SID "4848" - Position [390, 353, 420, 367] - ZOrder 554 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "dqj" - SID "4849" - Position [390, 413, 420, 427] - ZOrder 555 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "legsInContact" - SID "4850" - Position [20, 18, 50, 32] - ZOrder 562 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name " " - SID "4851" - Ports [4, 1] - Position [590, 334, 735, 376] - ZOrder 568 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - SourceType "DotJ Nu" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_LEG" - } - Block { - BlockType Reference - Name " \n " - SID "4852" - Ports [4, 1] - Position [590, 384, 735, 426] - ZOrder 547 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - SourceType "DotJ Nu" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Reference - Name "DotJ Nu l_sole\n" - SID "4853" - Ports [4, 1] - Position [590, 504, 735, 546] - ZOrder 545 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - SourceType "DotJ Nu" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType Reference - Name "DotJ Nu l_sole\n1" - SID "4854" - Ports [4, 1] - Position [590, 454, 735, 496] - ZOrder 566 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - SourceType "DotJ Nu" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_LEG" - } - Block { - BlockType Reference - Name "Jacobian l_sole" - SID "4855" - Ports [2, 1] - Position [580, 215, 740, 250] - ZOrder 573 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType Reference - Name "Jacobian l_sole1" - SID "4856" - Ports [2, 1] - Position [580, 165, 740, 200] - ZOrder 565 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_LEG" - } - Block { - BlockType Reference - Name "Jacobian r_sole" - SID "4857" - Ports [2, 1] - Position [580, 91, 740, 129] - ZOrder 546 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Reference - Name "Jacobian r_sole1" - SID "4858" - Ports [2, 1] - Position [580, 41, 740, 79] - ZOrder 574 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_LEG" - } - Block { - BlockType Switch - Name "Switch" - SID "4859" - Position [380, 90, 435, 170] - ZOrder 569 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch1" - SID "4860" - Position [380, 205, 435, 285] - ZOrder 572 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch2" - SID "4861" - Position [800, 45, 855, 125] - ZOrder 575 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch3" - SID "4862" - Position [800, 170, 855, 250] - ZOrder 576 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch4" - SID "4863" - Position [800, 340, 855, 420] - ZOrder 577 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch5" - SID "4864" - Position [800, 460, 855, 540] - ZOrder 578 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Reference - Name "l_sole" - SID "4865" - Ports [2, 1] - Position [170, 250, 330, 290] - ZOrder 571 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType Reference - Name "l_sole1" - SID "4866" - Ports [2, 1] - Position [170, 200, 330, 240] - ZOrder 563 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_LEG" - } - Block { - BlockType Reference - Name "r_sole" - SID "4867" - Ports [2, 1] - Position [170, 135, 330, 175] - ZOrder 543 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Reference - Name "r_sole1" - SID "4868" - Ports [2, 1] - Position [170, 85, 330, 125] - ZOrder 570 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_LEG" - } - Block { - BlockType Outport - Name "w_H_lContact" - SID "4869" - Position [470, 238, 500, 252] - ZOrder 556 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_rContact" - SID "4870" - Position [470, 123, 500, 137] - ZOrder 557 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "JL" - SID "4871" - Position [880, 203, 910, 217] - ZOrder 558 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "JR" - SID "4872" - Position [880, 78, 910, 92] - ZOrder 559 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "dJLnu" - SID "4873" - Position [880, 493, 910, 507] - ZOrder 560 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "dJRnu" - SID "4874" - Position [880, 373, 910, 387] - ZOrder 561 - Port "6" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "r_sole1" - SrcPort 1 - DstBlock "Switch" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "r_sole" - SrcPort 1 - DstBlock "Switch" - DstPort 3 - } - Line { - ZOrder 3 - SrcBlock "l_sole1" - SrcPort 1 - DstBlock "Switch1" - DstPort 1 - } - Line { - ZOrder 4 - SrcBlock "l_sole" - SrcPort 1 - DstBlock "Switch1" - DstPort 3 - } - Line { - ZOrder 5 - SrcBlock "Switch1" - SrcPort 1 - DstBlock "w_H_lContact" - DstPort 1 - } - Line { - ZOrder 6 - SrcBlock "Switch" - SrcPort 1 - DstBlock "w_H_rContact" - DstPort 1 - } - Line { - ZOrder 7 - SrcBlock "w_H_b" - SrcPort 1 - Points [53, 0] - Branch { - ZOrder 8 - Points [0, -45; 446, 0] - Branch { - ZOrder 9 - Points [0, 60; 2, 0] - Branch { - ZOrder 10 - Points [0, 65] - Branch { - ZOrder 11 - DstBlock "Jacobian l_sole1" - DstPort 1 - } - Branch { - ZOrder 12 - Points [0, 50] - Branch { - ZOrder 13 - Points [0, 115] - Branch { - ZOrder 14 - Points [0, 50] - Branch { - ZOrder 15 - Points [0, 70] - Branch { - ZOrder 16 - Points [0, 50] - DstBlock "DotJ Nu l_sole\n" - DstPort 1 - } - Branch { - ZOrder 17 - DstBlock "DotJ Nu l_sole\n1" - DstPort 1 - } - } - Branch { - ZOrder 18 - DstBlock " \n " - DstPort 1 - } - } - Branch { - ZOrder 19 - DstBlock " " - DstPort 1 - } - } - Branch { - ZOrder 20 - DstBlock "Jacobian l_sole" - DstPort 1 - } - } - } - Branch { - ZOrder 21 - Points [0, -10] - DstBlock "Jacobian r_sole" - DstPort 1 - } - } - Branch { - ZOrder 22 - DstBlock "Jacobian r_sole1" - DstPort 1 - } - } - Branch { - ZOrder 23 - DstBlock "r_sole1" - DstPort 1 - } - Branch { - ZOrder 24 - Points [0, 50] - Branch { - ZOrder 25 - Points [0, 65] - Branch { - ZOrder 26 - Points [0, 50] - DstBlock "l_sole" - DstPort 1 - } - Branch { - ZOrder 27 - DstBlock "l_sole1" - DstPort 1 - } - } - Branch { - ZOrder 28 - DstBlock "r_sole" - DstPort 1 - } - } - } - Line { - ZOrder 29 - SrcBlock "Jacobian r_sole1" - SrcPort 1 - DstBlock "Switch2" - DstPort 1 - } - Line { - ZOrder 30 - SrcBlock "legsInContact" - SrcPort 1 - Points [294, 0] - Branch { - ZOrder 31 - Points [0, 105] - Branch { - ZOrder 32 - Points [0, 115] - DstBlock "Switch1" - DstPort 2 - } - Branch { - ZOrder 33 - DstBlock "Switch" - DstPort 2 - } - } - Branch { - ZOrder 34 - Points [421, 0; 0, 60] - Branch { - ZOrder 35 - Points [0, 125] - Branch { - ZOrder 36 - Points [0, 170] - Branch { - ZOrder 37 - Points [0, 120] - DstBlock "Switch5" - DstPort 2 - } - Branch { - ZOrder 38 - DstBlock "Switch4" - DstPort 2 - } - } - Branch { - ZOrder 39 - DstBlock "Switch3" - DstPort 2 - } - } - Branch { - ZOrder 40 - DstBlock "Switch2" - DstPort 2 - } - } - } - Line { - ZOrder 41 - SrcBlock "Jacobian r_sole" - SrcPort 1 - DstBlock "Switch2" - DstPort 3 - } - Line { - ZOrder 42 - SrcBlock "Jacobian l_sole1" - SrcPort 1 - DstBlock "Switch3" - DstPort 1 - } - Line { - ZOrder 43 - SrcBlock "Jacobian l_sole" - SrcPort 1 - DstBlock "Switch3" - DstPort 3 - } - Line { - ZOrder 44 - SrcBlock "Switch2" - SrcPort 1 - DstBlock "JR" - DstPort 1 - } - Line { - ZOrder 45 - SrcBlock "Switch3" - SrcPort 1 - DstBlock "JL" - DstPort 1 - } - Line { - ZOrder 46 - SrcBlock "qj" - SrcPort 1 - Points [67, 0] - Branch { - ZOrder 47 - Points [0, 50] - DstBlock "l_sole" - DstPort 2 - } - Branch { - ZOrder 48 - Points [0, -65] - Branch { - ZOrder 49 - Points [0, -50] - Branch { - ZOrder 50 - Points [0, -45; 420, 0] - Branch { - ZOrder 51 - Points [0, 50] - Branch { - ZOrder 52 - Points [0, 70; 1, 0] - Branch { - ZOrder 53 - Points [0, 50] - Branch { - ZOrder 54 - Points [0, 110] - Branch { - ZOrder 55 - Points [0, 50] - Branch { - ZOrder 56 - Points [0, 70] - Branch { - ZOrder 57 - Points [0, 50] - DstBlock "DotJ Nu l_sole\n" - DstPort 2 - } - Branch { - ZOrder 58 - DstBlock "DotJ Nu l_sole\n1" - DstPort 2 - } - } - Branch { - ZOrder 59 - DstBlock " \n " - DstPort 2 - } - } - Branch { - ZOrder 60 - DstBlock " " - DstPort 2 - } - } - Branch { - ZOrder 61 - DstBlock "Jacobian l_sole" - DstPort 2 - } - } - Branch { - ZOrder 62 - DstBlock "Jacobian l_sole1" - DstPort 2 - } - } - Branch { - ZOrder 63 - DstBlock "Jacobian r_sole" - DstPort 2 - } - } - Branch { - ZOrder 64 - DstBlock "Jacobian r_sole1" - DstPort 2 - } - } - Branch { - ZOrder 65 - DstBlock "r_sole1" - DstPort 2 - } - } - Branch { - ZOrder 66 - DstBlock "r_sole" - DstPort 2 - } - } - Branch { - ZOrder 67 - DstBlock "l_sole1" - DstPort 2 - } - } - Line { - ZOrder 68 - SrcBlock "nu_b" - SrcPort 1 - Points [99, 0] - Branch { - ZOrder 69 - Points [0, 50] - Branch { - ZOrder 70 - Points [0, 70] - Branch { - ZOrder 71 - Points [0, 50] - DstBlock "DotJ Nu l_sole\n" - DstPort 3 - } - Branch { - ZOrder 72 - DstBlock "DotJ Nu l_sole\n1" - DstPort 3 - } - } - Branch { - ZOrder 73 - DstBlock " \n " - DstPort 3 - } - } - Branch { - ZOrder 74 - DstBlock " " - DstPort 3 - } - } - Line { - ZOrder 75 - SrcBlock "dqj" - SrcPort 1 - Points [142, 0] - Branch { - ZOrder 76 - Points [0, 70] - Branch { - ZOrder 77 - Points [0, 50] - DstBlock "DotJ Nu l_sole\n" - DstPort 4 - } - Branch { - ZOrder 78 - DstBlock "DotJ Nu l_sole\n1" - DstPort 4 - } - } - Branch { - ZOrder 79 - Points [0, -50] - DstBlock " " - DstPort 4 - } - Branch { - ZOrder 80 - DstBlock " \n " - DstPort 4 - } - } - Line { - ZOrder 81 - SrcBlock " " - SrcPort 1 - DstBlock "Switch4" - DstPort 1 - } - Line { - ZOrder 82 - SrcBlock " \n " - SrcPort 1 - DstBlock "Switch4" - DstPort 3 - } - Line { - ZOrder 83 - SrcBlock "DotJ Nu l_sole\n1" - SrcPort 1 - DstBlock "Switch5" - DstPort 1 - } - Line { - ZOrder 84 - SrcBlock "Switch4" - SrcPort 1 - DstBlock "dJRnu" - DstPort 1 - } - Line { - ZOrder 85 - SrcBlock "Switch5" - SrcPort 1 - DstBlock "dJLnu" - DstPort 1 - } - Line { - ZOrder 86 - SrcBlock "DotJ Nu l_sole\n" - SrcPort 1 - DstBlock "Switch5" - DstPort 3 - } - } - } - Block { - BlockType Reference - Name "Jacobian com" - SID "2378" - Ports [2, 1] - Position [105, 210, 270, 250] - ZOrder 835 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.COM" - } - Block { - BlockType SubSystem - Name "Legs in Contact" - SID "4875" - Ports [2, 1] - Position [-165, 571, 10, 649] - ZOrder 887 - LibraryVersion "1.32" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "Legs in Contact" - 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 "4554" - Block { - BlockType Inport - Name "icubStandup" - SID "4875::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4875::19" - Position [20, 136, 40, 154] - ZOrder 10 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4875::4553" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 66 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4875::4552" - Tag "Stateflow S-Function torqueBalancingStandup 9" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 65 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "legsInContact" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4875::4554" - Position [460, 241, 480, 259] - ZOrder 67 - } - Block { - BlockType Outport - Name "legsInContact" - SID "4875::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 81 - SrcBlock "icubStandup" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 82 - SrcBlock "state" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "legsInContact" - ZOrder 83 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "legsInContact" - DstPort 1 - } - Line { - ZOrder 84 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 85 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "xCom" - SID "4360" - Ports [2, 1] - Position [110, 281, 270, 319] - ZOrder 872 - ShowName off - RequestExecContextInheritance off - System { - Name "xCom" - 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 "942" - Block { - BlockType Inport - Name "w_H_b" - SID "4361" - Position [30, 237, 60, 253] - ZOrder 581 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "4362" - Position [30, 267, 60, 283] - ZOrder 582 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "CoM" - SID "4363" - Ports [2, 1] - Position [110, 230, 275, 290] - ZOrder 578 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.COM" - } - Block { - BlockType Selector - Name "CoM6D -> \nCoMXYZ2" - SID "4364" - Ports [1, 1] - Position [310, 241, 350, 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 "xCoM" - SID "4365" - Position [385, 253, 415, 267] - ZOrder 583 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "qj" - SrcPort 1 - DstBlock "CoM" - DstPort 2 - } - Line { - ZOrder 2 - SrcBlock "CoM6D -> \nCoMXYZ2" - SrcPort 1 - DstBlock "xCoM" - 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_l_contact" - SID "4350" - Position [380, 378, 410, 392] - ZOrder 865 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_r_contact" - SID "4367" - Position [380, 423, 410, 437] - ZOrder 874 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_l_contact" - SID "4368" - Position [380, 468, 410, 482] - ZOrder 875 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_r_contact" - SID "4369" - Position [380, 513, 410, 527] - ZOrder 876 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_CoM" - SID "4370" - Position [380, 223, 410, 237] - ZOrder 877 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "xCoM" - SID "4371" - Position [380, 293, 410, 307] - ZOrder 878 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "JDot_nu_l_contact" - SID "4372" - Position [380, 558, 410, 572] - ZOrder 879 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "JDot_nu_r_contact" - SID "4374" - Position [380, 603, 410, 617] - ZOrder 881 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_LArm" - SID "4876" - Position [380, 123, 410, 137] - ZOrder 888 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_RArm" - SID "4877" - Position [380, 153, 410, 167] - ZOrder 889 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "legsInContact" - SID "4890" - Position [380, 658, 410, 672] - ZOrder 890 - Port "11" - IconDisplay "Port number" - } - Line { - ZOrder 129 - SrcBlock "nu" - SrcPort 1 - Points [72, 0] - Branch { - ZOrder 266 - Points [0, 55] - DstBlock "CoM6D -> \nCoMXYZ3" - DstPort 1 - } - Branch { - ZOrder 188 - DstBlock "CoM6D -> \nCoMXYZ4" - DstPort 1 - } - } - Line { - ZOrder 145 - SrcBlock "w_H_b" - SrcPort 1 - Points [188, 0] - Branch { - ZOrder 267 - Points [0, 100] - DstBlock "Contacts kinematics" - DstPort 1 - } - Branch { - ZOrder 261 - DstBlock "xCom" - DstPort 1 - } - Branch { - ZOrder 259 - Points [0, -70] - Branch { - ZOrder 277 - Points [0, -90] - DstBlock "Arm External Forces" - DstPort 1 - } - Branch { - ZOrder 276 - DstBlock "Jacobian com" - DstPort 1 - } - } - } - Line { - ZOrder 144 - SrcBlock "qj" - SrcPort 1 - Points [215, 0] - Branch { - ZOrder 278 - Points [0, -80] - DstBlock "Arm External Forces" - DstPort 2 - } - Branch { - ZOrder 178 - DstBlock "Jacobian com" - DstPort 2 - } - Branch { - ZOrder 177 - Points [0, 70] - Branch { - ZOrder 269 - Points [0, 135] - DstBlock "Contacts kinematics" - DstPort 2 - } - Branch { - ZOrder 268 - DstBlock "xCom" - DstPort 2 - } - } - } - Line { - ZOrder 183 - SrcBlock "Jacobian com" - SrcPort 1 - DstBlock "J_CoM" - DstPort 1 - } - Line { - ZOrder 184 - SrcBlock "xCom" - SrcPort 1 - DstBlock "xCoM" - DstPort 1 - } - Line { - ZOrder 258 - SrcBlock "state " - SrcPort 1 - DstBlock "Legs in Contact" - DstPort 2 - } - Line { - ZOrder 257 - SrcBlock "Constant" - SrcPort 1 - DstBlock "Legs in Contact" - DstPort 1 - } - Line { - ZOrder 262 - SrcBlock "CoM6D -> \nCoMXYZ3" - SrcPort 1 - DstBlock "Contacts kinematics" - DstPort 4 - } - Line { - ZOrder 263 - SrcBlock "CoM6D -> \nCoMXYZ4" - SrcPort 1 - DstBlock "Contacts kinematics" - DstPort 3 - } - Line { - ZOrder 264 - SrcBlock "Legs in Contact" - SrcPort 1 - Points [44, 0] - Branch { - ZOrder 284 - Points [0, 55] - DstBlock "legsInContact" - DstPort 1 - } - Branch { - ZOrder 283 - DstBlock "Contacts kinematics" - DstPort 5 - } - } - Line { - ZOrder 270 - SrcBlock "Contacts kinematics" - SrcPort 1 - DstBlock "w_H_l_contact" - DstPort 1 - } - Line { - ZOrder 271 - SrcBlock "Contacts kinematics" - SrcPort 2 - DstBlock "w_H_r_contact" - DstPort 1 - } - Line { - ZOrder 272 - SrcBlock "Contacts kinematics" - SrcPort 5 - DstBlock "JDot_nu_l_contact" - DstPort 1 - } - Line { - ZOrder 273 - SrcBlock "Contacts kinematics" - SrcPort 6 - DstBlock "JDot_nu_r_contact" - DstPort 1 - } - Line { - ZOrder 274 - SrcBlock "Contacts kinematics" - SrcPort 3 - DstBlock "J_l_contact" - DstPort 1 - } - Line { - ZOrder 275 - SrcBlock "Contacts kinematics" - SrcPort 4 - DstBlock "J_r_contact" - DstPort 1 - } - Line { - ZOrder 282 - SrcBlock "Arm External Forces" - SrcPort 1 - DstBlock "w_H_LArm" - DstPort 1 - } - Line { - ZOrder 281 - SrcBlock "Arm External Forces" - SrcPort 2 - DstBlock "w_H_RArm" - DstPort 1 - } - } - } - Block { - BlockType Outport - Name "M" - SID "2350" - Position [-465, -167, -435, -153] - ZOrder -2 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "biasTorques" - SID "2351" - Position [-465, -97, -435, -83] - ZOrder 217 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "L" - SID "2352" - Position [-465, -27, -435, -13] - ZOrder 216 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_l_contact" - SID "4878" - Position [-70, 28, -40, 42] - ZOrder 890 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_r_contact" - SID "4879" - Position [-70, 63, -40, 77] - ZOrder 891 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_l_contact" - SID "4880" - Position [-70, 98, -40, 112] - ZOrder 892 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_r_contact" - SID "4881" - Position [-70, 133, -40, 147] - ZOrder 893 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_CoM" - SID "4882" - Position [-70, 168, -40, 182] - ZOrder 894 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "xCoM" - SID "4883" - Position [-70, 203, -40, 217] - ZOrder 895 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "JDot_nu_l_contact" - SID "4884" - Position [-70, 238, -40, 252] - ZOrder 896 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "JDot_nu_r_contact" - SID "4885" - Position [-70, 273, -40, 287] - ZOrder 897 - Port "11" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_LArm" - SID "4886" - Position [-70, 308, -40, 322] - ZOrder 898 - Port "12" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_RArm" - SID "4887" - Position [-70, 343, -40, 357] - ZOrder 899 - Port "13" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "legsInContact" - SID "4891" - Position [-70, 378, -40, 392] - ZOrder 900 - Port "14" - IconDisplay "Port number" - } - Line { - ZOrder 109 - SrcBlock "w_H_b" - SrcPort 1 - Points [25, 0] - Branch { - ZOrder 137 - Points [0, 415] - DstBlock "Kinematics" - DstPort 3 - } - Branch { - ZOrder 135 - DstBlock "Dynamics" - DstPort 1 - } - } - Line { - ZOrder 110 - SrcBlock "qj" - SrcPort 1 - Points [78, 0] - Branch { - ZOrder 142 - DstBlock "Dynamics" - DstPort 2 - } - Branch { - ZOrder 138 - Points [0, 255] - DstBlock "Kinematics" - DstPort 2 - } - } - Line { - ZOrder 111 - SrcBlock "nu" - SrcPort 1 - Points [122, 0] - Branch { - ZOrder 143 - DstBlock "Dynamics" - DstPort 3 - } - Branch { - ZOrder 139 - Points [0, 95] - DstBlock "Kinematics" - DstPort 1 - } - } - Line { - ZOrder 112 - SrcBlock "Dynamics" - SrcPort 1 - DstBlock "M" - DstPort 1 - } - Line { - ZOrder 113 - SrcBlock "Dynamics" - SrcPort 2 - DstBlock "biasTorques" - DstPort 1 - } - Line { - ZOrder 114 - SrcBlock "Dynamics" - SrcPort 3 - DstBlock "L" - DstPort 1 - } - Line { - ZOrder 172 - SrcBlock "Kinematics" - SrcPort 5 - DstBlock "J_CoM" - DstPort 1 - } - Line { - ZOrder 173 - SrcBlock "Kinematics" - SrcPort 6 - DstBlock "xCoM" - DstPort 1 - } - Line { - ZOrder 168 - SrcBlock "Kinematics" - SrcPort 1 - DstBlock "w_H_l_contact" - DstPort 1 - } - Line { - ZOrder 169 - SrcBlock "Kinematics" - SrcPort 2 - DstBlock "w_H_r_contact" - DstPort 1 - } - Line { - ZOrder 174 - SrcBlock "Kinematics" - SrcPort 7 - DstBlock "JDot_nu_l_contact" - DstPort 1 - } - Line { - ZOrder 175 - SrcBlock "Kinematics" - SrcPort 8 - DstBlock "JDot_nu_r_contact" - DstPort 1 - } - Line { - ZOrder 170 - SrcBlock "Kinematics" - SrcPort 3 - DstBlock "J_l_contact" - DstPort 1 - } - Line { - ZOrder 171 - SrcBlock "Kinematics" - SrcPort 4 - DstBlock "J_r_contact" - DstPort 1 - } - Line { - ZOrder 176 - SrcBlock "Kinematics" - SrcPort 9 - DstBlock "w_H_LArm" - DstPort 1 - } - Line { - ZOrder 177 - SrcBlock "Kinematics" - SrcPort 10 - DstBlock "w_H_RArm" - DstPort 1 - } - Line { - ZOrder 178 - SrcBlock "Kinematics" - SrcPort 11 - DstBlock "legsInContact" - DstPort 1 - } - Line { - ZOrder 183 - SrcBlock "state " - SrcPort 1 - DstBlock "Kinematics" - DstPort 4 - } - } - } - Block { - BlockType SubSystem - Name "Robot State and References" - SID "2087" - Ports [0, 10] - Position [800, 670, 965, 915] - 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 "254" - Block { - BlockType Reference - Name "Compare\nTo Zero" - SID "2916" - Ports [1, 1] - Position [-280, 2850, -250, 2880] - ZOrder 613 - ShowName off - LibraryVersion "1.441" - SourceBlock "simulink/Logic and Bit\nOperations/Compare\nTo Zero" - SourceType "Compare To Zero" - SourceProductName "Simulink" - SourceProductBaseCode "SL" - ContentPreviewEnabled off - relop "~=" - OutDataTypeStr "boolean" - ZeroCross on - } - Block { - BlockType Reference - Name "Compare\nTo Zero1" - SID "2917" - Ports [1, 1] - Position [-280, 2580, -250, 2610] - ZOrder 614 - ShowName off - LibraryVersion "1.441" - SourceBlock "simulink/Logic and Bit\nOperations/Compare\nTo Zero" - SourceType "Compare To Zero" - SourceProductName "Simulink" - SourceProductBaseCode "SL" - ContentPreviewEnabled off - relop "~=" - OutDataTypeStr "boolean" - ZeroCross on - } - Block { - BlockType Constant - Name "Constant" - SID "2096" - Position [-630, 2854, -520, 2876] - ZOrder 501 - ShowName off - Priority "-10" - Value "Sm.SM_TYPE_BIN" - OutDataTypeStr "uint8" - } - Block { - BlockType Constant - Name "Constant2" - SID "3692" - Position [80, 2784, 190, 2806] - ZOrder 699 - ShowName off - Priority "-10" - Value "Sm.SM_TYPE_BIN" - OutDataTypeStr "uint8" - } - Block { - BlockType Reference - Name "Coordinator" - SID "2908" - Ports [1, 1] - Position [-445, 2581, -385, 2609] - ZOrder 605 - ShowName off - LibraryVersion "1.441" - SourceBlock "simulink/Logic and Bit\nOperations/Bitwise\nOperator" - SourceType "Bitwise Operator" - SourceProductName "Simulink" - SourceProductBaseCode "SL" - MultiThreadCoSim "auto" - logicop "AND" - UseBitMask on - NumInputPorts "1" - BitMask "Sm.SM_MASK_COORDINATOR" - BitMaskRealWorld "Stored Integer" - } - Block { - BlockType Reference - Name "Get Measurement" - SID "5012" - Ports [0, 1] - Position [-450, 2706, -380, 2734] - ZOrder 868 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Position" - } - Block { - BlockType Goto - Name "Goto2" - SID "4496" - Position [670, 3081, 750, 3099] - ZOrder 863 - GotoTag "w_H_b" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto3" - SID "4497" - Position [670, 3041, 750, 3059] - ZOrder 864 - GotoTag "state" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto4" - SID "4498" - Position [670, 3006, 750, 3024] - ZOrder 865 - GotoTag "qj_des" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto7" - SID "4501" - Position [670, 3121, 750, 3139] - ZOrder 866 - GotoTag "CoM_des" - TagVisibility "global" - } - Block { - BlockType SubSystem - Name "Internal Coordinator" - SID "2098" - Ports [1, 11, 1] - Position [-210, 2617, -30, 2823] - ZOrder 505 - RequestExecContextInheritance off - System { - Name "Internal Coordinator" - 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 "304" - Block { - BlockType Inport - Name "jointAngles" - SID "3165" - Position [-665, 283, -635, 297] - ZOrder 676 - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "2099" - Ports [] - Position [-243, 340, -223, 360] - ZOrder 538 - } - Block { - BlockType SubSystem - Name "Base to fixed_link" - SID "4073" - Ports [2, 1] - Position [-480, 281, -350, 319] - ZOrder 886 - RequestExecContextInheritance off - System { - Name "Base to fixed_link" - 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 "396" - Block { - BlockType Inport - Name "qj" - SID "4078" - Position [380, -57, 410, -43] - ZOrder 890 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4079" - Position [915, -142, 945, -128] - ZOrder 891 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant1" - SID "4085" - Position [690, -43, 880, -27] - ZOrder 897 - ShowName off - Value "Config.LEFT_FOOT_IN_CONTACT_AT_0" - } - Block { - BlockType Constant - Name "Constant7" - SID "4072" - Position [380, 50, 410, 80] - ZOrder 885 - ShowName off - Value "eye(4)" - } - Block { - BlockType Reference - Name "LFoot to world transform (fixed base)" - SID "4071" - Ports [2, 1] - Position [490, -73, 655, -42] - ZOrder 725 - NamePlacement "alternate" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType Reference - Name "RFoot to world transform (fixed base)" - SID "4084" - Ports [2, 1] - Position [485, -33, 655, 3] - ZOrder 896 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Switch - Name "Switch" - SID "4083" - Position [905, -68, 945, -2] - ZOrder 895 - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType SubSystem - Name "fixedFoot to base link transform " - SID "4047" - Ports [4, 1] - Position [1000, -287, 1195, 117] - ZOrder 718 - RequestExecContextInheritance off - System { - Name "fixedFoot to base 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 "248" - Block { - BlockType Inport - Name "qj" - SID "4048" - Position [470, 227, 495, 243] - ZOrder 736 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4049" - Position [1000, 212, 1030, 228] - ZOrder 741 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_link" - SID "4050" - Position [655, 132, 680, 148] - ZOrder 744 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_base_fixed" - SID "4051" - Position [470, 32, 495, 48] - ZOrder 745 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "Fixed base to imu transform" - SID "4052" - Ports [2, 1] - Position [575, 25, 740, 85] - ZOrder 729 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.IMU" - } - Block { - BlockType Reference - Name "Fixed base to root link transform" - SID "4053" - Ports [2, 1] - Position [575, 190, 740, 250] - ZOrder 737 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.BASE" - } - Block { - BlockType Product - Name "Inv\\*" - SID "4054" - Ports [2, 1] - Position [825, 40, 895, 95] - ZOrder 731 - 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\\* " - SID "4055" - Ports [2, 1] - Position [825, 184, 895, 231] - ZOrder 738 - 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 "4056" - Ports [0, 1] - Position [980, 308, 1055, 332] - ZOrder 742 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.NECK_POS" - signalSize "6" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - Block { - BlockType Switch - Name "Switch6" - SID "4057" - Position [1760, 151, 1840, 419] - ZOrder 734 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE" - SID "4058" - Position [1570, 275, 1725, 295] - ZOrder 735 - ShowName off - Value "Config.USE_IMU4EST_BASE" - } - Block { - BlockType SubSystem - Name "fromImuToHomogeousTransformFCN" - SID "4059" - Ports [6, 1] - Position [1305, 47, 1540, 343] - ZOrder 733 - ShowName off - LibraryVersion "1.30" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "fromImuToHomogeousTransformFCN" - 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 "3785" - SIDPrevWatermark "9" - Block { - BlockType Inport - Name "imu_H_link" - SID "4059::112" - Position [20, 101, 40, 119] - ZOrder 82 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "imu_H_link_0" - SID "4059::113" - Position [20, 136, 40, 154] - ZOrder 83 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "link_H_base" - SID "4059::111" - Position [20, 171, 40, 189] - ZOrder 81 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial_0" - SID "4059::115" - Position [20, 206, 40, 224] - ZOrder 85 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4059::93" - Position [20, 246, 40, 264] - ZOrder 69 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "neck_pos" - SID "4059::118" - Position [20, 281, 40, 299] - ZOrder 86 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4059::3784" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 184 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4059::3783" - Tag "Stateflow S-Function torqueBalancingStandup 2" - Ports [6, 2] - Position [180, 102, 230, 243] - ZOrder 183 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[6 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "w_H_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4059::3785" - Position [460, 241, 480, 259] - ZOrder 185 - } - Block { - BlockType Outport - Name "w_H_b" - SID "4059::106" - Position [460, 101, 480, 119] - ZOrder 80 - IconDisplay "Port number" - } - Line { - ZOrder 181 - SrcBlock "imu_H_link" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 182 - SrcBlock "imu_H_link_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 183 - SrcBlock "link_H_base" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 184 - SrcBlock "inertial_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 185 - SrcBlock "inertial" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 186 - SrcBlock "neck_pos" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - Name "w_H_b" - ZOrder 187 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 188 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 189 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "holder\n1" - SID "4060" - Ports [1, 1] - Position [1175, 113, 1225, 127] - ZOrder 739 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "holder\n2" - SID "4061" - Ports [1, 1] - Position [1175, 213, 1225, 227] - ZOrder 740 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "neck transform" - SID "4062" - Ports [1, 1] - Position [1135, 305, 1260, 335] - ZOrder 743 - RequestExecContextInheritance off - System { - Name "neck transform" - Location [65, 24, 2560, 1440] - 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 "562" - Block { - BlockType Inport - Name "neck port" - SID "4063" - Position [45, 18, 75, 32] - ZOrder 725 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant" - SID "4064" - Position [95, 85, 170, 105] - ZOrder 720 - ShowName off - Value "zeros(3,1)" - } - Block { - BlockType Gain - Name "Gain" - SID "4065" - 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 "4066" - Ports [1, 1] - Position [190, 13, 230, 37] - ZOrder 723 - ShowName off - InputPortWidth "6" - IndexOptions "Index vector (dialog)" - Indices "[1:3]" - OutputSizes "1" - Port { - PortNumber 1 - Name "neck pos" - } - } - Block { - BlockType Switch - Name "Switch" - SID "4067" - Position [300, 7, 375, 113] - ZOrder 722 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE1" - SID "4068" - Position [60, 50, 210, 70] - ZOrder 719 - ShowName off - Value "Config.CORRECT_NECK_IMU" - } - Block { - BlockType Outport - Name "neck position" - SID "4069" - Position [425, 53, 455, 67] - ZOrder 726 - IconDisplay "Port number" - } - 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 "4070" - Position [1875, 278, 1905, 292] - ZOrder 732 - IconDisplay "Port number" - } - 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\\* " - SrcPort 1 - Points [65, 0] - Branch { - ZOrder 3 - Points [0, 165] - DstBlock "Switch6" - DstPort 3 - } - Branch { - ZOrder 4 - Points [0, -40] - DstBlock "fromImuToHomogeousTransformFCN" - 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 "fromImuToHomogeousTransformFCN" - DstPort 1 - } - Branch { - ZOrder 8 - Points [0, 50] - DstBlock "holder\n1" - DstPort 1 - } - } - Line { - ZOrder 9 - SrcBlock "holder\n2" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 4 - } - Line { - ZOrder 10 - SrcBlock "Fixed base to imu transform" - SrcPort 1 - DstBlock "Inv\\*" - DstPort 1 - } - Line { - ZOrder 11 - SrcBlock "holder\n1" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 2 - } - Line { - ZOrder 12 - SrcBlock "qj" - 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 "fromImuToHomogeousTransformFCN" - SrcPort 1 - DstBlock "Switch6" - DstPort 1 - } - Line { - ZOrder 16 - SrcBlock "Fixed base to root link transform" - SrcPort 1 - DstBlock "Inv\\* " - 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 "fromImuToHomogeousTransformFCN" - DstPort 5 - } - } - Line { - ZOrder 20 - SrcBlock "Neck Position" - SrcPort 1 - DstBlock "neck transform" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "neck transform" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 6 - } - Line { - ZOrder 22 - SrcBlock "w_H_link" - SrcPort 1 - Points [98, 0] - Branch { - ZOrder 23 - Points [0, 55] - DstBlock "Inv\\* " - 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 - } - } - } - } - Block { - BlockType Outport - Name "fixed_LFoot_H_b" - SID "4082" - Position [1245, -92, 1275, -78] - ZOrder 894 - IconDisplay "Port number" - } - Line { - ZOrder 182 - SrcBlock "fixedFoot to base link transform " - SrcPort 1 - DstBlock "fixed_LFoot_H_b" - DstPort 1 - } - Line { - ZOrder 185 - SrcBlock "inertial" - SrcPort 1 - DstBlock "fixedFoot to base link transform " - DstPort 2 - } - Line { - ZOrder 186 - SrcBlock "qj" - SrcPort 1 - Points [25, 0] - Branch { - ZOrder 195 - Points [0, -185] - DstBlock "fixedFoot to base link transform " - DstPort 1 - } - Branch { - ZOrder 202 - Points [0, 45] - DstBlock "RFoot to world transform (fixed base)" - DstPort 2 - } - Branch { - ZOrder 191 - DstBlock "LFoot to world transform (fixed base)" - DstPort 2 - } - } - Line { - ZOrder 187 - SrcBlock "Constant7" - SrcPort 1 - Points [42, 0] - Branch { - ZOrder 211 - Points [0, -90] - Branch { - ZOrder 217 - Points [0, -40] - DstBlock "LFoot to world transform (fixed base)" - DstPort 1 - } - Branch { - ZOrder 201 - DstBlock "RFoot to world transform (fixed base)" - DstPort 1 - } - } - Branch { - ZOrder 189 - DstBlock "fixedFoot to base link transform " - DstPort 4 - } - } - Line { - ZOrder 193 - SrcBlock "Switch" - SrcPort 1 - DstBlock "fixedFoot to base link transform " - DstPort 3 - } - Line { - ZOrder 196 - SrcBlock "LFoot to world transform (fixed base)" - SrcPort 1 - DstBlock "Switch" - DstPort 1 - } - Line { - ZOrder 197 - SrcBlock "RFoot to world transform (fixed base)" - SrcPort 1 - DstBlock "Switch" - DstPort 3 - } - Line { - ZOrder 204 - SrcBlock "Constant1" - SrcPort 1 - DstBlock "Switch" - DstPort 2 - } - } - } - Block { - BlockType Clock - Name "Clock1" - SID "2133" - Position [25, 460, 45, 480] - ZOrder 888 - ShowName off - } - Block { - BlockType SubSystem - Name "Compute State Velocity" - SID "4086" - Ports [3, 1] - Position [165, 239, 315, 311] - ZOrder 887 - 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 "519" - Block { - BlockType Inport - Name "qj" - SID "4087" - Position [-10, -187, 20, -173] - ZOrder 562 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_b" - SID "4088" - Position [-10, -167, 20, -153] - ZOrder 561 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "feetInContact\n" - SID "4089" - Position [235, -147, 265, -133] - ZOrder 244 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "Compute Base Velocity" - SID "4090" - Ports [4, 1] - Position [340, -194, 605, -106] - 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 "3794" - Block { - BlockType Inport - Name "J_LeftFoot" - SID "4090::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "J_RightFoot" - SID "4090::1472" - Position [20, 136, 40, 154] - ZOrder 42 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "feetInContact" - SID "4090::1473" - Position [20, 171, 40, 189] - ZOrder 43 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qjDot" - SID "4090::23" - Position [20, 206, 40, 224] - ZOrder 9 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4090::3793" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 162 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4090::3792" - Tag "Stateflow S-Function torqueBalancingStandup 5" - Ports [4, 2] - Position [180, 102, 230, 203] - ZOrder 161 - FunctionName "sf_sfun" - Parameters "Reg" - PortCounts "[4 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "nu_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4090::3794" - Position [460, 241, 480, 259] - ZOrder 163 - } - Block { - BlockType Outport - Name "nu_b" - SID "4090::5" - Position [460, 101, 480, 119] - ZOrder -6 - IconDisplay "Port number" - } - Line { - ZOrder 141 - SrcBlock "J_LeftFoot" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 142 - SrcBlock "J_RightFoot" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 143 - SrcBlock "feetInContact" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 144 - SrcBlock "qjDot" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - Name "nu_b" - ZOrder 145 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "nu_b" - DstPort 1 - } - Line { - ZOrder 146 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 147 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Feet Jacobians" - SID "4091" - Ports [2, 2] - Position [55, -191, 205, -149] - ZOrder -21 - RequestExecContextInheritance off - System { - Name "Feet Jacobians" - Location [65, 24, 2560, 1440] - 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 "629" - Block { - BlockType Inport - Name "qj" - SID "4092" - Position [585, 598, 615, 612] - ZOrder 673 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_b" - SID "4093" - Position [585, 483, 615, 497] - ZOrder 680 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "Jacobian LFoot" - SID "4094" - Ports [2, 1] - Position [745, 475, 905, 530] - ZOrder 687 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType Reference - Name "Jacobian RFoot" - SID "4095" - Ports [2, 1] - Position [745, 565, 905, 620] - ZOrder 688 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Outport - Name "J_LFoot" - SID "4096" - Position [935, 498, 965, 512] - ZOrder 676 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_RFoot" - SID "4097" - Position [935, 588, 965, 602] - ZOrder 677 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "Jacobian LFoot" - SrcPort 1 - DstBlock "J_LFoot" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "Jacobian RFoot" - SrcPort 1 - DstBlock "J_RFoot" - DstPort 1 - } - Line { - ZOrder 3 - SrcBlock "w_H_b" - SrcPort 1 - Points [56, 0] - Branch { - ZOrder 4 - Points [0, 90] - DstBlock "Jacobian RFoot" - DstPort 1 - } - Branch { - ZOrder 5 - DstBlock "Jacobian LFoot" - DstPort 1 - } - } - Line { - ZOrder 6 - SrcBlock "qj" - SrcPort 1 - Points [82, 0] - Branch { - ZOrder 7 - Points [0, -90] - DstBlock "Jacobian LFoot" - DstPort 2 - } - Branch { - ZOrder 8 - DstBlock "Jacobian RFoot" - DstPort 2 - } - } - } - } - Block { - BlockType Reference - Name "Get Measurement" - SID "5013" - Ports [0, 1] - Position [215, -99, 285, -71] - ZOrder 869 - BackgroundColor "[0.513700, 0.674500, 1.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Velocity" - } - Block { - BlockType Mux - Name "Mux" - SID "4099" - Ports [2, 1] - Position [635, -182, 640, -53] - ZOrder -12 - ShowName off - Inputs "2" - DisplayOption "bar" - } - Block { - BlockType Outport - Name "nu" - SID "4100" - Position [660, -122, 690, -108] - ZOrder -15 - IconDisplay "Port number" - } - Line { - ZOrder 11 - SrcBlock "Get Measurement" - SrcPort 1 - Points [23, 0] - Branch { - ZOrder 2 - 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 "feetInContact\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 "qj" - 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 Constant - Name "Constant" - SID "2100" - Position [155, 492, 300, 508] - ZOrder 543 - ShowName off - Value "Config.DEMO_MOVEMENTS" - } - Block { - BlockType Constant - Name "Constant1" - SID "2101" - Position [-105, 290, 95, 310] - ZOrder 464 - ShowName off - Value "Config.LEFT_RIGHT_FOOT_IN_CONTACT" - } - Block { - BlockType Constant - Name "Constant2" - SID "2102" - Position [-65, 542, 0, 558] - ZOrder 548 - ShowName off - Value "zeros(6,1)" - } - Block { - BlockType Constant - Name "Constant3" - SID "2103" - Position [-570, 357, -445, 373] - ZOrder 550 - ShowName off - Value "Gain.impedances(1,:)" - } - Block { - BlockType Constant - Name "Constant4" - SID "2104" - Position [-525, 485, -495, 505] - ZOrder 553 - ShowName off - } - Block { - BlockType Constant - Name "Constant5" - SID "3321" - Position [-570, 397, -450, 413] - ZOrder 694 - ShowName off - Value "diag(Gain.KP_COM)" - } - Block { - BlockType Constant - Name "Constant6" - SID "3322" - Position [-570, 442, -450, 458] - ZOrder 695 - ShowName off - Value "diag(Gain.KD_COM)" - } - Block { - BlockType Reference - Name "IMU measurements" - SID "3222" - Ports [0, 1] - Position [-560, 303, -510, 317] - ZOrder 679 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.IMU" - signalSize "12" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on - } - Block { - BlockType Mux - Name "Mux" - SID "2106" - Ports [2, 1] - Position [30, 521, 35, 559] - ZOrder 547 - ShowName off - Inputs "2" - DisplayOption "bar" - } - Block { - BlockType SubSystem - Name "Reference Generator CoM" - SID "2134" - Ports [2, 1] - Position [95, 434, 365, 481] - ZOrder 889 - NamePlacement "alternate" - LibraryVersion "1.32" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "Reference Generator CoM" - Location [53, 45, 896, 1715] - 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 "3770" - Block { - BlockType Inport - Name "pos_CoM_0" - SID "2134::778" - Position [20, 101, 40, 119] - ZOrder 88 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "t" - SID "2134::768" - Position [20, 136, 40, 154] - ZOrder 78 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "2134::3769" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 172 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "2134::3768" - Tag "Stateflow S-Function torqueBalancingStandup 10" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 171 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "references_CoM" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "2134::3770" - Position [460, 241, 480, 259] - ZOrder 173 - } - Block { - BlockType Outport - Name "references_CoM" - SID "2134::45" - Position [460, 101, 480, 119] - ZOrder 31 - IconDisplay "Port number" - } - Line { - ZOrder 101 - SrcBlock "pos_CoM_0" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 102 - SrcBlock "t" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "references_CoM" - ZOrder 103 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "references_CoM" - DstPort 1 - } - Line { - ZOrder 104 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 105 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reshape - Name "Reshape1" - SID "4045" - Ports [1, 1] - Position [-255, 289, -210, 311] - ZOrder 716 - ShowName off - OutputDimensions "[4;4]" - } - Block { - BlockType Switch - Name "Switch1" - SID "2107" - Position [415, 442, 455, 558] - ZOrder 546 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Reference - Name "holder\n" - SID "2109" - Ports [1, 1] - Position [-150, 594, -95, 616] - ZOrder 577 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "holder\n1" - SID "2110" - Ports [1, 1] - Position [-150, 519, -95, 541] - ZOrder 579 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Constant - Name "joints.smoothingTime" - SID "3176" - Position [150, 556, 305, 574] - ZOrder 677 - ShowName off - Value "Sm.smoothingTimeCoM_Joints(1)" - } - Block { - BlockType SubSystem - Name "xCom" - SID "2137" - Ports [2, 1] - Position [-280, 500, -180, 560] - ZOrder -30 - ShowName off - RequestExecContextInheritance off - System { - Name "xCom" - 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 "942" - Block { - BlockType Inport - Name "w_H_b" - SID "2138" - Position [30, 237, 60, 253] - ZOrder 581 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "2139" - Position [30, 267, 60, 283] - ZOrder 582 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "CoM" - SID "2141" - Ports [2, 1] - Position [110, 230, 275, 290] - ZOrder 578 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.COM" - } - Block { - BlockType Selector - Name "CoM6D -> \nCoMXYZ2" - SID "2140" - Ports [1, 1] - Position [310, 241, 350, 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 "xCoM" - SID "2142" - Position [385, 253, 415, 267] - ZOrder 583 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "qj" - SrcPort 1 - DstBlock "CoM" - DstPort 2 - } - Line { - ZOrder 2 - SrcBlock "CoM6D -> \nCoMXYZ2" - SrcPort 1 - DstBlock "xCoM" - 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 "CoMDes" - SID "2143" - Position [485, 493, 515, 507] - ZOrder 478 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qjDes" - SID "2144" - Position [-45, 598, -15, 612] - ZOrder 479 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "2145" - Position [225, 342, 255, 358] - ZOrder 480 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "impedances" - SID "2146" - Position [-395, 357, -365, 373] - ZOrder 549 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "state" - SID "2147" - Position [-395, 487, -365, 503] - ZOrder 554 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "nu_b + qjDot" - SID "4046" - Position [400, 267, 430, 283] - ZOrder 717 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qj" - SID "2149" - Position [-245, 637, -215, 653] - ZOrder 576 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "smoothingTimeCoM_Joints" - SID "3177" - Position [355, 558, 385, 572] - ZOrder 678 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "KpCoM" - SID "3319" - Position [-395, 397, -365, 413] - ZOrder 692 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "KdCoM" - SID "3320" - Position [-395, 442, -365, 458] - ZOrder 693 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_b" - SID "2150" - Position [-175, 292, -145, 308] - ZOrder 575 - Port "11" - IconDisplay "Port number" - } - Line { - ZOrder 7 - SrcBlock "Constant" - SrcPort 1 - DstBlock "Switch1" - DstPort 2 - } - Line { - ZOrder 207 - SrcBlock "holder\n" - SrcPort 1 - DstBlock "qjDes" - DstPort 1 - } - Line { - ZOrder 13 - SrcBlock "Switch1" - SrcPort 1 - DstBlock "CoMDes" - DstPort 1 - } - Line { - ZOrder 131 - SrcBlock "Mux" - SrcPort 1 - DstBlock "Switch1" - DstPort 3 - } - Line { - ZOrder 17 - SrcBlock "Constant2" - SrcPort 1 - DstBlock "Mux" - DstPort 2 - } - Line { - ZOrder 18 - SrcBlock "Constant3" - SrcPort 1 - DstBlock "impedances" - DstPort 1 - } - Line { - ZOrder 19 - SrcBlock "Constant4" - SrcPort 1 - DstBlock "state" - DstPort 1 - } - Line { - ZOrder 191 - SrcBlock "Compute State Velocity" - SrcPort 1 - DstBlock "nu_b + qjDot" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "holder\n1" - SrcPort 1 - Points [79, 0] - Branch { - ZOrder 212 - Points [0, -85] - DstBlock "Reference Generator CoM" - DstPort 1 - } - Branch { - ZOrder 209 - DstBlock "Mux" - DstPort 1 - } - } - Line { - ZOrder 182 - SrcBlock "Base to fixed_link" - SrcPort 1 - Points [42, 0] - Branch { - ZOrder 188 - Points [0, -25] - DstBlock "Compute State Velocity" - DstPort 2 - } - Branch { - ZOrder 96 - Points [0, 215] - DstBlock "xCom" - DstPort 1 - } - Branch { - ZOrder 64 - DstBlock "Reshape1" - DstPort 1 - } - } - Line { - ZOrder 29 - SrcBlock "xCom" - SrcPort 1 - DstBlock "holder\n1" - DstPort 1 - } - Line { - ZOrder 30 - SrcBlock "joints.smoothingTime" - SrcPort 1 - DstBlock "smoothingTimeCoM_Joints" - DstPort 1 - } - Line { - ZOrder 31 - SrcBlock "jointAngles" - SrcPort 1 - Points [38, 0] - Branch { - ZOrder 189 - Points [0, -40] - DstBlock "Compute State Velocity" - DstPort 1 - } - Branch { - ZOrder 181 - DstBlock "Base to fixed_link" - DstPort 1 - } - Branch { - ZOrder 165 - Points [0, 255; 290, 0] - Branch { - ZOrder 109 - Points [0, 60] - Branch { - ZOrder 185 - Points [0, 40] - DstBlock "qj" - DstPort 1 - } - Branch { - ZOrder 37 - DstBlock "holder\n" - DstPort 1 - } - } - Branch { - ZOrder 36 - DstBlock "xCom" - DstPort 2 - } - } - } - Line { - ZOrder 41 - SrcBlock "Constant5" - SrcPort 1 - DstBlock "KpCoM" - DstPort 1 - } - Line { - ZOrder 42 - SrcBlock "Constant6" - SrcPort 1 - DstBlock "KdCoM" - DstPort 1 - } - Line { - ZOrder 66 - SrcBlock "Reshape1" - SrcPort 1 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 69 - SrcBlock "Constant1" - SrcPort 1 - Points [14, 0] - Branch { - ZOrder 190 - DstBlock "Compute State Velocity" - DstPort 3 - } - Branch { - ZOrder 95 - Points [0, 50] - DstBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - DstPort 1 - } - } - Line { - ZOrder 178 - SrcBlock "IMU measurements" - SrcPort 1 - DstBlock "Base to fixed_link" - DstPort 2 - } - Line { - ZOrder 204 - SrcBlock "Clock1" - SrcPort 1 - DstBlock "Reference Generator CoM" - DstPort 2 - } - Line { - ZOrder 208 - SrcBlock "Reference Generator CoM" - SrcPort 1 - DstBlock "Switch1" - DstPort 1 - } - } - } - Block { - BlockType MultiPortSwitch - Name "Multiport\nSwitch1" - SID "2913" - Ports [3, 1] - Position [230, 2778, 290, 2882] - ZOrder 610 - ShowName off - DataPortOrder "Specify indices" - DataPortIndices "{1, 2}" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Mux - Name "Mux1" - SID "2155" - Ports [11, 1] - Position [10, 2901, 15, 3119] - ZOrder 508 - ShowName off - Inputs "11" - DisplayOption "bar" - } - Block { - BlockType Mux - Name "Mux2" - SID "2156" - Ports [11, 1] - Position [10, 2613, 15, 2827] - ZOrder 509 - ShowName off - Inputs "11" - DisplayOption "bar" - } - Block { - BlockType SubSystem - Name "Select State and References" - SID "4030" - Ports [1, 10] - Position [320, 2689, 515, 2971] - ZOrder 702 - RequestExecContextInheritance off - System { - Name "Select 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 "230" - Block { - BlockType Inport - Name "StateAndReferences" - SID "4033" - Position [15, 288, 45, 302] - ZOrder 704 - IconDisplay "Port number" - } - Block { - BlockType Demux - Name "Demux" - SID "3687" - Ports [1, 11] - Position [100, 9, 105, 581] - ZOrder 697 - ShowName off - Outputs "[9,ROBOT_DOF,2,ROBOT_DOF,1,ROBOT_DOF+6,ROBOT_DOF,1,3,3,16]" - DisplayOption "bar" - Port { - PortNumber 1 - Name "CoMDes" - } - Port { - PortNumber 2 - Name "qDes" - } - Port { - PortNumber 5 - Name "state" - } - Port { - PortNumber 6 - Name "nu" - } - Port { - PortNumber 7 - Name "qj" - } - Port { - PortNumber 8 - Name "smoothingTimeMinJerkComDesQDes" - } - } - Block { - BlockType Goto - Name "Goto \"Compute joint torques\"" - SID "5121" - Position [980, 160, 1065, 170] - ZOrder 905 - GotoTag "qjDDot_des" - TagVisibility "global" - } - Block { - BlockType Logic - Name "Logical\nOperator1" - SID "4101" - Ports [2, 1] - Position [460, -193, 490, -162] - ZOrder 722 - ShowName off - Operator "OR" - AllPortsSameDT off - OutDataTypeStr "boolean" - } - Block { - BlockType Reference - Name "Minimum Jerk Trajectory Generator1" - SID "5122" - Ports [2, 3] - Position [715, 130, 870, 170] - ZOrder 903 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - SourceType "Minimum Jerk Trajectory Generator" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - externalSettlingTime on - settlingTime "0.01" - sampleTime "Config.Ts" - resetOnSettlingTime off - firstDerivatives on - secondDerivatives on - explicitInitialValue off - } - Block { - BlockType Reference - Name "Minimum Jerk Trajectory Generator2" - SID "2153" - Ports [2, 3] - Position [755, -10, 905, 30] - ZOrder 597 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - SourceType "Minimum Jerk Trajectory Generator" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - externalSettlingTime on - settlingTime "0.1" - sampleTime "Config.Ts" - resetOnSettlingTime off - firstDerivatives on - secondDerivatives on - explicitInitialValue off - } - Block { - BlockType Mux - Name "Mux" - SID "2154" - Ports [3, 1] - Position [935, -14, 940, 34] - ZOrder 486 - ShowName off - Inputs "3" - DisplayOption "bar" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n3" - SID "4102" - Position [295, -191, 400, -179] - ZOrder 721 - ShowName off - Value "Config.SCOPES_ALL" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n4" - SID "4103" - Position [255, -176, 435, -164] - ZOrder 723 - ShowName off - Value "Config.SCOPES_GAIN_SCHE_INFO" - } - Block { - BlockType Reshape - Name "Reshape" - SID "2159" - Ports [1, 1] - Position [1235, 55, 1280, 95] - ZOrder 456 - ShowName off - OutputDimensionality "Customize" - OutputDimensions "[3,3]" - } - Block { - BlockType Reshape - Name "Reshape1" - SID "4044" - Ports [1, 1] - Position [170, 525, 215, 565] - ZOrder 715 - ShowName off - OutputDimensionality "Customize" - OutputDimensions "[4;4]" - } - Block { - BlockType Constant - Name "SMOOTH_DES_COM2" - SID "5123" - Position [1110, 155, 1250, 175] - ZOrder 902 - ShowName off - Value "Config.SMOOTH_JOINT_DES" - } - Block { - BlockType Selector - Name "Selector" - SID "2162" - Ports [1, 1] - Position [505, -9, 540, 9] - ZOrder 481 - ShowName off - InputPortWidth "9" - IndexOptions "Index vector (dialog)" - Indices "[1 2 3]" - OutputSizes "1" - } - Block { - BlockType Switch - Name "Switch1" - SID "4825" - Position [640, 5, 685, 35] - ZOrder 732 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch3" - SID "2246" - Position [1130, 45, 1180, 105] - ZOrder 487 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch5" - SID "5124" - Position [1280, 117, 1335, 213] - ZOrder 901 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Terminator - Name "Terminator" - SID "5126" - Position [900, 144, 915, 156] - ZOrder 906 - ShowName off - HideAutomaticName off - } - Block { - BlockType SubSystem - Name "Visualize Gain Tuning " - SID "4104" - Ports [5, 0, 1] - Position [380, -137, 690, -53] - ZOrder 720 - RequestExecContextInheritance off - System { - Name "Visualize Gain Tuning " - 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 "305" - Block { - BlockType Inport - Name "smoothingTimeComAndJoints" - SID "4105" - Position [185, 144, 215, 156] - ZOrder 645 - IconDisplay "Port number" - Port { - PortNumber 1 - Name "smoothingTimeComAndJoints" - PropagatedSignals "smoothingTimeMinJerkComDesQDes" - } - } - Block { - BlockType Inport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "4106" - Position [185, 79, 215, 91] - ZOrder 646 - Port "2" - IconDisplay "Port number" - Port { - PortNumber 1 - Name "Left right foot in contact" - } - } - Block { - BlockType Inport - Name "impedances" - SID "4107" - Position [365, 287, 395, 303] - ZOrder 647 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4108" - Position [25, 179, 55, 191] - ZOrder 648 - Port "4" - IconDisplay "Port number" - Port { - PortNumber 1 - Name "state" - PropagatedSignals "state" - } - } - Block { - BlockType Inport - Name "nu" - SID "4109" - Position [-120, 557, -90, 573] - ZOrder 652 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "4110" - Ports [] - Position [27, 335, 47, 355] - ZOrder 649 - } - Block { - BlockType Demux - Name "Demux" - SID "4111" - Ports [1, 2] - Position [210, 406, 215, 444] - ZOrder 655 - ShowName off - Outputs "2" - DisplayOption "bar" - Port { - PortNumber 1 - Name "vb" - } - Port { - PortNumber 2 - Name "wb" - } - } - Block { - BlockType Demux - Name "Demux1" - SID "5016" - Ports [1, 5] - Position [585, 246, 590, 344] - ZOrder 662 - 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 "5017" - Ports [1, 5] - Position [585, 516, 590, 614] - ZOrder 663 - 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 Gain - Name "Gain" - SID "4114" - Position [355, 552, 405, 578] - ZOrder 657 - 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 "4115" - Position [360, 421, 405, 449] - ZOrder 658 - ShowName off - Gain "180/pi" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Scope - Name "Impedances(t)" - SID "4116" - Ports [6] - Position [710, 247, 795, 363] - ZOrder 639 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','ScopeData3','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','8.75','Max" - "YLimReal','21.25','YLabelReal','','MinYLimMag','8.75','MaxYLimMag','21.25','LegendVisibility','on','XGrid',true,'Y" - "Grid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745" - "09803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784" - "31372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294" - "117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color'" - ",[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','L" - "ineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" - "Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle'," - "'-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]" - "}},'ShowContent',true,'Placement',1),struct('MinYLimReal','7.75','MaxYLimReal','10.25','YLabelReal','','MinYLimMag" - "','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0]" - ",'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07" - "45098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745" - "098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]," - "'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','" - "none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyl" - "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" - "}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinYLi" - "mReal','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.6862745098039" - "22 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882" - "353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;" - "0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{st" - "ruct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'" - "Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1]," - "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'Lin" - "eNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','3.75','MaxYLimReal','66.25','YLabelReal',''" - ",'MinYLimMag','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.066666666" - "6666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.8313725490" - "19608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098" - "0392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0." - "5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'" - ",'on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 " - "0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWid" - "th',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vi" - "sible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),st" - "ruct('MinYLimReal','-1.00000','MaxYLimReal','1.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" - "sibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098039" - "22 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 " - "1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0." - "274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePro" - "pertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" - "or',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-'" - ",'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','" - "none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}}" - ",'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','1.00000','MaxYLimReal','3" - ".00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'Plot" - "MagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Color" - "Order',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.39215" - "6862745098 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],'LineSt" - "yle','-','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','-','LineWidth'," - "0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent'" - ",true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','LegendVisibility','on','XGrid',true,'YGrid" - "',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980" - "3922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843137" - "2549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352941176" - "47 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 " - "1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'" - "ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',t" - "rue,'OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Lo" - "cation',[76 116 1387 805])" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "Left right foot in contact" - SID "4117" - Ports [1] - Position [735, 69, 765, 101] - ZOrder 644 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','ScopeData1','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-0.09603'," - "'MaxYLimReal','1.127','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGr" - "id',true,'PlotMagPhase',false,'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.16078431" - "372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411" - "7647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}}" - ",'ShowContent',true,'Placement',1)},'DisplayPropertyDefaults',struct('MinYLimReal','-0.09603','MaxYLimReal','1.127" - "','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPha" - "se',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.39215686274" - "5098 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,'Marker'" - ",'none','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],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true," - "'Placement',1)),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tool" - "s','Measurements',true,'Version','2017b')),'Version','2018a','Location',[1000 538 1560 958])" - NumInputPorts "1" - Floating off - } - Block { - BlockType Selector - Name "Selector1" - SID "4118" - Ports [1, 1] - Position [5, 553, 65, 577] - ZOrder 654 - 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 [5, 413, 65, 437] - ZOrder 653 - ShowName off - InputPortWidth "6+ROBOT_DOF" - IndexOptions "Index vector (dialog)" - Indices "[1:6]" - OutputSizes "1" - } - Block { - BlockType Scope - Name "jointsSmoothingTime" - SID "4120" - Ports [2] - Position [725, 133, 775, 202] - ZOrder 643 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','ScopeData4','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-0.375','M" - "axYLimReal','13.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',true,'YGr" - "id',true,'PlotMagPhase',false,'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.16078431" - "372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411" - "7647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}}" - ",'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.375','MaxYLimReal','13.375','YLabelReal','','MinYLimMa" - "g','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.07" - "45098039215686;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'),str" - "uct('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,'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',2)},'DisplayPro" - "pertyDefaults',struct('MinYLimReal','-0.375','MaxYLimReal','13.375','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.6" - "23529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717" - "647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1]," - "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth'" - ",0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" - "le','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedCha" - "nnelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[2 1])" - ",extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tools','Measurement" - "s',true,'Version','2017b')),'Version','2018a','Location',[179 459 1300 1439])" - NumInputPorts "2" - Floating off - } - Block { - BlockType Scope - Name "linear and angular \nbase velocity" - SID "4121" - Ports [3] - Position [720, 402, 790, 468] - ZOrder 656 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," - "'DataLoggingVariableName','baseVelocityData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','" - "1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct(" - "'MinYLimReal','-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibil" - "ity','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0." - "686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0" - ".411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450" - "9803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperti" - "esCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[" - "1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'Num" - "Lines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.04416','MaxYLimReal','0.050" - "54','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagP" - "hase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrde" - "r',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862" - "745098 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],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" - "truct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',tru" - "e,'Placement',2),struct('MinYLimReal','-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLim" - "Mag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColo" - "r',[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'),struct" - "('Color',[1 0 1],'LineStyle','-','LineWidth',0.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,'Mark" - "er','none','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)},'DisplayPropertyDefaults',struct('MinYLim" - "Real','-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.68627450" - "9803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647" - "05882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039215" - "69 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache'" - ",{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'" - "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth'," - "0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" - "e','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " - "0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0" - ",'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[3 1]),extmgr.Configuration('Tools" - "','Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2017b'))," - "'Version','2018a','Location',[66 459 1986 1439])" - NumInputPorts "3" - Floating off - } - Block { - BlockType Scope - Name "qjDot(t) " - SID "4122" - Ports [6] - Position [715, 513, 800, 637] - ZOrder 650 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," - "'DataLoggingVariableName','jointVelocitiesData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation" - "','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{stru" - "ct('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" - "sibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098039" - "22 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 " - "1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0." - "274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePro" - "pertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" - "or',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-'" - ",'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','" - "none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}}" - ",'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-20.50153','MaxYLimReal'," - "'20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'P" - "lotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Co" - "lorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.39" - "2156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 " - "0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5" - ",'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidt" - "h',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowConte" - "nt',true,'Placement',2),struct('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0'" - ",'MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'Axe" - "sTickColor',[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.074509803" - "9215686;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','none'" - ",'Visible','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,'Marker'," - "'none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'U" - "serDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal" - "','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on'," - "'XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098" - "03922 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.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],'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','Visible'" - ",'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 " - "1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'" - "LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLa" - "belReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',fa" - "lse,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 " - "0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0" - ".831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215" - "686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','Li" - "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none" - "','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" - "olor',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','" - "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker'" - ",'none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Place" - "ment',5),struct('MinYLimReal','-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.623" - "529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764" - "7058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'L" - "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0" - ".5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChann" - "elNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct(" - "'MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisib" - "ility','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.274" - "509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProper" - "tiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'" - ",[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','L" - "ineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" - "Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'N" - "umLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','60','TimeRangeFrames','60','Di" - "splayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),extmgr.Confi" - "guration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[384 347 2304 1327])" - NumInputPorts "6" - Floating off - } - Line { - Name "state" - ZOrder 1 - SrcBlock "state" - SrcPort 1 - Points [65, 0] - Branch { - ZOrder 36 - Points [0, 170] - Branch { - ZOrder 56 - Points [0, 100] - Branch { - ZOrder 46 - Points [0, 170] - DstBlock "qjDot(t) " - DstPort 6 - } - Branch { - ZOrder 43 - DstBlock "linear and angular \nbase velocity" - DstPort 3 - } - } - Branch { - ZOrder 42 - DstBlock "Impedances(t)" - DstPort 6 - } - } - Branch { - ZOrder 35 - Labels [1, 1] - DstBlock "jointsSmoothingTime" - DstPort 2 - } - } - Line { - Name "Left right foot in contact" - ZOrder 8 - Labels [1, 1] - SrcBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - SrcPort 1 - DstBlock "Left right foot in contact" - DstPort 1 - } - Line { - ZOrder 115 - SrcBlock "impedances" - SrcPort 1 - DstBlock "Demux1" - DstPort 1 - } - Line { - Name "smoothingTimeComAndJoints" - ZOrder 10 - Labels [1, 1] - SrcBlock "smoothingTimeComAndJoints" - SrcPort 1 - DstBlock "jointsSmoothingTime" - DstPort 1 - } - Line { - Name "left_leg" - ZOrder 114 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 4 - DstBlock "Impedances(t)" - DstPort 4 - } - Line { - Name "right_leg" - ZOrder 110 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 5 - DstBlock "Impedances(t)" - DstPort 5 - } - Line { - Name "right_arm" - ZOrder 111 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 3 - DstBlock "Impedances(t)" - DstPort 3 - } - Line { - Name "torso" - ZOrder 113 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 1 - DstBlock "Impedances(t)" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 112 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 2 - DstBlock "Impedances(t)" - DstPort 2 - } - Line { - ZOrder 16 - SrcBlock "Selector1" - SrcPort 1 - DstBlock "Gain" - DstPort 1 - } - Line { - Name "left_leg" - ZOrder 107 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 4 - DstBlock "qjDot(t) " - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 105 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 3 - DstBlock "qjDot(t) " - DstPort 3 - } - Line { - Name "torso" - ZOrder 109 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 1 - DstBlock "qjDot(t) " - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 108 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 2 - DstBlock "qjDot(t) " - DstPort 2 - } - Line { - ZOrder 22 - SrcBlock "nu" - SrcPort 1 - Points [27, 0] - Branch { - ZOrder 50 - Points [0, -140] - DstBlock "Selector2" - DstPort 1 - } - Branch { - ZOrder 23 - DstBlock "Selector1" - DstPort 1 - } - } - Line { - ZOrder 25 - SrcBlock "Selector2" - SrcPort 1 - DstBlock "Demux" - DstPort 1 - } - Line { - Name "vb" - ZOrder 26 - Labels [-1, 0] - SrcBlock "Demux" - SrcPort 1 - DstBlock "linear and angular \nbase velocity" - DstPort 1 - } - Line { - Name "wb" - ZOrder 27 - Labels [1, 1] - SrcBlock "Demux" - SrcPort 2 - DstBlock "Gain1" - DstPort 1 - } - Line { - ZOrder 116 - SrcBlock "Gain" - SrcPort 1 - DstBlock "Demux2" - DstPort 1 - } - Line { - ZOrder 29 - SrcBlock "Gain1" - SrcPort 1 - DstBlock "linear and angular \nbase velocity" - DstPort 2 - } - Line { - Name "right_leg" - ZOrder 106 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 5 - DstBlock "qjDot(t) " - DstPort 5 - } - } - } - Block { - BlockType Constant - Name "joints.smoothingTime" - SID "4819" - Position [470, 23, 580, 37] - ZOrder 724 - ShowName off - Value "Sm.smoothingTimeCoM" - } - Block { - BlockType Outport - Name "impedances" - SID "4034" - Position [330, 188, 360, 202] - ZOrder 705 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "gainsPCOM" - SID "4035" - Position [330, 438, 360, 452] - ZOrder 706 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "gainsDCOM" - SID "4036" - Position [330, 488, 360, 502] - ZOrder 707 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_b" - SID "4037" - Position [330, 537, 360, 553] - ZOrder 708 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qj" - SID "4038" - Position [330, 337, 360, 353] - ZOrder 709 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "nu" - SID "4039" - Position [330, 287, 360, 303] - ZOrder 710 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "4040" - Position [330, 138, 360, 152] - ZOrder 711 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "desired_x_dx_ddx_CoM" - SID "4041" - Position [1380, 68, 1410, 82] - ZOrder 712 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qjDes" - SID "5125" - Position [1380, 158, 1410, 172] - ZOrder 904 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "state" - SID "4043" - Position [330, 238, 360, 252] - ZOrder 714 - Port "10" - IconDisplay "Port number" - } - Line { - ZOrder 405 - SrcBlock "Reshape" - SrcPort 1 - DstBlock "desired_x_dx_ddx_CoM" - DstPort 1 - } - Line { - ZOrder 8 - SrcBlock "Minimum Jerk Trajectory Generator2" - SrcPort 1 - DstBlock "Mux" - DstPort 1 - } - Line { - ZOrder 9 - SrcBlock "Minimum Jerk Trajectory Generator2" - SrcPort 2 - DstBlock "Mux" - DstPort 2 - } - Line { - ZOrder 11 - SrcBlock "Switch3" - SrcPort 1 - DstBlock "Reshape" - DstPort 1 - } - Line { - ZOrder 10 - SrcBlock "Minimum Jerk Trajectory Generator2" - SrcPort 3 - DstBlock "Mux" - DstPort 3 - } - Line { - Name "qj" - ZOrder 402 - Labels [0, 0] - SrcBlock "Demux" - SrcPort 7 - DstBlock "qj" - DstPort 1 - } - Line { - ZOrder 401 - SrcBlock "Demux" - SrcPort 11 - DstBlock "Reshape1" - DstPort 1 - } - Line { - Name "state" - ZOrder 407 - Labels [0, 0] - SrcBlock "Demux" - SrcPort 5 - Points [125, 0] - Branch { - ZOrder 453 - Points [0, -325] - DstBlock "Visualize Gain Tuning " - DstPort 4 - } - Branch { - ZOrder 452 - DstBlock "state" - DstPort 1 - } - } - Line { - ZOrder 83 - SrcBlock "Mux" - SrcPort 1 - Points [25, 0; 0, 45] - DstBlock "Switch3" - DstPort 1 - } - Line { - ZOrder 84 - SrcBlock "Selector" - SrcPort 1 - DstBlock "Minimum Jerk Trajectory Generator2" - DstPort 1 - } - Line { - Name "nu" - ZOrder 403 - Labels [0, 0] - SrcBlock "Demux" - SrcPort 6 - Points [138, 0] - Branch { - ZOrder 450 - Points [0, -360] - DstBlock "Visualize Gain Tuning " - DstPort 5 - } - Branch { - ZOrder 449 - DstBlock "nu" - DstPort 1 - } - } - Line { - ZOrder 397 - SrcBlock "StateAndReferences" - SrcPort 1 - DstBlock "Demux" - DstPort 1 - } - Line { - ZOrder 398 - SrcBlock "Demux" - SrcPort 4 - Points [111, 0] - Branch { - ZOrder 455 - Points [0, -290] - DstBlock "Visualize Gain Tuning " - DstPort 3 - } - Branch { - ZOrder 454 - DstBlock "impedances" - DstPort 1 - } - } - Line { - ZOrder 404 - SrcBlock "Demux" - SrcPort 3 - Points [98, 0] - Branch { - ZOrder 460 - Points [0, -255] - DstBlock "Visualize Gain Tuning " - DstPort 2 - } - Branch { - ZOrder 459 - DstBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - DstPort 1 - } - } - Line { - ZOrder 399 - SrcBlock "Demux" - SrcPort 9 - DstBlock "gainsPCOM" - DstPort 1 - } - Line { - ZOrder 400 - SrcBlock "Demux" - SrcPort 10 - DstBlock "gainsDCOM" - DstPort 1 - } - Line { - Name "CoMDes" - ZOrder 409 - Labels [0, 0] - SrcBlock "Demux" - SrcPort 1 - Points [343, 0] - Branch { - ZOrder 488 - Points [0, -45] - DstBlock "Selector" - DstPort 1 - } - Branch { - ZOrder 134 - Points [0, 50] - DstBlock "Switch3" - DstPort 3 - } - } - Line { - ZOrder 415 - SrcBlock "Reshape1" - SrcPort 1 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 446 - SrcBlock "ON_GAZEBO\n4" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 2 - } - Line { - ZOrder 447 - SrcBlock "ON_GAZEBO\n3" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 1 - } - Line { - ZOrder 448 - SrcBlock "Logical\nOperator1" - SrcPort 1 - Points [40, 0] - DstBlock "Visualize Gain Tuning " - DstPort enable - } - Line { - ZOrder 509 - SrcBlock "Switch1" - SrcPort 1 - DstBlock "Minimum Jerk Trajectory Generator2" - DstPort 2 - } - Line { - ZOrder 510 - SrcBlock "joints.smoothingTime" - SrcPort 1 - DstBlock "Switch1" - DstPort 3 - } - Line { - Name "smoothingTimeMinJerkComDesQDes" - ZOrder 76 - Labels [0, 0] - SrcBlock "Demux" - SrcPort 8 - Points [86, 0] - Branch { - ZOrder 458 - Points [414, 0; 0, -235] - Branch { - ZOrder 576 - Points [0, -85] - Branch { - ZOrder 513 - Points [0, -55] - Branch { - ZOrder 516 - Points [0, -10] - DstBlock "Switch1" - DstPort 1 - } - Branch { - ZOrder 515 - DstBlock "Switch1" - DstPort 2 - } - } - Branch { - ZOrder 512 - Labels [1, 1] - DstBlock "Switch3" - DstPort 2 - } - } - Branch { - ZOrder 559 - DstBlock "Minimum Jerk Trajectory Generator1" - DstPort 2 - } - } - Branch { - ZOrder 457 - Points [0, -520] - DstBlock "Visualize Gain Tuning " - DstPort 1 - } - } - Line { - ZOrder 549 - SrcBlock "Minimum Jerk Trajectory Generator1" - SrcPort 1 - DstBlock "Switch5" - DstPort 1 - } - Line { - ZOrder 552 - SrcBlock "SMOOTH_DES_COM2" - SrcPort 1 - DstBlock "Switch5" - DstPort 2 - } - Line { - Name "qDes" - ZOrder 555 - Labels [0, 0] - SrcBlock "Demux" - SrcPort 2 - Points [322, 0; 0, 45] - Branch { - ZOrder 563 - Labels [1, 0] - Points [0, 55] - DstBlock "Switch5" - DstPort 3 - } - Branch { - ZOrder 562 - Labels [-1, 0] - DstBlock "Minimum Jerk Trajectory Generator1" - DstPort 1 - } - } - Line { - ZOrder 554 - SrcBlock "Switch5" - SrcPort 1 - DstBlock "qjDes" - DstPort 1 - } - Line { - ZOrder 577 - SrcBlock "Minimum Jerk Trajectory Generator1" - SrcPort 3 - DstBlock "Goto \"Compute joint torques\"" - DstPort 1 - } - Line { - ZOrder 578 - SrcBlock "Minimum Jerk Trajectory Generator1" - SrcPort 2 - DstBlock "Terminator" - DstPort 1 - } - } - } - Block { - BlockType Selector - Name "Selector" - SID "4502" - Ports [1, 1] - Position [590, 3116, 630, 3144] - ZOrder 867 - NumberOfDimensions "2" - InputPortWidth "3" - IndexOptions "Index vector (dialog),Index vector (dialog)" - Indices "[1 2 3],[1]" - OutputSizes "1,1" - } - Block { - BlockType SubSystem - Name "State Machine Standup" - SID "2163" - Ports [1, 11, 1] - Position [-210, 2903, -30, 3117] - ZOrder 506 - Priority "-100" - RequestExecContextInheritance off - System { - Name "State Machine Standup" - 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 "275" - Block { - BlockType Inport - Name "jointAngles" - SID "3166" - Position [-575, 283, -545, 297] - ZOrder 676 - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "4515" - Ports [] - Position [-318, -10, -288, 20] - ZOrder 943 - ShowName off - } - Block { - BlockType Clock - Name "Clock1" - SID "2166" - Position [-30, 420, -10, 440] - ZOrder 532 - ShowName off - } - Block { - BlockType SubSystem - Name "Compute State Velocity" - SID "4211" - Ports [4, 1] - Position [455, -116, 605, -44] - ZOrder 889 - 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 "515" - Block { - BlockType Inport - Name "qj" - SID "4212" - Position [425, -432, 455, -418] - ZOrder 562 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_b" - SID "4213" - Position [425, -382, 455, -368] - ZOrder 561 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "feetInContact\n" - SID "4214" - Position [675, -402, 705, -388] - ZOrder 244 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state " - SID "4796" - Position [425, -477, 455, -463] - ZOrder 760 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "Compute Base Velocity" - SID "4215" - Ports [4, 1] - Position [780, -449, 1045, -361] - 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 "3794" - Block { - BlockType Inport - Name "J_LeftFixedLink" - SID "4215::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "J_RightFixedLink" - SID "4215::1472" - Position [20, 136, 40, 154] - ZOrder 42 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "feetInContact" - SID "4215::1473" - Position [20, 171, 40, 189] - ZOrder 43 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qjDot" - SID "4215::23" - Position [20, 206, 40, 224] - ZOrder 9 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4215::3793" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 162 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4215::3792" - Tag "Stateflow S-Function torqueBalancingStandup 7" - Ports [4, 2] - Position [180, 102, 230, 203] - ZOrder 161 - FunctionName "sf_sfun" - Parameters "Reg" - PortCounts "[4 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "nu_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4215::3794" - Position [460, 241, 480, 259] - ZOrder 163 - } - Block { - BlockType Outport - Name "nu_b" - SID "4215::5" - Position [460, 101, 480, 119] - ZOrder -6 - IconDisplay "Port number" - } - Line { - ZOrder 141 - SrcBlock "J_LeftFixedLink" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 142 - SrcBlock "J_RightFixedLink" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 143 - SrcBlock "feetInContact" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 144 - SrcBlock "qjDot" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - Name "nu_b" - ZOrder 145 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "nu_b" - DstPort 1 - } - Line { - ZOrder 146 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 147 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Fixed Links Jacobians" - SID "4809" - Ports [3, 2] - Position [515, -446, 645, -404] - ZOrder -26 - RequestExecContextInheritance off - System { - Name "Fixed Links Jacobians" - Location [65, 24, 2560, 1440] - 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 "372" - Block { - BlockType Inport - Name "state " - SID "4810" - Position [120, 168, 150, 182] - ZOrder 763 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "4813" - Position [70, 263, 100, 277] - ZOrder 766 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_b" - SID "4814" - Position [70, 363, 100, 377] - ZOrder 767 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name " " - SID "4797" - Ports [2, 1] - Position [240, 240, 400, 280] - ZOrder 756 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_LEG" - } - Block { - BlockType Reference - Name " " - SID "4798" - Ports [2, 1] - Position [235, 360, 395, 400] - ZOrder 755 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_LEG" - } - Block { - BlockType Reference - Name " " - SID "4799" - Ports [2, 1] - Position [235, 410, 395, 450] - ZOrder 757 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Reference - Name " " - SID "4800" - Ports [2, 1] - Position [240, 290, 395, 330] - ZOrder 754 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType Constant - Name "Constant" - SID "4801" - Position [85, 142, 190, 158] - ZOrder 761 - ShowName off - Value "Config.iCubStandUp" - } - Block { - BlockType SubSystem - Name "Legs in Contact" - SID "4804" - Ports [2, 1] - Position [235, 136, 405, 189] - ZOrder 762 - LibraryVersion "1.32" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "Legs in Contact" - 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 "4554" - Block { - BlockType Inport - Name "icubStandup" - SID "4804::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4804::19" - Position [20, 136, 40, 154] - ZOrder 10 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4804::4553" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 66 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4804::4552" - Tag "Stateflow S-Function torqueBalancingStandup 14" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 65 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "legsInContact" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4804::4554" - Position [460, 241, 480, 259] - ZOrder 67 - } - Block { - BlockType Outport - Name "legsInContact" - SID "4804::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 81 - SrcBlock "icubStandup" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 82 - SrcBlock "state" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "legsInContact" - ZOrder 83 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "legsInContact" - DstPort 1 - } - Line { - ZOrder 84 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 85 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Switch - Name "Switch1" - SID "4806" - Position [455, 245, 505, 325] - ZOrder 758 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch2" - SID "4807" - Position [455, 365, 505, 445] - ZOrder 759 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Outport - Name "J_LeftFixed" - SID "4815" - Position [550, 278, 580, 292] - ZOrder 768 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_RightFixed" - SID "4816" - Position [550, 398, 580, 412] - ZOrder 769 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 58 - SrcBlock "Switch2" - SrcPort 1 - DstBlock "J_RightFixed" - DstPort 1 - } - Line { - ZOrder 26 - SrcBlock "Legs in Contact" - SrcPort 1 - Points [18, 0; 0, 120] - Branch { - ZOrder 25 - Points [0, 120] - DstBlock "Switch2" - DstPort 2 - } - Branch { - ZOrder 24 - DstBlock "Switch1" - DstPort 2 - } - } - Line { - ZOrder 27 - SrcBlock " " - SrcPort 1 - DstBlock "Switch1" - DstPort 1 - } - Line { - ZOrder 28 - SrcBlock " " - SrcPort 1 - DstBlock "Switch1" - DstPort 3 - } - Line { - ZOrder 30 - SrcBlock " " - SrcPort 1 - DstBlock "Switch2" - DstPort 3 - } - Line { - ZOrder 29 - SrcBlock " " - SrcPort 1 - DstBlock "Switch2" - DstPort 1 - } - Line { - ZOrder 52 - SrcBlock "state " - SrcPort 1 - DstBlock "Legs in Contact" - DstPort 2 - } - Line { - ZOrder 41 - SrcBlock "Constant" - SrcPort 1 - DstBlock "Legs in Contact" - DstPort 1 - } - Line { - ZOrder 57 - SrcBlock "Switch1" - SrcPort 1 - DstBlock "J_LeftFixed" - DstPort 1 - } - Line { - ZOrder 61 - SrcBlock "qj" - SrcPort 1 - Points [91, 0] - Branch { - ZOrder 63 - Points [0, 50] - Branch { - ZOrder 65 - Points [0, 70] - Branch { - ZOrder 67 - Points [0, 50] - DstBlock " " - DstPort 2 - } - Branch { - ZOrder 66 - DstBlock " " - DstPort 2 - } - } - Branch { - ZOrder 64 - DstBlock " " - DstPort 2 - } - } - Branch { - ZOrder 62 - DstBlock " " - DstPort 2 - } - } - Line { - ZOrder 68 - SrcBlock "w_H_b" - SrcPort 1 - Points [45, 0] - Branch { - ZOrder 74 - Points [0, 50] - DstBlock " " - DstPort 1 - } - Branch { - ZOrder 70 - Points [0, -70] - Branch { - ZOrder 72 - Points [0, -50] - DstBlock " " - DstPort 1 - } - Branch { - ZOrder 71 - DstBlock " " - DstPort 1 - } - } - Branch { - ZOrder 69 - DstBlock " " - DstPort 1 - } - } - } - } - Block { - BlockType Reference - Name "Get Measurement" - SID "5014" - Ports [0, 1] - Position [655, -354, 725, -326] - ZOrder 869 - BackgroundColor "[0.513700, 0.674500, 1.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Velocity" - } - Block { - BlockType Mux - Name "Mux" - SID "4224" - Ports [2, 1] - Position [1075, -437, 1080, -308] - ZOrder -12 - ShowName off - Inputs "2" - DisplayOption "bar" - } - Block { - BlockType Outport - Name "nu" - SID "4225" - Position [1100, -377, 1130, -363] - ZOrder -15 - IconDisplay "Port number" - } - Line { - ZOrder 61 - SrcBlock "Get Measurement" - SrcPort 1 - Points [23, 0] - Branch { - ZOrder 2 - 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 "feetInContact\n" - SrcPort 1 - DstBlock "Compute Base Velocity" - DstPort 3 - } - Line { - ZOrder 6 - SrcBlock "Compute Base Velocity" - SrcPort 1 - DstBlock "Mux" - DstPort 1 - } - Line { - ZOrder 56 - SrcBlock "Fixed Links Jacobians" - SrcPort 1 - DstBlock "Compute Base Velocity" - DstPort 1 - } - Line { - ZOrder 57 - SrcBlock "Fixed Links Jacobians" - SrcPort 2 - DstBlock "Compute Base Velocity" - DstPort 2 - } - Line { - ZOrder 52 - SrcBlock "state " - SrcPort 1 - Points [25, 0; 0, 30] - DstBlock "Fixed Links Jacobians" - DstPort 1 - } - Line { - ZOrder 53 - SrcBlock "qj" - SrcPort 1 - DstBlock "Fixed Links Jacobians" - DstPort 2 - } - Line { - ZOrder 54 - SrcBlock "w_H_b" - SrcPort 1 - Points [25, 0; 0, -35] - DstBlock "Fixed Links Jacobians" - DstPort 3 - } - } - } - Block { - BlockType SubSystem - Name "Compute base to fixed link transform" - SID "4307" - Ports [2, 2] - Position [-375, 342, -265, 413] - ZOrder 900 - 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 "372" - Block { - BlockType Inport - Name "qj" - SID "4310" - Position [50, 38, 80, 52] - ZOrder 902 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4311" - Position [230, 183, 260, 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 SubSystem - Name "LFoot to base link transform " - SID "4245" - Ports [4, 1] - Position [435, 15, 580, 55] - ZOrder 893 - RequestExecContextInheritance off - System { - Name "LFoot to base 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 "248" - Block { - BlockType Inport - Name "qj" - 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.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - 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.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.BASE" - } - Block { - BlockType Product - Name "Inv\\*" - SID "4252" - Ports [2, 1] - Position [825, 40, 895, 95] - ZOrder 731 - 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\\* " - SID "4253" - Ports [2, 1] - Position [825, 184, 895, 231] - ZOrder 738 - 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 [980, 308, 1055, 332] - ZOrder 742 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.NECK_POS" - signalSize "6" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - 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 SubSystem - Name "fromImuToHomogeousTransformFCN" - SID "4257" - Ports [6, 1] - Position [1305, 47, 1540, 343] - ZOrder 733 - ShowName off - LibraryVersion "1.30" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "fromImuToHomogeousTransformFCN" - 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 "3785" - SIDPrevWatermark "9" - Block { - BlockType Inport - Name "imu_H_link" - SID "4257::112" - Position [20, 101, 40, 119] - ZOrder 82 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "imu_H_link_0" - SID "4257::113" - Position [20, 136, 40, 154] - ZOrder 83 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "link_H_base" - SID "4257::111" - Position [20, 171, 40, 189] - ZOrder 81 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial_0" - SID "4257::115" - Position [20, 206, 40, 224] - ZOrder 85 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - 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::3784" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 184 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4257::3783" - Tag "Stateflow S-Function torqueBalancingStandup 8" - Ports [6, 2] - Position [180, 102, 230, 243] - ZOrder 183 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[6 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "w_H_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4257::3785" - Position [460, 241, 480, 259] - ZOrder 185 - } - Block { - BlockType Outport - Name "w_H_b" - SID "4257::106" - Position [460, 101, 480, 119] - ZOrder 80 - IconDisplay "Port number" - } - Line { - ZOrder 181 - SrcBlock "imu_H_link" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 182 - SrcBlock "imu_H_link_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 183 - SrcBlock "link_H_base" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 184 - SrcBlock "inertial_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 185 - SrcBlock "inertial" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 186 - SrcBlock "neck_pos" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - Name "w_H_b" - ZOrder 187 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 188 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 189 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - 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.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - 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.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - 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 [65, 24, 2560, 1440] - 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 "562" - 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 "6" - 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" - } - 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" - } - 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\\* " - SrcPort 1 - Points [65, 0] - Branch { - ZOrder 3 - Points [0, 165] - DstBlock "Switch6" - DstPort 3 - } - Branch { - ZOrder 4 - Points [0, -40] - DstBlock "fromImuToHomogeousTransformFCN" - 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 "fromImuToHomogeousTransformFCN" - DstPort 1 - } - Branch { - ZOrder 8 - Points [0, 50] - DstBlock "holder\n1" - DstPort 1 - } - } - Line { - ZOrder 9 - SrcBlock "holder\n2" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 4 - } - Line { - ZOrder 10 - SrcBlock "Fixed base to imu transform" - SrcPort 1 - DstBlock "Inv\\*" - DstPort 1 - } - Line { - ZOrder 11 - SrcBlock "holder\n1" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 2 - } - Line { - ZOrder 12 - SrcBlock "qj" - 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 "fromImuToHomogeousTransformFCN" - SrcPort 1 - DstBlock "Switch6" - DstPort 1 - } - Line { - ZOrder 16 - SrcBlock "Fixed base to root link transform" - SrcPort 1 - DstBlock "Inv\\* " - 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 "fromImuToHomogeousTransformFCN" - DstPort 5 - } - } - Line { - ZOrder 20 - SrcBlock "Neck Position" - SrcPort 1 - DstBlock "neck transform" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "neck transform" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 6 - } - Line { - ZOrder 22 - SrcBlock "w_H_link" - SrcPort 1 - Points [98, 0] - Branch { - ZOrder 23 - Points [0, 55] - DstBlock "Inv\\* " - 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 - } - } - } - } - Block { - BlockType Reference - Name "LFoot to world transform (fixed base)" - SID "4269" - Ports [2, 1] - Position [165, 22, 330, 53] - ZOrder 894 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType SubSystem - Name "LLeg to base link transform" - SID "4270" - Ports [4, 1] - Position [435, 100, 580, 140] - ZOrder 896 - RequestExecContextInheritance off - System { - Name "LLeg to base 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 "248" - Block { - BlockType Inport - Name "qj" - SID "4271" - Position [470, 227, 495, 243] - ZOrder 736 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4272" - Position [1000, 212, 1030, 228] - ZOrder 741 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_link" - SID "4273" - Position [655, 132, 680, 148] - ZOrder 744 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_base_fixed" - SID "4274" - Position [470, 32, 495, 48] - ZOrder 745 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "Fixed base to imu transform" - SID "4275" - Ports [2, 1] - Position [575, 25, 740, 85] - ZOrder 729 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.IMU" - } - Block { - BlockType Reference - Name "Fixed base to root link transform" - SID "4276" - Ports [2, 1] - Position [575, 190, 740, 250] - ZOrder 737 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.BASE" - } - Block { - BlockType Product - Name "Inv\\*" - SID "4277" - Ports [2, 1] - Position [825, 40, 895, 95] - ZOrder 731 - 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\\* " - SID "4278" - Ports [2, 1] - Position [825, 184, 895, 231] - ZOrder 738 - 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 "4279" - Ports [0, 1] - Position [980, 308, 1055, 332] - ZOrder 742 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.NECK_POS" - signalSize "6" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - Block { - BlockType Switch - Name "Switch6" - SID "4280" - Position [1760, 151, 1840, 419] - ZOrder 734 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE" - SID "4281" - Position [1570, 275, 1725, 295] - ZOrder 735 - ShowName off - Value "Config.USE_IMU4EST_BASE" - } - Block { - BlockType SubSystem - Name "fromImuToHomogeousTransformFCN" - SID "4282" - Ports [6, 1] - Position [1305, 47, 1540, 343] - ZOrder 733 - ShowName off - LibraryVersion "1.30" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "fromImuToHomogeousTransformFCN" - 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 "3785" - SIDPrevWatermark "9" - Block { - BlockType Inport - Name "imu_H_link" - SID "4282::112" - Position [20, 101, 40, 119] - ZOrder 82 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "imu_H_link_0" - SID "4282::113" - Position [20, 136, 40, 154] - ZOrder 83 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "link_H_base" - SID "4282::111" - Position [20, 171, 40, 189] - ZOrder 81 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial_0" - SID "4282::115" - Position [20, 206, 40, 224] - ZOrder 85 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4282::93" - Position [20, 246, 40, 264] - ZOrder 69 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "neck_pos" - SID "4282::118" - Position [20, 281, 40, 299] - ZOrder 86 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4282::3784" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 184 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4282::3783" - Tag "Stateflow S-Function torqueBalancingStandup 3" - Ports [6, 2] - Position [180, 102, 230, 243] - ZOrder 183 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[6 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "w_H_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4282::3785" - Position [460, 241, 480, 259] - ZOrder 185 - } - Block { - BlockType Outport - Name "w_H_b" - SID "4282::106" - Position [460, 101, 480, 119] - ZOrder 80 - IconDisplay "Port number" - } - Line { - ZOrder 181 - SrcBlock "imu_H_link" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 182 - SrcBlock "imu_H_link_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 183 - SrcBlock "link_H_base" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 184 - SrcBlock "inertial_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 185 - SrcBlock "inertial" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 186 - SrcBlock "neck_pos" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - Name "w_H_b" - ZOrder 187 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 188 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 189 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "holder\n1" - SID "4283" - Ports [1, 1] - Position [1175, 113, 1225, 127] - ZOrder 739 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "holder\n2" - SID "4284" - Ports [1, 1] - Position [1175, 213, 1225, 227] - ZOrder 740 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "neck transform" - SID "4285" - Ports [1, 1] - Position [1135, 305, 1260, 335] - ZOrder 743 - RequestExecContextInheritance off - System { - Name "neck transform" - Location [65, 24, 2560, 1440] - 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 "562" - Block { - BlockType Inport - Name "neck port" - SID "4286" - Position [45, 18, 75, 32] - ZOrder 725 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant" - SID "4287" - Position [95, 85, 170, 105] - ZOrder 720 - ShowName off - Value "zeros(3,1)" - } - Block { - BlockType Gain - Name "Gain" - SID "4288" - 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 "4289" - Ports [1, 1] - Position [190, 13, 230, 37] - ZOrder 723 - ShowName off - InputPortWidth "6" - IndexOptions "Index vector (dialog)" - Indices "[1:3]" - OutputSizes "1" - Port { - PortNumber 1 - Name "neck pos" - } - } - Block { - BlockType Switch - Name "Switch" - SID "4290" - Position [300, 7, 375, 113] - ZOrder 722 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE1" - SID "4291" - Position [60, 50, 210, 70] - ZOrder 719 - ShowName off - Value "Config.CORRECT_NECK_IMU" - } - Block { - BlockType Outport - Name "neck position" - SID "4292" - Position [425, 53, 455, 67] - ZOrder 726 - IconDisplay "Port number" - } - 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 "4293" - Position [1875, 278, 1905, 292] - ZOrder 732 - IconDisplay "Port number" - } - 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\\* " - SrcPort 1 - Points [65, 0] - Branch { - ZOrder 3 - Points [0, 165] - DstBlock "Switch6" - DstPort 3 - } - Branch { - ZOrder 4 - Points [0, -40] - DstBlock "fromImuToHomogeousTransformFCN" - 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 "fromImuToHomogeousTransformFCN" - DstPort 1 - } - Branch { - ZOrder 8 - Points [0, 50] - DstBlock "holder\n1" - DstPort 1 - } - } - Line { - ZOrder 9 - SrcBlock "holder\n2" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 4 - } - Line { - ZOrder 10 - SrcBlock "Fixed base to imu transform" - SrcPort 1 - DstBlock "Inv\\*" - DstPort 1 - } - Line { - ZOrder 11 - SrcBlock "holder\n1" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 2 - } - Line { - ZOrder 12 - SrcBlock "qj" - 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 "fromImuToHomogeousTransformFCN" - SrcPort 1 - DstBlock "Switch6" - DstPort 1 - } - Line { - ZOrder 16 - SrcBlock "Fixed base to root link transform" - SrcPort 1 - DstBlock "Inv\\* " - 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 "fromImuToHomogeousTransformFCN" - DstPort 5 - } - } - Line { - ZOrder 20 - SrcBlock "Neck Position" - SrcPort 1 - DstBlock "neck transform" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "neck transform" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 6 - } - Line { - ZOrder 22 - SrcBlock "w_H_link" - SrcPort 1 - Points [98, 0] - Branch { - ZOrder 23 - Points [0, 55] - DstBlock "Inv\\* " - 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 - } - } - } - } - Block { - BlockType Reference - Name "LLeg to world transform (fixed base)" - SID "4306" - Ports [2, 1] - Position [165, 107, 330, 138] - ZOrder 899 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_LEG" - } - Block { - BlockType Outport - Name "fixed_LFoot_H_b" - SID "4318" - Position [650, 28, 680, 42] - ZOrder 910 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "fixed_LLeg_H_b" - SID "4320" - Position [650, 113, 680, 127] - ZOrder 912 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 250 - SrcBlock "Constant7" - SrcPort 1 - Points [10, 0] - Branch { - ZOrder 263 - Points [0, -22; 255, 0; 0, 42] - Branch { - ZOrder 265 - Points [0, 85] - DstBlock "LLeg to base link transform" - DstPort 4 - } - Branch { - ZOrder 264 - DstBlock "LFoot to base link transform " - DstPort 4 - } - } - Branch { - ZOrder 252 - Points [0, 85] - DstBlock "LLeg to world transform (fixed base)" - DstPort 1 - } - Branch { - ZOrder 251 - DstBlock "LFoot to world transform (fixed base)" - DstPort 1 - } - } - Line { - ZOrder 244 - SrcBlock "LLeg to base link transform" - SrcPort 1 - DstBlock "fixed_LLeg_H_b" - DstPort 1 - } - Line { - ZOrder 245 - SrcBlock "LFoot to base link transform " - SrcPort 1 - DstBlock "fixed_LFoot_H_b" - DstPort 1 - } - Line { - ZOrder 247 - SrcBlock "LFoot to world transform (fixed base)" - SrcPort 1 - DstBlock "LFoot to base link transform " - DstPort 3 - } - Line { - ZOrder 248 - SrcBlock "LLeg to world transform (fixed base)" - SrcPort 1 - DstBlock "LLeg to base link transform" - DstPort 3 - } - Line { - ZOrder 249 - SrcBlock "qj" - SrcPort 1 - Points [51, 0] - Branch { - ZOrder 254 - Points [0, 85] - Branch { - ZOrder 260 - Points [0, 37; 274, 0; 0, -62] - Branch { - ZOrder 262 - Points [0, -85] - DstBlock "LFoot to base link transform " - DstPort 1 - } - Branch { - ZOrder 261 - DstBlock "LLeg to base link transform" - DstPort 1 - } - } - Branch { - ZOrder 259 - DstBlock "LLeg to world transform (fixed base)" - DstPort 2 - } - } - Branch { - ZOrder 253 - DstBlock "LFoot to world transform (fixed base)" - DstPort 2 - } - } - Line { - ZOrder 255 - SrcBlock "inertial" - SrcPort 1 - Points [119, 0; 0, -75] - Branch { - ZOrder 258 - Points [0, -85] - DstBlock "LFoot to base link transform " - DstPort 2 - } - Branch { - ZOrder 257 - DstBlock "LLeg to base link transform" - DstPort 2 - } - } - } - } - Block { - BlockType Constant - Name "Constant1" - SID "2170" - Position [390, 219, 445, 231] - ZOrder 543 - ShowName off - Value "zeros(6,1)" - } - Block { - BlockType Demux - Name "Demux1" - SID "3690" - Ports [1, 3] - Position [525, 273, 530, 387] - ZOrder 695 - ShowName off - Outputs "[ROBOT_DOF,3,3]" - DisplayOption "bar" - } - Block { - BlockType Logic - Name "Logical\nOperator2" - SID "4520" - Ports [2, 1] - Position [130, -93, 160, -62] - ZOrder 948 - ShowName off - Operator "OR" - AllPortsSameDT off - OutDataTypeStr "boolean" - } - Block { - BlockType Reference - Name "Minimum Jerk Trajectory Generator" - SID "2176" - Ports [1, 1] - Position [405, 320, 505, 340] - ZOrder 594 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - SourceType "Minimum Jerk Trajectory Generator" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - externalSettlingTime off - settlingTime "Gain.SmoothingTimeGainScheduling" - sampleTime "Config.Ts" - resetOnSettlingTime off - firstDerivatives off - secondDerivatives off - explicitInitialValue off - } - Block { - BlockType Mux - Name "Mux" - SID "3689" - Ports [3, 1] - Position [380, 273, 385, 387] - ZOrder 694 - ShowName off - Inputs "3" - DisplayOption "bar" - } - Block { - BlockType Mux - Name "Mux2" - SID "2178" - Ports [2, 1] - Position [465, 203, 470, 232] - ZOrder 542 - ShowName off - Inputs "2" - DisplayOption "bar" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n5" - SID "4526" - Position [-60, -91, 45, -79] - ZOrder 947 - ShowName off - Value "Config.SCOPES_ALL" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n6" - SID "4527" - Position [-90, -76, 75, -64] - ZOrder 949 - ShowName off - Value "Config.SCOPES_EXT_WRENCHES" - } - Block { - BlockType Reshape - Name "Reshape1" - SID "4503" - Ports [1, 1] - Position [525, 119, 570, 141] - ZOrder 901 - ShowName off - OutputDimensions "[4;4]" - } - Block { - BlockType Constant - Name "Standup with human" - SID "4569" - Position [-105, 459, 50, 471] - ZOrder 942 - Value "Config.STANDUP_WITH_HUMAN" - } - Block { - BlockType SubSystem - Name "Visualise external wrenches" - SID "4528" - Ports [5, 0, 1] - Position [115, -42, 255, 62] - ZOrder 946 - RequestExecContextInheritance off - System { - Name "Visualise external wrenches" - 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 "212" - Block { - BlockType Inport - Name "wL_arm" - SID "4529" - Position [715, 32, 760, 48] - ZOrder 702 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "wR_arm" - SID "4530" - Position [715, 157, 760, 173] - ZOrder 701 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "wL_WBDT" - SID "4531" - Position [715, 272, 760, 288] - ZOrder 648 - Port "3" - IconDisplay "Port number" - Port { - PortNumber 1 - Name "wL_WBDT" - PropagatedSignals "state" - } - } - Block { - BlockType Inport - Name "wR_WBDT" - SID "4532" - Position [715, 392, 760, 408] - ZOrder 682 - Port "4" - IconDisplay "Port number" - Port { - PortNumber 1 - Name "wR_WBDT" - } - } - Block { - BlockType Inport - Name "state" - SID "4533" - Position [-130, 92, -85, 108] - ZOrder 700 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "4534" - Ports [] - Position [-118, 185, -98, 205] - ZOrder 649 - } - Block { - BlockType SubSystem - Name "Compute current threshold" - SID "4536" - Ports [1, 4] - Position [245, 183, 465, 387] - ZOrder 705 - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "Compute current threshold" - 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 "4556" - Block { - BlockType Inport - Name "state" - SID "4536::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4536::4555" - Ports [1, 1] - Position [270, 280, 320, 320] - ZOrder 69 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4536::4554" - Tag "Stateflow S-Function torqueBalancingStandup 37" - Ports [1, 5] - Position [180, 100, 230, 220] - ZOrder 68 - FunctionName "sf_sfun" - Parameters "Sm" - PortCounts "[1 5]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "thresholdContactLeftHand" - } - Port { - PortNumber 3 - Name "thresholdContactRightHand" - } - Port { - PortNumber 4 - Name "thresholdContactLeftFoot" - } - Port { - PortNumber 5 - Name "thresholdContactRightFoot" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4536::4556" - Position [460, 291, 480, 309] - ZOrder 70 - } - Block { - BlockType Outport - Name "thresholdContactLeftHand" - SID "4536::4507" - Position [460, 101, 480, 119] - ZOrder 21 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "thresholdContactRightHand" - SID "4536::4508" - Position [460, 136, 480, 154] - ZOrder 22 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "thresholdContactLeftFoot" - SID "4536::5" - Position [460, 171, 480, 189] - ZOrder -5 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "thresholdContactRightFoot" - SID "4536::23" - Position [460, 206, 480, 224] - ZOrder 14 - Port "4" - IconDisplay "Port number" - } - Line { - ZOrder 138 - SrcBlock "state" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - Name "thresholdContactLeftHand" - ZOrder 139 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - Points [0, -25] - DstBlock "thresholdContactLeftHand" - DstPort 1 - } - Line { - Name "thresholdContactRightHand" - ZOrder 140 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 3 - DstBlock "thresholdContactRightHand" - DstPort 1 - } - Line { - Name "thresholdContactLeftFoot" - ZOrder 141 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 4 - DstBlock "thresholdContactLeftFoot" - DstPort 1 - } - Line { - Name "thresholdContactRightFoot" - ZOrder 142 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 5 - DstBlock "thresholdContactRightFoot" - DstPort 1 - } - Line { - ZOrder 143 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 144 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Sum - Name "Sum" - SID "4537" - Ports [2, 1] - Position [820, 505, 840, 525] - ZOrder 709 - ShowName off - IconShape "round" - Inputs "++|" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Sum - Name "Sum1" - SID "4538" - Ports [2, 1] - Position [575, 535, 595, 555] - ZOrder 710 - ShowName off - IconShape "round" - Inputs "++|" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Scope - Name "left arm ext wrench" - SID "4539" - Ports [3] - Position [870, 24, 950, 116] - ZOrder 707 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','lArmExtWrenchData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDec" - "imateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct" - "('MinYLimReal','-21.838','MaxYLimReal','22.3485','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','22.3485','Le" - "gendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745" - "09803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294117" - "64706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823" - "529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','L" - "inePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyl" - "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" - ",struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames" - "',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.9375','MaxYLimRe" - "al','8.4375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true," - "'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'" - "ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0." - "392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;" - "1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'L" - "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0" - ".5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" - " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowCon" - "tent',true,'Placement',2),struct('MinYLimReal','0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','Max" - "YLimMag','10','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.074509803921" - "5686 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','Vis" - "ible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.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','none" - "','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDe" - "finedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3)},'DisplayPropertyDefault" - "s',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745098039" - "22],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784313725" - "49;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647" - " 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 " - "0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWid" - "th',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vi" - "sible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color'" - ",[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','L" - "ineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'Sh" - "owContent',true,'Placement',1),'DisplayLayoutDimensions',[3 1]),extmgr.Configuration('Tools','Plot Navigation',tru" - "e,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')" - "),'Version','2018a','Location',[66 106 1356 784])" - NumInputPorts "3" - Floating off - } - Block { - BlockType Scope - Name "left foot ext wrench" - SID "4540" - Ports [3] - Position [870, 267, 950, 353] - ZOrder 680 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','leftFootExtWrenchData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggin" - "gDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{st" - "ruct('MinYLimReal','-93.64546','MaxYLimReal','130.58195','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','130." - "58195','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.71" - "7647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" - "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedCh" - "annelNames',{{}},'NumLines',6,'LineNames',{{'wL_WBDT:1','wL_WBDT:2','wL_WBDT:3','wL_WBDT:4','wL_WBDT:5','wL_WBDT:6" - "'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-15.00000','MaxYLimReal','135.00000','YLabelReal','','" - "MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColo" - "r',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.06666666666" - "66667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019" - "608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6509803" - "92156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" - "ble','on')}},'UserDefinedChannelNames',{{}},'NumLines',1,'LineNames',{{'Compute current threshold/thresholdContact" - "LeftFoot'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','0.75','MaxYLimReal','3.25','YLabelReal','','Mi" - "nYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor'" - ",[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666" - "667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137254901960" - "8 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392" - "156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'" - "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth'," - "0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" - "e','on')}},'UserDefinedChannelNames',{{}},'NumLines',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',3)}," - "'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450" - "9803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647" - "05882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039215" - "69 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache'" - ",{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'" - "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth'," - "0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" - "e','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " - "0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0" - ",'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[3 1]),extmgr.Configuration('Tools" - "','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements'," - "true,'Version','2017b')),'Version','2018a','Location',[220 286 2085 1289])" - NumInputPorts "3" - Floating off - } - Block { - BlockType Scope - Name "right arm ext wrench" - SID "4541" - Ports [3] - Position [870, 150, 950, 240] - ZOrder 706 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','rArmExtWrenchData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDec" - "imateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct" - "('MinYLimReal','-5.06473','MaxYLimReal','4.14076','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','5.06473','L" - "egendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274" - "509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411" - "764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882" - "3529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','" - "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" - "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" - "s',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-1.25','MaxYLimRea" - "l','11.25','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'P" - "lotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Co" - "lorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.39" - "2156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 " - "0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5" - ",'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidt" - "h',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowConte" - "nt',true,'Placement',2),struct('MinYLimReal','0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYL" - "imMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickCo" - "lor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980392156" - "86 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','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','-','Line" - "Width',0.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')}},'UserDefi" - "nedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3)},'DisplayPropertyDefaults'" - ",struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922" - "],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549" - ";0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1" - " 1;1 0.0745098039215686 0.650980392156863],'Title','%','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','Visi" - "ble','on'),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','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'Show" - "Content',true,'Placement',1),'DisplayLayoutDimensions',[3 1]),extmgr.Configuration('Tools','Plot Navigation',true," - "'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b'))," - "'Version','2018a','Location',[66 106 1931 1137])" - NumInputPorts "3" - Floating off - } - Block { - BlockType Scope - Name "right foot ext wrench" - SID "4542" - Ports [3] - Position [870, 388, 950, 472] - ZOrder 678 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','rightFootExtWrenchData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggi" - "ngDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{s" - "truct('MinYLimReal','-90.33322','MaxYLimReal','127.13718','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','127" - ".13718','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor'" - ",[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0" - ".623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7" - "17647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidt" - "h',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" - "ible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedC" - "hannelNames',{{}},'NumLines',6,'LineNames',{{'wR_WBDT:1','wR_WBDT:2','wR_WBDT:3','wR_WBDT:4','wR_WBDT:5','wR_WBDT:" - "6'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-15.00000','MaxYLimReal','135.00000','YLabelReal',''," - "'MinYLimMag','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.0666666666" - "666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137254901" - "9608 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','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','Vis" - "ible','on')}},'UserDefinedChannelNames',{{}},'NumLines',1,'LineNames',{{'Compute current threshold/thresholdContac" - "tRightFoot'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','0.75','MaxYLimReal','3.25','YLabelReal','','" - "MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColo" - "r',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.06666666666" - "66667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019" - "608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6509803" - "92156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" - "ble','on')}},'UserDefinedChannelNames',{{}},'NumLines',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',3)" - "},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274" - "509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.41176" - "4705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450980392" - "1569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCach" - "e',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" - "ble','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[" - "0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'" - ",0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[3 1],'TimeRangeSamples','42.68'" - ",'TimeRangeFrames','42.68'),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutosc" - "ale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[76 1" - "34 1941 1165])" - NumInputPorts "3" - Floating off - } - Block { - BlockType Scope - Name "sum feet ext wrench" - SID "4543" - Ports [3] - Position [870, 503, 950, 587] - ZOrder 708 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','sumExtLegForces','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecim" - "ateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','158.1" - "4122','MaxYLimReal','214.16568','YLabelReal','','MinYLimMag','158.14122','MaxYLimMag','214.16568','LegendVisibilit" - "y','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.4" - "11764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098" - "03921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperties" - "Cache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 " - "0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineW" - "idth',0.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('Colo" - "r',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLi" - "nes',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-35.00000','MaxYLimReal','315.00" - "000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMag" - "Phase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrd" - "er',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.39215686" - "2745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745" - "098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle" - "','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Mark" - "er','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')," - "struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5" - ",'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',tr" - "ue,'Placement',2),struct('MinYLimReal','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.6" - "23529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717" - "647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1]," - "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth'" - ",0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" - "le','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedCha" - "nnelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3)},'DisplayPropertyDefaults',struc" - "t('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Col" - "orOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392" - "156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0" - ".0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowConten" - "t',true,'Placement',1),'DisplayLayoutDimensions',[3 1],'TimeRangeSamples','42.68','TimeRangeFrames','42.68'),extmg" - "r.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('" - "Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[132 304 1987 1277])" - NumInputPorts "3" - Floating off - } - Line { - Name "wR_WBDT" - ZOrder 1 - SrcBlock "wR_WBDT" - SrcPort 1 - Points [35, 0] - Branch { - ZOrder 44 - Points [0, 115] - DstBlock "Sum" - DstPort 2 - } - Branch { - ZOrder 3 - Labels [-1, 0] - DstBlock "right foot ext wrench" - DstPort 1 - } - } - Line { - Name "wL_WBDT" - ZOrder 4 - Labels [-1, 0] - SrcBlock "wL_WBDT" - SrcPort 1 - Points [65, 0] - Branch { - ZOrder 42 - DstBlock "Sum" - DstPort 1 - } - Branch { - ZOrder 6 - DstBlock "left foot ext wrench" - DstPort 1 - } - } - Line { - ZOrder 15 - SrcBlock "wR_arm" - SrcPort 1 - DstBlock "right arm ext wrench" - DstPort 1 - } - Line { - ZOrder 16 - SrcBlock "wL_arm" - SrcPort 1 - DstBlock "left arm ext wrench" - DstPort 1 - } - Line { - ZOrder 17 - SrcBlock "state" - SrcPort 1 - Points [223, 0] - Branch { - ZOrder 41 - Points [0, 185] - Branch { - ZOrder 57 - DstBlock "Compute current threshold" - DstPort 1 - } - Branch { - ZOrder 56 - Points [0, 290] - DstBlock "sum feet ext wrench" - DstPort 3 - } - } - Branch { - ZOrder 32 - Points [510, 0] - Branch { - ZOrder 40 - Points [0, 125] - Branch { - ZOrder 34 - Points [0, 115] - Branch { - ZOrder 39 - Points [0, 120] - DstBlock "right foot ext wrench" - DstPort 3 - } - Branch { - ZOrder 24 - DstBlock "left foot ext wrench" - DstPort 3 - } - } - Branch { - ZOrder 25 - DstBlock "right arm ext wrench" - DstPort 3 - } - } - Branch { - ZOrder 26 - DstBlock "left arm ext wrench" - DstPort 3 - } - } - } - Line { - ZOrder 30 - SrcBlock "Sum" - SrcPort 1 - DstBlock "sum feet ext wrench" - DstPort 1 - } - Line { - ZOrder 31 - SrcBlock "Sum1" - SrcPort 1 - DstBlock "sum feet ext wrench" - DstPort 2 - } - Line { - ZOrder 48 - SrcBlock "Compute current threshold" - SrcPort 3 - Points [115, 0] - Branch { - ZOrder 53 - DstBlock "left foot ext wrench" - DstPort 2 - } - Branch { - ZOrder 52 - DstBlock "Sum1" - DstPort 1 - } - } - Line { - ZOrder 49 - SrcBlock "Compute current threshold" - SrcPort 4 - Points [90, 0; 0, 70] - Branch { - ZOrder 55 - DstBlock "right foot ext wrench" - DstPort 2 - } - Branch { - ZOrder 54 - DstBlock "Sum1" - DstPort 2 - } - } - Line { - ZOrder 50 - SrcBlock "Compute current threshold" - SrcPort 1 - Points [89, 0; 0, -140] - DstBlock "left arm ext wrench" - DstPort 2 - } - Line { - ZOrder 51 - SrcBlock "Compute current threshold" - SrcPort 2 - Points [114, 0; 0, -65] - DstBlock "right arm ext wrench" - DstPort 2 - } - } - } - Block { - BlockType Reference - Name "holder\n1" - SID "2187" - Ports [1, 1] - Position [-5, 247, 55, 263] - ZOrder 583 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "holder\n2" - SID "2188" - Ports [1, 1] - Position [-5, 282, 55, 298] - ZOrder 584 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "inertial" - SID "2630" - Ports [0, 1] - Position [-460, 388, -410, 402] - ZOrder 654 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.IMU" - signalSize "12" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - Block { - BlockType Reference - Name "left_foot_wrench1" - SID "4547" - Ports [0, 1] - Position [-160, 0, -105, 20] - ZOrder 944 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.WRENCH_LEFT_FOOT" - signalSize "6" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on - Port { - PortNumber 1 - Name "wL_WBDT" - } - } - Block { - BlockType Reference - Name "left_foot_wrench2" - SID "4548" - Ports [0, 1] - Position [-160, -20, -105, 0] - ZOrder 950 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.WRENCH_RIGHT_ARM" - signalSize "6" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on - Port { - PortNumber 1 - Name "wR_arm" - } - } - Block { - BlockType Reference - Name "left_foot_wrench3" - SID "4549" - Ports [0, 1] - Position [-160, -40, -105, -20] - ZOrder 951 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.WRENCH_LEFT_ARM" - signalSize "6" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on - Port { - PortNumber 1 - Name "wL_arm" - } - } - Block { - BlockType Reference - Name "right_foot_wrench1" - SID "4567" - Ports [0, 1] - Position [-160, 21, -105, 39] - ZOrder 945 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.WRENCH_RIGHT_FOOT" - signalSize "6" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on - Port { - PortNumber 1 - Name "wR_WBDT" - } - } - Block { - BlockType SubSystem - Name "stateMachineStandupFCN" - SID "2220" - Ports [11, 9] - Position [90, 107, 300, 473] - ZOrder 497 - ForegroundColor "red" - LibraryVersion "1.30" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - Port { - PortNumber 2 - Name "constraintsSmooth" - PropagatedSignals "constraints" - } - System { - Name "stateMachineStandupFCN" - 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 "3771" - SIDPrevWatermark "9" - Block { - BlockType Inport - Name "wrench_rightFoot" - SID "2220::93" - Position [20, 101, 40, 119] - ZOrder 69 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "wrench_leftFoot" - SID "2220::103" - Position [20, 136, 40, 154] - ZOrder 77 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "wrench_leftHand" - SID "2220::58" - Position [20, 171, 40, 189] - ZOrder 34 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "wrench_rightHand" - SID "2220::82" - Position [20, 206, 40, 224] - ZOrder 58 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "xCoM_0" - SID "2220::92" - Position [20, 246, 40, 264] - ZOrder 68 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj_0" - SID "2220::107" - Position [20, 281, 40, 299] - ZOrder 81 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "xCoM" - SID "2220::97" - Position [20, 316, 40, 334] - ZOrder 72 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "l_sole_H_b" - SID "2220::101" - Position [20, 351, 40, 369] - ZOrder 75 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "l_upper_leg_contact_H_b" - SID "2220::108" - Position [20, 386, 40, 404] - ZOrder 82 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "t" - SID "2220::84" - Position [20, 426, 40, 444] - ZOrder 60 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "STANDUP_WITH_HUMAN" - SID "2220::3723" - Position [20, 461, 40, 479] - ZOrder 122 - Port "11" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "2220::3770" - Ports [1, 1] - Position [270, 460, 320, 500] - ZOrder 169 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "2220::3769" - Tag "Stateflow S-Function torqueBalancingStandup 13" - Ports [11, 10] - Position [180, 95, 230, 335] - ZOrder 168 - FunctionName "sf_sfun" - Parameters "Gain,Sm" - PortCounts "[11 10]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "w_H_b" - } - Port { - PortNumber 3 - Name "constraints" - } - Port { - PortNumber 4 - Name "CoM_des" - } - Port { - PortNumber 5 - Name "qj_des" - } - Port { - PortNumber 6 - Name "impedances" - } - Port { - PortNumber 7 - Name "KPCoM" - } - Port { - PortNumber 8 - Name "KDCoM" - } - Port { - PortNumber 9 - Name "currentState" - } - Port { - PortNumber 10 - Name "jointsAndCoMSmoothingTime" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "2220::3771" - Position [460, 471, 480, 489] - ZOrder 170 - } - Block { - BlockType Outport - Name "w_H_b" - SID "2220::106" - Position [460, 101, 480, 119] - ZOrder 80 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "constraints" - SID "2220::64" - Position [460, 136, 480, 154] - ZOrder 40 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "CoM_des" - SID "2220::77" - Position [460, 171, 480, 189] - ZOrder 53 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qj_des" - SID "2220::67" - Position [460, 206, 480, 224] - ZOrder 43 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "impedances" - SID "2220::94" - Position [460, 246, 480, 264] - ZOrder 70 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "KPCoM" - SID "2220::112" - Position [460, 281, 480, 299] - ZOrder 84 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "KDCoM" - SID "2220::113" - Position [460, 316, 480, 334] - ZOrder 85 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "currentState" - SID "2220::96" - Position [460, 351, 480, 369] - ZOrder 71 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "jointsAndCoMSmoothingTime" - SID "2220::111" - Position [460, 386, 480, 404] - ZOrder 83 - Port "9" - IconDisplay "Port number" - } - Line { - ZOrder 608 - SrcBlock "wrench_rightFoot" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 609 - SrcBlock "wrench_leftFoot" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 610 - SrcBlock "wrench_leftHand" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 611 - SrcBlock "wrench_rightHand" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 612 - SrcBlock "xCoM_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 613 - SrcBlock "qj_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - ZOrder 614 - SrcBlock "xCoM" - SrcPort 1 - DstBlock " SFunction " - DstPort 7 - } - Line { - ZOrder 615 - SrcBlock "l_sole_H_b" - SrcPort 1 - DstBlock " SFunction " - DstPort 8 - } - Line { - ZOrder 616 - SrcBlock "l_upper_leg_contact_H_b" - SrcPort 1 - DstBlock " SFunction " - DstPort 9 - } - Line { - ZOrder 617 - SrcBlock "t" - SrcPort 1 - DstBlock " SFunction " - DstPort 10 - } - Line { - ZOrder 618 - SrcBlock "STANDUP_WITH_HUMAN" - SrcPort 1 - DstBlock " SFunction " - DstPort 11 - } - Line { - Name "w_H_b" - ZOrder 619 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "w_H_b" - DstPort 1 - } - Line { - Name "constraints" - ZOrder 620 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 3 - DstBlock "constraints" - DstPort 1 - } - Line { - Name "CoM_des" - ZOrder 621 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 4 - DstBlock "CoM_des" - DstPort 1 - } - Line { - Name "qj_des" - ZOrder 622 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 5 - DstBlock "qj_des" - DstPort 1 - } - Line { - Name "impedances" - ZOrder 623 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 6 - DstBlock "impedances" - DstPort 1 - } - Line { - Name "KPCoM" - ZOrder 624 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 7 - DstBlock "KPCoM" - DstPort 1 - } - Line { - Name "KDCoM" - ZOrder 625 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 8 - DstBlock "KDCoM" - DstPort 1 - } - Line { - Name "currentState" - ZOrder 626 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 9 - DstBlock "currentState" - DstPort 1 - } - Line { - Name "jointsAndCoMSmoothingTime" - ZOrder 627 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 10 - DstBlock "jointsAndCoMSmoothingTime" - DstPort 1 - } - Line { - ZOrder 628 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 629 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "xCom" - SID "4226" - Ports [2, 1] - Position [-200, 309, -105, 336] - ZOrder 890 - ShowName off - RequestExecContextInheritance off - System { - Name "xCom" - Location [65, 24, 2560, 1440] - 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 "628" - 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 "qj" - 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.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.COM" - } - Block { - BlockType Selector - Name "CoM6D -> \nCoMXYZ2" - SID "4230" - Ports [1, 1] - Position [310, 241, 350, 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 "xCoM" - SID "4231" - Position [385, 253, 415, 267] - ZOrder 583 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "qj" - SrcPort 1 - DstBlock "CoM" - DstPort 2 - } - Line { - ZOrder 2 - SrcBlock "CoM6D -> \nCoMXYZ2" - SrcPort 1 - DstBlock "xCoM" - 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 "CoMDes" - SID "2234" - Position [640, 213, 670, 227] - ZOrder 478 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qjDes" - SID "2235" - Position [405, 243, 435, 257] - ZOrder 479 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "2236" - Position [635, 162, 675, 178] - ZOrder 480 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "impedances" - SID "2237" - Position [640, 282, 675, 298] - ZOrder 545 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "state" - SID "2238" - Position [410, 402, 445, 418] - ZOrder 550 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "nu_b + qjDot" - SID "2239" - Position [640, -88, 670, -72] - ZOrder 577 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qj" - SID "2240" - Position [-450, 457, -420, 473] - ZOrder 579 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "smoothingTimeCoM_Joints" - SID "3181" - Position [415, 443, 445, 457] - ZOrder 680 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "KpCoM" - SID "3316" - Position [640, 322, 675, 338] - ZOrder 689 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "KdCoM" - SID "3318" - Position [640, 360, 675, 380] - ZOrder 691 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_b" - SID "2241" - Position [640, 122, 670, 138] - ZOrder 578 - Port "11" - IconDisplay "Port number" - } - Line { - ZOrder 173 - SrcBlock "xCom" - SrcPort 1 - Points [45, 0] - Branch { - ZOrder 476 - DstBlock "stateMachineStandupFCN" - DstPort 7 - } - Branch { - ZOrder 475 - Points [0, -70] - DstBlock "holder\n1" - DstPort 1 - } - } - Line { - ZOrder 10 - SrcBlock "stateMachineStandupFCN" - SrcPort 4 - DstBlock "qjDes" - DstPort 1 - } - Line { - ZOrder 11 - SrcBlock "stateMachineStandupFCN" - SrcPort 3 - DstBlock "Mux2" - DstPort 1 - } - Line { - ZOrder 12 - SrcBlock "Mux2" - SrcPort 1 - DstBlock "CoMDes" - DstPort 1 - } - Line { - ZOrder 13 - SrcBlock "Constant1" - SrcPort 1 - DstBlock "Mux2" - DstPort 2 - } - Line { - ZOrder 14 - SrcBlock "stateMachineStandupFCN" - SrcPort 8 - Points [51, 0] - Branch { - ZOrder 491 - Points [0, -326] - Branch { - ZOrder 495 - Points [-275, 0; 0, -34] - DstBlock "Visualise external wrenches" - DstPort 5 - } - Branch { - ZOrder 494 - Points [0, -134] - DstBlock "Compute State Velocity" - DstPort 4 - } - } - Branch { - ZOrder 490 - DstBlock "state" - DstPort 1 - } - } - Line { - Name "constraintsSmooth" - ZOrder 15 - SrcBlock "stateMachineStandupFCN" - SrcPort 2 - Points [30, 0] - Branch { - ZOrder 493 - DstBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - DstPort 1 - } - Branch { - ZOrder 485 - Labels [2, 1] - Points [0, -240] - DstBlock "Compute State Velocity" - DstPort 3 - } - } - Line { - ZOrder 21 - SrcBlock "stateMachineStandupFCN" - SrcPort 1 - Points [10, 0] - Branch { - ZOrder 492 - DstBlock "Reshape1" - DstPort 1 - } - Branch { - ZOrder 486 - Points [0, -220] - DstBlock "Compute State Velocity" - DstPort 2 - } - } - Line { - ZOrder 273 - SrcBlock "Compute State Velocity" - SrcPort 1 - DstBlock "nu_b + qjDot" - DstPort 1 - } - Line { - ZOrder 53 - SrcBlock "stateMachineStandupFCN" - SrcPort 9 - DstBlock "smoothingTimeCoM_Joints" - DstPort 1 - } - Line { - ZOrder 60 - SrcBlock "stateMachineStandupFCN" - SrcPort 6 - DstBlock "Mux" - DstPort 2 - } - Line { - ZOrder 61 - SrcBlock "Demux1" - SrcPort 3 - DstBlock "KdCoM" - DstPort 1 - } - Line { - ZOrder 62 - SrcBlock "stateMachineStandupFCN" - SrcPort 7 - DstBlock "Mux" - DstPort 3 - } - Line { - ZOrder 63 - SrcBlock "stateMachineStandupFCN" - SrcPort 5 - DstBlock "Mux" - DstPort 1 - } - Line { - ZOrder 64 - SrcBlock "Mux" - SrcPort 1 - DstBlock "Minimum Jerk Trajectory Generator" - DstPort 1 - } - Line { - ZOrder 65 - SrcBlock "Minimum Jerk Trajectory Generator" - SrcPort 1 - DstBlock "Demux1" - DstPort 1 - } - Line { - ZOrder 66 - SrcBlock "Demux1" - SrcPort 1 - DstBlock "impedances" - DstPort 1 - } - Line { - ZOrder 67 - SrcBlock "Demux1" - SrcPort 2 - DstBlock "KpCoM" - DstPort 1 - } - Line { - ZOrder 32 - SrcBlock "jointAngles" - SrcPort 1 - Points [54, 0] - Branch { - ZOrder 463 - Points [0, -400] - DstBlock "Compute State Velocity" - DstPort 1 - } - Branch { - ZOrder 473 - Points [0, 40] - Branch { - ZOrder 262 - DstBlock "xCom" - DstPort 2 - } - Branch { - ZOrder 471 - Points [0, 30] - Branch { - ZOrder 479 - Points [0, 105] - DstBlock "qj" - DstPort 1 - } - Branch { - ZOrder 249 - DstBlock "Compute base to fixed link transform" - DstPort 1 - } - } - } - Branch { - ZOrder 264 - DstBlock "holder\n2" - DstPort 1 - } - } - Line { - ZOrder 239 - SrcBlock "Compute base to fixed link transform" - SrcPort 1 - Points [13, 0] - Branch { - ZOrder 478 - DstBlock "stateMachineStandupFCN" - DstPort 8 - } - Branch { - ZOrder 242 - Points [0, -45] - DstBlock "xCom" - DstPort 1 - } - } - Line { - ZOrder 240 - SrcBlock "Compute base to fixed link transform" - SrcPort 2 - DstBlock "stateMachineStandupFCN" - DstPort 9 - } - Line { - ZOrder 250 - SrcBlock "inertial" - SrcPort 1 - DstBlock "Compute base to fixed link transform" - DstPort 2 - } - Line { - ZOrder 367 - SrcBlock "Reshape1" - SrcPort 1 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 438 - SrcBlock "Standup with human" - SrcPort 1 - DstBlock "stateMachineStandupFCN" - DstPort 11 - } - Line { - Name "wR_WBDT" - ZOrder 441 - SrcBlock "right_foot_wrench1" - SrcPort 1 - Points [148, 0] - Branch { - ZOrder 460 - Points [0, 85] - DstBlock "stateMachineStandupFCN" - DstPort 1 - } - Branch { - ZOrder 459 - Labels [-1, 0] - DstBlock "Visualise external wrenches" - DstPort 4 - } - } - Line { - Name "wL_WBDT" - ZOrder 444 - SrcBlock "left_foot_wrench1" - SrcPort 1 - Points [137, 0] - Branch { - ZOrder 462 - Points [0, 140] - DstBlock "stateMachineStandupFCN" - DstPort 2 - } - Branch { - ZOrder 461 - Labels [-1, 0] - DstBlock "Visualise external wrenches" - DstPort 3 - } - } - Line { - ZOrder 445 - SrcBlock "Logical\nOperator2" - SrcPort 1 - Points [20, 0] - DstBlock "Visualise external wrenches" - DstPort enable - } - Line { - ZOrder 446 - SrcBlock "ON_GAZEBO\n5" - SrcPort 1 - DstBlock "Logical\nOperator2" - DstPort 1 - } - Line { - ZOrder 447 - SrcBlock "ON_GAZEBO\n6" - SrcPort 1 - DstBlock "Logical\nOperator2" - DstPort 2 - } - Line { - Name "wR_arm" - ZOrder 450 - SrcBlock "left_foot_wrench2" - SrcPort 1 - Points [112, 0] - Branch { - ZOrder 468 - Points [0, 230] - DstBlock "stateMachineStandupFCN" - DstPort 4 - } - Branch { - ZOrder 467 - Labels [-1, 0] - DstBlock "Visualise external wrenches" - DstPort 2 - } - } - Line { - Name "wL_arm" - ZOrder 453 - SrcBlock "left_foot_wrench3" - SrcPort 1 - Points [124, 0] - Branch { - ZOrder 466 - Points [0, 215] - DstBlock "stateMachineStandupFCN" - DstPort 3 - } - Branch { - ZOrder 465 - Labels [-1, 0] - DstBlock "Visualise external wrenches" - DstPort 1 - } - } - Line { - ZOrder 469 - SrcBlock "holder\n1" - SrcPort 1 - DstBlock "stateMachineStandupFCN" - DstPort 5 - } - Line { - ZOrder 472 - SrcBlock "holder\n2" - SrcPort 1 - DstBlock "stateMachineStandupFCN" - DstPort 6 - } - Line { - ZOrder 480 - SrcBlock "Clock1" - SrcPort 1 - DstBlock "stateMachineStandupFCN" - DstPort 10 - } - } - } - Block { - BlockType Reference - Name "Yoga" - SID "2909" - Ports [1, 1] - Position [-445, 2851, -385, 2879] - ZOrder 606 - ShowName off - LibraryVersion "1.441" - SourceBlock "simulink/Logic and Bit\nOperations/Bitwise\nOperator" - SourceType "Bitwise Operator" - SourceProductName "Simulink" - SourceProductBaseCode "SL" - MultiThreadCoSim "auto" - logicop "AND" - UseBitMask on - NumInputPorts "1" - BitMask "Sm.SM_MASK_STANDUP" - BitMaskRealWorld "Stored Integer" - } - Block { - BlockType Outport - Name "w_H_b" - SID "2269" - Position [705, 2779, 735, 2791] - ZOrder 578 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qj" - SID "2270" - Position [705, 2809, 735, 2821] - ZOrder 579 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "nu" - SID "2271" - Position [705, 2839, 735, 2851] - ZOrder 577 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "state" - SID "3201" - Position [705, 2960, 735, 2970] - ZOrder 647 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "2272" - Position [705, 2870, 735, 2880] - ZOrder 410 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "impedances" - SID "2268" - Position [705, 2690, 735, 2700] - ZOrder 517 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "gainsPCOM" - SID "3330" - Position [705, 2720, 735, 2730] - ZOrder 692 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "gainsDCOM" - SID "3331" - Position [705, 2750, 735, 2760] - ZOrder 693 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "desired_x_dx_ddx_CoM" - SID "2274" - Position [705, 2900, 735, 2910] - ZOrder 323 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qjDes" - SID "2273" - Position [705, 2930, 735, 2940] - ZOrder 418 - Port "10" - IconDisplay "Port number" - } - Line { - ZOrder 15 - SrcBlock "Constant" - SrcPort 1 - Points [31, 0] - Branch { - ZOrder 441 - DstBlock "Yoga" - DstPort 1 - } - Branch { - ZOrder 16 - Points [0, -270] - DstBlock "Coordinator" - DstPort 1 - } - } - Line { - ZOrder 20 - SrcBlock "State Machine Standup" - SrcPort 3 - DstBlock "Mux1" - DstPort 3 - } - Line { - ZOrder 21 - SrcBlock "State Machine Standup" - SrcPort 2 - DstBlock "Mux1" - DstPort 2 - } - Line { - ZOrder 22 - SrcBlock "State Machine Standup" - SrcPort 1 - DstBlock "Mux1" - DstPort 1 - } - Line { - ZOrder 23 - SrcBlock "Internal Coordinator" - SrcPort 1 - DstBlock "Mux2" - DstPort 1 - } - Line { - ZOrder 24 - SrcBlock "Internal Coordinator" - SrcPort 2 - DstBlock "Mux2" - DstPort 2 - } - Line { - ZOrder 25 - SrcBlock "Internal Coordinator" - SrcPort 3 - DstBlock "Mux2" - DstPort 3 - } - Line { - ZOrder 26 - SrcBlock "State Machine Standup" - SrcPort 4 - DstBlock "Mux1" - DstPort 4 - } - Line { - ZOrder 27 - SrcBlock "Internal Coordinator" - SrcPort 4 - DstBlock "Mux2" - DstPort 4 - } - Line { - ZOrder 28 - SrcBlock "State Machine Standup" - SrcPort 5 - DstBlock "Mux1" - DstPort 5 - } - Line { - ZOrder 29 - SrcBlock "Internal Coordinator" - SrcPort 5 - DstBlock "Mux2" - DstPort 5 - } - Line { - ZOrder 408 - SrcBlock "Internal Coordinator" - SrcPort 6 - DstBlock "Mux2" - DstPort 6 - } - Line { - ZOrder 31 - SrcBlock "Internal Coordinator" - SrcPort 7 - DstBlock "Mux2" - DstPort 7 - } - Line { - ZOrder 32 - SrcBlock "State Machine Standup" - SrcPort 6 - DstBlock "Mux1" - DstPort 6 - } - Line { - ZOrder 33 - SrcBlock "State Machine Standup" - SrcPort 7 - DstBlock "Mux1" - DstPort 7 - } - Line { - ZOrder 41 - SrcBlock "Coordinator" - SrcPort 1 - DstBlock "Compare\nTo Zero1" - DstPort 1 - } - Line { - ZOrder 42 - SrcBlock "Yoga" - SrcPort 1 - DstBlock "Compare\nTo Zero" - DstPort 1 - } - Line { - ZOrder 515 - SrcBlock "Compare\nTo Zero" - SrcPort 1 - Points [125, 0] - DstBlock "State Machine Standup" - DstPort enable - } - Line { - ZOrder 48 - SrcBlock "Compare\nTo Zero1" - SrcPort 1 - Points [125, 0] - DstBlock "Internal Coordinator" - DstPort enable - } - Line { - ZOrder 73 - SrcBlock "Internal Coordinator" - SrcPort 8 - DstBlock "Mux2" - DstPort 8 - } - Line { - ZOrder 75 - SrcBlock "State Machine Standup" - SrcPort 8 - DstBlock "Mux1" - DstPort 8 - } - Line { - ZOrder 101 - SrcBlock "State Machine Standup" - SrcPort 9 - DstBlock "Mux1" - DstPort 9 - } - Line { - ZOrder 102 - SrcBlock "State Machine Standup" - SrcPort 10 - DstBlock "Mux1" - DstPort 10 - } - Line { - ZOrder 103 - SrcBlock "Internal Coordinator" - SrcPort 9 - DstBlock "Mux2" - DstPort 9 - } - Line { - ZOrder 104 - SrcBlock "Internal Coordinator" - SrcPort 10 - DstBlock "Mux2" - DstPort 10 - } - Line { - ZOrder 117 - SrcBlock "Constant2" - SrcPort 1 - DstBlock "Multiport\nSwitch1" - DstPort 1 - } - Line { - ZOrder 555 - SrcBlock "Get Measurement" - SrcPort 1 - Points [50, 0] - Branch { - ZOrder 442 - DstBlock "Internal Coordinator" - DstPort 1 - } - Branch { - ZOrder 383 - Points [0, 290] - DstBlock "State Machine Standup" - DstPort 1 - } - } - Line { - ZOrder 384 - SrcBlock "State Machine Standup" - SrcPort 11 - DstBlock "Mux1" - DstPort 11 - } - Line { - ZOrder 385 - SrcBlock "Internal Coordinator" - SrcPort 11 - DstBlock "Mux2" - DstPort 11 - } - Line { - ZOrder 389 - SrcBlock "Mux2" - SrcPort 1 - Points [36, 0; 0, 110] - DstBlock "Multiport\nSwitch1" - DstPort 2 - } - Line { - ZOrder 395 - SrcBlock "Multiport\nSwitch1" - SrcPort 1 - DstBlock "Select State and References" - DstPort 1 - } - Line { - ZOrder 396 - SrcBlock "Select State and References" - SrcPort 1 - DstBlock "impedances" - DstPort 1 - } - Line { - ZOrder 397 - SrcBlock "Select State and References" - SrcPort 2 - DstBlock "gainsPCOM" - DstPort 1 - } - Line { - ZOrder 398 - SrcBlock "Select State and References" - SrcPort 3 - DstBlock "gainsDCOM" - DstPort 1 - } - Line { - ZOrder 399 - SrcBlock "Select State and References" - SrcPort 4 - Points [54, 0] - Branch { - ZOrder 601 - Points [0, 305] - DstBlock "Goto2" - DstPort 1 - } - Branch { - ZOrder 583 - DstBlock "w_H_b" - DstPort 1 - } - } - Line { - ZOrder 400 - SrcBlock "Select State and References" - SrcPort 5 - DstBlock "qj" - DstPort 1 - } - Line { - ZOrder 401 - SrcBlock "Select State and References" - SrcPort 6 - DstBlock "nu" - DstPort 1 - } - Line { - ZOrder 402 - SrcBlock "Select State and References" - SrcPort 7 - DstBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - DstPort 1 - } - Line { - ZOrder 403 - SrcBlock "Select State and References" - SrcPort 8 - Points [30, 0] - Branch { - ZOrder 599 - Points [0, 225] - DstBlock "Selector" - DstPort 1 - } - Branch { - ZOrder 582 - DstBlock "desired_x_dx_ddx_CoM" - DstPort 1 - } - } - Line { - ZOrder 600 - SrcBlock "Select State and References" - SrcPort 9 - Points [104, 0] - Branch { - ZOrder 502 - DstBlock "qjDes" - DstPort 1 - } - Branch { - ZOrder 491 - Points [0, 80] - DstBlock "Goto4" - DstPort 1 - } - } - Line { - ZOrder 407 - SrcBlock "Select State and References" - SrcPort 10 - Points [78, 0] - Branch { - ZOrder 501 - DstBlock "state" - DstPort 1 - } - Branch { - ZOrder 489 - Points [0, 85] - DstBlock "Goto3" - DstPort 1 - } - } - Line { - ZOrder 503 - SrcBlock "Selector" - SrcPort 1 - DstBlock "Goto7" - DstPort 1 - } - Line { - ZOrder 554 - SrcBlock "Mux1" - SrcPort 1 - Points [39, 0; 0, -145] - DstBlock "Multiport\nSwitch1" - DstPort 3 - } - } - } - Block { - BlockType Saturate - Name "Saturation" - SID "2353" - Ports [1, 1] - Position [1680, 612, 1720, 648] - ZOrder 545 - ShowName off - InputPortMap "u0" - UpperLimit "Sat.torque" - LowerLimit "-Sat.torque" - } - Block { - BlockType Reference - Name "Set References" - SID "2354" - Ports [1] - Position [1760, 617, 1820, 643] - ZOrder 549 - BackgroundColor "[0.827500, 0.576500, 0.603900]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyActuators/Set References" - SourceType "Set References" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - controlType "Torque" - refSpeed "50" - refAcceleration "1000000*(pi/180)" - } - Block { - BlockType SubSystem - Name "controller_QP" - SID "2355" - Ports [23, 1] - Position [1430, 355, 1640, 905] - ZOrder 542 - BackgroundColor "lightBlue" - RequestExecContextInheritance off - System { - Name "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 "110" - Block { - BlockType Inport - Name "M" - SID "2360" - Position [1290, 363, 1320, 377] - ZOrder 326 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "biasTorques" - SID "2361" - Position [1290, 398, 1320, 412] - ZOrder 327 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "L" - SID "3219" - Position [1290, 433, 1320, 447] - ZOrder 639 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_l_contact" - SID "4384" - Position [1290, 503, 1320, 517] - ZOrder 837 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_r_contact" - SID "4391" - Position [1290, 538, 1320, 552] - ZOrder 844 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "JL" - SID "2357" - Position [1290, 573, 1320, 587] - ZOrder 550 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "JR" - SID "4392" - Position [1290, 608, 1320, 622] - ZOrder 845 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "J_CoM" - SID "4390" - Position [1290, 748, 1320, 762] - ZOrder 843 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "xCoM" - SID "4383" - Position [1290, 713, 1320, 727] - ZOrder 836 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "dJL_nu" - SID "4386" - Position [1290, 643, 1320, 657] - ZOrder 839 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "dJR_nu" - SID "4388" - Position [1290, 678, 1320, 692] - ZOrder 841 - Port "11" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_l_hand" - SID "4888" - Position [1290, 923, 1320, 937] - ZOrder 868 - Port "12" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_r_hand" - SID "4889" - Position [1290, 958, 1320, 972] - ZOrder 869 - Port "13" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "legsInContact" - SID "4892" - Position [1160, 208, 1190, 222] - ZOrder 870 - Port "14" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "2358" - Position [950, 258, 980, 272] - ZOrder 323 - Port "15" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "nu" - SID "2359" - Position [1290, 328, 1320, 342] - ZOrder 325 - Port "16" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4387" - Position [950, 488, 980, 502] - ZOrder 840 - Port "17" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "2363" - Position [950, 118, 980, 132] - ZOrder 386 - Port "18" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "impedances" - SID "2356" - Position [1290, 888, 1320, 902] - ZOrder 336 - Port "19" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gainsPCOM" - SID "3328" - Position [1290, 818, 1320, 832] - ZOrder 644 - Port "20" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gainsDCOM" - SID "3329" - Position [1290, 853, 1320, 867] - ZOrder 645 - Port "21" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "desired_x_dx_ddx_CoM" - SID "2365" - Position [1290, 783, 1320, 797] - ZOrder 335 - Port "22" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qjDes" - SID "2364" - Position [950, 293, 980, 307] - ZOrder 324 - Port "23" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "\n\n 5" - SID "2366" - Position [1230, 153, 1380, 167] - ZOrder 342 - ShowName off - Value "ROBOT_DOF_FOR_SIMULINK" - } - Block { - BlockType SubSystem - Name "Balancing Controller\n" - SID "2407" - Ports [29, 17] - Position [1435, 101, 1680, 1129] - ZOrder 278 - LibraryVersion "1.32" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - Port { - PortNumber 13 - Name "CoM_Error" - PropagatedSignals "errorCoM" - } - Port { - PortNumber 14 - Name "fNoQP" - PropagatedSignals "f_noQP" - } - System { - Name "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 "3779" - Block { - BlockType Inport - Name "constraints" - SID "2407::28" - Position [20, 101, 40, 119] - ZOrder 14 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ROBOT_DOF_FOR_SIMULINK" - SID "2407::29" - Position [20, 136, 40, 154] - ZOrder 15 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ConstraintsMatrix" - SID "2407::808" - Position [20, 171, 40, 189] - ZOrder 56 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "bVectorConstraints" - SID "2407::809" - Position [20, 206, 40, 224] - ZOrder 57 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "2407::1" - Position [20, 246, 40, 264] - ZOrder -1 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qjDes" - SID "2407::21" - Position [20, 281, 40, 299] - ZOrder 7 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "nu" - SID "2407::22" - Position [20, 316, 40, 334] - ZOrder 8 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "M" - SID "2407::23" - Position [20, 351, 40, 369] - ZOrder 9 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "h" - SID "2407::24" - Position [20, 386, 40, 404] - ZOrder 10 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "L" - SID "2407::26" - Position [20, 426, 40, 444] - ZOrder 12 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "intLw" - SID "2407::841" - Position [20, 461, 40, 479] - ZOrder 84 - Port "11" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_l_contact" - SID "2407::800" - Position [20, 496, 40, 514] - ZOrder 50 - Port "12" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_r_contact" - SID "2407::27" - Position [20, 531, 40, 549] - ZOrder 13 - Port "13" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "JL" - SID "2407::790" - Position [20, 566, 40, 584] - ZOrder 41 - Port "14" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "JR" - SID "2407::784" - Position [20, 606, 40, 624] - ZOrder 36 - Port "15" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "dJL_nu" - SID "2407::785" - Position [20, 641, 40, 659] - ZOrder 37 - Port "16" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "dJR_nu" - SID "2407::798" - Position [20, 676, 40, 694] - ZOrder 48 - Port "17" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "xCoM" - SID "2407::30" - Position [20, 711, 40, 729] - ZOrder 16 - Port "18" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "J_CoM" - SID "2407::31" - Position [20, 746, 40, 764] - ZOrder 17 - Port "19" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "desired_x_dx_ddx_CoM" - SID "2407::32" - Position [20, 786, 40, 804] - ZOrder 18 - Port "20" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gainsPCOM" - SID "2407::845" - Position [20, 821, 40, 839] - ZOrder 88 - Port "21" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gainsDCOM" - SID "2407::846" - Position [20, 856, 40, 874] - ZOrder 89 - Port "22" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "impedances" - SID "2407::34" - Position [20, 891, 40, 909] - ZOrder 20 - Port "23" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_lArm" - SID "2407::3723" - Position [20, 926, 40, 944] - ZOrder 126 - Port "24" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_rArm" - SID "2407::3724" - Position [20, 966, 40, 984] - ZOrder 127 - Port "25" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "LArmWrench" - SID "2407::3725" - Position [20, 1001, 40, 1019] - ZOrder 128 - Port "26" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "RArmWrench" - SID "2407::3726" - Position [20, 1036, 40, 1054] - ZOrder 129 - Port "27" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "STANDUP_WITH_HUMAN" - SID "2407::3727" - Position [20, 1071, 40, 1089] - ZOrder 130 - Port "28" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "2407::3731" - Position [20, 1106, 40, 1124] - ZOrder 134 - Port "29" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "2407::3778" - Ports [1, 1] - Position [270, 750, 320, 790] - ZOrder 181 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "2407::3777" - Tag "Stateflow S-Function torqueBalancingStandup 17" - Ports [29, 18] - Position [180, 50, 230, 650] - ZOrder 180 - FunctionName "sf_sfun" - Parameters "Gain,Reg" - PortCounts "[29 18]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "tauModel" - } - Port { - PortNumber 3 - Name "Sigma" - } - Port { - PortNumber 4 - Name "NA" - } - Port { - PortNumber 5 - Name "f_LDot" - } - Port { - PortNumber 6 - Name "HessianMatrixQP1Foot" - } - Port { - PortNumber 7 - Name "gradientQP1Foot" - } - Port { - PortNumber 8 - Name "ConstraintsMatrixQP1Foot" - } - Port { - PortNumber 9 - Name "bVectorConstraintsQp1Foot" - } - Port { - PortNumber 10 - Name "HessianMatrixQP2FeetOrLegs" - } - Port { - PortNumber 11 - Name "gradientQP2FeetOrLegs" - } - Port { - PortNumber 12 - Name "ConstraintsMatrixQP2FeetOrLegs" - } - Port { - PortNumber 13 - Name "bVectorConstraintsQp2FeetOrLegs" - } - Port { - PortNumber 14 - Name "errorCoM" - } - Port { - PortNumber 15 - Name "f_noQP" - } - Port { - PortNumber 16 - Name "correctionFromSupportForce" - } - Port { - PortNumber 17 - Name "L_error" - } - Port { - PortNumber 18 - Name "V" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "2407::3779" - Position [460, 761, 480, 779] - ZOrder 182 - } - Block { - BlockType Outport - Name "tauModel" - SID "2407::815" - Position [460, 101, 480, 119] - ZOrder 63 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "Sigma" - SID "2407::816" - Position [460, 136, 480, 154] - ZOrder 64 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "NA" - SID "2407::824" - Position [460, 171, 480, 189] - ZOrder 72 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "f_LDot" - SID "2407::822" - Position [460, 206, 480, 224] - ZOrder 70 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "HessianMatrixQP1Foot" - SID "2407::5" - Position [460, 246, 480, 264] - ZOrder -5 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "gradientQP1Foot" - SID "2407::810" - Position [460, 281, 480, 299] - ZOrder 58 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "ConstraintsMatrixQP1Foot" - SID "2407::827" - Position [460, 316, 480, 334] - ZOrder 75 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "bVectorConstraintsQp1Foot" - SID "2407::828" - Position [460, 351, 480, 369] - ZOrder 76 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "HessianMatrixQP2FeetOrLegs" - SID "2407::811" - Position [460, 386, 480, 404] - ZOrder 59 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "gradientQP2FeetOrLegs" - SID "2407::812" - Position [460, 426, 480, 444] - ZOrder 60 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "ConstraintsMatrixQP2FeetOrLegs" - SID "2407::820" - Position [460, 461, 480, 479] - ZOrder 68 - Port "11" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "bVectorConstraintsQp2FeetOrLegs" - SID "2407::821" - Position [460, 496, 480, 514] - ZOrder 69 - Port "12" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "errorCoM" - SID "2407::806" - Position [460, 531, 480, 549] - ZOrder 54 - Port "13" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "f_noQP" - SID "2407::807" - Position [460, 566, 480, 584] - ZOrder 55 - Port "14" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "correctionFromSupportForce" - SID "2407::3728" - Position [460, 606, 480, 624] - ZOrder 131 - Port "15" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "L_error" - SID "2407::3729" - Position [460, 641, 480, 659] - ZOrder 132 - Port "16" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "V" - SID "2407::3730" - Position [460, 676, 480, 694] - ZOrder 133 - Port "17" - IconDisplay "Port number" - } - Line { - ZOrder 1644 - SrcBlock "constraints" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 1645 - SrcBlock "ROBOT_DOF_FOR_SIMULINK" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 1646 - SrcBlock "ConstraintsMatrix" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 1647 - SrcBlock "bVectorConstraints" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 1648 - SrcBlock "qj" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 1649 - SrcBlock "qjDes" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - ZOrder 1650 - SrcBlock "nu" - SrcPort 1 - DstBlock " SFunction " - DstPort 7 - } - Line { - ZOrder 1651 - SrcBlock "M" - SrcPort 1 - DstBlock " SFunction " - DstPort 8 - } - Line { - ZOrder 1652 - SrcBlock "h" - SrcPort 1 - DstBlock " SFunction " - DstPort 9 - } - Line { - ZOrder 1653 - SrcBlock "L" - SrcPort 1 - DstBlock " SFunction " - DstPort 10 - } - Line { - ZOrder 1654 - SrcBlock "intLw" - SrcPort 1 - DstBlock " SFunction " - DstPort 11 - } - Line { - ZOrder 1655 - SrcBlock "w_H_l_contact" - SrcPort 1 - DstBlock " SFunction " - DstPort 12 - } - Line { - ZOrder 1656 - SrcBlock "w_H_r_contact" - SrcPort 1 - DstBlock " SFunction " - DstPort 13 - } - Line { - ZOrder 1657 - SrcBlock "JL" - SrcPort 1 - DstBlock " SFunction " - DstPort 14 - } - Line { - ZOrder 1658 - SrcBlock "JR" - SrcPort 1 - DstBlock " SFunction " - DstPort 15 - } - Line { - ZOrder 1659 - SrcBlock "dJL_nu" - SrcPort 1 - DstBlock " SFunction " - DstPort 16 - } - Line { - ZOrder 1660 - SrcBlock "dJR_nu" - SrcPort 1 - DstBlock " SFunction " - DstPort 17 - } - Line { - ZOrder 1661 - SrcBlock "xCoM" - SrcPort 1 - DstBlock " SFunction " - DstPort 18 - } - Line { - ZOrder 1662 - SrcBlock "J_CoM" - SrcPort 1 - DstBlock " SFunction " - DstPort 19 - } - Line { - ZOrder 1663 - SrcBlock "desired_x_dx_ddx_CoM" - SrcPort 1 - DstBlock " SFunction " - DstPort 20 - } - Line { - ZOrder 1664 - SrcBlock "gainsPCOM" - SrcPort 1 - DstBlock " SFunction " - DstPort 21 - } - Line { - ZOrder 1665 - SrcBlock "gainsDCOM" - SrcPort 1 - DstBlock " SFunction " - DstPort 22 - } - Line { - ZOrder 1666 - SrcBlock "impedances" - SrcPort 1 - DstBlock " SFunction " - DstPort 23 - } - Line { - ZOrder 1667 - SrcBlock "w_H_lArm" - SrcPort 1 - DstBlock " SFunction " - DstPort 24 - } - Line { - ZOrder 1668 - SrcBlock "w_H_rArm" - SrcPort 1 - DstBlock " SFunction " - DstPort 25 - } - Line { - ZOrder 1669 - SrcBlock "LArmWrench" - SrcPort 1 - DstBlock " SFunction " - DstPort 26 - } - Line { - ZOrder 1670 - SrcBlock "RArmWrench" - SrcPort 1 - DstBlock " SFunction " - DstPort 27 - } - Line { - ZOrder 1671 - SrcBlock "STANDUP_WITH_HUMAN" - SrcPort 1 - DstBlock " SFunction " - DstPort 28 - } - Line { - ZOrder 1672 - SrcBlock "state" - SrcPort 1 - DstBlock " SFunction " - DstPort 29 - } - Line { - Name "tauModel" - ZOrder 1673 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "tauModel" - DstPort 1 - } - Line { - Name "Sigma" - ZOrder 1674 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 3 - DstBlock "Sigma" - DstPort 1 - } - Line { - Name "NA" - ZOrder 1675 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 4 - DstBlock "NA" - DstPort 1 - } - Line { - Name "f_LDot" - ZOrder 1676 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 5 - DstBlock "f_LDot" - DstPort 1 - } - Line { - Name "HessianMatrixQP1Foot" - ZOrder 1677 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 6 - DstBlock "HessianMatrixQP1Foot" - DstPort 1 - } - Line { - Name "gradientQP1Foot" - ZOrder 1678 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 7 - DstBlock "gradientQP1Foot" - DstPort 1 - } - Line { - Name "ConstraintsMatrixQP1Foot" - ZOrder 1679 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 8 - DstBlock "ConstraintsMatrixQP1Foot" - DstPort 1 - } - Line { - Name "bVectorConstraintsQp1Foot" - ZOrder 1680 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 9 - DstBlock "bVectorConstraintsQp1Foot" - DstPort 1 - } - Line { - Name "HessianMatrixQP2FeetOrLegs" - ZOrder 1681 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 10 - DstBlock "HessianMatrixQP2FeetOrLegs" - DstPort 1 - } - Line { - Name "gradientQP2FeetOrLegs" - ZOrder 1682 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 11 - DstBlock "gradientQP2FeetOrLegs" - DstPort 1 - } - Line { - Name "ConstraintsMatrixQP2FeetOrLegs" - ZOrder 1683 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 12 - DstBlock "ConstraintsMatrixQP2FeetOrLegs" - DstPort 1 - } - Line { - Name "bVectorConstraintsQp2FeetOrLegs" - ZOrder 1684 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 13 - DstBlock "bVectorConstraintsQp2FeetOrLegs" - DstPort 1 - } - Line { - Name "errorCoM" - ZOrder 1685 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 14 - DstBlock "errorCoM" - DstPort 1 - } - Line { - Name "f_noQP" - ZOrder 1686 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 15 - DstBlock "f_noQP" - DstPort 1 - } - Line { - Name "correctionFromSupportForce" - ZOrder 1687 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 16 - DstBlock "correctionFromSupportForce" - DstPort 1 - } - Line { - Name "L_error" - ZOrder 1688 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 17 - DstBlock "L_error" - DstPort 1 - } - Line { - Name "V" - ZOrder 1689 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 18 - DstBlock "V" - DstPort 1 - } - Line { - ZOrder 1690 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 1691 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Compute angular momentum integral" - SID "3714" - Ports [4, 1] - Position [1130, 444, 1225, 501] - ZOrder 835 - RequestExecContextInheritance off - System { - Name "Compute angular momentum integral" - 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 "162" - Block { - BlockType Inport - Name "qjDes" - SID "3715" - Position [55, 23, 85, 37] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "3716" - Position [415, 58, 445, 72] - ZOrder 1 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "constraints" - SID "3717" - Position [715, 143, 745, 157] - ZOrder 80 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4028" - Position [55, -192, 85, -178] - ZOrder 732 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "Centroidal Momentum" - SID "3718" - Ports [4, 1] - Position [1135, -184, 1360, 254] - ZOrder 73 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" - SourceType "Centroidal Momentum" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "Compute base to fixed link transform" - SID "4926" - Ports [2, 2] - Position [220, -96, 340, -54] - ZOrder 905 - 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 "557" - Block { - BlockType Inport - Name "qj" - SID "4927" - Position [50, 38, 80, 52] - ZOrder 902 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4928" - Position [230, 183, 260, 197] - ZOrder 903 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant7" - SID "4929" - Position [100, 22, 130, 38] - ZOrder 895 - ShowName off - Value "eye(4)" - } - Block { - BlockType SubSystem - Name "LFoot to base link transform " - SID "4930" - Ports [4, 1] - Position [435, 15, 580, 55] - ZOrder 893 - RequestExecContextInheritance off - System { - Name "LFoot to base 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 "248" - Block { - BlockType Inport - Name "qj" - SID "4931" - Position [470, 227, 495, 243] - ZOrder 736 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4932" - Position [1000, 212, 1030, 228] - ZOrder 741 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_link" - SID "4933" - Position [655, 132, 680, 148] - ZOrder 744 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_base_fixed" - SID "4934" - Position [470, 32, 495, 48] - ZOrder 745 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "Fixed base to imu transform" - SID "4935" - Ports [2, 1] - Position [575, 25, 740, 85] - ZOrder 729 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.IMU" - } - Block { - BlockType Reference - Name "Fixed base to root link transform" - SID "4936" - Ports [2, 1] - Position [575, 190, 740, 250] - ZOrder 737 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.BASE" - } - Block { - BlockType Product - Name "Inv\\*" - SID "4937" - Ports [2, 1] - Position [825, 40, 895, 95] - ZOrder 731 - 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\\* " - SID "4938" - Ports [2, 1] - Position [825, 184, 895, 231] - ZOrder 738 - 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 "4939" - Ports [0, 1] - Position [980, 308, 1055, 332] - ZOrder 742 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.NECK_POS" - signalSize "6" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - Block { - BlockType Switch - Name "Switch6" - SID "4940" - Position [1760, 151, 1840, 419] - ZOrder 734 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE" - SID "4941" - Position [1570, 275, 1725, 295] - ZOrder 735 - ShowName off - Value "Config.USE_IMU4EST_BASE" - } - Block { - BlockType SubSystem - Name "fromImuToHomogeousTransformFCN" - SID "4942" - Ports [6, 1] - Position [1305, 47, 1540, 343] - ZOrder 733 - ShowName off - LibraryVersion "1.30" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "fromImuToHomogeousTransformFCN" - 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 "3785" - SIDPrevWatermark "9" - Block { - BlockType Inport - Name "imu_H_link" - SID "4942::112" - Position [20, 101, 40, 119] - ZOrder 82 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "imu_H_link_0" - SID "4942::113" - Position [20, 136, 40, 154] - ZOrder 83 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "link_H_base" - SID "4942::111" - Position [20, 171, 40, 189] - ZOrder 81 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial_0" - SID "4942::115" - Position [20, 206, 40, 224] - ZOrder 85 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4942::93" - Position [20, 246, 40, 264] - ZOrder 69 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "neck_pos" - SID "4942::118" - Position [20, 281, 40, 299] - ZOrder 86 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4942::3784" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 184 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4942::3783" - Tag "Stateflow S-Function torqueBalancingStandup 1" - Ports [6, 2] - Position [180, 102, 230, 243] - ZOrder 183 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[6 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "w_H_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4942::3785" - Position [460, 241, 480, 259] - ZOrder 185 - } - Block { - BlockType Outport - Name "w_H_b" - SID "4942::106" - Position [460, 101, 480, 119] - ZOrder 80 - IconDisplay "Port number" - } - Line { - ZOrder 145 - SrcBlock "imu_H_link" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 146 - SrcBlock "imu_H_link_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 147 - SrcBlock "link_H_base" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 148 - SrcBlock "inertial_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 149 - SrcBlock "inertial" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 150 - SrcBlock "neck_pos" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - Name "w_H_b" - ZOrder 151 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 152 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 153 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "holder\n1" - SID "4943" - Ports [1, 1] - Position [1175, 113, 1225, 127] - ZOrder 739 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "holder\n2" - SID "4944" - Ports [1, 1] - Position [1175, 213, 1225, 227] - ZOrder 740 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "neck transform" - SID "4945" - Ports [1, 1] - Position [1135, 305, 1260, 335] - ZOrder 743 - RequestExecContextInheritance off - System { - Name "neck transform" - Location [65, 24, 2560, 1440] - 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 "562" - Block { - BlockType Inport - Name "neck port" - SID "4946" - Position [45, 18, 75, 32] - ZOrder 725 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant" - SID "4947" - Position [95, 85, 170, 105] - ZOrder 720 - ShowName off - Value "zeros(3,1)" - } - Block { - BlockType Gain - Name "Gain" - SID "4948" - 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 "4949" - Ports [1, 1] - Position [190, 13, 230, 37] - ZOrder 723 - ShowName off - InputPortWidth "6" - IndexOptions "Index vector (dialog)" - Indices "[1:3]" - OutputSizes "1" - Port { - PortNumber 1 - Name "neck pos" - } - } - Block { - BlockType Switch - Name "Switch" - SID "4950" - Position [300, 7, 375, 113] - ZOrder 722 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE1" - SID "4951" - Position [60, 50, 210, 70] - ZOrder 719 - ShowName off - Value "Config.CORRECT_NECK_IMU" - } - Block { - BlockType Outport - Name "neck position" - SID "4952" - Position [425, 53, 455, 67] - ZOrder 726 - IconDisplay "Port number" - } - 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 "4953" - Position [1875, 278, 1905, 292] - ZOrder 732 - IconDisplay "Port number" - } - 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\\* " - SrcPort 1 - Points [65, 0] - Branch { - ZOrder 3 - Points [0, 165] - DstBlock "Switch6" - DstPort 3 - } - Branch { - ZOrder 4 - Points [0, -40] - DstBlock "fromImuToHomogeousTransformFCN" - 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 "fromImuToHomogeousTransformFCN" - DstPort 1 - } - Branch { - ZOrder 8 - Points [0, 50] - DstBlock "holder\n1" - DstPort 1 - } - } - Line { - ZOrder 9 - SrcBlock "holder\n2" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 4 - } - Line { - ZOrder 10 - SrcBlock "Fixed base to imu transform" - SrcPort 1 - DstBlock "Inv\\*" - DstPort 1 - } - Line { - ZOrder 11 - SrcBlock "holder\n1" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 2 - } - Line { - ZOrder 12 - SrcBlock "qj" - 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 "fromImuToHomogeousTransformFCN" - SrcPort 1 - DstBlock "Switch6" - DstPort 1 - } - Line { - ZOrder 16 - SrcBlock "Fixed base to root link transform" - SrcPort 1 - DstBlock "Inv\\* " - 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 "fromImuToHomogeousTransformFCN" - DstPort 5 - } - } - Line { - ZOrder 20 - SrcBlock "Neck Position" - SrcPort 1 - DstBlock "neck transform" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "neck transform" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 6 - } - Line { - ZOrder 22 - SrcBlock "w_H_link" - SrcPort 1 - Points [98, 0] - Branch { - ZOrder 23 - Points [0, 55] - DstBlock "Inv\\* " - 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 - } - } - } - } - Block { - BlockType Reference - Name "LFoot to world transform (fixed base)" - SID "4954" - Ports [2, 1] - Position [165, 22, 330, 53] - ZOrder 894 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType SubSystem - Name "LLeg to base link transform" - SID "4955" - Ports [4, 1] - Position [435, 100, 580, 140] - ZOrder 896 - RequestExecContextInheritance off - System { - Name "LLeg to base 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 "248" - Block { - BlockType Inport - Name "qj" - SID "4956" - Position [470, 227, 495, 243] - ZOrder 736 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4957" - Position [1000, 212, 1030, 228] - ZOrder 741 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_link" - SID "4958" - Position [655, 132, 680, 148] - ZOrder 744 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_base_fixed" - SID "4959" - Position [470, 32, 495, 48] - ZOrder 745 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "Fixed base to imu transform" - SID "4960" - Ports [2, 1] - Position [575, 25, 740, 85] - ZOrder 729 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.IMU" - } - Block { - BlockType Reference - Name "Fixed base to root link transform" - SID "4961" - Ports [2, 1] - Position [575, 190, 740, 250] - ZOrder 737 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.BASE" - } - Block { - BlockType Product - Name "Inv\\*" - SID "4962" - Ports [2, 1] - Position [825, 40, 895, 95] - ZOrder 731 - 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\\* " - SID "4963" - Ports [2, 1] - Position [825, 184, 895, 231] - ZOrder 738 - 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 "4964" - Ports [0, 1] - Position [980, 308, 1055, 332] - ZOrder 742 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.NECK_POS" - signalSize "6" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - Block { - BlockType Switch - Name "Switch6" - SID "4965" - Position [1760, 151, 1840, 419] - ZOrder 734 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE" - SID "4966" - Position [1570, 275, 1725, 295] - ZOrder 735 - ShowName off - Value "Config.USE_IMU4EST_BASE" - } - Block { - BlockType SubSystem - Name "fromImuToHomogeousTransformFCN" - SID "4967" - Ports [6, 1] - Position [1305, 47, 1540, 343] - ZOrder 733 - ShowName off - LibraryVersion "1.30" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "fromImuToHomogeousTransformFCN" - 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 "3785" - SIDPrevWatermark "9" - Block { - BlockType Inport - Name "imu_H_link" - SID "4967::112" - Position [20, 101, 40, 119] - ZOrder 82 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "imu_H_link_0" - SID "4967::113" - Position [20, 136, 40, 154] - ZOrder 83 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "link_H_base" - SID "4967::111" - Position [20, 171, 40, 189] - ZOrder 81 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial_0" - SID "4967::115" - Position [20, 206, 40, 224] - ZOrder 85 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4967::93" - Position [20, 246, 40, 264] - ZOrder 69 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "neck_pos" - SID "4967::118" - Position [20, 281, 40, 299] - ZOrder 86 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4967::3784" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 184 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4967::3783" - Tag "Stateflow S-Function torqueBalancingStandup 4" - Ports [6, 2] - Position [180, 102, 230, 243] - ZOrder 183 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[6 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "w_H_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4967::3785" - Position [460, 241, 480, 259] - ZOrder 185 - } - Block { - BlockType Outport - Name "w_H_b" - SID "4967::106" - Position [460, 101, 480, 119] - ZOrder 80 - IconDisplay "Port number" - } - Line { - ZOrder 145 - SrcBlock "imu_H_link" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 146 - SrcBlock "imu_H_link_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 147 - SrcBlock "link_H_base" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 148 - SrcBlock "inertial_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 149 - SrcBlock "inertial" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 150 - SrcBlock "neck_pos" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - Name "w_H_b" - ZOrder 151 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 152 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 153 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "holder\n1" - SID "4968" - Ports [1, 1] - Position [1175, 113, 1225, 127] - ZOrder 739 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "holder\n2" - SID "4969" - Ports [1, 1] - Position [1175, 213, 1225, 227] - ZOrder 740 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "neck transform" - SID "4970" - Ports [1, 1] - Position [1135, 305, 1260, 335] - ZOrder 743 - RequestExecContextInheritance off - System { - Name "neck transform" - Location [65, 24, 2560, 1440] - 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 "562" - Block { - BlockType Inport - Name "neck port" - SID "4971" - Position [45, 18, 75, 32] - ZOrder 725 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant" - SID "4972" - Position [95, 85, 170, 105] - ZOrder 720 - ShowName off - Value "zeros(3,1)" - } - Block { - BlockType Gain - Name "Gain" - SID "4973" - 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 "4974" - Ports [1, 1] - Position [190, 13, 230, 37] - ZOrder 723 - ShowName off - InputPortWidth "6" - IndexOptions "Index vector (dialog)" - Indices "[1:3]" - OutputSizes "1" - Port { - PortNumber 1 - Name "neck pos" - } - } - Block { - BlockType Switch - Name "Switch" - SID "4975" - Position [300, 7, 375, 113] - ZOrder 722 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE1" - SID "4976" - Position [60, 50, 210, 70] - ZOrder 719 - ShowName off - Value "Config.CORRECT_NECK_IMU" - } - Block { - BlockType Outport - Name "neck position" - SID "4977" - Position [425, 53, 455, 67] - ZOrder 726 - IconDisplay "Port number" - } - 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 "4978" - Position [1875, 278, 1905, 292] - ZOrder 732 - IconDisplay "Port number" - } - 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\\* " - SrcPort 1 - Points [65, 0] - Branch { - ZOrder 3 - Points [0, 165] - DstBlock "Switch6" - DstPort 3 - } - Branch { - ZOrder 4 - Points [0, -40] - DstBlock "fromImuToHomogeousTransformFCN" - 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 "fromImuToHomogeousTransformFCN" - DstPort 1 - } - Branch { - ZOrder 8 - Points [0, 50] - DstBlock "holder\n1" - DstPort 1 - } - } - Line { - ZOrder 9 - SrcBlock "holder\n2" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 4 - } - Line { - ZOrder 10 - SrcBlock "Fixed base to imu transform" - SrcPort 1 - DstBlock "Inv\\*" - DstPort 1 - } - Line { - ZOrder 11 - SrcBlock "holder\n1" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 2 - } - Line { - ZOrder 12 - SrcBlock "qj" - 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 "fromImuToHomogeousTransformFCN" - SrcPort 1 - DstBlock "Switch6" - DstPort 1 - } - Line { - ZOrder 16 - SrcBlock "Fixed base to root link transform" - SrcPort 1 - DstBlock "Inv\\* " - 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 "fromImuToHomogeousTransformFCN" - DstPort 5 - } - } - Line { - ZOrder 20 - SrcBlock "Neck Position" - SrcPort 1 - DstBlock "neck transform" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "neck transform" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 6 - } - Line { - ZOrder 22 - SrcBlock "w_H_link" - SrcPort 1 - Points [98, 0] - Branch { - ZOrder 23 - Points [0, 55] - DstBlock "Inv\\* " - 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 - } - } - } - } - Block { - BlockType Reference - Name "LLeg to world transform (fixed base)" - SID "4979" - Ports [2, 1] - Position [165, 107, 330, 138] - ZOrder 899 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_LEG" - } - Block { - BlockType Outport - Name "fixed_LLeg_H_b" - SID "4981" - Position [650, 113, 680, 127] - ZOrder 912 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "fixed_LFoot_H_b" - SID "4980" - Position [650, 28, 680, 42] - ZOrder 910 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "Constant7" - SrcPort 1 - Points [10, 0] - Branch { - ZOrder 2 - Points [0, -22; 255, 0; 0, 42] - Branch { - ZOrder 3 - Points [0, 85] - DstBlock "LLeg to base link transform" - DstPort 4 - } - Branch { - ZOrder 4 - DstBlock "LFoot to base link transform " - DstPort 4 - } - } - Branch { - ZOrder 5 - Points [0, 85] - DstBlock "LLeg to world transform (fixed base)" - DstPort 1 - } - Branch { - ZOrder 6 - DstBlock "LFoot to world transform (fixed base)" - DstPort 1 - } - } - Line { - ZOrder 7 - SrcBlock "LLeg to base link transform" - SrcPort 1 - DstBlock "fixed_LLeg_H_b" - DstPort 1 - } - Line { - ZOrder 8 - SrcBlock "LFoot to base link transform " - SrcPort 1 - DstBlock "fixed_LFoot_H_b" - DstPort 1 - } - Line { - ZOrder 9 - SrcBlock "LFoot to world transform (fixed base)" - SrcPort 1 - DstBlock "LFoot to base link transform " - DstPort 3 - } - Line { - ZOrder 10 - SrcBlock "LLeg to world transform (fixed base)" - SrcPort 1 - DstBlock "LLeg to base link transform" - DstPort 3 - } - Line { - ZOrder 11 - SrcBlock "qj" - SrcPort 1 - Points [51, 0] - Branch { - ZOrder 12 - Points [0, 85] - Branch { - ZOrder 13 - Points [0, 37; 274, 0; 0, -62] - Branch { - ZOrder 14 - Points [0, -85] - DstBlock "LFoot to base link transform " - DstPort 1 - } - Branch { - ZOrder 15 - DstBlock "LLeg to base link transform" - DstPort 1 - } - } - Branch { - ZOrder 16 - DstBlock "LLeg to world transform (fixed base)" - DstPort 2 - } - } - Branch { - ZOrder 17 - DstBlock "LFoot to world transform (fixed base)" - DstPort 2 - } - } - Line { - ZOrder 18 - SrcBlock "inertial" - SrcPort 1 - Points [119, 0; 0, -75] - Branch { - ZOrder 19 - Points [0, -85] - DstBlock "LFoot to base link transform " - DstPort 2 - } - Branch { - ZOrder 20 - DstBlock "LLeg to base link transform" - DstPort 2 - } - } - } - } - Block { - BlockType Constant - Name "Constant" - SID "4919" - Position [20, -218, 125, -202] - ZOrder 903 - ShowName off - Value "Config.iCubStandUp" - } - Block { - BlockType SubSystem - Name "Fixed Links Jacobians" - SID "4911" - Ports [3, 2] - Position [645, 50, 815, 130] - ZOrder 902 - NamePlacement "alternate" - RequestExecContextInheritance off - System { - Name "Fixed Links 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 "447" - Block { - BlockType Inport - Name "legsInContact " - SID "4912" - Position [70, 193, 100, 207] - ZOrder 763 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "4913" - Position [70, 263, 100, 277] - ZOrder 766 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_b" - SID "4914" - Position [70, 363, 100, 377] - ZOrder 767 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name " " - SID "4915" - Ports [2, 1] - Position [240, 240, 390, 280] - ZOrder 756 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_LEG" - } - Block { - BlockType Reference - Name " " - SID "4916" - Ports [2, 1] - Position [235, 360, 395, 400] - ZOrder 755 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_LEG" - } - Block { - BlockType Reference - Name " " - SID "4917" - Ports [2, 1] - Position [235, 410, 395, 450] - ZOrder 757 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Reference - Name " " - SID "4918" - Ports [2, 1] - Position [240, 290, 395, 330] - ZOrder 754 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType Switch - Name "Switch1" - SID "4921" - Position [455, 245, 505, 325] - ZOrder 758 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch2" - SID "4922" - Position [455, 365, 505, 445] - ZOrder 759 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Outport - Name "J_LeftFixed" - SID "4923" - Position [550, 278, 580, 292] - ZOrder 768 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_RIghtFixed" - SID "4924" - Position [550, 398, 580, 412] - ZOrder 769 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "Switch2" - SrcPort 1 - DstBlock "J_RIghtFixed" - DstPort 1 - } - Line { - ZOrder 5 - SrcBlock " " - SrcPort 1 - DstBlock "Switch1" - DstPort 1 - } - Line { - ZOrder 6 - SrcBlock " " - SrcPort 1 - DstBlock "Switch1" - DstPort 3 - } - Line { - ZOrder 7 - SrcBlock " " - SrcPort 1 - DstBlock "Switch2" - DstPort 3 - } - Line { - ZOrder 8 - SrcBlock " " - SrcPort 1 - DstBlock "Switch2" - DstPort 1 - } - Line { - ZOrder 11 - SrcBlock "Switch1" - SrcPort 1 - DstBlock "J_LeftFixed" - DstPort 1 - } - Line { - ZOrder 12 - SrcBlock "qj" - SrcPort 1 - Points [91, 0] - Branch { - ZOrder 13 - Points [0, 50] - Branch { - ZOrder 14 - Points [0, 70] - Branch { - ZOrder 15 - Points [0, 50] - DstBlock " " - DstPort 2 - } - Branch { - ZOrder 16 - DstBlock " " - DstPort 2 - } - } - Branch { - ZOrder 17 - DstBlock " " - DstPort 2 - } - } - Branch { - ZOrder 18 - DstBlock " " - DstPort 2 - } - } - Line { - ZOrder 19 - SrcBlock "w_H_b" - SrcPort 1 - Points [45, 0] - Branch { - ZOrder 20 - Points [0, 50] - DstBlock " " - DstPort 1 - } - Branch { - ZOrder 21 - Points [0, -70] - Branch { - ZOrder 22 - Points [0, -50] - DstBlock " " - DstPort 1 - } - Branch { - ZOrder 23 - DstBlock " " - DstPort 1 - } - } - Branch { - ZOrder 24 - DstBlock " " - DstPort 1 - } - } - Line { - ZOrder 29 - SrcBlock "legsInContact " - SrcPort 1 - Points [318, 0; 0, 85; 1, 0] - Branch { - ZOrder 31 - Points [0, 120] - DstBlock "Switch2" - DstPort 2 - } - Branch { - ZOrder 30 - DstBlock "Switch1" - DstPort 2 - } - } - } - } - Block { - BlockType SubSystem - Name "Legs in Contact" - SID "4920" - Ports [2, 1] - Position [195, -224, 365, -171] - ZOrder 904 - LibraryVersion "1.32" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "Legs in Contact" - 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 "4554" - Block { - BlockType Inport - Name "icubStandup" - SID "4920::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4920::19" - Position [20, 136, 40, 154] - ZOrder 10 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4920::4553" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 66 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4920::4552" - Tag "Stateflow S-Function torqueBalancingStandup 11" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 65 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "legsInContact" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4920::4554" - Position [460, 241, 480, 259] - ZOrder 67 - } - Block { - BlockType Outport - Name "legsInContact" - SID "4920::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 81 - SrcBlock "icubStandup" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 82 - SrcBlock "state" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "legsInContact" - ZOrder 83 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "legsInContact" - DstPort 1 - } - Line { - ZOrder 84 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 85 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "References for L" - SID "3721" - Ports [4, 1] - Position [845, 10, 1105, 170] - ZOrder 75 - LibraryVersion "1.32" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "References for L" - 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 "3771" - Block { - BlockType Inport - Name "qjErr" - SID "3721::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "JL" - SID "3721::23" - Position [20, 136, 40, 154] - ZOrder 9 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "JR" - SID "3721::3701" - Position [20, 171, 40, 189] - ZOrder 26 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "activeFeetConstraints" - SID "3721::3702" - Position [20, 206, 40, 224] - ZOrder 27 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "3721::3770" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 95 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "3721::3769" - Tag "Stateflow S-Function torqueBalancingStandup 20" - Ports [4, 2] - Position [180, 102, 230, 203] - ZOrder 94 - FunctionName "sf_sfun" - Parameters "Reg" - PortCounts "[4 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "nu_b_equivalent" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "3721::3771" - Position [460, 241, 480, 259] - ZOrder 96 - } - Block { - BlockType Outport - Name "nu_b_equivalent" - SID "3721::24" - Position [460, 101, 480, 119] - ZOrder 10 - IconDisplay "Port number" - } - Line { - ZOrder 141 - SrcBlock "qjErr" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 142 - SrcBlock "JL" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 143 - SrcBlock "JR" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 144 - SrcBlock "activeFeetConstraints" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - Name "nu_b_equivalent" - ZOrder 145 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "nu_b_equivalent" - DstPort 1 - } - Line { - ZOrder 146 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 147 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Selector - Name "Selector" - SID "3722" - Ports [1, 1] - Position [1405, 16, 1445, 54] - ZOrder 78 - ShowName off - InputPortWidth "6" - IndexOptions "Index vector (dialog)" - Indices "[4 5 6]" - OutputSizes "1" - } - Block { - BlockType Sum - Name "Sum" - SID "3723" - Ports [2, 1] - Position [490, 20, 510, 40] - ZOrder 77 - ShowName off - IconShape "round" - Inputs "|-+" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch" - SID "4029" - Position [470, -166, 530, -94] - ZOrder 733 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Reference - Name "inertial" - SID "3724" - Ports [0, 1] - Position [45, -72, 95, -58] - ZOrder 727 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.IMU" - signalSize "12" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on - } - Block { - BlockType Outport - Name "intHw" - SID "3753" - Position [1485, 28, 1515, 42] - ZOrder -2 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "qjDes" - SrcPort 1 - Points [38, 0] - Branch { - ZOrder 92 - DstBlock "Sum" - DstPort 1 - } - Branch { - ZOrder 10 - Points [0, -50] - Branch { - ZOrder 143 - Points [0, -65] - DstBlock "Compute base to fixed link transform" - DstPort 1 - } - Branch { - ZOrder 91 - Points [450, 0] - Branch { - ZOrder 136 - Points [0, 110] - DstBlock "Fixed Links Jacobians" - DstPort 2 - } - Branch { - ZOrder 135 - DstBlock "Centroidal Momentum" - DstPort 2 - } - } - } - } - Line { - ZOrder 12 - SrcBlock "qj" - SrcPort 1 - Points [50, 0] - DstBlock "Sum" - DstPort 2 - } - Line { - ZOrder 13 - SrcBlock "Sum" - SrcPort 1 - Points [21, 0] - Branch { - ZOrder 138 - Points [0, 170] - DstBlock "Centroidal Momentum" - DstPort 4 - } - Branch { - ZOrder 35 - DstBlock "References for L" - DstPort 1 - } - } - Line { - ZOrder 16 - SrcBlock "References for L" - SrcPort 1 - DstBlock "Centroidal Momentum" - DstPort 3 - } - Line { - ZOrder 129 - SrcBlock "Fixed Links Jacobians" - SrcPort 1 - DstBlock "References for L" - DstPort 2 - } - Line { - ZOrder 18 - SrcBlock "Centroidal Momentum" - SrcPort 1 - DstBlock "Selector" - DstPort 1 - } - Line { - ZOrder 19 - SrcBlock "Selector" - SrcPort 1 - DstBlock "intHw" - DstPort 1 - } - Line { - ZOrder 130 - SrcBlock "Fixed Links Jacobians" - SrcPort 2 - DstBlock "References for L" - DstPort 3 - } - Line { - ZOrder 21 - SrcBlock "constraints" - SrcPort 1 - DstBlock "References for L" - DstPort 4 - } - Line { - ZOrder 28 - SrcBlock "Switch" - SrcPort 1 - Points [26, 0] - Branch { - ZOrder 134 - DstBlock "Centroidal Momentum" - DstPort 1 - } - Branch { - ZOrder 132 - Points [0, 245] - DstBlock "Fixed Links Jacobians" - DstPort 3 - } - } - Line { - ZOrder 144 - SrcBlock "inertial" - SrcPort 1 - DstBlock "Compute base to fixed link transform" - DstPort 2 - } - Line { - ZOrder 25 - SrcBlock "state" - SrcPort 1 - DstBlock "Legs in Contact" - DstPort 2 - } - Line { - ZOrder 125 - SrcBlock "Constant" - SrcPort 1 - DstBlock "Legs in Contact" - DstPort 1 - } - Line { - ZOrder 137 - SrcBlock "Legs in Contact" - SrcPort 1 - Points [33, 0] - Branch { - ZOrder 168 - Points [191, 0; 0, 260] - DstBlock "Fixed Links Jacobians" - DstPort 1 - } - Branch { - ZOrder 146 - Points [0, 65] - DstBlock "Switch" - DstPort 2 - } - } - Line { - ZOrder 149 - SrcBlock "Compute base to fixed link transform" - SrcPort 1 - Points [86, 0; 0, -70] - DstBlock "Switch" - DstPort 1 - } - Line { - ZOrder 151 - SrcBlock "Compute base to fixed link transform" - SrcPort 2 - Points [103, 0; 0, -40] - DstBlock "Switch" - DstPort 3 - } - } - } - Block { - BlockType SubSystem - Name "Compute joint torques" - SID "4393" - Ports [14, 2] - Position [1915, 41, 2100, 889] - ZOrder 846 - RequestExecContextInheritance off - System { - Name "Compute joint torques" - 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 "279" - Block { - BlockType Inport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "4410" - Position [430, 499, 460, 511] - ZOrder 640 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "tauModel" - SID "4395" - Position [875, 313, 905, 327] - ZOrder 412 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Sigma" - SID "4396" - Position [875, 408, 905, 422] - ZOrder 413 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "NA" - SID "4397" - Position [880, 713, 910, 727] - ZOrder 414 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "fLDes" - SID "4420" - Position [875, 518, 905, 532] - ZOrder 650 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "HessianMatrixQP1Foot" - SID "4411" - Position [430, 533, 460, 547] - ZOrder 641 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gradientQP1Foot" - SID "4412" - Position [430, 568, 460, 582] - ZOrder 642 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ConstraintsMatrixQP1Foot" - SID "4413" - Position [430, 603, 460, 617] - ZOrder 647 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "bVectorConstraintsQP1Foot" - SID "4414" - Position [430, 638, 460, 652] - ZOrder 648 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "HessianMatrixQP2Feet" - SID "4415" - Position [430, 673, 460, 687] - ZOrder 643 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gradientQP2Feet" - SID "4416" - Position [430, 708, 460, 722] - ZOrder 644 - Port "11" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ConstraintsMatrixQP2Feet" - SID "4417" - Position [430, 743, 460, 757] - ZOrder 645 - Port "12" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "bVectorConstraintsQp2Feet" - SID "4418" - Position [430, 778, 460, 792] - ZOrder 646 - Port "13" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4398" - Position [430, 813, 460, 827] - ZOrder 415 - Port "14" - IconDisplay "Port number" - } - Block { - BlockType Sum - Name "Add" - SID "2367" - Ports [2, 1] - Position [1510, 266, 1590, 484] - 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 [1255, 503, 1325, 587] - 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 [1405, 399, 1470, 461] - 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 [975, 704, 1080, 771] - ZOrder 410 - ShowName off - Multiplication "Matrix(*)" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - RndMeth "Floor" - SaturateOnIntegerOverflow off - } - Block { - BlockType SubSystem - Name "QPSolver" - SID "5024" - Ports [10, 2] - Position [580, 478, 815, 847] - ZOrder 651 - RequestExecContextInheritance off - System { - Name "QPSolver" - 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 "313" - Block { - BlockType Inport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "5025" - Position [-445, -6, -415, 6] - ZOrder 394 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "HessianMatrixQP1Foot" - SID "5026" - Position [-445, 63, -415, 77] - ZOrder 395 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gradientQP1Foot" - SID "5027" - Position [-445, 98, -415, 112] - ZOrder 396 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ConstraintsMatrixQP1Foot" - SID "5028" - Position [-445, 133, -415, 147] - ZOrder 403 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "bVectorConstraintsQP1Foot" - SID "5029" - Position [-445, 168, -415, 182] - ZOrder 404 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "HessianMatrixQP2Feet" - SID "5030" - Position [-445, -192, -415, -178] - ZOrder 397 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gradientQP2Feet" - SID "5031" - Position [-445, -157, -415, -143] - ZOrder 398 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ConstraintsMatrixQP2Feet" - SID "5032" - Position [-445, -122, -415, -108] - ZOrder 399 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "bVectorConstraintsQp2Feet" - SID "5033" - Position [-445, -87, -415, -73] - ZOrder 400 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "5034" - Position [390, 38, 420, 52] - ZOrder 553 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "ContactsTransition" - SID "5036" - Ports [1, 1] - Position [-250, -27, 10, 27] - ZOrder 721 - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "ContactsTransition" - 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 "48" - Block { - BlockType Inport - Name "LR_FootInContact" - SID "5036::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "5036::47" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 38 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "5036::46" - Tag "Stateflow S-Function torqueBalancingStandup 15" - Ports [1, 2] - Position [180, 100, 230, 160] - ZOrder 37 - FunctionName "sf_sfun" - PortCounts "[1 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "onOneFoot" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "5036::48" - Position [460, 241, 480, 259] - ZOrder 39 - } - Block { - BlockType Outport - Name "onOneFoot" - SID "5036::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 29 - SrcBlock "LR_FootInContact" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - Name "onOneFoot" - ZOrder 30 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "onOneFoot" - 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 Logic - Name "Logical\nOperator1" - SID "5037" - Ports [2, 1] - Position [530, -68, 560, -37] - ZOrder 693 - ShowName off - Operator "OR" - AllPortsSameDT off - OutDataTypeStr "boolean" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n3" - SID "5039" - Position [395, -66, 500, -54] - ZOrder 692 - ShowName off - Value "Config.SCOPES_ALL" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n4" - SID "5040" - Position [390, -51, 505, -39] - ZOrder 694 - ShowName off - Value "Config.SCOPES_QP" - } - Block { - BlockType SubSystem - Name "One Foot" - SID "5041" - Ports [4, 2, 1] - Position [55, 51, 150, 194] - ZOrder 722 - TreatAsAtomicUnit on - RequestExecContextInheritance off - System { - Name "One Foot" - 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 "445" - Block { - BlockType Inport - Name "H" - SID "5042" - Position [105, -22, 135, -8] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "g" - SID "5043" - Position [105, 8, 135, 22] - ZOrder 698 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "A" - SID "5044" - Position [105, 38, 135, 52] - ZOrder 699 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ubA" - SID "5045" - Position [105, 68, 135, 82] - ZOrder 700 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "5046" - Ports [] - Position [265, -100, 285, -80] - ZOrder 704 - } - Block { - BlockType SubSystem - Name "Analytical Solution One Foot (unconstrained)" - SID "5076" - Ports [2, 1] - Position [330, -200, 510, -100] - ZOrder 734 - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "Analytical Solution 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 "49" - Block { - BlockType Inport - Name "H" - SID "5076::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "g" - SID "5076::22" - Position [20, 136, 40, 154] - ZOrder 13 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "5076::48" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 39 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "5076::47" - Tag "Stateflow S-Function torqueBalancingStandup 22" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 38 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "analyticalSolution" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "5076::49" - Position [460, 241, 480, 259] - ZOrder 40 - } - Block { - BlockType Outport - Name "analyticalSolution" - SID "5076::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - 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 "Match Signal Sizes1" - SID "5047" - Ports [2, 1] - Position [390, -61, 475, 21] - ZOrder 712 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Match Signal Sizes" - SourceType "Match Signal Sizes" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "Process QP output" - SID "5080" - Ports [3, 1] - Position [555, -212, 690, 172] - ZOrder 737 - 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 "56" - Block { - BlockType Inport - Name "analyticalSolution" - SID "5080::29" - Position [20, 101, 40, 119] - ZOrder 20 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "primalSolution" - SID "5080::1" - Position [20, 136, 40, 154] - ZOrder -1 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "QPStatus" - SID "5080::22" - Position [20, 171, 40, 189] - ZOrder 13 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "5080::55" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 40 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "5080::54" - Tag "Stateflow S-Function torqueBalancingStandup 19" - Ports [3, 2] - Position [180, 100, 230, 180] - ZOrder 39 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[3 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "f0" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "5080::56" - Position [460, 241, 480, 259] - ZOrder 41 - } - Block { - BlockType Outport - Name "f0" - SID "5080::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 43 - SrcBlock "analyticalSolution" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 44 - SrcBlock "primalSolution" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 45 - SrcBlock "QPStatus" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - Name "f0" - ZOrder 46 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "f0" - DstPort 1 - } - Line { - ZOrder 47 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 48 - SrcBlock " SFunction " - SrcPort 1 - Points [20, 0] - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "QP Two Feet" - SID "5048" - Ports [4, 2] - Position [235, -29, 340, 89] - ZOrder 697 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/QP" - SourceType "QP" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - lbA off - ubA on - lb off - ub off - computeObjVal off - stopIfFails off - } - Block { - BlockType Outport - Name "QPStatus" - SID "5050" - Position [420, 53, 450, 67] - ZOrder 701 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "f0_1Foot" - SID "5081" - Position [730, -27, 760, -13] - ZOrder 738 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "H" - SrcPort 1 - Points [20, 0] - Branch { - ZOrder 18 - Points [0, -160] - DstBlock "Analytical Solution One Foot (unconstrained)" - DstPort 1 - } - Branch { - ZOrder 13 - DstBlock "QP Two Feet" - DstPort 1 - } - } - Line { - ZOrder 2 - SrcBlock "ubA" - SrcPort 1 - DstBlock "QP Two Feet" - DstPort 4 - } - Line { - ZOrder 3 - SrcBlock "A" - SrcPort 1 - DstBlock "QP Two Feet" - DstPort 3 - } - Line { - ZOrder 4 - SrcBlock "QP Two Feet" - SrcPort 2 - Points [40, 0] - Branch { - ZOrder 22 - Points [0, 50] - DstBlock "Process QP output" - DstPort 3 - } - Branch { - ZOrder 20 - DstBlock "QPStatus" - DstPort 1 - } - } - Line { - ZOrder 5 - SrcBlock "QP Two Feet" - SrcPort 1 - DstBlock "Match Signal Sizes1" - DstPort 2 - } - Line { - ZOrder 6 - SrcBlock "g" - SrcPort 1 - Points [51, 0] - Branch { - ZOrder 19 - Points [0, -55] - Branch { - ZOrder 16 - Points [0, -85] - DstBlock "Analytical Solution One Foot (unconstrained)" - DstPort 2 - } - Branch { - ZOrder 11 - DstBlock "Match Signal Sizes1" - DstPort 1 - } - } - Branch { - ZOrder 8 - DstBlock "QP Two Feet" - DstPort 2 - } - } - Line { - ZOrder 17 - SrcBlock "Match Signal Sizes1" - SrcPort 1 - DstBlock "Process QP output" - DstPort 2 - } - Line { - ZOrder 15 - SrcBlock "Analytical Solution One Foot (unconstrained)" - SrcPort 1 - DstBlock "Process QP output" - DstPort 1 - } - Line { - ZOrder 23 - SrcBlock "Process QP output" - SrcPort 1 - DstBlock "f0_1Foot" - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Process One Foot Output" - SID "5038" - Ports [2, 1] - Position [190, 134, 420, 241] - ZOrder 727 - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "Process One Foot 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 "49" - Block { - BlockType Inport - Name "primalSolution" - SID "5038::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "LR_FootInContact" - SID "5038::22" - Position [20, 136, 40, 154] - ZOrder 13 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "5038::48" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 39 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "5038::47" - Tag "Stateflow S-Function torqueBalancingStandup 16" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 38 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "f0_oneFoot" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "5038::49" - Position [460, 241, 480, 259] - ZOrder 40 - } - Block { - BlockType Outport - Name "f0_oneFoot" - SID "5038::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 36 - SrcBlock "primalSolution" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 37 - SrcBlock "LR_FootInContact" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "f0_oneFoot" - ZOrder 38 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "f0_oneFoot" - 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 Switch - Name "Switch" - SID "5082" - Position [275, -33, 340, 33] - ZOrder 732 - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType SubSystem - Name "Two Feet" - SID "5054" - Ports [4, 2, 1] - Position [55, -199, 150, -66] - ZOrder 724 - NamePlacement "alternate" - TreatAsAtomicUnit on - RequestExecContextInheritance off - System { - Name "Two Feet" - 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 "445" - Block { - BlockType Inport - Name "H" - SID "5055" - Position [105, -27, 135, -13] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "g" - SID "5056" - Position [105, 3, 135, 17] - ZOrder 698 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "A" - SID "5057" - Position [105, 33, 135, 47] - ZOrder 699 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ubA" - SID "5058" - Position [105, 63, 135, 77] - ZOrder 700 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "5059" - Ports [] - Position [265, -90, 285, -70] - ZOrder -2 - } - Block { - BlockType SubSystem - Name "Analytical Solution Two Feet (unconstrained)" - SID "5074" - Ports [2, 1] - Position [325, -205, 505, -105] - ZOrder 732 - 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 "49" - Block { - BlockType Inport - Name "H" - SID "5074::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "g" - SID "5074::22" - Position [20, 136, 40, 154] - ZOrder 13 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "5074::48" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 39 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "5074::47" - Tag "Stateflow S-Function torqueBalancingStandup 21" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 38 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "analyticalSolution" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "5074::49" - Position [460, 241, 480, 259] - ZOrder 40 - } - Block { - BlockType Outport - Name "analyticalSolution" - SID "5074::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - 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 "Match Signal Sizes" - SID "5060" - Ports [2, 1] - Position [375, -66, 465, 16] - ZOrder 702 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Match Signal Sizes" - SourceType "Match Signal Sizes" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "Process QP output" - SID "5078" - Ports [3, 1] - Position [545, -217, 680, 167] - ZOrder 734 - 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 "56" - Block { - BlockType Inport - Name "analyticalSolution" - SID "5078::29" - Position [20, 101, 40, 119] - ZOrder 20 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "primalSolution" - SID "5078::1" - Position [20, 136, 40, 154] - ZOrder -1 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "QPStatus" - SID "5078::22" - Position [20, 171, 40, 189] - ZOrder 13 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "5078::55" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 40 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "5078::54" - Tag "Stateflow S-Function torqueBalancingStandup 23" - Ports [3, 2] - Position [180, 100, 230, 180] - ZOrder 39 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[3 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "f0" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "5078::56" - Position [460, 241, 480, 259] - ZOrder 41 - } - Block { - BlockType Outport - Name "f0" - SID "5078::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 55 - SrcBlock "analyticalSolution" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 56 - SrcBlock "primalSolution" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 57 - SrcBlock "QPStatus" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - Name "f0" - ZOrder 58 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "f0" - DstPort 1 - } - Line { - ZOrder 59 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 60 - SrcBlock " SFunction " - SrcPort 1 - Points [20, 0] - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "QP Two Feet" - SID "5061" - Ports [4, 2] - Position [225, -34, 330, 84] - ZOrder 697 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/QP" - SourceType "QP" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - lbA off - ubA on - lb off - ub off - computeObjVal off - stopIfFails off - } - Block { - BlockType Outport - Name "QPStatus" - SID "5063" - Position [405, 48, 435, 62] - ZOrder 701 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "f0_twoFeet" - SID "5079" - Position [715, -32, 745, -18] - ZOrder 735 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "H" - SrcPort 1 - Points [12, 0] - Branch { - ZOrder 14 - DstBlock "QP Two Feet" - DstPort 1 - } - Branch { - ZOrder 11 - Points [0, -160] - DstBlock "Analytical Solution Two Feet (unconstrained)" - DstPort 1 - } - } - Line { - ZOrder 2 - SrcBlock "ubA" - SrcPort 1 - DstBlock "QP Two Feet" - DstPort 4 - } - Line { - ZOrder 3 - SrcBlock "A" - SrcPort 1 - DstBlock "QP Two Feet" - DstPort 3 - } - Line { - ZOrder 4 - SrcBlock "g" - SrcPort 1 - Points [52, 0] - Branch { - ZOrder 13 - Points [0, -55] - Branch { - ZOrder 34 - Points [0, -85] - DstBlock "Analytical Solution Two Feet (unconstrained)" - DstPort 2 - } - Branch { - ZOrder 5 - DstBlock "Match Signal Sizes" - DstPort 1 - } - } - Branch { - ZOrder 6 - DstBlock "QP Two Feet" - DstPort 2 - } - } - Line { - ZOrder 7 - SrcBlock "QP Two Feet" - SrcPort 2 - Points [28, 0] - Branch { - ZOrder 20 - DstBlock "QPStatus" - DstPort 1 - } - Branch { - ZOrder 19 - Points [0, 50] - DstBlock "Process QP output" - DstPort 3 - } - } - Line { - ZOrder 8 - SrcBlock "QP Two Feet" - SrcPort 1 - DstBlock "Match Signal Sizes" - DstPort 2 - } - Line { - ZOrder 17 - SrcBlock "Match Signal Sizes" - SrcPort 1 - DstBlock "Process QP output" - DstPort 2 - } - Line { - ZOrder 16 - SrcBlock "Analytical Solution Two Feet (unconstrained)" - SrcPort 1 - DstBlock "Process QP output" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "Process QP output" - SrcPort 1 - DstBlock "f0_twoFeet" - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Visualize eventual QP failures" - SID "5064" - Ports [2, 0, 1] - Position [500, -21, 650, 66] - ZOrder -17 - RequestExecContextInheritance off - System { - Name "Visualize eventual QP failures" - 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 "exitFlagQP" - SID "5065" - Position [100, 164, 125, 176] - ZOrder 409 - IconDisplay "Port number" - Port { - PortNumber 1 - Name "exitFlagQP" - } - } - Block { - BlockType Inport - Name "state" - SID "5066" - Position [100, 198, 125, 212] - ZOrder 554 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "5067" - Ports [] - Position [322, 175, 342, 195] - ZOrder 540 - } - Block { - BlockType Scope - Name "QP status\n(0: no failure,\n1: not using)" - SID "5068" - Ports [2] - Position [230, 153, 275, 222] - ZOrder 408 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extm" - "gr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039])" - ",extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingV" - "ariableName','ScopeData1','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDe" - "cimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','" - "-1.00000','MaxYLimReal','1.00000','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','1.00000','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.2" - "74509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePr" - "opertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" - "Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle" - "','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannel" - "Names',{{}},'NumLines',1,'LineNames',{{'exitFlagQP'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','0." - "875','MaxYLimReal','2.125','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',tr" - "ue,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0." - "686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.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',{{s" - "truct('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','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',{{}},'NumLi" - "nes',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',2)},'DisplayPropertyDefaults',struct('YLabelReal'," - "'','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','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,'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),'DisplayLayoutD" - "imensions',[2 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY" - "'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[355 645 15" - "48 1372])" - NumInputPorts "2" - Floating off - } - Line { - Name "exitFlagQP" - ZOrder 1 - Labels [-1, 0] - SrcBlock "exitFlagQP" - SrcPort 1 - DstBlock "QP status\n(0: no failure,\n1: not using)" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "state" - SrcPort 1 - DstBlock "QP status\n(0: no failure,\n1: not using)" - DstPort 2 - } - } - } - Block { - BlockType Logic - Name "not" - SID "5069" - Ports [1, 1] - Position [90, -62, 110, -27] - ZOrder 723 - BlockRotation 270 - NamePlacement "alternate" - Operator "NOT" - IconShape "distinctive" - AllPortsSameDT off - OutDataTypeStr "boolean" - } - Block { - BlockType Outport - Name "f0OneFoot" - SID "5070" - Position [490, 183, 520, 197] - ZOrder 401 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "f0TwoFeet" - SID "5071" - Position [295, -107, 325, -93] - ZOrder 728 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 3 - SrcBlock "Logical\nOperator1" - SrcPort 1 - Points [10, 0] - DstBlock "Visualize eventual QP failures" - DstPort enable - } - Line { - ZOrder 4 - SrcBlock "state" - SrcPort 1 - DstBlock "Visualize eventual QP failures" - DstPort 2 - } - Line { - ZOrder 5 - SrcBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - SrcPort 1 - Points [93, 0] - Branch { - ZOrder 32 - Points [0, 215] - DstBlock "Process One Foot Output" - DstPort 2 - } - Branch { - ZOrder 27 - DstBlock "ContactsTransition" - DstPort 1 - } - } - Line { - ZOrder 9 - SrcBlock "ON_GAZEBO\n3" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 1 - } - Line { - ZOrder 10 - SrcBlock "ON_GAZEBO\n4" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 2 - } - Line { - ZOrder 11 - SrcBlock "HessianMatrixQP1Foot" - SrcPort 1 - DstBlock "One Foot" - DstPort 1 - } - Line { - ZOrder 12 - SrcBlock "bVectorConstraintsQp2Feet" - SrcPort 1 - DstBlock "Two Feet" - DstPort 4 - } - Line { - ZOrder 14 - SrcBlock "gradientQP1Foot" - SrcPort 1 - DstBlock "One Foot" - DstPort 2 - } - Line { - ZOrder 15 - SrcBlock "ConstraintsMatrixQP1Foot" - SrcPort 1 - DstBlock "One Foot" - DstPort 3 - } - Line { - ZOrder 17 - SrcBlock "gradientQP2Feet" - SrcPort 1 - DstBlock "Two Feet" - DstPort 2 - } - Line { - ZOrder 18 - SrcBlock "ConstraintsMatrixQP2Feet" - SrcPort 1 - DstBlock "Two Feet" - DstPort 3 - } - Line { - ZOrder 19 - SrcBlock "not" - SrcPort 1 - DstBlock "Two Feet" - DstPort enable - } - Line { - ZOrder 20 - SrcBlock "ContactsTransition" - SrcPort 1 - Points [85, 0] - Branch { - ZOrder 52 - DstBlock "One Foot" - DstPort enable - } - Branch { - ZOrder 38 - DstBlock "Switch" - DstPort 2 - } - Branch { - ZOrder 31 - DstBlock "not" - DstPort 1 - } - } - Line { - ZOrder 23 - SrcBlock "bVectorConstraintsQP1Foot" - SrcPort 1 - DstBlock "One Foot" - DstPort 4 - } - Line { - ZOrder 26 - SrcBlock "HessianMatrixQP2Feet" - SrcPort 1 - DstBlock "Two Feet" - DstPort 1 - } - Line { - ZOrder 35 - SrcBlock "One Foot" - SrcPort 2 - DstBlock "Process One Foot Output" - DstPort 1 - } - Line { - ZOrder 36 - SrcBlock "Process One Foot Output" - SrcPort 1 - DstBlock "f0OneFoot" - DstPort 1 - } - Line { - ZOrder 40 - SrcBlock "One Foot" - SrcPort 1 - Points [18, 0; 0, -110] - DstBlock "Switch" - DstPort 1 - } - Line { - ZOrder 48 - SrcBlock "Two Feet" - SrcPort 1 - Points [37, 0; 0, 185] - DstBlock "Switch" - DstPort 3 - } - Line { - ZOrder 54 - SrcBlock "Switch" - SrcPort 1 - DstBlock "Visualize eventual QP failures" - DstPort 1 - } - Line { - ZOrder 37 - SrcBlock "Two Feet" - SrcPort 2 - DstBlock "f0TwoFeet" - DstPort 1 - } - } - } - Block { - BlockType Sum - Name "Sum" - SID "2402" - Ports [2, 1] - Position [1030, 515, 1050, 535] - ZOrder 395 - ShowName off - IconShape "round" - Inputs "|++" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Outport - Name "tau " - SID "4408" - Position [1635, 368, 1665, 382] - ZOrder 425 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "fDes_QP" - SID "4409" - Position [1440, 538, 1470, 552] - ZOrder 639 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 293 - SrcBlock "NA" - SrcPort 1 - DstBlock "Product2" - DstPort 1 - } - Line { - ZOrder 344 - SrcBlock "QPSolver" - SrcPort 1 - Points [220, 0] - DstBlock "Sum" - DstPort 2 - } - 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 " - DstPort 1 - } - Line { - ZOrder 57 - SrcBlock "Add1" - SrcPort 1 - Points [32, 0] - Branch { - ZOrder 327 - DstBlock "fDes_QP" - DstPort 1 - } - Branch { - ZOrder 59 - Points [0, -100] - DstBlock "Product1" - DstPort 2 - } - } - Line { - ZOrder 65 - SrcBlock "Sum" - SrcPort 1 - DstBlock "Add1" - DstPort 1 - } - Line { - ZOrder 66 - SrcBlock "Product2" - SrcPort 1 - Points [89, 0; 0, -175] - DstBlock "Add1" - DstPort 2 - } - Line { - ZOrder 292 - SrcBlock "Sigma" - SrcPort 1 - DstBlock "Product1" - DstPort 1 - } - Line { - ZOrder 353 - SrcBlock "QPSolver" - SrcPort 2 - DstBlock "Product2" - DstPort 2 - } - Line { - ZOrder 347 - SrcBlock "gradientQP1Foot" - SrcPort 1 - DstBlock "QPSolver" - DstPort 3 - } - Line { - ZOrder 345 - SrcBlock "ConstraintsMatrixQP2Feet" - SrcPort 1 - DstBlock "QPSolver" - DstPort 8 - } - Line { - ZOrder 346 - SrcBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - SrcPort 1 - DstBlock "QPSolver" - DstPort 1 - } - Line { - ZOrder 349 - SrcBlock "bVectorConstraintsQP1Foot" - SrcPort 1 - DstBlock "QPSolver" - DstPort 5 - } - Line { - ZOrder 350 - SrcBlock "HessianMatrixQP1Foot" - SrcPort 1 - DstBlock "QPSolver" - DstPort 2 - } - Line { - ZOrder 354 - SrcBlock "ConstraintsMatrixQP1Foot" - SrcPort 1 - DstBlock "QPSolver" - DstPort 4 - } - Line { - ZOrder 351 - SrcBlock "gradientQP2Feet" - SrcPort 1 - DstBlock "QPSolver" - DstPort 7 - } - Line { - ZOrder 352 - SrcBlock "HessianMatrixQP2Feet" - SrcPort 1 - DstBlock "QPSolver" - DstPort 6 - } - Line { - ZOrder 355 - SrcBlock "bVectorConstraintsQp2Feet" - SrcPort 1 - DstBlock "QPSolver" - DstPort 9 - } - Line { - ZOrder 348 - SrcBlock "state" - SrcPort 1 - DstBlock "QPSolver" - DstPort 10 - } - Line { - ZOrder 326 - SrcBlock "fLDes" - SrcPort 1 - DstBlock "Sum" - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Compute joint torques (motor reflected inertia)" - SID "5096" - Ports [1, 1] - Position [2150, 226, 2305, 284] - ZOrder 877 - RequestExecContextInheritance off - System { - Name "Compute joint torques (motor reflected inertia)" - 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 "339" - Block { - BlockType Inport - Name "u" - SID "5097" - Position [-330, 3, -300, 17] - ZOrder 591 - IconDisplay "Port number" - } - Block { - BlockType Constant - Name " " - SID "5098" - Position [130, -56, 345, -44] - ZOrder 607 - Value "Config.USE_MOTOR_REFLECTED_INERTIA" - } - Block { - BlockType Constant - Name " 1" - SID "5099" - Position [-550, -134, -285, -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 "5100" - 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 "86" - Block { - BlockType Demux - Name " Demux " - SID "5100::84" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 75 - Outputs "1" - } - Block { - BlockType Ground - Name " Ground " - SID "5100::86" - Position [20, 121, 40, 139] - ZOrder 77 - } - Block { - BlockType S-Function - Name " SFunction " - SID "5100::83" - Tag "Stateflow S-Function torqueBalancingStandup 12" - Ports [1, 2] - Position [180, 100, 230, 160] - ZOrder 74 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[1 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "reflectedInertia" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "5100::85" - Position [460, 241, 480, 259] - ZOrder 76 - } - Block { - BlockType Outport - Name "reflectedInertia" - SID "5100::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - Name "reflectedInertia" - ZOrder 17 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "reflectedInertia" - DstPort 1 - } - Line { - ZOrder 18 - SrcBlock " Ground " - SrcPort 1 - DstBlock " SFunction " - 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 Sum - Name "Add" - SID "5101" - Ports [2, 1] - Position [65, -209, 110, -16] - ZOrder 602 - Inputs "-+" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType SubSystem - Name "Check the contribution of feedforward" - SID "5102" - Ports [2, 0, 1] - Position [30, 66, 140, 119] - ZOrder 894 - RequestExecContextInheritance off - System { - Name "Check the contribution of feedforward" - Location [65, 24, 2560, 1440] - 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 "300" - Block { - BlockType Inport - Name "feedForward" - SID "5103" - Position [-10, 58, 20, 72] - ZOrder 896 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "u" - SID "5104" - Position [-10, -2, 20, 12] - ZOrder 894 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "5105" - Ports [] - Position [0, -105, 20, -85] - ZOrder 902 - } - Block { - BlockType Demux - Name "Demux1" - SID "5106" - Ports [1, 5] - Position [285, 96, 290, 194] - ZOrder 899 - 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 "5107" - Ports [1, 5] - Position [285, -44, 290, 54] - ZOrder 893 - 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 "5108" - Ports [1, 5] - Position [285, -184, 290, -86] - ZOrder 901 - 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 Sum - Name "Sum" - SID "5109" - Ports [2, 1] - Position [150, -5, 170, 15] - ZOrder 886 - ShowName off - IconShape "round" - Inputs "|+-" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - Port { - PortNumber 1 - Name "u_with_feedForward" - } - } - Block { - BlockType Scope - Name "feedforward_only" - SID "5110" - Ports [5] - Position [365, 88, 470, 202] - ZOrder 898 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','ScopeData3','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-23.63249'" - ",'MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.95301','LegendVisibility','On','" - "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" - "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" - "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " - "1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{" - "struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5" - ",'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'L" - "ineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabel" - "Real','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false" - ",'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0" - "666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83" - "1372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" - " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" - "t',2),struct('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" - "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" - "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" - "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" - "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," - "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" - "uct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNam" - "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-66.44849','MaxYL" - "imReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" - ",true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803" - "922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" - "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411764" - "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1" - " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" - "',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','" - "LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'S" - "howContent',true,'Placement',4),struct('MinYLimReal','-66.44849','MaxYLimReal','41.64929','YLabelReal','','MinYLim" - "Mag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0" - " 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0" - ".0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0" - "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[" - "]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" - "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803" - "9215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392" - "15686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title'" - ",'%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Use" - "rDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','9" - "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',{struct('Title','%','" - "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" - "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" - "s',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" - " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('T" - "itle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" - ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" - "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2018a','Location',[385 262 2230 1233])" - NumInputPorts "5" - Floating off - } - Block { - BlockType Scope - Name "u_only" - SID "5111" - Ports [5] - Position [365, -192, 470, -78] - ZOrder 900 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','ScopeData2','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-23.63249'" - ",'MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.95301','LegendVisibility','On','" - "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" - "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" - "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " - "1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{" - "struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5" - ",'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'L" - "ineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabel" - "Real','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false" - ",'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0" - "666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83" - "1372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" - " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" - "t',2),struct('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" - "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" - "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" - "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" - "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," - "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" - "uct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNam" - "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-66.44849','MaxYL" - "imReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" - ",true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803" - "922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" - "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411764" - "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1" - " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" - "',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','" - "LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'S" - "howContent',true,'Placement',4),struct('MinYLimReal','-66.44849','MaxYLimReal','41.64929','YLabelReal','','MinYLim" - "Mag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0" - " 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0" - ".0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0" - "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[" - "]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" - "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803" - "9215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392" - "15686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title'" - ",'%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Use" - "rDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','9" - "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',{struct('Title','%','" - "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" - "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" - "s',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" - " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('T" - "itle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" - ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" - "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2018a','Location',[385 262 2230 1233])" - NumInputPorts "5" - Floating off - } - Block { - BlockType Scope - Name "u_with_feedforward" - SID "5112" - Ports [5] - Position [365, -52, 470, 62] - ZOrder 887 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','ScopeData1','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-23.63249'" - ",'MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.95301','LegendVisibility','On','" - "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" - "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" - "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " - "1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{" - "struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5" - ",'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'L" - "ineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabel" - "Real','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false" - ",'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0" - "666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83" - "1372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" - " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" - "t',2),struct('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" - "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" - "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" - "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" - "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," - "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" - "uct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNam" - "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-66.44849','MaxYL" - "imReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" - ",true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803" - "922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" - "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411764" - "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1" - " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" - "',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','" - "LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'S" - "howContent',true,'Placement',4),struct('MinYLimReal','-66.44849','MaxYLimReal','41.64929','YLabelReal','','MinYLim" - "Mag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0" - " 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0" - ".0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0" - "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[" - "]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" - "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803" - "9215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392" - "15686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title'" - ",'%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Use" - "rDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','9" - "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',{struct('Title','%','" - "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" - "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" - "s',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" - " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('T" - "itle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" - ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" - "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2018a','Location',[385 262 2230 1233])" - NumInputPorts "5" - Floating off - } - Line { - Name "right_leg" - ZOrder 1 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 5 - DstBlock "u_with_feedforward" - DstPort 5 - } - Line { - Name "left_arm" - ZOrder 2 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 2 - DstBlock "u_with_feedforward" - DstPort 2 - } - Line { - Name "right_arm" - ZOrder 3 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 3 - DstBlock "u_with_feedforward" - DstPort 3 - } - Line { - Name "torso" - ZOrder 4 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 1 - DstBlock "u_with_feedforward" - DstPort 1 - } - Line { - Name "left_leg" - ZOrder 5 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 4 - DstBlock "u_with_feedforward" - DstPort 4 - } - Line { - Name "u_with_feedForward" - ZOrder 6 - Labels [1, 1] - SrcBlock "Sum" - SrcPort 1 - DstBlock "Demux10" - DstPort 1 - } - Line { - ZOrder 7 - SrcBlock "feedForward" - SrcPort 1 - Points [136, 0] - Branch { - ZOrder 8 - Points [0, 80] - DstBlock "Demux1" - DstPort 1 - } - Branch { - ZOrder 9 - DstBlock "Sum" - DstPort 2 - } - } - Line { - Name "left_leg" - ZOrder 10 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 4 - DstBlock "feedforward_only" - DstPort 4 - } - Line { - Name "right_leg" - ZOrder 11 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 5 - DstBlock "feedforward_only" - DstPort 5 - } - Line { - Name "left_arm" - ZOrder 12 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 2 - DstBlock "feedforward_only" - DstPort 2 - } - Line { - Name "torso" - ZOrder 13 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 1 - DstBlock "feedforward_only" - DstPort 1 - } - Line { - Name "right_arm" - ZOrder 14 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 3 - DstBlock "feedforward_only" - DstPort 3 - } - Line { - Name "left_leg" - ZOrder 15 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 4 - DstBlock "u_only" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 16 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 3 - DstBlock "u_only" - DstPort 3 - } - Line { - Name "right_leg" - ZOrder 17 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 5 - DstBlock "u_only" - DstPort 5 - } - Line { - Name "left_arm" - ZOrder 18 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 2 - DstBlock "u_only" - DstPort 2 - } - Line { - ZOrder 19 - SrcBlock "u" - SrcPort 1 - Points [95, 0] - Branch { - ZOrder 20 - Points [0, -140] - DstBlock "Demux2" - DstPort 1 - } - Branch { - ZOrder 21 - DstBlock "Sum" - DstPort 1 - } - } - Line { - Name "torso" - ZOrder 22 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 1 - DstBlock "u_only" - DstPort 1 - } - } - } - Block { - BlockType Constant - Name "Constant" - SID "5113" - Position [-75, 30, 45, 50] - ZOrder 895 - ShowName off - Value "Config.SCOPES_INERTIA" - } - Block { - BlockType From - Name "From" - SID "5114" - Position [-465, -155, -380, -145] - ZOrder 902 - ShowName off - HideAutomaticName off - GotoTag "qjDDot_des" - TagVisibility "global" - } - Block { - BlockType Gain - Name "Gain" - SID "5115" - Position [-75, -175, 15, -145] - ZOrder 596 - Gain "Config.K_ff" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Reference - Name "Get Measurement" - SID "5116" - Ports [0, 1] - Position [-465, -109, -375, -91] - ZOrder 869 - BackgroundColor "[0.925500, 0.870600, 0.133300]" - ShowName off - HideAutomaticName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Acceleration" - } - Block { - BlockType Product - Name "Product" - SID "5117" - Ports [2, 1] - Position [-160, -239, -125, -86] - ZOrder 597 - Multiplication "Matrix(*)" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - RndMeth "Floor" - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch" - SID "5118" - Position [380, -138, 430, 38] - ZOrder 606 - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch1" - SID "5119" - Position [-260, -166, -195, -84] - ZOrder 896 - ShowName off - HideAutomaticName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Outport - Name "tau_joints" - SID "5120" - Position [485, -57, 515, -43] - ZOrder 592 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "Product" - SrcPort 1 - Points [18, 0] - Branch { - ZOrder 2 - DstBlock "Gain" - DstPort 1 - } - Branch { - ZOrder 3 - Points [0, 240] - DstBlock "Check the contribution of feedforward" - DstPort 1 - } - } - Line { - ZOrder 4 - SrcBlock "Gain" - SrcPort 1 - DstBlock "Add" - DstPort 1 - } - Line { - ZOrder 5 - SrcBlock "Add" - SrcPort 1 - DstBlock "Switch" - DstPort 1 - } - Line { - ZOrder 6 - SrcBlock " " - SrcPort 1 - DstBlock "Switch" - DstPort 2 - } - Line { - ZOrder 7 - SrcBlock "Switch" - SrcPort 1 - DstBlock "tau_joints" - DstPort 1 - } - Line { - ZOrder 8 - SrcBlock "u" - SrcPort 1 - Points [145, 0] - Branch { - ZOrder 9 - Points [0, -75] - DstBlock "Add" - DstPort 2 - } - Branch { - ZOrder 10 - Points [0, 95] - DstBlock "Check the contribution of feedforward" - DstPort 2 - } - Branch { - ZOrder 11 - DstBlock "Switch" - DstPort 3 - } - } - Line { - ZOrder 12 - SrcBlock "(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{-1}" - SrcPort 1 - DstBlock "Product" - DstPort 1 - } - Line { - ZOrder 13 - SrcBlock "Constant" - SrcPort 1 - Points [35, 0] - DstBlock "Check the contribution of feedforward" - DstPort enable - } - Line { - ZOrder 14 - SrcBlock " 1" - SrcPort 1 - DstBlock "Switch1" - DstPort 2 - } - Line { - ZOrder 15 - SrcBlock "Switch1" - SrcPort 1 - DstBlock "Product" - DstPort 2 - } - Line { - ZOrder 16 - SrcBlock "Get Measurement" - SrcPort 1 - DstBlock "Switch1" - DstPort 3 - } - Line { - ZOrder 17 - SrcBlock "From" - SrcPort 1 - DstBlock "Switch1" - DstPort 1 - } - } - } - Block { - BlockType Constant - Name "Constant2" - SID "4832" - Position [1230, 1062, 1385, 1078] - ZOrder 865 - Value "Config.STANDUP_WITH_HUMAN" - } - Block { - BlockType SubSystem - Name "Constraints" - SID "4893" - Ports [1, 2] - Position [1230, 178, 1380, 247] - ZOrder 871 - RequestExecContextInheritance off - System { - Name "Constraints" - 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 "legsInContact" - SID "4894" - Position [110, 18, 140, 32] - ZOrder 563 - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant" - SID "4895" - Position [65, 112, 185, 138] - ZOrder 383 - ShowName off - Value "ConstraintsMatrixFeet" - } - Block { - BlockType Constant - Name "Constant1" - SID "4896" - Position [65, 172, 185, 198] - ZOrder 384 - ShowName off - Value "bVectorConstraintsLegs" - } - Block { - BlockType Constant - Name "Constant2" - SID "4897" - Position [65, 62, 185, 88] - ZOrder 580 - ShowName off - Value "ConstraintsMatrixLegs" - } - Block { - BlockType Constant - Name "Constant3" - SID "4898" - Position [65, 222, 185, 248] - ZOrder 579 - ShowName off - Value "bVectorConstraintsFeet" - } - Block { - BlockType Switch - Name "Switch1" - SID "4899" - Position [245, 60, 300, 140] - ZOrder 577 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch2" - SID "4900" - Position [245, 170, 300, 250] - ZOrder 576 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Outport - Name "ConstraintsMatrix" - SID "4901" - Position [365, 93, 395, 107] - ZOrder 385 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "bVectorConstraints" - SID "4902" - Position [365, 203, 395, 217] - ZOrder 386 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "Constant2" - SrcPort 1 - DstBlock "Switch1" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "Constant" - SrcPort 1 - DstBlock "Switch1" - DstPort 3 - } - Line { - ZOrder 3 - SrcBlock "Constant1" - SrcPort 1 - DstBlock "Switch2" - DstPort 1 - } - Line { - ZOrder 4 - SrcBlock "Constant3" - SrcPort 1 - DstBlock "Switch2" - DstPort 3 - } - Line { - ZOrder 5 - SrcBlock "legsInContact" - SrcPort 1 - Points [70, 0; 0, 75] - Branch { - ZOrder 6 - Points [0, 110] - DstBlock "Switch2" - DstPort 2 - } - Branch { - ZOrder 7 - DstBlock "Switch1" - DstPort 2 - } - } - Line { - ZOrder 8 - SrcBlock "Switch1" - SrcPort 1 - DstBlock "ConstraintsMatrix" - DstPort 1 - } - Line { - ZOrder 9 - SrcBlock "Switch2" - SrcPort 1 - DstBlock "bVectorConstraints" - DstPort 1 - } - } - } - Block { - BlockType Goto - Name "Goto" - SID "4494" - Position [1760, 906, 1840, 924] - ZOrder 855 - GotoTag "f_noQP" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto1" - SID "4495" - Position [2440, 316, 2520, 334] - ZOrder 856 - GotoTag "tau_des" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto2" - SID "4903" - Position [1760, 966, 1845, 984] - ZOrder 872 - GotoTag "corrFromSupportF" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto3" - SID "4904" - Position [1760, 1026, 1840, 1044] - ZOrder 873 - GotoTag "L_error" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto4" - SID "4905" - Position [1760, 1086, 1840, 1104] - ZOrder 874 - GotoTag "V" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto5" - SID "4499" - Position [2185, 671, 2265, 689] - ZOrder 860 - GotoTag "f_QP" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto6" - SID "4500" - Position [1760, 846, 1840, 864] - ZOrder 861 - GotoTag "CoM_error" - TagVisibility "global" - } - Block { - BlockType Reference - Name "left_foot_wrench1" - SID "4833" - Ports [0, 1] - Position [1280, 1025, 1335, 1045] - ZOrder 866 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.WRENCH_RIGHT_ARM" - signalSize "6" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on - } - Block { - BlockType Reference - Name "left_foot_wrench2" - SID "4834" - Ports [0, 1] - Position [1280, 990, 1335, 1010] - ZOrder 867 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.WRENCH_LEFT_ARM" - signalSize "6" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on - } - Block { - BlockType Outport - Name "tau " - SID "2414" - Position [2465, 248, 2495, 262] - ZOrder 279 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - SrcPort 1 - Points [77, 0] - Branch { - ZOrder 444 - Points [0, -50] - DstBlock "Compute joint torques" - DstPort 1 - } - Branch { - ZOrder 513 - DstBlock "Balancing Controller\n" - DstPort 1 - } - Branch { - ZOrder 445 - Points [0, 355] - DstBlock "Compute angular momentum integral" - DstPort 3 - } - } - Line { - ZOrder 12 - SrcBlock "qj" - SrcPort 1 - Points [94, 0] - Branch { - ZOrder 514 - DstBlock "Balancing Controller\n" - DstPort 5 - } - Branch { - ZOrder 446 - Points [0, 200] - DstBlock "Compute angular momentum integral" - DstPort 2 - } - } - Line { - ZOrder 31 - SrcBlock "qjDes" - SrcPort 1 - Points [111, 0] - Branch { - ZOrder 515 - DstBlock "Balancing Controller\n" - DstPort 6 - } - Branch { - ZOrder 447 - Points [0, 150] - DstBlock "Compute angular momentum integral" - DstPort 1 - } - } - Line { - ZOrder 34 - SrcBlock "M" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 8 - } - Line { - ZOrder 35 - SrcBlock "biasTorques" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 9 - } - Line { - ZOrder 36 - SrcBlock "desired_x_dx_ddx_CoM" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 20 - } - Line { - ZOrder 37 - SrcBlock "impedances" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 23 - } - Line { - ZOrder 234 - SrcBlock "L" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 10 - } - Line { - ZOrder 60 - SrcBlock "\n\n 5" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 2 - } - Line { - ZOrder 389 - SrcBlock "Constraints" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 3 - } - Line { - ZOrder 387 - SrcBlock "Constraints" - SrcPort 2 - DstBlock "Balancing Controller\n" - DstPort 4 - } - Line { - ZOrder 97 - SrcBlock "nu" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 7 - } - Line { - ZOrder 106 - SrcBlock "gainsPCOM" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 21 - } - Line { - ZOrder 107 - SrcBlock "gainsDCOM" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 22 - } - Line { - ZOrder 237 - SrcBlock "w_H_r_contact" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 13 - } - Line { - ZOrder 238 - SrcBlock "w_H_l_contact" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 12 - } - Line { - ZOrder 267 - SrcBlock "JL" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 14 - } - Line { - ZOrder 268 - SrcBlock "JR" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 15 - } - Line { - ZOrder 269 - SrcBlock "Compute angular momentum integral" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 11 - } - Line { - ZOrder 273 - SrcBlock "dJL_nu" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 16 - } - Line { - ZOrder 274 - SrcBlock "dJR_nu" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 17 - } - Line { - ZOrder 275 - SrcBlock "J_CoM" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 19 - } - Line { - ZOrder 276 - SrcBlock "xCoM" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 18 - } - Line { - ZOrder 284 - SrcBlock "state" - SrcPort 1 - Points [77, 0] - Branch { - ZOrder 517 - Points [0, 610] - Branch { - ZOrder 519 - Points [0, 62; 818, 0; 0, -312] - DstBlock "Compute joint torques" - DstPort 14 - } - Branch { - ZOrder 450 - DstBlock "Balancing Controller\n" - DstPort 29 - } - } - Branch { - ZOrder 318 - DstBlock "Compute angular momentum integral" - DstPort 4 - } - } - Line { - ZOrder 575 - SrcBlock "Compute joint torques" - SrcPort 1 - DstBlock "Compute joint torques (motor reflected inertia)" - DstPort 1 - } - Line { - ZOrder 306 - SrcBlock "Balancing Controller\n" - SrcPort 1 - DstBlock "Compute joint torques" - DstPort 2 - } - Line { - ZOrder 307 - SrcBlock "Balancing Controller\n" - SrcPort 2 - DstBlock "Compute joint torques" - DstPort 3 - } - Line { - ZOrder 308 - SrcBlock "Balancing Controller\n" - SrcPort 3 - DstBlock "Compute joint torques" - DstPort 4 - } - Line { - ZOrder 309 - SrcBlock "Balancing Controller\n" - SrcPort 4 - DstBlock "Compute joint torques" - DstPort 5 - } - Line { - ZOrder 310 - SrcBlock "Balancing Controller\n" - SrcPort 5 - DstBlock "Compute joint torques" - DstPort 6 - } - Line { - ZOrder 311 - SrcBlock "Balancing Controller\n" - SrcPort 6 - DstBlock "Compute joint torques" - DstPort 7 - } - Line { - ZOrder 312 - SrcBlock "Balancing Controller\n" - SrcPort 7 - DstBlock "Compute joint torques" - DstPort 8 - } - Line { - ZOrder 313 - SrcBlock "Balancing Controller\n" - SrcPort 8 - DstBlock "Compute joint torques" - DstPort 9 - } - Line { - ZOrder 314 - SrcBlock "Balancing Controller\n" - SrcPort 9 - DstBlock "Compute joint torques" - DstPort 10 - } - Line { - ZOrder 315 - SrcBlock "Balancing Controller\n" - SrcPort 10 - DstBlock "Compute joint torques" - DstPort 11 - } - Line { - ZOrder 316 - SrcBlock "Balancing Controller\n" - SrcPort 11 - DstBlock "Compute joint torques" - DstPort 12 - } - Line { - ZOrder 317 - SrcBlock "Balancing Controller\n" - SrcPort 12 - DstBlock "Compute joint torques" - DstPort 13 - } - Line { - Name "CoM_Error" - ZOrder 323 - Labels [0, 0] - SrcBlock "Balancing Controller\n" - SrcPort 13 - DstBlock "Goto6" - DstPort 1 - } - Line { - ZOrder 324 - SrcBlock "Compute joint torques" - SrcPort 2 - DstBlock "Goto5" - DstPort 1 - } - Line { - Name "fNoQP" - ZOrder 325 - Labels [0, 0] - SrcBlock "Balancing Controller\n" - SrcPort 14 - DstBlock "Goto" - DstPort 1 - } - Line { - ZOrder 394 - SrcBlock "Constant2" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 28 - } - Line { - ZOrder 392 - SrcBlock "left_foot_wrench2" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 26 - } - Line { - ZOrder 393 - SrcBlock "left_foot_wrench1" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 27 - } - Line { - ZOrder 390 - SrcBlock "w_H_l_hand" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 24 - } - Line { - ZOrder 391 - SrcBlock "w_H_r_hand" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 25 - } - Line { - ZOrder 395 - SrcBlock "legsInContact" - SrcPort 1 - DstBlock "Constraints" - DstPort 1 - } - Line { - ZOrder 396 - SrcBlock "Balancing Controller\n" - SrcPort 17 - DstBlock "Goto4" - DstPort 1 - } - Line { - ZOrder 397 - SrcBlock "Balancing Controller\n" - SrcPort 16 - DstBlock "Goto3" - DstPort 1 - } - Line { - ZOrder 398 - SrcBlock "Balancing Controller\n" - SrcPort 15 - DstBlock "Goto2" - DstPort 1 - } - Line { - ZOrder 574 - SrcBlock "Compute joint torques (motor reflected inertia)" - SrcPort 1 - Points [58, 0] - Branch { - ZOrder 520 - DstBlock "tau " - DstPort 1 - } - Branch { - ZOrder 382 - Points [0, 70] - DstBlock "Goto1" - DstPort 1 - } - } - } - } - Block { - BlockType SubSystem - Name "emergency stop: joint limits" - SID "5127" - Ports [1] - Position [1040, 436, 1165, 464] - ZOrder 554 - BackgroundColor "red" - ShowName off - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 22 - $ClassName "Simulink.Mask" - Display "disp('EMERGENCY STOP')" - RunInitForIconRedraw "off" - } - System { - Name "emergency stop: joint limits" - 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 "904" - Block { - BlockType Inport - Name "qj" - SID "5128" - Position [150, 238, 180, 252] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n1" - SID "5129" - 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 "5130" - 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 "5131" - 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] - 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 "300" - Block { - BlockType Inport - Name "qj" - SID "5132" - Position [60, 103, 90, 117] - ZOrder 552 - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "5133" - Ports [] - Position [227, -45, 247, -25] - ZOrder 553 - } - Block { - BlockType Assertion - Name "Assertion" - SID "5134" - Position [340, 72, 400, 118] - ZOrder 207 - } - Block { - BlockType Reference - Name "Get Limits" - SID "5135" - Ports [0, 2] - Position [20, 23, 140, 92] - ZOrder 551 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Limits" - SourceType "Get Limits" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - limitsSource "ControlBoard" - limitsType "Position" - } - Block { - BlockType SubSystem - Name "MATLAB Function" - SID "5136" - Ports [4, 1] - 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 "3770" - Block { - BlockType Inport - Name "umin" - SID "5136::18" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "umax" - SID "5136::19" - Position [20, 136, 40, 154] - ZOrder -2 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "u" - SID "5136::1" - Position [20, 171, 40, 189] - ZOrder -3 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "tol" - SID "5136::20" - Position [20, 206, 40, 224] - ZOrder -4 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "5136::3769" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 119 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "5136::3768" - Tag "Stateflow S-Function torqueBalancingStandup 18" - Ports [4, 2] - Position [180, 102, 230, 203] - ZOrder 118 - FunctionName "sf_sfun" - PortCounts "[4 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "inRange" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "5136::3770" - Position [460, 241, 480, 259] - ZOrder 120 - } - Block { - BlockType Outport - Name "inRange" - SID "5136::5" - Position [460, 101, 480, 119] - ZOrder -8 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "umin" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "umax" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 3 - SrcBlock "u" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 4 - SrcBlock "tol" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - Name "inRange" - ZOrder 5 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "inRange" - DstPort 1 - } - Line { - ZOrder 6 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 7 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Constant - Name "index1" - SID "5137" - Position [70, 140, 95, 150] - ZOrder 204 - ShowName off - Value "0.01" - VectorParams1D off - } - Line { - ZOrder 1 - SrcBlock "qj" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 3 - } - Line { - ZOrder 2 - SrcBlock "MATLAB Function" - SrcPort 1 - DstBlock "Assertion" - DstPort 1 - } - Line { - ZOrder 3 - SrcBlock "Get Limits" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 1 - } - Line { - ZOrder 4 - SrcBlock "Get Limits" - 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 "5138" - 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] - 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 "827" - Block { - BlockType Inport - Name "qj" - SID "5139" - Position [15, 53, 45, 67] - ZOrder 552 - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "5140" - Ports [] - Position [217, -25, 237, -5] - ZOrder 553 - } - Block { - BlockType Assertion - Name "Assertion" - SID "5141" - Position [340, 72, 400, 118] - ZOrder 207 - } - Block { - BlockType SubSystem - Name "MATLAB Function" - SID "5142" - Ports [2, 1] - Position [165, 24, 285, 166] - 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 "3770" - Block { - BlockType Inport - Name "u" - SID "5142::1" - Position [20, 101, 40, 119] - ZOrder -3 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "delta_u_max" - SID "5142::18" - Position [20, 136, 40, 154] - ZOrder -1 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "5142::3769" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 119 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "5142::3768" - Tag "Stateflow S-Function torqueBalancingStandup 24" - Ports [2, 2] - Position [180, 105, 230, 165] - ZOrder 118 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "isSpike" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "5142::3770" - Position [460, 241, 480, 259] - ZOrder 120 - } - Block { - BlockType Outport - Name "isSpike" - SID "5142::5" - Position [460, 101, 480, 119] - ZOrder -8 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "u" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "delta_u_max" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "isSpike" - ZOrder 3 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "isSpike" - DstPort 1 - } - Line { - ZOrder 4 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 5 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Constant - Name "index1" - SID "5143" - Position [-40, 124, 100, 136] - ZOrder 554 - ShowName off - Value "Sat.maxJointsPositionDelta" - VectorParams1D off - } - Line { - ZOrder 1 - SrcBlock "qj" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "MATLAB Function" - SrcPort 1 - DstBlock "Assertion" - DstPort 1 - } - Line { - ZOrder 3 - SrcBlock "index1" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 2 - } - } - } - Line { - ZOrder 1 - SrcBlock "ON_GAZEBO\n1" - SrcPort 1 - DstBlock "STOP IF JOINTS HIT THE LIMITS" - DstPort enable - } - Line { - ZOrder 2 - SrcBlock "qj" - 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 SubSystem - Name "synchronizer" - SID "2423" - Ports [] - Position [1702, 513, 1827, 548] - ZOrder 546 - ForegroundColor "red" - BackgroundColor "green" - ShowName off - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 23 - $ClassName "Simulink.Mask" - Display "disp('SYNCHRONIZER')" - } - System { - Name "synchronizer" - 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 "600" - Block { - BlockType SubSystem - Name "GAZEBO_SYNCHRONIZER" - SID "2424" - Ports [0, 0, 1] - Position [5, 15, 130, 75] - ZOrder -7 - RequestExecContextInheritance off - System { - Name "GAZEBO_SYNCHRONIZER" - Location [134, 55, 3840, 2160] - Open off - PortBlocksUseCompactNotation off - 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 EnablePort - Name "Enable" - SID "2425" - Ports [] - Position [177, 85, 197, 105] - ZOrder 538 - } - Block { - BlockType Reference - Name "Simulator Synchronizer" - SID "2426" - Ports [] - Position [120, 24, 250, 61] - ZOrder 539 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" - SourceType "Simulator Synchronizer" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - period "Config.Ts" - serverPortName "'/clock/rpc'" - clientPortName "'/WBT_synchronizer/rpc:o'" - } - } - } - Block { - BlockType Logic - Name "Logical\nOperator" - SID "2427" - Ports [1, 1] - Position [-25, -34, 0, -16] - ZOrder 307 - BlockMirror on - NamePlacement "alternate" - ShowName off - Operator "NOT" - AllPortsSameDT off - OutDataTypeStr "boolean" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n" - SID "2428" - Position [120, -35, 235, -15] - ZOrder 304 - BlockMirror on - NamePlacement "alternate" - ShowName off - Value "Config.ON_GAZEBO" - } - Block { - BlockType SubSystem - Name "REAL_TIME_SYNC" - SID "2429" - Ports [0, 0, 1] - Position [-170, 12, -45, 72] - ZOrder -9 - RequestExecContextInheritance off - System { - Name "REAL_TIME_SYNC" - 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 EnablePort - Name "Enable" - SID "2430" - Ports [] - Position [167, 90, 187, 110] - ZOrder 538 - } - Block { - BlockType Reference - Name "Real Time Synchronizer" - SID "2431" - Ports [] - Position [115, 34, 240, 71] - ZOrder 539 - ForegroundColor "[0.916667, 0.916667, 0.916667]" - BackgroundColor "gray" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" - SourceType "Real Time Synchronizer" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - period "Config.Ts" - } - } - } - Block { - BlockType ToWorkspace - Name "To Workspace" - SID "4827" - Ports [1] - Position [145, -90, 205, -70] - ZOrder 697 - VariableName "yarp_time" - MaxDataPoints "inf" - SaveFormat "Structure With Time" - Save2DSignal "3-D array (concatenate along third dimension)" - FixptAsFi on - SampleTime "-1" - } - Block { - BlockType Reference - Name "Yarp Clock" - SID "4828" - Ports [0, 1] - Position [-45, -89, 20, -71] - ZOrder 696 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Clock" - SourceType "YARP Clock" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - } - Line { - ZOrder 1 - SrcBlock "Logical\nOperator" - SrcPort 1 - Points [-75, 0] - DstBlock "REAL_TIME_SYNC" - DstPort enable - } - Line { - ZOrder 2 - SrcBlock "ON_GAZEBO\n" - SrcPort 1 - Points [-50, 0] - Branch { - ZOrder 6 - DstBlock "GAZEBO_SYNCHRONIZER" - DstPort enable - } - Branch { - ZOrder 4 - DstBlock "Logical\nOperator" - DstPort 1 - } - } - Line { - ZOrder 8 - SrcBlock "Yarp Clock" - SrcPort 1 - DstBlock "To Workspace" - DstPort 1 - } - } - } - Line { - ZOrder 20 - SrcBlock "Saturation" - SrcPort 1 - DstBlock "Set References" - DstPort 1 - } - Line { - ZOrder 139 - SrcBlock "controller_QP" - SrcPort 1 - DstBlock "Saturation" - DstPort 1 - } - Line { - ZOrder 176 - SrcBlock "Dynamics and Kinematics" - SrcPort 1 - DstBlock "controller_QP" - DstPort 1 - } - Line { - ZOrder 177 - SrcBlock "Dynamics and Kinematics" - SrcPort 2 - DstBlock "controller_QP" - DstPort 2 - } - Line { - ZOrder 178 - SrcBlock "Dynamics and Kinematics" - SrcPort 3 - DstBlock "controller_QP" - DstPort 3 - } - Line { - ZOrder 179 - SrcBlock "Dynamics and Kinematics" - SrcPort 4 - DstBlock "controller_QP" - DstPort 4 - } - Line { - ZOrder 180 - SrcBlock "Dynamics and Kinematics" - SrcPort 5 - DstBlock "controller_QP" - DstPort 5 - } - Line { - ZOrder 181 - SrcBlock "Dynamics and Kinematics" - SrcPort 6 - DstBlock "controller_QP" - DstPort 6 - } - Line { - ZOrder 182 - SrcBlock "Dynamics and Kinematics" - SrcPort 7 - DstBlock "controller_QP" - DstPort 7 - } - Line { - ZOrder 183 - SrcBlock "Dynamics and Kinematics" - SrcPort 8 - DstBlock "controller_QP" - DstPort 8 - } - Line { - ZOrder 184 - SrcBlock "Dynamics and Kinematics" - SrcPort 9 - DstBlock "controller_QP" - DstPort 9 - } - Line { - ZOrder 187 - SrcBlock "Dynamics and Kinematics" - SrcPort 10 - DstBlock "controller_QP" - DstPort 10 - } - Line { - ZOrder 186 - SrcBlock "Dynamics and Kinematics" - SrcPort 11 - DstBlock "controller_QP" - DstPort 11 - } - Line { - ZOrder 188 - SrcBlock "Dynamics and Kinematics" - SrcPort 12 - DstBlock "controller_QP" - DstPort 12 - } - Line { - ZOrder 189 - SrcBlock "Dynamics and Kinematics" - SrcPort 13 - DstBlock "controller_QP" - DstPort 13 - } - Line { - ZOrder 194 - SrcBlock "Robot State and References" - SrcPort 2 - Points [38, 0] - Branch { - ZOrder 242 - DstBlock "controller_QP" - DstPort 15 - } - Branch { - ZOrder 239 - Points [0, -225] - Branch { - ZOrder 260 - Points [0, -30] - DstBlock "emergency stop: joint limits" - DstPort 1 - } - Branch { - ZOrder 241 - DstBlock "Dynamics and Kinematics" - DstPort 2 - } - } - } - Line { - ZOrder 195 - SrcBlock "Robot State and References" - SrcPort 3 - Points [58, 0] - Branch { - ZOrder 243 - DstBlock "controller_QP" - DstPort 16 - } - Branch { - ZOrder 211 - Points [0, -170] - DstBlock "Dynamics and Kinematics" - DstPort 3 - } - } - Line { - ZOrder 196 - SrcBlock "Robot State and References" - SrcPort 4 - Points [75, 0] - Branch { - ZOrder 244 - DstBlock "controller_QP" - DstPort 17 - } - Branch { - ZOrder 229 - Points [0, -115] - DstBlock "Dynamics and Kinematics" - DstPort 4 - } - } - Line { - ZOrder 197 - SrcBlock "Robot State and References" - SrcPort 5 - DstBlock "controller_QP" - DstPort 18 - } - Line { - ZOrder 198 - SrcBlock "Robot State and References" - SrcPort 6 - DstBlock "controller_QP" - DstPort 19 - } - Line { - ZOrder 199 - SrcBlock "Robot State and References" - SrcPort 7 - DstBlock "controller_QP" - DstPort 20 - } - Line { - ZOrder 200 - SrcBlock "Robot State and References" - SrcPort 8 - DstBlock "controller_QP" - DstPort 21 - } - Line { - ZOrder 201 - SrcBlock "Robot State and References" - SrcPort 9 - DstBlock "controller_QP" - DstPort 22 - } - Line { - ZOrder 202 - SrcBlock "Robot State and References" - SrcPort 10 - DstBlock "controller_QP" - DstPort 23 - } - Line { - ZOrder 203 - SrcBlock "Robot State and References" - SrcPort 1 - Points [19, 0; 0, -280] - DstBlock "Dynamics and Kinematics" - DstPort 1 - } - Line { - ZOrder 215 - SrcBlock "Dynamics and Kinematics" - SrcPort 14 - DstBlock "controller_QP" - DstPort 14 - } - } -} -#Finite State Machines -# -# Stateflow 80000014 -# -# -Stateflow { - machine { - id 1 - name "torqueBalancingStandup" - created "18-Feb-2014 10:14:40" - isLibrary 0 - sfVersion 80000012 - firstTarget 308 - } - chart { - id 2 - machine 1 - name "controller_QP/Compute angular momentum integral/Compute base to fixed link transform/LFoot to base link" - " transform /fromImuToHomogeousTransformFCN" - windowPosition [326.89 491.339 161 328] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.000677131425054] - treeNode [0 3 0 0] - viewObj 2 - ssIdHighWaterMark 88 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 1 - disableImplicitCasting 1 - eml { - name "worldToBaseTransformWithIMUFCN" - } - firstData 4 - firstTransition 13 - firstJunction 12 - } - 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 w_H_b = worldToBaseTransformWithIMUFCN(imu_H_link,imu_H_link_0,link_H_base,inertial_0,in" - "ertial,neck_pos,Config)\n\n w_H_b = wbc.worldToBaseTransformWithIMU(imu_H_link,imu_H_link_0,link_H_base,inert" - "ial_0,inertial,neck_pos,Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 4 - ssIdNumber 82 - name "imu_H_link" - 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 [2 0 5] - } - data { - id 5 - ssIdNumber 83 - name "imu_H_link_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 [2 4 6] - } - data { - id 6 - ssIdNumber 81 - name "link_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 [2 5 7] - } - data { - id 7 - ssIdNumber 85 - name "inertial_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 [2 6 8] - } - data { - id 8 - ssIdNumber 65 - name "inertial" - 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 [2 7 9] - } - data { - id 9 - 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 [2 8 10] - } - data { - id 10 - 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 [2 9 11] - } - data { - id 11 - 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 [2 10 0] - } - junction { - id 12 - position [23.5747 49.5747 7] - chart 2 - subviewer 2 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [2 0 0] - } - transition { - id 13 - 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 12 - 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 14 - machine 1 - name "controller_QP/Compute angular momentum integral/Compute base to fixed link transform/LFoot to base link" - " transform /fromImuToHomogeousTransformFCN" - chart 2 - } - chart { - id 15 - machine 1 - name "Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot to base link transform /fr" - "omImuToHomogeousTransformFCN" - windowPosition [326.89 491.339 161 328] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.000677131425054] - treeNode [0 16 0 0] - viewObj 15 - ssIdHighWaterMark 88 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 2 - disableImplicitCasting 1 - eml { - name "worldToBaseTransformWithIMUFCN" - } - firstData 17 - firstTransition 26 - firstJunction 25 - } - state { - id 16 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 15 - treeNode [15 0 0 0] - superState SUBCHART - subviewer 15 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function w_H_b = worldToBaseTransformWithIMUFCN(imu_H_link,imu_H_link_0,link_H_base,inertial_0,in" - "ertial,neck_pos,Config)\n\n w_H_b = wbc.worldToBaseTransformWithIMU(imu_H_link,imu_H_link_0,link_H_base,inert" - "ial_0,inertial,neck_pos,Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 17 - ssIdNumber 82 - name "imu_H_link" - 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 [15 0 18] - } - data { - id 18 - ssIdNumber 83 - name "imu_H_link_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 [15 17 19] - } - data { - id 19 - ssIdNumber 81 - name "link_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 [15 18 20] - } - data { - id 20 - ssIdNumber 85 - name "inertial_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 [15 19 21] - } - data { - id 21 - ssIdNumber 65 - name "inertial" - 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 [15 20 22] - } - data { - id 22 - 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 [15 21 23] - } - data { - id 23 - 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 [15 22 24] - } - data { - id 24 - 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 [15 23 0] - } - junction { - id 25 - position [23.5747 49.5747 7] - chart 15 - subviewer 15 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [15 0 0] - } - transition { - id 26 - 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 25 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 15 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 15 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [15 0 0] - } - instance { - id 27 - machine 1 - name "Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot to base link transform /fr" - "omImuToHomogeousTransformFCN" - chart 15 - } - chart { - id 28 - machine 1 - name "Robot State and References/State Machine Standup/Compute base to fixed link transform/LLeg to base link" - " transform/fromImuToHomogeousTransformFCN" - windowPosition [326.89 491.339 161 328] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.000677131425054] - treeNode [0 29 0 0] - viewObj 28 - ssIdHighWaterMark 88 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 3 - disableImplicitCasting 1 - eml { - name "worldToBaseTransformWithIMUFCN" - } - firstData 30 - firstTransition 39 - firstJunction 38 - } - state { - id 29 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 28 - treeNode [28 0 0 0] - superState SUBCHART - subviewer 28 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function w_H_b = worldToBaseTransformWithIMUFCN(imu_H_link,imu_H_link_0,link_H_base,inertial_0,in" - "ertial,neck_pos,Config)\n\n w_H_b = wbc.worldToBaseTransformWithIMU(imu_H_link,imu_H_link_0,link_H_base,inert" - "ial_0,inertial,neck_pos,Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 30 - ssIdNumber 82 - name "imu_H_link" - 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 [28 0 31] - } - data { - id 31 - ssIdNumber 83 - name "imu_H_link_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 [28 30 32] - } - data { - id 32 - ssIdNumber 81 - name "link_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 [28 31 33] - } - data { - id 33 - ssIdNumber 85 - name "inertial_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 [28 32 34] - } - data { - id 34 - ssIdNumber 65 - name "inertial" - 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 [28 33 35] - } - data { - id 35 - 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 [28 34 36] - } - data { - id 36 - 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 [28 35 37] - } - data { - id 37 - 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 [28 36 0] - } - junction { - id 38 - position [23.5747 49.5747 7] - chart 28 - subviewer 28 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [28 0 0] - } - transition { - id 39 - 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 38 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 28 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 28 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [28 0 0] - } - instance { - id 40 - machine 1 - name "Robot State and References/State Machine Standup/Compute base to fixed link transform/LLeg to base link" - " transform/fromImuToHomogeousTransformFCN" - chart 28 - } - chart { - id 41 - machine 1 - name "controller_QP/Compute angular momentum integral/Compute base to fixed link transform/LLeg to base link " - "transform/fromImuToHomogeousTransformFCN" - windowPosition [326.89 491.339 161 328] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.000677131425054] - treeNode [0 42 0 0] - viewObj 41 - ssIdHighWaterMark 88 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 4 - disableImplicitCasting 1 - eml { - name "worldToBaseTransformWithIMUFCN" - } - firstData 43 - firstTransition 52 - firstJunction 51 - } - state { - id 42 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 41 - treeNode [41 0 0 0] - superState SUBCHART - subviewer 41 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function w_H_b = worldToBaseTransformWithIMUFCN(imu_H_link,imu_H_link_0,link_H_base,inertial_0,in" - "ertial,neck_pos,Config)\n\n w_H_b = wbc.worldToBaseTransformWithIMU(imu_H_link,imu_H_link_0,link_H_base,inert" - "ial_0,inertial,neck_pos,Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 43 - ssIdNumber 82 - name "imu_H_link" - 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 [41 0 44] - } - data { - id 44 - ssIdNumber 83 - name "imu_H_link_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 [41 43 45] - } - data { - id 45 - ssIdNumber 81 - name "link_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 [41 44 46] - } - data { - id 46 - ssIdNumber 85 - name "inertial_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 [41 45 47] - } - data { - id 47 - ssIdNumber 65 - name "inertial" - 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 [41 46 48] - } - data { - id 48 - 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 [41 47 49] - } - data { - id 49 - 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 [41 48 50] - } - data { - id 50 - 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 [41 49 0] - } - junction { - id 51 - position [23.5747 49.5747 7] - chart 41 - subviewer 41 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [41 0 0] - } - transition { - id 52 - 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 51 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 41 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 41 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [41 0 0] - } - instance { - id 53 - machine 1 - name "controller_QP/Compute angular momentum integral/Compute base to fixed link transform/LLeg to base link " - "transform/fromImuToHomogeousTransformFCN" - chart 41 - } - chart { - id 54 - machine 1 - name "Robot State and References/Internal Coordinator/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 55 0 0] - viewObj 54 - ssIdHighWaterMark 10 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 5 - disableImplicitCasting 1 - eml { - name "computeBaseVelocityFCN" - } - firstData 56 - firstTransition 63 - firstJunction 62 - } - state { - id 55 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 54 - treeNode [54 0 0 0] - superState SUBCHART - subviewer 54 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function nu_b = computeBaseVelocityFCN(J_LeftFoot, J_RightFoot, feetInContact, qjDot, Reg)\n\n " - " nu_b = wbc.computeBaseVelocity(J_LeftFoot, J_RightFoot, feetInContact, qjDot, Reg);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 56 - ssIdNumber 4 - name "J_LeftFoot" - 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 [54 0 57] - } - data { - id 57 - ssIdNumber 8 - name "J_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 [54 56 58] - } - data { - id 58 - ssIdNumber 9 - name "feetInContact" - 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 [54 57 59] - } - data { - id 59 - 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 [54 58 60] - } - data { - id 60 - ssIdNumber 6 - name "qjDot" - 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 [54 59 61] - } - data { - id 61 - 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 [54 60 0] - } - junction { - id 62 - position [23.5747 49.5747 7] - chart 54 - subviewer 54 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [54 0 0] - } - transition { - id 63 - 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 62 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 54 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 54 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [54 0 0] - } - instance { - id 64 - machine 1 - name "Robot State and References/Internal Coordinator/Compute State Velocity/Compute Base Velocity" - chart 54 - } - chart { - id 65 - machine 1 - name "Dynamics and Kinematics/Dynamics/Add motors reflected inertias/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 66 0 0] - viewObj 65 - ssIdHighWaterMark 6 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 6 - disableImplicitCasting 1 - eml { - name "addMotorsInertiaFCN" - } - firstData 67 - firstTransition 71 - firstJunction 70 - } - state { - id 66 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 65 - treeNode [65 0 0 0] - superState SUBCHART - subviewer 65 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function M_with_inertia = addMotorsInertiaFCN(M,Config)\n\n M_with_inertia = wbc.addMotorsIner" - "tia(M,Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 67 - 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 [65 0 68] - } - data { - id 68 - 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 [65 67 69] - } - data { - id 69 - 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 [65 68 0] - } - junction { - id 70 - position [23.5747 49.5747 7] - chart 65 - subviewer 65 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [65 0 0] - } - transition { - id 71 - 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 70 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 65 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 65 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [65 0 0] - } - instance { - id 72 - machine 1 - name "Dynamics and Kinematics/Dynamics/Add motors reflected inertias/Add motor reflected inertias" - chart 65 - } - chart { - id 73 - machine 1 - name "Robot State and References/State Machine Standup/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 74 0 0] - viewObj 73 - ssIdHighWaterMark 10 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 7 - disableImplicitCasting 1 - eml { - name "computeBaseVelocityFCN" - } - firstData 75 - firstTransition 82 - firstJunction 81 - } - state { - id 74 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 73 - treeNode [73 0 0 0] - superState SUBCHART - subviewer 73 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function nu_b = computeBaseVelocityFCN(J_LeftFixedLink, J_RightFixedLink, feetInContact, qjDot, R" - "eg)\n\n nu_b = wbc.computeBaseVelocity(J_LeftFixedLink, J_RightFixedLink, feetInContact, qjDot, Reg);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 75 - ssIdNumber 4 - name "J_LeftFixedLink" - 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 [73 0 76] - } - data { - id 76 - ssIdNumber 8 - name "J_RightFixedLink" - 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 [73 75 77] - } - data { - id 77 - ssIdNumber 9 - name "feetInContact" - 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 [73 76 78] - } - data { - id 78 - 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 [73 77 79] - } - data { - id 79 - ssIdNumber 6 - name "qjDot" - 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 [73 78 80] - } - data { - id 80 - 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 [73 79 0] - } - junction { - id 81 - position [23.5747 49.5747 7] - chart 73 - subviewer 73 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [73 0 0] - } - transition { - id 82 - 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 81 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 73 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 73 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [73 0 0] - } - instance { - id 83 - machine 1 - name "Robot State and References/State Machine Standup/Compute State Velocity/Compute Base Velocity" - chart 73 - } - chart { - id 84 - machine 1 - name "Robot State and References/State Machine Standup/Compute base to fixed link transform/LFoot to base lin" - "k transform /fromImuToHomogeousTransformFCN" - windowPosition [326.89 491.339 161 328] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.000677131425054] - treeNode [0 85 0 0] - viewObj 84 - ssIdHighWaterMark 88 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 8 - disableImplicitCasting 1 - eml { - name "worldToBaseTransformWithIMUFCN" - } - firstData 86 - firstTransition 95 - firstJunction 94 - } - state { - id 85 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 84 - treeNode [84 0 0 0] - superState SUBCHART - subviewer 84 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function w_H_b = worldToBaseTransformWithIMUFCN(imu_H_link,imu_H_link_0,link_H_base,inertial_0,in" - "ertial,neck_pos,Config)\n\n w_H_b = wbc.worldToBaseTransformWithIMU(imu_H_link,imu_H_link_0,link_H_base,inert" - "ial_0,inertial,neck_pos,Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 86 - ssIdNumber 82 - name "imu_H_link" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [84 0 87] - } - data { - id 87 - ssIdNumber 83 - name "imu_H_link_0" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [84 86 88] - } - data { - id 88 - ssIdNumber 81 - name "link_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 [84 87 89] - } - data { - id 89 - ssIdNumber 85 - name "inertial_0" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [84 88 90] - } - data { - id 90 - ssIdNumber 65 - name "inertial" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [84 89 91] - } - data { - id 91 - 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 [84 90 92] - } - data { - id 92 - 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 [84 91 93] - } - data { - id 93 - 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 [84 92 0] - } - junction { - id 94 - position [23.5747 49.5747 7] - chart 84 - subviewer 84 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [84 0 0] - } - transition { - id 95 - 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 94 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 84 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 84 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [84 0 0] - } - instance { - id 96 - machine 1 - name "Robot State and References/State Machine Standup/Compute base to fixed link transform/LFoot to base lin" - "k transform /fromImuToHomogeousTransformFCN" - chart 84 - } - chart { - id 97 - machine 1 - name "Dynamics and Kinematics/Kinematics/Legs in Contact" - windowPosition [422 539.941 189 413] - viewLimits [0 156.75 0 153.75] - screen [1 1 3600 1200 1.180555555555556] - treeNode [0 98 0 0] - viewObj 97 - ssIdHighWaterMark 6 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 9 - disableImplicitCasting 1 - eml { - name "legsContactDetectorFCN" - } - firstData 99 - firstTransition 103 - firstJunction 102 - } - state { - id 98 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 97 - treeNode [97 0 0 0] - superState SUBCHART - subviewer 97 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function legsInContact = legsContactDetectorFCN(icubStandup,state)\n\n legsInContact = legsCont" - "actDetector(icubStandup,state);\nend\n \n " - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 99 - ssIdNumber 4 - name "icubStandup" - 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 [97 0 100] - } - data { - id 100 - ssIdNumber 5 - name "legsInContact" - 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 [97 99 101] - } - data { - id 101 - ssIdNumber 6 - name "state" - 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 [97 100 0] - } - junction { - id 102 - position [23.5747 49.5747 7] - chart 97 - subviewer 97 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [97 0 0] - } - transition { - id 103 - 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 102 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 97 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 97 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [97 0 0] - } - instance { - id 104 - machine 1 - name "Dynamics and Kinematics/Kinematics/Legs in Contact" - chart 97 - } - chart { - id 105 - machine 1 - name "Robot State and References/Internal Coordinator/Reference Generator CoM" - windowPosition [319.546 -65.92 97 534.4] - viewLimits [0 112 0 601] - screen [1 1 1366 768 1.25] - treeNode [0 106 0 0] - viewObj 105 - ssIdHighWaterMark 19 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 10 - disableImplicitCasting 1 - eml { - name "referenceGeneratorCoMFCN" - } - supportVariableSizing 0 - firstData 107 - firstTransition 112 - firstJunction 111 - } - state { - id 106 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 105 - treeNode [105 0 0 0] - superState SUBCHART - subviewer 105 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function references_CoM = referenceGeneratorCoMFCN(pos_CoM_0, t, Config)\n\n references_CoM = " - "wbc.referenceGeneratorCoM(pos_CoM_0, t, Config);\nend\n" - 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 107 - ssIdNumber 17 - 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 [105 0 108] - } - data { - id 108 - ssIdNumber 13 - name "t" - 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 [105 107 109] - } - data { - id 109 - ssIdNumber 4 - name "references_CoM" - 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 [105 108 110] - } - data { - id 110 - 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 [105 109 0] - } - junction { - id 111 - position [23.5747 49.5747 7] - chart 105 - subviewer 105 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [105 0 0] - } - transition { - id 112 - labelString "{eML_blk_kernel();}" - labelPosition [40.125 31.875 104 16.042] - fontSize 12 - src { - intersection [0 0 1 0 23.5747 14.625 0 0] - } - dst { - id 111 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 105 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 105 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [105 0 0] - } - instance { - id 113 - machine 1 - name "Robot State and References/Internal Coordinator/Reference Generator CoM" - chart 105 - } - chart { - id 114 - machine 1 - name "controller_QP/Compute angular momentum integral/Legs in Contact" - windowPosition [422 539.941 189 413] - viewLimits [0 156.75 0 153.75] - screen [1 1 3600 1200 1.180555555555556] - treeNode [0 115 0 0] - viewObj 114 - ssIdHighWaterMark 6 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 11 - disableImplicitCasting 1 - eml { - name "legsContactDetectorFCN" - } - firstData 116 - firstTransition 120 - firstJunction 119 - } - state { - id 115 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 114 - treeNode [114 0 0 0] - superState SUBCHART - subviewer 114 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function legsInContact = legsContactDetectorFCN(icubStandup,state)\n\n legsInContact = legsCont" - "actDetector(icubStandup,state);\nend\n \n " - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 116 - ssIdNumber 4 - name "icubStandup" - 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 [114 0 117] - } - data { - id 117 - ssIdNumber 5 - name "legsInContact" - 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 [114 116 118] - } - data { - id 118 - ssIdNumber 6 - name "state" - 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 [114 117 0] - } - junction { - id 119 - position [23.5747 49.5747 7] - chart 114 - subviewer 114 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [114 0 0] - } - transition { - id 120 - labelString "{eML_blk_kernel();}" - labelPosition [28.125 13.875 102.544 14.964] - fontSize 12 - src { - intersection [0 0 1 0 23.5747 14.625 0 0] - } - dst { - id 119 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 114 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 114 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [114 0 0] - } - instance { - id 121 - machine 1 - name "controller_QP/Compute angular momentum integral/Legs in Contact" - chart 114 - } - chart { - id 122 - machine 1 - name "controller_QP/Compute joint torques (motor reflected inertia)/(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 123 0 0] - viewObj 122 - ssIdHighWaterMark 5 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 12 - disableImplicitCasting 1 - eml { - name "computeMotorsInertiaFCN" - } - firstData 124 - firstTransition 127 - firstJunction 126 - } - state { - id 123 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 122 - treeNode [122 0 0 0] - superState SUBCHART - subviewer 122 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function reflectedInertia = computeMotorsInertiaFCN(Config)\n\n [reflectedInertia] = wbc.comp" - "uteMotorsInertia(Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 124 - 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 [122 0 125] - } - data { - id 125 - 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 [122 124 0] - } - junction { - id 126 - position [23.5747 49.5747 7] - chart 122 - subviewer 122 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [122 0 0] - } - transition { - id 127 - labelString "{eML_blk_kernel();}" - labelPosition [28.125 13.875 102.544 14.964] - fontSize 12 - src { - intersection [0 0 1 0 23.5747 14.625 0 0] - } - dst { - id 126 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 122 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 122 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [122 0 0] - } - instance { - id 128 - machine 1 - name "controller_QP/Compute joint torques (motor reflected inertia)/(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{" - "-1}" - chart 122 - } - chart { - id 129 - machine 1 - name "Robot State and References/State Machine Standup/stateMachineStandupFCN" - windowPosition [326.89 491.339 161 328] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.000677131425054] - treeNode [0 130 0 0] - viewObj 129 - ssIdHighWaterMark 84 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 13 - disableImplicitCasting 1 - eml { - name "stateMachineStandupFCN" - } - firstData 131 - firstTransition 154 - firstJunction 153 - } - state { - id 130 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 129 - treeNode [129 0 0 0] - superState SUBCHART - subviewer 129 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function [w_H_b, constraints, CoM_des, qj_des, impedances, KPCoM, KDCoM, currentState, jointsAndC" - "oMSmoothingTime] = ...\n stateMachineStandupFCN(wrench_rightFoot, wrench_leftFoot, wrench_leftHand, " - "wrench_rightHand, xCoM_0, qj_0, xCoM, l_sole_H_b, l_upper_leg_contact_H_b, t, STANDUP_WITH_HUMAN, Sm, Gain)\n " - " \n [w_H_b, constraints, CoM_des, qj_des, impedances, KPCoM, KDCoM, currentState, jointsAndCoMSmoot" - "hingTime] = ...\n stateMachineStandup(wrench_rightFoot, wrench_leftFoot, wrench_leftHand, wrench_rig" - "htHand, xCoM_0, qj_0, xCoM, l_sole_H_b, l_upper_leg_contact_H_b, t, STANDUP_WITH_HUMAN, Sm, Gain);\nend\n \n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 131 - 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 [129 0 132] - } - data { - id 132 - 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 [129 131 133] - } - data { - id 133 - 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 [129 132 134] - } - data { - id 134 - ssIdNumber 43 - name "constraints" - scope OUTPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_NO - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [129 133 135] - } - data { - id 135 - ssIdNumber 52 - name "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 [129 134 136] - } - data { - id 136 - ssIdNumber 46 - name "qj_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 [129 135 137] - } - data { - id 137 - ssIdNumber 37 - name "wrench_leftHand" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [129 136 138] - } - data { - id 138 - ssIdNumber 54 - name "wrench_rightHand" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [129 137 139] - } - data { - id 139 - ssIdNumber 64 - name "xCoM_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 [129 138 140] - } - data { - id 140 - ssIdNumber 77 - name "qj_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 [129 139 141] - } - data { - id 141 - ssIdNumber 69 - name "xCoM" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [129 140 142] - } - data { - id 142 - 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 [129 141 143] - } - data { - id 143 - ssIdNumber 78 - name "l_upper_leg_contact_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 [129 142 144] - } - data { - id 144 - ssIdNumber 56 - name "t" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [129 143 145] - } - data { - id 145 - ssIdNumber 84 - name "STANDUP_WITH_HUMAN" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [129 144 146] - } - data { - id 146 - ssIdNumber 55 - name "Sm" - scope PARAMETER_DATA - paramIndexForInitFromWorkspace 1 - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [129 145 147] - } - data { - id 147 - ssIdNumber 66 - name "impedances" - scope OUTPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_NO - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [129 146 148] - } - data { - id 148 - ssIdNumber 82 - name "KPCoM" - scope OUTPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_NO - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [129 147 149] - } - data { - id 149 - ssIdNumber 83 - name "KDCoM" - scope OUTPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_NO - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [129 148 150] - } - data { - id 150 - ssIdNumber 67 - name "Gain" - scope PARAMETER_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [129 149 151] - } - data { - id 151 - ssIdNumber 68 - name "currentState" - scope OUTPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_NO - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [129 150 152] - } - data { - id 152 - ssIdNumber 81 - name "jointsAndCoMSmoothingTime" - scope OUTPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_NO - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [129 151 0] - } - junction { - id 153 - position [23.5747 49.5747 7] - chart 129 - subviewer 129 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [129 0 0] - } - transition { - id 154 - 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 153 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 129 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 129 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [129 0 0] - } - instance { - id 155 - machine 1 - name "Robot State and References/State Machine Standup/stateMachineStandupFCN" - chart 129 - } - chart { - id 156 - machine 1 - name "Robot State and References/State Machine Standup/Compute State Velocity/Fixed Links Jacobians/Legs in C" - "ontact" - windowPosition [422 539.941 189 413] - viewLimits [0 156.75 0 153.75] - screen [1 1 3600 1200 1.180555555555556] - treeNode [0 157 0 0] - viewObj 156 - ssIdHighWaterMark 6 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 14 - disableImplicitCasting 1 - eml { - name "legsContactDetectorFCN" - } - firstData 158 - firstTransition 162 - firstJunction 161 - } - state { - id 157 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 156 - treeNode [156 0 0 0] - superState SUBCHART - subviewer 156 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function legsInContact = legsContactDetectorFCN(icubStandup,state)\n\n legsInContact = legsCont" - "actDetector(icubStandup,state);\nend\n \n " - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 158 - ssIdNumber 4 - name "icubStandup" - 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 [156 0 159] - } - data { - id 159 - ssIdNumber 5 - name "legsInContact" - 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 [156 158 160] - } - data { - id 160 - ssIdNumber 6 - name "state" - 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 [156 159 0] - } - junction { - id 161 - position [23.5747 49.5747 7] - chart 156 - subviewer 156 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [156 0 0] - } - transition { - id 162 - 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 161 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 156 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 156 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [156 0 0] - } - instance { - id 163 - machine 1 - name "Robot State and References/State Machine Standup/Compute State Velocity/Fixed Links Jacobians/Legs in C" - "ontact" - chart 156 - } - chart { - id 164 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/ContactsTransition" - windowPosition [422 539.941 189 413] - viewLimits [0 156.75 0 153.75] - screen [1 1 3600 1200 1.180555555555556] - treeNode [0 165 0 0] - viewObj 164 - ssIdHighWaterMark 5 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 15 - disableImplicitCasting 1 - eml { - name "contactsTransitionQPFCN" - } - firstData 166 - firstTransition 169 - firstJunction 168 - } - state { - id 165 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 164 - treeNode [164 0 0 0] - superState SUBCHART - subviewer 164 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function onOneFoot = contactsTransitionQPFCN(LR_FootInContact)\n\n onOneFoot = wbc.contactsTra" - "nsitionQP(LR_FootInContact);\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 166 - ssIdNumber 4 - name "LR_FootInContact" - 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 [164 0 167] - } - data { - id 167 - 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 [164 166 0] - } - junction { - id 168 - position [23.5747 49.5747 7] - chart 164 - subviewer 164 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [164 0 0] - } - transition { - id 169 - 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 168 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 164 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 164 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [164 0 0] - } - instance { - id 170 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/ContactsTransition" - chart 164 - } - chart { - id 171 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/Process One Foot Output" - windowPosition [422 539.941 189 413] - viewLimits [0 156.75 0 153.75] - screen [1 1 3600 1200 1.180555555555556] - treeNode [0 172 0 0] - viewObj 171 - ssIdHighWaterMark 6 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 16 - disableImplicitCasting 1 - eml { - name "processOneFootOutputQPFCN" - } - firstData 173 - firstTransition 177 - firstJunction 176 - } - state { - id 172 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 171 - treeNode [171 0 0 0] - superState SUBCHART - subviewer 171 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function f0_oneFoot = processOneFootOutputQPFCN(primalSolution,LR_FootInContact)\n\n f0_oneFoo" - "t = wbc.processOneFootOutputQP(primalSolution,LR_FootInContact);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 173 - 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 [171 0 174] - } - data { - id 174 - ssIdNumber 5 - name "f0_oneFoot" - scope OUTPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_NO - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [171 173 175] - } - data { - id 175 - ssIdNumber 6 - name "LR_FootInContact" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [171 174 0] - } - junction { - id 176 - position [23.5747 49.5747 7] - chart 171 - subviewer 171 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [171 0 0] - } - transition { - id 177 - 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 176 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 171 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 171 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [171 0 0] - } - instance { - id 178 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/Process One Foot Output" - chart 171 - } - chart { - id 179 - machine 1 - name "controller_QP/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 180 0 0] - viewObj 179 - ssIdHighWaterMark 91 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 17 - disableImplicitCasting 1 - eml { - name "balancingControllerStandupFCN" - } - firstData 181 - firstTransition 230 - firstJunction 229 - } - state { - id 180 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 179 - treeNode [179 0 0 0] - superState SUBCHART - subviewer 179 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function [tauModel, Sigma, NA, f_LDot, ...\n HessianMatrixQP1Foot, gradientQP1Foot, Cons" - "traintsMatrixQP1Foot, bVectorConstraintsQp1Foot, ...\n HessianMatrixQP2FeetOrLegs, gradientQP2FeetOrLeg" - "s, ConstraintsMatrixQP2FeetOrLegs, bVectorConstraintsQp2FeetOrLegs, ...\n errorCoM, f_noQP, correctionF" - "romSupportForce, L_error, V] = ...\n balancingControllerStandupFCN(constraints, ROBOT_DOF_FOR_SIMU" - "LINK, ConstraintsMatrix, bVectorConstraints, ...\n qj, qjDes, nu, M, " - "h, L, intLw, w_H_l_contact, w_H_r_contact, JL, JR, dJL_nu, dJR_nu, xCoM, J_CoM, desired_x_dx_ddx_CoM, ...\n " - " gainsPCOM, gainsDCOM, impedances, Reg, Gain, w_H_lArm, w_H_rArm, LArmWren" - "ch, RArmWrench, STANDUP_WITH_HUMAN, state)\n \n [tauModel, Sigma, NA, f_LDot, ...\n Hessian" - "MatrixQP1Foot, gradientQP1Foot, ConstraintsMatrixQP1Foot, bVectorConstraintsQp1Foot, ...\n HessianMatri" - "xQP2FeetOrLegs, gradientQP2FeetOrLegs, ConstraintsMatrixQP2FeetOrLegs, bVectorConstraintsQp2FeetOrLegs, ...\n " - " errorCoM, f_noQP, correctionFromSupportForce, L_error, V] = ...\n balancingControllerStandu" - "p(constraints, ROBOT_DOF_FOR_SIMULINK, ConstraintsMatrix, bVectorConstraints, ...\n " - " qj, qjDes, nu, M, h, L, intLw, w_H_l_contact, w_H_r_contact, JL, JR, dJL_nu, dJR_nu, xCoM, J_CoM, de" - "sired_x_dx_ddx_CoM, ...\n gainsPCOM, gainsDCOM, impedances, Reg, Gain, w" - "_H_lArm, w_H_rArm, LArmWrench, RArmWrench, STANDUP_WITH_HUMAN, state);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 181 - 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 [179 0 182] - } - data { - id 182 - 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 [179 181 183] - } - data { - id 183 - ssIdNumber 66 - 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 [179 182 184] - } - data { - id 184 - ssIdNumber 64 - 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 [179 183 185] - } - data { - id 185 - ssIdNumber 5 - name "HessianMatrixQP1Foot" - 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 [179 184 186] - } - data { - id 186 - ssIdNumber 52 - name "gradientQP1Foot" - 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 [179 185 187] - } - data { - id 187 - ssIdNumber 69 - name "ConstraintsMatrixQP1Foot" - 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 [179 186 188] - } - data { - id 188 - ssIdNumber 70 - name "bVectorConstraintsQp1Foot" - 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 [179 187 189] - } - data { - id 189 - ssIdNumber 53 - name "HessianMatrixQP2FeetOrLegs" - 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 [179 188 190] - } - data { - id 190 - ssIdNumber 54 - name "gradientQP2FeetOrLegs" - 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 [179 189 191] - } - data { - id 191 - ssIdNumber 62 - name "ConstraintsMatrixQP2FeetOrLegs" - 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 [179 190 192] - } - data { - id 192 - ssIdNumber 63 - name "bVectorConstraintsQp2FeetOrLegs" - 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 [179 191 193] - } - data { - id 193 - ssIdNumber 13 - name "constraints" - 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 [179 192 194] - } - data { - id 194 - ssIdNumber 14 - name "ROBOT_DOF_FOR_SIMULINK" - 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 [179 193 195] - } - data { - id 195 - 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 [179 194 196] - } - data { - id 196 - 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 [179 195 197] - } - data { - id 197 - ssIdNumber 4 - name "qj" - 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 [179 196 198] - } - data { - id 198 - ssIdNumber 6 - name "qjDes" - 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 [179 197 199] - } - data { - id 199 - 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 [179 198 200] - } - data { - id 200 - 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 [179 199 201] - } - data { - id 201 - 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 [179 200 202] - } - data { - id 202 - 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 [179 201 203] - } - data { - id 203 - ssIdNumber 77 - name "intLw" - 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 [179 202 204] - } - data { - id 204 - ssIdNumber 42 - name "w_H_l_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 [179 203 205] - } - data { - id 205 - ssIdNumber 12 - name "w_H_r_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 [179 204 206] - } - data { - id 206 - ssIdNumber 38 - 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 [179 205 207] - } - data { - id 207 - ssIdNumber 32 - 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 [179 206 208] - } - data { - id 208 - ssIdNumber 33 - name "dJL_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 [179 207 209] - } - data { - id 209 - ssIdNumber 40 - name "dJR_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 [179 208 210] - } - data { - id 210 - ssIdNumber 15 - name "xCoM" - 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 [179 209 211] - } - data { - id 211 - 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 [179 210 212] - } - data { - id 212 - ssIdNumber 17 - name "desired_x_dx_ddx_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 [179 211 213] - } - data { - id 213 - ssIdNumber 81 - name "gainsPCOM" - 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 [179 212 214] - } - data { - id 214 - ssIdNumber 82 - name "gainsDCOM" - 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 [179 213 215] - } - data { - id 215 - ssIdNumber 19 - name "impedances" - 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 [179 214 216] - } - data { - id 216 - ssIdNumber 20 - name "Reg" - 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 [179 215 217] - } - data { - id 217 - ssIdNumber 48 - name "errorCoM" - 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 [179 216 218] - } - data { - id 218 - ssIdNumber 49 - name "f_noQP" - 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 [179 217 219] - } - data { - id 219 - ssIdNumber 47 - name "Gain" - 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 [179 218 220] - } - data { - id 220 - ssIdNumber 83 - name "w_H_lArm" - 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 [179 219 221] - } - data { - id 221 - ssIdNumber 84 - name "w_H_rArm" - 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 [179 220 222] - } - data { - id 222 - ssIdNumber 85 - name "LArmWrench" - 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 [179 221 223] - } - data { - id 223 - ssIdNumber 86 - name "RArmWrench" - 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 [179 222 224] - } - data { - id 224 - ssIdNumber 87 - name "STANDUP_WITH_HUMAN" - 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 [179 223 225] - } - data { - id 225 - ssIdNumber 88 - name "correctionFromSupportForce" - 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 [179 224 226] - } - data { - id 226 - ssIdNumber 89 - name "L_error" - 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 [179 225 227] - } - data { - id 227 - ssIdNumber 90 - name "V" - 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 [179 226 228] - } - data { - id 228 - ssIdNumber 91 - name "state" - 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 [179 227 0] - } - junction { - id 229 - position [23.5747 49.5747 7] - chart 179 - subviewer 179 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [179 0 0] - } - transition { - id 230 - 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 229 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 179 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 179 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [179 0 0] - } - instance { - id 231 - machine 1 - name "controller_QP/Balancing Controller\n" - chart 179 - } - chart { - id 232 - 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 233 0 0] - viewObj 232 - ssIdHighWaterMark 8 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 18 - disableImplicitCasting 1 - eml { - name "checkRangeFCN" - } - supportVariableSizing 0 - firstData 234 - firstTransition 240 - firstJunction 239 - } - state { - id 233 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 232 - treeNode [232 0 0 0] - superState SUBCHART - subviewer 232 - 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" - 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 234 - 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 [232 0 235] - } - data { - id 235 - 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 [232 234 236] - } - data { - id 236 - 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 [232 235 237] - } - data { - id 237 - 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 [232 236 238] - } - data { - id 238 - 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 [232 237 0] - } - junction { - id 239 - position [23.5747 49.5747 7] - chart 232 - subviewer 232 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [232 0 0] - } - transition { - id 240 - 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 239 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 232 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 232 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [232 0 0] - } - instance { - id 241 - machine 1 - name "emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" - chart 232 - } - chart { - id 242 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/One Foot/Process QP output" - windowPosition [422 539.941 189 413] - viewLimits [0 156.75 0 153.75] - screen [1 1 3600 1200 1.180555555555556] - treeNode [0 243 0 0] - viewObj 242 - ssIdHighWaterMark 13 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 19 - disableImplicitCasting 1 - eml { - name "processOutputQPFCN" - } - firstData 244 - firstTransition 250 - firstJunction 249 - } - state { - id 243 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 242 - treeNode [242 0 0 0] - superState SUBCHART - subviewer 242 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function f0 = processOutputQPFCN(analyticalSolution,primalSolution,QPStatus,Config)\n\n f0 = " - "wbc.processOutputQP(analyticalSolution,primalSolution,QPStatus,Config);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 244 - 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 [242 0 245] - } - data { - id 245 - 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 [242 244 246] - } - data { - id 246 - 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 [242 245 247] - } - data { - id 247 - ssIdNumber 5 - name "f0" - 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 [242 246 248] - } - data { - id 248 - 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 [242 247 0] - } - junction { - id 249 - position [23.5747 49.5747 7] - chart 242 - subviewer 242 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [242 0 0] - } - transition { - id 250 - 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 249 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 242 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 242 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [242 0 0] - } - instance { - id 251 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/One Foot/Process QP output" - chart 242 - } - chart { - id 252 - machine 1 - name "controller_QP/Compute angular momentum integral/References for L" - windowPosition [357.12 483.496 167 391] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.041666666666667] - treeNode [0 253 0 0] - viewObj 252 - ssIdHighWaterMark 10 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 20 - disableImplicitCasting 1 - eml { - name "generateReferencesForH_FCN" - } - firstData 254 - firstTransition 261 - firstJunction 260 - } - state { - id 253 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 252 - treeNode [252 0 0 0] - superState SUBCHART - subviewer 252 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function nu_b_equivalent = generateReferencesForH_FCN(qjErr, JL, JR, Reg, activeFeetConstraints)\n" - "\n if activeFeetConstraints(1) == 1\n \n pinvJb = (JL(:,1:6)'*JL(:,1:6) + Reg.pinvDamp_nu_" - "b*eye(6))\\JL(:,1:6)';\n nu_b_equivalent = -pinvJb*JL(:,7:end)*qjErr;\n else\n \n pinvJb " - " = (JR(:,1:6)'*JR(:,1:6) + Reg.pinvDamp_nu_b*eye(6))\\JR(:,1:6)';\n nu_b_equivalent = -pinvJb*JR(:,7" - ":end)*qjErr;\n end\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 254 - ssIdNumber 4 - name "qjErr" - 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 [252 0 255] - } - data { - id 255 - ssIdNumber 6 - 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 [252 254 256] - } - data { - id 256 - ssIdNumber 9 - 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 [252 255 257] - } - data { - id 257 - ssIdNumber 7 - name "nu_b_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 [252 256 258] - } - data { - id 258 - 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 [252 257 259] - } - data { - id 259 - ssIdNumber 10 - name "activeFeetConstraints" - 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 [252 258 0] - } - junction { - id 260 - position [23.5747 49.5747 7] - chart 252 - subviewer 252 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [252 0 0] - } - transition { - id 261 - 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 260 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 252 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 252 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [252 0 0] - } - instance { - id 262 - machine 1 - name "controller_QP/Compute angular momentum integral/References for L" - chart 252 - } - chart { - id 263 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/Two Feet/Analytical 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 264 0 0] - viewObj 263 - ssIdHighWaterMark 6 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 21 - disableImplicitCasting 1 - eml { - name "analyticalSolutionQPFCN" - } - firstData 265 - firstTransition 269 - firstJunction 268 - } - state { - id 264 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 263 - treeNode [263 0 0 0] - superState SUBCHART - subviewer 263 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function analyticalSolution = analyticalSolutionQPFCN(H,g)\n\n analyticalSolution = wbc.analyt" - "icalSolutionQP(H,g);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 265 - 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 [263 0 266] - } - data { - id 266 - 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 [263 265 267] - } - data { - id 267 - 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 [263 266 0] - } - junction { - id 268 - position [23.5747 49.5747 7] - chart 263 - subviewer 263 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [263 0 0] - } - transition { - id 269 - 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 268 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 263 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 263 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [263 0 0] - } - instance { - id 270 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/Two Feet/Analytical Solution Two Feet (unconstrained)" - chart 263 - } - chart { - id 271 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/One Foot/Analytical Solution 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 272 0 0] - viewObj 271 - ssIdHighWaterMark 6 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 22 - disableImplicitCasting 1 - eml { - name "analyticalSolutionQPFCN" - } - firstData 273 - firstTransition 277 - firstJunction 276 - } - state { - id 272 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 271 - treeNode [271 0 0 0] - superState SUBCHART - subviewer 271 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function analyticalSolution = analyticalSolutionQPFCN(H,g)\n\n analyticalSolution = wbc.analyt" - "icalSolutionQP(H,g);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 273 - 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 [271 0 274] - } - data { - id 274 - 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 [271 273 275] - } - data { - id 275 - 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 [271 274 0] - } - junction { - id 276 - position [23.5747 49.5747 7] - chart 271 - subviewer 271 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [271 0 0] - } - transition { - id 277 - 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 276 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 271 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 271 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [271 0 0] - } - instance { - id 278 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/One Foot/Analytical Solution One Foot (unconstrained)" - chart 271 - } - chart { - id 279 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/Two Feet/Process QP output" - windowPosition [422 539.941 189 413] - viewLimits [0 156.75 0 153.75] - screen [1 1 3600 1200 1.180555555555556] - treeNode [0 280 0 0] - viewObj 279 - ssIdHighWaterMark 13 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 23 - disableImplicitCasting 1 - eml { - name "processOutputQPFCN" - } - firstData 281 - firstTransition 287 - firstJunction 286 - } - state { - id 280 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 279 - treeNode [279 0 0 0] - superState SUBCHART - subviewer 279 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function f0 = processOutputQPFCN(analyticalSolution,primalSolution,QPStatus,Config)\n\n f0 = " - "wbc.processOutputQP(analyticalSolution,primalSolution,QPStatus,Config);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 281 - 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 [279 0 282] - } - data { - id 282 - 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 [279 281 283] - } - data { - id 283 - 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 [279 282 284] - } - data { - id 284 - ssIdNumber 5 - name "f0" - 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 [279 283 285] - } - data { - id 285 - 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 [279 284 0] - } - junction { - id 286 - position [23.5747 49.5747 7] - chart 279 - subviewer 279 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [279 0 0] - } - transition { - id 287 - 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 286 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 279 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 279 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [279 0 0] - } - instance { - id 288 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/Two Feet/Process QP output" - chart 279 - } - chart { - id 289 - machine 1 - name "emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/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 290 0 0] - viewObj 289 - ssIdHighWaterMark 8 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 24 - disableImplicitCasting 1 - eml { - name "checkSpikesFCN" - } - supportVariableSizing 0 - firstData 291 - firstTransition 295 - firstJunction 294 - } - state { - id 290 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 289 - treeNode [289 0 0 0] - superState SUBCHART - subviewer 289 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function isSpike = checkSpikesFCN(u, delta_u_max)\n\n isSpike = wbc.checkSpikes(u, delta_u_ma" - "x);\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 291 - 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 [289 0 292] - } - data { - id 292 - 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 [289 291 293] - } - data { - id 293 - ssIdNumber 7 - name "isSpike" - 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 [289 292 0] - } - junction { - id 294 - position [23.5747 49.5747 7] - chart 289 - subviewer 289 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [289 0 0] - } - transition { - id 295 - 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 294 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 289 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 289 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [289 0 0] - } - instance { - id 296 - machine 1 - name "emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/MATLAB Function" - chart 289 - } - chart { - id 297 - machine 1 - name "Robot State and References/State Machine Standup/Visualise external wrenches/Compute current threshold" - windowPosition [422 539.941 189 413] - viewLimits [0 156.75 0 153.75] - screen [1 1 3600 1200 1.180555555555556] - treeNode [0 298 0 0] - viewObj 297 - ssIdHighWaterMark 9 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 37 - disableImplicitCasting 1 - eml { - name "currentThreshold" - } - firstData 299 - firstTransition 306 - firstJunction 305 - } - state { - id 298 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 297 - treeNode [297 0 0 0] - superState SUBCHART - subviewer 297 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function [thresholdContactLeftHand,thresholdContactRightHand, ...\n thresholdContactLeft" - "Foot,thresholdContactRightFoot] = currentThreshold(state,Sm)\n\n thresholdContactLeftHand = Sm.wrench_thresh" - "oldContactLHand(state);\n thresholdContactRightHand = Sm.wrench_thresholdContactRHand(state);\n thresholdC" - "ontactLeftFoot = Sm.wrench_thresholdContactLFoot(state);\n thresholdContactRightFoot = Sm.wrench_thresholdCo" - "ntactRFoot(state);\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 299 - ssIdNumber 8 - name "thresholdContactLeftHand" - 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 [297 0 300] - } - data { - id 300 - ssIdNumber 9 - name "thresholdContactRightHand" - 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 [297 299 301] - } - data { - id 301 - 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 [297 300 302] - } - data { - id 302 - ssIdNumber 5 - name "thresholdContactLeftFoot" - 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 [297 301 303] - } - data { - id 303 - ssIdNumber 7 - name "thresholdContactRightFoot" - 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 [297 302 304] - } - data { - id 304 - ssIdNumber 6 - name "Sm" - 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 [297 303 0] - } - junction { - id 305 - position [23.5747 49.5747 7] - chart 297 - subviewer 297 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [297 0 0] - } - transition { - id 306 - 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 305 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 297 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 297 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [297 0 0] - } - instance { - id 307 - machine 1 - name "Robot State and References/State Machine Standup/Visualise external wrenches/Compute current threshold" - chart 297 - } - target { - id 308 - machine 1 - name "sfun" - codeFlags "" - linkNode [1 0 309] - } - target { - id 309 - machine 1 - name "rtw" - linkNode [1 308 0] - } -} diff --git a/torque-controllers/momentum-based-yoga/CMakeLists.txt b/torque-controllers/momentum-based-yoga/CMakeLists.txt deleted file mode 100644 index d64b97c..0000000 --- a/torque-controllers/momentum-based-yoga/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -register_mdl(MODELNAME "torqueBalancingYoga") diff --git a/torque-controllers/momentum-based-yoga/README.md b/torque-controllers/momentum-based-yoga/README.md deleted file mode 100644 index 2df17c5..0000000 --- a/torque-controllers/momentum-based-yoga/README.md +++ /dev/null @@ -1,24 +0,0 @@ -## Module description - -This module implements a torque control balancing strategy. It computes the interaction forces at the feet in order to stabilise a desired centroidal 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 [iCub whole-body control through force regulation on rigid non-coplanar contacts](http://journal.frontiersin.org/article/10.3389/frobt.2015.00006/abstract) - -### Compatibility - -The repository contains the Simulink model `torqueBalancingYoga.mdl`, which is generated by using Matlab R2016b. - -### Supported robots - -Currently, supported robots are: `iCubGenova04`, `iCubGenova02`, `icubGazeboSim`, `iCubGazeboV2_5`. - -## Module details - -### Configuration file - -At start, the module calls the initialization file initTorqueBalancingYoga.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/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m deleted file mode 100644 index 73c6af5..0000000 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initCoordinator.m +++ /dev/null @@ -1,238 +0,0 @@ -% INITCOORDINATOR initializes the robot configuration for running -% 'COORDINATOR' demo. -% - -%% --- Initialization --- - -% Feet in contact (COORDINATOR DEMO ONLY) -Config.LEFT_RIGHT_FOOT_IN_CONTACT = [1 1]; - -% Initial foot on ground. If false, right foot is used as default contact -% frame (this does not means that the other foot cannot be in contact too). -% (COORDINATOR DEMO ONLY) -Config.LEFT_FOOT_IN_CONTACT_AT_0 = true; - -% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) -Config.DEMO_MOVEMENTS = false; - -% If equal to true, the desired streamed values of the center of mass -% are smoothed internally -Config.SMOOTH_COM_DES = false; - -% If equal to true, the desired streamed values of the postural tasks are -% smoothed internally -Config.SMOOTH_JOINT_DES = false; - -% torque saturation -Sat.torque = 34; - -% torque derivative max absolute value -Config.tauDot_maxAbs = 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] - -%% Control gains - -% PARAMETERS FOR TWO FEET BALANCING -if sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 2 - - Gain.KP_COM = diag([50 100 5]); - Gain.KD_COM = 2*sqrt(Gain.KP_COM)*0; - Gain.KP_AngularMomentum = 5; - Gain.KD_AngularMomentum = 2*sqrt(Gain.KP_AngularMomentum); - - % Impedances acting in the null space of the desired contact forces - impTorso = [10 10 20]; - - impArms = [10 10 10 8]; - - impLeftLeg = [30 30 30 60 10 10]; - - impRightLeg = [30 30 30 60 10 10]; -end - -% PARAMETERS FOR ONE FOOT BALANCING -if sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 1 - - Gain.KP_COM = diag([50 100 50]); - Gain.KD_COM = diag([0 0 0]); - Gain.KP_AngularMomentum = 1 ; - Gain.KD_AngularMomentum = 1 ; - - % Impedances acting in the null space of the desired contact forces - impTorso = [20 20 30]; - - impArms = [15 15 15 8]; - - impLeftLeg = [30 30 30 120 10 10]; - - impRightLeg = [30 30 30 60 10 10]; -end - -Gain.impedances = [impTorso(1,:),impArms(1,:),impArms(1,:),impLeftLeg(1,:),impRightLeg(1,:)]; -Gain.dampings = 0*sqrt(Gain.impedances); - -if (size(Gain.impedances,2) ~= ROBOT_DOF) - error('Dimension mismatch between ROBOT_DOF and dimension of the variable impedences. Check these variables in the file gains.m'); -end - -% Smoothing time gain scheduling (YOGA DEMO ONLY) -Gain.SmoothingTimeGainScheduling = 2; - -%% Parameters for motors reflected inertia - -% transmission ratio -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; - -%% References for CoM trajectory (COORDINATOR DEMO ONLY) - -% that the robot waits before starting the left-and-right -Config.noOscillationTime = 0; - -if Config.DEMO_MOVEMENTS && sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 2 - - Config.directionOfOscillation = [0;1;0]; - % amplitude of oscillations in meters - Config.amplitudeOfOscillation = 0.02; - % frequency of oscillations in hertz - Config.frequencyOfOscillation = 1; -else - Config.directionOfOscillation = [0;0;0]; - Config.amplitudeOfOscillation = 0.0; - Config.frequencyOfOscillation = 0.0; -end - -%% State machine parameters - -% smoothing time for joints and CoM -Sm.smoothingTimeCoM_Joints = 3; - -% scale factor smoothing time (YOGA DEMO ONLY) -Sm.scaleFactorSmoothingTime = 0.9; - -% time between two yoga positions (YOGA DEMO ONLY) -Sm.joints_pauseBetweenYogaMoves = 0; - -% contact forces threshold (YOGA DEMO ONLY) -Sm.wrench_thresholdContactOn = 1; -Sm.wrench_thresholdContactOff = 1; - -% threshold on CoM and joints error (YOGA DEMO ONLY) -Sm.CoM_threshold = 0; -Sm.joints_thresholdNotInContact = 0; -Sm.joints_thresholdInContact = 0; - -% initial state for state machine (YOGA DEMO ONLY) -Sm.stateAt0 = 1; - -% delta to be summed to the reference CoM position (YOGA DEMO ONLY) -Sm.CoM_delta = [0; 0; 0]; - -% joint references (YOGA DEMO ONLY) -Sm.joints_references = zeros(1,ROBOT_DOF); -Sm.joints_leftYogaRef = zeros(1,ROBOT_DOF+1); -Sm.joints_rightYogaRef = zeros(1,ROBOT_DOF+1); -Sm.joints_leftSecondYogaRef = zeros(1,ROBOT_DOF+1); -Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1); - -% configuration parameters for state machine (YOGA DEMO ONLY) -Sm.tBalancing = 1; -Sm.tBalancingBeforeYoga = 1; -Sm.skipYoga = false; -Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; -Sm.yogaAlsoOnRightFoot = false; -Sm.twoFeetYogaInLoop = false; -Sm.oneFootYogaInLoop = false; -Sm.yogaCounter = 5; -Sm.repeatTwiceYogaWithDifferentSpeed = false; -Sm.smoothingTimeSecondYogaLeft = 1; -Sm.smoothingTimeSecondYogaRight = 1; - -%% 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; - -% physical size of the foot -feet_size = [-0.07 0.12; % xMin, xMax - -0.04 0.04 ]; % yMin, yMax - -fZmin = 10; - -%% Regularization parameters -Reg.pinvDamp_nu_b = 1e-7; -Reg.pinvDamp = 2; -Reg.pinvTol = 1e-5; -Reg.impedances = 0.1; -Reg.dampings = 0; -Reg.HessianQP = 1e-5; diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initStateMachineYoga.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initStateMachineYoga.m deleted file mode 100644 index a0d4176..0000000 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGazeboV2_5/initStateMachineYoga.m +++ /dev/null @@ -1,512 +0,0 @@ -% INITSTATEMACHINEYOGA initializes the robot configuration for running -% 'YOGA' demo. -% - -%% --- Initialization --- - -% Feet in contact (COORDINATOR DEMO ONLY) -Config.LEFT_RIGHT_FOOT_IN_CONTACT = [1 1]; - -% Initial foot on ground. If false, right foot is used as default contact -% frame (this does not means that the other foot cannot be in contact too). -% (COORDINATOR DEMO ONLY) -Config.LEFT_FOOT_IN_CONTACT_AT_0 = true; - -% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) -Config.DEMO_MOVEMENTS = false; - -% If equal to one, the desired streamed values of the center of mass -% are smoothed internally -Config.SMOOTH_COM_DES = true; - -% If equal to one, the desired streamed values of the postural tasks are -% smoothed internally -Config.SMOOTH_JOINT_DES = true; - -% torque saturation -Sat.torque = 60; - -% torque derivative max absolute value -Config.tauDot_maxAbs = 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_nu_b = 1e-7; -Reg.pinvDamp = 1; -Reg.pinvTol = 1e-5; -Reg.impedances = 0.1; -Reg.dampings = 0; -Reg.HessianQP = 1e-7; - -%% COM AND JOINT GAINS -Gain.KP_COM = [50 50 10 % state == 1 TWO FEET BALANCING - 50 50 10 % state == 2 COM TRANSITION TO LEFT - 50 50 10 % state == 3 LEFT FOOT BALANCING - 50 50 10 % state == 4 YOGA LEFT FOOT - 50 50 10 % state == 5 PREPARING FOR SWITCHING - 50 50 10 % state == 6 LOOKING FOR CONTACT - 50 50 10 % state == 7 TRANSITION TO INITIAL POSITION - 50 50 10 % state == 8 COM TRANSITION TO RIGHT FOOT - 50 50 10 % state == 9 RIGHT FOOT BALANCING - 50 50 10 % state == 10 YOGA RIGHT FOOT - 50 50 10 % state == 11 PREPARING FOR SWITCHING - 50 50 10 % state == 12 LOOKING FOR CONTACT - 50 50 10];% state == 13 TRANSITION TO INITIAL POSITION - -Gain.KD_COM = 2*sqrt(Gain.KP_COM)/20; - -Gain.KP_AngularMomentum = 0.25 ; -Gain.KD_AngularMomentum = 2*sqrt(Gain.KP_AngularMomentum); - -% % TORSO %% LEFT ARM %% RIGHT ARM %% LEFT LEG %% RIGHT LEG %% -Gain.impedances = [10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 1 TWO FEET BALANCING - 10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 2 COM TRANSITION TO LEFT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 100 100, 30 30 20 20 100 100 % state == 3 LEFT FOOT BALANCING - 30 30 30, 10 10 10 10, 10 10 10 10,100 200 100 400 100 100,100 50 30 100 100 100 % state == 4 YOGA LEFT FOOT - 30 30 30, 5 5 10 10, 10 10 20 10,200 250 20 20 10 50,220 350 120 200 65 100 % state == 5 PREPARING FOR SWITCHING - 30 30 30, 10 10 20 10, 10 10 20 10,100 350 20 200 10 100,220 350 120 200 65 100 % state == 6 LOOKING FOR CONTACT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 5 5, 30 30 30 20 5 5 % state == 7 TRANSITION TO INITIAL POSITION - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 100 100, 30 30 30 20 100 100 % state == 8 COM TRANSITION TO RIGHT FOOT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 9 RIGHT FOOT BALANCING - 30 30 30, 10 10 10 10, 10 10 10 10,100 200 100 400 100 100,100 50 30 100 100 100 % state == 10 YOGA RIGHT FOOT - 30 30 30, 5 5 10 10, 10 10 20 10,200 250 20 20 10 50,220 350 120 200 65 100 % state == 11 PREPARING FOR SWITCHING - 30 30 30, 10 10 20 10, 10 10 20 10,100 350 20 200 10 100,220 350 120 200 65 100 % state == 12 LOOKING FOR CONTACT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100];% state == 13 TRANSITION TO INITIAL POSITION - -Gain.dampings = 0*sqrt(Gain.impedances(1,:)); - -% symmetric gains -Gain.impedances(8:12,:) = Gain.impedances(2:6,:); -Gain.impedances(8:12,12:17) = Gain.impedances(2:6,18:23); -Gain.impedances(8:12,18:23) = Gain.impedances(2:6,12:17); - -% Smoothing time gain scheduling (YOGA DEMO ONLY) -Gain.SmoothingTimeGainScheduling = 2; - -%% STATE MACHINE PARMETERS - -% smoothing time for joints and CoM -Sm.smoothingTimeCoM_Joints = [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 (YOGA DEMO ONLY) -Sm.scaleFactorSmoothingTime = 0.9; - -% time between two yoga positions (YOGA DEMO ONLY) -Sm.joints_pauseBetweenYogaMoves = 5; - -% contact forces threshold (YOGA DEMO ONLY) -Sm.wrench_thresholdContactOn = 50; -Sm.wrench_thresholdContactOff = 100; - -% threshold on CoM and joints error (YOGA DEMO ONLY) -Sm.CoM_threshold = 0.01; -Sm.joints_thresholdNotInContact = 5; -Sm.joints_thresholdInContact = 50; - -% initial state for state machine (YOGA DEMO ONLY) -Sm.stateAt0 = 1; - -% delta to be summed to the reference CoM position (YOGA DEMO ONLY) - -Sm.CoM_delta = [% THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE LEFT FOOT - 0.0, 0.00, 0.0; %% NOT USED - 0.0, 0.00, 0.0; %% state == 2 COM TRANSITION TO LEFT FOOT - 0.0, 0.00, 0.0; %% state == 3 LEFT FOOT BALANCING - 0.0, 0.005, 0.0; %% state == 4 YOGA LEFT FOOT - 0.0, 0.00, 0.0; %% state == 5 PREPARING FOR SWITCHING - 0.0, -0.09, 0.0; %% state == 6 LOOKING FOR CONTACT - 0.0, 0.00, 0.0; %% NOT USED - % 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 - -% configuration parameters for state machine (YOGA DEMO ONLY) -Sm.tBalancing = 1; -Sm.tBalancingBeforeYoga = 1; -Sm.yogaExtended = true; -Sm.skipYoga = false; -Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first -Sm.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot (not available for now) - -%%%% List of possible "Yoga in loop" %%%% - -% the robot will repeat the FULL DEMO (two feet balancing, yoga on left -% foot, back on two feet, yoga right foot, back on two feet). The demo is -% repeated until the user stops the Simulink model. This option is ignored -% if Sm.demoStartsOnRightSupport = true. -Sm.twoFeetYogaInLoop = false; - -% the robot will repeat the ONE FOOT yoga for the number of times the user -% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two -% feet balancing in between to consecutive yoga. WARNING: if the option -% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga -% on left foot for the number of times the user specifies in the Sm.yogaCounter, -% and then it will repeat the yoga on the right foot for the same number of times. -% This option is ignored if Sm.repeatTwiceYogaWithDifferentSpeed = true. -Sm.oneFootYogaInLoop = false; -Sm.yogaCounter = 5; - -% the robot will repeat the yoga moveset twice. This option works as the -% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However, -% it is possible to set a different yoga speed for the two yoga. -% Uneffective if Sm.yogaExtended = false; -Sm.repeatTwiceYogaWithDifferentSpeed = false; - -% smoothing time for the second time the Yoga moveset are performed -Sm.smoothingTimeSecondYogaLeft = 1.2; -Sm.smoothingTimeSecondYogaRight = 1.2; - -%% Joint references (YOGA DEMO ONLY) -Sm.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 (LEFT YOGA, EXTENDED) -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]; - -Sm.joints_leftYogaRef = [ 0, q1; - 1*Sm.smoothingTimeCoM_Joints(4),q2; - 2*Sm.smoothingTimeCoM_Joints(4),q3; - 3*Sm.smoothingTimeCoM_Joints(4),q4; - 4*Sm.smoothingTimeCoM_Joints(4),q5; - 5*Sm.smoothingTimeCoM_Joints(4),q6; - 6*Sm.smoothingTimeCoM_Joints(4),q7; - 7*Sm.smoothingTimeCoM_Joints(4),q8; - 8*Sm.smoothingTimeCoM_Joints(4),q9; - 9*Sm.smoothingTimeCoM_Joints(4),q10; - 10*Sm.smoothingTimeCoM_Joints(4),q11; - 11*Sm.smoothingTimeCoM_Joints(4),q12; - 12*Sm.smoothingTimeCoM_Joints(4),q13; - 13*Sm.smoothingTimeCoM_Joints(4),q14; - 14*Sm.smoothingTimeCoM_Joints(4),q15; - 15*Sm.smoothingTimeCoM_Joints(4),q16; - 16*Sm.smoothingTimeCoM_Joints(4),q17; - 17*Sm.smoothingTimeCoM_Joints(4),q10; - 18*Sm.smoothingTimeCoM_Joints(4),q11; - 19*Sm.smoothingTimeCoM_Joints(4),q12; - 20*Sm.smoothingTimeCoM_Joints(4),q13; - 21*Sm.smoothingTimeCoM_Joints(4),q14; - 22*Sm.smoothingTimeCoM_Joints(4),q15; - 23*Sm.smoothingTimeCoM_Joints(4),q16; - 24*Sm.smoothingTimeCoM_Joints(4),q17; - 25*Sm.smoothingTimeCoM_Joints(4),q8]; - -Sm.joints_rightYogaRef = Sm.joints_leftYogaRef; -Sm.joints_rightYogaRef(:,1) = [0, ; - 1*Sm.smoothingTimeCoM_Joints(10); - 2*Sm.smoothingTimeCoM_Joints(10); - 3*Sm.smoothingTimeCoM_Joints(10); - 4*Sm.smoothingTimeCoM_Joints(10); - 5*Sm.smoothingTimeCoM_Joints(10); - 6*Sm.smoothingTimeCoM_Joints(10); - 7*Sm.smoothingTimeCoM_Joints(10); - 8*Sm.smoothingTimeCoM_Joints(10); - 9*Sm.smoothingTimeCoM_Joints(10); - 10*Sm.smoothingTimeCoM_Joints(10); - 11*Sm.smoothingTimeCoM_Joints(10); - 12*Sm.smoothingTimeCoM_Joints(10); - 13*Sm.smoothingTimeCoM_Joints(10); - 14*Sm.smoothingTimeCoM_Joints(10); - 15*Sm.smoothingTimeCoM_Joints(10); - 16*Sm.smoothingTimeCoM_Joints(10); - 17*Sm.smoothingTimeCoM_Joints(10); - 18*Sm.smoothingTimeCoM_Joints(10); - 19*Sm.smoothingTimeCoM_Joints(10); - 20*Sm.smoothingTimeCoM_Joints(10); - 21*Sm.smoothingTimeCoM_Joints(10); - 22*Sm.smoothingTimeCoM_Joints(10); - 23*Sm.smoothingTimeCoM_Joints(10); - 24*Sm.smoothingTimeCoM_Joints(10); - 25*Sm.smoothingTimeCoM_Joints(10)]; - -% smoothing time vector for the second time the Yoga moveset are performed -Sm.joints_leftSecondYogaRef = Sm.smoothingTimeSecondYogaLeft.*(0:(length(Sm.joints_rightYogaRef(:,1))-1)); -Sm.joints_rightSecondYogaRef = Sm.smoothingTimeSecondYogaRight.*(0:(length(Sm.joints_rightYogaRef(:,1))-1)); - -% keep a high smoothing time for switching from the first to the second yoga moveset -Sm.joints_leftSecondYogaRef(1) = Sm.smoothingTimeCoM_Joints(4); -Sm.joints_rightSecondYogaRef(1) = Sm.smoothingTimeCoM_Joints(10); - -% if the demo is not "yogaExtended", stop at the 8th move -% also, Sm.repeatTwiceYogaWithDifferentSpeed must be set to "false". The -% reason is that the first and the last Yoga moveset for the "not extended" -% one are very different, and the robot may "jump" when restarting the Yoga -% the second time -if ~Sm.yogaExtended - - Sm.repeatTwiceYogaWithDifferentSpeed = false; - Sm.joints_leftYogaRef = Sm.joints_leftYogaRef(1:8,:); - Sm.joints_rightYogaRef = Sm.joints_rightYogaRef(1:8,:); - Sm.joints_leftYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(4); - Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10); -end - -% MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA -for i = 1:size(Sm.joints_rightYogaRef,1) - - Sm.joints_rightYogaRef(i,2:4) = [Sm.joints_rightYogaRef(i,2) -Sm.joints_rightYogaRef(i,3) -Sm.joints_rightYogaRef(i,4)]; - rightArm = Sm.joints_rightYogaRef(i,end-15:end-12); - Sm.joints_rightYogaRef(i,end-15:end-12) = Sm.joints_rightYogaRef(i,end-19:end-16); - Sm.joints_rightYogaRef(i,end-19:end-16) = rightArm; - rightLeg = Sm.joints_rightYogaRef(i,end-5:end); - Sm.joints_rightYogaRef(i,end-5:end) = Sm.joints_rightYogaRef(i,end-11:end-6); - Sm.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;0;0]; -Config.amplitudeOfOscillation = 0.0; -Config.frequencyOfOscillation = 0.0; - -%% Parameters for motors reflected inertia - -% transmission ratio -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 - -%% Cleanup -clear q1 q2 q3 q4; diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m deleted file mode 100644 index a80f09f..0000000 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initCoordinator.m +++ /dev/null @@ -1,238 +0,0 @@ -% INITCOORDINATOR initializes the robot configuration for running -% 'COORDINATOR' demo. -% - -%% --- Initialization --- - -% Feet in contact (COORDINATOR DEMO ONLY) -Config.LEFT_RIGHT_FOOT_IN_CONTACT = [1 1]; - -% Initial foot on ground. If false, right foot is used as default contact -% frame (this does not means that the other foot cannot be in contact too). -% (COORDINATOR DEMO ONLY) -Config.LEFT_FOOT_IN_CONTACT_AT_0 = true; - -% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) -Config.DEMO_MOVEMENTS = false; - -% If equal to true, the desired streamed values of the center of mass -% are smoothed internally -Config.SMOOTH_COM_DES = false; - -% If equal to true, the desired streamed values of the postural tasks are -% smoothed internally -Config.SMOOTH_JOINT_DES = false; - -% torque saturation -Sat.torque = 34; - -% torque derivative max absolute value -Config.tauDot_maxAbs = 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] - -%% Control gains - -% PARAMETERS FOR TWO FEET BALANCING -if sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 2 - - Gain.KP_COM = diag([50 100 5]); - Gain.KD_COM = 2*sqrt(Gain.KP_COM)*0; - Gain.KP_AngularMomentum = 5; - Gain.KD_AngularMomentum = 2*sqrt(Gain.KP_AngularMomentum); - - % Impedances acting in the null space of the desired contact forces - impTorso = [10 10 20]; - - impArms = [10 10 10 8]; - - impLeftLeg = [30 30 30 60 10 10]; - - impRightLeg = [30 30 30 60 10 10]; -end - -% PARAMETERS FOR ONE FOOT BALANCING -if sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 1 - - Gain.KP_COM = diag([50 100 50]); - Gain.KD_COM = diag([0 0 0]); - Gain.KP_AngularMomentum = 1 ; - Gain.KD_AngularMomentum = 1 ; - - % Impedances acting in the null space of the desired contact forces - impTorso = [20 20 30]; - - impArms = [15 15 15 8]; - - impLeftLeg = [30 30 30 120 10 10]; - - impRightLeg = [30 30 30 60 10 10]; -end - -Gain.impedances = [impTorso(1,:),impArms(1,:),impArms(1,:),impLeftLeg(1,:),impRightLeg(1,:)]; -Gain.dampings = 0*sqrt(Gain.impedances); - -if (size(Gain.impedances,2) ~= ROBOT_DOF) - error('Dimension mismatch between ROBOT_DOF and dimension of the variable impedences. Check these variables in the file gains.m'); -end - -% Smoothing time gain scheduling (YOGA DEMO ONLY) -Gain.SmoothingTimeGainScheduling = 2; - -%% Parameters for motors reflected inertia - -% transmission ratio -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; - -%% References for CoM trajectory (COORDINATOR DEMO ONLY) - -% that the robot waits before starting the left-and-right -Config.noOscillationTime = 0; - -if Config.DEMO_MOVEMENTS && sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 2 - - Config.directionOfOscillation = [0;1;0]; - % amplitude of oscillations in meters - Config.amplitudeOfOscillation = 0.02; - % frequency of oscillations in hertz - Config.frequencyOfOscillation = 1; -else - Config.directionOfOscillation = [0;0;0]; - Config.amplitudeOfOscillation = 0.0; - Config.frequencyOfOscillation = 0.0; -end - -%% State machine parameters - -% smoothing time for joints and CoM -Sm.smoothingTimeCoM_Joints = 3; - -% scale factor smoothing time (YOGA DEMO ONLY) -Sm.scaleFactorSmoothingTime = 0.9; - -% time between two yoga positions (YOGA DEMO ONLY) -Sm.joints_pauseBetweenYogaMoves = 0; - -% contact forces threshold (YOGA DEMO ONLY) -Sm.wrench_thresholdContactOn = 1; -Sm.wrench_thresholdContactOff = 1; - -% threshold on CoM and joints error (YOGA DEMO ONLY) -Sm.CoM_threshold = 0; -Sm.joints_thresholdNotInContact = 0; -Sm.joints_thresholdInContact = 0; - -% initial state for state machine (YOGA DEMO ONLY) -Sm.stateAt0 = 1; - -% delta to be summed to the reference CoM position (YOGA DEMO ONLY) -Sm.CoM_delta = [0; 0; 0]; - -% joint references (YOGA DEMO ONLY) -Sm.joints_references = zeros(1,ROBOT_DOF); -Sm.joints_leftYogaRef = zeros(1,ROBOT_DOF+1); -Sm.joints_rightYogaRef = zeros(1,ROBOT_DOF+1); -Sm.joints_leftSecondYogaRef = zeros(1,ROBOT_DOF+1); -Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1); - -% configuration parameters for state machine (YOGA DEMO ONLY) -Sm.tBalancing = 1; -Sm.tBalancingBeforeYoga = 1; -Sm.skipYoga = false; -Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; -Sm.yogaAlsoOnRightFoot = false; -Sm.twoFeetYogaInLoop = false; -Sm.oneFootYogaInLoop = false; -Sm.yogaCounter = 5; -Sm.repeatTwiceYogaWithDifferentSpeed = false; -Sm.smoothingTimeSecondYogaLeft = 1; -Sm.smoothingTimeSecondYogaRight = 1; - -%% 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; - -% physical size of the foot -feet_size = [-0.07 0.12; % xMin, xMax - -0.04 0.04 ]; % yMin, yMax - -fZmin = 10; - -%% Regularization parameters -Reg.pinvDamp_nu_b = 1e-7; -Reg.pinvDamp = 2; -Reg.pinvTol = 1e-5; -Reg.impedances = 0.1; -Reg.dampings = 0; -Reg.HessianQP = 1e-5; diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m deleted file mode 100644 index 26ee4a3..0000000 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova02/initStateMachineYoga.m +++ /dev/null @@ -1,514 +0,0 @@ -% INITSTATEMACHINEYOGA initializes the robot configuration for running -% 'YOGA' demo. -% - -%% --- Initialization --- - -% Feet in contact (COORDINATOR DEMO ONLY) -Config.LEFT_RIGHT_FOOT_IN_CONTACT = [1 1]; - -% Initial foot on ground. If false, right foot is used as default contact -% frame (this does not means that the other foot cannot be in contact too). -% (COORDINATOR DEMO ONLY) -Config.LEFT_FOOT_IN_CONTACT_AT_0 = true; - -% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) -Config.DEMO_MOVEMENTS = false; - -% If equal to one, the desired streamed values of the center of mass -% are smoothed internally -Config.SMOOTH_COM_DES = true; - -% If equal to one, the desired streamed values of the postural tasks are -% smoothed internally -Config.SMOOTH_JOINT_DES = true; - -% torque saturation -Sat.torque = 60; - -% torque derivative max absolute value -Config.tauDot_maxAbs = 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_nu_b = 1e-7; -Reg.pinvDamp = 0.07; -Reg.pinvTol = 1e-5; -Reg.impedances = 0.1; -Reg.dampings = 0; -Reg.HessianQP = 1e-7; - -%% COM AND JOINT GAINS -Gain.KP_COM = [50 100 5 % state == 1 TWO FEET BALANCING - 50 100 5 % state == 2 COM TRANSITION TO LEFT - 50 100 5 % state == 3 LEFT FOOT BALANCING - 50 100 5 % state == 4 YOGA LEFT FOOT - 50 100 5 % state == 5 PREPARING FOR SWITCHING - 50 100 5 % state == 6 LOOKING FOR CONTACT - 50 100 5 % state == 7 TRANSITION TO INITIAL POSITION - 50 100 5 % state == 8 COM TRANSITION TO RIGHT FOOT - 50 100 5 % state == 9 RIGHT FOOT BALANCING - 50 100 5 % state == 10 YOGA RIGHT FOOT - 50 100 5 % state == 11 PREPARING FOR SWITCHING - 50 100 5 % state == 12 LOOKING FOR CONTACT - 50 100 5];% state == 13 TRANSITION TO INITIAL POSITION - -Gain.KD_COM = 2*sqrt(Gain.KP_COM)/20; - -Gain.KP_AngularMomentum = 3; -Gain.KD_AngularMomentum = 2*sqrt(Gain.KP_AngularMomentum)/5; - -% % TORSO %% LEFT ARM %% RIGHT ARM %% LEFT LEG %% RIGHT LEG %% -Gain.impedances = [10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 1 TWO FEET BALANCING - 10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 2 COM TRANSITION TO LEFT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 100 100, 30 30 20 20 100 100 % state == 3 LEFT FOOT BALANCING - 30 30 30, 10 10 10 10, 10 10 10 10,100 200 100 400 100 100,100 50 30 100 100 100 % state == 4 YOGA LEFT FOOT - 30 30 30, 5 5 10 10, 10 10 20 10,200 250 20 20 10 10,220 550 220 200 65 300 % state == 5 PREPARING FOR SWITCHING - 30 30 30, 10 10 20 10, 10 10 20 10,100 350 20 200 10 100,220 550 220 200 65 300 % state == 6 LOOKING FOR CONTACT - 30 30 30, 10 10 10 8, 10 10 10 8, 30 50 60 30 5 5, 30 30 30 20 5 5 % state == 7 TRANSITION TO INITIAL POSITION - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 100 100, 30 30 30 20 100 100 % state == 8 COM TRANSITION TO RIGHT FOOT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 100 100, 30 30 20 20 100 100 % state == 9 RIGHT FOOT BALANCING - 30 30 30, 10 10 10 10, 10 10 10 10,100 50 30 100 100 100,100 200 100 100 10 10 % state == 10 YOGA RIGHT FOOT - 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,200 250 20 20 10 10 % state == 11 PREPARING FOR SWITCHING - 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100 % state == 12 LOOKING FOR CONTACT - 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100];% state == 13 TRANSITION TO INITIAL POSITION - -Gain.impedances(10,18:23) = Gain.impedances(10,18:23)*1.5; -Gain.impedances(10,1:3) = Gain.impedances(10,1:3)*2; -Gain.impedances(4,12:17) = Gain.impedances(4,12:17)*1.5; -Gain.impedances(4,1:3) = Gain.impedances(4,1:3)*2; -Gain.impedances(4,4:11) = Gain.impedances(4,4:11)*1.5; -Gain.impedances(7,2:3) = Gain.impedances(7,2:3)*1.5; - -Gain.dampings = 0*2*sqrt(Gain.impedances(1,:))/40; - -% Smoothing time gain scheduling (YOGA DEMO ONLY) -Gain.SmoothingTimeGainScheduling = 2; - -%% STATE MACHINE PARMETERS - -% smoothing time for joints and CoM -Sm.smoothingTimeCoM_Joints = [1; %% state == 1 TWO FEET BALANCING - 1; %% state == 2 COM TRANSITION TO LEFT FOOT - 1; %% state == 3 LEFT FOOT BALANCING - 0.65; %% state == 4 YOGA LEFT FOOT - 2; %% state == 5 PREPARING FOR SWITCHING - 2; %% state == 6 LOOKING FOR CONTACT - 1; %% state == 7 TRANSITION INIT POSITION - 1; %% state == 8 COM TRANSITION TO RIGHT FOOT - 1; %% state == 9 RIGHT FOOT BALANCING - 0.65; %% state == 10 YOGA RIGHT FOOT - 2; %% state == 11 PREPARING FOR SWITCHING - 5; %% state == 12 LOOKING FOR CONTACT - 10]; %% state == 13 TRANSITION INIT POSITION - -% scale factor smoothing time multiplies the smoothing factor during the -% Yoga (state 4 and 10). The purpose is to reduce the time necessary for -% the reference to converge to the next position, but without changing also -% the valuse stored in Sm.joints_leftYogaRef/Sm.joints_rightYogaRef (YOGA DEMO ONLY) -Sm.scaleFactorSmoothingTime = 0.9; - -% time between two yoga positions (YOGA DEMO ONLY) -Sm.joints_pauseBetweenYogaMoves = 5; - -% contact forces threshold (YOGA DEMO ONLY) -Sm.wrench_thresholdContactOn = 50; -Sm.wrench_thresholdContactOff = 100; - -% threshold on CoM and joints error (YOGA DEMO ONLY) -Sm.CoM_threshold = 0.01; -Sm.joints_thresholdNotInContact = 5; -Sm.joints_thresholdInContact = 50; - -% initial state for state machine (YOGA DEMO ONLY) -Sm.stateAt0 = 1; - -% delta to be summed to the reference CoM position (YOGA DEMO ONLY) - -Sm.CoM_delta = [% THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE LEFT FOOT - 0.0, 0.00, 0.0; %% NOT USED - 0.0, 0.00, 0.0; %% state == 2 COM TRANSITION TO LEFT FOOT - 0.0, 0.005, 0.0; %% state == 3 LEFT FOOT BALANCING - 0.0, 0.005, 0.0; %% state == 4 YOGA LEFT FOOT - 0.0, 0.00, 0.0; %% state == 5 PREPARING FOR SWITCHING - 0.02,-0.08, 0.0; %% state == 6 LOOKING FOR CONTACT - 0.0, 0.00, 0.0; %% NOT USED - % THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE RIGHT FOOT - 0.0, 0.00, 0.0; %% state == 8 COM TRANSITION TO RIGHT FOOT - 0.0, 0.00, 0.0; %% state == 9 RIGHT FOOT BALANCING - 0.0, -0.01, 0.0; %% state == 10 YOGA RIGHT FOOT - 0.0, 0.00, 0.0; %% state == 11 PREPARING FOR SWITCHING - 0.0, 0.09, 0.0; %% state == 12 LOOKING FOR CONTACT - 0.0, 0.00, 0.0]; %% NOT USED - -% configuration parameters for state machine (YOGA DEMO ONLY) -Sm.tBalancing = 1; -Sm.tBalancingBeforeYoga = 1; -Sm.yogaExtended = true; -Sm.skipYoga = false; -Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first -Sm.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot - -%%%% List of possible "Yoga in loop" %%%% - -% the robot will repeat the FULL DEMO (two feet balancing, yoga on left -% foot, back on two feet, yoga right foot, back on two feet). The demo is -% repeated until the user stops the Simulink model. This option is ignored -% if Sm.demoStartsOnRightSupport = true. -Sm.twoFeetYogaInLoop = false; - -% the robot will repeat the ONE FOOT yoga for the number of times the user -% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two -% feet balancing in between to consecutive yoga. WARNING: if the option -% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga -% on left foot for the number of times the user specifies in the Sm.yogaCounter, -% and then it will repeat the yoga on the right foot for the same number of times. -% This option is ignored if Sm.repeatTwiceYogaWithDifferentSpeed = true. -Sm.oneFootYogaInLoop = false; -Sm.yogaCounter = 5; - -% the robot will repeat the yoga moveset twice. This option works as the -% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However, -% it is possible to set a different yoga speed for the two yoga. -% Uneffective if Sm.yogaExtended = false; -Sm.repeatTwiceYogaWithDifferentSpeed = false; - -% smoothing time for the second time the Yoga moveset are performed -Sm.smoothingTimeSecondYogaLeft = 0.6; -Sm.smoothingTimeSecondYogaRight = 0.6; - -%% Joint references (YOGA DEMO ONLY) -Sm.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED - [-0.0348,0.0779,0.0429, ... %% state == 2 COM TRANSITION TO LEFT - -0.1493,0.8580,0.2437,0.8710, ... % - -0.1493,0.8580,0.2437,0.8710, ... % - -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % - 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % - [ 0.0864,0.0258,0.0152, ... %% state == 3 LEFT FOOT BALANCING - 0.1253,0.8135,0.3051,0.7928, ... % - 0.0563,0.6789,0.3340,0.6214, ... % - -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % - 0.0005,0.0793,-0.0014,-0.0051,-0.1060,-0.1151]; % - zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED - [-0.0348,0.0779,0.0429, ... %% state == 5 PREPARING FOR SWITCHING - -0.1493,0.8580,0.2437,0.8710, ... % - -0.1493,0.8580,0.2437,0.8710, ... % - -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % - 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % - [ 0.0864,0.0258,0.0152, ... %% state == 6 LOOKING FOR CONTACT - 0.1253,0.8135,0.3051,0.7928, ... % - 0.0563,0.6789,0.3340,0.6214, ... % - 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % - -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % - zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED - [ 0.0864,0.0258,0.0152, ... %% state == 8 COM TRANSITION TO RIGHT FOOT - 0.1253,0.8135,0.3051,0.7928, ... % - 0.0563,0.6789,0.3340,0.6214, ... % - 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % - -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % - [ 0.0864,0.0258,0.0152, ... %% state == 9 RIGHT FOOT BALANCING - 0.1253,0.8135,0.3051,0.7928, ... % - 0.0563,0.6789,0.3340,0.6214, ... % - 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % - -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % - zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED - [-0.0348,0.0779,0.0429, ... %% state == 11 PREPARING FOR SWITCHING - -0.1493,0.8580,0.2437,0.8710, ... % - -0.1493,0.8580,0.2437,0.8710, ... % - 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % - -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % - [ 0.0864,0.0258,0.0152, ... %% state == 12 LOOKING FOR CONTACT - 0.1253,0.8135,0.3051,0.7928, ... % - 0.0563,0.6789,0.3340,0.6214, ... % - -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277, ... % - 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369]; % - zeros(1,ROBOT_DOF)]; %% THIS REFERENCE IS IGNORED - -% YOGA MOVESET (LEFT YOGA, EXTENDED) -q1 = [-0.0790,0.2279, 0.4519, ... - -1.1621,0.6663, 0.4919, 0.9947, ... - -1.0717,1.2904,-0.2447, 1.0948, ... - 0.2092,0.2060, 0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3484,0.4008,-0.0004,-0.3672,-0.1060,-0.0875]; - -q2 = [-0.0790,0.1279, 0.4519, ... - -1.1621,0.6663, 0.4965, 0.9947, ... - -1.0717,1.2904,-0.2493, 1.0948, ... - 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... - 0.3714,0.9599, 1.3253,-1.6594,-0.1060,-0.0614]; - -q3 = [-0.0852,-0.3273,0.0821,... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... - 0.3714,0.9599, 1.3253,-1.6594, 0.2443,-0.0614]; - -q4 = [-0.0852,-0.4273,0.0821,... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.3473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; - -q5 = [-0.0790,-0.2273, 0.4519, ... - -1.1621,0.6663, 0.4965, 0.9947, ... - -1.0717,1.2904,-0.2493, 1.0948, ... - 0.2092, 0.4473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; - -q6 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; - -q7 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 1.3107,1.3253, -1.6217, 0.2443,-0.0614]; - -q8 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; - -q9 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 0.0107,1.3253,-0.0189, 0.2443,-0.0614]; - -q10 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; - -q11 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.8514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; - -q12 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.8514, 0.3107,1.3253,-0.0189, 0.2443,-0.0614]; - -q13 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.8514, 1.3107,1.3253,-0.0189, 0.2443,-0.0614]; - -q14 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.8514, 0.0107,1.3253,-0.0189, 0.2443,-0.0614]; - -q15 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 1.5514, 0.3107,1.3253,-0.0189, 0.2443,-0.0614]; - -q16 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.2514, 0.0107,1.3253,-0.0189, 0.2443,-0.0614]; - -q17 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - -0.3514, 0.3107,1.3253,-0.0189, 0.2443,-0.0614]; - -Sm.joints_leftYogaRef = [ 0, q1; - 1*Sm.smoothingTimeCoM_Joints(4),q2; - 2*Sm.smoothingTimeCoM_Joints(4),q3; - 3*Sm.smoothingTimeCoM_Joints(4),q4; - 4*Sm.smoothingTimeCoM_Joints(4),q5; - 5*Sm.smoothingTimeCoM_Joints(4),q6; - 6*Sm.smoothingTimeCoM_Joints(4),q7; - 7*Sm.smoothingTimeCoM_Joints(4),q8; - 8*Sm.smoothingTimeCoM_Joints(4),q9; - 9*Sm.smoothingTimeCoM_Joints(4),q10; - 10*Sm.smoothingTimeCoM_Joints(4),q11; - 11*Sm.smoothingTimeCoM_Joints(4),q12; - 12*Sm.smoothingTimeCoM_Joints(4),q13; - 13*Sm.smoothingTimeCoM_Joints(4),q14; - 14*Sm.smoothingTimeCoM_Joints(4),q15; - 15*Sm.smoothingTimeCoM_Joints(4),q16; - 16*Sm.smoothingTimeCoM_Joints(4),q17; - 17*Sm.smoothingTimeCoM_Joints(4),q10; - 18*Sm.smoothingTimeCoM_Joints(4),q11; - 19*Sm.smoothingTimeCoM_Joints(4),q12; - 20*Sm.smoothingTimeCoM_Joints(4),q13; - 21*Sm.smoothingTimeCoM_Joints(4),q14; - 22*Sm.smoothingTimeCoM_Joints(4),q15; - 23*Sm.smoothingTimeCoM_Joints(4),q16; - 24*Sm.smoothingTimeCoM_Joints(4),q17; - 25*Sm.smoothingTimeCoM_Joints(4),q8]; - -Sm.joints_rightYogaRef = Sm.joints_leftYogaRef; -Sm.joints_rightYogaRef(:,1) = [0, ; - 1*Sm.smoothingTimeCoM_Joints(10); - 2*Sm.smoothingTimeCoM_Joints(10); - 3*Sm.smoothingTimeCoM_Joints(10); - 4*Sm.smoothingTimeCoM_Joints(10); - 5*Sm.smoothingTimeCoM_Joints(10); - 6*Sm.smoothingTimeCoM_Joints(10); - 7*Sm.smoothingTimeCoM_Joints(10); - 8*Sm.smoothingTimeCoM_Joints(10); - 9*Sm.smoothingTimeCoM_Joints(10); - 10*Sm.smoothingTimeCoM_Joints(10); - 11*Sm.smoothingTimeCoM_Joints(10); - 12*Sm.smoothingTimeCoM_Joints(10); - 13*Sm.smoothingTimeCoM_Joints(10); - 14*Sm.smoothingTimeCoM_Joints(10); - 15*Sm.smoothingTimeCoM_Joints(10); - 16*Sm.smoothingTimeCoM_Joints(10); - 17*Sm.smoothingTimeCoM_Joints(10); - 18*Sm.smoothingTimeCoM_Joints(10); - 19*Sm.smoothingTimeCoM_Joints(10); - 20*Sm.smoothingTimeCoM_Joints(10); - 21*Sm.smoothingTimeCoM_Joints(10); - 22*Sm.smoothingTimeCoM_Joints(10); - 23*Sm.smoothingTimeCoM_Joints(10); - 24*Sm.smoothingTimeCoM_Joints(10); - 25*Sm.smoothingTimeCoM_Joints(10)]; - -% smoothing time vector for the second time the Yoga moveset are performed -Sm.joints_leftSecondYogaRef = Sm.smoothingTimeSecondYogaLeft.*(0:(length(Sm.joints_rightYogaRef(:,1))-1)); -Sm.joints_rightSecondYogaRef = Sm.smoothingTimeSecondYogaRight.*(0:(length(Sm.joints_rightYogaRef(:,1))-1)); - -% keep a high smoothing time for switching from the first to the second yoga moveset -Sm.joints_leftSecondYogaRef(1) = Sm.smoothingTimeCoM_Joints(4); -Sm.joints_rightSecondYogaRef(1) = Sm.smoothingTimeCoM_Joints(10); - -% if the demo is not "yogaExtended", stop at the 8th move -% also, Sm.repeatTwiceYogaWithDifferentSpeed must be set to "false". The -% reason is that the first and the last Yoga moveset for the "not extended" -% one are very different, and the robot may "jump" when restarting the Yoga -% the second time -if ~Sm.yogaExtended - - Sm.repeatTwiceYogaWithDifferentSpeed = false; - Sm.joints_leftYogaRef = Sm.joints_leftYogaRef(1:8,:); - Sm.joints_rightYogaRef = Sm.joints_rightYogaRef(1:8,:); - Sm.joints_leftYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(4); - Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10); -end - -% MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA -for i = 1:size(Sm.joints_rightYogaRef,1) - - Sm.joints_rightYogaRef(i,2:4) = [Sm.joints_rightYogaRef(i,2) -Sm.joints_rightYogaRef(i,3) -Sm.joints_rightYogaRef(i,4)]; - rightArm = Sm.joints_rightYogaRef(i,end-15:end-12); - Sm.joints_rightYogaRef(i,end-15:end-12) = Sm.joints_rightYogaRef(i,end-19:end-16); - Sm.joints_rightYogaRef(i,end-19:end-16) = rightArm; - rightLeg = Sm.joints_rightYogaRef(i,end-5:end); - Sm.joints_rightYogaRef(i,end-5:end) = Sm.joints_rightYogaRef(i,end-11:end-6); - Sm.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;0;0]; -Config.amplitudeOfOscillation = 0.0; -Config.frequencyOfOscillation = 0.0; - -%% Parameters for motors reflected inertia - -% transmission ratio -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 - -%% Cleanup -clear q1 q2 q3 q4; diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m deleted file mode 100644 index 348c5b5..0000000 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initCoordinator.m +++ /dev/null @@ -1,238 +0,0 @@ -% INITCOORDINATOR initializes the robot configuration for running -% 'COORDINATOR' demo. -% - -%% --- Initialization --- - -% Feet in contact (COORDINATOR DEMO ONLY) -Config.LEFT_RIGHT_FOOT_IN_CONTACT = [1 1]; - -% Initial foot on ground. If false, right foot is used as default contact -% frame (this does not means that the other foot cannot be in contact too). -% (COORDINATOR DEMO ONLY) -Config.LEFT_FOOT_IN_CONTACT_AT_0 = true; - -% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) -Config.DEMO_MOVEMENTS = true; - -% If equal to true, the desired streamed values of the center of mass -% are smoothed internally -Config.SMOOTH_COM_DES = false; - -% If equal to true, the desired streamed values of the postural tasks are -% smoothed internally -Config.SMOOTH_JOINT_DES = false; - -% torque saturation -Sat.torque = 34; - -% torque derivative max absolute value -Config.tauDot_maxAbs = 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] - -%% Control gains - -% PARAMETERS FOR TWO FEET BALANCING -if sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 2 - - Gain.KP_COM = diag([50 100 5]); - Gain.KD_COM = 2*sqrt(Gain.KP_COM)*0; - Gain.KP_AngularMomentum = 5; - Gain.KD_AngularMomentum = 2*sqrt(Gain.KP_AngularMomentum); - - % Impedances acting in the null space of the desired contact forces - impTorso = [10 10 20]; - - impArms = [10 10 10 8]; - - impLeftLeg = [30 30 30 60 10 10]; - - impRightLeg = [30 30 30 60 10 10]; -end - -% PARAMETERS FOR ONE FOOT BALANCING -if sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 1 - - Gain.KP_COM = diag([50 100 50]); - Gain.KD_COM = diag([0 0 0]); - Gain.KP_AngularMomentum = 1 ; - Gain.KD_AngularMomentum = 1 ; - - % Impedances acting in the null space of the desired contact forces - impTorso = [20 20 30]; - - impArms = [15 15 15 8]; - - impLeftLeg = [30 30 30 120 10 10]; - - impRightLeg = [30 30 30 60 10 10]; -end - -Gain.impedances = [impTorso(1,:),impArms(1,:),impArms(1,:),impLeftLeg(1,:),impRightLeg(1,:)]; -Gain.dampings = 0*sqrt(Gain.impedances); - -if (size(Gain.impedances,2) ~= ROBOT_DOF) - error('Dimension mismatch between ROBOT_DOF and dimension of the variable impedences. Check these variables in the file gains.m'); -end - -% Smoothing time gain scheduling (YOGA DEMO ONLY) -Gain.SmoothingTimeGainScheduling = 2; - -%% Parameters for motors reflected inertia - -% transmission ratio -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; - -%% References for CoM trajectory (COORDINATOR DEMO ONLY) - -% that the robot waits before starting the left-and-right -Config.noOscillationTime = 0; - -if Config.DEMO_MOVEMENTS && sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 2 - - Config.directionOfOscillation = [0;1;0]; - % amplitude of oscillations in meters - Config.amplitudeOfOscillation = 0.02; - % frequency of oscillations in hertz - Config.frequencyOfOscillation = 1; -else - Config.directionOfOscillation = [0;0;0]; - Config.amplitudeOfOscillation = 0.0; - Config.frequencyOfOscillation = 0.0; -end - -%% State machine parameters - -% smoothing time for joints and CoM -Sm.smoothingTimeCoM_Joints = 3; - -% scale factor smoothing time (YOGA DEMO ONLY) -Sm.scaleFactorSmoothingTime = 0.9; - -% time between two yoga positions (YOGA DEMO ONLY) -Sm.joints_pauseBetweenYogaMoves = 0; - -% contact forces threshold (YOGA DEMO ONLY) -Sm.wrench_thresholdContactOn = 1; -Sm.wrench_thresholdContactOff = 1; - -% threshold on CoM and joints error (YOGA DEMO ONLY) -Sm.CoM_threshold = 0; -Sm.joints_thresholdNotInContact = 0; -Sm.joints_thresholdInContact = 0; - -% initial state for state machine (YOGA DEMO ONLY) -Sm.stateAt0 = 1; - -% delta to be summed to the reference CoM position (YOGA DEMO ONLY) -Sm.CoM_delta = [0; 0; 0]; - -% joint references (YOGA DEMO ONLY) -Sm.joints_references = zeros(1,ROBOT_DOF); -Sm.joints_leftYogaRef = zeros(1,ROBOT_DOF+1); -Sm.joints_rightYogaRef = zeros(1,ROBOT_DOF+1); -Sm.joints_leftSecondYogaRef = zeros(1,ROBOT_DOF+1); -Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1); - -% configuration parameters for state machine (YOGA DEMO ONLY) -Sm.tBalancing = 1; -Sm.tBalancingBeforeYoga = 1; -Sm.skipYoga = false; -Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; -Sm.yogaAlsoOnRightFoot = false; -Sm.twoFeetYogaInLoop = false; -Sm.oneFootYogaInLoop = false; -Sm.yogaCounter = 5; -Sm.repeatTwiceYogaWithDifferentSpeed = false; -Sm.smoothingTimeSecondYogaLeft = 1; -Sm.smoothingTimeSecondYogaRight = 1; - -%% 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; - -% physical size of the foot -feet_size = [-0.07 0.12; % xMin, xMax - -0.04 0.04 ]; % yMin, yMax - -fZmin = 10; - -%% Regularization parameters -Reg.pinvDamp_nu_b = 1e-7; -Reg.pinvDamp = 2; -Reg.pinvTol = 1e-5; -Reg.impedances = 0.1; -Reg.dampings = 0; -Reg.HessianQP = 1e-5; \ No newline at end of file diff --git a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m b/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m deleted file mode 100644 index a15381c..0000000 --- a/torque-controllers/momentum-based-yoga/app/robots/iCubGenova04/initStateMachineYoga.m +++ /dev/null @@ -1,517 +0,0 @@ -% INITSTATEMACHINEYOGA initializes the robot configuration for running -% 'YOGA' demo. -% - -%% --- Initialization --- - -% Feet in contact (COORDINATOR DEMO ONLY) -Config.LEFT_RIGHT_FOOT_IN_CONTACT = [1 1]; - -% Initial foot on ground. If false, right foot is used as default contact -% frame (this does not means that the other foot cannot be in contact too). -% (COORDINATOR DEMO ONLY) -Config.LEFT_FOOT_IN_CONTACT_AT_0 = true; - -% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) -Config.DEMO_MOVEMENTS = false; - -% If equal to one, the desired streamed values of the center of mass -% are smoothed internally -Config.SMOOTH_COM_DES = true; - -% If equal to one, the desired streamed values of the postural tasks are -% smoothed internally -Config.SMOOTH_JOINT_DES = true; - -% torque saturation -Sat.torque = 60; - -% torque derivative max absolute value -Config.tauDot_maxAbs = 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_nu_b = 1e-7; -Reg.pinvDamp = 0.07; -Reg.pinvTol = 1e-5; -Reg.impedances = 0.1; -Reg.dampings = 0; -Reg.HessianQP = 1e-7; - -%% COM AND JOINT GAINS -Gain.KP_COM = [50 100 5 % state == 1 TWO FEET BALANCING - 50 100 5 % state == 2 COM TRANSITION TO LEFT - 50 100 5 % state == 3 LEFT FOOT BALANCING - 50 100 5 % state == 4 YOGA LEFT FOOT - 50 100 5 % state == 5 PREPARING FOR SWITCHING - 50 100 5 % state == 6 LOOKING FOR CONTACT - 50 100 5 % state == 7 TRANSITION TO INITIAL POSITION - 50 150 5 % state == 8 COM TRANSITION TO RIGHT FOOT - 50 100 5 % state == 9 RIGHT FOOT BALANCING - 50 100 5 % state == 10 YOGA RIGHT FOOT - 50 100 5 % state == 11 PREPARING FOR SWITCHING - 50 100 5 % state == 12 LOOKING FOR CONTACT - 50 100 5];% state == 13 TRANSITION TO INITIAL POSITION - -Gain.KD_COM = 2*sqrt(Gain.KP_COM)/15; - -Gain.KP_AngularMomentum = 3; -Gain.KD_AngularMomentum = 2*sqrt(Gain.KP_AngularMomentum)/5; - -% % TORSO %% LEFT ARM %% RIGHT ARM %% LEFT LEG %% RIGHT LEG %% -Gain.impedances = [10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 1 TWO FEET BALANCING - 10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 2 COM TRANSITION TO LEFT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 100 100, 30 30 20 20 100 100 % state == 3 LEFT FOOT BALANCING - 30 30 30, 10 10 10 10, 10 10 10 10,100 200 100 400 100 100,100 50 30 50 100 100 % state == 4 YOGA LEFT FOOT - 30 30 30, 5 5 10 10, 10 10 20 10,200 250 20 20 10 10,220 550 220 200 65 300 % state == 5 PREPARING FOR SWITCHING - 30 30 30, 10 10 20 10, 10 10 20 10,100 350 20 200 10 100,220 550 220 200 65 300 % state == 6 LOOKING FOR CONTACT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 5 5, 30 30 30 20 5 5 % state == 7 TRANSITION TO INITIAL POSITION - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 100 100, 30 30 30 20 100 100 % state == 8 COM TRANSITION TO RIGHT FOOT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 100 100, 30 30 20 20 100 100 % state == 9 RIGHT FOOT BALANCING - 30 30 30, 10 10 10 10, 10 10 10 10,100 50 30 50 100 100,100 200 100 100 10 10 % state == 10 YOGA RIGHT FOOT - 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,200 250 20 20 10 10 % state == 11 PREPARING FOR SWITCHING - 30 30 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100 % state == 12 LOOKING FOR CONTACT - 30 40 30, 10 10 10 10, 10 10 10 10,220 550 220 200 65 300,100 350 20 200 10 100];% state == 13 TRANSITION TO INITIAL POSITION - -Gain.impedances(4,1:3) = Gain.impedances(4,1:3)*3; -Gain.impedances(6,18:23) = Gain.impedances(6,18:23)*2; -Gain.impedances(10,1:3) = Gain.impedances(10,1:3)*3; -Gain.impedances(13,1:3) = Gain.impedances(13,1:3)*3; - -Gain.dampings = 0*sqrt(Gain.impedances(1,:)); - -% Smoothing time gain scheduling (YOGA DEMO ONLY) -Gain.SmoothingTimeGainScheduling = 2; - -%% STATE MACHINE PARMETERS - -% smoothing time for joints and CoM -Sm.smoothingTimeCoM_Joints = [1; %% state == 1 TWO FEET BALANCING - 1; %% state == 2 COM TRANSITION TO LEFT FOOT - 1; %% state == 3 LEFT FOOT BALANCING - 0.9; %% state == 4 YOGA LEFT FOOT - 2; %% state == 5 PREPARING FOR SWITCHING - 2; %% state == 6 LOOKING FOR CONTACT - 1; %% state == 7 TRANSITION INIT POSITION - 1; %% state == 8 COM TRANSITION TO RIGHT FOOT - 1; %% state == 9 RIGHT FOOT BALANCING - 0.9; %% state == 10 YOGA RIGHT FOOT - 2; %% state == 11 PREPARING FOR SWITCHING - 2; %% state == 12 LOOKING FOR CONTACT - 5]; %% state == 13 TRANSITION INIT POSITION - -% scale factor smoothing time multiplies the smoothing factor during the -% Yoga (state 4 and 10). The purpose is to reduce the time necessary for -% the reference to converge to the next position, but without changing also -% the valuse stored in Sm.joints_leftYogaRef/Sm.joints_rightYogaRef (YOGA DEMO ONLY) -Sm.scaleFactorSmoothingTime = 0.9; - -% time between two yoga positions (YOGA DEMO ONLY) -Sm.joints_pauseBetweenYogaMoves = 5; - -% contact forces threshold (YOGA DEMO ONLY) -Sm.wrench_thresholdContactOn = 50; -Sm.wrench_thresholdContactOff = 100; - -% threshold on CoM and joints error (YOGA DEMO ONLY) -Sm.CoM_threshold = 0.02; -Sm.joints_thresholdNotInContact = 15; -Sm.joints_thresholdInContact = 50; - -% initial state for state machine (YOGA DEMO ONLY) -Sm.stateAt0 = 1; - -% delta to be summed to the reference CoM position (YOGA DEMO ONLY) - -Sm.CoM_delta = [% THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE LEFT FOOT - 0.0, 0.00, 0.0; %% NOT USED - 0.0, 0.00, 0.0; %% state == 2 COM TRANSITION TO LEFT FOOT - 0.0, 0.005, 0.0; %% state == 3 LEFT FOOT BALANCING - 0.0, 0.005, 0.0; %% state == 4 YOGA LEFT FOOT - 0.0, 0.005, 0.0; %% state == 5 PREPARING FOR SWITCHING - 0.02,-0.09, 0.0; %% state == 6 LOOKING FOR CONTACT - 0.0, 0.00, 0.0; %% NOT USED - % THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE RIGHT FOOT - 0.0, 0.00, 0.0; %% state == 8 COM TRANSITION TO RIGHT FOOT - 0.0, -0.005, 0.0; %% state == 9 RIGHT FOOT BALANCING - 0.0, -0.005, 0.0; %% state == 10 YOGA RIGHT FOOT - 0.0, -0.015, 0.0; %% state == 11 PREPARING FOR SWITCHING - 0.02, 0.02, 0.0; %% state == 12 LOOKING FOR CONTACT - 0.0, 0.00, 0.0]; %% NOT USED - -% configuration parameters for state machine (YOGA DEMO ONLY) -Sm.tBalancing = 1; -Sm.tBalancingBeforeYoga = 1; -Sm.yogaExtended = true; -Sm.skipYoga = false; -Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first -Sm.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot (not available for now) - -%%%% List of possible "Yoga in loop" %%%% - -% the robot will repeat the FULL DEMO (two feet balancing, yoga on left -% foot, back on two feet, yoga right foot, back on two feet). The demo is -% repeated until the user stops the Simulink model. This option is ignored -% if Sm.demoStartsOnRightSupport = true. -Sm.twoFeetYogaInLoop = false; - -% the robot will repeat the ONE FOOT yoga for the number of times the user -% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two -% feet balancing in between to consecutive yoga. WARNING: if the option -% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga -% on left foot for the number of times the user specifies in the Sm.yogaCounter, -% and then it will repeat the yoga on the right foot for the same number of times. -% This option is ignored if Sm.repeatTwiceYogaWithDifferentSpeed = true. -Sm.oneFootYogaInLoop = false; -Sm.yogaCounter = 10; - -% the robot will repeat the yoga moveset twice. This option works as the -% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However, -% it is possible to set a different yoga speed for the two yoga. -% Uneffective if Sm.yogaExtended = false; -Sm.repeatTwiceYogaWithDifferentSpeed = false; - -% smoothing time for the second time the Yoga moveset are performed -Sm.smoothingTimeSecondYogaLeft = 0.7; -Sm.smoothingTimeSecondYogaRight = 0.7; - -%% Joint references (YOGA DEMO ONLY) -Sm.joints_references = [ zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED - [-0.0348,0.0779,0.0429, ... %% state == 2 COM TRANSITION TO LEFT - -0.1493,0.8580,0.2437,0.8710, ... % - -0.1493,0.8580,0.2437,0.8710, ... % - -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % - 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % - [ 0.0864,0.0258,0.0152, ... %% state == 3 LEFT FOOT BALANCING - 0.1253,0.8135,0.3051,0.7928, ... % - 0.0563,0.6789,0.3340,0.6214, ... % - -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % - 0.0005,0.0793,-0.0014,-0.0051,-0.1060,-0.1151]; % - zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED - [-0.0348,0.0779,0.0429, ... %% state == 5 PREPARING FOR SWITCHING - -0.1493,0.8580,0.2437,0.8710, ... % - -0.1493,0.8580,0.2437,0.8710, ... % - -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ... % - 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151]; % - [ 0.0864,0.0258,0.0152, ... %% state == 6 LOOKING FOR CONTACT - 0.1253,0.8135,0.3051,0.7928, ... % - 0.0563,0.6789,0.3340,0.6214, ... % - 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % - -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % - zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED - [ 0.0864,0.0258,0.0152, ... %% state == 8 COM TRANSITION TO RIGHT FOOT - 0.1253,0.8135,0.3051,0.7928, ... % - 0.0563,0.6789,0.3340,0.6214, ... % - 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ... % - -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277]; % - [ 0.0864,0.0258,0.0152, ... %% state == 9 RIGHT FOOT BALANCING - 0.1253,0.8135,0.3051,0.7928, ... % - 0.0563,0.6789,0.3340,0.6214, ... % - 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % - -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % - zeros(1,ROBOT_DOF); %% THIS REFERENCE IS IGNORED - [-0.0348,0.0779,0.0429, ... %% state == 11 PREPARING FOR SWITCHING - -0.1493,0.8580,0.2437,0.8710, ... % - -0.1493,0.8580,0.2437,0.8710, ... % - 0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ... % - -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630]; % - [ 0.0864,0.0258,0.0152, ... %% state == 12 LOOKING FOR CONTACT - 0.1253,0.8135,0.3051,0.7928, ... % - 0.0563,0.6789,0.3340,0.6214, ... % - -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277, ... % - 0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369]; % - zeros(1,ROBOT_DOF)]; %% THIS REFERENCE IS IGNORED - -% YOGA MOVESET (LEFT YOGA, EXTENDED) -q1 = [-0.0790,0.2279, 0.4519, ... - -1.1621,0.6663, 0.4919, 0.9947, ... - -1.0717,1.2904,-0.2447, 1.0948, ... - 0.2092,0.2060, 0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3484,0.4008,-0.0004,-0.3672,-0.1060,-0.0875]; - -q2 = [-0.0790,0.1279, 0.4519, ... - -1.1621,0.6663, 0.4965, 0.9947, ... - -1.0717,1.2904,-0.2493, 1.0948, ... - 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... - 0.3714,0.9599, 1.3253,-1.6594,-0.1060,-0.0614]; - -q3 = [-0.0852,-0.3273,0.0821,... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ... - 0.3714,0.9599, 1.3253,-1.6594, 0.5000,-0.0614]; - -q4 = [-0.0852,-0.4273,0.0821,... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.3473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; - -q5 = [-0.0790,-0.2273, 0.4519, ... - -1.1621,0.6663, 0.4965, 0.9947, ... - -1.0717,1.2904,-0.2493, 1.0948, ... - 0.2092, 0.4473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; - -q6 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; - -q7 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 1.3107,1.3253, -1.6217, 0.5000,-0.0614]; - -q8 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; - -q9 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614]; - -q10 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; - -q11 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.8514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; - -q12 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.8514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614]; - -q13 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.8514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; - -q14 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.8514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614]; - -q15 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 1.5514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614]; - -q16 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.2514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614]; - -q17 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - -0.3514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614]; - -q18 = [-0.0852,-0.4273,0.0821, ... - 0.1391, 1.4585,0.2464, 0.3042, ... - -0.4181, 1.6800,0.7373, 0.3031, ... - 0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614]; - -Sm.joints_leftYogaRef = [ 0, q1; - 1*Sm.smoothingTimeCoM_Joints(4),q2; - 2*Sm.smoothingTimeCoM_Joints(4),q3; - 3*Sm.smoothingTimeCoM_Joints(4),q4; - 4*Sm.smoothingTimeCoM_Joints(4),q5; - 5*Sm.smoothingTimeCoM_Joints(4),q6; - 6*Sm.smoothingTimeCoM_Joints(4),q7; - 7*Sm.smoothingTimeCoM_Joints(4),q8; - 8*Sm.smoothingTimeCoM_Joints(4),q9; - 9*Sm.smoothingTimeCoM_Joints(4),q10; - 10*Sm.smoothingTimeCoM_Joints(4),q11; - 11*Sm.smoothingTimeCoM_Joints(4),q12; - 12*Sm.smoothingTimeCoM_Joints(4),q13; - 13*Sm.smoothingTimeCoM_Joints(4),q14; - 14*Sm.smoothingTimeCoM_Joints(4),q15; - 15*Sm.smoothingTimeCoM_Joints(4),q16; - 16*Sm.smoothingTimeCoM_Joints(4),q17; - 17*Sm.smoothingTimeCoM_Joints(4),q10; - 18*Sm.smoothingTimeCoM_Joints(4),q11; - 19*Sm.smoothingTimeCoM_Joints(4),q12; - 20*Sm.smoothingTimeCoM_Joints(4),q13; - 21*Sm.smoothingTimeCoM_Joints(4),q14; - 22*Sm.smoothingTimeCoM_Joints(4),q15; - 23*Sm.smoothingTimeCoM_Joints(4),q16; - 24*Sm.smoothingTimeCoM_Joints(4),q17; - 25*Sm.smoothingTimeCoM_Joints(4),q18]; - -Sm.joints_rightYogaRef = Sm.joints_leftYogaRef; -Sm.joints_rightYogaRef(:,1) = [0, ; - 1*Sm.smoothingTimeCoM_Joints(10); - 2*Sm.smoothingTimeCoM_Joints(10); - 3*Sm.smoothingTimeCoM_Joints(10); - 4*Sm.smoothingTimeCoM_Joints(10); - 5*Sm.smoothingTimeCoM_Joints(10); - 6*Sm.smoothingTimeCoM_Joints(10); - 7*Sm.smoothingTimeCoM_Joints(10); - 8*Sm.smoothingTimeCoM_Joints(10); - 9*Sm.smoothingTimeCoM_Joints(10); - 10*Sm.smoothingTimeCoM_Joints(10); - 11*Sm.smoothingTimeCoM_Joints(10); - 12*Sm.smoothingTimeCoM_Joints(10); - 13*Sm.smoothingTimeCoM_Joints(10); - 14*Sm.smoothingTimeCoM_Joints(10); - 15*Sm.smoothingTimeCoM_Joints(10); - 16*Sm.smoothingTimeCoM_Joints(10); - 17*Sm.smoothingTimeCoM_Joints(10); - 18*Sm.smoothingTimeCoM_Joints(10); - 19*Sm.smoothingTimeCoM_Joints(10); - 20*Sm.smoothingTimeCoM_Joints(10); - 21*Sm.smoothingTimeCoM_Joints(10); - 22*Sm.smoothingTimeCoM_Joints(10); - 23*Sm.smoothingTimeCoM_Joints(10); - 24*Sm.smoothingTimeCoM_Joints(10); - 25*Sm.smoothingTimeCoM_Joints(10)]; - -% smoothing time vector for the second time the Yoga moveset are performed -Sm.joints_leftSecondYogaRef = Sm.smoothingTimeSecondYogaLeft.*(0:(length(Sm.joints_rightYogaRef(:,1))-1)); -Sm.joints_rightSecondYogaRef = Sm.smoothingTimeSecondYogaRight.*(0:(length(Sm.joints_rightYogaRef(:,1))-1)); - -% keep a high smoothing time for switching from the first to the second yoga moveset -Sm.joints_leftSecondYogaRef(1) = Sm.smoothingTimeCoM_Joints(4); -Sm.joints_rightSecondYogaRef(1) = Sm.smoothingTimeCoM_Joints(10); - -% if the demo is not "yogaExtended", stop at the 8th move -% also, Sm.repeatTwiceYogaWithDifferentSpeed must be set to "false". The -% reason is that the first and the last Yoga moveset for the "not extended" -% one are very different, and the robot may "jump" when restarting the Yoga -% the second time -if ~Sm.yogaExtended - - Sm.repeatTwiceYogaWithDifferentSpeed = false; - Sm.joints_leftYogaRef = Sm.joints_leftYogaRef(1:8,:); - Sm.joints_rightYogaRef = Sm.joints_rightYogaRef(1:8,:); - Sm.joints_leftYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(4); - Sm.joints_rightYogaRef(8,1) = 15*Sm.smoothingTimeCoM_Joints(10); -end - -% MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA -for i = 1:size(Sm.joints_rightYogaRef,1) - - Sm.joints_rightYogaRef(i,2:4) = [Sm.joints_rightYogaRef(i,2) -Sm.joints_rightYogaRef(i,3) -Sm.joints_rightYogaRef(i,4)]; - rightArm = Sm.joints_rightYogaRef(i,end-15:end-12); - Sm.joints_rightYogaRef(i,end-15:end-12) = Sm.joints_rightYogaRef(i,end-19:end-16); - Sm.joints_rightYogaRef(i,end-19:end-16) = rightArm; - rightLeg = Sm.joints_rightYogaRef(i,end-5:end); - Sm.joints_rightYogaRef(i,end-5:end) = Sm.joints_rightYogaRef(i,end-11:end-6); - Sm.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;0;0]; -Config.amplitudeOfOscillation = 0.0; -Config.frequencyOfOscillation = 0.0; - -%% Parameters for motors reflected inertia -% transmission ratio -Config.Gamma = 0.01*eye(ROBOT_DOF); - -% modify the value of the transmission ratio for the hip pitch. -% TODO: avoid to hard-code the joint numbering -Config.Gamma(end-5, end-5) = 0.0067; -Config.Gamma(end-11,end-11) = 0.0067; - -% motors inertia (Kg*m^2) -legsMotors_I_m = 0.0827*1e-4; -torsoPitchRollMotors_I_m = 0.0827*1e-4; -torsoYawMotors_I_m = 0.0585*1e-4; -armsMotors_I_m = 0.0585*1e-4; - -% add harmonic drives reflected inertia -if Config.INCLUDE_HARMONIC_DRIVE_INERTIA - - legsMotors_I_m = legsMotors_I_m + 0.054*1e-4; - torsoPitchRollMotors_I_m = torsoPitchRollMotors_I_m + 0.054*1e-4; - torsoYawMotors_I_m = torsoYawMotors_I_m + 0.021*1e-4; - armsMotors_I_m = armsMotors_I_m + 0.021*1e-4; -end - -Config.I_m = diag([torsoPitchRollMotors_I_m*ones(2,1); - torsoYawMotors_I_m; - armsMotors_I_m*ones(8,1); - legsMotors_I_m*ones(12,1)]); - -% parameters for coupling matrices. Updated according to the wiki: -% -% http://wiki.icub.org/wiki/ICub_coupled_joints -% -% and corrected according to https://github.com/robotology/robots-configuration/issues/39 -t = 0.615; -r = 0.022; -R = 0.04; - -% coupling matrices -T_LShoulder = [-1 0 0; - -1 -t 0; - 0 t -t]; - -T_RShoulder = [ 1 0 0; - 1 t 0; - 0 -t t]; - -T_torso = [ 0.5 -0.5 0; - 0.5 0.5 0; - r/(2*R) r/(2*R) r/R]; - -if Config.INCLUDE_COUPLING - - Config.T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12)); -else - Config.T = eye(ROBOT_DOF); -end - -% gain for feedforward term in joint torques calculation. Valid range: a -% value between 0 and 1 -Config.K_ff = 1; - -% Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA if true, the desired joints -% accelerations are used for computing the feedforward term in joint -% torques calculations. Not effective if Config.K_ff = 0. -Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA = true; - -%% Constraints for QP for balancing - -% The friction cone is approximated by using linear interpolation of the circle. -% So, numberOfPoints defines the number of points used to interpolate the circle -% in each cicle's quadrant -numberOfPoints = 4; -forceFrictionCoefficient = 1/3; -torsionalFrictionCoefficient = 1/75; -fZmin = 10; - -% physical size of the foot -feet_size = [-0.07 0.12 ; % xMin, xMax - -0.045 0.05 ]; % yMin, yMax - -%% Cleanup -clear q1 q2 q3 q4; diff --git a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m b/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m deleted file mode 100644 index e3384c1..0000000 --- a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initCoordinator.m +++ /dev/null @@ -1,238 +0,0 @@ -% INITCOORDINATOR initializes the robot configuration for running -% 'COORDINATOR' demo. -% - -%% --- Initialization --- - -% Feet in contact (COORDINATOR DEMO ONLY) -Config.LEFT_RIGHT_FOOT_IN_CONTACT = [1 1]; - -% Initial foot on ground. If false, right foot is used as default contact -% frame (this does not means that the other foot cannot be in contact too). -% (COORDINATOR DEMO ONLY) -Config.LEFT_FOOT_IN_CONTACT_AT_0 = true; - -% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) -Config.DEMO_MOVEMENTS = false; - -% If equal to true, the desired streamed values of the center of mass -% are smoothed internally -Config.SMOOTH_COM_DES = false; - -% If equal to true, the desired streamed values of the postural tasks are -% smoothed internally -Config.SMOOTH_JOINT_DES = false; - -% torque saturation -Sat.torque = 34; - -% torque derivative max absolute value -Config.tauDot_maxAbs = 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] - -%% Control gains - -% PARAMETERS FOR TWO FEET BALANCING -if sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 2 - - Gain.KP_COM = diag([50 50 50]); - Gain.KD_COM = 2*sqrt(Gain.KP_COM)*0; - Gain.KP_AngularMomentum = 10 ; - Gain.KD_AngularMomentum = 2*sqrt(Gain.KP_AngularMomentum); - - % Impedances acting in the null space of the desired contact forces - impTorso = [10 10 20]; - - impArms = [10 10 10 8]; - - impLeftLeg = [30 30 30 60 10 10]; - - impRightLeg = [30 30 30 60 10 10]; -end - -% PARAMETERS FOR ONE FOOT BALANCING -if sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 1 - - Gain.KP_COM = diag([50 100 50]); - Gain.KD_COM = diag([0 0 0]); - Gain.KP_AngularMomentum = 1 ; - Gain.KD_AngularMomentum = 1 ; - - % Impedances acting in the null space of the desired contact forces - impTorso = [20 20 30]; - - impArms = [15 15 15 8]; - - impLeftLeg = [30 30 30 120 10 10]; - - impRightLeg = [30 30 30 60 10 10]; -end - -Gain.impedances = [impTorso(1,:),impArms(1,:),impArms(1,:),impLeftLeg(1,:),impRightLeg(1,:)]; -Gain.dampings = 0*sqrt(Gain.impedances); - -if (size(Gain.impedances,2) ~= ROBOT_DOF) - error('Dimension mismatch between ROBOT_DOF and dimension of the variable impedences. Check these variables in the file gains.m'); -end - -% Smoothing time gain scheduling (YOGA DEMO ONLY) -Gain.SmoothingTimeGainScheduling = 0.02; - -%% Parameters for motors reflected inertia - -% transmission ratio -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; - -%% References for CoM trajectory (COORDINATOR DEMO ONLY) - -% that the robot waits before starting the left-and-right -Config.noOscillationTime = 0; - -if Config.DEMO_MOVEMENTS && sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 2 - - Config.directionOfOscillation = [0;1;0]; - % amplitude of oscillations in meters - Config.amplitudeOfOscillation = 0.02; - % frequency of oscillations in hertz - Config.frequencyOfOscillation = 0.2; -else - Config.directionOfOscillation = [0;0;0]; - Config.amplitudeOfOscillation = 0.0; - Config.frequencyOfOscillation = 0.0; -end - -%% State machine parameters - -% smoothing time for joints and CoM -Sm.smoothingTimeCoM_Joints = 1; - -% scale factor smoothing time (YOGA DEMO ONLY) -Sm.scaleFactorSmoothingTime = 0.9; - -% time between two yoga positions (YOGA DEMO ONLY) -Sm.joints_pauseBetweenYogaMoves = 0; - -% contact forces threshold (YOGA DEMO ONLY) -Sm.wrench_thresholdContactOn = 1; -Sm.wrench_thresholdContactOff = 1; - -% threshold on CoM and joints error (YOGA DEMO ONLY) -Sm.CoM_threshold = 0; -Sm.joints_thresholdNotInContact = 0; -Sm.joints_thresholdInContact = 0; - -% initial state for state machine (YOGA DEMO ONLY) -Sm.stateAt0 = 1; - -% delta to be summed to the reference CoM position (YOGA DEMO ONLY) -Sm.CoM_delta = [0; 0; 0]; - -% joint references (YOGA DEMO ONLY) -Sm.joints_references = zeros(1,ROBOT_DOF); -Sm.joints_leftYogaRef = zeros(1,ROBOT_DOF+1); -Sm.joints_rightYogaRef = zeros(1,ROBOT_DOF+1); -Sm.joints_leftSecondYogaRef = zeros(1,ROBOT_DOF+1); -Sm.joints_rightSecondYogaRef = zeros(1,ROBOT_DOF+1); - -% configuration parameters for state machine (YOGA DEMO ONLY) -Sm.tBalancing = 1; -Sm.tBalancingBeforeYoga = 1; -Sm.skipYoga = false; -Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; -Sm.yogaAlsoOnRightFoot = false; -Sm.twoFeetYogaInLoop = false; -Sm.oneFootYogaInLoop = false; -Sm.yogaCounter = 5; -Sm.repeatTwiceYogaWithDifferentSpeed = false; -Sm.smoothingTimeSecondYogaLeft = 1; -Sm.smoothingTimeSecondYogaRight = 1; - -%% 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; -torsionalFrictionCoefficient = 1/75; - -% physical size of the foot -feet_size = [-0.07 0.07; % xMin, xMax - -0.03 0.03]; % yMin, yMax - -fZmin = 10; - -%% Regularization parameters -Reg.pinvDamp_nu_b = 1e-7; -Reg.pinvDamp = 0.01; -Reg.pinvTol = 1e-5; -Reg.impedances = 0.1; -Reg.dampings = 0; -Reg.HessianQP = 1e-5; diff --git a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m b/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m deleted file mode 100644 index d2d34f3..0000000 --- a/torque-controllers/momentum-based-yoga/app/robots/icubGazeboSim/initStateMachineYoga.m +++ /dev/null @@ -1,398 +0,0 @@ -% INITSTATEMACHINEYOGA initializes the robot configuration for running -% 'YOGA' demo. -% - -%% --- Initialization --- - -% Feet in contact (COORDINATOR DEMO ONLY) -Config.LEFT_RIGHT_FOOT_IN_CONTACT = [1 1]; - -% Initial foot on ground. If false, right foot is used as default contact -% frame (this does not means that the other foot cannot be in contact too). -% (COORDINATOR DEMO ONLY) -Config.LEFT_FOOT_IN_CONTACT_AT_0 = true; - -% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY) -Config.DEMO_MOVEMENTS = false; - -% If equal to one, the desired streamed values of the center of mass -% are smoothed internally -Config.SMOOTH_COM_DES = true; - -% If equal to one, the desired streamed values of the postural tasks are -% smoothed internally -Config.SMOOTH_JOINT_DES = true; - -% torque saturation -Sat.torque = 60; - -% torque derivative max absolute value -Config.tauDot_maxAbs = 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_nu_b = 1e-7; -Reg.pinvDamp = 1; -Reg.pinvTol = 1e-5; -Reg.impedances = 0.1; -Reg.dampings = 0; -Reg.HessianQP = 1e-7; - -%% COM AND JOINT GAINS -Gain.KP_COM = [10 50 10 % state == 1 TWO FEET BALANCING - 10 50 10 % state == 2 COM TRANSITION TO LEFT - 10 50 10 % state == 3 LEFT FOOT BALANCING - 10 50 10 % state == 4 YOGA LEFT FOOT - 10 50 10 % state == 5 PREPARING FOR SWITCHING - 10 50 10 % state == 6 LOOKING FOR CONTACT - 10 50 10 % state == 7 TRANSITION TO INITIAL POSITION - 10 50 10 % state == 8 COM TRANSITION TO RIGHT FOOT - 10 50 10 % state == 9 RIGHT FOOT BALANCING - 10 50 10 % state == 10 YOGA RIGHT FOOT - 10 50 10 % state == 11 PREPARING FOR SWITCHING - 10 50 10 % state == 12 LOOKING FOR CONTACT - 10 50 10];% state == 13 TRANSITION TO INITIAL POSITION - -Gain.KD_COM = 2*sqrt(Gain.KP_COM); - -Gain.KP_AngularMomentum = 0.25 ; -Gain.KD_AngularMomentum = 2*sqrt(Gain.KP_AngularMomentum); - -% % TORSO %% LEFT ARM %% RIGHT ARM %% LEFT LEG %% RIGHT LEG %% -Gain.impedances = [10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 1 TWO FEET BALANCING - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 2 COM TRANSITION TO LEFT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 3 LEFT FOOT BALANCING - 30 30 30, 10 10 10 10, 10 10 10 10, 50 50 250 200 50 50, 50 50 50 50 50 50 % state == 4 YOGA LEFT FOOT - 30 30 30, 10 10 10 10, 10 10 10 10, 30 50 300 60 50 50, 30 50 30 60 50 50 % state == 5 PREPARING FOR SWITCHING - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 6 LOOKING FOR CONTACT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 7 TRANSITION TO INITIAL POSITION - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 8 COM TRANSITION TO RIGHT FOOT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 9 RIGHT FOOT BALANCING - 30 30 30, 10 10 10 10, 10 10 10 10, 50 50 50 50 50 50, 50 50 250 200 50 50 % state == 10 YOGA RIGHT FOOT - 30 30 30, 10 10 10 10, 10 10 10 10, 30 50 30 60 50 50, 30 50 300 60 50 50 % state == 11 PREPARING FOR SWITCHING - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50 % state == 12 LOOKING FOR CONTACT - 10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 50 50, 30 50 30 60 50 50];% state == 13 TRANSITION TO INITIAL POSITION - -Gain.dampings = 0*sqrt(Gain.impedances(1,:)); - -% Smoothing time gain scheduling (YOGA DEMO ONLY) -Gain.SmoothingTimeGainScheduling = 2; - -%% STATE MACHINE PARMETERS - -% smoothing time for joints and CoM -Sm.smoothingTimeCoM_Joints = [5; %% state == 1 TWO FEET BALANCING - 5; %% state == 2 COM TRANSITION TO LEFT FOOT - 3; %% state == 3 LEFT FOOT BALANCING - 4; %% state == 4 YOGA LEFT FOOT - 5; %% state == 5 PREPARING FOR SWITCHING - 5; %% state == 6 LOOKING FOR CONTACT - 4; %% state == 7 TRANSITION INIT POSITION - 5; %% state == 8 COM TRANSITION TO RIGHT FOOT - 3; %% state == 9 RIGHT FOOT BALANCING - 4; %% state == 10 YOGA RIGHT FOOT - 5; %% state == 11 PREPARING FOR SWITCHING - 5; %% state == 12 LOOKING FOR CONTACT - 4]; %% state == 13 TRANSITION INIT POSITION - -% scale factor smoothing time multiplies the smoothing factor during the -% Yoga (state 4 and 10). The purpose is to reduce the time necessary for -% the reference to converge to the next position, but without changing also -% the valuse stored in Sm.joints_leftYogaRef/Sm.joints_rightYogaRef (YOGA DEMO ONLY) -Sm.scaleFactorSmoothingTime = 0.9; - -% time between two yoga positions (YOGA DEMO ONLY) -Sm.joints_pauseBetweenYogaMoves = 3; - -% contact forces threshold (YOGA DEMO ONLY) -Sm.wrench_thresholdContactOn = 25; -Sm.wrench_thresholdContactOff = 85; - -% threshold on CoM and joints error (YOGA DEMO ONLY) -Sm.CoM_threshold = 0.01; -Sm.joints_thresholdNotInContact = 5; -Sm.joints_thresholdInContact = 50; - -% initial state for state machine (YOGA DEMO ONLY) -Sm.stateAt0 = 1; - -% delta to be summed to the reference CoM position (YOGA DEMO ONLY) - -Sm.CoM_delta = [% THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE LEFT FOOT - 0.0, 0.00, 0.0; %% NOT USED - 0.0, 0.01, 0.0; %% state == 2 COM TRANSITION TO LEFT FOOT - 0.0, 0.00, 0.0; %% state == 3 LEFT FOOT BALANCING - 0.0, 0.01, 0.0; %% state == 4 YOGA LEFT FOOT - 0.0, 0.00, 0.0; %% state == 5 PREPARING FOR SWITCHING - 0.0, -0.09, 0.0; %% state == 6 LOOKING FOR CONTACT - 0.0, 0.00, 0.0; %% NOT USED - % THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE RIGHT FOOT - 0.0, -0.01, 0.0; %% state == 8 COM TRANSITION TO RIGHT FOOT - 0.0, 0.00, 0.0; %% state == 9 RIGHT FOOT BALANCING - 0.0, -0.00, 0.0; %% state == 10 YOGA RIGHT FOOT - 0.0, -0.00, 0.0; %% state == 11 PREPARING FOR SWITCHING - 0.0, 0.09, 0.0; %% state == 12 LOOKING FOR CONTACT - 0.0, 0.00, 0.0]; %% NOT USED - -% configuration parameters for state machine (YOGA DEMO ONLY) -Sm.tBalancing = 1; -Sm.tBalancingBeforeYoga = 1; -Sm.skipYoga = false; -Sm.demoOnlyBalancing = false; -Sm.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first -Sm.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot (not available for now) - -%%%% List of possible "Yoga in loop" %%%% - -% the robot will repeat the FULL DEMO (two feet balancing, yoga on left -% foot, back on two feet, yoga right foot, back on two feet). The demo is -% repeated until the user stops the Simulink model. This option is ignored -% if Sm.demoStartsOnRightSupport = true. -Sm.twoFeetYogaInLoop = false; - -% the robot will repeat the ONE FOOT yoga for the number of times the user -% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two -% feet balancing in between to consecutive yoga. WARNING: if the option -% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga -% on left foot for the number of times the user specifies in the Sm.yogaCounter, -% and then it will repeat the yoga on the right foot for the same number of times. -% This option is ignored if Sm.repeatTwiceYogaWithDifferentSpeed = true. -Sm.oneFootYogaInLoop = false; -Sm.yogaCounter = 5; - -% the robot will repeat the yoga moveset twice. This option works as the -% option Sm.oneFootYogaInLoop, but the yoga is repeated only twice. However, -% it is possible to set a different yoga speed for the two yoga. -% (NOT AVAILABLE FOR ICUBGAZEBOSIM) -Sm.repeatTwiceYogaWithDifferentSpeed = false; - -% smoothing time for the second time the Yoga moveset are performed -Sm.smoothingTimeSecondYogaLeft = 1.2; -Sm.smoothingTimeSecondYogaRight = 1.2; - -%% Joint references (YOGA DEMO ONLY) -Sm.joints_references = [zeros(1,ROBOT_DOF); %% NOT USED - [-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); %% NOT USED - [-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); %% NOT USED - [ 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); %% NOT USED - [-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)]; %% NOT USED - -% YOGA MOVESET (LEFT YOGA) -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.2960, 0.0006,-0.1741,-0.1044, 0.0700, ... - 0.3484,0.4008,-0.0004,-0.3672,-0.0530,-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.2960, 0.0006,-0.1741,-0.1044,0.0700, ... - 0.3714,0.9599, 1.3253,-1.6594, 0.6374,-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.2960, 0.0006,-0.1741,-0.1044,0.0700, ... - 0.3714,0.9599, 1.3253,-1.6594, 0.6374,-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.6374,-0.0614]; - -q5 = [-0.0790,-0.1273, 0.4519, ... - -1.1621,0.6663, 0.4965, 0.9947, ... - -1.0717,1.2904,-0.2493, 1.0948, ... - 0.2092, 0.3473,0.0006,-0.1741,-0.1044, 0.0700,... - 0.3514, 1.3107,1.3253,-0.0189, 0.6374,-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.3473,0.0006,-0.1741,-0.1044, 0.0700,... - 0.3514, 1.3107,1.3253,-0.0189, 0.6374,-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.3473,0.0006,-0.1741,-0.1044, 0.0700,... - 0.3514, 1.3107,1.3253, -1.6217, 0.6374,-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.3473,0.0006,-0.1741,-0.1044, 0.0700,... - 0.3514, 1.3107,1.3253,-0.0189, 0.6374,-0.0614]; - -Sm.joints_leftYogaRef = [0, q1; - 1*Sm.smoothingTimeCoM_Joints(4),q2; - 2*Sm.smoothingTimeCoM_Joints(4),q3; - 3*Sm.smoothingTimeCoM_Joints(4),q4; - 4*Sm.smoothingTimeCoM_Joints(4),q5; - 5*Sm.smoothingTimeCoM_Joints(4),q6; - 6*Sm.smoothingTimeCoM_Joints(4),q7; - 7*Sm.smoothingTimeCoM_Joints(4),q8]; - -Sm.joints_rightYogaRef = Sm.joints_leftYogaRef; -Sm.joints_rightYogaRef(:,1) = [0; - 1*Sm.smoothingTimeCoM_Joints(10); - 2*Sm.smoothingTimeCoM_Joints(10); - 3*Sm.smoothingTimeCoM_Joints(10); - 4*Sm.smoothingTimeCoM_Joints(10); - 5*Sm.smoothingTimeCoM_Joints(10); - 6*Sm.smoothingTimeCoM_Joints(10); - 7*Sm.smoothingTimeCoM_Joints(10)]; - -% smoothing time vector for the second time the Yoga moveset are performed (NOT USED) -Sm.joints_leftSecondYogaRef = Sm.smoothingTimeSecondYogaLeft.*(0:(length(Sm.joints_rightYogaRef(:,1))-1)); -Sm.joints_rightSecondYogaRef = Sm.smoothingTimeSecondYogaRight.*(0:(length(Sm.joints_rightYogaRef(:,1))-1)); - -% MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA -for i = 1:size(Sm.joints_rightYogaRef,1) - - Sm.joints_rightYogaRef(i,2:4) = [Sm.joints_rightYogaRef(i,2) -Sm.joints_rightYogaRef(i,3) -Sm.joints_rightYogaRef(i,4)]; - rightArm = Sm.joints_rightYogaRef(i,end-15:end-12); - Sm.joints_rightYogaRef(i,end-15:end-12) = Sm.joints_rightYogaRef(i,end-19:end-16); - Sm.joints_rightYogaRef(i,end-19:end-16) = rightArm; - rightLeg = Sm.joints_rightYogaRef(i,end-5:end); - Sm.joints_rightYogaRef(i,end-5:end) = Sm.joints_rightYogaRef(i,end-11:end-6); - Sm.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;0;0]; -Config.amplitudeOfOscillation = 0.0; -Config.frequencyOfOscillation = 0.0; - -%% Parameters for motors reflected inertia - -% transmission ratio -Config.Gamma = 0.01*eye(ROBOT_DOF); - -% modify the value of the transmission ratio for the hip pitch. -% TODO: avoid to hard-code the joint numbering -Config.Gamma(end-5, end-5) = 0.0067; -Config.Gamma(end-11,end-11) = 0.0067; - -% motors inertia (Kg*m^2) -legsMotors_I_m = 0.0827*1e-4; -torsoPitchRollMotors_I_m = 0.0827*1e-4; -torsoYawMotors_I_m = 0.0585*1e-4; -armsMotors_I_m = 0.0585*1e-4; - -% add harmonic drives reflected inertia -if Config.INCLUDE_HARMONIC_DRIVE_INERTIA - - legsMotors_I_m = legsMotors_I_m + 0.054*1e-4; - torsoPitchRollMotors_I_m = torsoPitchRollMotors_I_m + 0.054*1e-4; - torsoYawMotors_I_m = torsoYawMotors_I_m + 0.021*1e-4; - armsMotors_I_m = armsMotors_I_m + 0.021*1e-4; -end - -Config.I_m = diag([torsoPitchRollMotors_I_m*ones(2,1); - torsoYawMotors_I_m; - armsMotors_I_m*ones(8,1); - legsMotors_I_m*ones(12,1)]); - -% parameters for coupling matrices. Updated according to the wiki: -% -% http://wiki.icub.org/wiki/ICub_coupled_joints -% -% and corrected according to https://github.com/robotology/robots-configuration/issues/39 -t = 0.615; -r = 0.022; -R = 0.04; - -% coupling matrices -T_LShoulder = [-1 0 0; - -1 -t 0; - 0 t -t]; - -T_RShoulder = [ 1 0 0; - 1 t 0; - 0 -t t]; - -T_torso = [ 0.5 -0.5 0; - 0.5 0.5 0; - r/(2*R) r/(2*R) r/R]; - -if Config.INCLUDE_COUPLING - - Config.T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12)); -else - Config.T = eye(ROBOT_DOF); -end - -% gain for feedforward term in joint torques calculation. Valid range: a -% value between 0 and 1 -Config.K_ff = 0; - -% Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA if true, the desired joints -% accelerations are used for computing the feedforward term in joint -% torques calculations. Not effective if Config.K_ff = 0. -Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA = false; - -%% Constraints for QP for balancing - -% The friction cone is approximated by using linear interpolation of the circle. -% So, numberOfPoints defines the number of points used to interpolate the circle -% in each cicle's quadrant -numberOfPoints = 4; -forceFrictionCoefficient = 1/3; -torsionalFrictionCoefficient = 1/75; -fZmin = 10; - -% physical size of the foot -feet_size = [-0.05 0.10; % xMin, xMax - -0.025 0.025]; % yMin, yMax - -%% Cleanup -clear q1 q2 q3 q4; diff --git a/torque-controllers/momentum-based-yoga/initTorqueBalancingYoga.m b/torque-controllers/momentum-based-yoga/initTorqueBalancingYoga.m deleted file mode 100644 index 1ab0b0b..0000000 --- a/torque-controllers/momentum-based-yoga/initTorqueBalancingYoga.m +++ /dev/null @@ -1,105 +0,0 @@ -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% /** -% * 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 -% */ -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% In the Simulink model, this script is run every time the user presses -% the 'start' button. -clearvars -except sl_synch_handles torqueBalGUI -clc - -% Import +wbc scope and add path to "src" folder -import wbc.* -addpath('./src/') - -%% GENERAL SIMULATION INFO -% If you are simulating the robot with Gazebo, -% remember that you have to launch Gazebo as follow: -% -% gazebo -slibgazebo_yarp_clock.so -% -% and set the environmental variable YARP_ROBOT_NAME in the .bashrc file. - -% Simulation time in seconds -Config.SIMULATION_TIME = 600000; % high number (not inf) for automatic code generation - -%% PRELIMINARY CONFIGURATIONS -% Sm.SM_TYPE: defines the kind of state machines that can be chosen. -% -% 'YOGA': the robot will perform the YOGA++ demo. The associated -% configuration parameters can be found under the folder: -% -% app/robots/YARP_ROBOT_NAME/initStateMachineYoga.m -% -% 'COORDINATOR': the robot can either stay still, or follow a desired -% center-of-mass trajectory. The associated configuration -% parameters can be found under the folder: -% -% app/robots/YARP_ROBOT_NAME/initCoordinator.m -% -SM_TYPE = 'YOGA'; - -% Config.SCOPES: if set to true, all visualizers for debugging are active -Config.SCOPES_ALL = true; - -% You can also activate only some specific debugging scopes -Config.SCOPES_EXT_WRENCHES = false; -Config.SCOPES_GAIN_SCHE_INFO = false; -Config.SCOPES_MAIN = false; -Config.SCOPES_QP = false; -Config.SCOPES_INERTIA = true; - -% DATA PROCESSING -% -% If Config.SAVE_WORKSPACE = True, every time the simulink model is run the -% workspace is saved after stopping the simulation -Config.SAVE_WORKSPACE = false; - -% If CHECK_INTEGRATION_TIME = True, after stopping the simulation the -% Simulink time is compared with the Yarp time to check if the desired -% integration time step is respected -Config.CHECK_INTEGRATION_TIME = true; -Config.PLOT_INTEGRATION_TIME = false; - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% CONFIGURATIONS COMPLETED: loading gains and parameters for the specific robot -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Controller period [s] -Config.Ts = 0.01; - -% Run robot-specific configuration parameters -run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configRobot.m')); - -% Run demo-specific configuration parameters -Sm.SM_MASK_COORDINATOR = bin2dec('001'); -Sm.SM_MASK_YOGA = bin2dec('010'); - -if strcmpi(SM_TYPE, 'COORDINATOR') - - Sm.SM_TYPE_BIN = Sm.SM_MASK_COORDINATOR; - demoSpecificParameters = fullfile('app/robots',getenv('YARP_ROBOT_NAME'),'initCoordinator.m'); - run(demoSpecificParameters); - -elseif strcmpi(SM_TYPE, 'YOGA') - - Sm.SM_TYPE_BIN = Sm.SM_MASK_YOGA; - demoSpecificParameters = fullfile('app/robots',getenv('YARP_ROBOT_NAME'),'initStateMachineYoga.m'); - run(demoSpecificParameters); -end - -% Compute contact constraints (friction cone, unilateral constraints) -[ConstraintsMatrix,bVectorConstraints] = constraints(forceFrictionCoefficient,numberOfPoints,torsionalFrictionCoefficient,feet_size,fZmin); diff --git a/torque-controllers/momentum-based-yoga/src-gui/closeModel.m b/torque-controllers/momentum-based-yoga/src-gui/closeModel.m deleted file mode 100644 index 590ce9d..0000000 --- a/torque-controllers/momentum-based-yoga/src-gui/closeModel.m +++ /dev/null @@ -1,17 +0,0 @@ -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Save and close the Simulink model through Matlab command line. -% It also closes the associate GUI - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -disp('[closeModel]: closing the model...') - -save_system('torqueBalancingYoga.mdl'); -close_system('torqueBalancingYoga.mdl'); -close all - -% remove paths -rmpath('../../library/matlab-gui'); -rmpath('./src-gui'); - -disp('[closeModel]: done') \ No newline at end of file diff --git a/torque-controllers/momentum-based-yoga/src/balancingControllerYoga.m b/torque-controllers/momentum-based-yoga/src/balancingControllerYoga.m deleted file mode 100644 index 014f7a6..0000000 --- a/torque-controllers/momentum-based-yoga/src/balancingControllerYoga.m +++ /dev/null @@ -1,227 +0,0 @@ - -function [tauModel, Sigma, NA, f_LDot, ... - HessianMatrixQP1Foot, gradientQP1Foot, ConstraintsMatrixQP1Foot, bVectorConstraintsQp1Foot, ... - HessianMatrixQP2Feet, gradientQP2Feet, ConstraintsMatrixQP2Feet, bVectorConstraintsQp2Feet, ... - errorCoM, f_noQP] = ... - balancingControllerYoga(constraints, ROBOT_DOF_FOR_SIMULINK, ConstraintsMatrix, bVectorConstraints, ... - qj, qjDes, nu, M, h, L, intLw, w_H_l_sole, w_H_r_sole, JL, JR, dJL_nu, dJR_nu, xCoM, J_CoM, desired_x_dx_ddx_CoM, ... - gainsPCOM, gainsDCOM, impedances, Reg, Gain) - - % BALANCINGCONTROLLERYOGA momentum-based balancing controller. - % - - %% --- Initialization --- - - %% DEFINITION OF CONTROL AND DYNAMIC VARIABLES - pos_leftFoot = w_H_l_sole(1:3,4); - w_R_l_sole = w_H_l_sole(1:3,1:3); - pos_rightFoot = w_H_r_sole(1:3,4); - w_R_r_sole = w_H_r_sole(1:3,1:3); - - dampings = Gain.dampings; - ROBOT_DOF = size(ROBOT_DOF_FOR_SIMULINK,1); - gravAcc = 9.81; - - % Mass of the robot - m = M(1,1); - - % The mass matrix is partitioned as: - % - % M = [ Mb, Mbj - % Mbj', Mj ]; - % - % where: Mb \in R^{6x6} - % Mbj \in R^{6x6+nDof} - % Mj \in R^{nDofxnDof} - % - Mb = M(1:6,1:6); - Mbj = M(1:6,7:end); - Mj = M(7:end,7:end); - - St = [zeros(6,ROBOT_DOF); - eye(ROBOT_DOF,ROBOT_DOF)]; - gravityWrench = [zeros(2,1); - -m*gravAcc; - zeros(3,1)]; - - % Velocity of the center of mass - xCoM_dot = J_CoM(1:3,:)*nu; - - % Joint velocity - qjDot = nu(7:end); - - % Joint position error - qjTilde = qj-qjDes; - - % Desired acceleration for the center of mass - xDDcomStar = desired_x_dx_ddx_CoM(:,3) -gainsPCOM.*(xCoM - desired_x_dx_ddx_CoM(:,1)) -gainsDCOM.*(xCoM_dot - desired_x_dx_ddx_CoM(:,2)); - - % Application point of the contact force on the right foot w.r.t. CoM - Pr = pos_rightFoot -xCoM; - - % Application point of the contact force on the left foot w.r.t. CoM - Pl = pos_leftFoot -xCoM; - - % The following variables serve for determening the rate-of-change of - % the robot's momentum. In particular, when balancing on two feet, one has: - % - % dot(L) = gravityWrench + AL*f_L + AR*f_R - % = gravityWrench + [AL,AR]*f - % - % where f_L and f_R are the contact wrenches acting on the left and - % right foot, respectively, and f = [f_L;f_R]. - % - AL = [ eye(3),zeros(3); - wbc.skew(Pl), eye(3)]; - AR = [ eye(3), zeros(3); - wbc.skew(Pr), eye(3)]; - - % dot(L) = mg + A*f - A = [AL, AR]; - - pinvA = pinv(A, Reg.pinvTol)*constraints(1)*constraints(2) ... - + [inv(AL);zeros(6)]*constraints(1)*(1-constraints(2)) ... - + [zeros(6);inv(AR)]*constraints(2)*(1-constraints(1)); - - % Null space of the matrix A - NA = (eye(12,12)-pinvA*A)*constraints(1)*constraints(2); - - % Time varying contact jacobian - Jc = [JL*constraints(1); - JR*constraints(2)]; - - % Time varying dot(J)*nu - Jc_nuDot = [dJL_nu*constraints(1) ; - dJR_nu*constraints(2)]; - - JcMinv = Jc/M; - JcMinvSt = JcMinv*St; - JcMinvJct = JcMinv*transpose(Jc); - - % multiplier of f in tau - JBar = transpose(Jc(:,7:end)) -Mbj'/Mb*transpose(Jc(:,1:6)); - - Pinv_JcMinvSt = wbc.pinvDamped(JcMinvSt,Reg.pinvDamp); - - % nullJcMinvSt --> null space of Pinv_JcMinvSt - nullJcMinvSt = eye(ROBOT_DOF) - Pinv_JcMinvSt*JcMinvSt; - - % Mbar is the mass matrix associated with the joint dynamics, i.e. - Mbar = Mj-Mbj'/Mb*Mbj; - NLMbar = nullJcMinvSt*Mbar; - - % Adaptation of control gains for back compatibility with older - % versions of the controller - impedances = diag(impedances)*pinv(NLMbar,Reg.pinvTol) + Reg.impedances*eye(ROBOT_DOF); - dampings = diag(dampings)*pinv(NLMbar,Reg.pinvTol) + Reg.dampings*eye(ROBOT_DOF); - - %% 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. By direct calculations one shows that the joint - % torqes take the following form: - % - % 0) tau = tauModel + Sigma*f_LDot + SigmaNA*f0 - % - % where f0 is the redundancy of the contact wrenches. Then, the problem - % is defined as follows: - % - % 1) f0 = argmin |tau(f0)|^2 - % s.t. - % ConstraintsMatrixQP2Feet*f0 < bVectorConstraintsQp2Feet - - % Update constraint matrices. The constraint matrix for the inequality - % constraints in the problem 1) is built up startin from the constraint - % matrix associated with each single foot. More precisely, the contact - % wrench associated with the left foot (resp. right foot) is subject to - % the following constraint: - % - % constraintMatrixLeftFoot*l_sole_f_L < bVectorConstraints - % - % In this case, however, f_L is expressed w.r.t. the frame l_sole, - % which is solidal to the left foot. Since the controller uses contact - % wrenches expressed w.r.t. a frame whose orientation is that of the - % inertial frame, we have to update the constraint matrix according to - % the transformation w_R_l_sole, i.e. - % - % constraintMatrixLeftFoot = ConstraintsMatrix*w_R_l_sole - % - % The same hold for the right foot - % - constraintMatrixLeftFoot = ConstraintsMatrix * blkdiag(w_R_l_sole',w_R_l_sole'); - constraintMatrixRightFoot = ConstraintsMatrix * blkdiag(w_R_r_sole',w_R_r_sole'); - ConstraintsMatrix2Feet = blkdiag(constraintMatrixLeftFoot,constraintMatrixRightFoot); - bVectorConstraints2Feet = [bVectorConstraints;bVectorConstraints]; - - % Terms used in Eq. 0) - tauModel = Pinv_JcMinvSt*(JcMinv*h - Jc_nuDot) + nullJcMinvSt*(h(7:end) - Mbj'/Mb*h(1:6) ... - -impedances*NLMbar*qjTilde -dampings*NLMbar*qjDot); - - Sigma = -(Pinv_JcMinvSt*JcMinvJct + nullJcMinvSt*JBar); - - % Desired rate-of-change of the robot momentum - LDotDes = [m*xDDcomStar ; - -Gain.KD_AngularMomentum*L(4:end)-Gain.KP_AngularMomentum*intLw]; - - % Contact wrenches realizing the desired rate-of-change of the robot - % momentum LDotDes when standing on two feet. Note that f_LDot is - % different from zero only when both foot are in contact, i.e. - % constraints(1) = constraints(2) = 1. This because when the robot - % stands on one foot, the f_LDot is evaluated directly from the - % optimizer (see next section). - f_LDot = pinvA*(LDotDes - gravityWrench)*constraints(1)*constraints(2); - SigmaNA = Sigma*NA; - - % The optimization problem 1) 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 - % - % ConstraintsMatrix2Feet*f < bVectorConstraints, - % - % which in terms of f0 is: - % - % ConstraintsMatrix2Feet*NA*f0 < bVectorConstraints - ConstraintsMatrix2Feet*f_LDot - ConstraintsMatrixQP2Feet = ConstraintsMatrix2Feet*NA; - bVectorConstraintsQp2Feet = bVectorConstraints2Feet-ConstraintsMatrix2Feet*f_LDot; - - % Evaluation of Hessian matrix and gradient vector for solving the - % optimization problem 1). - HessianMatrixQP2Feet = SigmaNA'*SigmaNA + eye(size(SigmaNA,2))*Reg.HessianQP; - gradientQP2Feet = SigmaNA'*(tauModel + Sigma*f_LDot); - - %% QP PARAMETERS FOR ONE FOOT STANDING - % In the case the robot stands on one foot, there is no redundancy of - % the contact wrenches. Hence, we cannot use this redundancy for - % minimizing the joint torques. For this reason, the minimization - % problem is modified as follows: - % - % 2) f = argmin|dot(L)(f) - dot(L)_des|^2 - % s.t. - % ConstraintsMatrixQP1Foot*f < bVectorConstraintsQp1Foot - % - % where f is the contact wrench either of the left or on the right - % foot. - % - ConstraintsMatrixQP1Foot = constraints(1) * (1 - constraints(2)) * constraintMatrixLeftFoot + ... - constraints(2) * (1 - constraints(1)) * constraintMatrixRightFoot; - bVectorConstraintsQp1Foot = bVectorConstraints; - - A1Foot = AL*constraints(1)*(1-constraints(2)) + AR*constraints(2)*(1-constraints(1)); - HessianMatrixQP1Foot = A1Foot'*A1Foot + eye(size(A1Foot,2))*Reg.HessianQP; - gradientQP1Foot = -A1Foot'*(LDotDes - gravityWrench); - - %% DEBUG DIAGNOSTICS - - % Unconstrained solution for the problem 1) - f0 = -wbc.pinvDamped(SigmaNA, Reg.pinvDamp*1e-5)*(tauModel + Sigma*f_LDot); - - % Unconstrained contact wrenches - f_noQP = pinvA*(LDotDes - gravityWrench) + NA*f0*constraints(1)*constraints(2); - - % Error on the center of mass - errorCoM = xCoM - desired_x_dx_ddx_CoM(:,1); -end diff --git a/torque-controllers/momentum-based-yoga/src/saturateInputDerivative.m b/torque-controllers/momentum-based-yoga/src/saturateInputDerivative.m deleted file mode 100644 index c6f0d92..0000000 --- a/torque-controllers/momentum-based-yoga/src/saturateInputDerivative.m +++ /dev/null @@ -1,58 +0,0 @@ -function uSat = saturateInputDerivative(u, u_0, Config) - - % SATURATEINPUTDERIVATIVE Saturates the input u such that the absolute value - % of its numerical derivative uDelta = (uPrev-u)/Ts cannot be greater than - % a predefined value. uPrev = u at the previous integration step; Ts integration step. - % - - %% --- Initialization --- - persistent uPrev; - - % initialize the input value at the previous step - if isempty(uPrev) - - uPrev = u_0; - end - - % max allowed input derivative - uDelta_maxAbs = Config.tauDot_maxAbs; - - % evaluate the max and min allowed input - delta_u_max = uDelta_maxAbs*Config.Ts; - delta_u_min = -uDelta_maxAbs*Config.Ts; - - delta_u_Sat = saturateInput(u-uPrev, delta_u_min, delta_u_max); - - % update uPrev - uSat = uPrev + delta_u_Sat; - uPrev = uSat; - -end - -% utility function: saturates the input value -function y = saturateInput(u, min, max) - - assert(isequal(size(min), size(max)), 'Min and max must be same size') - - if length(min) == 1 - - y = u; - y(y > max) = max; - y(y < min) = min; - else - - assert(length(min) == length(u), 'input and saturation must have same size'); - y = u; - - for i = 1:length(min) - - if y(i) > max(i) - - y(i) = max(i); - elseif y(i) < min(i) - - y(i) = min(i); - end - end - end -end \ No newline at end of file diff --git a/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m b/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m deleted file mode 100644 index 03fe763..0000000 --- a/torque-controllers/momentum-based-yoga/src/stateMachineYoga.m +++ /dev/null @@ -1,460 +0,0 @@ -function [w_H_b, CoM_des, qj_des, constraints, impedances, KPCoM, KDCoM, currentState, jointsSmoothingTime] = ... - stateMachineYoga(CoM_0, qj_0, l_sole_CoM, r_sole_CoM, qj, t, wrench_rightFoot, wrench_leftFoot, l_sole_H_b, r_sole_H_b, Sm, Gain) - - % SATEMACHINEYOGA state machine for performing Yoga ++ movements. - % - - %% --- Initialization --- - - persistent state; - persistent tSwitch; - persistent w_H_fixedLink; - persistent secondYoga; - persistent yogaMovesetCounter; - - if isempty(state) || isempty(tSwitch) || isempty(w_H_fixedLink) || isempty(secondYoga) || isempty(yogaMovesetCounter) - state = Sm.stateAt0; - tSwitch = 0; - w_H_fixedLink = eye(4); - secondYoga = false; - yogaMovesetCounter = 1; - end - - CoM_des = CoM_0; - constraints = [1; 1]; - qj_des = qj_0; - w_H_b = eye(4); - impedances = Gain.impedances(1,:); - KPCoM = Gain.KP_COM(1,:); - KDCoM = Gain.KD_COM(1,:); - - %% TWO FEET BALANCING - if state == 1 - - w_H_b = w_H_fixedLink * l_sole_H_b; - - % after tBalancing time start moving weight to the left - if t > Sm.tBalancing && ~Sm.demoOnlyBalancing - - state = 2; - - if Sm.demoStartsOnRightSupport - - w_H_fixedLink = w_H_fixedLink*l_sole_H_b/r_sole_H_b; - state = 8; - end - end - end - - %% TRANSITION TO THE LEFT FOOT - if state == 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 - CoM_des = [w_H_fixedLink(1:2,4);CoM_0(3)] + Sm.CoM_delta(state,:)'; - - impedances = Gain.impedances(state,:); - KPCoM = Gain.KP_COM(state,:); - KDCoM = Gain.KD_COM(state,:); - - fixed_link_CoMDes = w_H_fixedLink\[CoM_des;1]; - CoM_Error = fixed_link_CoMDes(1:3) -l_sole_CoM(1:3); - qj_des = Sm.joints_references(state,:)'; - - if norm(CoM_Error(2)) < Sm.CoM_threshold && wrench_rightFoot(3) < Sm.wrench_thresholdContactOff - state = 3; - tSwitch = t; - end - end - - %% LEFT FOOT BALANCING - if state == 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 - CoM_des = [w_H_fixedLink(1:2,4);CoM_0(3)] + Sm.CoM_delta(state,:)'; - - % right foot is no longer a constraints - constraints = [1; 0]; - - qj_des = Sm.joints_references(state,:)'; - impedances = Gain.impedances(state,:); - KPCoM = Gain.KP_COM(state,:); - KDCoM = Gain.KD_COM(state,:); - - if t > (tSwitch + Sm.tBalancingBeforeYoga) - - state = 4; - tSwitch = t; - - if Sm.skipYoga - state = 5; - end - end - end - - %% YOGA LEFT FOOT - if state == 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 - CoM_des = [w_H_fixedLink(1:2,4);CoM_0(3)] + Sm.CoM_delta(state,:)'; - - constraints = [1; 0]; - - qj_des = Sm.joints_references(state,:)'; - impedances = Gain.impedances(state,:); - KPCoM = Gain.KP_COM(state,:); - KDCoM = Gain.KD_COM(state,:); - - % iterate over the yoga positions - for i = 1: size(Sm.joints_leftYogaRef,1)-1 - - % positions for the first yoga - if t > (Sm.joints_leftYogaRef(i,1) + tSwitch) && t <= (Sm.joints_leftYogaRef(i+1,1)+ tSwitch) && secondYoga == false - - qj_des = Sm.joints_leftYogaRef(i,2:end)'; - - % positions for the second yoga - elseif t > (Sm.joints_leftSecondYogaRef(i) + tSwitch) && t <= (Sm.joints_leftSecondYogaRef(i+1)+ tSwitch) && secondYoga == true - - qj_des = Sm.joints_leftYogaRef(i,2:end)'; - - end - end - if t > (Sm.joints_leftYogaRef(end,1) + tSwitch) && secondYoga == false - - qj_des = Sm.joints_leftYogaRef(end,2:end)'; - - % if Sm.repeatTwiceYogaWithDifferentSpeed == true, repeat the Yoga - if t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatTwiceYogaWithDifferentSpeed == true - - tSwitch = t; - secondYoga = true; - - elseif t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatTwiceYogaWithDifferentSpeed == false && Sm.oneFootYogaInLoop - - tSwitch = t; - yogaMovesetCounter = yogaMovesetCounter +1; - - % if the robot repeated the Yoga moveset for the number of - % times required by the user, then exit the loop - if yogaMovesetCounter > Sm.yogaCounter - - state = 5; - end - - elseif t > (Sm.joints_leftYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatTwiceYogaWithDifferentSpeed == false && Sm.oneFootYogaInLoop == false - - state = 5; - tSwitch = t; - secondYoga = false; - end - end - if t > (Sm.joints_leftSecondYogaRef(end) + tSwitch) && secondYoga == true - - qj_des = Sm.joints_leftYogaRef(end,2:end)'; - - % exit loop condition for the second yoga - if t > (Sm.joints_leftSecondYogaRef(end) + tSwitch + Sm.smoothingTimeSecondYogaLeft + Sm.joints_pauseBetweenYogaMoves) - - state = 5; - tSwitch = t; - secondYoga = false; - end - end - end - - %% PREPARING FOR SWITCHING - if state == 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 - CoM_des = [w_H_fixedLink(1:2,4);CoM_0(3)] + Sm.CoM_delta(state,:)'; - - constraints = [1; 0]; - - qj_des = Sm.joints_references(state,:)'; - impedances = Gain.impedances(state,:); - KPCoM = Gain.KP_COM(state,:); - KDCoM = Gain.KD_COM(state,:); - - qj_errorRLeg = qj(end-5:end) -qj_des(end-5:end); - qj_errorLLeg = qj(end-11:end-6) -qj_des(end-11:end-6); - - if norm(qj_errorRLeg)*180/pi < Sm.joints_thresholdNotInContact && norm(qj_errorLLeg)*180/pi < Sm.joints_thresholdInContact - - state = 6; - tSwitch = t; - end - end - - %% LOOKING FOR A CONTACT - if state == 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 - CoM_des = [w_H_fixedLink(1:2,4);CoM_0(3)] + Sm.CoM_delta(state,:)'; - - constraints = [1; 0]; - - qj_des = Sm.joints_references(state,:)'; - impedances = Gain.impedances(state,:); - KPCoM = Gain.KP_COM(state,:); - KDCoM = Gain.KD_COM(state,:); - - if wrench_rightFoot(3) > Sm.wrench_thresholdContactOn - - state = 7; - tSwitch = t; - end - end - - %% TRANSITION TO INITIAL POSITION - if state == 7 - - w_H_b = w_H_fixedLink * l_sole_H_b; - - - CoM_des = CoM_0 + Sm.CoM_delta(state,:)'; - - constraints = [1; 1]; - impedances = Gain.impedances(state,:); - KPCoM = Gain.KP_COM(state,:); - KDCoM = Gain.KD_COM(state,:); - - if norm(l_sole_CoM(1:2)-CoM_des(1:2)) < 10*Sm.CoM_threshold && Sm.yogaAlsoOnRightFoot && t > tSwitch + Sm.tBalancing - - w_H_fixedLink = w_H_fixedLink*l_sole_H_b/r_sole_H_b; - state = 8; - tSwitch = t; - end - end - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - %% TRANSITION TO THE RIGHT FOOT - if state == 8 - - constraints = [1; 1]; - 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 left foot (l_sole) plus a - % configurable delta - CoM_des = [w_H_fixedLink(1:2,4);CoM_0(3)] + Sm.CoM_delta(state,:)'; - - fixed_link_CoMDes = w_H_fixedLink\[CoM_des;1]; - CoM_Error = fixed_link_CoMDes(1:3) - r_sole_CoM(1:3); - - impedances = Gain.impedances(state,:); - KPCoM = Gain.KP_COM(state,:); - KDCoM = Gain.KD_COM(state,:); - qj_des = Sm.joints_references(state,:)'; - - if norm(CoM_Error(2)) < Sm.CoM_threshold && wrench_leftFoot(3) < Sm.wrench_thresholdContactOff - - state = 9; - tSwitch = t; - end - end - - %% RIGHT FOOT BALANCING - if state == 9 - - % left foot is no longer a constraints - constraints = [0; 1]; - w_H_b = w_H_fixedLink * r_sole_H_b; - - CoM_des = [w_H_fixedLink(1:2,4);CoM_0(3)] + Sm.CoM_delta(state,:)'; - qj_des = Sm.joints_references(state,:)'; - impedances = Gain.impedances(state,:); - KPCoM = Gain.KP_COM(state,:); - KDCoM = Gain.KD_COM(state,:); - - if t > (tSwitch + Sm.tBalancingBeforeYoga) - - state = 10; - tSwitch = t; - - if Sm.skipYoga - state = 11; - end - end - end - - %% YOGA RIGHT FOOT - if state == 10 - - constraints = [0; 1]; - w_H_b = w_H_fixedLink*r_sole_H_b; - - CoM_des = [w_H_fixedLink(1:2,4);CoM_0(3)] + Sm.CoM_delta(state,:)'; - qj_des = Sm.joints_references(state,:)'; - impedances = Gain.impedances(state,:); - KPCoM = Gain.KP_COM(state,:); - KDCoM = Gain.KD_COM(state,:); - - % iterate over the yoga positions - for i = 1: size(Sm.joints_rightYogaRef,1)-1 - - % positions for the first yoga - if t > (Sm.joints_rightYogaRef(i,1) + tSwitch) && t <= (Sm.joints_rightYogaRef(i+1,1)+ tSwitch) && secondYoga == false - - qj_des = Sm.joints_rightYogaRef(i,2:end)'; - - % positions for the second yoga - elseif t > (Sm.joints_rightSecondYogaRef(i) + tSwitch) && t <= (Sm.joints_rightSecondYogaRef(i+1)+ tSwitch) && secondYoga == true - - qj_des = Sm.joints_rightYogaRef(i,2:end)'; - - end - end - if t > (Sm.joints_rightYogaRef(end,1) + tSwitch) && secondYoga == false - - qj_des = Sm.joints_rightYogaRef(end,2:end)'; - - % if Sm.repeatTwiceYogaWithDifferentSpeed == true, repeat the Yoga - if t > (Sm.joints_rightYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatTwiceYogaWithDifferentSpeed == true - - tSwitch = t; - secondYoga = true; - - elseif t > (Sm.joints_rightYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatTwiceYogaWithDifferentSpeed == false && Sm.oneFootYogaInLoop - - tSwitch = t; - yogaMovesetCounter = yogaMovesetCounter +1; - - % if the robot repeated the Yoga moveset for the number of - % times required by the user, then exit the loop - if yogaMovesetCounter > Sm.yogaCounter - - state = 11; - end - - elseif t > (Sm.joints_rightYogaRef(end,1) + tSwitch + Sm.smoothingTimeCoM_Joints(state) + Sm.joints_pauseBetweenYogaMoves) && Sm.repeatTwiceYogaWithDifferentSpeed == false && Sm.oneFootYogaInLoop == false - - state = 11; - tSwitch = t; - secondYoga = false; - end - end - if t > (Sm.joints_rightSecondYogaRef(end) + tSwitch) && secondYoga == true - - qj_des = Sm.joints_rightYogaRef(end,2:end)'; - - % exit loop condition for the second yoga - if t > (Sm.joints_rightSecondYogaRef(end) + tSwitch + Sm.smoothingTimeSecondYogaRight + Sm.joints_pauseBetweenYogaMoves) - - state = 11; - tSwitch = t; - secondYoga = false; - end - end - end - - %% PREPARING FOR SWITCHING - if state == 11 - - constraints = [0; 1]; - w_H_b = w_H_fixedLink * r_sole_H_b; - - CoM_des = [w_H_fixedLink(1:2,4);CoM_0(3)] + Sm.CoM_delta(state,:)'; - qj_des = Sm.joints_references(state,:)'; - impedances = Gain.impedances(state,:); - KPCoM = Gain.KP_COM(state,:); - KDCoM = Gain.KD_COM(state,:); - - qj_errorRLeg = qj(end-5:end)-qj_des(end-5:end); - qj_errorLLeg = qj(end-11:end-6)-qj_des(end-11:end-6); - - if norm(qj_errorRLeg)*180/pi < Sm.joints_thresholdInContact && norm(qj_errorLLeg)*180/pi < Sm.joints_thresholdNotInContact - state = 12; - tSwitch = t; - end - end - - %% LOOKING FOR A CONTACT - if state == 12 - - constraints = [0; 1]; - w_H_b = w_H_fixedLink * r_sole_H_b; - - CoM_des = [w_H_fixedLink(1:2,4);CoM_0(3)] + Sm.CoM_delta(state,:)'; - qj_des = Sm.joints_references(state,:)'; - impedances = Gain.impedances(state,:); - KPCoM = Gain.KP_COM(state,:); - KDCoM = Gain.KD_COM(state,:); - - if wrench_leftFoot(3) > Sm.wrench_thresholdContactOn - - state = 13; - tSwitch = t; - end - end - - %% TRANSITION TO INITIAL POSITION - if state == 13 - - w_H_b = w_H_fixedLink * r_sole_H_b; - constraints = [1; 1]; - impedances = Gain.impedances(state,:); - KPCoM = Gain.KP_COM(state,:); - KDCoM = Gain.KD_COM(state,:); - - if t - tSwitch > Sm.tBalancing - - if Sm.twoFeetYogaInLoop - - state = 2; - w_H_fixedLink = w_H_fixedLink*r_sole_H_b/l_sole_H_b; - - if Sm.demoStartsOnRightSupport - state = 8; - w_H_fixedLink = w_H_fixedLink*l_sole_H_b/r_sole_H_b; - end - end - end - end - - %% Update parameters - currentState = state; - - % Update CoM and joints smoothing time for the repeated yoga demo - if secondYoga && state == 4 && t >= (Sm.joints_leftSecondYogaRef(2) + tSwitch) - - jointsSmoothingTime = Sm.smoothingTimeSecondYogaLeft; - - elseif secondYoga && state == 10 && t >= (Sm.joints_rightSecondYogaRef(2) + tSwitch) - - jointsSmoothingTime = Sm.smoothingTimeSecondYogaRight; - else - - if state == 4 || state == 10 - - % during the yoga, reduce the time necessary for the reference - % to converge to the next position - jointsSmoothingTime = Sm.scaleFactorSmoothingTime*Sm.smoothingTimeCoM_Joints(state); - else - jointsSmoothingTime = Sm.smoothingTimeCoM_Joints(state); - end - end - -end diff --git a/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl b/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl deleted file mode 100644 index 3a2919e..0000000 --- a/torque-controllers/momentum-based-yoga/torqueBalancingYoga.mdl +++ /dev/null @@ -1,27700 +0,0 @@ -Model { - Name "torqueBalancingYoga" - Version 9.0 - SavedCharacterEncoding "UTF-8" - GraphicalInterface { - NumRootInports 0 - NumRootOutports 0 - ParameterArgumentNames "" - ComputedModelVersion "1.3394" - NumModelReferences 0 - NumTestPointedSignals 0 - NumProvidedFunctions 0 - NumRequiredFunctions 0 - NumResetEvents 0 - HasInitializeEvent 0 - HasTerminateEvent 0 - IsExportFunctionModel 0 - NumParameterArguments 0 - NumExternalFileReferences 85 - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Configuration" - Path "torqueBalancingYoga/Configuration" - SID "4537" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingYoga/Dump and visualize/Visualizer/Get Measurement" - SID "4542" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingYoga/Dump and visualize/Visualizer/Get Measurement1" - SID "4543" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" - Path "torqueBalancingYoga/Dynamics and Kinematics/Dynamics/Centroidal Momentum" - SID "2345" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Get Bias Forces" - Path "torqueBalancingYoga/Dynamics and Kinematics/Dynamics/Get Bias Forces" - SID "2348" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Mass Matrix" - Path "torqueBalancingYoga/Dynamics and Kinematics/Dynamics/Mass Matrix" - SID "2349" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/DotJ Nu l_sole " - SID "2375" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/DotJ Nu r_sole " - SID "2376" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/Jacobian com" - SID "2378" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/Jacobian l_sole" - SID "2379" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/Jacobian r_sole" - SID "2380" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/l_sole" - SID "2405" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/r_sole" - SID "2406" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Dynamics and Kinematics/Kinematics/xCom/CoM" - SID "4363" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "simulink/Logic and Bit\nOperations/Compare\nTo Zero" - Path "torqueBalancingYoga/Robot State and References/Compare To Zero" - SID "2916" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "simulink/Logic and Bit\nOperations/Compare\nTo Zero" - Path "torqueBalancingYoga/Robot State and References/Compare To Zero1" - SID "2917" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "simulink/Logic and Bit\nOperations/Bitwise\nOperator" - Path "torqueBalancingYoga/Robot State and References/Coordinator" - SID "2908" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingYoga/Robot State and References/Get Measurement" - SID "4538" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/LFoot to wor" - "ld transform (fixed base)" - SID "4071" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/RFoot to wor" - "ld transform (fixed base)" - SID "4084" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot to" - " base link transform /Fixed base to imu transform" - SID "4052" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot to" - " base link transform /Fixed base to root link transform" - SID "4053" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot to" - " base link transform /Neck Position" - SID "4056" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot to" - " base link transform /holder 1" - SID "4060" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot to" - " base link transform /holder 2" - SID "4061" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Compute State Velocity/Feet Jac" - "obians/Jacobian LFoot" - SID "4094" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Compute State Velocity/Feet Jac" - "obians/Jacobian RFoot" - SID "4095" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/Compute State Velocity/Get Meas" - "urement" - SID "4540" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/IMU measurements" - SID "3222" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/holder " - SID "2109" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/holder 1" - SID "2110" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/Internal Coordinator/xCom/CoM" - SID "2141" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - Path "torqueBalancingYoga/Robot State and References/Select State and References/Minimum Jerk Trajectory " - "Generator1" - SID "2152" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - Path "torqueBalancingYoga/Robot State and References/Select State and References/Minimum Jerk Trajectory " - "Generator2" - SID "2153" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute State Velocity/Feet Jacob" - "ians/Jacobian LFoot" - SID "4219" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute State Velocity/Feet Jacob" - "ians/Jacobian RFoot" - SID "4220" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute State Velocity/Get Measur" - "ement" - SID "4541" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/LFoot to base link transform /Fixed base to imu transform" - SID "4250" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/LFoot to base link transform /Fixed base to root link transform" - SID "4251" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/LFoot to base link transform /Neck Position" - SID "4254" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/LFoot to base link transform /holder 1" - SID "4258" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/LFoot to base link transform /holder 2" - SID "4259" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/LFoot to world transform (fixed base)" - SID "4269" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/RFoot to base link transform/Fixed base to imu transform" - SID "4275" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/RFoot to base link transform/Fixed base to root link transform" - SID "4276" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/RFoot to base link transform/Neck Position" - SID "4279" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/RFoot to base link transform/holder 1" - SID "4283" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/RFoot to base link transform/holder 2" - SID "4284" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Compute base to fixed link transf" - "orm/RFoot to world transform (fixed base)" - SID "4306" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/Minimum Jerk Trajectory Generator" - SID "2176" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/holder 1" - SID "2187" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/holder 2" - SID "2188" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/inertial" - SID "2630" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/left_foot_wrench" - SID "2206" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/right_foot_wrench" - SID "2218" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/xCom/CoM" - SID "4229" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/Robot State and References/State Machine Yoga/xCom1/CoM" - SID "4303" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "simulink/Logic and Bit\nOperations/Bitwise\nOperator" - Path "torqueBalancingYoga/Robot State and References/Yoga" - SID "2909" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyActuators/Set References" - Path "torqueBalancingYoga/Set References" - SID "2354" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Centroidal Momentum" - SID "3718" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/LFoot to base link transform /Fixed base to imu transform" - SID "4431" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/LFoot to base link transform /Fixed base to root link transform" - SID "4432" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/LFoot to base link transform /Neck Position" - SID "4435" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/LFoot to base link transform /holder 1" - SID "4439" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/LFoot to base link transform /holder 2" - SID "4440" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/LFoot to world transform (fixed base)" - SID "4450" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/RFoot to base link transform/Fixed base to imu transform" - SID "4456" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/RFoot to base link transform/Fixed base to root link transform" - SID "4457" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/RFoot to base link transform/Neck Position" - SID "4460" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/RFoot to base link transform/holder 1" - SID "4464" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/RFoot to base link transform/holder 2" - SID "4465" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Compute base to fixed link tran" - "sform/RFoot to world transform (fixed base)" - SID "4475" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Jacobian" - SID "3719" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/Jacobian1" - SID "3720" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "torqueBalancingYoga/controller_QP/Compute angular momentum integral/inertial" - SID "3724" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Match Signal Sizes" - Path "torqueBalancingYoga/controller_QP/Compute joint torques/QPSolver/One Foot/Match Signal Sizes1" - SID "4601" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/QP" - Path "torqueBalancingYoga/controller_QP/Compute joint torques/QPSolver/One Foot/QP Two Feet" - SID "4603" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Match Signal Sizes" - Path "torqueBalancingYoga/controller_QP/Compute joint torques/QPSolver/Two Feet/Match Signal Sizes" - SID "4615" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/QP" - Path "torqueBalancingYoga/controller_QP/Compute joint torques/QPSolver/Two Feet/QP Two Feet" - SID "4617" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "torqueBalancingYoga/controller_QP/Compute joint torques (motor reflected inertia)/Get Measurement" - SID "4539" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Limits" - Path "torqueBalancingYoga/emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/Get Limits" - SID "2432" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Simulator Synchronizer" - Path "torqueBalancingYoga/synchronizer/GAZEBO_SYNCHRONIZER/Simulator Synchronizer" - SID "2426" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Real Time Synchronizer" - Path "torqueBalancingYoga/synchronizer/REAL_TIME_SYNC/Real Time Synchronizer" - SID "2431" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Clock" - Path "torqueBalancingYoga/synchronizer/Yarp Clock" - SID "4629" - Type "LIBRARY_BLOCK" - } - ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/holder\n" - Path "torqueBalancingYoga/tauDot Saturation/holder " - SID "4559" - Type "LIBRARY_BLOCK" - } - OrderedModelArguments 1 - } - DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" - LogicAnalyzerGraphicalSettings "" - LogicAnalyzerPlugin "on" - LogicAnalyzerSignalOrdering "" - CustomCodeFunctionData "" - SLCCPlugin "on" - PostLoadFcn "%% Try to open the static GUI and try to adjust it \ntry\n\n torqueBalGUI = torqueBalancingGui;\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(torqueBalGUI)\n\nend" - InitFcn "%% initialize the simulation\ncd(fileparts(which(bdroot)));\ninitTorqueBalancingYoga;" - StartFcn "%% Try to edit the GUI (the user may have already closed it)\ntry\n\n %% Get the current status of t" - "he push button\n pushButtonString = sl_synch_handles.pushbutton5.String;\n\n %% If the string is 'Start Model'" - ", the start was not driven by the GUI.\n %% Therefore it is necessary to update the button message to 'Stop Model" - "' manually.\n\n if strcmp(pushButtonString, 'Start Model')\n\n set(sl_synch_handles.pushbutton5,'String','" - "Stop Model')\n set(sl_synch_handles.pushbutton5,'Backgroundcolor','r');\n set(sl_synch_handles.pushbut" - "ton5,'Enable','on')\n\n end \nend\n\n" - StopFcn "%% stop the simulation\ncd(fileparts(which(bdroot)));\nstopTorqueBalancingYoga;\n\n%% Try to edit the GU" - "I (the user may have already closed it)\ntry\n\n %% Get the current status of the push button\n pushButtonStri" - "ng = sl_synch_handles.pushbutton5.String;\n\n %% If the string is 'Stop Model', the stop was not driven by the GU" - "I.\n %% Therefore it is necessary to update the button message to 'Start Model' manually.\n\n if strcmp(pushBu" - "ttonString, 'Stop Model')\n\n set(sl_synch_handles.pushbutton5,'String','Start Model')\n set(sl_synch_" - "handles.pushbutton5,'Backgroundcolor',[0.8,0.8,0.8]);\n\n %% disable the button (only after compiling it will" - " be enables again)\n set(sl_synch_handles.pushbutton5,'Enable','inactive')\n end \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 [134.0, 55.0, 3706.0, 2105.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] - } - Object { - $PropName "EditorsInfo" - $ObjectID 5 - $ClassName "Simulink.EditorInfo" - IsActive [1] - ViewObjType "SimulinkTopLevel" - LoadSaveID "0" - Extents [3668.0, 1809.0] - ZoomFactor [2.8450479233226837] - Offset [567.871139809096, 212.0791690061763] - } - Object { - $PropName "DockComponentsInfo" - $ObjectID 6 - $ClassName "Simulink.DockComponentInfo" - Type "GLUE2:PropertyInspector" - ID "Property Inspector" - Visible [0] - CreateCallback "" - UserData "" - Floating [0] - DockPosition "Right" - Width [640] - Height [480] - } - WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" - "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAC6AP///wAAAAEAAAAAAAAAAPwCA" - "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAAOgD///8AAA56AAAHbQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACe/////wAAAAAAAAAA/////wEAAAEM/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGq/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAASj/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" - } - } - HideAutomaticNames on - Created "Tue Feb 18 10:13:36 2014" - Creator "daniele" - UpdateHistory "UpdateHistoryNever" - ModifiedByFormat "%" - LastModifiedBy "gnava" - ModifiedDateFormat "%" - LastModifiedDate "Mon Dec 17 19:11:23 2018" - RTWModifiedTimeStamp 466974682 - 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" - VisualizeLoggedSignalsWhenLoggingToFile off - 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 7 - $ClassName "Simulink.SimulationData.ModelLoggingInfo" - model_ "torqueBalancingYoga" - signals_ [] - overrideMode_ [0.0] - Array { - Type "Cell" - Dimension 1 - Cell "torqueBalancingYoga" - 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 8 - Version "1.17.1" - DisabledProps [] - Description "" - Array { - Type "Handle" - Dimension 9 - Simulink.SolverCC { - $ObjectID 9 - Version "1.17.1" - DisabledProps [] - Description "" - StartTime "0.0" - StopTime "Config.SIMULATION_TIME " - AbsTol "auto" - FixedStep "Config.Ts" - 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 10 - Version "1.17.1" - DisabledProps [] - Description "" - 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 11 - Version "1.17.1" - Array { - Type "Cell" - Dimension 8 - Cell "BooleansAsBitfields" - Cell "PassReuseOutputArgsAs" - Cell "PassReuseOutputArgsThreshold" - Cell "ZeroExternalMemoryAtStartup" - Cell "ZeroInternalMemoryAtStartup" - Cell "OptimizeModelRefInitCode" - Cell "NoFixptDivByZeroProtection" - Cell "UseSpecifiedMinMax" - PropName "DisabledProps" - } - Description "" - 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 12 - Version "1.17.1" - Array { - Type "Cell" - Dimension 1 - Cell "UseOnlyExistingSharedCode" - PropName "DisabledProps" - } - Description "" - 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 13 - Version "1.17.1" - DisabledProps [] - Description "" - 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 14 - Version "1.17.1" - DisabledProps [] - Description "" - 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 15 - Version "1.17.1" - DisabledProps [] - Description "" - 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 16 - 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" - } - SystemTargetFile "grt.tlc" - HardwareBoard "None" - TLCOptions "" - GenCodeOnly on - MakeCommand "make_rtw" - GenerateMakefile on - PackageGeneratedCodeAndArtifacts off - PackageName "" - TemplateMakefile "grt_default_tmf" - PostCodeGenCommand "" - Description "" - GenerateReport off - SaveLog 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 17 - 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 "" - 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 18 - Version "1.17.1" - Array { - Type "Cell" - Dimension 17 - 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" - PropName "DisabledProps" - } - Description "" - Array { - Type "Handle" - Dimension 1 - Simulink.CPPComponent { - $ObjectID 19 - Version "1.18.0" - Array { - Type "Cell" - Dimension 10 - Cell "Description" - Cell "Components" - Cell "Name" - Cell "GenerateDestructor" - Cell "GenerateExternalIOAccessMethods" - Cell "ParameterMemberVisibility" - Cell "InternalMemberVisibility" - Cell "GenerateParameterAccessMethods" - Cell "GenerateInternalMemberAccessMethods" - Cell "UseOperatorNewForModelRefRegistration" - PropName "DisabledProps" - } - Description "" - 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 20 - Version "1.17.1" - DisabledProps [] - Description "Simulink Coverage Configuration Component" - 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" - ExtraOptions "-aBlockCommentType=\"BlockPathComment\" -aIgnoreTestpoints=0 " - CurrentDlgPage "Code Generation/Interface" - ConfigPrmDlgPosition [ 730, 510, 2252, 1380 ] - } - PropName "ConfigurationSets" - } - Simulink.ConfigSet { - $PropName "ActiveConfigurationSet" - $ObjectID 8 - } - Object { - $PropName "DataTransfer" - $ObjectID 21 - $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 { - 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 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 MultiPortSwitch - DataPortOrder "One-based contiguous" - Inputs "3" - DataPortIndices "{1,2,3}" - DataPortForDefault "Last data port" - DiagnosticForDefault "Error" - zeroidx off - InputSameDT on - OutMin "[]" - OutMax "[]" - OutDataTypeStr "Inherit: Inherit via internal rule" - LockScale off - RndMeth "Floor" - SaturateOnIntegerOverflow on - SampleTime "-1" - AllowDiffInputSizes off - } - 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 off - } - 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 - IsObserver 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 "Inherit from input (this choice will be removed - see release notes)" - FixptAsFi off - NumInputs "1" - SampleTime "0" - } - } - System { - Name "torqueBalancingYoga" - 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 "285" - ReportName "simulink-default.rpt" - SIDHighWatermark "4671" - Block { - BlockType Reference - Name "Configuration" - SID "4537" - Ports [] - Position [635, 460, 710, 490] - ZOrder 553 - HideAutomaticName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Configuration" - SourceType "" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - ConfigSource "Workspace" - ConfigObject "'WBTConfigRobot'" - RobotName "'icubSim'" - UrdfFile "'model.urdf'" - ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}" - ControlBoardsNames "{'left_arm','right_arm','torso'}" - LocalName "'WBT'" - GravityVector "[0,0,-9.81]" - } - Block { - BlockType SubSystem - Name "Dump and visualize" - SID "2275" - Ports [] - Position [1580, 306, 1705, 334] - ZOrder 547 - ForegroundColor "blue" - BackgroundColor "[0.333333, 1.000000, 1.000000]" - ShowName off - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 22 - $ClassName "Simulink.Mask" - Display "disp('VISUALIZER')" - } - System { - Name "Dump and visualize" - 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 "410" - Block { - BlockType From - Name "From" - SID "4478" - Position [585, 161, 665, 179] - ZOrder 708 - GotoTag "tau_des" - TagVisibility "global" - } - Block { - BlockType From - Name "From1" - SID "4479" - Position [585, 241, 665, 259] - ZOrder 709 - GotoTag "state" - TagVisibility "global" - } - Block { - BlockType From - Name "From2" - SID "4480" - Position [585, 321, 665, 339] - ZOrder 710 - GotoTag "CoM_error" - TagVisibility "global" - } - Block { - BlockType From - Name "From3" - SID "4481" - Position [585, 401, 665, 419] - ZOrder 711 - GotoTag "CoM_des" - TagVisibility "global" - } - Block { - BlockType From - Name "From4" - SID "4482" - Position [585, 281, 665, 299] - ZOrder 712 - GotoTag "qj_des" - TagVisibility "global" - } - Block { - BlockType From - Name "From5" - SID "4483" - Position [585, 361, 665, 379] - ZOrder 713 - GotoTag "f_QP" - TagVisibility "global" - } - Block { - BlockType From - Name "From6" - SID "4484" - Position [585, 441, 665, 459] - ZOrder 714 - GotoTag "f_noQP" - TagVisibility "global" - } - Block { - BlockType From - Name "From7" - SID "4485" - Position [585, 201, 665, 219] - ZOrder 715 - GotoTag "w_H_b" - TagVisibility "global" - } - Block { - BlockType From - Name "From8" - SID "4563" - Position [575, 481, 670, 499] - ZOrder 716 - GotoTag "tau_des_afterFilter" - TagVisibility "global" - } - Block { - BlockType Logic - Name "Logical\nOperator1" - SID "3273" - Ports [2, 1] - Position [715, 97, 745, 128] - ZOrder 705 - ShowName off - Operator "OR" - AllPortsSameDT off - OutDataTypeStr "boolean" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n3" - SID "3274" - Position [575, 99, 680, 111] - ZOrder 704 - ShowName off - Value "Config.SCOPES_ALL" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n4" - SID "3275" - Position [570, 114, 685, 126] - ZOrder 706 - ShowName off - Value "Config.SCOPES_MAIN" - } - Block { - BlockType SubSystem - Name "Visualizer" - SID "4504" - Ports [9, 0, 1] - Position [715, 151, 855, 509] - ZOrder 707 - RequestExecContextInheritance off - System { - Name "Visualizer" - 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 "185" - Block { - BlockType Inport - Name "tau_des" - SID "4505" - Position [65, 53, 95, 67] - ZOrder 591 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_b" - SID "4506" - Position [865, -182, 895, -168] - ZOrder 592 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4507" - Position [205, 143, 235, 157] - ZOrder 593 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj_des" - SID "4508" - Position [730, 218, 760, 232] - ZOrder 594 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "CoM_Error" - SID "4509" - Position [65, 473, 95, 487] - ZOrder 595 - Port "5" - IconDisplay "Port number" - Port { - PortNumber 1 - Name "CoM_Error" - } - } - Block { - BlockType Inport - Name "fDesQP" - SID "4510" - Position [815, 493, 845, 507] - ZOrder 596 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "CoM_des" - SID "4511" - Position [65, 508, 95, 522] - ZOrder 597 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "fDesNoQP" - SID "4512" - Position [815, 613, 845, 627] - ZOrder 598 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "tau_des_afterFilter" - SID "4562" - Position [65, -92, 95, -78] - ZOrder 873 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "4513" - Ports [] - Position [72, 290, 92, 310] - ZOrder 599 - } - Block { - BlockType Scope - Name "CoM" - SID "2286" - Ports [4] - Position [540, 465, 645, 600] - 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','comData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation" - "','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{s" - "truct('MinYLimReal','-0.15969','MaxYLimReal','0.73753','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','0.7" - "3753','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor" - "',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980392156" - "86 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215" - "686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title" - "','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0." - "5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" - "le','on')}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'CoM_Measured:1','CoM_Measured:2','CoM_Me" - "asured:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.14765','MaxYLimReal','0.63025','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',3,'LineNames',{{'CoM_" - "Desired:1','CoM_Desired:2','CoM_Desired:3'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-0.02168'," - "'MaxYLimReal','0.10835','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true" - ",'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6" - "86274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353" - " 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;" - "0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{" - "{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1]," - "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWid" - "th',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none'," - "'Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" - "Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}}" - ",'NumLines',3,'LineNames',{{'CoM_Error:1','CoM_Error:2','CoM_Error:3'}},'ShowContent',true,'Placement',3),struc" - "t('MinYLimReal','0.625','MaxYLimReal','4.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibil" - "ity','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922" - " 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706" - " 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882352" - "9 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','" - "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" - "truct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'L" - "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDef" - "inedChannelNames',{{}},'NumLines',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',4)},'DisplayProperty" - "Defaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686" - "274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0" - ".16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 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',1),'TimeRangeSamples','400','TimeRangeFrames','40" - "0','DisplayLayoutDimensions',[4 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'Pre" - "viousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','L" - "ocation',[66 78 2561 1439])" - NumInputPorts "4" - Floating off - } - Block { - BlockType Selector - Name "CoM6D -> \nCoMXYZ1" - SID "3703" - Ports [1, 1] - Position [970, -99, 1010, -61] - 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 [970, -194, 1010, -156] - 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 "3311" - Ports [1, 2] - Position [990, 468, 995, 532] - ZOrder 543 - ShowName off - Outputs "2" - DisplayOption "bar" - Port { - PortNumber 1 - Name "wL_QP" - } - Port { - PortNumber 2 - Name "wR_QP" - } - } - Block { - BlockType Demux - Name "Demux1" - SID "4571" - Ports [1, 5] - Position [1090, 41, 1095, 139] - 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, 176, 450, 274] - 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 [1090, 176, 1095, 274] - 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 "3312" - Ports [1, 2] - Position [1070, 531, 1075, 589] - ZOrder 544 - ShowName off - Outputs "2" - DisplayOption "bar" - Port { - PortNumber 1 - Name "error_L" - } - Port { - PortNumber 2 - Name "error_R" - } - } - Block { - BlockType Demux - Name "Demux4" - SID "3313" - Ports [1, 2] - Position [1070, 591, 1075, 649] - ZOrder 545 - ShowName off - Outputs "2" - DisplayOption "bar" - Port { - PortNumber 1 - Name "wL_NO_QP" - } - Port { - PortNumber 2 - Name "wR_NO_QP" - } - } - Block { - BlockType Demux - Name "Demux5" - SID "4573" - Ports [1, 5] - Position [1090, 311, 1095, 409] - 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 "Demux6" - SID "4574" - Ports [1, 5] - Position [445, -269, 450, -171] - ZOrder 881 - 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, -134, 450, -36] - 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, 11, 450, 109] - 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, 311, 450, 409] - 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 Gain - Name "Gain" - SID "2289" - Position [880, 72, 930, 108] - 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 "Gain3" - SID "2290" - Position [880, 207, 930, 243] - 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 [880, 342, 930, 378] - 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 "Get Measurement" - SID "4542" - Ports [0, 1] - Position [705, 76, 780, 104] - ZOrder 869 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Position" - Port { - PortNumber 1 - Name "qj" - } - } - Block { - BlockType Reference - Name "Get Measurement1" - SID "4543" - Ports [0, 1] - Position [50, 346, 115, 374] - ZOrder 870 - BackgroundColor "[0.827500, 0.576500, 0.603900]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Torque" - } - Block { - BlockType Selector - Name "Selector" - SID "2294" - Ports [1, 1] - Position [135, 506, 190, 524] - ZOrder 507 - ShowName off - NumberOfDimensions "2" - IndexOptions "Index vector (dialog),Index vector (dialog)" - Indices "[1:3],1" - OutputSizes "1,1" - Port { - PortNumber 1 - Name "CoM_Desired" - } - } - Block { - BlockType Sum - Name "Sum" - SID "2295" - Ports [2, 1] - Position [210, 215, 230, 235] - ZOrder 493 - ShowName off - IconShape "round" - Inputs "|+-" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - Port { - PortNumber 1 - Name "torqueError" - } - } - Block { - BlockType Sum - Name "Sum1" - SID "2296" - Ports [2, 1] - Position [255, 470, 275, 490] - ZOrder 276 - ShowName off - IconShape "round" - Inputs "|++" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - Port { - PortNumber 1 - Name "CoM_Measured" - } - } - Block { - BlockType Sum - Name "Sum2" - SID "2297" - Ports [2, 1] - Position [985, 550, 1005, 570] - ZOrder 484 - ShowName off - IconShape "round" - Inputs "|+-" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Sum - Name "Sum3" - SID "2298" - Ports [2, 1] - Position [815, 350, 835, 370] - ZOrder 506 - ShowName off - IconShape "round" - Inputs "-+|" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Sum - Name "Sum4" - SID "4566" - Ports [2, 1] - Position [215, -230, 235, -210] - ZOrder 875 - ShowName off - IconShape "round" - Inputs "|+-" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - Port { - PortNumber 1 - Name "torqueError" - } - } - Block { - BlockType Scope - Name "base Pose" - SID "3704" - Ports [2] - Position [1105, -222, 1210, -33] - 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','basePoseData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecim" - "ation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplay" - "s',{struct('MinYLimReal','-0.24275','MaxYLimReal','0.62134','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',3,'LineNames',{{'basePosition:1','basePosition:2','basePosition" - ":3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.24275','MaxYLimReal','0.62134','YLabelReal',''" - ",'MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'Axe" - "sColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.066" - "6666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.8" - "31372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509803921" - "5686 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','-','LineWid" - "th',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',9,'LineNames',{{'baseOrient" - "ation:1','baseOrientation:2','baseOrientation:3','baseOrientation:4','baseOrientation:5','baseOrientation:6','b" - "aseOrientation:7','baseOrientation:8','baseOrientation:9'}},'ShowContent',true,'Placement',2)},'DisplayProperty" - "Defaults',struct('MinYLimReal','-0.24275','MaxYLimReal','0.62134','YLabelReal','','MinYLimMag','0','MaxYLimMag'" - ",'10','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.392156862" - "745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07" - "45098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0" - ".5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" - "ble','on'),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','400','TimeRangeFrames','400','DisplayLayoutDimensions" - "',[2 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tools','" - "Measurements',true,'Version','2017b')),'Version','2018a','Location',[640 424 1920 1072])" - NumInputPorts "2" - Floating off - } - Block { - BlockType Scope - Name "f" - SID "2318" - Ports [7] - Position [1230, 475, 1345, 675] - ZOrder 483 - 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','feetForcesData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataL" - "oggingDecimateData',true,'DataLogging',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispl" - "ays',{struct('MinYLimReal','-89.58974','MaxYLimReal','373.15892','YLabelReal','','MinYLimMag','0.00000','MaxYLi" - "mMag','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.0666666666666667;0.0" - "745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0." - "0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6509803921" - "56863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[" - "1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','" - "LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker'," - "'none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',6,'LineNames',{{'wL_QP:1','wL_QP:2','wL_QP:3" - "','wL_QP:4','wL_QP:5','wL_QP:6'}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-90.08017','MaxYLimRe" - "al','373.36109','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','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',{{'wR_QP:1','wR_QP:2','wR_QP:3','wR_QP:4','wR_QP:5','wR_QP:6'}},'ShowContent',true,'Placemen" - "t',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],'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',6,'LineNames',{{'error_L:1','error_L:2','error_L:3','error" - "_L:4','error_L:5','error_L:6'}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-39.55039','MaxYLimReal" - "','20.45784','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','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('C" - "olor',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle" - "','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " - "0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines" - "',6,'LineNames',{{'error_R:1','error_R:2','error_R:3','error_R:4','error_R:5','error_R:6'}},'ShowContent',true," - "'Placement',4),struct('MinYLimReal','-100.81491','MaxYLimReal','374.4062','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',6,'LineNames',{{'wL_NO_QP:1','wL_NO_QP:2','wL_NO" - "_QP:3','wL_NO_QP:4','wL_NO_QP:5','wL_NO_QP:6'}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-102.34" - "192','MaxYLimReal','374.72354','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGr" - "id',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" - "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647" - "05882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039" - "21569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperties" - "Cache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.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')}},'UserDefinedChannelNam" - "es',{{}},'NumLines',6,'LineNames',{{'wR_NO_QP:1','wR_NO_QP:2','wR_NO_QP:3','wR_NO_QP:4','wR_NO_QP:5','wR_NO_QP:" - "6'}},'ShowContent',true,'Placement',6),struct('MinYLimReal','-0.5','MaxYLimReal','14.5','YLabelReal','','MinYLi" - "mMag','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.0666666666" - "666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137254" - "9019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0." - "650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none" - "','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct" - "('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineSt" - "yle','-','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'}},'ShowCon" - "tent',true,'Placement',7)},'DisplayPropertyDefaults',struct('YLabelReal','','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',1),'Ti" - "meRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[7 1]),extmgr.Configuration('Tools','Plot" - " Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true" - ",'Version','2017b')),'Version','2018a','Location',[411 452 1701 1130])" - NumInputPorts "7" - Floating off - } - Block { - BlockType Scope - Name "qj" - SID "2333" - Ports [6] - Position [1245, 42, 1330, 158] - ZOrder 312 - ShowName 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','jointData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimati" - "on','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays'," - "{struct('MinYLimReal','-2.41385','MaxYLimReal','5.58471','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','5" - ".58471','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickCol" - "or',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803921" - "5686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392" - "15686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Tit" - "le','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" - "uct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth'," - "0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vis" - "ible','on')}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'torso:1','torso:2','torso:3'}},'ShowCo" - "ntent',true,'Placement',1),struct('MinYLimReal','-39.61854','MaxYLimReal','58.9684','YLabelReal','','MinYLimMag" - "','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0" - " 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.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',4,'LineNames',{{'left_arm:1','left_arm:" - "2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-39.58369','MaxYLimReal'" - ",'58.65801','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',tru" - "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745098039" - "22],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784313" - "72549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294" - "117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Col" - "or',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle'," - "'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 " - "1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'," - "4,'LineNames',{{'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3),str" - "uct('MinYLimReal','-8.16303','MaxYLimReal','11.36009','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',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','-11.45092','MaxYLimReal','7." - "90757','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'Pl" - "otMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'" - "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.058823529411764" - "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','" - "LineWidth',0.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'),s" - "truct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'L" - "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',6,'Li" - "neNames',{{'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.75','MaxYLimReal','3.25','YLabelReal','','MinYLimMag','0','MaxYLimM" - "ag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickCo" - "lor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07450980392" - "15686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039" - "215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Ti" - "tle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','" - "none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" - "ruct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'Li" - "neStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth'" - ",0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vi" - "sible','on')}},'UserDefinedChannelNames',{{}},'NumLines',1,'LineNames',{{'state'}},'ShowContent',true,'Placemen" - "t',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.650980392156863],'Title','%','L" - "inePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" - "ruct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'Li" - "neStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth'" - ",0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Vi" - "sible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefi" - "nedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','40'" - ",'TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'O" - "nceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2018a'))" - ",'Version','2018a','Location',[135 165 3841 2159])" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "qjDes" - SID "2334" - Ports [6] - Position [1245, 177, 1330, 293] - ZOrder 501 - ShowName 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','jointDesiredData','DataLoggingSaveFormat','StructureWithTime','DataLoggingD" - "ecimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDis" - "plays',{struct('MinYLimReal','-30.77422','MaxYLimReal','32.14309','YLabelReal','','MinYLimMag','0.00000','MaxYL" - "imMag','32.14309','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','-85.21733','MaxYLimReal','102.31959','YLabelReal',''" - ",'MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'Axe" - "sColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.066" - "6666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.8" - "31372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509803921" - "5686 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','-','LineWid" - "th',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','-80.99471','" - "MaxYLimReal','115.95153','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',tru" - "e,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0." - "686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.41176470588235" - "3 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1" - ";0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache'," - "{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}" - "},'NumLines',4,'LineNames',{{'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Plac" - "ement',3),struct('MinYLimReal','-15.85804','MaxYLimReal','42.9704','YLabelReal','','MinYLimMag','0','MaxYLimMag" - "','10','LegendVisibility','on','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',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.50179','Ma" - "xYLimReal','107.90787','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',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.625','MaxYLimReal','4.375','YLabelReal','','MinYLim" - "Mag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[" - "0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.066666666666" - "6667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.8313725490" - "19608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65" - "0980392156863],'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',1,'LineNames',{{'state'}},'ShowContent',true,'Plac" - "ement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803" - "922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764" - "706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882" - "3529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%" - "','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'User" - "DefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples'," - "'40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',tru" - "e,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017" - "b')),'Version','2018a','Location',[76 179 1784 1070])" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "qj_err" - SID "2335" - Ports [6] - Position [1245, 310, 1330, 430] - 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','jointErrorData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDec" - "imation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispl" - "ays',{struct('MinYLimReal','-10.85715','MaxYLimReal','13.2441','YLabelReal','','MinYLimMag','0.00000','MaxYLimM" - "ag','13.2441','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesT" - "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509" - "8039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450" - "98039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863" - "],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Mark" - "er','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none" - "','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" - "t',1),struct('MinYLimReal','-24.65677','MaxYLimReal','13.37484','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',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),stru" - "ct('MinYLimReal','-37.11648','MaxYLimReal','33.25109','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','-28.71162','MaxYLimReal','40.05399','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','-1" - "6.66332','MaxYLimReal','13.39274','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','X" - "Grid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509" - "803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.41176" - "4705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450980" - "3921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperti" - "esCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" - "',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelN" - "ames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-0.375','Max" - "YLimReal','13.375','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',{{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',0,'Line" - "Names',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[" - "0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.066666666666" - "6667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.8313725490" - "19608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65" - "0980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWid" - "th',0.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],'LineStyl" - "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'" - "Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',t" - "rue,'Placement',1),'TimeRangeSamples','40','TimeRangeFrames','40','DisplayLayoutDimensions',[6 1]),extmgr.Confi" - "guration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tool" - "s','Measurements',true,'Version','2017b')),'Version','2018a','Location',[76 146 1996 1074])" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "tauDes" - SID "2336" - Ports [6] - Position [540, 15, 645, 125] - 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','tauDesiredData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDec" - "imation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDispl" - "ays',{struct('MinYLimReal','-19.59759','MaxYLimReal','27.79759','YLabelReal','','MinYLimMag','0.00000','MaxYLim" - "Mag','27.79759','LegendVisibility','On','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',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.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',2),stru" - "ct('MinYLimReal','-11.21869','MaxYLimReal','8.29753','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legend" - "Visibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450" - "9803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" - "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470" - "58823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " - "1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','Li" - "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'" - "UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLim" - "Real','-51.65238','MaxYLimReal','50.72666','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility" - "','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0." - "686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;" - "1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0" - ".274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Lin" - "ePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" - "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0" - ".5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visi" - "ble','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-5." - "64312','MaxYLimReal','14.99269','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGr" - "id',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" - "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647" - "05882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039" - "21569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperties" - "Cache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.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')}},'UserDefinedChannelNam" - "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','-0.5','MaxYLim" - "Real','14.5','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('Color',[1 1 0],'" - "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidt" - "h',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" - "olor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle" - "','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames'" - ",{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0]" - ",'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0" - ".0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 " - "0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.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',0,'LineNames',{{[]}},'ShowContent',true,'P" - "lacement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configurati" - "on('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Me" - "asurements',true,'Version','2017b')),'Version','2018a','Location',[816 261 1744 1234])" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "tauDes_afterFilter" - SID "4561" - Ports [6] - Position [540, -130, 645, -20] - 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','ScopeData1','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimat" - "ion','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays'" - ",{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],'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',3,'LineNames',{{'torso:1','torso:2','torso:3'}},'Sh" - "owContent',true,'Placement',1),struct('MinYLimReal','-10.12731','MaxYLimReal','6.52875','YLabelReal','','MinYLi" - "mMag','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.06666666666" - "66667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549" - "019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.6" - "50980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'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],'LineSty" - "le','-','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','-14.66185','MaxYLimR" - "eal','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.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',4,'LineNames',{{'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3)," - "struct('MinYLimReal','-23.11356','MaxYLimReal','33.28118','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','L" - "egendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686" - "274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.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',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','-30.29324','MaxYLimReal'" - ",'27.04418','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',tru" - "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745098039" - "22],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784313" - "72549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294" - "117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Col" - "or',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle'," - "'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 " - "1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'," - "6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right_leg:6'}},'ShowCont" - "ent',true,'Placement',5),struct('MinYLimReal','0.25','MaxYLimReal','7.75','YLabelReal','','MinYLimMag','0','Max" - "YLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesT" - "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509" - "8039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450" - "98039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863" - "],'Title','','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','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',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',6)},'D" - "isplayPropertyDefaults',struct('YLabelReal','','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',0,'LineNames',{{" - "[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions'" - ",[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr" - ".Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[706 340 2551 1268]" - ")" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "tauDes_diff" - SID "4568" - Ports [6] - Position [540, -265, 645, -155] - ZOrder 876 - 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','ScopeData2','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimat" - "ion','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays'" - ",{struct('MinYLimReal','-1.00000','MaxYLimReal','1.00000','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','" - "1.00000','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',3,'LineNames',{{'torso:1','torso:2','torso:3'}},'ShowC" - "ontent',true,'Placement',1),struct('MinYLimReal','-1.00000','MaxYLimReal','1.00000','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.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',4,'LineNames',{{'left_arm:1','left_arm:" - "2','left_arm:3','left_arm:4'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-1.00000','MaxYLimReal'," - "'1.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true," - "'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922" - "],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" - "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411" - "7647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color" - "',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',4," - "'LineNames',{{'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3),struc" - "t('MinYLimReal','-1.00000','MaxYLimReal','1.00000','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',6,'LineNames',{{'left_leg:1','left_leg:2','left_leg:3','left_leg:4','lef" - "t_leg:5','left_leg:6'}},'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',6,'LineNa" - "mes',{{'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.5','MaxYLimReal','14.5','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','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',6)},'DisplayPro" - "pertyDefaults',struct('YLabelReal','','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',0,'LineNames',{{[]}},'Sho" - "wContent',true,'Placement',1),'TimeRangeSamples','14','TimeRangeFrames','14','DisplayLayoutDimensions',[6 1]),e" - "xtmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configur" - "ation('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[712 340 2557 1268])" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "tauError" - SID "2337" - Ports [6] - Position [540, 178, 645, 292] - 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','tauErrorData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecim" - "ation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplay" - "s',{struct('MinYLimReal','-23.63249','MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMa" - "g','27.95301','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesT" - "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509" - "8039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450" - "98039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863" - "],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Mark" - "er','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none" - "','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'torso:1','torso:2','torso:3'}},'" - "ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabelReal','','MinYL" - "imMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor'" - ",[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666" - "666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83137254" - "9019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0." - "650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none" - "','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct" - "('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineSt" - "yle','-','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.17969','MaxYLimR" - "eal','7.65402','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',4,'LineNames',{{'right_arm:1','right_arm:2','right_arm:3','right_arm:4'}},'ShowContent',true,'Placement',3)," - "struct('MinYLimReal','-23.22756','MaxYLimReal','25.22297','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','L" - "egendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686" - "274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.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',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','-66.44849','MaxYLimReal'" - ",'41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',tru" - "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745098039" - "22],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784313" - "72549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294" - "117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Col" - "or',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle'," - "'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 " - "1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines'," - "6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right_leg:6'}},'ShowCont" - "ent',true,'Placement',5),struct('MinYLimReal','0.25','MaxYLimReal','7.75','YLabelReal','','MinYLimMag','0','Max" - "YLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesT" - "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509" - "8039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450" - "98039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863" - "],'Title','','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','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',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',6)},'D" - "isplayPropertyDefaults',struct('YLabelReal','','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 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" - "or',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle'," - "'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChanne" - "lNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','90','TimeRan" - "geFrames','90','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop" - "',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version" - "','2018a','Location',[66 78 1921 1079])" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "tauMeasured" - SID "2338" - Ports [6] - Position [540, 313, 645, 427] - 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','tauMeasuredData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDe" - "cimation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisp" - "lays',{struct('MinYLimReal','-13.53306','MaxYLimReal','13.58663','YLabelReal','','MinYLimMag','0.00000','MaxYLi" - "mMag','13.58663','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',3,'LineNames',{{'torso:1','torso:2','torso:3'}" - "},'ShowContent',true,'Placement',1),struct('MinYLimReal','-4.19072','MaxYLimReal','4.87188','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','-4.21765','MaxYL" - "imReal','4.8762','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid" - "',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.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',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.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',6,'LineNames',{{'left_leg:1','left_leg:2','left_leg:3','left_le" - "g:4','left_leg:5','left_leg:6'}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-44.89002','MaxYLimRea" - "l','34.01019','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('C" - "olor',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle" - "','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " - "0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines" - "',6,'LineNames',{{'right_leg:1','right_leg:2','right_leg:3','right_leg:4','right_leg:5','right_leg:6'}},'ShowCo" - "ntent',true,'Placement',5),struct('MinYLimReal','-0.5','MaxYLimReal','14.5','YLabelReal','','MinYLimMag','0','M" - "axYLimMag','10','LegendVisibility','on','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,'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',1,'LineNames',{{'state'}},'ShowContent',true,'Placement',6)}," - "'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627" - "4509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4" - "11764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745" - "09803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProp" - "ertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" - "olor',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle" - "','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChan" - "nelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','14','TimeR" - "angeFrames','14','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtSt" - "op',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2018a','Location',[1271 533 2561 1263])" - NumInputPorts "6" - Floating off - } - Line { - Name "CoM_Error" - ZOrder 391 - SrcBlock "CoM_Error" - SrcPort 1 - Points [114, 0] - Branch { - ZOrder 346 - DstBlock "Sum1" - DstPort 1 - } - Branch { - ZOrder 3 - Labels [-1, 0] - Points [0, 70] - DstBlock "CoM" - DstPort 3 - } - } - Line { - ZOrder 393 - SrcBlock "CoM_des" - SrcPort 1 - DstBlock "Selector" - DstPort 1 - } - Line { - ZOrder 702 - SrcBlock "Gain" - SrcPort 1 - DstBlock "Demux1" - DstPort 1 - } - Line { - Name "CoM_Measured" - ZOrder 5 - Labels [-1, 0] - SrcBlock "Sum1" - SrcPort 1 - DstBlock "CoM" - DstPort 1 - } - Line { - Name "qj" - ZOrder 479 - Labels [1, 1] - SrcBlock "Get Measurement" - SrcPort 1 - Points [40, 0] - Branch { - ZOrder 364 - 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 "qj" - DstPort 5 - } - Line { - Name "torso" - ZOrder 698 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 1 - DstBlock "qj" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 697 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 2 - DstBlock "qj" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 701 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 4 - DstBlock "qj" - DstPort 4 - } - Line { - ZOrder 392 - SrcBlock "fDesQP" - SrcPort 1 - Points [44, 0] - Branch { - ZOrder 377 - Points [0, 60] - DstBlock "Sum2" - DstPort 1 - } - Branch { - ZOrder 376 - DstBlock "Demux" - DstPort 1 - } - } - Line { - Name "right_arm" - ZOrder 699 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 3 - DstBlock "qj" - DstPort 3 - } - Line { - ZOrder 25 - SrcBlock "Sum2" - SrcPort 1 - DstBlock "Demux3" - DstPort 1 - } - Line { - ZOrder 394 - SrcBlock "fDesNoQP" - SrcPort 1 - Points [145, 0] - Branch { - ZOrder 385 - DstBlock "Sum2" - DstPort 2 - } - Branch { - ZOrder 380 - DstBlock "Demux4" - DstPort 1 - } - } - Line { - ZOrder 480 - SrcBlock "Get Measurement1" - SrcPort 1 - Points [100, 0] - Branch { - ZOrder 686 - DstBlock "Demux9" - DstPort 1 - } - Branch { - ZOrder 332 - DstBlock "Sum" - DstPort 2 - } - } - Line { - Name "right_leg" - ZOrder 666 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 5 - DstBlock "tauDes" - DstPort 5 - } - Line { - Name "torso" - ZOrder 668 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 1 - DstBlock "tauDes" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 667 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 2 - DstBlock "tauDes" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 665 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 4 - DstBlock "tauDes" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 669 - Labels [-1, 0] - SrcBlock "Demux8" - SrcPort 3 - DstBlock "tauDes" - DstPort 3 - } - Line { - Name "right_leg" - ZOrder 656 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 5 - DstBlock "tauError" - DstPort 5 - } - Line { - Name "torso" - ZOrder 659 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 1 - DstBlock "tauError" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 658 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 2 - DstBlock "tauError" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 657 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 4 - DstBlock "tauError" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 655 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 3 - DstBlock "tauError" - DstPort 3 - } - Line { - Name "torqueError" - ZOrder 685 - Labels [0, 0] - SrcBlock "Sum" - SrcPort 1 - DstBlock "Demux10" - DstPort 1 - } - Line { - Name "right_leg" - ZOrder 660 - Labels [-1, 0] - SrcBlock "Demux9" - SrcPort 5 - DstBlock "tauMeasured" - DstPort 5 - } - Line { - Name "torso" - ZOrder 661 - Labels [-1, 0] - SrcBlock "Demux9" - SrcPort 1 - DstBlock "tauMeasured" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 662 - Labels [-1, 0] - SrcBlock "Demux9" - SrcPort 2 - DstBlock "tauMeasured" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 663 - Labels [-1, 0] - SrcBlock "Demux9" - SrcPort 4 - DstBlock "tauMeasured" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 664 - Labels [-1, 0] - SrcBlock "Demux9" - SrcPort 3 - DstBlock "tauMeasured" - DstPort 3 - } - Line { - ZOrder 390 - SrcBlock "qj_des" - SrcPort 1 - Points [28, 0] - Branch { - ZOrder 52 - Points [0, 135] - 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 "qjDes" - DstPort 5 - } - Line { - Name "torso" - ZOrder 693 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 1 - DstBlock "qjDes" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 695 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 2 - DstBlock "qjDes" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 692 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 4 - DstBlock "qjDes" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 696 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 3 - DstBlock "qjDes" - 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 "qj_err" - DstPort 5 - } - Line { - Name "torso" - ZOrder 688 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 1 - DstBlock "qj_err" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 689 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 2 - DstBlock "qj_err" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 691 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 4 - DstBlock "qj_err" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 687 - Labels [-1, 0] - SrcBlock "Demux5" - SrcPort 3 - DstBlock "qj_err" - DstPort 3 - } - Line { - ZOrder 70 - SrcBlock "Sum3" - SrcPort 1 - DstBlock "Gain4" - DstPort 1 - } - Line { - Name "CoM_Desired" - ZOrder 71 - SrcBlock "Selector" - SrcPort 1 - Points [70, 0] - Branch { - ZOrder 353 - DstBlock "Sum1" - DstPort 2 - } - Branch { - ZOrder 348 - Labels [-1, 0] - DstBlock "CoM" - DstPort 2 - } - } - Line { - Name "wL_QP" - ZOrder 91 - Labels [-1, 0] - SrcBlock "Demux" - SrcPort 1 - DstBlock "f" - DstPort 1 - } - Line { - Name "wR_QP" - ZOrder 92 - Labels [-1, 0] - SrcBlock "Demux" - SrcPort 2 - DstBlock "f" - DstPort 2 - } - Line { - Name "error_L" - ZOrder 93 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 1 - DstBlock "f" - DstPort 3 - } - Line { - Name "error_R" - ZOrder 94 - Labels [-1, 0] - SrcBlock "Demux3" - SrcPort 2 - DstBlock "f" - DstPort 4 - } - Line { - Name "wL_NO_QP" - ZOrder 95 - Labels [-1, 0] - SrcBlock "Demux4" - SrcPort 1 - DstBlock "f" - DstPort 5 - } - Line { - Name "wR_NO_QP" - ZOrder 96 - Labels [-1, 0] - SrcBlock "Demux4" - SrcPort 2 - DstBlock "f" - DstPort 6 - } - Line { - ZOrder 388 - SrcBlock "w_H_b" - SrcPort 1 - Points [24, 0] - Branch { - ZOrder 102 - Points [0, 95] - DstBlock "CoM6D -> \nCoMXYZ1" - DstPort 1 - } - Branch { - ZOrder 101 - DstBlock "CoM6D -> \nCoMXYZ2" - DstPort 1 - } - } - Line { - Name "basePosition" - ZOrder 103 - Labels [1, 1] - SrcBlock "CoM6D -> \nCoMXYZ2" - SrcPort 1 - DstBlock "base Pose" - DstPort 1 - } - Line { - Name "baseOrientation" - ZOrder 104 - Labels [1, 1] - SrcBlock "CoM6D -> \nCoMXYZ1" - SrcPort 1 - DstBlock "base Pose" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 670 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 4 - DstBlock "tauDes_afterFilter" - DstPort 4 - } - Line { - Name "right_leg" - ZOrder 673 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 5 - DstBlock "tauDes_afterFilter" - DstPort 5 - } - Line { - Name "left_arm" - ZOrder 671 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 2 - DstBlock "tauDes_afterFilter" - DstPort 2 - } - Line { - Name "torso" - ZOrder 674 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 1 - DstBlock "tauDes_afterFilter" - DstPort 1 - } - Line { - Name "right_arm" - ZOrder 672 - Labels [-1, 0] - SrcBlock "Demux7" - SrcPort 3 - DstBlock "tauDes_afterFilter" - DstPort 3 - } - Line { - ZOrder 581 - SrcBlock "tau_des_afterFilter" - SrcPort 1 - Points [125, 0] - Branch { - ZOrder 681 - DstBlock "Demux7" - DstPort 1 - } - Branch { - ZOrder 599 - DstBlock "Sum4" - DstPort 2 - } - } - Line { - Name "torqueError" - ZOrder 680 - Labels [0, 0] - SrcBlock "Sum4" - SrcPort 1 - DstBlock "Demux6" - DstPort 1 - } - Line { - ZOrder 387 - SrcBlock "tau_des" - SrcPort 1 - Points [41, 0] - Branch { - ZOrder 684 - DstBlock "Demux8" - DstPort 1 - } - Branch { - ZOrder 629 - Points [0, 165] - DstBlock "Sum" - DstPort 1 - } - Branch { - ZOrder 600 - Points [0, -280] - DstBlock "Sum4" - DstPort 1 - } - } - Line { - Name "right_leg" - ZOrder 676 - Labels [-1, 0] - SrcBlock "Demux6" - SrcPort 5 - DstBlock "tauDes_diff" - DstPort 5 - } - Line { - ZOrder 389 - SrcBlock "state" - SrcPort 1 - Points [94, 0] - Branch { - ZOrder 580 - Points [0, -175] - Branch { - ZOrder 595 - Points [0, -135] - DstBlock "tauDes_diff" - DstPort 6 - } - Branch { - ZOrder 594 - DstBlock "tauDes_afterFilter" - DstPort 6 - } - } - Branch { - ZOrder 75 - Points [76, 0] - Branch { - ZOrder 329 - Points [554, 0] - Branch { - ZOrder 78 - Points [0, 135] - Branch { - ZOrder 79 - Points [0, 135] - DstBlock "qj_err" - DstPort 6 - } - Branch { - ZOrder 82 - DstBlock "qjDes" - DstPort 6 - } - } - Branch { - ZOrder 83 - DstBlock "qj" - DstPort 6 - } - } - Branch { - ZOrder 76 - Points [0, -30] - DstBlock "tauDes" - DstPort 6 - } - } - Branch { - ZOrder 84 - Points [0, 135] - Branch { - ZOrder 85 - Points [0, 135] - Branch { - ZOrder 86 - Points [0, 165] - Branch { - ZOrder 350 - Points [0, 80] - DstBlock "f" - DstPort 7 - } - Branch { - ZOrder 88 - DstBlock "CoM" - DstPort 4 - } - } - Branch { - ZOrder 89 - DstBlock "tauMeasured" - DstPort 6 - } - } - Branch { - ZOrder 90 - DstBlock "tauError" - DstPort 6 - } - } - } - Line { - Name "right_arm" - ZOrder 675 - Labels [-1, 0] - SrcBlock "Demux6" - SrcPort 3 - DstBlock "tauDes_diff" - DstPort 3 - } - Line { - Name "left_arm" - ZOrder 679 - Labels [-1, 0] - SrcBlock "Demux6" - SrcPort 2 - DstBlock "tauDes_diff" - DstPort 2 - } - Line { - Name "left_leg" - ZOrder 677 - Labels [-1, 0] - SrcBlock "Demux6" - SrcPort 4 - DstBlock "tauDes_diff" - DstPort 4 - } - Line { - Name "torso" - ZOrder 678 - Labels [-1, 0] - SrcBlock "Demux6" - SrcPort 1 - DstBlock "tauDes_diff" - DstPort 1 - } - } - } - Line { - ZOrder 387 - SrcBlock "ON_GAZEBO\n3" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 1 - } - Line { - ZOrder 388 - SrcBlock "ON_GAZEBO\n4" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 2 - } - Line { - ZOrder 398 - SrcBlock "Logical\nOperator1" - SrcPort 1 - Points [35, 0] - DstBlock "Visualizer" - DstPort enable - } - Line { - ZOrder 390 - SrcBlock "From1" - SrcPort 1 - DstBlock "Visualizer" - DstPort 3 - } - Line { - ZOrder 391 - SrcBlock "From2" - SrcPort 1 - DstBlock "Visualizer" - DstPort 5 - } - Line { - ZOrder 392 - SrcBlock "From5" - SrcPort 1 - DstBlock "Visualizer" - DstPort 6 - } - Line { - ZOrder 393 - SrcBlock "From6" - SrcPort 1 - DstBlock "Visualizer" - DstPort 8 - } - Line { - ZOrder 394 - SrcBlock "From4" - SrcPort 1 - DstBlock "Visualizer" - DstPort 4 - } - Line { - ZOrder 395 - SrcBlock "From7" - SrcPort 1 - DstBlock "Visualizer" - DstPort 2 - } - Line { - ZOrder 396 - SrcBlock "From" - SrcPort 1 - DstBlock "Visualizer" - DstPort 1 - } - Line { - ZOrder 397 - SrcBlock "From3" - SrcPort 1 - DstBlock "Visualizer" - DstPort 7 - } - Line { - ZOrder 407 - SrcBlock "From8" - SrcPort 1 - DstBlock "Visualizer" - DstPort 9 - } - } - } - Block { - BlockType SubSystem - Name "Dynamics and Kinematics" - SID "2341" - Ports [3, 11] - Position [1040, 233, 1170, 557] - ZOrder 541 - BackgroundColor "[0.000000, 1.000000, 0.498039]" - NamePlacement "alternate" - RequestExecContextInheritance off - System { - Name "Dynamics and Kinematics" - 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 "312" - Block { - BlockType Inport - Name "w_H_b" - SID "2342" - Position [-850, -167, -820, -153] - ZOrder 209 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "2343" - Position [-850, -97, -820, -83] - ZOrder 224 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "nu" - SID "2344" - Position [-850, -27, -820, -13] - ZOrder 582 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "Dynamics" - SID "4353" - Ports [3, 3] - Position [-655, -196, -550, 16] - ZOrder 838 - RequestExecContextInheritance off - System { - Name "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 "479" - Block { - BlockType Inport - Name "w_H_b" - SID "4354" - Position [180, 28, 210, 42] - ZOrder 584 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "4355" - Position [180, 63, 210, 77] - ZOrder 585 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "nu" - SID "4356" - Position [20, 188, 50, 202] - ZOrder 586 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "Add motors reflected inertias" - SID "4515" - Ports [1, 1] - Position [580, 34, 705, 76] - ZOrder -19 - RequestExecContextInheritance off - System { - Name "Add motors reflected inertias" - 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 "594" - Block { - BlockType Inport - Name "M" - SID "4516" - Position [40, 138, 70, 152] - ZOrder 591 - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "Add motor reflected inertias" - SID "4518" - Ports [1, 1] - Position [140, 110, 305, 180] - 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 "70" - 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::69" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 59 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4518::68" - Tag "Stateflow S-Function torqueBalancingYoga 6" - Ports [1, 2] - Position [180, 100, 230, 160] - ZOrder 58 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[1 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "M_with_inertia" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4518::70" - Position [460, 241, 480, 259] - ZOrder 60 - } - Block { - BlockType Outport - Name "M_with_inertia" - SID "4518::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 78 - SrcBlock "M" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - Name "M_with_inertia" - ZOrder 79 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "M_with_inertia" - DstPort 1 - } - Line { - ZOrder 80 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 81 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Outport - Name "M_with_motor_inertia" - SID "4517" - Position [385, 138, 415, 152] - ZOrder 592 - IconDisplay "Port number" - } - Line { - ZOrder 109 - SrcBlock "M" - SrcPort 1 - DstBlock "Add motor reflected inertias" - DstPort 1 - } - Line { - ZOrder 110 - SrcBlock "Add motor reflected inertias" - SrcPort 1 - DstBlock "M_with_motor_inertia" - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "Centroidal Momentum" - SID "2345" - Ports [4, 1] - Position [370, 281, 555, 344] - ZOrder 223 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" - SourceType "Centroidal Momentum" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Selector - Name "CoM6D -> \nCoMXYZ1" - SID "2346" - Ports [1, 1] - Position [200, 211, 240, 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 [200, 186, 240, 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 "Get Bias Forces" - SID "2348" - Ports [4, 1] - Position [395, 132, 535, 233] - ZOrder 222 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Get Bias Forces" - SourceType "Get Generalized Bias Forces" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "Mass Matrix" - SID "2349" - Ports [2, 1] - Position [395, 19, 535, 86] - ZOrder 221 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Mass Matrix" - SourceType "Mass Matrix" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Outport - Name "M(q)" - SID "4357" - Position [750, 48, 780, 62] - ZOrder 587 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "biasTorques" - SID "4358" - Position [625, 178, 655, 192] - ZOrder 588 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "L(q,dq)" - SID "4359" - Position [625, 308, 655, 322] - ZOrder 589 - Port "3" - IconDisplay "Port number" - } - Line { - ZOrder 83 - SrcBlock "Centroidal Momentum" - SrcPort 1 - DstBlock "L(q,dq)" - DstPort 1 - } - Line { - ZOrder 82 - SrcBlock "Get Bias Forces" - SrcPort 1 - DstBlock "biasTorques" - DstPort 1 - } - Line { - ZOrder 78 - SrcBlock "w_H_b" - SrcPort 1 - Points [108, 0] - Branch { - ZOrder 5 - Points [0, 110] - Branch { - ZOrder 6 - Points [0, 145] - DstBlock "Centroidal Momentum" - DstPort 1 - } - Branch { - ZOrder 7 - DstBlock "Get Bias Forces" - DstPort 1 - } - } - Branch { - ZOrder 8 - DstBlock "Mass Matrix" - DstPort 1 - } - } - Line { - ZOrder 79 - SrcBlock "qj" - SrcPort 1 - Points [97, 0] - Branch { - ZOrder 10 - Points [0, 100] - Branch { - ZOrder 11 - Points [0, 135] - DstBlock "Centroidal Momentum" - DstPort 2 - } - Branch { - ZOrder 12 - DstBlock "Get Bias Forces" - DstPort 2 - } - } - Branch { - ZOrder 13 - DstBlock "Mass Matrix" - DstPort 2 - } - } - Line { - ZOrder 14 - SrcBlock "CoM6D -> \nCoMXYZ2" - SrcPort 1 - Points [56, 0] - Branch { - ZOrder 75 - Points [0, 125] - DstBlock "Centroidal Momentum" - DstPort 3 - } - Branch { - ZOrder 73 - DstBlock "Get Bias Forces" - DstPort 3 - } - } - Line { - ZOrder 17 - SrcBlock "CoM6D -> \nCoMXYZ1" - SrcPort 1 - Points [35, 0] - Branch { - ZOrder 77 - Points [0, 115] - DstBlock "Centroidal Momentum" - DstPort 4 - } - Branch { - ZOrder 76 - DstBlock "Get Bias Forces" - 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 "Mass Matrix" - SrcPort 1 - DstBlock "Add motors reflected inertias" - DstPort 1 - } - Line { - ZOrder 107 - SrcBlock "Add motors reflected inertias" - SrcPort 1 - DstBlock "M(q)" - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Kinematics" - SID "4321" - Ports [3, 8] - Position [-400, 21, -200, 359] - ZOrder 837 - RequestExecContextInheritance off - System { - Name "Kinematics" - 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 "335" - Block { - BlockType Inport - Name "nu" - SID "4342" - Position [-250, 508, -220, 522] - ZOrder 857 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - 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 Selector - Name "CoM6D -> \nCoMXYZ3" - SID "2369" - Ports [1, 1] - Position [-155, 525, -115, 535] - 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 [-155, 510, -115, 520] - 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 "DotJ Nu l_sole\n" - SID "2375" - Ports [4, 1] - Position [60, 395, 235, 460] - ZOrder 829 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - SourceType "DotJ Nu" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType Reference - Name "DotJ Nu r_sole\n " - SID "2376" - Ports [4, 1] - Position [65, 479, 235, 536] - ZOrder 831 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/DotJ Nu" - SourceType "DotJ Nu" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Reference - Name "Jacobian com" - SID "2378" - Ports [2, 1] - Position [60, 260, 225, 300] - ZOrder 835 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - 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.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - 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.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Reference - Name "l_sole" - SID "2405" - Ports [2, 1] - Position [60, 33, 220, 77] - ZOrder 826 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - 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.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType SubSystem - Name "xCom" - SID "4360" - Ports [2, 1] - Position [65, 331, 225, 369] - ZOrder 872 - ShowName off - RequestExecContextInheritance off - System { - Name "xCom" - 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 "942" - Block { - BlockType Inport - Name "w_H_b" - SID "4361" - Position [30, 237, 60, 253] - ZOrder 581 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "4362" - Position [30, 267, 60, 283] - ZOrder 582 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "CoM" - SID "4363" - Ports [2, 1] - Position [110, 230, 275, 290] - ZOrder 578 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.COM" - } - Block { - BlockType Selector - Name "CoM6D -> \nCoMXYZ2" - SID "4364" - Ports [1, 1] - Position [310, 241, 350, 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 "xCoM" - SID "4365" - Position [385, 253, 415, 267] - ZOrder 583 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "qj" - SrcPort 1 - DstBlock "CoM" - DstPort 2 - } - Line { - ZOrder 2 - SrcBlock "CoM6D -> \nCoMXYZ2" - SrcPort 1 - DstBlock "xCoM" - 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_l_sole" - SID "4350" - Position [335, 48, 365, 62] - ZOrder 865 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_r_sole" - SID "4367" - Position [335, 108, 365, 122] - ZOrder 874 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_l_sole" - SID "4368" - Position [335, 173, 365, 187] - ZOrder 875 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_r_sole" - SID "4369" - Position [335, 223, 365, 237] - ZOrder 876 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_CoM" - SID "4370" - Position [335, 273, 365, 287] - ZOrder 877 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "xCoM" - SID "4371" - Position [335, 343, 365, 357] - ZOrder 878 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "JDot_nu_l_sole" - SID "4372" - Position [335, 423, 365, 437] - ZOrder 879 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "JDot_nu_r_sole" - SID "4374" - Position [335, 503, 365, 517] - ZOrder 881 - Port "8" - IconDisplay "Port number" - } - Line { - ZOrder 37 - SrcBlock "CoM6D -> \nCoMXYZ3" - SrcPort 1 - Points [72, 0] - Branch { - ZOrder 203 - DstBlock "DotJ Nu r_sole\n " - DstPort 4 - } - Branch { - ZOrder 193 - Points [0, -80] - DstBlock "DotJ Nu l_sole\n" - DstPort 4 - } - } - Line { - ZOrder 62 - SrcBlock "CoM6D -> \nCoMXYZ4" - SrcPort 1 - Points [95, 0] - Branch { - ZOrder 206 - DstBlock "DotJ Nu r_sole\n " - DstPort 3 - } - Branch { - ZOrder 204 - Points [0, -80] - DstBlock "DotJ Nu l_sole\n" - DstPort 3 - } - } - Line { - ZOrder 129 - SrcBlock "nu" - SrcPort 1 - Points [12, 0] - Branch { - ZOrder 202 - Points [0, 15] - DstBlock "CoM6D -> \nCoMXYZ3" - DstPort 1 - } - Branch { - ZOrder 188 - DstBlock "CoM6D -> \nCoMXYZ4" - DstPort 1 - } - } - 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, 70] - Branch { - ZOrder 208 - Points [0, 65] - Branch { - ZOrder 220 - DstBlock "DotJ Nu l_sole\n" - DstPort 1 - } - Branch { - ZOrder 210 - Points [0, 80] - DstBlock "DotJ Nu r_sole\n " - DstPort 1 - } - } - Branch { - ZOrder 207 - DstBlock "xCom" - 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 144 - SrcBlock "qj" - SrcPort 1 - Points [235, 0] - Branch { - ZOrder 150 - 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, 70] - Branch { - ZOrder 213 - DstBlock "xCom" - DstPort 2 - } - Branch { - ZOrder 212 - Points [0, 60] - Branch { - ZOrder 219 - Points [0, 80] - DstBlock "DotJ Nu r_sole\n " - DstPort 2 - } - Branch { - ZOrder 214 - DstBlock "DotJ Nu l_sole\n" - 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 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 184 - SrcBlock "xCom" - SrcPort 1 - DstBlock "xCoM" - DstPort 1 - } - Line { - ZOrder 217 - SrcBlock "DotJ Nu l_sole\n" - SrcPort 1 - DstBlock "JDot_nu_l_sole" - DstPort 1 - } - Line { - ZOrder 218 - SrcBlock "DotJ Nu r_sole\n " - SrcPort 1 - DstBlock "JDot_nu_r_sole" - DstPort 1 - } - } - } - Block { - BlockType Outport - Name "M" - SID "2350" - Position [-475, -167, -445, -153] - ZOrder -2 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "biasTorques" - SID "2351" - Position [-475, -97, -445, -83] - ZOrder 217 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "L" - SID "2352" - Position [-475, -27, -445, -13] - ZOrder 216 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_l_sole" - SID "4375" - Position [-50, 43, -20, 57] - ZOrder 882 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_r_sole" - SID "4376" - Position [-50, 83, -20, 97] - ZOrder 883 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_l_sole" - SID "4377" - Position [-50, 123, -20, 137] - ZOrder 884 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_r_sole" - SID "4378" - Position [-50, 163, -20, 177] - ZOrder 885 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_CoM" - SID "4379" - Position [-50, 203, -20, 217] - ZOrder 886 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "xCoM" - SID "4380" - Position [-50, 243, -20, 257] - ZOrder 887 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "JDot_nu_l_sole" - SID "4381" - Position [-50, 283, -20, 297] - ZOrder 888 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "JDot_nu_r_sole" - SID "4382" - Position [-50, 323, -20, 337] - ZOrder 889 - Port "11" - IconDisplay "Port number" - } - 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 "qj" - 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 [94, 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 "biasTorques" - DstPort 1 - } - Line { - ZOrder 114 - SrcBlock "Dynamics" - SrcPort 3 - DstBlock "L" - DstPort 1 - } - Line { - ZOrder 129 - SrcBlock "Kinematics" - SrcPort 7 - DstBlock "JDot_nu_l_sole" - DstPort 1 - } - Line { - ZOrder 130 - SrcBlock "Kinematics" - SrcPort 8 - DstBlock "JDot_nu_r_sole" - 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 "xCoM" - DstPort 1 - } - Line { - ZOrder 124 - SrcBlock "Kinematics" - SrcPort 2 - DstBlock "w_H_r_sole" - DstPort 1 - } - } - } - Block { - BlockType Goto - Name "Goto" - SID "4564" - Position [1730, 578, 1835, 602] - ZOrder 718 - GotoTag "tau_des_afterFilter" - TagVisibility "global" - } - Block { - BlockType SubSystem - Name "Robot State and References" - SID "2087" - Ports [0, 10] - Position [590, 533, 750, 827] - 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 "254" - Block { - BlockType Reference - Name "Compare\nTo Zero" - SID "2916" - Ports [1, 1] - Position [-280, 2850, -250, 2880] - ZOrder 613 - ShowName off - LibraryVersion "1.441" - SourceBlock "simulink/Logic and Bit\nOperations/Compare\nTo Zero" - SourceType "Compare To Zero" - SourceProductName "Simulink" - SourceProductBaseCode "SL" - ContentPreviewEnabled off - relop "~=" - OutDataTypeStr "boolean" - ZeroCross on - } - Block { - BlockType Reference - Name "Compare\nTo Zero1" - SID "2917" - Ports [1, 1] - Position [-280, 2580, -250, 2610] - ZOrder 614 - ShowName off - LibraryVersion "1.441" - SourceBlock "simulink/Logic and Bit\nOperations/Compare\nTo Zero" - SourceType "Compare To Zero" - SourceProductName "Simulink" - SourceProductBaseCode "SL" - ContentPreviewEnabled off - relop "~=" - OutDataTypeStr "boolean" - ZeroCross on - } - Block { - BlockType Constant - Name "Constant" - SID "2096" - Position [-630, 2854, -520, 2876] - ZOrder 501 - ShowName off - Priority "-10" - Value "Sm.SM_TYPE_BIN" - OutDataTypeStr "uint8" - } - Block { - BlockType Constant - Name "Constant2" - SID "3692" - Position [80, 2784, 190, 2806] - ZOrder 699 - ShowName off - Priority "-10" - Value "Sm.SM_TYPE_BIN" - OutDataTypeStr "uint8" - } - Block { - BlockType Reference - Name "Coordinator" - SID "2908" - Ports [1, 1] - Position [-440, 2581, -380, 2609] - ZOrder 605 - ShowName off - LibraryVersion "1.441" - SourceBlock "simulink/Logic and Bit\nOperations/Bitwise\nOperator" - SourceType "Bitwise Operator" - SourceProductName "Simulink" - SourceProductBaseCode "SL" - MultiThreadCoSim "auto" - logicop "AND" - UseBitMask on - NumInputPorts "1" - BitMask "Sm.SM_MASK_COORDINATOR" - BitMaskRealWorld "Stored Integer" - } - Block { - BlockType Reference - Name "Get Measurement" - SID "4538" - Ports [0, 1] - Position [-460, 2706, -385, 2734] - ZOrder 868 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Position" - } - Block { - BlockType Goto - Name "Goto2" - SID "4496" - Position [670, 3081, 750, 3099] - ZOrder 863 - GotoTag "w_H_b" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto3" - SID "4497" - Position [670, 3041, 750, 3059] - ZOrder 864 - GotoTag "state" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto4" - SID "4498" - Position [670, 3006, 750, 3024] - ZOrder 865 - GotoTag "qj_des" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto7" - SID "4501" - Position [670, 3121, 750, 3139] - ZOrder 866 - GotoTag "CoM_des" - TagVisibility "global" - } - Block { - BlockType SubSystem - Name "Internal Coordinator" - SID "2098" - Ports [1, 11, 1] - Position [-210, 2617, -30, 2823] - ZOrder 505 - RequestExecContextInheritance off - System { - Name "Internal Coordinator" - 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 "304" - Block { - BlockType Inport - Name "jointAngles" - SID "3165" - Position [-665, 283, -635, 297] - ZOrder 676 - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "2099" - Ports [] - Position [-243, 340, -223, 360] - ZOrder 538 - } - Block { - BlockType SubSystem - Name "Base to fixed_link" - SID "4073" - Ports [2, 1] - Position [-480, 281, -350, 319] - ZOrder 886 - RequestExecContextInheritance off - System { - Name "Base to fixed_link" - 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 "396" - Block { - BlockType Inport - Name "qj" - SID "4078" - Position [380, -57, 410, -43] - ZOrder 890 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4079" - Position [915, -142, 945, -128] - ZOrder 891 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant1" - SID "4085" - Position [690, -43, 880, -27] - ZOrder 897 - ShowName off - Value "Config.LEFT_FOOT_IN_CONTACT_AT_0" - } - Block { - BlockType Constant - Name "Constant7" - SID "4072" - Position [380, 50, 410, 80] - ZOrder 885 - ShowName off - Value "eye(4)" - } - Block { - BlockType Reference - Name "LFoot to world transform (fixed base)" - SID "4071" - Ports [2, 1] - Position [490, -73, 655, -42] - ZOrder 725 - NamePlacement "alternate" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType Reference - Name "RFoot to world transform (fixed base)" - SID "4084" - Ports [2, 1] - Position [485, -33, 655, 3] - ZOrder 896 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Switch - Name "Switch" - SID "4083" - Position [905, -68, 945, -2] - ZOrder 895 - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType SubSystem - Name "fixedFoot to base link transform " - SID "4047" - Ports [4, 1] - Position [1000, -287, 1195, 117] - ZOrder 718 - RequestExecContextInheritance off - System { - Name "fixedFoot to base 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 "248" - Block { - BlockType Inport - Name "qj" - SID "4048" - Position [470, 227, 495, 243] - ZOrder 736 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4049" - Position [1000, 212, 1030, 228] - ZOrder 741 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_link" - SID "4050" - Position [655, 132, 680, 148] - ZOrder 744 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_base_fixed" - SID "4051" - Position [470, 32, 495, 48] - ZOrder 745 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "Fixed base to imu transform" - SID "4052" - Ports [2, 1] - Position [575, 25, 740, 85] - ZOrder 729 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.IMU" - } - Block { - BlockType Reference - Name "Fixed base to root link transform" - SID "4053" - Ports [2, 1] - Position [575, 190, 740, 250] - ZOrder 737 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.BASE" - } - Block { - BlockType Product - Name "Inv\\*" - SID "4054" - Ports [2, 1] - Position [825, 40, 895, 95] - ZOrder 731 - 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\\* " - SID "4055" - Ports [2, 1] - Position [825, 184, 895, 231] - ZOrder 738 - 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 "4056" - Ports [0, 1] - Position [980, 308, 1055, 332] - ZOrder 742 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.NECK_POS" - signalSize "Ports.NECK_POS_PORT_SIZE" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - Block { - BlockType Switch - Name "Switch6" - SID "4057" - Position [1760, 151, 1840, 419] - ZOrder 734 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE" - SID "4058" - Position [1570, 275, 1725, 295] - ZOrder 735 - ShowName off - Value "Config.USE_IMU4EST_BASE" - } - Block { - BlockType SubSystem - Name "fromImuToHomogeousTransformFCN" - SID "4059" - Ports [6, 1] - Position [1305, 47, 1540, 343] - ZOrder 733 - ShowName off - LibraryVersion "1.30" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "fromImuToHomogeousTransformFCN" - 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 "3785" - SIDPrevWatermark "9" - Block { - BlockType Inport - Name "imu_H_link" - SID "4059::112" - Position [20, 101, 40, 119] - ZOrder 82 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "imu_H_link_0" - SID "4059::113" - Position [20, 136, 40, 154] - ZOrder 83 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "link_H_base" - SID "4059::111" - Position [20, 171, 40, 189] - ZOrder 81 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial_0" - SID "4059::115" - Position [20, 206, 40, 224] - ZOrder 85 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4059::93" - Position [20, 246, 40, 264] - ZOrder 69 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "neck_pos" - SID "4059::118" - Position [20, 281, 40, 299] - ZOrder 86 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4059::3784" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 184 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4059::3783" - Tag "Stateflow S-Function torqueBalancingYoga 2" - Ports [6, 2] - Position [180, 102, 230, 243] - ZOrder 183 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[6 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "w_H_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4059::3785" - Position [460, 241, 480, 259] - ZOrder 185 - } - Block { - BlockType Outport - Name "w_H_b" - SID "4059::106" - Position [460, 101, 480, 119] - ZOrder 80 - IconDisplay "Port number" - } - Line { - ZOrder 181 - SrcBlock "imu_H_link" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 182 - SrcBlock "imu_H_link_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 183 - SrcBlock "link_H_base" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 184 - SrcBlock "inertial_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 185 - SrcBlock "inertial" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 186 - SrcBlock "neck_pos" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - Name "w_H_b" - ZOrder 187 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 188 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 189 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "holder\n1" - SID "4060" - Ports [1, 1] - Position [1175, 113, 1225, 127] - ZOrder 739 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "holder\n2" - SID "4061" - Ports [1, 1] - Position [1175, 213, 1225, 227] - ZOrder 740 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "neck transform" - SID "4062" - Ports [1, 1] - Position [1135, 305, 1260, 335] - ZOrder 743 - RequestExecContextInheritance off - System { - Name "neck transform" - Location [134, 55, 3840, 2160] - Open off - PortBlocksUseCompactNotation off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "843" - Block { - BlockType Inport - Name "neck port" - SID "4063" - Position [45, 18, 75, 32] - ZOrder 725 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant" - SID "4064" - Position [95, 85, 170, 105] - ZOrder 720 - ShowName off - Value "zeros(3,1)" - } - Block { - BlockType Gain - Name "Gain" - SID "4065" - 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 "4066" - 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 "4067" - Position [300, 7, 375, 113] - ZOrder 722 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE1" - SID "4068" - Position [60, 50, 210, 70] - ZOrder 719 - ShowName off - Value "Config.CORRECT_NECK_IMU" - } - Block { - BlockType Outport - Name "neck position" - SID "4069" - Position [425, 53, 455, 67] - ZOrder 726 - IconDisplay "Port number" - } - 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 "4070" - Position [1875, 278, 1905, 292] - ZOrder 732 - IconDisplay "Port number" - } - 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\\* " - SrcPort 1 - Points [65, 0] - Branch { - ZOrder 3 - Points [0, 165] - DstBlock "Switch6" - DstPort 3 - } - Branch { - ZOrder 4 - Points [0, -40] - DstBlock "fromImuToHomogeousTransformFCN" - 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 "fromImuToHomogeousTransformFCN" - DstPort 1 - } - Branch { - ZOrder 8 - Points [0, 50] - DstBlock "holder\n1" - DstPort 1 - } - } - Line { - ZOrder 9 - SrcBlock "holder\n2" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 4 - } - Line { - ZOrder 10 - SrcBlock "Fixed base to imu transform" - SrcPort 1 - DstBlock "Inv\\*" - DstPort 1 - } - Line { - ZOrder 11 - SrcBlock "holder\n1" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 2 - } - Line { - ZOrder 12 - SrcBlock "qj" - 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 "fromImuToHomogeousTransformFCN" - SrcPort 1 - DstBlock "Switch6" - DstPort 1 - } - Line { - ZOrder 16 - SrcBlock "Fixed base to root link transform" - SrcPort 1 - DstBlock "Inv\\* " - 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 "fromImuToHomogeousTransformFCN" - DstPort 5 - } - } - Line { - ZOrder 20 - SrcBlock "Neck Position" - SrcPort 1 - DstBlock "neck transform" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "neck transform" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 6 - } - Line { - ZOrder 22 - SrcBlock "w_H_link" - SrcPort 1 - Points [98, 0] - Branch { - ZOrder 23 - Points [0, 55] - DstBlock "Inv\\* " - 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 - } - } - } - } - Block { - BlockType Outport - Name "fixed_LFoot_H_b" - SID "4082" - Position [1245, -92, 1275, -78] - ZOrder 894 - IconDisplay "Port number" - } - Line { - ZOrder 182 - SrcBlock "fixedFoot to base link transform " - SrcPort 1 - DstBlock "fixed_LFoot_H_b" - DstPort 1 - } - Line { - ZOrder 185 - SrcBlock "inertial" - SrcPort 1 - DstBlock "fixedFoot to base link transform " - DstPort 2 - } - Line { - ZOrder 186 - SrcBlock "qj" - SrcPort 1 - Points [25, 0] - Branch { - ZOrder 195 - Points [0, -185] - DstBlock "fixedFoot to base link transform " - DstPort 1 - } - Branch { - ZOrder 202 - Points [0, 45] - DstBlock "RFoot to world transform (fixed base)" - DstPort 2 - } - Branch { - ZOrder 191 - DstBlock "LFoot to world transform (fixed base)" - DstPort 2 - } - } - Line { - ZOrder 187 - SrcBlock "Constant7" - SrcPort 1 - Points [42, 0] - Branch { - ZOrder 211 - Points [0, -90] - Branch { - ZOrder 217 - Points [0, -40] - DstBlock "LFoot to world transform (fixed base)" - DstPort 1 - } - Branch { - ZOrder 201 - DstBlock "RFoot to world transform (fixed base)" - DstPort 1 - } - } - Branch { - ZOrder 189 - DstBlock "fixedFoot to base link transform " - DstPort 4 - } - } - Line { - ZOrder 193 - SrcBlock "Switch" - SrcPort 1 - DstBlock "fixedFoot to base link transform " - DstPort 3 - } - Line { - ZOrder 196 - SrcBlock "LFoot to world transform (fixed base)" - SrcPort 1 - DstBlock "Switch" - DstPort 1 - } - Line { - ZOrder 197 - SrcBlock "RFoot to world transform (fixed base)" - SrcPort 1 - DstBlock "Switch" - DstPort 3 - } - Line { - ZOrder 204 - SrcBlock "Constant1" - SrcPort 1 - DstBlock "Switch" - DstPort 2 - } - } - } - Block { - BlockType Clock - Name "Clock1" - SID "2133" - Position [25, 460, 45, 480] - ZOrder 888 - ShowName off - } - Block { - BlockType SubSystem - Name "Compute State Velocity" - SID "4086" - Ports [3, 1] - Position [165, 239, 315, 311] - ZOrder 887 - 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 "519" - Block { - BlockType Inport - Name "qj" - SID "4087" - Position [-10, -187, 20, -173] - ZOrder 562 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_b" - SID "4088" - Position [-10, -167, 20, -153] - ZOrder 561 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "feetInContact\n" - SID "4089" - Position [235, -147, 265, -133] - ZOrder 244 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "Compute Base Velocity" - SID "4090" - Ports [4, 1] - Position [340, -194, 605, -106] - 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 "3794" - Block { - BlockType Inport - Name "J_LeftFoot" - SID "4090::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "J_RightFoot" - SID "4090::1472" - Position [20, 136, 40, 154] - ZOrder 42 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "feetInContact" - SID "4090::1473" - Position [20, 171, 40, 189] - ZOrder 43 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qjDot" - SID "4090::23" - Position [20, 206, 40, 224] - ZOrder 9 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4090::3793" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 162 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4090::3792" - Tag "Stateflow S-Function torqueBalancingYoga 5" - Ports [4, 2] - Position [180, 102, 230, 203] - ZOrder 161 - FunctionName "sf_sfun" - Parameters "Reg" - PortCounts "[4 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "nu_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4090::3794" - Position [460, 241, 480, 259] - ZOrder 163 - } - Block { - BlockType Outport - Name "nu_b" - SID "4090::5" - Position [460, 101, 480, 119] - ZOrder -6 - IconDisplay "Port number" - } - Line { - ZOrder 141 - SrcBlock "J_LeftFoot" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 142 - SrcBlock "J_RightFoot" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 143 - SrcBlock "feetInContact" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 144 - SrcBlock "qjDot" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - Name "nu_b" - ZOrder 145 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "nu_b" - DstPort 1 - } - Line { - ZOrder 146 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 147 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Feet Jacobians" - SID "4091" - Ports [2, 2] - Position [55, -191, 205, -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 "942" - Block { - BlockType Inport - Name "qj" - SID "4092" - Position [585, 598, 615, 612] - ZOrder 673 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_b" - SID "4093" - Position [585, 483, 615, 497] - ZOrder 680 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "Jacobian LFoot" - SID "4094" - Ports [2, 1] - Position [745, 475, 905, 530] - ZOrder 687 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType Reference - Name "Jacobian RFoot" - SID "4095" - Ports [2, 1] - Position [745, 565, 905, 620] - ZOrder 688 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Outport - Name "J_LFoot" - SID "4096" - Position [935, 498, 965, 512] - ZOrder 676 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_RFoot" - SID "4097" - Position [935, 588, 965, 602] - ZOrder 677 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "Jacobian LFoot" - SrcPort 1 - DstBlock "J_LFoot" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "Jacobian RFoot" - SrcPort 1 - DstBlock "J_RFoot" - DstPort 1 - } - Line { - ZOrder 3 - SrcBlock "w_H_b" - SrcPort 1 - Points [56, 0] - Branch { - ZOrder 4 - Points [0, 90] - DstBlock "Jacobian RFoot" - DstPort 1 - } - Branch { - ZOrder 5 - DstBlock "Jacobian LFoot" - DstPort 1 - } - } - Line { - ZOrder 6 - SrcBlock "qj" - SrcPort 1 - Points [82, 0] - Branch { - ZOrder 7 - Points [0, -90] - DstBlock "Jacobian LFoot" - DstPort 2 - } - Branch { - ZOrder 8 - DstBlock "Jacobian RFoot" - DstPort 2 - } - } - } - } - Block { - BlockType Reference - Name "Get Measurement" - SID "4540" - Ports [0, 1] - Position [215, -99, 290, -71] - ZOrder 869 - BackgroundColor "[0.513700, 0.674500, 1.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Velocity" - } - Block { - BlockType Mux - Name "Mux" - SID "4099" - Ports [2, 1] - Position [635, -182, 640, -53] - ZOrder -12 - ShowName off - Inputs "2" - DisplayOption "bar" - } - Block { - BlockType Outport - Name "nu" - SID "4100" - Position [660, -122, 690, -108] - ZOrder -15 - IconDisplay "Port number" - } - Line { - ZOrder 11 - SrcBlock "Get Measurement" - SrcPort 1 - Points [18, 0] - Branch { - ZOrder 2 - 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 "feetInContact\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 "qj" - 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 Constant - Name "Constant" - SID "2100" - Position [155, 492, 300, 508] - ZOrder 543 - ShowName off - Value "Config.DEMO_MOVEMENTS" - } - Block { - BlockType Constant - Name "Constant1" - SID "2101" - Position [-105, 290, 95, 310] - ZOrder 464 - ShowName off - Value "Config.LEFT_RIGHT_FOOT_IN_CONTACT" - } - Block { - BlockType Constant - Name "Constant2" - SID "2102" - Position [-65, 542, 0, 558] - ZOrder 548 - ShowName off - Value "zeros(6,1)" - } - Block { - BlockType Constant - Name "Constant3" - SID "2103" - Position [-570, 357, -445, 373] - ZOrder 550 - ShowName off - Value "Gain.impedances(1,:)" - } - Block { - BlockType Constant - Name "Constant4" - SID "2104" - Position [-525, 485, -495, 505] - ZOrder 553 - ShowName off - } - Block { - BlockType Constant - Name "Constant5" - SID "3321" - Position [-570, 397, -450, 413] - ZOrder 694 - ShowName off - Value "diag(Gain.KP_COM)" - } - Block { - BlockType Constant - Name "Constant6" - SID "3322" - Position [-570, 442, -450, 458] - ZOrder 695 - ShowName off - Value "diag(Gain.KD_COM)" - } - Block { - BlockType Reference - Name "IMU measurements" - SID "3222" - Ports [0, 1] - Position [-550, 303, -510, 317] - ZOrder 679 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.IMU" - signalSize "12" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on - } - Block { - BlockType Mux - Name "Mux" - SID "2106" - Ports [2, 1] - Position [30, 521, 35, 559] - ZOrder 547 - ShowName off - Inputs "2" - DisplayOption "bar" - } - Block { - BlockType SubSystem - Name "Reference Generator CoM" - SID "2134" - Ports [2, 1] - Position [95, 434, 365, 481] - ZOrder 889 - NamePlacement "alternate" - LibraryVersion "1.32" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "Reference Generator CoM" - Location [53, 45, 896, 1715] - 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 "3770" - Block { - BlockType Inport - Name "pos_CoM_0" - SID "2134::778" - Position [20, 101, 40, 119] - ZOrder 88 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "t" - SID "2134::768" - Position [20, 136, 40, 154] - ZOrder 78 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "2134::3769" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 172 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "2134::3768" - Tag "Stateflow S-Function torqueBalancingYoga 10" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 171 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "references_CoM" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "2134::3770" - Position [460, 241, 480, 259] - ZOrder 173 - } - Block { - BlockType Outport - Name "references_CoM" - SID "2134::45" - Position [460, 101, 480, 119] - ZOrder 31 - IconDisplay "Port number" - } - Line { - ZOrder 101 - SrcBlock "pos_CoM_0" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 102 - SrcBlock "t" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "references_CoM" - ZOrder 103 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "references_CoM" - DstPort 1 - } - Line { - ZOrder 104 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 105 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reshape - Name "Reshape1" - SID "4045" - Ports [1, 1] - Position [-255, 289, -210, 311] - ZOrder 716 - ShowName off - OutputDimensions "[4;4]" - } - Block { - BlockType Switch - Name "Switch1" - SID "2107" - Position [415, 442, 455, 558] - ZOrder 546 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Reference - Name "holder\n" - SID "2109" - Ports [1, 1] - Position [-150, 594, -95, 616] - ZOrder 577 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "holder\n1" - SID "2110" - Ports [1, 1] - Position [-150, 519, -95, 541] - ZOrder 579 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Constant - Name "joints.smoothingTime" - SID "3176" - Position [150, 556, 305, 574] - ZOrder 677 - ShowName off - Value "Sm.smoothingTimeCoM_Joints(1)" - } - Block { - BlockType SubSystem - Name "xCom" - SID "2137" - Ports [2, 1] - Position [-280, 500, -180, 560] - ZOrder -30 - ShowName off - RequestExecContextInheritance off - System { - Name "xCom" - 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 "942" - Block { - BlockType Inport - Name "w_H_b" - SID "2138" - Position [30, 237, 60, 253] - ZOrder 581 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "2139" - Position [30, 267, 60, 283] - ZOrder 582 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "CoM" - SID "2141" - Ports [2, 1] - Position [110, 230, 275, 290] - ZOrder 578 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.COM" - } - Block { - BlockType Selector - Name "CoM6D -> \nCoMXYZ2" - SID "2140" - Ports [1, 1] - Position [310, 241, 350, 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 "xCoM" - SID "2142" - Position [385, 253, 415, 267] - ZOrder 583 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "qj" - SrcPort 1 - DstBlock "CoM" - DstPort 2 - } - Line { - ZOrder 2 - SrcBlock "CoM6D -> \nCoMXYZ2" - SrcPort 1 - DstBlock "xCoM" - 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 "CoMDes" - SID "2143" - Position [485, 493, 515, 507] - ZOrder 478 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qjDes" - SID "2144" - Position [-45, 598, -15, 612] - ZOrder 479 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "2145" - Position [225, 342, 255, 358] - ZOrder 480 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "impedances" - SID "2146" - Position [-395, 357, -365, 373] - ZOrder 549 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "state" - SID "2147" - Position [-395, 487, -365, 503] - ZOrder 554 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "nu_b + qjDot" - SID "4046" - Position [400, 267, 430, 283] - ZOrder 717 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qj" - SID "2149" - Position [-245, 637, -215, 653] - ZOrder 576 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "smoothingTimeCoM_Joints" - SID "3177" - Position [355, 558, 385, 572] - ZOrder 678 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "KpCoM" - SID "3319" - Position [-395, 397, -365, 413] - ZOrder 692 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "KdCoM" - SID "3320" - Position [-395, 442, -365, 458] - ZOrder 693 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_b" - SID "2150" - Position [-175, 292, -145, 308] - ZOrder 575 - Port "11" - IconDisplay "Port number" - } - Line { - ZOrder 7 - SrcBlock "Constant" - SrcPort 1 - DstBlock "Switch1" - DstPort 2 - } - Line { - ZOrder 207 - SrcBlock "holder\n" - SrcPort 1 - DstBlock "qjDes" - DstPort 1 - } - Line { - ZOrder 13 - SrcBlock "Switch1" - SrcPort 1 - DstBlock "CoMDes" - DstPort 1 - } - Line { - ZOrder 131 - SrcBlock "Mux" - SrcPort 1 - DstBlock "Switch1" - DstPort 3 - } - Line { - ZOrder 17 - SrcBlock "Constant2" - SrcPort 1 - DstBlock "Mux" - DstPort 2 - } - Line { - ZOrder 18 - SrcBlock "Constant3" - SrcPort 1 - DstBlock "impedances" - DstPort 1 - } - Line { - ZOrder 19 - SrcBlock "Constant4" - SrcPort 1 - DstBlock "state" - DstPort 1 - } - Line { - ZOrder 191 - SrcBlock "Compute State Velocity" - SrcPort 1 - DstBlock "nu_b + qjDot" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "holder\n1" - SrcPort 1 - Points [79, 0] - Branch { - ZOrder 212 - Points [0, -85] - DstBlock "Reference Generator CoM" - DstPort 1 - } - Branch { - ZOrder 209 - DstBlock "Mux" - DstPort 1 - } - } - Line { - ZOrder 182 - SrcBlock "Base to fixed_link" - SrcPort 1 - Points [42, 0] - Branch { - ZOrder 188 - Points [0, -25] - DstBlock "Compute State Velocity" - DstPort 2 - } - Branch { - ZOrder 96 - Points [0, 215] - DstBlock "xCom" - DstPort 1 - } - Branch { - ZOrder 64 - DstBlock "Reshape1" - DstPort 1 - } - } - Line { - ZOrder 29 - SrcBlock "xCom" - SrcPort 1 - DstBlock "holder\n1" - DstPort 1 - } - Line { - ZOrder 30 - SrcBlock "joints.smoothingTime" - SrcPort 1 - DstBlock "smoothingTimeCoM_Joints" - DstPort 1 - } - Line { - ZOrder 31 - SrcBlock "jointAngles" - SrcPort 1 - Points [38, 0] - Branch { - ZOrder 189 - Points [0, -40] - DstBlock "Compute State Velocity" - DstPort 1 - } - Branch { - ZOrder 181 - DstBlock "Base to fixed_link" - DstPort 1 - } - Branch { - ZOrder 165 - Points [0, 255; 290, 0] - Branch { - ZOrder 109 - Points [0, 60] - Branch { - ZOrder 185 - Points [0, 40] - DstBlock "qj" - DstPort 1 - } - Branch { - ZOrder 37 - DstBlock "holder\n" - DstPort 1 - } - } - Branch { - ZOrder 36 - DstBlock "xCom" - DstPort 2 - } - } - } - Line { - ZOrder 41 - SrcBlock "Constant5" - SrcPort 1 - DstBlock "KpCoM" - DstPort 1 - } - Line { - ZOrder 42 - SrcBlock "Constant6" - SrcPort 1 - DstBlock "KdCoM" - DstPort 1 - } - Line { - ZOrder 66 - SrcBlock "Reshape1" - SrcPort 1 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 69 - SrcBlock "Constant1" - SrcPort 1 - Points [14, 0] - Branch { - ZOrder 190 - DstBlock "Compute State Velocity" - DstPort 3 - } - Branch { - ZOrder 95 - Points [0, 50] - DstBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - DstPort 1 - } - } - Line { - ZOrder 178 - SrcBlock "IMU measurements" - SrcPort 1 - DstBlock "Base to fixed_link" - DstPort 2 - } - Line { - ZOrder 204 - SrcBlock "Clock1" - SrcPort 1 - DstBlock "Reference Generator CoM" - DstPort 2 - } - Line { - ZOrder 208 - SrcBlock "Reference Generator CoM" - SrcPort 1 - DstBlock "Switch1" - DstPort 1 - } - } - } - Block { - BlockType MultiPortSwitch - Name "Multiport\nSwitch1" - SID "2913" - Ports [3, 1] - Position [230, 2778, 290, 2882] - ZOrder 610 - ShowName off - DataPortOrder "Specify indices" - DataPortIndices "{1, 2}" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Mux - Name "Mux1" - SID "2155" - Ports [11, 1] - Position [10, 2881, 15, 3099] - ZOrder 508 - ShowName off - Inputs "11" - DisplayOption "bar" - } - Block { - BlockType Mux - Name "Mux2" - SID "2156" - Ports [11, 1] - Position [10, 2613, 15, 2827] - ZOrder 509 - ShowName off - Inputs "11" - DisplayOption "bar" - } - Block { - BlockType SubSystem - Name "Select State and References" - SID "4030" - Ports [1, 10] - Position [320, 2689, 515, 2971] - ZOrder 702 - RequestExecContextInheritance off - System { - Name "Select 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 "246" - Block { - BlockType Inport - Name "StateAndReferences" - SID "4033" - Position [15, 288, 45, 302] - ZOrder 704 - IconDisplay "Port number" - } - Block { - BlockType Demux - Name "Demux" - SID "3687" - Ports [1, 11] - Position [100, 9, 105, 581] - ZOrder 697 - ShowName off - Outputs "[9,ROBOT_DOF,2,ROBOT_DOF,1,ROBOT_DOF+6,ROBOT_DOF,1,3,3,16]" - DisplayOption "bar" - Port { - PortNumber 1 - Name "CoMDes" - } - Port { - PortNumber 2 - Name "qDes" - } - Port { - PortNumber 5 - Name "state" - } - Port { - PortNumber 6 - Name "nu" - } - Port { - PortNumber 7 - Name "qj" - } - Port { - PortNumber 8 - Name "smoothingTimeMinJerkComDesQDes" - } - } - Block { - BlockType Goto - Name "Goto \"Compute joint torques\"" - SID "4653" - Position [800, 160, 885, 170] - ZOrder 900 - GotoTag "qjDDot_des" - TagVisibility "global" - } - Block { - BlockType Logic - Name "Logical\nOperator1" - SID "4101" - Ports [2, 1] - Position [460, -143, 490, -112] - ZOrder 722 - ShowName off - Operator "OR" - AllPortsSameDT off - OutDataTypeStr "boolean" - } - Block { - BlockType Reference - Name "Minimum Jerk Trajectory Generator1" - SID "2152" - Ports [2, 3] - Position [595, 130, 750, 170] - ZOrder 598 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - SourceType "Minimum Jerk Trajectory Generator" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - externalSettlingTime on - settlingTime "0.01" - sampleTime "Config.Ts" - resetOnSettlingTime off - firstDerivatives on - secondDerivatives on - explicitInitialValue off - } - Block { - BlockType Reference - Name "Minimum Jerk Trajectory Generator2" - SID "2153" - Ports [2, 3] - Position [605, 35, 745, 75] - ZOrder 597 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - SourceType "Minimum Jerk Trajectory Generator" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - externalSettlingTime on - settlingTime "0.01" - sampleTime "Config.Ts" - resetOnSettlingTime off - firstDerivatives on - secondDerivatives on - explicitInitialValue off - } - Block { - BlockType Mux - Name "Mux" - SID "2154" - Ports [3, 1] - Position [800, 31, 805, 79] - ZOrder 486 - ShowName off - Inputs "3" - DisplayOption "bar" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n3" - SID "4102" - Position [295, -141, 400, -129] - ZOrder 721 - ShowName off - Value "Config.SCOPES_ALL" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n4" - SID "4103" - Position [255, -126, 435, -114] - ZOrder 723 - ShowName off - Value "Config.SCOPES_GAIN_SCHE_INFO" - } - Block { - BlockType Reshape - Name "Reshape" - SID "2159" - Ports [1, 1] - Position [1105, 55, 1150, 95] - ZOrder 456 - ShowName off - OutputDimensionality "Customize" - OutputDimensions "[3,3]" - } - Block { - BlockType Reshape - Name "Reshape1" - SID "4044" - Ports [1, 1] - Position [170, 525, 215, 565] - ZOrder 715 - ShowName off - OutputDimensionality "Customize" - OutputDimensions "[4;4]" - } - Block { - BlockType Constant - Name "SMOOTH_DES_COM" - SID "2160" - Position [825, 65, 965, 85] - ZOrder 488 - ShowName off - Value "Config.SMOOTH_COM_DES" - } - Block { - BlockType Constant - Name "SMOOTH_DES_COM2" - SID "2161" - Position [920, 155, 1060, 175] - ZOrder 492 - ShowName off - Value "Config.SMOOTH_JOINT_DES" - } - Block { - BlockType Selector - Name "Selector" - SID "2162" - Ports [1, 1] - Position [520, 36, 555, 54] - ZOrder 481 - ShowName off - InputPortWidth "9" - IndexOptions "Index vector (dialog)" - Indices "[1 2 3]" - OutputSizes "1" - } - Block { - BlockType Switch - Name "Switch3" - SID "2246" - Position [1000, 45, 1050, 105] - ZOrder 487 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch5" - SID "2248" - Position [1100, 122, 1155, 208] - ZOrder 491 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Terminator - Name "Terminator" - SID "4656" - Position [765, 144, 780, 156] - ZOrder 901 - ShowName off - HideAutomaticName off - } - Block { - BlockType SubSystem - Name "Visualize Gain Tuning " - SID "4104" - Ports [5, 0, 1] - Position [380, -87, 690, -3] - ZOrder 720 - RequestExecContextInheritance off - System { - Name "Visualize Gain Tuning " - 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 "305" - Block { - BlockType Inport - Name "smoothingTimeComAndJoints" - SID "4105" - Position [185, 144, 215, 156] - ZOrder 645 - IconDisplay "Port number" - Port { - PortNumber 1 - Name "smoothingTimeComAndJoints" - PropagatedSignals "smoothingTimeMinJerkComDesQDes" - } - } - Block { - BlockType Inport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "4106" - Position [185, 79, 215, 91] - ZOrder 646 - Port "2" - IconDisplay "Port number" - Port { - PortNumber 1 - Name "Left right foot in contact" - } - } - Block { - BlockType Inport - Name "impedances" - SID "4107" - Position [365, 287, 395, 303] - ZOrder 647 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4108" - Position [25, 179, 55, 191] - ZOrder 648 - Port "4" - IconDisplay "Port number" - Port { - PortNumber 1 - Name "state" - PropagatedSignals "state" - } - } - Block { - BlockType Inport - Name "nu" - SID "4109" - Position [-120, 557, -90, 573] - ZOrder 652 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "4110" - Ports [] - Position [27, 335, 47, 355] - ZOrder 649 - } - Block { - BlockType Demux - Name "Demux" - SID "4111" - Ports [1, 2] - Position [210, 406, 215, 444] - ZOrder 655 - ShowName off - Outputs "2" - DisplayOption "bar" - Port { - PortNumber 1 - Name "vb" - } - Port { - PortNumber 2 - Name "wb" - } - } - Block { - BlockType Demux - Name "Demux1" - SID "4569" - Ports [1, 5] - Position [585, 246, 590, 344] - 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 Demux - Name "Demux2" - SID "4570" - Ports [1, 5] - Position [585, 516, 590, 614] - ZOrder 662 - 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 Gain - Name "Gain" - SID "4114" - Position [355, 552, 405, 578] - ZOrder 657 - 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 "4115" - Position [360, 421, 405, 449] - ZOrder 658 - ShowName off - Gain "180/pi" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Scope - Name "Impedances(t)" - SID "4116" - Ports [6] - Position [710, 247, 795, 363] - ZOrder 639 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','ScopeData3','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','8.75','Max" - "YLimReal','21.25','YLabelReal','','MinYLimMag','8.75','MaxYLimMag','21.25','LegendVisibility','on','XGrid',true,'Y" - "Grid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862745" - "09803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.160784" - "31372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294" - "117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color'" - ",[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','L" - "ineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','non" - "e','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" - "Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle'," - "'-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]" - "}},'ShowContent',true,'Placement',1),struct('MinYLimReal','7.75','MaxYLimReal','10.25','YLabelReal','','MinYLimMag" - "','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0]" - ",'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.07" - "45098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745" - "098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]," - "'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','" - "none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyl" - "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" - "}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinYLi" - "mReal','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.6862745098039" - "22 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882" - "353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;" - "0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{st" - "ruct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineS" - "tyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'" - "Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1]," - "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'Lin" - "eNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','3.75','MaxYLimReal','66.25','YLabelReal',''" - ",'MinYLimMag','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.066666666" - "6666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.8313725490" - "19608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098" - "0392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0." - "5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'" - ",'on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 " - "0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWid" - "th',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Vi" - "sible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',4),st" - "ruct('MinYLimReal','-1.00000','MaxYLimReal','1.00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" - "sibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098039" - "22 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 " - "1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0." - "274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePro" - "pertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" - "or',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-'" - ",'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','" - "none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}}" - ",'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',5),struct('MinYLimReal','1.00000','MaxYLimReal','3" - ".00000','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'Plot" - "MagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Color" - "Order',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.39215" - "6862745098 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],'LineSt" - "yle','-','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','-','LineWidth'," - "0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent'" - ",true,'Placement',6)},'DisplayPropertyDefaults',struct('YLabelReal','','LegendVisibility','on','XGrid',true,'YGrid" - "',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980" - "3922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1607843137" - "2549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352941176" - "47 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 " - "1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'" - "ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Navigation',t" - "rue,'OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Lo" - "cation',[76 116 1387 805])" - NumInputPorts "6" - Floating off - } - Block { - BlockType Scope - Name "Left right foot in contact" - SID "4117" - Ports [1] - Position [735, 69, 765, 101] - ZOrder 644 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','ScopeData1','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-0.09603'," - "'MaxYLimReal','1.127','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGr" - "id',true,'PlotMagPhase',false,'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.16078431" - "372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411" - "7647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}}" - ",'ShowContent',true,'Placement',1)},'DisplayPropertyDefaults',struct('MinYLimReal','-0.09603','MaxYLimReal','1.127" - "','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPha" - "se',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.39215686274" - "5098 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,'Marker'" - ",'none','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],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true," - "'Placement',1)),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tool" - "s','Measurements',true,'Version','2017b')),'Version','2018a','Location',[1000 538 1560 958])" - NumInputPorts "1" - Floating off - } - Block { - BlockType Selector - Name "Selector1" - SID "4118" - Ports [1, 1] - Position [5, 553, 65, 577] - ZOrder 654 - 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 [5, 413, 65, 437] - ZOrder 653 - ShowName off - InputPortWidth "6+ROBOT_DOF" - IndexOptions "Index vector (dialog)" - Indices "[1:6]" - OutputSizes "1" - } - Block { - BlockType Scope - Name "jointsSmoothingTime" - SID "4120" - Ports [2] - Position [725, 133, 775, 202] - ZOrder 643 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','ScopeData4','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-0.375','M" - "axYLimReal','13.375','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',true,'YGr" - "id',true,'PlotMagPhase',false,'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.16078431" - "372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411" - "7647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}}" - ",'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.375','MaxYLimReal','13.375','YLabelReal','','MinYLimMa" - "g','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.07" - "45098039215686;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'),str" - "uct('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,'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',2)},'DisplayPro" - "pertyDefaults',struct('MinYLimReal','-0.375','MaxYLimReal','13.375','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.6" - "23529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717" - "647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','o" - "n'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1]," - "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth'" - ",0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" - "le','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedCha" - "nnelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[2 1])" - ",extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tools','Measurement" - "s',true,'Version','2017b')),'Version','2018a','Location',[179 459 1300 1439])" - NumInputPorts "2" - Floating off - } - Block { - BlockType Scope - Name "linear and angular \nbase velocity" - SID "4121" - Ports [3] - Position [720, 402, 790, 468] - ZOrder 656 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," - "'DataLoggingVariableName','baseVelocityData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','" - "1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct(" - "'MinYLimReal','-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibil" - "ity','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0." - "686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0" - ".411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450" - "9803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProperti" - "esCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[" - "1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'Num" - "Lines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.04416','MaxYLimReal','0.050" - "54','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagP" - "hase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrde" - "r',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862" - "745098 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],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" - "truct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',tru" - "e,'Placement',2),struct('MinYLimReal','-0.04416','MaxYLimReal','0.05054','YLabelReal','','MinYLimMag','0','MaxYLim" - "Mag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColo" - "r',[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'),struct" - "('Color',[1 0 1],'LineStyle','-','LineWidth',0.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,'Mark" - "er','none','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)},'DisplayPropertyDefaults',struct('MinYLim" - "Real','-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.68627450" - "9803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647" - "05882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039215" - "69 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache'" - ",{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'" - "LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth'," - "0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visibl" - "e','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 " - "0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0" - ",'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[3 1]),extmgr.Configuration('Tools" - "','Plot Navigation',true,'OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2017b'))," - "'Version','2018a','Location',[76 459 1996 1439])" - NumInputPorts "3" - Floating off - } - Block { - BlockType Scope - Name "qjDot(t) " - SID "4122" - Ports [6] - Position [715, 513, 800, 637] - ZOrder 650 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," - "'DataLoggingVariableName','jointVelocitiesData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation" - "','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{stru" - "ct('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVi" - "sibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098039" - "22 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 " - "1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0." - "274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePro" - "pertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Col" - "or',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-'" - ",'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','" - "none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}}" - ",'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-20.50153','MaxYLimReal'," - "'20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'P" - "lotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Co" - "lorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.39" - "2156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 " - "0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5" - ",'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidt" - "h',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowConte" - "nt',true,'Placement',2),struct('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0'" - ",'MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'Axe" - "sTickColor',[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.074509803" - "9215686;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','none'" - ",'Visible','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,'Marker'," - "'none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'U" - "serDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal" - "','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on'," - "'XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862745098" - "03922 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.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],'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','Visible'" - ",'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 " - "1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'" - "LineNames',{{[]}},'ShowContent',true,'Placement',4),struct('MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLa" - "belReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',fa" - "lse,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 " - "0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0" - ".831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215" - "686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','Li" - "neWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none" - "','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('C" - "olor',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','" - "-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker'" - ",'none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Place" - "ment',5),struct('MinYLimReal','-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.623" - "529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764" - "7058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'L" - "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0" - ".5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChann" - "elNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',6)},'DisplayPropertyDefaults',struct(" - "'MinYLimReal','-20.50153','MaxYLimReal','20.29706','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','AxesColor'," - "[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.06666666666666" - "67;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.6509803921" - "56863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'L" - "ineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0" - ".5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRa" - "ngeSamples','60','TimeRangeFrames','60','DisplayLayoutDimensions',[6 1]),extmgr.Configuration('Tools','Plot Naviga" - "tion',true,'OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','201" - "8a','Location',[384 319 2304 1299])" - NumInputPorts "6" - Floating off - } - Line { - Name "state" - ZOrder 1 - SrcBlock "state" - SrcPort 1 - Points [65, 0] - Branch { - ZOrder 36 - Points [0, 170] - Branch { - ZOrder 56 - Points [0, 100] - Branch { - ZOrder 46 - Points [0, 170] - DstBlock "qjDot(t) " - DstPort 6 - } - Branch { - ZOrder 43 - DstBlock "linear and angular \nbase velocity" - DstPort 3 - } - } - Branch { - ZOrder 42 - DstBlock "Impedances(t)" - DstPort 6 - } - } - Branch { - ZOrder 35 - Labels [1, 1] - DstBlock "jointsSmoothingTime" - DstPort 2 - } - } - Line { - Name "Left right foot in contact" - ZOrder 8 - Labels [1, 1] - SrcBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - SrcPort 1 - DstBlock "Left right foot in contact" - DstPort 1 - } - Line { - ZOrder 109 - SrcBlock "impedances" - SrcPort 1 - DstBlock "Demux1" - DstPort 1 - } - Line { - Name "smoothingTimeComAndJoints" - ZOrder 10 - Labels [1, 1] - SrcBlock "smoothingTimeComAndJoints" - SrcPort 1 - DstBlock "jointsSmoothingTime" - DstPort 1 - } - Line { - Name "left_leg" - ZOrder 105 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 4 - DstBlock "Impedances(t)" - DstPort 4 - } - Line { - Name "right_leg" - ZOrder 104 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 5 - DstBlock "Impedances(t)" - DstPort 5 - } - Line { - Name "right_arm" - ZOrder 108 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 3 - DstBlock "Impedances(t)" - DstPort 3 - } - Line { - Name "torso" - ZOrder 107 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 1 - DstBlock "Impedances(t)" - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 106 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 2 - DstBlock "Impedances(t)" - DstPort 2 - } - Line { - ZOrder 16 - SrcBlock "Selector1" - SrcPort 1 - DstBlock "Gain" - DstPort 1 - } - Line { - Name "left_leg" - ZOrder 112 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 4 - DstBlock "qjDot(t) " - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 114 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 3 - DstBlock "qjDot(t) " - DstPort 3 - } - Line { - Name "torso" - ZOrder 111 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 1 - DstBlock "qjDot(t) " - DstPort 1 - } - Line { - Name "left_arm" - ZOrder 110 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 2 - DstBlock "qjDot(t) " - DstPort 2 - } - Line { - ZOrder 22 - SrcBlock "nu" - SrcPort 1 - Points [27, 0] - Branch { - ZOrder 50 - Points [0, -140] - DstBlock "Selector2" - DstPort 1 - } - Branch { - ZOrder 23 - DstBlock "Selector1" - DstPort 1 - } - } - Line { - ZOrder 25 - SrcBlock "Selector2" - SrcPort 1 - DstBlock "Demux" - DstPort 1 - } - Line { - Name "vb" - ZOrder 26 - Labels [-1, 0] - SrcBlock "Demux" - SrcPort 1 - DstBlock "linear and angular \nbase velocity" - DstPort 1 - } - Line { - Name "wb" - ZOrder 27 - Labels [1, 1] - SrcBlock "Demux" - SrcPort 2 - DstBlock "Gain1" - DstPort 1 - } - Line { - ZOrder 115 - SrcBlock "Gain" - SrcPort 1 - DstBlock "Demux2" - DstPort 1 - } - Line { - ZOrder 29 - SrcBlock "Gain1" - SrcPort 1 - DstBlock "linear and angular \nbase velocity" - DstPort 2 - } - Line { - Name "right_leg" - ZOrder 113 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 5 - DstBlock "qjDot(t) " - DstPort 5 - } - } - } - Block { - BlockType Outport - Name "impedances" - SID "4034" - Position [330, 188, 360, 202] - ZOrder 705 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "gainsPCOM" - SID "4035" - Position [330, 438, 360, 452] - ZOrder 706 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "gainsDCOM" - SID "4036" - Position [330, 488, 360, 502] - ZOrder 707 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_b" - SID "4037" - Position [330, 537, 360, 553] - ZOrder 708 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qj" - SID "4038" - Position [330, 337, 360, 353] - ZOrder 709 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "nu" - SID "4039" - Position [330, 287, 360, 303] - ZOrder 710 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "4040" - Position [330, 138, 360, 152] - ZOrder 711 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "desired_x_dx_ddx_CoM" - SID "4041" - Position [1240, 68, 1270, 82] - ZOrder 712 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qjDes" - SID "4042" - Position [1240, 158, 1270, 172] - ZOrder 713 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "state" - SID "4043" - Position [330, 238, 360, 252] - ZOrder 714 - Port "10" - IconDisplay "Port number" - } - Line { - ZOrder 405 - SrcBlock "Reshape" - SrcPort 1 - DstBlock "desired_x_dx_ddx_CoM" - DstPort 1 - } - Line { - ZOrder 8 - SrcBlock "Minimum Jerk Trajectory Generator2" - SrcPort 1 - DstBlock "Mux" - DstPort 1 - } - Line { - ZOrder 9 - SrcBlock "Minimum Jerk Trajectory Generator2" - SrcPort 2 - DstBlock "Mux" - DstPort 2 - } - Line { - ZOrder 11 - SrcBlock "Switch3" - SrcPort 1 - DstBlock "Reshape" - DstPort 1 - } - Line { - ZOrder 10 - SrcBlock "Minimum Jerk Trajectory Generator2" - SrcPort 3 - DstBlock "Mux" - DstPort 3 - } - Line { - ZOrder 12 - SrcBlock "SMOOTH_DES_COM" - SrcPort 1 - DstBlock "Switch3" - DstPort 2 - } - Line { - ZOrder 13 - SrcBlock "SMOOTH_DES_COM2" - SrcPort 1 - DstBlock "Switch5" - DstPort 2 - } - Line { - ZOrder 406 - SrcBlock "Switch5" - SrcPort 1 - DstBlock "qjDes" - DstPort 1 - } - Line { - Name "qj" - ZOrder 402 - Labels [0, 0] - SrcBlock "Demux" - SrcPort 7 - DstBlock "qj" - DstPort 1 - } - Line { - ZOrder 401 - SrcBlock "Demux" - SrcPort 11 - DstBlock "Reshape1" - DstPort 1 - } - Line { - Name "state" - ZOrder 407 - Labels [0, 0] - SrcBlock "Demux" - SrcPort 5 - Points [125, 0] - Branch { - ZOrder 453 - Points [0, -275] - DstBlock "Visualize Gain Tuning " - DstPort 4 - } - Branch { - ZOrder 452 - DstBlock "state" - DstPort 1 - } - } - Line { - Name "smoothingTimeMinJerkComDesQDes" - ZOrder 76 - Labels [0, 0] - SrcBlock "Demux" - SrcPort 8 - Points [86, 0] - Branch { - ZOrder 458 - Points [363, 0; 0, -235] - Branch { - ZOrder 558 - Points [0, -95] - DstBlock "Minimum Jerk Trajectory Generator2" - DstPort 2 - } - Branch { - ZOrder 497 - DstBlock "Minimum Jerk Trajectory Generator1" - DstPort 2 - } - } - Branch { - ZOrder 457 - Points [0, -470] - DstBlock "Visualize Gain Tuning " - DstPort 1 - } - } - Line { - ZOrder 83 - SrcBlock "Mux" - SrcPort 1 - DstBlock "Switch3" - DstPort 1 - } - Line { - ZOrder 84 - SrcBlock "Selector" - SrcPort 1 - DstBlock "Minimum Jerk Trajectory Generator2" - DstPort 1 - } - Line { - Name "qDes" - ZOrder 85 - Labels [0, 0] - SrcBlock "Demux" - SrcPort 2 - Points [324, 0; 0, 45] - Branch { - ZOrder 559 - Points [0, 55] - DstBlock "Switch5" - DstPort 3 - } - Branch { - ZOrder 412 - DstBlock "Minimum Jerk Trajectory Generator1" - DstPort 1 - } - } - Line { - ZOrder 90 - SrcBlock "Minimum Jerk Trajectory Generator1" - SrcPort 1 - DstBlock "Switch5" - DstPort 1 - } - Line { - Name "nu" - ZOrder 403 - Labels [0, 0] - SrcBlock "Demux" - SrcPort 6 - Points [138, 0] - Branch { - ZOrder 450 - Points [0, -310] - DstBlock "Visualize Gain Tuning " - DstPort 5 - } - Branch { - ZOrder 449 - DstBlock "nu" - DstPort 1 - } - } - Line { - ZOrder 397 - SrcBlock "StateAndReferences" - SrcPort 1 - DstBlock "Demux" - DstPort 1 - } - Line { - ZOrder 398 - SrcBlock "Demux" - SrcPort 4 - Points [111, 0] - Branch { - ZOrder 455 - Points [0, -240] - DstBlock "Visualize Gain Tuning " - DstPort 3 - } - Branch { - ZOrder 454 - DstBlock "impedances" - DstPort 1 - } - } - Line { - ZOrder 404 - SrcBlock "Demux" - SrcPort 3 - Points [98, 0] - Branch { - ZOrder 460 - Points [0, -205] - DstBlock "Visualize Gain Tuning " - DstPort 2 - } - Branch { - ZOrder 459 - DstBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - DstPort 1 - } - } - Line { - ZOrder 399 - SrcBlock "Demux" - SrcPort 9 - DstBlock "gainsPCOM" - DstPort 1 - } - Line { - ZOrder 400 - SrcBlock "Demux" - SrcPort 10 - DstBlock "gainsDCOM" - DstPort 1 - } - Line { - Name "CoMDes" - ZOrder 409 - Labels [0, 0] - SrcBlock "Demux" - SrcPort 1 - Points [376, 0] - Branch { - ZOrder 134 - Points [0, 50] - DstBlock "Switch3" - DstPort 3 - } - Branch { - ZOrder 133 - DstBlock "Selector" - DstPort 1 - } - } - Line { - ZOrder 415 - SrcBlock "Reshape1" - SrcPort 1 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 446 - SrcBlock "ON_GAZEBO\n4" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 2 - } - Line { - ZOrder 447 - SrcBlock "ON_GAZEBO\n3" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 1 - } - Line { - ZOrder 448 - SrcBlock "Logical\nOperator1" - SrcPort 1 - Points [40, 0] - DstBlock "Visualize Gain Tuning " - DstPort enable - } - Line { - ZOrder 561 - SrcBlock "Minimum Jerk Trajectory Generator1" - SrcPort 3 - DstBlock "Goto \"Compute joint torques\"" - DstPort 1 - } - Line { - ZOrder 562 - SrcBlock "Minimum Jerk Trajectory Generator1" - SrcPort 2 - DstBlock "Terminator" - DstPort 1 - } - } - } - Block { - BlockType Selector - Name "Selector" - SID "4502" - Ports [1, 1] - Position [590, 3116, 630, 3144] - ZOrder 867 - NumberOfDimensions "2" - InputPortWidth "3" - IndexOptions "Index vector (dialog),Index vector (dialog)" - Indices "[1 2 3],[1]" - OutputSizes "1,1" - } - Block { - BlockType SubSystem - Name "State Machine Yoga" - SID "2163" - Ports [1, 11, 1] - Position [-210, 2887, -30, 3093] - ZOrder 506 - Priority "-100" - RequestExecContextInheritance off - System { - Name "State Machine Yoga" - 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 "261" - Block { - BlockType Inport - Name "jointAngles" - SID "3166" - Position [-575, 263, -545, 277] - ZOrder 676 - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "2164" - Ports [] - Position [2, 40, 22, 60] - ZOrder 537 - ShowName off - } - Block { - BlockType Clock - Name "Clock1" - SID "2166" - Position [5, 360, 25, 380] - ZOrder 532 - ShowName off - } - Block { - BlockType SubSystem - Name "Compute State Velocity" - SID "4211" - Ports [3, 1] - Position [590, -76, 740, -4] - ZOrder 889 - 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 "519" - Block { - BlockType Inport - Name "qj" - SID "4212" - Position [-10, -187, 20, -173] - ZOrder 562 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_b" - SID "4213" - Position [-10, -167, 20, -153] - ZOrder 561 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "feetInContact\n" - SID "4214" - Position [235, -147, 265, -133] - ZOrder 244 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "Compute Base Velocity" - SID "4215" - Ports [4, 1] - Position [340, -194, 605, -106] - 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 "3794" - Block { - BlockType Inport - Name "J_LeftFoot" - SID "4215::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "J_RightFoot" - SID "4215::1472" - Position [20, 136, 40, 154] - ZOrder 42 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "feetInContact" - SID "4215::1473" - Position [20, 171, 40, 189] - ZOrder 43 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qjDot" - SID "4215::23" - Position [20, 206, 40, 224] - ZOrder 9 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4215::3793" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 162 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4215::3792" - Tag "Stateflow S-Function torqueBalancingYoga 7" - Ports [4, 2] - Position [180, 102, 230, 203] - ZOrder 161 - FunctionName "sf_sfun" - Parameters "Reg" - PortCounts "[4 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "nu_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4215::3794" - Position [460, 241, 480, 259] - ZOrder 163 - } - Block { - BlockType Outport - Name "nu_b" - SID "4215::5" - Position [460, 101, 480, 119] - ZOrder -6 - IconDisplay "Port number" - } - Line { - ZOrder 141 - SrcBlock "J_LeftFoot" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 142 - SrcBlock "J_RightFoot" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 143 - SrcBlock "feetInContact" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 144 - SrcBlock "qjDot" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - Name "nu_b" - ZOrder 145 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "nu_b" - DstPort 1 - } - Line { - ZOrder 146 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 147 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Feet Jacobians" - SID "4216" - Ports [2, 2] - Position [55, -191, 205, -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 "942" - Block { - BlockType Inport - Name "qj" - 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 LFoot" - SID "4219" - Ports [2, 1] - Position [745, 475, 905, 530] - ZOrder 687 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType Reference - Name "Jacobian RFoot" - SID "4220" - Ports [2, 1] - Position [745, 565, 905, 620] - ZOrder 688 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Outport - Name "J_LFoot" - SID "4221" - Position [935, 498, 965, 512] - ZOrder 676 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "J_RFoot" - SID "4222" - Position [935, 588, 965, 602] - ZOrder 677 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "Jacobian LFoot" - SrcPort 1 - DstBlock "J_LFoot" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "Jacobian RFoot" - SrcPort 1 - DstBlock "J_RFoot" - DstPort 1 - } - Line { - ZOrder 3 - SrcBlock "w_H_b" - SrcPort 1 - Points [56, 0] - Branch { - ZOrder 4 - Points [0, 90] - DstBlock "Jacobian RFoot" - DstPort 1 - } - Branch { - ZOrder 5 - DstBlock "Jacobian LFoot" - DstPort 1 - } - } - Line { - ZOrder 6 - SrcBlock "qj" - SrcPort 1 - Points [82, 0] - Branch { - ZOrder 7 - Points [0, -90] - DstBlock "Jacobian LFoot" - DstPort 2 - } - Branch { - ZOrder 8 - DstBlock "Jacobian RFoot" - DstPort 2 - } - } - } - } - Block { - BlockType Reference - Name "Get Measurement" - SID "4541" - Ports [0, 1] - Position [215, -99, 290, -71] - ZOrder 869 - BackgroundColor "[0.513700, 0.674500, 1.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Velocity" - } - Block { - BlockType Mux - Name "Mux" - SID "4224" - Ports [2, 1] - Position [635, -182, 640, -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" - } - Line { - ZOrder 11 - SrcBlock "Get Measurement" - SrcPort 1 - Points [18, 0] - Branch { - ZOrder 2 - 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 "feetInContact\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 "qj" - 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 [-375, 387, -265, 458] - ZOrder 900 - 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 "556" - Block { - BlockType Inport - Name "qj" - SID "4310" - Position [50, 38, 80, 52] - ZOrder 902 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4311" - Position [230, 183, 260, 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 SubSystem - Name "LFoot to base link transform " - SID "4245" - Ports [4, 1] - Position [435, 15, 580, 55] - ZOrder 893 - RequestExecContextInheritance off - System { - Name "LFoot to base 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 "248" - Block { - BlockType Inport - Name "qj" - 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.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - 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.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.BASE" - } - Block { - BlockType Product - Name "Inv\\*" - SID "4252" - Ports [2, 1] - Position [825, 40, 895, 95] - ZOrder 731 - 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\\* " - SID "4253" - Ports [2, 1] - Position [825, 184, 895, 231] - ZOrder 738 - 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 [980, 308, 1055, 332] - ZOrder 742 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.NECK_POS" - signalSize "Ports.NECK_POS_PORT_SIZE" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - Block { - BlockType 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 SubSystem - Name "fromImuToHomogeousTransformFCN" - SID "4257" - Ports [6, 1] - Position [1305, 47, 1540, 343] - ZOrder 733 - ShowName off - LibraryVersion "1.30" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "fromImuToHomogeousTransformFCN" - 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 "3785" - SIDPrevWatermark "9" - Block { - BlockType Inport - Name "imu_H_link" - SID "4257::112" - Position [20, 101, 40, 119] - ZOrder 82 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "imu_H_link_0" - SID "4257::113" - Position [20, 136, 40, 154] - ZOrder 83 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "link_H_base" - SID "4257::111" - Position [20, 171, 40, 189] - ZOrder 81 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial_0" - SID "4257::115" - Position [20, 206, 40, 224] - ZOrder 85 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - 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::3784" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 184 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4257::3783" - Tag "Stateflow S-Function torqueBalancingYoga 8" - Ports [6, 2] - Position [180, 102, 230, 243] - ZOrder 183 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[6 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "w_H_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4257::3785" - Position [460, 241, 480, 259] - ZOrder 185 - } - Block { - BlockType Outport - Name "w_H_b" - SID "4257::106" - Position [460, 101, 480, 119] - ZOrder 80 - IconDisplay "Port number" - } - Line { - ZOrder 181 - SrcBlock "imu_H_link" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 182 - SrcBlock "imu_H_link_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 183 - SrcBlock "link_H_base" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 184 - SrcBlock "inertial_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 185 - SrcBlock "inertial" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 186 - SrcBlock "neck_pos" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - Name "w_H_b" - ZOrder 187 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 188 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 189 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - 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.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - 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.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "neck transform" - SID "4260" - Ports [1, 1] - Position [1135, 305, 1260, 335] - ZOrder 743 - RequestExecContextInheritance off - System { - Name "neck transform" - Location [134, 55, 3840, 2160] - Open off - PortBlocksUseCompactNotation off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "843" - 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" - } - 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" - } - 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\\* " - SrcPort 1 - Points [65, 0] - Branch { - ZOrder 3 - Points [0, 165] - DstBlock "Switch6" - DstPort 3 - } - Branch { - ZOrder 4 - Points [0, -40] - DstBlock "fromImuToHomogeousTransformFCN" - 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 "fromImuToHomogeousTransformFCN" - DstPort 1 - } - Branch { - ZOrder 8 - Points [0, 50] - DstBlock "holder\n1" - DstPort 1 - } - } - Line { - ZOrder 9 - SrcBlock "holder\n2" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 4 - } - Line { - ZOrder 10 - SrcBlock "Fixed base to imu transform" - SrcPort 1 - DstBlock "Inv\\*" - DstPort 1 - } - Line { - ZOrder 11 - SrcBlock "holder\n1" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 2 - } - Line { - ZOrder 12 - SrcBlock "qj" - 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 "fromImuToHomogeousTransformFCN" - SrcPort 1 - DstBlock "Switch6" - DstPort 1 - } - Line { - ZOrder 16 - SrcBlock "Fixed base to root link transform" - SrcPort 1 - DstBlock "Inv\\* " - 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 "fromImuToHomogeousTransformFCN" - DstPort 5 - } - } - Line { - ZOrder 20 - SrcBlock "Neck Position" - SrcPort 1 - DstBlock "neck transform" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "neck transform" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 6 - } - Line { - ZOrder 22 - SrcBlock "w_H_link" - SrcPort 1 - Points [98, 0] - Branch { - ZOrder 23 - Points [0, 55] - DstBlock "Inv\\* " - 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 - } - } - } - } - Block { - BlockType Reference - Name "LFoot to world transform (fixed base)" - SID "4269" - Ports [2, 1] - Position [165, 22, 330, 53] - ZOrder 894 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType SubSystem - Name "RFoot to base link transform" - SID "4270" - Ports [4, 1] - Position [435, 100, 580, 140] - ZOrder 896 - RequestExecContextInheritance off - System { - Name "RFoot to base 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 "248" - Block { - BlockType Inport - Name "qj" - SID "4271" - Position [470, 227, 495, 243] - ZOrder 736 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4272" - Position [1000, 212, 1030, 228] - ZOrder 741 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_link" - SID "4273" - Position [655, 132, 680, 148] - ZOrder 744 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_base_fixed" - SID "4274" - Position [470, 32, 495, 48] - ZOrder 745 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "Fixed base to imu transform" - SID "4275" - Ports [2, 1] - Position [575, 25, 740, 85] - ZOrder 729 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.IMU" - } - Block { - BlockType Reference - Name "Fixed base to root link transform" - SID "4276" - Ports [2, 1] - Position [575, 190, 740, 250] - ZOrder 737 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.BASE" - } - Block { - BlockType Product - Name "Inv\\*" - SID "4277" - Ports [2, 1] - Position [825, 40, 895, 95] - ZOrder 731 - 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\\* " - SID "4278" - Ports [2, 1] - Position [825, 184, 895, 231] - ZOrder 738 - 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 "4279" - Ports [0, 1] - Position [980, 308, 1055, 332] - ZOrder 742 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.NECK_POS" - signalSize "Ports.NECK_POS_PORT_SIZE" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - Block { - BlockType Switch - Name "Switch6" - SID "4280" - Position [1760, 151, 1840, 419] - ZOrder 734 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE" - SID "4281" - Position [1570, 275, 1725, 295] - ZOrder 735 - ShowName off - Value "Config.USE_IMU4EST_BASE" - } - Block { - BlockType SubSystem - Name "fromImuToHomogeousTransformFCN" - SID "4282" - Ports [6, 1] - Position [1305, 47, 1540, 343] - ZOrder 733 - ShowName off - LibraryVersion "1.30" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "fromImuToHomogeousTransformFCN" - 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 "3785" - SIDPrevWatermark "9" - Block { - BlockType Inport - Name "imu_H_link" - SID "4282::112" - Position [20, 101, 40, 119] - ZOrder 82 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "imu_H_link_0" - SID "4282::113" - Position [20, 136, 40, 154] - ZOrder 83 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "link_H_base" - SID "4282::111" - Position [20, 171, 40, 189] - ZOrder 81 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial_0" - SID "4282::115" - Position [20, 206, 40, 224] - ZOrder 85 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4282::93" - Position [20, 246, 40, 264] - ZOrder 69 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "neck_pos" - SID "4282::118" - Position [20, 281, 40, 299] - ZOrder 86 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4282::3784" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 184 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4282::3783" - Tag "Stateflow S-Function torqueBalancingYoga 3" - Ports [6, 2] - Position [180, 102, 230, 243] - ZOrder 183 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[6 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "w_H_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4282::3785" - Position [460, 241, 480, 259] - ZOrder 185 - } - Block { - BlockType Outport - Name "w_H_b" - SID "4282::106" - Position [460, 101, 480, 119] - ZOrder 80 - IconDisplay "Port number" - } - Line { - ZOrder 181 - SrcBlock "imu_H_link" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 182 - SrcBlock "imu_H_link_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 183 - SrcBlock "link_H_base" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 184 - SrcBlock "inertial_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 185 - SrcBlock "inertial" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 186 - SrcBlock "neck_pos" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - Name "w_H_b" - ZOrder 187 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 188 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 189 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "holder\n1" - SID "4283" - Ports [1, 1] - Position [1175, 113, 1225, 127] - ZOrder 739 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "holder\n2" - SID "4284" - Ports [1, 1] - Position [1175, 213, 1225, 227] - ZOrder 740 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "neck transform" - SID "4285" - Ports [1, 1] - Position [1135, 305, 1260, 335] - ZOrder 743 - RequestExecContextInheritance off - System { - Name "neck transform" - Location [134, 55, 3840, 2160] - Open off - PortBlocksUseCompactNotation off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "843" - Block { - BlockType Inport - Name "neck port" - SID "4286" - Position [45, 18, 75, 32] - ZOrder 725 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant" - SID "4287" - Position [95, 85, 170, 105] - ZOrder 720 - ShowName off - Value "zeros(3,1)" - } - Block { - BlockType Gain - Name "Gain" - SID "4288" - 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 "4289" - 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 "4290" - Position [300, 7, 375, 113] - ZOrder 722 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE1" - SID "4291" - Position [60, 50, 210, 70] - ZOrder 719 - ShowName off - Value "Config.CORRECT_NECK_IMU" - } - Block { - BlockType Outport - Name "neck position" - SID "4292" - Position [425, 53, 455, 67] - ZOrder 726 - IconDisplay "Port number" - } - 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 "4293" - Position [1875, 278, 1905, 292] - ZOrder 732 - IconDisplay "Port number" - } - 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\\* " - SrcPort 1 - Points [65, 0] - Branch { - ZOrder 3 - Points [0, 165] - DstBlock "Switch6" - DstPort 3 - } - Branch { - ZOrder 4 - Points [0, -40] - DstBlock "fromImuToHomogeousTransformFCN" - 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 "fromImuToHomogeousTransformFCN" - DstPort 1 - } - Branch { - ZOrder 8 - Points [0, 50] - DstBlock "holder\n1" - DstPort 1 - } - } - Line { - ZOrder 9 - SrcBlock "holder\n2" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 4 - } - Line { - ZOrder 10 - SrcBlock "Fixed base to imu transform" - SrcPort 1 - DstBlock "Inv\\*" - DstPort 1 - } - Line { - ZOrder 11 - SrcBlock "holder\n1" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 2 - } - Line { - ZOrder 12 - SrcBlock "qj" - 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 "fromImuToHomogeousTransformFCN" - SrcPort 1 - DstBlock "Switch6" - DstPort 1 - } - Line { - ZOrder 16 - SrcBlock "Fixed base to root link transform" - SrcPort 1 - DstBlock "Inv\\* " - 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 "fromImuToHomogeousTransformFCN" - DstPort 5 - } - } - Line { - ZOrder 20 - SrcBlock "Neck Position" - SrcPort 1 - DstBlock "neck transform" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "neck transform" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 6 - } - Line { - ZOrder 22 - SrcBlock "w_H_link" - SrcPort 1 - Points [98, 0] - Branch { - ZOrder 23 - Points [0, 55] - DstBlock "Inv\\* " - 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 - } - } - } - } - Block { - BlockType Reference - Name "RFoot to world transform (fixed base)" - SID "4306" - Ports [2, 1] - Position [165, 107, 330, 138] - ZOrder 899 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Outport - Name "fixed_LFoot_H_b" - SID "4318" - Position [650, 28, 680, 42] - ZOrder 910 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "fixed_RFoot_H_b" - SID "4320" - Position [650, 113, 680, 127] - ZOrder 912 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 250 - SrcBlock "Constant7" - SrcPort 1 - Points [10, 0] - Branch { - ZOrder 263 - Points [0, -22; 255, 0; 0, 42] - Branch { - ZOrder 265 - Points [0, 85] - DstBlock "RFoot to base link transform" - DstPort 4 - } - Branch { - ZOrder 264 - DstBlock "LFoot to base link transform " - DstPort 4 - } - } - Branch { - ZOrder 252 - Points [0, 85] - DstBlock "RFoot to world transform (fixed base)" - DstPort 1 - } - Branch { - ZOrder 251 - DstBlock "LFoot to world transform (fixed base)" - DstPort 1 - } - } - Line { - ZOrder 244 - SrcBlock "RFoot to base link transform" - SrcPort 1 - DstBlock "fixed_RFoot_H_b" - DstPort 1 - } - Line { - ZOrder 245 - SrcBlock "LFoot to base link transform " - SrcPort 1 - DstBlock "fixed_LFoot_H_b" - DstPort 1 - } - Line { - ZOrder 247 - SrcBlock "LFoot to world transform (fixed base)" - SrcPort 1 - DstBlock "LFoot to base link transform " - DstPort 3 - } - Line { - ZOrder 248 - SrcBlock "RFoot to world transform (fixed base)" - SrcPort 1 - DstBlock "RFoot to base link transform" - DstPort 3 - } - Line { - ZOrder 249 - SrcBlock "qj" - SrcPort 1 - Points [51, 0] - Branch { - ZOrder 254 - Points [0, 85] - Branch { - ZOrder 260 - Points [0, 37; 274, 0; 0, -62] - Branch { - ZOrder 262 - Points [0, -85] - DstBlock "LFoot to base link transform " - DstPort 1 - } - Branch { - ZOrder 261 - DstBlock "RFoot to base link transform" - DstPort 1 - } - } - Branch { - ZOrder 259 - DstBlock "RFoot to world transform (fixed base)" - DstPort 2 - } - } - Branch { - ZOrder 253 - DstBlock "LFoot to world transform (fixed base)" - DstPort 2 - } - } - Line { - ZOrder 255 - SrcBlock "inertial" - SrcPort 1 - Points [119, 0; 0, -75] - Branch { - ZOrder 258 - Points [0, -85] - DstBlock "LFoot to base link transform " - DstPort 2 - } - Branch { - ZOrder 257 - DstBlock "RFoot to base link transform" - DstPort 2 - } - } - } - } - Block { - BlockType Constant - Name "Constant1" - SID "2170" - Position [355, 174, 410, 186] - ZOrder 543 - ShowName off - Value "zeros(6,1)" - } - Block { - BlockType Demux - Name "Demux1" - SID "3690" - Ports [1, 3] - Position [525, 268, 530, 382] - ZOrder 695 - ShowName off - Outputs "[ROBOT_DOF,3,3]" - DisplayOption "bar" - } - Block { - BlockType Logic - Name "Logical\nOperator1" - SID "3267" - Ports [2, 1] - Position [-200, -33, -170, -2] - ZOrder 687 - ShowName off - Operator "OR" - AllPortsSameDT off - OutDataTypeStr "boolean" - } - Block { - BlockType Reference - Name "Minimum Jerk Trajectory Generator" - SID "2176" - Ports [1, 1] - Position [405, 315, 505, 335] - ZOrder 594 - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Minimum Jerk Trajectory Generator" - SourceType "Minimum Jerk Trajectory Generator" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - externalSettlingTime off - settlingTime "Gain.SmoothingTimeGainScheduling" - sampleTime "Config.Ts" - resetOnSettlingTime off - firstDerivatives off - secondDerivatives off - explicitInitialValue off - } - Block { - BlockType Mux - Name "Mux" - SID "3689" - Ports [3, 1] - Position [380, 268, 385, 382] - ZOrder 694 - ShowName off - Inputs "3" - DisplayOption "bar" - } - Block { - BlockType Mux - Name "Mux2" - SID "2178" - Ports [2, 1] - Position [465, 158, 470, 187] - ZOrder 542 - ShowName off - Inputs "2" - DisplayOption "bar" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n3" - SID "3268" - Position [-360, -31, -255, -19] - ZOrder 686 - ShowName off - Value "Config.SCOPES_ALL" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n4" - SID "3269" - Position [-390, -16, -225, -4] - ZOrder 688 - ShowName off - Value "Config.SCOPES_EXT_WRENCHES" - } - Block { - BlockType Reshape - Name "Reshape1" - SID "4503" - Ports [1, 1] - Position [545, 114, 590, 136] - ZOrder 901 - ShowName off - OutputDimensions "[4;4]" - } - Block { - BlockType SubSystem - Name "Visualise external wrenches " - SID "3241" - Ports [2, 0, 1] - Position [-240, 11, -40, 84] - ZOrder 681 - RequestExecContextInheritance off - System { - Name "Visualise external wrenches " - 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 "685" - Block { - BlockType Inport - Name "wL_WBDT" - SID "3245" - Position [160, 247, 205, 263] - ZOrder 648 - IconDisplay "Port number" - Port { - PortNumber 1 - Name "wL_WBDT" - PropagatedSignals "state" - } - } - Block { - BlockType Inport - Name "wR_WBDT" - SID "3251" - Position [160, 362, 205, 378] - ZOrder 682 - Port "2" - IconDisplay "Port number" - Port { - PortNumber 1 - Name "wR_WBDT" - } - } - Block { - BlockType EnablePort - Name "Enable" - SID "3246" - Ports [] - Position [352, 320, 372, 340] - ZOrder 649 - } - Block { - BlockType Constant - Name "Constant" - SID "2169" - Position [290, 390, 435, 420] - ZOrder 679 - ShowName off - Value "Sm.wrench_thresholdContactOn" - Port { - PortNumber 1 - Name "thresholdContactOn" - } - } - Block { - BlockType Constant - Name "Constant2" - SID "3167" - Position [290, 425, 435, 455] - ZOrder 681 - ShowName off - Value "Sm.wrench_thresholdContactOff" - Port { - PortNumber 1 - Name "thresholdContactOff" - } - } - Block { - BlockType Scope - Name "left foot ext wrench" - SID "2205" - Ports [3] - Position [605, 239, 680, 321] - ZOrder 680 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," - "'DataLoggingVariableName','leftFootExtWrenchData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimati" - "on','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{st" - "ruct('MinYLimReal','81.68454','MaxYLimReal','109.97631','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Legend" - "Visibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.6862745098" - "03922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.6235294117647" - "06 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529" - " 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Line" - "PropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('" - "Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle'," - "'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker" - "','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),st" - "ruct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{" - "{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','81.68454','MaxYLimReal" - "','109.97631','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','off','XGrid',true,'YGrid',tru" - "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922]" - ",'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;" - "0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 " - "1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0]," - "'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth'" - ",0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visib" - "le','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0" - " 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','Line" - "Width',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowC" - "ontent',true,'Placement',2),struct('MinYLimReal','81.68454','MaxYLimReal','109.97631','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.07" - "45098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745" - "098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]," - "'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','" - "none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struc" - "t('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyl" - "e','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Mar" - "ker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')" - "}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3)},'DisplayPrope" - "rtyDefaults',struct('MinYLimReal','81.68454','MaxYLimReal','109.97631','YLabelReal','','MinYLimMag','0','MaxYLimMa" - "g','10','LegendVisibility','off','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor" - "',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 " - "0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0." - "717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'" - ",'on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 " - "1],'LineStyle','-','LineWidth',0.5,'Marker','none','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','Vi" - "sible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefined" - "ChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[3 " - "1]),extmgr.Configuration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Config" - "uration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Location',[248 273 1228 835])" - NumInputPorts "3" - Floating off - } - Block { - BlockType Scope - Name "right foot ext wrench" - SID "2217" - Ports [3] - Position [605, 353, 680, 457] - ZOrder 678 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLogging',true," - "'DataLoggingVariableName','rightFootExtWrenchData','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimat" - "ion','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{s" - "truct('MinYLimReal','-65.30999','MaxYLimReal','249.85279','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','Lege" - "ndVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509" - "803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764" - "706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882352" - "9 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','Lin" - "ePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),s" - "truct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames'," - "{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-65.30999','MaxYLimRe" - "al','249.85279','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',tr" - "ue,'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','Visi" - "ble','on'),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','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'Show" - "Content',true,'Placement',2),struct('MinYLimReal','-65.30999','MaxYLimReal','249.85279','YLabelReal','','MinYLimMa" - "g','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.0" - "745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.074" - "5098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]" - ",'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker'," - "'none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" - "ct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - ")}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3)},'DisplayProp" - "ertyDefaults',struct('MinYLimReal','-65.30999','MaxYLimReal','249.85279','YLabelReal','','MinYLimMag','0','MaxYLim" - "Mag','10','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.39215686274" - "5098 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,'Marker'" - ",'none','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],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true," - "'Placement',1),'TimeRangeSamples','42.68','TimeRangeFrames','42.68','DisplayLayoutDimensions',[3 1]),extmgr.Config" - "uration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','" - "Measurements',true,'Version','2017b')),'Version','2018a','Location',[76 151 1355 851])" - NumInputPorts "3" - Floating off - } - Line { - Name "thresholdContactOn" - ZOrder 1 - Labels [1, 1] - SrcBlock "Constant" - SrcPort 1 - Points [123, 0] - Branch { - ZOrder 2 - Points [0, -125] - DstBlock "left foot ext wrench" - DstPort 2 - } - Branch { - ZOrder 3 - DstBlock "right foot ext wrench" - DstPort 2 - } - } - Line { - Name "wR_WBDT" - ZOrder 4 - Labels [0, 0] - SrcBlock "wR_WBDT" - SrcPort 1 - DstBlock "right foot ext wrench" - DstPort 1 - } - Line { - Name "wL_WBDT" - ZOrder 5 - Labels [0, 0] - SrcBlock "wL_WBDT" - SrcPort 1 - DstBlock "left foot ext wrench" - DstPort 1 - } - Line { - Name "thresholdContactOff" - ZOrder 6 - Labels [1, 1] - SrcBlock "Constant2" - SrcPort 1 - Points [143, 0] - Branch { - ZOrder 7 - Points [0, -135] - DstBlock "left foot ext wrench" - DstPort 3 - } - Branch { - ZOrder 8 - DstBlock "right foot ext wrench" - DstPort 3 - } - } - } - } - Block { - BlockType Reference - Name "holder\n1" - SID "2187" - Ports [1, 1] - Position [-15, 187, 45, 203] - ZOrder 583 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "holder\n2" - SID "2188" - Ports [1, 1] - Position [-15, 222, 45, 238] - ZOrder 584 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "inertial" - SID "2630" - Ports [0, 1] - Position [-460, 433, -410, 447] - ZOrder 654 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.IMU" - signalSize "12" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - Block { - BlockType Reference - Name "left_foot_wrench" - SID "2206" - Ports [0, 1] - Position [-465, 16, -405, 44] - ZOrder 598 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.WRENCH_LEFT_FOOT" - signalSize "6" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on - Port { - PortNumber 1 - Name "wL_WBDT" - } - } - Block { - BlockType Reference - Name "right_foot_wrench" - SID "2218" - Ports [0, 1] - Position [-465, 53, -405, 77] - ZOrder 599 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.WRENCH_RIGHT_FOOT" - signalSize "6" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on - Port { - PortNumber 1 - Name "wR_WBDT" - } - } - Block { - BlockType SubSystem - Name "stateMachineYogaFCN" - SID "2220" - Ports [10, 9] - Position [75, 101, 300, 464] - ZOrder 497 - ForegroundColor "red" - LibraryVersion "1.30" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - Port { - PortNumber 4 - Name "constraintsSmooth" - PropagatedSignals "constraints" - } - System { - Name "stateMachineYogaFCN" - 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 "3771" - SIDPrevWatermark "9" - Block { - BlockType Inport - Name "wrench_rightFoot" - SID "2220::93" - Position [20, 101, 40, 119] - ZOrder 69 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "wrench_leftFoot" - SID "2220::103" - Position [20, 136, 40, 154] - ZOrder 77 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "CoM_0" - SID "2220::58" - Position [20, 171, 40, 189] - ZOrder 34 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "q0" - SID "2220::82" - Position [20, 206, 40, 224] - ZOrder 58 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "l_sole_CoM" - SID "2220::92" - Position [20, 246, 40, 264] - ZOrder 68 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "r_sole_CoM" - SID "2220::107" - Position [20, 281, 40, 299] - ZOrder 81 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "2220::97" - Position [20, 316, 40, 334] - ZOrder 72 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "t" - SID "2220::84" - Position [20, 351, 40, 369] - ZOrder 60 - 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::3770" - Ports [1, 1] - Position [270, 460, 320, 500] - ZOrder 168 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "2220::3769" - Tag "Stateflow S-Function torqueBalancingYoga 13" - Ports [10, 10] - Position [180, 100, 230, 320] - ZOrder 167 - FunctionName "sf_sfun" - Parameters "Gain,Sm" - PortCounts "[10 10]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "w_H_b" - } - Port { - PortNumber 3 - Name "CoM_des" - } - Port { - PortNumber 4 - Name "qj_des" - } - Port { - PortNumber 5 - Name "constraints" - } - Port { - PortNumber 6 - Name "impedances" - } - Port { - PortNumber 7 - Name "KPCoM" - } - Port { - PortNumber 8 - Name "KDCoM" - } - Port { - PortNumber 9 - Name "currentState" - } - Port { - PortNumber 10 - Name "jointsSmoothingTime" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "2220::3771" - Position [460, 471, 480, 489] - ZOrder 169 - } - Block { - BlockType Outport - Name "w_H_b" - SID "2220::106" - Position [460, 101, 480, 119] - ZOrder 80 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "CoM_des" - SID "2220::77" - Position [460, 136, 480, 154] - ZOrder 53 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qj_des" - SID "2220::67" - Position [460, 171, 480, 189] - ZOrder 43 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "constraints" - SID "2220::64" - Position [460, 206, 480, 224] - ZOrder 40 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "impedances" - SID "2220::94" - Position [460, 246, 480, 264] - ZOrder 70 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "KPCoM" - SID "2220::112" - Position [460, 281, 480, 299] - ZOrder 84 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "KDCoM" - SID "2220::113" - Position [460, 316, 480, 334] - ZOrder 85 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "currentState" - SID "2220::96" - Position [460, 351, 480, 369] - ZOrder 71 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "jointsSmoothingTime" - SID "2220::111" - Position [460, 386, 480, 404] - ZOrder 83 - Port "9" - IconDisplay "Port number" - } - Line { - ZOrder 569 - SrcBlock "wrench_rightFoot" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 570 - SrcBlock "wrench_leftFoot" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 571 - SrcBlock "CoM_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 572 - SrcBlock "q0" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 573 - SrcBlock "l_sole_CoM" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 574 - SrcBlock "r_sole_CoM" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - ZOrder 575 - SrcBlock "qj" - SrcPort 1 - DstBlock " SFunction " - DstPort 7 - } - Line { - ZOrder 576 - SrcBlock "t" - SrcPort 1 - DstBlock " SFunction " - DstPort 8 - } - Line { - ZOrder 577 - SrcBlock "l_sole_H_b" - SrcPort 1 - DstBlock " SFunction " - DstPort 9 - } - Line { - ZOrder 578 - SrcBlock "r_sole_H_b" - SrcPort 1 - DstBlock " SFunction " - DstPort 10 - } - Line { - Name "w_H_b" - ZOrder 579 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "w_H_b" - DstPort 1 - } - Line { - Name "CoM_des" - ZOrder 580 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 3 - DstBlock "CoM_des" - DstPort 1 - } - Line { - Name "qj_des" - ZOrder 581 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 4 - DstBlock "qj_des" - DstPort 1 - } - Line { - Name "constraints" - ZOrder 582 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 5 - DstBlock "constraints" - DstPort 1 - } - Line { - Name "impedances" - ZOrder 583 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 6 - DstBlock "impedances" - DstPort 1 - } - Line { - Name "KPCoM" - ZOrder 584 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 7 - DstBlock "KPCoM" - DstPort 1 - } - Line { - Name "KDCoM" - ZOrder 585 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 8 - DstBlock "KDCoM" - DstPort 1 - } - Line { - Name "currentState" - ZOrder 586 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 9 - DstBlock "currentState" - DstPort 1 - } - Line { - Name "jointsSmoothingTime" - ZOrder 587 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 10 - DstBlock "jointsSmoothingTime" - DstPort 1 - } - Line { - ZOrder 588 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 589 - SrcBlock " SFunction " - SrcPort 1 - Points [20, 0] - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "xCom" - SID "4226" - Ports [2, 1] - Position [-185, 249, -90, 276] - ZOrder 890 - ShowName off - RequestExecContextInheritance off - System { - Name "xCom" - 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 "942" - 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 "qj" - 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.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.COM" - } - Block { - BlockType Selector - Name "CoM6D -> \nCoMXYZ2" - SID "4230" - Ports [1, 1] - Position [310, 241, 350, 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 "xCoM" - SID "4231" - Position [385, 253, 415, 267] - ZOrder 583 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "qj" - SrcPort 1 - DstBlock "CoM" - DstPort 2 - } - Line { - ZOrder 2 - SrcBlock "CoM6D -> \nCoMXYZ2" - SrcPort 1 - DstBlock "xCoM" - 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 "xCom1" - SID "4300" - Ports [2, 1] - Position [-185, 284, -90, 311] - ZOrder 898 - ShowName off - RequestExecContextInheritance off - System { - Name "xCom1" - 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 "942" - Block { - BlockType Inport - Name "w_H_b" - SID "4301" - Position [30, 237, 60, 253] - ZOrder 581 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "4302" - Position [30, 267, 60, 283] - ZOrder 582 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "CoM" - SID "4303" - Ports [2, 1] - Position [110, 230, 275, 290] - ZOrder 578 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.COM" - } - Block { - BlockType Selector - Name "CoM6D -> \nCoMXYZ2" - SID "4304" - Ports [1, 1] - Position [310, 241, 350, 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 "xCoM" - SID "4305" - Position [385, 253, 415, 267] - ZOrder 583 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "qj" - SrcPort 1 - DstBlock "CoM" - DstPort 2 - } - Line { - ZOrder 2 - SrcBlock "CoM6D -> \nCoMXYZ2" - SrcPort 1 - DstBlock "xCoM" - 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 "CoMDes" - SID "2234" - Position [640, 168, 670, 182] - ZOrder 478 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qjDes" - SID "2235" - Position [370, 198, 400, 212] - ZOrder 479 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "2236" - Position [635, 237, 675, 253] - ZOrder 480 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "impedances" - SID "2237" - Position [640, 277, 675, 293] - ZOrder 545 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "state" - SID "2238" - Position [370, 397, 405, 413] - ZOrder 550 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "nu_b + qjDot" - SID "2239" - Position [765, -48, 795, -32] - ZOrder 577 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qj" - SID "2240" - Position [-450, 487, -420, 503] - ZOrder 579 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "smoothingTimeCoM_Joints" - SID "3181" - Position [375, 438, 405, 452] - ZOrder 680 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "KpCoM" - SID "3316" - Position [640, 317, 675, 333] - ZOrder 689 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "KdCoM" - SID "3318" - Position [640, 355, 675, 375] - ZOrder 691 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "w_H_b" - SID "2241" - Position [640, 117, 670, 133] - ZOrder 578 - Port "11" - IconDisplay "Port number" - } - Line { - Name "wR_WBDT" - ZOrder 1 - SrcBlock "right_foot_wrench" - SrcPort 1 - Points [42, 0] - Branch { - ZOrder 163 - DstBlock "Visualise external wrenches " - DstPort 2 - } - Branch { - ZOrder 161 - Labels [-1, 1] - Points [0, 60] - DstBlock "stateMachineYogaFCN" - DstPort 1 - } - } - Line { - ZOrder 173 - SrcBlock "xCom" - SrcPort 1 - Points [19, 0] - Branch { - ZOrder 315 - DstBlock "stateMachineYogaFCN" - DstPort 5 - } - Branch { - ZOrder 314 - Points [0, -70] - DstBlock "holder\n1" - DstPort 1 - } - } - Line { - ZOrder 7 - SrcBlock "holder\n1" - SrcPort 1 - DstBlock "stateMachineYogaFCN" - DstPort 3 - } - Line { - ZOrder 8 - SrcBlock "holder\n2" - SrcPort 1 - DstBlock "stateMachineYogaFCN" - DstPort 4 - } - Line { - ZOrder 9 - SrcBlock "Clock1" - SrcPort 1 - DstBlock "stateMachineYogaFCN" - DstPort 8 - } - Line { - ZOrder 10 - SrcBlock "stateMachineYogaFCN" - SrcPort 3 - DstBlock "qjDes" - DstPort 1 - } - Line { - ZOrder 11 - SrcBlock "stateMachineYogaFCN" - SrcPort 2 - DstBlock "Mux2" - DstPort 1 - } - Line { - ZOrder 12 - SrcBlock "Mux2" - SrcPort 1 - DstBlock "CoMDes" - DstPort 1 - } - Line { - ZOrder 13 - SrcBlock "Constant1" - SrcPort 1 - DstBlock "Mux2" - DstPort 2 - } - Line { - ZOrder 14 - SrcBlock "stateMachineYogaFCN" - SrcPort 8 - DstBlock "state" - DstPort 1 - } - Line { - Name "constraintsSmooth" - ZOrder 15 - SrcBlock "stateMachineYogaFCN" - SrcPort 4 - Points [185, 0] - Branch { - ZOrder 272 - DstBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - DstPort 1 - } - Branch { - ZOrder 270 - Labels [2, 1] - Points [0, -260] - DstBlock "Compute State Velocity" - DstPort 3 - } - } - Line { - Name "wL_WBDT" - ZOrder 18 - SrcBlock "left_foot_wrench" - SrcPort 1 - Points [25, 0] - Branch { - ZOrder 162 - DstBlock "Visualise external wrenches " - DstPort 1 - } - Branch { - ZOrder 68 - Labels [-1, 1] - Points [0, 130] - DstBlock "stateMachineYogaFCN" - DstPort 2 - } - } - Line { - ZOrder 21 - SrcBlock "stateMachineYogaFCN" - SrcPort 1 - Points [164, 0] - Branch { - ZOrder 268 - Points [0, -165] - DstBlock "Compute State Velocity" - DstPort 2 - } - Branch { - ZOrder 267 - DstBlock "Reshape1" - DstPort 1 - } - } - Line { - ZOrder 207 - SrcBlock "xCom1" - SrcPort 1 - DstBlock "stateMachineYogaFCN" - DstPort 6 - } - Line { - ZOrder 273 - SrcBlock "Compute State Velocity" - SrcPort 1 - DstBlock "nu_b + qjDot" - DstPort 1 - } - Line { - ZOrder 53 - SrcBlock "stateMachineYogaFCN" - SrcPort 9 - DstBlock "smoothingTimeCoM_Joints" - DstPort 1 - } - Line { - ZOrder 54 - SrcBlock "Logical\nOperator1" - SrcPort 1 - Points [25, 0] - DstBlock "Visualise external wrenches " - DstPort enable - } - Line { - ZOrder 58 - SrcBlock "ON_GAZEBO\n3" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 1 - } - Line { - ZOrder 59 - SrcBlock "ON_GAZEBO\n4" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 2 - } - Line { - ZOrder 60 - SrcBlock "stateMachineYogaFCN" - SrcPort 6 - DstBlock "Mux" - DstPort 2 - } - Line { - ZOrder 61 - SrcBlock "Demux1" - SrcPort 3 - DstBlock "KdCoM" - DstPort 1 - } - Line { - ZOrder 62 - SrcBlock "stateMachineYogaFCN" - SrcPort 7 - DstBlock "Mux" - DstPort 3 - } - Line { - ZOrder 63 - SrcBlock "stateMachineYogaFCN" - SrcPort 5 - DstBlock "Mux" - DstPort 1 - } - Line { - ZOrder 64 - SrcBlock "Mux" - SrcPort 1 - DstBlock "Minimum Jerk Trajectory Generator" - DstPort 1 - } - Line { - ZOrder 65 - SrcBlock "Minimum Jerk Trajectory Generator" - SrcPort 1 - DstBlock "Demux1" - DstPort 1 - } - Line { - ZOrder 66 - SrcBlock "Demux1" - SrcPort 1 - DstBlock "impedances" - DstPort 1 - } - Line { - ZOrder 67 - SrcBlock "Demux1" - SrcPort 2 - DstBlock "KpCoM" - DstPort 1 - } - Line { - ZOrder 32 - SrcBlock "jointAngles" - SrcPort 1 - Points [54, 0] - Branch { - ZOrder 262 - DstBlock "xCom" - DstPort 2 - } - Branch { - ZOrder 205 - Points [0, -40] - Branch { - ZOrder 266 - Points [0, -295] - DstBlock "Compute State Velocity" - DstPort 1 - } - Branch { - ZOrder 264 - DstBlock "holder\n2" - DstPort 1 - } - } - Branch { - ZOrder 256 - Points [0, 35] - Branch { - ZOrder 257 - Points [0, 30] - Branch { - ZOrder 216 - DstBlock "stateMachineYogaFCN" - DstPort 7 - } - Branch { - ZOrder 235 - Points [0, 70] - Branch { - ZOrder 253 - Points [0, 90] - DstBlock "qj" - DstPort 1 - } - Branch { - ZOrder 249 - DstBlock "Compute base to fixed link transform" - DstPort 1 - } - } - } - Branch { - ZOrder 214 - DstBlock "xCom1" - DstPort 2 - } - } - } - Line { - ZOrder 239 - SrcBlock "Compute base to fixed link transform" - SrcPort 1 - Points [13, 0] - Branch { - ZOrder 258 - DstBlock "stateMachineYogaFCN" - DstPort 9 - } - Branch { - ZOrder 242 - Points [0, -150] - DstBlock "xCom" - DstPort 1 - } - } - Line { - ZOrder 240 - SrcBlock "Compute base to fixed link transform" - SrcPort 2 - Points [32, 0] - Branch { - ZOrder 259 - DstBlock "stateMachineYogaFCN" - DstPort 10 - } - Branch { - ZOrder 244 - Points [0, -150] - DstBlock "xCom1" - DstPort 1 - } - } - Line { - ZOrder 250 - SrcBlock "inertial" - SrcPort 1 - DstBlock "Compute base to fixed link transform" - DstPort 2 - } - Line { - ZOrder 367 - SrcBlock "Reshape1" - SrcPort 1 - DstBlock "w_H_b" - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "Yoga" - SID "2909" - Ports [1, 1] - Position [-445, 2851, -385, 2879] - ZOrder 606 - ShowName off - LibraryVersion "1.441" - SourceBlock "simulink/Logic and Bit\nOperations/Bitwise\nOperator" - SourceType "Bitwise Operator" - SourceProductName "Simulink" - SourceProductBaseCode "SL" - MultiThreadCoSim "auto" - logicop "AND" - UseBitMask on - NumInputPorts "1" - BitMask "Sm.SM_MASK_YOGA" - BitMaskRealWorld "Stored Integer" - } - Block { - BlockType Outport - Name "w_H_b" - SID "2269" - Position [705, 2779, 735, 2791] - ZOrder 578 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qj" - SID "2270" - Position [705, 2809, 735, 2821] - ZOrder 579 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "nu" - SID "2271" - Position [705, 2839, 735, 2851] - ZOrder 577 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "state" - SID "3201" - Position [705, 2960, 735, 2970] - ZOrder 647 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "2272" - Position [705, 2870, 735, 2880] - ZOrder 410 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "impedances" - SID "2268" - Position [705, 2690, 735, 2700] - ZOrder 517 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "gainsPCOM" - SID "3330" - Position [705, 2720, 735, 2730] - ZOrder 692 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "gainsDCOM" - SID "3331" - Position [705, 2750, 735, 2760] - ZOrder 693 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "desired_x_dx_ddx_CoM" - SID "2274" - Position [705, 2900, 735, 2910] - ZOrder 323 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "qjDes" - SID "2273" - Position [705, 2930, 735, 2940] - ZOrder 418 - Port "10" - IconDisplay "Port number" - } - Line { - ZOrder 15 - SrcBlock "Constant" - SrcPort 1 - Points [31, 0] - Branch { - ZOrder 441 - DstBlock "Yoga" - DstPort 1 - } - Branch { - ZOrder 16 - Points [0, -270] - DstBlock "Coordinator" - DstPort 1 - } - } - Line { - ZOrder 20 - SrcBlock "State Machine Yoga" - SrcPort 3 - DstBlock "Mux1" - DstPort 3 - } - Line { - ZOrder 21 - SrcBlock "State Machine Yoga" - SrcPort 2 - DstBlock "Mux1" - DstPort 2 - } - Line { - ZOrder 22 - SrcBlock "State Machine Yoga" - SrcPort 1 - DstBlock "Mux1" - DstPort 1 - } - Line { - ZOrder 23 - SrcBlock "Internal Coordinator" - SrcPort 1 - DstBlock "Mux2" - DstPort 1 - } - Line { - ZOrder 24 - SrcBlock "Internal Coordinator" - SrcPort 2 - DstBlock "Mux2" - DstPort 2 - } - Line { - ZOrder 25 - SrcBlock "Internal Coordinator" - SrcPort 3 - DstBlock "Mux2" - DstPort 3 - } - Line { - ZOrder 26 - SrcBlock "State Machine Yoga" - SrcPort 4 - DstBlock "Mux1" - DstPort 4 - } - Line { - ZOrder 27 - SrcBlock "Internal Coordinator" - SrcPort 4 - DstBlock "Mux2" - DstPort 4 - } - Line { - ZOrder 28 - SrcBlock "State Machine Yoga" - SrcPort 5 - DstBlock "Mux1" - DstPort 5 - } - Line { - ZOrder 29 - SrcBlock "Internal Coordinator" - SrcPort 5 - DstBlock "Mux2" - DstPort 5 - } - Line { - ZOrder 408 - SrcBlock "Internal Coordinator" - SrcPort 6 - DstBlock "Mux2" - DstPort 6 - } - Line { - ZOrder 31 - SrcBlock "Internal Coordinator" - SrcPort 7 - DstBlock "Mux2" - DstPort 7 - } - Line { - ZOrder 32 - SrcBlock "State Machine Yoga" - SrcPort 6 - DstBlock "Mux1" - DstPort 6 - } - Line { - ZOrder 33 - SrcBlock "State Machine Yoga" - SrcPort 7 - DstBlock "Mux1" - DstPort 7 - } - Line { - ZOrder 41 - SrcBlock "Coordinator" - SrcPort 1 - DstBlock "Compare\nTo Zero1" - DstPort 1 - } - Line { - ZOrder 42 - SrcBlock "Yoga" - SrcPort 1 - DstBlock "Compare\nTo Zero" - DstPort 1 - } - Line { - ZOrder 47 - SrcBlock "Compare\nTo Zero" - SrcPort 1 - Points [125, 0] - DstBlock "State Machine Yoga" - DstPort enable - } - Line { - ZOrder 48 - SrcBlock "Compare\nTo Zero1" - SrcPort 1 - Points [125, 0] - DstBlock "Internal Coordinator" - DstPort enable - } - Line { - ZOrder 73 - SrcBlock "Internal Coordinator" - SrcPort 8 - DstBlock "Mux2" - DstPort 8 - } - Line { - ZOrder 75 - SrcBlock "State Machine Yoga" - SrcPort 8 - DstBlock "Mux1" - DstPort 8 - } - Line { - ZOrder 101 - SrcBlock "State Machine Yoga" - SrcPort 9 - DstBlock "Mux1" - DstPort 9 - } - Line { - ZOrder 102 - SrcBlock "State Machine Yoga" - SrcPort 10 - DstBlock "Mux1" - DstPort 10 - } - Line { - ZOrder 103 - SrcBlock "Internal Coordinator" - SrcPort 9 - DstBlock "Mux2" - DstPort 9 - } - Line { - ZOrder 104 - SrcBlock "Internal Coordinator" - SrcPort 10 - DstBlock "Mux2" - DstPort 10 - } - Line { - ZOrder 117 - SrcBlock "Constant2" - SrcPort 1 - DstBlock "Multiport\nSwitch1" - DstPort 1 - } - Line { - ZOrder 515 - SrcBlock "Get Measurement" - SrcPort 1 - Points [55, 0] - Branch { - ZOrder 442 - DstBlock "Internal Coordinator" - DstPort 1 - } - Branch { - ZOrder 383 - Points [0, 270] - DstBlock "State Machine Yoga" - DstPort 1 - } - } - Line { - ZOrder 384 - SrcBlock "State Machine Yoga" - SrcPort 11 - DstBlock "Mux1" - DstPort 11 - } - Line { - ZOrder 385 - SrcBlock "Internal Coordinator" - SrcPort 11 - DstBlock "Mux2" - DstPort 11 - } - Line { - ZOrder 389 - SrcBlock "Mux2" - SrcPort 1 - Points [36, 0; 0, 110] - DstBlock "Multiport\nSwitch1" - DstPort 2 - } - Line { - ZOrder 390 - SrcBlock "Mux1" - SrcPort 1 - Points [37, 0; 0, -125] - DstBlock "Multiport\nSwitch1" - DstPort 3 - } - Line { - ZOrder 395 - SrcBlock "Multiport\nSwitch1" - SrcPort 1 - DstBlock "Select State and References" - DstPort 1 - } - Line { - ZOrder 396 - SrcBlock "Select State and References" - SrcPort 1 - DstBlock "impedances" - DstPort 1 - } - Line { - ZOrder 397 - SrcBlock "Select State and References" - SrcPort 2 - DstBlock "gainsPCOM" - DstPort 1 - } - Line { - ZOrder 398 - SrcBlock "Select State and References" - SrcPort 3 - DstBlock "gainsDCOM" - DstPort 1 - } - Line { - ZOrder 399 - SrcBlock "Select State and References" - SrcPort 4 - Points [54, 0] - Branch { - ZOrder 500 - DstBlock "w_H_b" - DstPort 1 - } - Branch { - ZOrder 487 - Points [0, 305] - DstBlock "Goto2" - DstPort 1 - } - } - Line { - ZOrder 400 - SrcBlock "Select State and References" - SrcPort 5 - DstBlock "qj" - DstPort 1 - } - Line { - ZOrder 401 - SrcBlock "Select State and References" - SrcPort 6 - DstBlock "nu" - DstPort 1 - } - Line { - ZOrder 402 - SrcBlock "Select State and References" - SrcPort 7 - DstBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - DstPort 1 - } - Line { - ZOrder 403 - SrcBlock "Select State and References" - SrcPort 8 - Points [30, 0] - Branch { - ZOrder 499 - DstBlock "desired_x_dx_ddx_CoM" - DstPort 1 - } - Branch { - ZOrder 498 - Points [0, 225] - DstBlock "Selector" - DstPort 1 - } - } - Line { - ZOrder 404 - SrcBlock "Select State and References" - SrcPort 9 - Points [104, 0] - Branch { - ZOrder 502 - DstBlock "qjDes" - DstPort 1 - } - Branch { - ZOrder 491 - Points [0, 80] - DstBlock "Goto4" - DstPort 1 - } - } - Line { - ZOrder 407 - SrcBlock "Select State and References" - SrcPort 10 - Points [78, 0] - Branch { - ZOrder 501 - DstBlock "state" - DstPort 1 - } - Branch { - ZOrder 489 - Points [0, 85] - DstBlock "Goto3" - DstPort 1 - } - } - Line { - ZOrder 503 - SrcBlock "Selector" - SrcPort 1 - DstBlock "Goto7" - DstPort 1 - } - } - } - Block { - BlockType Saturate - Name "Saturation" - SID "2353" - Ports [1, 1] - Position [1625, 512, 1665, 548] - ZOrder 545 - ShowName off - InputPortMap "u0" - UpperLimit "Sat.torque" - LowerLimit "-Sat.torque" - } - Block { - BlockType Reference - Name "Set References" - SID "2354" - Ports [1] - Position [1755, 517, 1815, 543] - ZOrder 549 - BackgroundColor "[0.827500, 0.576500, 0.603900]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyActuators/Set References" - SourceType "Set References" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - controlType "Torque" - refSpeed "50" - refAcceleration "1000000*(pi/180)" - } - Block { - BlockType SubSystem - Name "controller_QP" - SID "2355" - Ports [20, 1] - Position [1280, 241, 1480, 819] - ZOrder 542 - BackgroundColor "lightBlue" - RequestExecContextInheritance off - System { - Name "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 "156" - Block { - BlockType Inport - Name "M" - SID "2360" - Position [1235, 428, 1265, 442] - ZOrder 326 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "biasTorques" - SID "2361" - Position [1235, 473, 1265, 487] - ZOrder 327 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "L " - SID "3219" - Position [1235, 518, 1265, 532] - ZOrder 639 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_l_sole" - SID "4384" - Position [1235, 608, 1265, 622] - ZOrder 837 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_r_sole" - SID "4391" - Position [1235, 653, 1265, 667] - ZOrder 844 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "JL" - SID "2357" - Position [1235, 698, 1265, 712] - ZOrder 550 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "JR" - SID "4392" - Position [1235, 743, 1265, 757] - ZOrder 845 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "J_CoM" - SID "4390" - Position [1235, 923, 1265, 937] - ZOrder 843 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "xCoM" - SID "4383" - Position [1235, 878, 1265, 892] - ZOrder 836 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "dJL_nu" - SID "4386" - Position [1235, 788, 1265, 802] - ZOrder 839 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "dJR_nu" - SID "4388" - Position [1235, 833, 1265, 847] - ZOrder 841 - Port "11" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "2358" - Position [1070, 293, 1100, 307] - ZOrder 323 - Port "12" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "nu" - SID "2359" - Position [1235, 383, 1265, 397] - ZOrder 325 - Port "13" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4387" - Position [1070, 578, 1100, 592] - ZOrder 840 - Port "14" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "2363" - Position [1070, 113, 1100, 127] - ZOrder 386 - Port "15" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "impedances" - SID "2356" - Position [1235, 1103, 1265, 1117] - ZOrder 336 - Port "16" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gainsPCOM" - SID "3328" - Position [1235, 1013, 1265, 1027] - ZOrder 644 - Port "17" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gainsDCOM" - SID "3329" - Position [1235, 1058, 1265, 1072] - ZOrder 645 - Port "18" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "desired_x_dx_ddx_CoM" - SID "2365" - Position [1235, 968, 1265, 982] - ZOrder 335 - Port "19" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qjDes" - SID "2364" - Position [1070, 338, 1100, 352] - ZOrder 324 - Port "20" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "\n\n 5" - SID "2366" - Position [1175, 158, 1325, 172] - ZOrder 342 - ShowName off - Value "ROBOT_DOF_FOR_SIMULINK" - } - Block { - BlockType SubSystem - Name "Balancing Controller\n" - SID "2407" - Ports [23, 14] - Position [1370, 102, 1615, 1128] - ZOrder 278 - LibraryVersion "1.32" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - Port { - PortNumber 13 - Name "CoM_Error" - PropagatedSignals "errorCoM" - } - Port { - PortNumber 14 - Name "fNoQP" - PropagatedSignals "f_noQP" - } - System { - Name "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 "3770" - Block { - BlockType Inport - Name "constraints" - SID "2407::28" - Position [20, 101, 40, 119] - ZOrder 14 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ROBOT_DOF_FOR_SIMULINK" - SID "2407::29" - Position [20, 136, 40, 154] - ZOrder 15 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ConstraintsMatrix" - SID "2407::808" - Position [20, 171, 40, 189] - ZOrder 56 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "bVectorConstraints" - SID "2407::809" - Position [20, 206, 40, 224] - ZOrder 57 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "2407::1" - Position [20, 246, 40, 264] - ZOrder -1 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qjDes" - SID "2407::21" - Position [20, 281, 40, 299] - ZOrder 7 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "nu" - SID "2407::22" - Position [20, 316, 40, 334] - ZOrder 8 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "M" - SID "2407::23" - Position [20, 351, 40, 369] - ZOrder 9 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "h" - SID "2407::24" - Position [20, 386, 40, 404] - ZOrder 10 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "L" - SID "2407::26" - Position [20, 426, 40, 444] - ZOrder 12 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "intLw" - SID "2407::841" - Position [20, 461, 40, 479] - ZOrder 84 - Port "11" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_l_sole" - SID "2407::800" - Position [20, 496, 40, 514] - ZOrder 50 - Port "12" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_r_sole" - SID "2407::27" - Position [20, 531, 40, 549] - ZOrder 13 - Port "13" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "JL" - SID "2407::790" - Position [20, 566, 40, 584] - ZOrder 41 - Port "14" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "JR" - SID "2407::784" - Position [20, 606, 40, 624] - ZOrder 36 - Port "15" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "dJLv" - SID "2407::785" - Position [20, 641, 40, 659] - ZOrder 37 - Port "16" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "dJRv" - SID "2407::798" - Position [20, 676, 40, 694] - ZOrder 48 - Port "17" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "xCoM" - SID "2407::30" - Position [20, 711, 40, 729] - ZOrder 16 - Port "18" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "J_CoM" - SID "2407::31" - Position [20, 746, 40, 764] - ZOrder 17 - Port "19" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "desired_x_dx_ddx_CoM" - SID "2407::32" - Position [20, 786, 40, 804] - ZOrder 18 - Port "20" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gainsPCOM" - SID "2407::845" - Position [20, 821, 40, 839] - ZOrder 88 - Port "21" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gainsDCOM" - SID "2407::846" - Position [20, 856, 40, 874] - ZOrder 89 - Port "22" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "impedances" - SID "2407::34" - Position [20, 891, 40, 909] - ZOrder 20 - Port "23" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "2407::3769" - Ports [1, 1] - Position [270, 640, 320, 680] - ZOrder 172 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "2407::3768" - Tag "Stateflow S-Function torqueBalancingYoga 17" - Ports [23, 15] - Position [180, 65, 230, 545] - ZOrder 171 - FunctionName "sf_sfun" - Parameters "Gain,Reg" - PortCounts "[23 15]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "tauModel" - } - Port { - PortNumber 3 - Name "Sigma" - } - Port { - PortNumber 4 - Name "NA" - } - Port { - PortNumber 5 - Name "fLdotDesC1C2" - } - Port { - PortNumber 6 - Name "HessianMatrixQP1Foot" - } - Port { - PortNumber 7 - Name "gradientQP1Foot" - } - Port { - PortNumber 8 - Name "ConstraintsMatrixQP1Foot" - } - Port { - PortNumber 9 - Name "bVectorConstraintsQp1Foot" - } - Port { - PortNumber 10 - Name "HessianMatrixQP2Feet" - } - Port { - PortNumber 11 - Name "gradientQP2Feet" - } - Port { - PortNumber 12 - Name "ConstraintsMatrixQP2Feet" - } - Port { - PortNumber 13 - Name "bVectorConstraintsQp2Feet" - } - Port { - PortNumber 14 - Name "errorCoM" - } - Port { - PortNumber 15 - Name "f_noQP" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "2407::3770" - Position [460, 651, 480, 669] - ZOrder 173 - } - Block { - BlockType Outport - Name "tauModel" - SID "2407::815" - Position [460, 101, 480, 119] - ZOrder 63 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "Sigma" - SID "2407::816" - Position [460, 136, 480, 154] - ZOrder 64 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "NA" - SID "2407::824" - Position [460, 171, 480, 189] - ZOrder 72 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "fLdotDesC1C2" - SID "2407::822" - Position [460, 206, 480, 224] - ZOrder 70 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "HessianMatrixQP1Foot" - SID "2407::5" - Position [460, 246, 480, 264] - ZOrder -5 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "gradientQP1Foot" - SID "2407::810" - Position [460, 281, 480, 299] - ZOrder 58 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "ConstraintsMatrixQP1Foot" - SID "2407::827" - Position [460, 316, 480, 334] - ZOrder 75 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "bVectorConstraintsQp1Foot" - SID "2407::828" - Position [460, 351, 480, 369] - ZOrder 76 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "HessianMatrixQP2Feet" - SID "2407::811" - Position [460, 386, 480, 404] - ZOrder 59 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "gradientQP2Feet" - SID "2407::812" - Position [460, 426, 480, 444] - ZOrder 60 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "ConstraintsMatrixQP2Feet" - SID "2407::820" - Position [460, 461, 480, 479] - ZOrder 68 - Port "11" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "bVectorConstraintsQp2Feet" - SID "2407::821" - Position [460, 496, 480, 514] - ZOrder 69 - Port "12" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "errorCoM" - SID "2407::806" - Position [460, 531, 480, 549] - ZOrder 54 - Port "13" - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "f_noQP" - SID "2407::807" - Position [460, 566, 480, 584] - ZOrder 55 - Port "14" - IconDisplay "Port number" - } - Line { - ZOrder 1113 - SrcBlock "constraints" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 1114 - SrcBlock "ROBOT_DOF_FOR_SIMULINK" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 1115 - SrcBlock "ConstraintsMatrix" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 1116 - SrcBlock "bVectorConstraints" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 1117 - SrcBlock "qj" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 1118 - SrcBlock "qjDes" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - ZOrder 1119 - SrcBlock "nu" - SrcPort 1 - DstBlock " SFunction " - DstPort 7 - } - Line { - ZOrder 1120 - SrcBlock "M" - SrcPort 1 - DstBlock " SFunction " - DstPort 8 - } - Line { - ZOrder 1121 - SrcBlock "h" - SrcPort 1 - DstBlock " SFunction " - DstPort 9 - } - Line { - ZOrder 1122 - SrcBlock "L" - SrcPort 1 - DstBlock " SFunction " - DstPort 10 - } - Line { - ZOrder 1123 - SrcBlock "intLw" - SrcPort 1 - DstBlock " SFunction " - DstPort 11 - } - Line { - ZOrder 1124 - SrcBlock "w_H_l_sole" - SrcPort 1 - DstBlock " SFunction " - DstPort 12 - } - Line { - ZOrder 1125 - SrcBlock "w_H_r_sole" - SrcPort 1 - DstBlock " SFunction " - DstPort 13 - } - Line { - ZOrder 1126 - SrcBlock "JL" - SrcPort 1 - DstBlock " SFunction " - DstPort 14 - } - Line { - ZOrder 1127 - SrcBlock "JR" - SrcPort 1 - DstBlock " SFunction " - DstPort 15 - } - Line { - ZOrder 1128 - SrcBlock "dJLv" - SrcPort 1 - DstBlock " SFunction " - DstPort 16 - } - Line { - ZOrder 1129 - SrcBlock "dJRv" - SrcPort 1 - DstBlock " SFunction " - DstPort 17 - } - Line { - ZOrder 1130 - SrcBlock "xCoM" - SrcPort 1 - DstBlock " SFunction " - DstPort 18 - } - Line { - ZOrder 1131 - SrcBlock "J_CoM" - SrcPort 1 - DstBlock " SFunction " - DstPort 19 - } - Line { - ZOrder 1132 - SrcBlock "desired_x_dx_ddx_CoM" - SrcPort 1 - DstBlock " SFunction " - DstPort 20 - } - Line { - ZOrder 1133 - SrcBlock "gainsPCOM" - SrcPort 1 - DstBlock " SFunction " - DstPort 21 - } - Line { - ZOrder 1134 - SrcBlock "gainsDCOM" - SrcPort 1 - DstBlock " SFunction " - DstPort 22 - } - Line { - ZOrder 1135 - SrcBlock "impedances" - SrcPort 1 - DstBlock " SFunction " - DstPort 23 - } - Line { - Name "tauModel" - ZOrder 1136 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "tauModel" - DstPort 1 - } - Line { - Name "Sigma" - ZOrder 1137 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 3 - DstBlock "Sigma" - DstPort 1 - } - Line { - Name "NA" - ZOrder 1138 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 4 - DstBlock "NA" - DstPort 1 - } - Line { - Name "fLdotDesC1C2" - ZOrder 1139 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 5 - DstBlock "fLdotDesC1C2" - DstPort 1 - } - Line { - Name "HessianMatrixQP1Foot" - ZOrder 1140 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 6 - DstBlock "HessianMatrixQP1Foot" - DstPort 1 - } - Line { - Name "gradientQP1Foot" - ZOrder 1141 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 7 - DstBlock "gradientQP1Foot" - DstPort 1 - } - Line { - Name "ConstraintsMatrixQP1Foot" - ZOrder 1142 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 8 - DstBlock "ConstraintsMatrixQP1Foot" - DstPort 1 - } - Line { - Name "bVectorConstraintsQp1Foot" - ZOrder 1143 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 9 - DstBlock "bVectorConstraintsQp1Foot" - DstPort 1 - } - Line { - Name "HessianMatrixQP2Feet" - ZOrder 1144 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 10 - DstBlock "HessianMatrixQP2Feet" - DstPort 1 - } - Line { - Name "gradientQP2Feet" - ZOrder 1145 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 11 - DstBlock "gradientQP2Feet" - DstPort 1 - } - Line { - Name "ConstraintsMatrixQP2Feet" - ZOrder 1146 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 12 - DstBlock "ConstraintsMatrixQP2Feet" - DstPort 1 - } - Line { - Name "bVectorConstraintsQp2Feet" - ZOrder 1147 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 13 - DstBlock "bVectorConstraintsQp2Feet" - DstPort 1 - } - Line { - Name "errorCoM" - ZOrder 1148 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 14 - DstBlock "errorCoM" - DstPort 1 - } - Line { - Name "f_noQP" - ZOrder 1149 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 15 - DstBlock "f_noQP" - DstPort 1 - } - Line { - ZOrder 1150 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 1151 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Compute angular momentum integral" - SID "3714" - Ports [4, 1] - Position [1195, 552, 1305, 588] - ZOrder 835 - RequestExecContextInheritance off - System { - Name "Compute angular momentum integral" - 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 "257" - Block { - BlockType Inport - Name "qjDes" - SID "3715" - Position [35, 23, 65, 37] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "qj" - SID "3716" - Position [390, 228, 420, 242] - ZOrder 1 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "constraints" - SID "3717" - Position [715, 133, 745, 147] - ZOrder 80 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4028" - Position [35, -137, 65, -123] - ZOrder 732 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "Centroidal Momentum" - SID "3718" - Ports [4, 1] - Position [1130, -184, 1280, 254] - ZOrder 73 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Dynamics/Centroidal Momentum" - SourceType "Centroidal Momentum" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "Compute base to fixed link transform" - SID "4422" - Ports [2, 2] - Position [185, -76, 345, -34] - ZOrder 901 - NamePlacement "alternate" - 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 "556" - Block { - BlockType Inport - Name "qj" - SID "4423" - Position [50, 38, 80, 52] - ZOrder 902 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4424" - Position [230, 183, 260, 197] - ZOrder 903 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant7" - SID "4425" - Position [100, 22, 130, 38] - ZOrder 895 - ShowName off - Value "eye(4)" - } - Block { - BlockType SubSystem - Name "LFoot to base link transform " - SID "4426" - Ports [4, 1] - Position [435, 15, 580, 55] - ZOrder 893 - RequestExecContextInheritance off - System { - Name "LFoot to base 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 "248" - Block { - BlockType Inport - Name "qj" - SID "4427" - Position [470, 227, 495, 243] - ZOrder 736 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4428" - Position [1000, 212, 1030, 228] - ZOrder 741 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_link" - SID "4429" - Position [655, 132, 680, 148] - ZOrder 744 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_base_fixed" - SID "4430" - Position [470, 32, 495, 48] - ZOrder 745 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "Fixed base to imu transform" - SID "4431" - Ports [2, 1] - Position [575, 25, 740, 85] - ZOrder 729 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.IMU" - } - Block { - BlockType Reference - Name "Fixed base to root link transform" - SID "4432" - Ports [2, 1] - Position [575, 190, 740, 250] - ZOrder 737 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.BASE" - } - Block { - BlockType Product - Name "Inv\\*" - SID "4433" - Ports [2, 1] - Position [825, 40, 895, 95] - ZOrder 731 - 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\\* " - SID "4434" - Ports [2, 1] - Position [825, 184, 895, 231] - ZOrder 738 - 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 "4435" - Ports [0, 1] - Position [980, 308, 1055, 332] - ZOrder 742 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.NECK_POS" - signalSize "Ports.NECK_POS_PORT_SIZE" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - Block { - BlockType Switch - Name "Switch6" - SID "4436" - Position [1760, 151, 1840, 419] - ZOrder 734 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE" - SID "4437" - Position [1570, 275, 1725, 295] - ZOrder 735 - ShowName off - Value "Config.USE_IMU4EST_BASE" - } - Block { - BlockType SubSystem - Name "fromImuToHomogeousTransformFCN" - SID "4438" - Ports [6, 1] - Position [1305, 47, 1540, 343] - ZOrder 733 - ShowName off - LibraryVersion "1.30" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "fromImuToHomogeousTransformFCN" - 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 "3785" - SIDPrevWatermark "9" - Block { - BlockType Inport - Name "imu_H_link" - SID "4438::112" - Position [20, 101, 40, 119] - ZOrder 82 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "imu_H_link_0" - SID "4438::113" - Position [20, 136, 40, 154] - ZOrder 83 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "link_H_base" - SID "4438::111" - Position [20, 171, 40, 189] - ZOrder 81 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial_0" - SID "4438::115" - Position [20, 206, 40, 224] - ZOrder 85 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4438::93" - Position [20, 246, 40, 264] - ZOrder 69 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "neck_pos" - SID "4438::118" - Position [20, 281, 40, 299] - ZOrder 86 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4438::3784" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 184 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4438::3783" - Tag "Stateflow S-Function torqueBalancingYoga 1" - Ports [6, 2] - Position [180, 102, 230, 243] - ZOrder 183 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[6 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "w_H_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4438::3785" - Position [460, 241, 480, 259] - ZOrder 185 - } - Block { - BlockType Outport - Name "w_H_b" - SID "4438::106" - Position [460, 101, 480, 119] - ZOrder 80 - IconDisplay "Port number" - } - Line { - ZOrder 181 - SrcBlock "imu_H_link" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 182 - SrcBlock "imu_H_link_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 183 - SrcBlock "link_H_base" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 184 - SrcBlock "inertial_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 185 - SrcBlock "inertial" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 186 - SrcBlock "neck_pos" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - Name "w_H_b" - ZOrder 187 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 188 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 189 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "holder\n1" - SID "4439" - Ports [1, 1] - Position [1175, 113, 1225, 127] - ZOrder 739 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "holder\n2" - SID "4440" - Ports [1, 1] - Position [1175, 213, 1225, 227] - ZOrder 740 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "neck transform" - SID "4441" - Ports [1, 1] - Position [1135, 305, 1260, 335] - ZOrder 743 - RequestExecContextInheritance off - System { - Name "neck transform" - Location [134, 55, 3840, 2160] - Open off - PortBlocksUseCompactNotation off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "843" - Block { - BlockType Inport - Name "neck port" - SID "4442" - Position [45, 18, 75, 32] - ZOrder 725 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant" - SID "4443" - Position [95, 85, 170, 105] - ZOrder 720 - ShowName off - Value "zeros(3,1)" - } - Block { - BlockType Gain - Name "Gain" - SID "4444" - 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 "4445" - 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 "4446" - Position [300, 7, 375, 113] - ZOrder 722 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE1" - SID "4447" - Position [60, 50, 210, 70] - ZOrder 719 - ShowName off - Value "Config.CORRECT_NECK_IMU" - } - Block { - BlockType Outport - Name "neck position" - SID "4448" - Position [425, 53, 455, 67] - ZOrder 726 - IconDisplay "Port number" - } - 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 "4449" - Position [1875, 278, 1905, 292] - ZOrder 732 - IconDisplay "Port number" - } - 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\\* " - SrcPort 1 - Points [65, 0] - Branch { - ZOrder 3 - Points [0, 165] - DstBlock "Switch6" - DstPort 3 - } - Branch { - ZOrder 4 - Points [0, -40] - DstBlock "fromImuToHomogeousTransformFCN" - 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 "fromImuToHomogeousTransformFCN" - DstPort 1 - } - Branch { - ZOrder 8 - Points [0, 50] - DstBlock "holder\n1" - DstPort 1 - } - } - Line { - ZOrder 9 - SrcBlock "holder\n2" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 4 - } - Line { - ZOrder 10 - SrcBlock "Fixed base to imu transform" - SrcPort 1 - DstBlock "Inv\\*" - DstPort 1 - } - Line { - ZOrder 11 - SrcBlock "holder\n1" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 2 - } - Line { - ZOrder 12 - SrcBlock "qj" - 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 "fromImuToHomogeousTransformFCN" - SrcPort 1 - DstBlock "Switch6" - DstPort 1 - } - Line { - ZOrder 16 - SrcBlock "Fixed base to root link transform" - SrcPort 1 - DstBlock "Inv\\* " - 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 "fromImuToHomogeousTransformFCN" - DstPort 5 - } - } - Line { - ZOrder 20 - SrcBlock "Neck Position" - SrcPort 1 - DstBlock "neck transform" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "neck transform" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 6 - } - Line { - ZOrder 22 - SrcBlock "w_H_link" - SrcPort 1 - Points [98, 0] - Branch { - ZOrder 23 - Points [0, 55] - DstBlock "Inv\\* " - 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 - } - } - } - } - Block { - BlockType Reference - Name "LFoot to world transform (fixed base)" - SID "4450" - Ports [2, 1] - Position [165, 22, 330, 53] - ZOrder 894 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType SubSystem - Name "RFoot to base link transform" - SID "4451" - Ports [4, 1] - Position [435, 100, 580, 140] - ZOrder 896 - RequestExecContextInheritance off - System { - Name "RFoot to base 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 "248" - Block { - BlockType Inport - Name "qj" - SID "4452" - Position [470, 227, 495, 243] - ZOrder 736 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4453" - Position [1000, 212, 1030, 228] - ZOrder 741 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_link" - SID "4454" - Position [655, 132, 680, 148] - ZOrder 744 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "w_H_base_fixed" - SID "4455" - Position [470, 32, 495, 48] - ZOrder 745 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Reference - Name "Fixed base to imu transform" - SID "4456" - Ports [2, 1] - Position [575, 25, 740, 85] - ZOrder 729 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.IMU" - } - Block { - BlockType Reference - Name "Fixed base to root link transform" - SID "4457" - Ports [2, 1] - Position [575, 190, 740, 250] - ZOrder 737 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.BASE" - } - Block { - BlockType Product - Name "Inv\\*" - SID "4458" - Ports [2, 1] - Position [825, 40, 895, 95] - ZOrder 731 - 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\\* " - SID "4459" - Ports [2, 1] - Position [825, 184, 895, 231] - ZOrder 738 - 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 "4460" - Ports [0, 1] - Position [980, 308, 1055, 332] - ZOrder 742 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.NECK_POS" - signalSize "Ports.NECK_POS_PORT_SIZE" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection off - } - Block { - BlockType Switch - Name "Switch6" - SID "4461" - Position [1760, 151, 1840, 419] - ZOrder 734 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE" - SID "4462" - Position [1570, 275, 1725, 295] - ZOrder 735 - ShowName off - Value "Config.USE_IMU4EST_BASE" - } - Block { - BlockType SubSystem - Name "fromImuToHomogeousTransformFCN" - SID "4463" - Ports [6, 1] - Position [1305, 47, 1540, 343] - ZOrder 733 - ShowName off - LibraryVersion "1.30" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "fromImuToHomogeousTransformFCN" - 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 "3785" - SIDPrevWatermark "9" - Block { - BlockType Inport - Name "imu_H_link" - SID "4463::112" - Position [20, 101, 40, 119] - ZOrder 82 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "imu_H_link_0" - SID "4463::113" - Position [20, 136, 40, 154] - ZOrder 83 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "link_H_base" - SID "4463::111" - Position [20, 171, 40, 189] - ZOrder 81 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial_0" - SID "4463::115" - Position [20, 206, 40, 224] - ZOrder 85 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "inertial" - SID "4463::93" - Position [20, 246, 40, 264] - ZOrder 69 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "neck_pos" - SID "4463::118" - Position [20, 281, 40, 299] - ZOrder 86 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4463::3784" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 184 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4463::3783" - Tag "Stateflow S-Function torqueBalancingYoga 4" - Ports [6, 2] - Position [180, 102, 230, 243] - ZOrder 183 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[6 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "w_H_b" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4463::3785" - Position [460, 241, 480, 259] - ZOrder 185 - } - Block { - BlockType Outport - Name "w_H_b" - SID "4463::106" - Position [460, 101, 480, 119] - ZOrder 80 - IconDisplay "Port number" - } - Line { - ZOrder 181 - SrcBlock "imu_H_link" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 182 - SrcBlock "imu_H_link_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 183 - SrcBlock "link_H_base" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 184 - SrcBlock "inertial_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - ZOrder 185 - SrcBlock "inertial" - SrcPort 1 - DstBlock " SFunction " - DstPort 5 - } - Line { - ZOrder 186 - SrcBlock "neck_pos" - SrcPort 1 - DstBlock " SFunction " - DstPort 6 - } - Line { - Name "w_H_b" - ZOrder 187 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "w_H_b" - DstPort 1 - } - Line { - ZOrder 188 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 189 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "holder\n1" - SID "4464" - Ports [1, 1] - Position [1175, 113, 1225, 127] - ZOrder 739 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Reference - Name "holder\n2" - SID "4465" - Ports [1, 1] - Position [1175, 213, 1225, 227] - ZOrder 740 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "neck transform" - SID "4466" - Ports [1, 1] - Position [1135, 305, 1260, 335] - ZOrder 743 - RequestExecContextInheritance off - System { - Name "neck transform" - Location [134, 55, 3840, 2160] - Open off - PortBlocksUseCompactNotation off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "843" - Block { - BlockType Inport - Name "neck port" - SID "4467" - Position [45, 18, 75, 32] - ZOrder 725 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant" - SID "4468" - Position [95, 85, 170, 105] - ZOrder 720 - ShowName off - Value "zeros(3,1)" - } - Block { - BlockType Gain - Name "Gain" - SID "4469" - 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 "4470" - 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 "4471" - Position [300, 7, 375, 113] - ZOrder 722 - ShowName off - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Constant - Name "USE_IMU4EST_BASE1" - SID "4472" - Position [60, 50, 210, 70] - ZOrder 719 - ShowName off - Value "Config.CORRECT_NECK_IMU" - } - Block { - BlockType Outport - Name "neck position" - SID "4473" - Position [425, 53, 455, 67] - ZOrder 726 - IconDisplay "Port number" - } - 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 "4474" - Position [1875, 278, 1905, 292] - ZOrder 732 - IconDisplay "Port number" - } - 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\\* " - SrcPort 1 - Points [65, 0] - Branch { - ZOrder 3 - Points [0, 165] - DstBlock "Switch6" - DstPort 3 - } - Branch { - ZOrder 4 - Points [0, -40] - DstBlock "fromImuToHomogeousTransformFCN" - 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 "fromImuToHomogeousTransformFCN" - DstPort 1 - } - Branch { - ZOrder 8 - Points [0, 50] - DstBlock "holder\n1" - DstPort 1 - } - } - Line { - ZOrder 9 - SrcBlock "holder\n2" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 4 - } - Line { - ZOrder 10 - SrcBlock "Fixed base to imu transform" - SrcPort 1 - DstBlock "Inv\\*" - DstPort 1 - } - Line { - ZOrder 11 - SrcBlock "holder\n1" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 2 - } - Line { - ZOrder 12 - SrcBlock "qj" - 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 "fromImuToHomogeousTransformFCN" - SrcPort 1 - DstBlock "Switch6" - DstPort 1 - } - Line { - ZOrder 16 - SrcBlock "Fixed base to root link transform" - SrcPort 1 - DstBlock "Inv\\* " - 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 "fromImuToHomogeousTransformFCN" - DstPort 5 - } - } - Line { - ZOrder 20 - SrcBlock "Neck Position" - SrcPort 1 - DstBlock "neck transform" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "neck transform" - SrcPort 1 - DstBlock "fromImuToHomogeousTransformFCN" - DstPort 6 - } - Line { - ZOrder 22 - SrcBlock "w_H_link" - SrcPort 1 - Points [98, 0] - Branch { - ZOrder 23 - Points [0, 55] - DstBlock "Inv\\* " - 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 - } - } - } - } - Block { - BlockType Reference - Name "RFoot to world transform (fixed base)" - SID "4475" - Ports [2, 1] - Position [165, 107, 330, 138] - ZOrder 899 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType Outport - Name "fixed_LFoot_H_b" - SID "4476" - Position [650, 28, 680, 42] - ZOrder 910 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "fixed_RFoot_H_b" - SID "4477" - Position [650, 113, 680, 127] - ZOrder 912 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "Constant7" - SrcPort 1 - Points [10, 0] - Branch { - ZOrder 2 - Points [0, -22; 255, 0; 0, 42] - Branch { - ZOrder 3 - 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 "RFoot to world transform (fixed base)" - DstPort 1 - } - Branch { - ZOrder 6 - DstBlock "LFoot to world transform (fixed base)" - DstPort 1 - } - } - Line { - ZOrder 7 - SrcBlock "RFoot to base link transform" - SrcPort 1 - DstBlock "fixed_RFoot_H_b" - DstPort 1 - } - Line { - ZOrder 8 - SrcBlock "LFoot to base link transform " - SrcPort 1 - DstBlock "fixed_LFoot_H_b" - DstPort 1 - } - Line { - ZOrder 9 - SrcBlock "LFoot to world transform (fixed base)" - SrcPort 1 - DstBlock "LFoot to base link transform " - DstPort 3 - } - Line { - ZOrder 10 - SrcBlock "RFoot to world transform (fixed base)" - SrcPort 1 - DstBlock "RFoot to base link transform" - DstPort 3 - } - Line { - ZOrder 11 - SrcBlock "qj" - SrcPort 1 - Points [51, 0] - Branch { - ZOrder 12 - Points [0, 85] - Branch { - ZOrder 13 - Points [0, 37; 274, 0; 0, -62] - Branch { - ZOrder 14 - Points [0, -85] - DstBlock "LFoot to base link transform " - DstPort 1 - } - Branch { - ZOrder 15 - DstBlock "RFoot to base link transform" - DstPort 1 - } - } - Branch { - ZOrder 16 - DstBlock "RFoot to world transform (fixed base)" - DstPort 2 - } - } - Branch { - ZOrder 17 - DstBlock "LFoot to world transform (fixed base)" - DstPort 2 - } - } - Line { - ZOrder 18 - SrcBlock "inertial" - SrcPort 1 - Points [119, 0; 0, -75] - Branch { - ZOrder 19 - Points [0, -85] - DstBlock "LFoot to base link transform " - DstPort 2 - } - Branch { - ZOrder 20 - DstBlock "RFoot to base link transform" - DstPort 2 - } - } - } - } - Block { - BlockType Reference - Name "Jacobian" - SID "3719" - Ports [2, 1] - Position [645, 59, 810, 81] - ZOrder 40 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.LEFT_FOOT" - } - Block { - BlockType Reference - Name "Jacobian1" - SID "3720" - Ports [2, 1] - Position [645, 94, 810, 116] - ZOrder 79 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Jacobians/Jacobian" - SourceType "Jacobian" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - frameName "Frames.RIGHT_FOOT" - } - Block { - BlockType SubSystem - Name "References for L" - SID "3721" - Ports [4, 1] - Position [840, 21, 1095, 154] - ZOrder 75 - LibraryVersion "1.32" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "References for L" - 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 "3771" - Block { - BlockType Inport - Name "qjErr" - SID "3721::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "JL" - SID "3721::23" - Position [20, 136, 40, 154] - ZOrder 9 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "JR" - SID "3721::3701" - Position [20, 171, 40, 189] - ZOrder 26 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "activeFeetConstraints" - SID "3721::3702" - Position [20, 206, 40, 224] - ZOrder 27 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "3721::3770" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 95 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "3721::3769" - Tag "Stateflow S-Function torqueBalancingYoga 20" - Ports [4, 2] - Position [180, 102, 230, 203] - ZOrder 94 - FunctionName "sf_sfun" - Parameters "Reg" - PortCounts "[4 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "nu_b_equivalent" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "3721::3771" - Position [460, 241, 480, 259] - ZOrder 96 - } - Block { - BlockType Outport - Name "nu_b_equivalent" - SID "3721::24" - Position [460, 101, 480, 119] - ZOrder 10 - IconDisplay "Port number" - } - Line { - ZOrder 141 - SrcBlock "qjErr" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 142 - SrcBlock "JL" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 143 - SrcBlock "JR" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 144 - SrcBlock "activeFeetConstraints" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - Name "nu_b_equivalent" - ZOrder 145 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "nu_b_equivalent" - DstPort 1 - } - Line { - ZOrder 146 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 147 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Selector - Name "Selector" - SID "3722" - Ports [1, 1] - Position [1325, 16, 1365, 54] - ZOrder 78 - ShowName off - InputPortWidth "6" - IndexOptions "Index vector (dialog)" - Indices "[4 5 6]" - OutputSizes "1" - } - Block { - BlockType Sum - Name "Sum" - SID "3723" - Ports [2, 1] - Position [500, 190, 520, 210] - ZOrder 77 - ShowName off - IconShape "round" - Inputs "|-+" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch" - SID "4029" - Position [460, -166, 520, -94] - ZOrder 733 - Criteria "u2 > Threshold" - Threshold "0.1" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType SubSystem - Name "choose base to world transform" - SID "4027" - Ports [1, 1] - Position [155, -149, 370, -111] - ZOrder 731 - NamePlacement "alternate" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "choose 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 "84" - 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::83" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 74 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4027::82" - Tag "Stateflow S-Function torqueBalancingYoga 19" - Ports [1, 2] - Position [180, 100, 230, 160] - ZOrder 73 - FunctionName "sf_sfun" - PortCounts "[1 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "booleanState" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4027::84" - Position [460, 241, 480, 259] - ZOrder 75 - } - Block { - BlockType Outport - Name "booleanState" - SID "4027::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 81 - SrcBlock "state" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - Name "booleanState" - ZOrder 82 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "booleanState" - DstPort 1 - } - Line { - ZOrder 83 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 84 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "inertial" - SID "3724" - Ports [0, 1] - Position [25, -52, 75, -38] - ZOrder 727 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" - SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - portName "Ports.IMU" - signalSize "12" - timeout "0.5" - blocking off - timestamp off - autoconnect on - errorOnConnection on - } - Block { - BlockType Outport - Name "intLw" - SID "3753" - Position [1410, 28, 1440, 42] - ZOrder -2 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "qjDes" - SrcPort 1 - Points [58, 0] - Branch { - ZOrder 92 - Points [0, 45] - Branch { - ZOrder 93 - DstBlock "Jacobian" - DstPort 2 - } - Branch { - ZOrder 87 - Points [0, 35] - Branch { - ZOrder 94 - Points [0, 90] - DstBlock "Sum" - DstPort 1 - } - Branch { - ZOrder 8 - DstBlock "Jacobian1" - DstPort 2 - } - } - } - Branch { - ZOrder 10 - Points [0, -50] - Branch { - ZOrder 91 - DstBlock "Centroidal Momentum" - DstPort 2 - } - Branch { - ZOrder 90 - Points [0, -45] - DstBlock "Compute base to fixed link transform" - DstPort 1 - } - } - } - Line { - ZOrder 12 - SrcBlock "qj" - SrcPort 1 - Points [85, 0] - DstBlock "Sum" - DstPort 2 - } - Line { - ZOrder 13 - SrcBlock "Sum" - SrcPort 1 - Points [86, 0] - Branch { - ZOrder 59 - DstBlock "Centroidal Momentum" - DstPort 4 - } - Branch { - ZOrder 35 - Points [0, -165] - DstBlock "References for L" - DstPort 1 - } - } - Line { - ZOrder 16 - SrcBlock "References for L" - SrcPort 1 - DstBlock "Centroidal Momentum" - DstPort 3 - } - Line { - ZOrder 17 - SrcBlock "Jacobian" - SrcPort 1 - DstBlock "References for L" - DstPort 2 - } - Line { - ZOrder 18 - SrcBlock "Centroidal Momentum" - SrcPort 1 - DstBlock "Selector" - DstPort 1 - } - Line { - ZOrder 19 - SrcBlock "Selector" - SrcPort 1 - DstBlock "intLw" - DstPort 1 - } - Line { - ZOrder 20 - SrcBlock "Jacobian1" - SrcPort 1 - DstBlock "References for L" - DstPort 3 - } - Line { - ZOrder 21 - SrcBlock "constraints" - SrcPort 1 - DstBlock "References for L" - DstPort 4 - } - Line { - ZOrder 25 - SrcBlock "state" - SrcPort 1 - DstBlock "choose base to world transform" - DstPort 1 - } - Line { - ZOrder 27 - SrcBlock "choose base to world transform" - SrcPort 1 - DstBlock "Switch" - DstPort 2 - } - Line { - ZOrder 28 - SrcBlock "Switch" - SrcPort 1 - Points [44, 0] - Branch { - ZOrder 85 - DstBlock "Centroidal Momentum" - DstPort 1 - } - Branch { - ZOrder 84 - Points [0, 195] - Branch { - ZOrder 63 - DstBlock "Jacobian" - DstPort 1 - } - Branch { - ZOrder 60 - Points [0, 35] - DstBlock "Jacobian1" - DstPort 1 - } - } - } - Line { - ZOrder 81 - SrcBlock "inertial" - SrcPort 1 - DstBlock "Compute base to fixed link transform" - DstPort 2 - } - Line { - ZOrder 82 - SrcBlock "Compute base to fixed link transform" - SrcPort 1 - Points [79, 0; 0, -90] - DstBlock "Switch" - DstPort 1 - } - Line { - ZOrder 83 - SrcBlock "Compute base to fixed link transform" - SrcPort 2 - Points [90, 0; 0, -60] - DstBlock "Switch" - DstPort 3 - } - } - } - Block { - BlockType SubSystem - Name "Compute joint torques" - SID "4393" - Ports [14, 2] - Position [1840, 29, 2050, 1056] - ZOrder 846 - RequestExecContextInheritance off - System { - Name "Compute joint torques" - 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 "279" - Block { - BlockType Inport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "4410" - Position [430, 499, 460, 511] - ZOrder 640 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "tauModel" - SID "4395" - Position [875, 313, 905, 327] - ZOrder 412 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Sigma" - SID "4396" - Position [875, 408, 905, 422] - ZOrder 413 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "NA" - SID "4397" - Position [880, 713, 910, 727] - ZOrder 414 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "fLDes" - SID "4420" - Position [875, 518, 905, 532] - ZOrder 650 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "HessianMatrixQP1Foot" - SID "4411" - Position [430, 533, 460, 547] - ZOrder 641 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gradientQP1Foot" - SID "4412" - Position [430, 568, 460, 582] - ZOrder 642 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ConstraintsMatrixQP1Foot" - SID "4413" - Position [430, 603, 460, 617] - ZOrder 647 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "bVectorConstraintsQP1Foot" - SID "4414" - Position [430, 638, 460, 652] - ZOrder 648 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "HessianMatrixQP2Feet" - SID "4415" - Position [430, 673, 460, 687] - ZOrder 643 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gradientQP2Feet" - SID "4416" - Position [430, 708, 460, 722] - ZOrder 644 - Port "11" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ConstraintsMatrixQP2Feet" - SID "4417" - Position [430, 743, 460, 757] - ZOrder 645 - Port "12" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "bVectorConstraintsQp2Feet" - SID "4418" - Position [430, 778, 460, 792] - ZOrder 646 - Port "13" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4398" - Position [430, 813, 460, 827] - ZOrder 415 - Port "14" - IconDisplay "Port number" - } - Block { - BlockType Sum - Name "Add" - SID "2367" - Ports [2, 1] - Position [1510, 266, 1590, 484] - 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 [1255, 503, 1325, 587] - 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 [1405, 399, 1470, 461] - 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 [975, 704, 1080, 771] - 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 [10, 2] - Position [580, 478, 815, 847] - ZOrder 652 - RequestExecContextInheritance off - System { - Name "QPSolver" - 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 "313" - Block { - BlockType Inport - Name "LEFT_RIGHT_FOOT_IN_CONTACT" - SID "4580" - Position [-445, -6, -415, 6] - ZOrder 394 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "HessianMatrixQP1Foot" - SID "4581" - Position [-445, 63, -415, 77] - ZOrder 395 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gradientQP1Foot" - SID "4582" - Position [-445, 98, -415, 112] - ZOrder 396 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ConstraintsMatrixQP1Foot" - SID "4583" - Position [-445, 133, -415, 147] - ZOrder 403 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "bVectorConstraintsQP1Foot" - SID "4584" - Position [-445, 168, -415, 182] - ZOrder 404 - Port "5" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "HessianMatrixQP2Feet" - SID "4585" - Position [-445, -192, -415, -178] - ZOrder 397 - Port "6" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "gradientQP2Feet" - SID "4586" - Position [-445, -157, -415, -143] - ZOrder 398 - Port "7" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ConstraintsMatrixQP2Feet" - SID "4587" - Position [-445, -122, -415, -108] - ZOrder 399 - Port "8" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "bVectorConstraintsQp2Feet" - SID "4588" - Position [-445, -87, -415, -73] - ZOrder 400 - Port "9" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "state" - SID "4589" - Position [390, 38, 420, 52] - ZOrder 553 - Port "10" - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "ContactsTransition" - SID "4590" - Ports [1, 1] - Position [-250, -27, 10, 27] - ZOrder 721 - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "ContactsTransition" - 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 "54" - Block { - BlockType Inport - Name "LR_FootInContact" - SID "4590::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4590::53" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 44 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4590::52" - Tag "Stateflow S-Function torqueBalancingYoga 15" - Ports [1, 2] - Position [180, 100, 230, 160] - ZOrder 43 - FunctionName "sf_sfun" - PortCounts "[1 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "onOneFoot" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4590::54" - Position [460, 241, 480, 259] - ZOrder 45 - } - Block { - BlockType Outport - Name "onOneFoot" - SID "4590::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 37 - SrcBlock "LR_FootInContact" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - Name "onOneFoot" - ZOrder 38 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "onOneFoot" - 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 Logic - Name "Logical\nOperator1" - SID "4591" - Ports [2, 1] - Position [530, -68, 560, -37] - ZOrder 693 - ShowName off - Operator "OR" - AllPortsSameDT off - OutDataTypeStr "boolean" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n3" - SID "4592" - Position [395, -66, 500, -54] - ZOrder 692 - ShowName off - Value "Config.SCOPES_ALL" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n4" - SID "4593" - Position [390, -51, 505, -39] - ZOrder 694 - ShowName off - Value "Config.SCOPES_QP" - } - Block { - BlockType SubSystem - Name "One Foot" - SID "4594" - Ports [4, 2, 1] - Position [55, 51, 150, 194] - ZOrder 722 - TreatAsAtomicUnit on - RequestExecContextInheritance off - System { - Name "One Foot" - 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 "445" - Block { - BlockType Inport - Name "H" - SID "4595" - Position [105, -22, 135, -8] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "g" - SID "4596" - Position [105, 8, 135, 22] - ZOrder 698 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "A" - SID "4597" - Position [105, 38, 135, 52] - ZOrder 699 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ubA" - SID "4598" - Position [105, 68, 135, 82] - ZOrder 700 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "4599" - Ports [] - Position [265, -100, 285, -80] - ZOrder 704 - } - Block { - BlockType SubSystem - Name "Analytical Solution One Foot (unconstrained)" - SID "4600" - Ports [2, 1] - Position [330, -200, 510, -100] - ZOrder 734 - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "Analytical Solution 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 "55" - Block { - BlockType Inport - Name "H" - SID "4600::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "g" - SID "4600::22" - Position [20, 136, 40, 154] - ZOrder 13 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4600::54" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 45 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4600::53" - Tag "Stateflow S-Function torqueBalancingYoga 22" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 44 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "analyticalSolution" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4600::55" - Position [460, 241, 480, 259] - ZOrder 46 - } - Block { - BlockType Outport - Name "analyticalSolution" - SID "4600::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 46 - SrcBlock "H" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 47 - SrcBlock "g" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "analyticalSolution" - ZOrder 48 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "analyticalSolution" - DstPort 1 - } - Line { - ZOrder 49 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 50 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "Match Signal Sizes1" - SID "4601" - Ports [2, 1] - Position [390, -61, 475, 21] - ZOrder 712 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Match Signal Sizes" - SourceType "Match Signal Sizes" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "Process QP output" - SID "4602" - Ports [3, 1] - Position [555, -212, 690, 172] - ZOrder 737 - 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 "62" - Block { - BlockType Inport - Name "analyticalSolution" - SID "4602::29" - Position [20, 101, 40, 119] - ZOrder 20 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "primalSolution" - SID "4602::1" - Position [20, 136, 40, 154] - ZOrder -1 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "QPStatus" - SID "4602::22" - Position [20, 171, 40, 189] - ZOrder 13 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4602::61" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 46 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4602::60" - Tag "Stateflow S-Function torqueBalancingYoga 12" - Ports [3, 2] - Position [180, 100, 230, 180] - ZOrder 45 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[3 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "f0" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4602::62" - Position [460, 241, 480, 259] - ZOrder 47 - } - Block { - BlockType Outport - Name "f0" - SID "4602::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 55 - SrcBlock "analyticalSolution" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 56 - SrcBlock "primalSolution" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 57 - SrcBlock "QPStatus" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - Name "f0" - ZOrder 58 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "f0" - DstPort 1 - } - Line { - ZOrder 59 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 60 - SrcBlock " SFunction " - SrcPort 1 - Points [20, 0] - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "QP Two Feet" - SID "4603" - Ports [4, 2] - Position [235, -29, 340, 89] - ZOrder 697 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/QP" - SourceType "QP" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - lbA off - ubA on - lb off - ub off - computeObjVal off - stopIfFails off - } - Block { - BlockType Outport - Name "QPStatus" - SID "4604" - Position [420, 53, 450, 67] - ZOrder 701 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "f0_1Foot" - SID "4605" - Position [730, -27, 760, -13] - ZOrder 738 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "H" - SrcPort 1 - Points [20, 0] - Branch { - ZOrder 2 - Points [0, -160] - DstBlock "Analytical Solution One Foot (unconstrained)" - DstPort 1 - } - Branch { - ZOrder 3 - DstBlock "QP Two Feet" - DstPort 1 - } - } - Line { - ZOrder 4 - SrcBlock "ubA" - SrcPort 1 - DstBlock "QP Two Feet" - DstPort 4 - } - Line { - ZOrder 5 - SrcBlock "A" - SrcPort 1 - DstBlock "QP Two Feet" - DstPort 3 - } - Line { - ZOrder 6 - SrcBlock "QP Two Feet" - SrcPort 2 - Points [40, 0] - Branch { - ZOrder 7 - Points [0, 50] - DstBlock "Process QP output" - DstPort 3 - } - Branch { - ZOrder 8 - DstBlock "QPStatus" - DstPort 1 - } - } - Line { - ZOrder 9 - SrcBlock "QP Two Feet" - SrcPort 1 - DstBlock "Match Signal Sizes1" - DstPort 2 - } - Line { - ZOrder 10 - SrcBlock "g" - SrcPort 1 - Points [51, 0] - Branch { - ZOrder 11 - Points [0, -55] - Branch { - ZOrder 12 - Points [0, -85] - DstBlock "Analytical Solution One Foot (unconstrained)" - DstPort 2 - } - Branch { - ZOrder 13 - DstBlock "Match Signal Sizes1" - DstPort 1 - } - } - Branch { - ZOrder 14 - DstBlock "QP Two Feet" - DstPort 2 - } - } - Line { - ZOrder 15 - SrcBlock "Match Signal Sizes1" - SrcPort 1 - DstBlock "Process QP output" - DstPort 2 - } - Line { - ZOrder 16 - SrcBlock "Analytical Solution One Foot (unconstrained)" - SrcPort 1 - DstBlock "Process QP output" - DstPort 1 - } - Line { - ZOrder 17 - SrcBlock "Process QP output" - SrcPort 1 - DstBlock "f0_1Foot" - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Process One Foot Output" - SID "4606" - Ports [2, 1] - Position [190, 134, 420, 241] - ZOrder 727 - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "Process One Foot 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 "55" - Block { - BlockType Inport - Name "primalSolution" - SID "4606::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "LR_FootInContact" - SID "4606::22" - Position [20, 136, 40, 154] - ZOrder 13 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4606::54" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 45 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4606::53" - Tag "Stateflow S-Function torqueBalancingYoga 16" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 44 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "f0_oneFoot" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4606::55" - Position [460, 241, 480, 259] - ZOrder 46 - } - Block { - BlockType Outport - Name "f0_oneFoot" - SID "4606::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 46 - SrcBlock "primalSolution" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 47 - SrcBlock "LR_FootInContact" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "f0_oneFoot" - ZOrder 48 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "f0_oneFoot" - DstPort 1 - } - Line { - ZOrder 49 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 50 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Switch - Name "Switch" - SID "4607" - Position [275, -33, 340, 33] - ZOrder 732 - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType SubSystem - Name "Two Feet" - SID "4608" - Ports [4, 2, 1] - Position [55, -199, 150, -66] - ZOrder 724 - NamePlacement "alternate" - TreatAsAtomicUnit on - RequestExecContextInheritance off - System { - Name "Two Feet" - 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 "450" - Block { - BlockType Inport - Name "H" - SID "4609" - Position [105, -27, 135, -13] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "g" - SID "4610" - Position [105, 3, 135, 17] - ZOrder 698 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "A" - SID "4611" - Position [105, 33, 135, 47] - ZOrder 699 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "ubA" - SID "4612" - Position [105, 63, 135, 77] - ZOrder 700 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "4613" - Ports [] - Position [265, -90, 285, -70] - ZOrder -2 - } - Block { - BlockType SubSystem - Name "Analytical Solution Two Feet (unconstrained)" - SID "4614" - Ports [2, 1] - Position [325, -200, 505, -100] - ZOrder 732 - 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 "55" - Block { - BlockType Inport - Name "H" - SID "4614::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "g" - SID "4614::22" - Position [20, 136, 40, 154] - ZOrder 13 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4614::54" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 45 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4614::53" - Tag "Stateflow S-Function torqueBalancingYoga 21" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 44 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "analyticalSolution" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4614::55" - Position [460, 241, 480, 259] - ZOrder 46 - } - Block { - BlockType Outport - Name "analyticalSolution" - SID "4614::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 46 - SrcBlock "H" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 47 - SrcBlock "g" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "analyticalSolution" - ZOrder 48 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "analyticalSolution" - DstPort 1 - } - Line { - ZOrder 49 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 50 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "Match Signal Sizes" - SID "4615" - Ports [2, 1] - Position [375, -66, 465, 16] - ZOrder 702 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Match Signal Sizes" - SourceType "Match Signal Sizes" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType SubSystem - Name "Process QP output" - SID "4616" - Ports [3, 1] - Position [545, -215, 680, 165] - ZOrder 734 - 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 "62" - Block { - BlockType Inport - Name "analyticalSolution" - SID "4616::29" - Position [20, 101, 40, 119] - ZOrder 20 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "primalSolution" - SID "4616::1" - Position [20, 136, 40, 154] - ZOrder -1 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "QPStatus" - SID "4616::22" - Position [20, 171, 40, 189] - ZOrder 13 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4616::61" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 46 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4616::60" - Tag "Stateflow S-Function torqueBalancingYoga 23" - Ports [3, 2] - Position [180, 100, 230, 180] - ZOrder 45 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[3 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "f0" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4616::62" - Position [460, 241, 480, 259] - ZOrder 47 - } - Block { - BlockType Outport - Name "f0" - SID "4616::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 55 - SrcBlock "analyticalSolution" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 56 - SrcBlock "primalSolution" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 57 - SrcBlock "QPStatus" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - Name "f0" - ZOrder 58 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "f0" - DstPort 1 - } - Line { - ZOrder 59 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 60 - SrcBlock " SFunction " - SrcPort 1 - Points [20, 0] - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Reference - Name "QP Two Feet" - SID "4617" - Ports [4, 2] - Position [225, -34, 330, 84] - ZOrder 697 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/QP" - SourceType "QP" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - lbA off - ubA on - lb off - ub off - computeObjVal off - stopIfFails off - } - Block { - BlockType Outport - Name "QPStatus" - SID "4618" - Position [405, 48, 435, 62] - ZOrder 701 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "f0_twoFeet" - SID "4619" - Position [715, -32, 745, -18] - ZOrder 735 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "H" - SrcPort 1 - Points [12, 0] - Branch { - ZOrder 2 - DstBlock "QP Two Feet" - DstPort 1 - } - Branch { - ZOrder 3 - Points [0, -155] - DstBlock "Analytical Solution Two Feet (unconstrained)" - DstPort 1 - } - } - Line { - ZOrder 4 - SrcBlock "ubA" - SrcPort 1 - DstBlock "QP Two Feet" - DstPort 4 - } - Line { - ZOrder 5 - SrcBlock "A" - SrcPort 1 - DstBlock "QP Two Feet" - DstPort 3 - } - Line { - ZOrder 6 - SrcBlock "g" - SrcPort 1 - Points [52, 0] - Branch { - ZOrder 7 - Points [0, -55] - Branch { - ZOrder 8 - Points [0, -80] - DstBlock "Analytical Solution Two Feet (unconstrained)" - DstPort 2 - } - Branch { - ZOrder 9 - DstBlock "Match Signal Sizes" - DstPort 1 - } - } - Branch { - ZOrder 10 - DstBlock "QP Two Feet" - DstPort 2 - } - } - Line { - ZOrder 11 - SrcBlock "QP Two Feet" - SrcPort 2 - Points [28, 0] - Branch { - ZOrder 12 - DstBlock "QPStatus" - DstPort 1 - } - Branch { - ZOrder 13 - Points [0, 45] - DstBlock "Process QP output" - DstPort 3 - } - } - Line { - ZOrder 14 - SrcBlock "QP Two Feet" - SrcPort 1 - DstBlock "Match Signal Sizes" - DstPort 2 - } - Line { - ZOrder 15 - SrcBlock "Match Signal Sizes" - SrcPort 1 - DstBlock "Process QP output" - DstPort 2 - } - Line { - ZOrder 16 - SrcBlock "Analytical Solution Two Feet (unconstrained)" - SrcPort 1 - DstBlock "Process QP output" - DstPort 1 - } - Line { - ZOrder 17 - SrcBlock "Process QP output" - SrcPort 1 - DstBlock "f0_twoFeet" - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Visualize eventual QP failures" - SID "4620" - Ports [2, 0, 1] - Position [500, -21, 650, 66] - ZOrder -17 - RequestExecContextInheritance off - System { - Name "Visualize eventual QP failures" - 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 "exitFlagQP" - SID "4621" - Position [100, 164, 125, 176] - ZOrder 409 - IconDisplay "Port number" - Port { - PortNumber 1 - Name "exitFlagQP" - } - } - 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 [322, 175, 342, 195] - ZOrder 540 - } - Block { - BlockType Scope - Name "QP status\n(0: no failure,\n1: not using)" - SID "4624" - Ports [2] - Position [230, 153, 275, 222] - ZOrder 408 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extm" - "gr.Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039])" - ",extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingV" - "ariableName','ScopeData1','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDe" - "cimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','" - "-2.25','MaxYLimReal','0.25','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','2.25','LegendVisibility','on','" - "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509" - "803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764" - "705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039" - "21569 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','-','Li" - "neWidth',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'),struc" - "t('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{" - "}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','0.25','MaxYLimReal','" - "7.75','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'Plot" - "MagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'Col" - "orOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.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','-','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('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle'," - "'-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{" - "[]}},'ShowContent',true,'Placement',2)},'DisplayPropertyDefaults',struct('YLabelReal','','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.4117" - "64705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450980" - "3921569 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'),str" - "uct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames'," - "{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'DisplayLayoutDimensions',[2 1]),extmgr.C" - "onfiguration('Tools','Plot Navigation',true,'OnceAtStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('T" - "ools','Measurements',true,'Version','2018a')),'Version','2018a','Location',[318 473 1511 1200])" - NumInputPorts "2" - Floating off - } - Line { - Name "exitFlagQP" - ZOrder 1 - Labels [-1, 0] - SrcBlock "exitFlagQP" - SrcPort 1 - DstBlock "QP status\n(0: no failure,\n1: not using)" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "state" - SrcPort 1 - DstBlock "QP status\n(0: no failure,\n1: not using)" - DstPort 2 - } - } - } - Block { - BlockType Logic - Name "not" - SID "4625" - Ports [1, 1] - Position [90, -62, 110, -27] - ZOrder 723 - BlockRotation 270 - NamePlacement "alternate" - Operator "NOT" - IconShape "distinctive" - AllPortsSameDT off - OutDataTypeStr "boolean" - } - Block { - BlockType Outport - Name "f0OneFoot" - SID "4626" - Position [490, 183, 520, 197] - ZOrder 401 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "f0TwoFeet" - SID "4627" - Position [295, -107, 325, -93] - ZOrder 728 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "Logical\nOperator1" - SrcPort 1 - Points [10, 0] - DstBlock "Visualize eventual QP failures" - DstPort enable - } - Line { - ZOrder 2 - SrcBlock "state" - SrcPort 1 - DstBlock "Visualize eventual QP failures" - DstPort 2 - } - Line { - ZOrder 3 - SrcBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - SrcPort 1 - Points [93, 0] - Branch { - ZOrder 4 - Points [0, 215] - DstBlock "Process One Foot Output" - DstPort 2 - } - Branch { - ZOrder 5 - DstBlock "ContactsTransition" - DstPort 1 - } - } - Line { - ZOrder 6 - SrcBlock "ON_GAZEBO\n3" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 1 - } - Line { - ZOrder 7 - SrcBlock "ON_GAZEBO\n4" - SrcPort 1 - DstBlock "Logical\nOperator1" - DstPort 2 - } - Line { - ZOrder 8 - SrcBlock "HessianMatrixQP1Foot" - SrcPort 1 - DstBlock "One Foot" - DstPort 1 - } - Line { - ZOrder 9 - SrcBlock "bVectorConstraintsQp2Feet" - SrcPort 1 - DstBlock "Two Feet" - DstPort 4 - } - Line { - ZOrder 10 - SrcBlock "gradientQP1Foot" - SrcPort 1 - DstBlock "One Foot" - DstPort 2 - } - Line { - ZOrder 11 - SrcBlock "ConstraintsMatrixQP1Foot" - SrcPort 1 - DstBlock "One Foot" - DstPort 3 - } - Line { - ZOrder 12 - SrcBlock "gradientQP2Feet" - SrcPort 1 - DstBlock "Two Feet" - DstPort 2 - } - Line { - ZOrder 13 - SrcBlock "ConstraintsMatrixQP2Feet" - SrcPort 1 - DstBlock "Two Feet" - DstPort 3 - } - Line { - ZOrder 14 - SrcBlock "not" - SrcPort 1 - DstBlock "Two Feet" - DstPort enable - } - Line { - ZOrder 15 - SrcBlock "ContactsTransition" - SrcPort 1 - Points [85, 0] - Branch { - ZOrder 16 - DstBlock "One Foot" - DstPort enable - } - Branch { - ZOrder 17 - DstBlock "Switch" - DstPort 2 - } - Branch { - ZOrder 18 - DstBlock "not" - DstPort 1 - } - } - Line { - ZOrder 19 - SrcBlock "bVectorConstraintsQP1Foot" - SrcPort 1 - DstBlock "One Foot" - DstPort 4 - } - Line { - ZOrder 20 - SrcBlock "HessianMatrixQP2Feet" - SrcPort 1 - DstBlock "Two Feet" - DstPort 1 - } - Line { - ZOrder 21 - SrcBlock "One Foot" - SrcPort 2 - DstBlock "Process One Foot Output" - DstPort 1 - } - Line { - ZOrder 22 - SrcBlock "Process One Foot Output" - SrcPort 1 - DstBlock "f0OneFoot" - DstPort 1 - } - Line { - ZOrder 23 - SrcBlock "One Foot" - SrcPort 1 - Points [18, 0; 0, -110] - DstBlock "Switch" - DstPort 1 - } - Line { - ZOrder 24 - SrcBlock "Two Feet" - SrcPort 1 - Points [37, 0; 0, 185] - DstBlock "Switch" - DstPort 3 - } - Line { - ZOrder 25 - SrcBlock "Switch" - SrcPort 1 - DstBlock "Visualize eventual QP failures" - DstPort 1 - } - Line { - ZOrder 26 - SrcBlock "Two Feet" - SrcPort 2 - DstBlock "f0TwoFeet" - DstPort 1 - } - } - } - Block { - BlockType Sum - Name "Sum" - SID "2402" - Ports [2, 1] - Position [1030, 515, 1050, 535] - ZOrder 395 - ShowName off - IconShape "round" - Inputs "|++" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Outport - Name "tau " - SID "4408" - Position [1635, 368, 1665, 382] - ZOrder 425 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "fDes_QP" - SID "4409" - Position [1440, 538, 1470, 552] - ZOrder 639 - Port "2" - IconDisplay "Port number" - } - Line { - ZOrder 293 - SrcBlock "NA" - SrcPort 1 - DstBlock "Product2" - DstPort 1 - } - Line { - ZOrder 354 - SrcBlock "QPSolver" - SrcPort 1 - Points [220, 0] - DstBlock "Sum" - DstPort 2 - } - 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 " - DstPort 1 - } - Line { - ZOrder 57 - SrcBlock "Add1" - SrcPort 1 - Points [32, 0] - Branch { - ZOrder 327 - DstBlock "fDes_QP" - DstPort 1 - } - Branch { - ZOrder 59 - Points [0, -100] - DstBlock "Product1" - DstPort 2 - } - } - Line { - ZOrder 65 - SrcBlock "Sum" - SrcPort 1 - DstBlock "Add1" - DstPort 1 - } - Line { - ZOrder 66 - SrcBlock "Product2" - SrcPort 1 - Points [89, 0; 0, -175] - DstBlock "Add1" - DstPort 2 - } - Line { - ZOrder 292 - SrcBlock "Sigma" - SrcPort 1 - DstBlock "Product1" - DstPort 1 - } - Line { - ZOrder 355 - SrcBlock "QPSolver" - SrcPort 2 - DstBlock "Product2" - DstPort 2 - } - Line { - ZOrder 346 - SrcBlock "gradientQP1Foot" - SrcPort 1 - DstBlock "QPSolver" - DstPort 3 - } - Line { - ZOrder 351 - SrcBlock "ConstraintsMatrixQP2Feet" - SrcPort 1 - DstBlock "QPSolver" - DstPort 8 - } - Line { - ZOrder 344 - SrcBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - SrcPort 1 - DstBlock "QPSolver" - DstPort 1 - } - Line { - ZOrder 348 - SrcBlock "bVectorConstraintsQP1Foot" - SrcPort 1 - DstBlock "QPSolver" - DstPort 5 - } - Line { - ZOrder 345 - SrcBlock "HessianMatrixQP1Foot" - SrcPort 1 - DstBlock "QPSolver" - DstPort 2 - } - Line { - ZOrder 347 - SrcBlock "ConstraintsMatrixQP1Foot" - SrcPort 1 - DstBlock "QPSolver" - DstPort 4 - } - Line { - ZOrder 350 - SrcBlock "gradientQP2Feet" - SrcPort 1 - DstBlock "QPSolver" - DstPort 7 - } - Line { - ZOrder 349 - SrcBlock "HessianMatrixQP2Feet" - SrcPort 1 - DstBlock "QPSolver" - DstPort 6 - } - Line { - ZOrder 352 - SrcBlock "bVectorConstraintsQp2Feet" - SrcPort 1 - DstBlock "QPSolver" - DstPort 9 - } - Line { - ZOrder 353 - SrcBlock "state" - SrcPort 1 - DstBlock "QPSolver" - DstPort 10 - } - Line { - ZOrder 326 - SrcBlock "fLDes" - SrcPort 1 - DstBlock "Sum" - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Compute joint torques (motor reflected inertia)" - SID "4519" - Ports [1, 1] - Position [2100, 256, 2255, 314] - ZOrder 862 - RequestExecContextInheritance off - System { - Name "Compute joint torques (motor reflected inertia)" - 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 "339" - Block { - BlockType Inport - Name "u" - SID "4520" - Position [-330, 3, -300, 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 [-550, -134, -285, -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 "86" - Block { - BlockType Demux - Name " Demux " - SID "4551::84" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 75 - Outputs "1" - } - Block { - BlockType Ground - Name " Ground " - SID "4551::86" - Position [20, 121, 40, 139] - ZOrder 77 - } - Block { - BlockType S-Function - Name " SFunction " - SID "4551::83" - Tag "Stateflow S-Function torqueBalancingYoga 9" - Ports [1, 2] - Position [180, 100, 230, 160] - ZOrder 74 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[1 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "reflectedInertia" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4551::85" - Position [460, 241, 480, 259] - ZOrder 76 - } - Block { - BlockType Outport - Name "reflectedInertia" - SID "4551::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - Name "reflectedInertia" - ZOrder 65 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "reflectedInertia" - DstPort 1 - } - Line { - ZOrder 66 - SrcBlock " Ground " - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 67 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 68 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Sum - Name "Add" - SID "4531" - Ports [2, 1] - Position [65, -209, 110, -16] - ZOrder 602 - Inputs "-+" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType SubSystem - Name "Check the contribution of feedforward" - SID "4639" - Ports [2, 0, 1] - Position [30, 66, 140, 119] - ZOrder 894 - RequestExecContextInheritance off - System { - Name "Check the contribution of feedforward" - 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 "434" - Block { - BlockType Inport - Name "feedForward" - SID "4642" - Position [-5, 58, 25, 72] - ZOrder 896 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "u" - SID "4640" - Position [-10, -2, 20, 12] - ZOrder 894 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "4648" - Ports [] - Position [0, -105, 20, -85] - ZOrder 902 - } - Block { - BlockType Demux - Name "Demux1" - SID "4644" - Ports [1, 5] - Position [285, 96, 290, 194] - ZOrder 899 - 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 "4633" - Ports [1, 5] - Position [285, -44, 290, 54] - ZOrder 893 - 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 "4646" - Ports [1, 5] - Position [285, -184, 290, -86] - ZOrder 901 - 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 Sum - Name "Sum" - SID "4636" - Ports [2, 1] - Position [150, -5, 170, 15] - ZOrder 886 - ShowName off - IconShape "round" - Inputs "|+-" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - Port { - PortNumber 1 - Name "u_with_feedForward" - } - } - Block { - BlockType Scope - Name "feedforward_only" - SID "4645" - Ports [5] - Position [365, 88, 470, 202] - ZOrder 898 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','ScopeData3','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-23.63249'" - ",'MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.95301','LegendVisibility','On','" - "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" - "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" - "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " - "1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{" - "struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5" - ",'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'L" - "ineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabel" - "Real','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false" - ",'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0" - "666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83" - "1372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" - " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" - "t',2),struct('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" - "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" - "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" - "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" - "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," - "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" - "uct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNam" - "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-66.44849','MaxYL" - "imReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" - ",true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803" - "922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" - "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411764" - "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1" - " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" - "',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','" - "LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'S" - "howContent',true,'Placement',4),struct('MinYLimReal','-66.44849','MaxYLimReal','41.64929','YLabelReal','','MinYLim" - "Mag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0" - " 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0" - ".0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0" - "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[" - "]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" - "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803" - "9215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392" - "15686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title'" - ",'%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Use" - "rDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','9" - "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',{struct('Title','%','" - "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" - "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" - "s',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" - " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('T" - "itle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" - ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" - "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2018a','Location',[385 262 2230 1233])" - NumInputPorts "5" - Floating off - } - Block { - BlockType Scope - Name "u_only" - SID "4647" - Ports [5] - Position [365, -192, 470, -78] - ZOrder 900 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','ScopeData2','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-23.63249'" - ",'MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.95301','LegendVisibility','On','" - "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" - "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" - "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " - "1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{" - "struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5" - ",'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'L" - "ineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabel" - "Real','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false" - ",'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0" - "666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83" - "1372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" - " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" - "t',2),struct('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" - "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" - "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" - "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" - "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," - "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" - "uct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNam" - "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-66.44849','MaxYL" - "imReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" - ",true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803" - "922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" - "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411764" - "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1" - " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" - "',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','" - "LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'S" - "howContent',true,'Placement',4),struct('MinYLimReal','-66.44849','MaxYLimReal','41.64929','YLabelReal','','MinYLim" - "Mag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0" - " 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0" - ".0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0" - "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[" - "]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" - "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803" - "9215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392" - "15686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title'" - ",'%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Use" - "rDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','9" - "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',{struct('Title','%','" - "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" - "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" - "s',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" - " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('T" - "itle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" - ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" - "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2018a','Location',[385 262 2230 1233])" - NumInputPorts "5" - Floating off - } - Block { - BlockType Scope - Name "u_with_feedforward" - SID "4637" - Ports [5] - Position [365, -52, 470, 62] - ZOrder 887 - ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extmgr" - ".Configuration('Core','General UI',true,'FigureColor',[0.156862745098039 0.156862745098039 0.156862745098039]),ext" - "mgr.Configuration('Core','Source UI',true),extmgr.Configuration('Sources','WiredSimulink',true,'DataLoggingVariabl" - "eName','ScopeData1','DataLoggingSaveFormat','StructureWithTime','DataLoggingDecimation','1','DataLoggingDecimateDa" - "ta',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-23.63249'" - ",'MaxYLimReal','27.95301','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','27.95301','LegendVisibility','On','" - "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627450980" - "3922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4117647058" - "82353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 " - "1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{" - "struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'Lin" - "eStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5" - ",'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible'," - "'on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1" - "],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'L" - "ineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-6.49775','MaxYLimReal','5.80954','YLabel" - "Real','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false" - ",'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0" - "666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.83" - "1372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686" - " 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineW" - "idth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placemen" - "t',2),struct('MinYLimReal','-8.17969','MaxYLimReal','7.65402','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','" - "LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" - "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.62352941" - "1764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" - "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%'," - "'LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),str" - "uct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSt" - "yle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'M" - "arker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on" - "'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNam" - "es',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-66.44849','MaxYL" - "imReal','41.64929','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid'" - ",true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803" - "922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" - "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411764" - "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{struct('Color',[1 1" - " 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color" - "',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','" - "LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'S" - "howContent',true,'Placement',4),struct('MinYLimReal','-66.44849','MaxYLimReal','41.64929','YLabelReal','','MinYLim" - "Mag','0','MaxYLimMag','10','LegendVisibility','on','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0" - " 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0" - ".0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0" - "745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039215686" - "3],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[" - "]}},'ShowContent',true,'Placement',5)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesT" - "ickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.074509803" - "9215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.07450980392" - "15686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title'" - ",'%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','" - "Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Colo" - "r',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-'," - "'LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','n" - "one','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'Use" - "rDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),'TimeRangeSamples','9" - "0','TimeRangeFrames','90','DisplayLayoutDimensions',[5 1],'DisplayContentCache',{struct('Title','%','" - "LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),stru" - "ct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineSty" - "le','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Ma" - "rker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'" - "),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelName" - "s',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'Line" - "Style','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5," - "'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','" - "on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0]" - ",'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth" - "',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible" - "','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1" - " 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWi" - "dth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','V" - "isible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefine" - "dChannelNames',{{}},'PlotMagPhase',false),struct('Title','%','LinePropertiesCache',{{struct('Color',[" - "1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','Lin" - "eWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none'" - ",'Visible','on'),struct('Color',[1 0 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Co" - "lor',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-" - "','LineWidth',0.5,'Marker','none','Visible','on')}},'UserDefinedChannelNames',{{}},'PlotMagPhase',false),struct('T" - "itle','%','LinePropertiesCache',{{struct('Color',[1 1 0],'LineStyle','-','LineWidth',0.5,'Marker','no" - "ne','Visible','on'),struct('Color',[1 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct(" - "'Color',[0 1 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[1 0 0],'LineStyle'" - ",'-','LineWidth',0.5,'Marker','none','Visible','on'),struct('Color',[0 1 0],'LineStyle','-','LineWidth',0.5,'Marke" - "r','none','Visible','on'),struct('Color',[0 0 1],'LineStyle','-','LineWidth',0.5,'Marker','none','Visible','on')}}" - ",'UserDefinedChannelNames',{{}},'PlotMagPhase',false)}),extmgr.Configuration('Tools','Plot Navigation',true,'OnceA" - "tStop',false,'PreviousAutoscale','XY'),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Versi" - "on','2018a','Location',[385 262 2230 1233])" - NumInputPorts "5" - Floating off - } - Line { - Name "right_leg" - ZOrder 68 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 5 - DstBlock "u_with_feedforward" - DstPort 5 - } - Line { - Name "left_arm" - ZOrder 80 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 2 - DstBlock "u_with_feedforward" - DstPort 2 - } - Line { - Name "right_arm" - ZOrder 83 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 3 - DstBlock "u_with_feedforward" - DstPort 3 - } - Line { - Name "torso" - ZOrder 85 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 1 - DstBlock "u_with_feedforward" - DstPort 1 - } - Line { - Name "left_leg" - ZOrder 67 - Labels [-1, 0] - SrcBlock "Demux10" - SrcPort 4 - DstBlock "u_with_feedforward" - DstPort 4 - } - Line { - Name "u_with_feedForward" - ZOrder 66 - Labels [1, 1] - SrcBlock "Sum" - SrcPort 1 - DstBlock "Demux10" - DstPort 1 - } - Line { - ZOrder 91 - SrcBlock "feedForward" - SrcPort 1 - Points [131, 0] - Branch { - ZOrder 93 - Points [0, 80] - DstBlock "Demux1" - DstPort 1 - } - Branch { - ZOrder 92 - DstBlock "Sum" - DstPort 2 - } - } - Line { - Name "left_leg" - ZOrder 95 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 4 - DstBlock "feedforward_only" - DstPort 4 - } - Line { - Name "right_leg" - ZOrder 96 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 5 - DstBlock "feedforward_only" - DstPort 5 - } - Line { - Name "left_arm" - ZOrder 97 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 2 - DstBlock "feedforward_only" - DstPort 2 - } - Line { - Name "torso" - ZOrder 98 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 1 - DstBlock "feedforward_only" - DstPort 1 - } - Line { - Name "right_arm" - ZOrder 99 - Labels [-1, 0] - SrcBlock "Demux1" - SrcPort 3 - DstBlock "feedforward_only" - DstPort 3 - } - Line { - Name "left_leg" - ZOrder 102 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 4 - DstBlock "u_only" - DstPort 4 - } - Line { - Name "right_arm" - ZOrder 103 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 3 - DstBlock "u_only" - DstPort 3 - } - Line { - Name "right_leg" - ZOrder 104 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 5 - DstBlock "u_only" - DstPort 5 - } - Line { - Name "left_arm" - ZOrder 105 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 2 - DstBlock "u_only" - DstPort 2 - } - Line { - ZOrder 88 - SrcBlock "u" - SrcPort 1 - Points [95, 0] - Branch { - ZOrder 109 - Points [0, -140] - DstBlock "Demux2" - DstPort 1 - } - Branch { - ZOrder 108 - DstBlock "Sum" - DstPort 1 - } - } - Line { - Name "torso" - ZOrder 107 - Labels [-1, 0] - SrcBlock "Demux2" - SrcPort 1 - DstBlock "u_only" - DstPort 1 - } - } - } - Block { - BlockType Constant - Name "Constant" - SID "4649" - Position [-75, 30, 45, 50] - ZOrder 895 - ShowName off - Value "Config.SCOPES_INERTIA" - } - Block { - BlockType From - Name "From" - SID "4654" - Position [-465, -155, -380, -145] - ZOrder 902 - ShowName off - HideAutomaticName off - GotoTag "qjDDot_des" - TagVisibility "global" - } - Block { - BlockType Gain - Name "Gain" - SID "4525" - Position [-75, -175, 15, -145] - ZOrder 596 - Gain "Config.K_ff" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Reference - Name "Get Measurement" - SID "4539" - Ports [0, 1] - Position [-465, -109, -375, -91] - ZOrder 869 - BackgroundColor "[0.925500, 0.870600, 0.133300]" - ShowName off - HideAutomaticName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - measuredType "Joints Acceleration" - } - Block { - BlockType Product - Name "Product" - SID "4526" - Ports [2, 1] - Position [-160, -239, -125, -86] - ZOrder 597 - Multiplication "Matrix(*)" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - RndMeth "Floor" - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch" - SID "4535" - Position [380, -138, 430, 38] - ZOrder 606 - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Switch - Name "Switch1" - SID "4650" - Position [-260, -166, -195, -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" - } - Line { - ZOrder 6 - SrcBlock "Product" - SrcPort 1 - Points [18, 0] - Branch { - ZOrder 95 - DstBlock "Gain" - DstPort 1 - } - Branch { - ZOrder 92 - Points [0, 240] - DstBlock "Check the contribution of feedforward" - 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 "u" - SrcPort 1 - Points [145, 0] - Branch { - ZOrder 62 - Points [0, -75] - DstBlock "Add" - DstPort 2 - } - Branch { - ZOrder 94 - Points [0, 95] - DstBlock "Check the contribution of feedforward" - 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 96 - SrcBlock "Constant" - SrcPort 1 - Points [35, 0] - DstBlock "Check the contribution of feedforward" - DstPort enable - } - Line { - ZOrder 108 - SrcBlock " 1" - SrcPort 1 - DstBlock "Switch1" - DstPort 2 - } - Line { - ZOrder 109 - SrcBlock "Switch1" - SrcPort 1 - DstBlock "Product" - DstPort 2 - } - Line { - ZOrder 110 - SrcBlock "Get Measurement" - SrcPort 1 - DstBlock "Switch1" - DstPort 3 - } - Line { - ZOrder 114 - SrcBlock "From" - SrcPort 1 - DstBlock "Switch1" - DstPort 1 - } - } - } - Block { - BlockType Constant - Name "Constant" - SID "2371" - Position [1190, 197, 1310, 223] - ZOrder 383 - ShowName off - Value "ConstraintsMatrix" - } - Block { - BlockType Constant - Name "Constant1" - SID "2372" - Position [1190, 242, 1310, 268] - ZOrder 384 - ShowName off - Value "bVectorConstraints" - } - Block { - BlockType Goto - Name "Goto" - SID "4494" - Position [1685, 1096, 1765, 1114] - ZOrder 855 - GotoTag "f_noQP" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto1" - SID "4495" - Position [2350, 346, 2430, 364] - ZOrder 856 - GotoTag "tau_des" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto5" - SID "4499" - Position [2135, 791, 2215, 809] - ZOrder 860 - GotoTag "f_QP" - TagVisibility "global" - } - Block { - BlockType Goto - Name "Goto6" - SID "4500" - Position [1685, 1021, 1765, 1039] - ZOrder 861 - GotoTag "CoM_error" - TagVisibility "global" - } - Block { - BlockType Outport - Name "tau " - SID "2414" - Position [2375, 278, 2405, 292] - ZOrder 279 - IconDisplay "Port number" - } - Line { - ZOrder 1 - SrcBlock "LEFT_RIGHT_FOOT_IN_CONTACT" - SrcPort 1 - Points [32, 0] - Branch { - ZOrder 280 - DstBlock "Balancing Controller\n" - DstPort 1 - } - Branch { - ZOrder 322 - Points [0, -65] - DstBlock "Compute joint torques" - DstPort 1 - } - Branch { - ZOrder 286 - Points [0, 455] - DstBlock "Compute angular momentum integral" - DstPort 3 - } - } - Line { - ZOrder 12 - SrcBlock "qj" - SrcPort 1 - Points [46, 0] - Branch { - ZOrder 287 - Points [0, 265] - DstBlock "Compute angular momentum integral" - DstPort 2 - } - Branch { - ZOrder 277 - DstBlock "Balancing Controller\n" - DstPort 5 - } - } - Line { - ZOrder 31 - SrcBlock "qjDes" - SrcPort 1 - Points [63, 0] - Branch { - ZOrder 288 - Points [0, 210] - DstBlock "Compute angular momentum integral" - DstPort 1 - } - Branch { - ZOrder 270 - DstBlock "Balancing Controller\n" - DstPort 6 - } - } - Line { - ZOrder 34 - SrcBlock "M" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 8 - } - Line { - ZOrder 35 - SrcBlock "biasTorques" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 9 - } - Line { - ZOrder 36 - SrcBlock "desired_x_dx_ddx_CoM" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 20 - } - Line { - ZOrder 37 - SrcBlock "impedances" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 23 - } - Line { - ZOrder 234 - SrcBlock "L " - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 10 - } - Line { - ZOrder 60 - SrcBlock "\n\n 5" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 2 - } - Line { - ZOrder 61 - SrcBlock "Constant" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 3 - } - Line { - ZOrder 62 - SrcBlock "Constant1" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 4 - } - Line { - ZOrder 97 - SrcBlock "nu" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 7 - } - Line { - ZOrder 106 - SrcBlock "gainsPCOM" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 21 - } - Line { - ZOrder 107 - SrcBlock "gainsDCOM" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 22 - } - Line { - ZOrder 237 - SrcBlock "w_H_r_sole" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 13 - } - Line { - ZOrder 238 - SrcBlock "w_H_l_sole" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 12 - } - Line { - ZOrder 267 - SrcBlock "JL" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 14 - } - Line { - ZOrder 268 - SrcBlock "JR" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 15 - } - Line { - ZOrder 269 - SrcBlock "Compute angular momentum integral" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 11 - } - Line { - ZOrder 273 - SrcBlock "dJL_nu" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 16 - } - Line { - ZOrder 274 - SrcBlock "dJR_nu" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 17 - } - Line { - ZOrder 275 - SrcBlock "J_CoM" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 19 - } - Line { - ZOrder 276 - SrcBlock "xCoM" - SrcPort 1 - DstBlock "Balancing Controller\n" - DstPort 18 - } - Line { - ZOrder 284 - SrcBlock "state" - SrcPort 1 - Points [32, 0] - Branch { - ZOrder 386 - DstBlock "Compute angular momentum integral" - DstPort 4 - } - Branch { - ZOrder 383 - Points [0, 587; 660, 0; 0, -142] - DstBlock "Compute joint torques" - DstPort 14 - } - } - Line { - ZOrder 303 - SrcBlock "Compute joint torques" - SrcPort 1 - DstBlock "Compute joint torques (motor reflected inertia)" - DstPort 1 - } - Line { - ZOrder 306 - SrcBlock "Balancing Controller\n" - SrcPort 1 - DstBlock "Compute joint torques" - DstPort 2 - } - Line { - ZOrder 307 - SrcBlock "Balancing Controller\n" - SrcPort 2 - DstBlock "Compute joint torques" - DstPort 3 - } - Line { - ZOrder 308 - SrcBlock "Balancing Controller\n" - SrcPort 3 - DstBlock "Compute joint torques" - DstPort 4 - } - Line { - ZOrder 309 - SrcBlock "Balancing Controller\n" - SrcPort 4 - DstBlock "Compute joint torques" - DstPort 5 - } - Line { - ZOrder 310 - SrcBlock "Balancing Controller\n" - SrcPort 5 - DstBlock "Compute joint torques" - DstPort 6 - } - Line { - ZOrder 311 - SrcBlock "Balancing Controller\n" - SrcPort 6 - DstBlock "Compute joint torques" - DstPort 7 - } - Line { - ZOrder 312 - SrcBlock "Balancing Controller\n" - SrcPort 7 - DstBlock "Compute joint torques" - DstPort 8 - } - Line { - ZOrder 313 - SrcBlock "Balancing Controller\n" - SrcPort 8 - DstBlock "Compute joint torques" - DstPort 9 - } - Line { - ZOrder 314 - SrcBlock "Balancing Controller\n" - SrcPort 9 - DstBlock "Compute joint torques" - DstPort 10 - } - Line { - ZOrder 315 - SrcBlock "Balancing Controller\n" - SrcPort 10 - DstBlock "Compute joint torques" - DstPort 11 - } - Line { - ZOrder 316 - SrcBlock "Balancing Controller\n" - SrcPort 11 - DstBlock "Compute joint torques" - DstPort 12 - } - Line { - ZOrder 317 - SrcBlock "Balancing Controller\n" - SrcPort 12 - DstBlock "Compute joint torques" - DstPort 13 - } - Line { - Name "CoM_Error" - ZOrder 323 - Labels [0, 0] - SrcBlock "Balancing Controller\n" - SrcPort 13 - DstBlock "Goto6" - DstPort 1 - } - Line { - ZOrder 324 - SrcBlock "Compute joint torques" - SrcPort 2 - DstBlock "Goto5" - DstPort 1 - } - Line { - Name "fNoQP" - ZOrder 325 - Labels [0, 0] - SrcBlock "Balancing Controller\n" - SrcPort 14 - DstBlock "Goto" - DstPort 1 - } - Line { - ZOrder 372 - SrcBlock "Compute joint torques (motor reflected inertia)" - SrcPort 1 - Points [44, 0] - Branch { - ZOrder 389 - DstBlock "tau " - DstPort 1 - } - Branch { - ZOrder 388 - Points [0, 70] - DstBlock "Goto1" - DstPort 1 - } - } - } - } - Block { - BlockType SubSystem - Name "emergency stop: joint limits" - SID "2416" - Ports [1] - Position [870, 336, 995, 364] - ZOrder 544 - BackgroundColor "red" - ShowName off - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 23 - $ClassName "Simulink.Mask" - Display "disp('EMERGENCY STOP')" - RunInitForIconRedraw "off" - } - System { - Name "emergency stop: joint limits" - 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 "600" - Block { - BlockType Inport - Name "qj" - SID "2419" - Position [150, 238, 180, 252] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n1" - SID "4660" - 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 "4663" - 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 "4661" - 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] - 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 "855" - Block { - BlockType Inport - Name "qj" - SID "4662" - Position [60, 103, 90, 117] - ZOrder 552 - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "3207" - Ports [] - Position [227, -45, 247, -25] - ZOrder 553 - } - Block { - BlockType Assertion - Name "Assertion" - SID "2420" - Position [340, 72, 400, 118] - ZOrder 207 - } - Block { - BlockType Reference - Name "Get Limits" - SID "2432" - Ports [0, 2] - Position [20, 23, 140, 92] - ZOrder 551 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Limits" - SourceType "Get Limits" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - limitsSource "ControlBoard" - limitsType "Position" - } - Block { - BlockType SubSystem - Name "MATLAB Function" - SID "2421" - Ports [4, 1] - 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 "3770" - Block { - BlockType Inport - Name "umin" - SID "2421::18" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "umax" - SID "2421::19" - Position [20, 136, 40, 154] - ZOrder -2 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "u" - SID "2421::1" - Position [20, 171, 40, 189] - ZOrder -3 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "tol" - SID "2421::20" - Position [20, 206, 40, 224] - ZOrder -4 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "2421::3769" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 119 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "2421::3768" - Tag "Stateflow S-Function torqueBalancingYoga 18" - Ports [4, 2] - Position [180, 102, 230, 203] - ZOrder 118 - FunctionName "sf_sfun" - PortCounts "[4 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "inRange" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "2421::3770" - Position [460, 241, 480, 259] - ZOrder 120 - } - Block { - BlockType Outport - Name "inRange" - SID "2421::5" - Position [460, 101, 480, 119] - ZOrder -8 - IconDisplay "Port number" - } - Line { - ZOrder 169 - SrcBlock "umin" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 170 - SrcBlock "umax" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - ZOrder 171 - SrcBlock "u" - SrcPort 1 - DstBlock " SFunction " - DstPort 3 - } - Line { - ZOrder 172 - SrcBlock "tol" - SrcPort 1 - DstBlock " SFunction " - DstPort 4 - } - Line { - Name "inRange" - ZOrder 173 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "inRange" - DstPort 1 - } - Line { - ZOrder 174 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 175 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Constant - Name "index1" - SID "2422" - Position [70, 140, 95, 150] - ZOrder 204 - ShowName off - Value "0.01" - VectorParams1D off - } - Line { - ZOrder 6 - SrcBlock "qj" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 3 - } - Line { - ZOrder 5 - SrcBlock "MATLAB Function" - SrcPort 1 - DstBlock "Assertion" - DstPort 1 - } - Line { - ZOrder 1 - SrcBlock "Get Limits" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "Get Limits" - SrcPort 2 - DstBlock "MATLAB Function" - DstPort 2 - } - Line { - ZOrder 3 - SrcBlock "index1" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 4 - } - } - } - Block { - BlockType SubSystem - Name "STOP IF THERE ARE SPIKES IN THE ENCODERS" - SID "4664" - 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] - 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 "827" - Block { - BlockType Inport - Name "qj" - SID "4665" - Position [15, 53, 45, 67] - ZOrder 552 - IconDisplay "Port number" - } - Block { - BlockType EnablePort - Name "Enable" - SID "4666" - Ports [] - Position [217, -25, 237, -5] - ZOrder 553 - } - Block { - BlockType Assertion - Name "Assertion" - SID "4667" - Position [340, 72, 400, 118] - ZOrder 207 - } - Block { - BlockType SubSystem - Name "MATLAB Function" - SID "4669" - Ports [2, 1] - Position [165, 24, 285, 166] - 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 "3770" - Block { - BlockType Inport - Name "u" - SID "4669::1" - Position [20, 101, 40, 119] - ZOrder -3 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "delta_u_max" - SID "4669::18" - Position [20, 136, 40, 154] - ZOrder -1 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "4669::3769" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 119 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4669::3768" - Tag "Stateflow S-Function torqueBalancingYoga 14" - Ports [2, 2] - Position [180, 105, 230, 165] - ZOrder 118 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "noSpikes" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4669::3770" - Position [460, 241, 480, 259] - ZOrder 120 - } - Block { - BlockType Outport - Name "noSpikes" - SID "4669::5" - Position [460, 101, 480, 119] - ZOrder -8 - IconDisplay "Port number" - } - Line { - ZOrder 30 - SrcBlock "u" - SrcPort 1 - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 31 - SrcBlock "delta_u_max" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "noSpikes" - ZOrder 32 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "noSpikes" - DstPort 1 - } - Line { - ZOrder 33 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 34 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Constant - Name "index1" - SID "4671" - Position [-40, 124, 100, 136] - ZOrder 554 - ShowName off - Value "Sat.maxJointsPositionDelta" - VectorParams1D off - } - Line { - ZOrder 1 - SrcBlock "qj" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "MATLAB Function" - SrcPort 1 - DstBlock "Assertion" - DstPort 1 - } - Line { - ZOrder 6 - SrcBlock "index1" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 2 - } - } - } - Line { - ZOrder 7 - SrcBlock "ON_GAZEBO\n1" - SrcPort 1 - DstBlock "STOP IF JOINTS HIT THE LIMITS" - DstPort enable - } - Line { - ZOrder 6 - SrcBlock "qj" - SrcPort 1 - Points [20, 0] - Branch { - ZOrder 11 - Points [0, 110] - DstBlock "STOP IF THERE ARE SPIKES IN THE ENCODERS" - DstPort 1 - } - Branch { - ZOrder 10 - DstBlock "STOP IF JOINTS HIT THE LIMITS" - DstPort 1 - } - } - Line { - ZOrder 9 - SrcBlock "ON_GAZEBO\n2" - SrcPort 1 - DstBlock "STOP IF THERE ARE SPIKES IN THE ENCODERS" - DstPort enable - } - } - } - Block { - BlockType SubSystem - Name "synchronizer" - SID "2423" - Ports [] - Position [1582, 358, 1702, 388] - ZOrder 546 - ForegroundColor "red" - BackgroundColor "green" - ShowName off - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 24 - $ClassName "Simulink.Mask" - Display "disp('SYNCHRONIZER')" - } - System { - Name "synchronizer" - 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 "958" - Block { - BlockType SubSystem - Name "GAZEBO_SYNCHRONIZER" - SID "2424" - Ports [0, 0, 1] - Position [5, 15, 130, 75] - ZOrder -7 - RequestExecContextInheritance off - System { - Name "GAZEBO_SYNCHRONIZER" - Location [65, 24, 2560, 1440] - 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 EnablePort - Name "Enable" - SID "2425" - Ports [] - Position [177, 85, 197, 105] - ZOrder 538 - } - Block { - BlockType Reference - Name "Simulator Synchronizer" - SID "2426" - Ports [] - Position [120, 24, 250, 61] - ZOrder 539 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Simulator Synchronizer" - SourceType "Simulator Synchronizer" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - period "Config.Ts" - serverPortName "'/clock/rpc'" - clientPortName "'/WBT_synchronizer/rpc:o'" - } - } - } - Block { - BlockType Logic - Name "Logical\nOperator" - SID "2427" - Ports [1, 1] - Position [-30, -34, -5, -16] - ZOrder 307 - BlockMirror on - NamePlacement "alternate" - ShowName off - Operator "NOT" - AllPortsSameDT off - OutDataTypeStr "boolean" - } - Block { - BlockType Constant - Name "ON_GAZEBO\n" - SID "2428" - Position [95, -35, 210, -15] - ZOrder 304 - BlockMirror on - NamePlacement "alternate" - ShowName off - Value "Config.ON_GAZEBO" - } - Block { - BlockType SubSystem - Name "REAL_TIME_SYNC" - SID "2429" - Ports [0, 0, 1] - Position [-170, 12, -45, 72] - ZOrder -9 - RequestExecContextInheritance off - System { - Name "REAL_TIME_SYNC" - Location [65, 24, 2560, 1440] - 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 EnablePort - Name "Enable" - SID "2430" - Ports [] - Position [167, 90, 187, 110] - ZOrder 538 - } - Block { - BlockType Reference - Name "Real Time Synchronizer" - SID "2431" - Ports [] - Position [115, 34, 240, 71] - ZOrder 539 - ForegroundColor "[0.916667, 0.916667, 0.916667]" - BackgroundColor "gray" - ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" - SourceType "Real Time Synchronizer" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - period "Config.Ts" - } - } - } - Block { - BlockType ToWorkspace - Name "To Workspace" - SID "4628" - Ports [1] - Position [140, -95, 200, -75] - ZOrder 699 - VariableName "yarp_time" - MaxDataPoints "inf" - SaveFormat "Structure With Time" - Save2DSignal "3-D array (concatenate along third dimension)" - FixptAsFi on - SampleTime "-1" - } - Block { - BlockType Reference - Name "Yarp Clock" - SID "4629" - Ports [0, 1] - Position [-50, -94, 15, -76] - ZOrder 698 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Clock" - SourceType "YARP Clock" - SourceProductName "Whole Body Toolbox" - MultiThreadCoSim "auto" - } - Line { - ZOrder 1 - SrcBlock "Logical\nOperator" - SrcPort 1 - Points [-70, 0] - DstBlock "REAL_TIME_SYNC" - DstPort enable - } - Line { - ZOrder 2 - SrcBlock "ON_GAZEBO\n" - SrcPort 1 - Points [-25, 0] - Branch { - ZOrder 6 - DstBlock "GAZEBO_SYNCHRONIZER" - DstPort enable - } - Branch { - ZOrder 4 - DstBlock "Logical\nOperator" - DstPort 1 - } - } - Line { - ZOrder 8 - SrcBlock "Yarp Clock" - SrcPort 1 - DstBlock "To Workspace" - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "tauDot Saturation" - SID "4553" - Ports [1, 1] - Position [1525, 511, 1580, 549] - ZOrder 555 - BackgroundColor "orange" - RequestExecContextInheritance off - System { - Name "tauDot Saturation" - 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 "607" - Block { - BlockType Inport - Name "tau" - SID "4554" - Position [-115, 153, -85, 167] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant" - SID "4558" - Position [95, 87, 295, 103] - ZOrder 3 - ShowName off - Value "Config.SATURATE_TORQUE_DERIVATIVE" - } - Block { - BlockType SubSystem - Name "Saturate the Torque Derivative" - SID "4556" - Ports [2, 1] - Position [90, -12, 295, 72] - ZOrder 1 - NamePlacement "alternate" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ExplicitOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - System { - Name "Saturate the 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 "62" - 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 Demux - Name " Demux " - SID "4556::61" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 51 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "4556::60" - Tag "Stateflow S-Function torqueBalancingYoga 11" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 50 - FunctionName "sf_sfun" - Parameters "Config" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport on - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "uSat" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "4556::62" - Position [460, 241, 480, 259] - ZOrder 52 - } - Block { - BlockType Outport - Name "uSat" - SID "4556::5" - Position [460, 101, 480, 119] - ZOrder -5 - IconDisplay "Port number" - } - Line { - ZOrder 85 - SrcBlock "u" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 86 - SrcBlock "u_0" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "uSat" - ZOrder 87 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "uSat" - DstPort 1 - } - Line { - ZOrder 88 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 89 - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - } - Block { - BlockType Switch - Name "Switch" - SID "4557" - Position [335, -5, 390, 195] - ZOrder 2 - Criteria "u2 > Threshold" - InputSameDT off - SaturateOnIntegerOverflow off - } - Block { - BlockType Reference - Name "holder\n" - SID "4559" - Ports [1, 1] - Position [20, 39, 60, 61] - ZOrder 15 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/holder\n" - SourceType "Holder" - SourceProductName "Whole Body Toolbox" - ContentPreviewEnabled off - } - Block { - BlockType Outport - Name "tau_s" - SID "4555" - Position [455, 88, 485, 102] - ZOrder -2 - IconDisplay "Port number" - } - Line { - ZOrder 2 - SrcBlock "Switch" - SrcPort 1 - DstBlock "tau_s" - DstPort 1 - } - Line { - ZOrder 3 - SrcBlock "Constant" - SrcPort 1 - DstBlock "Switch" - DstPort 2 - } - Line { - ZOrder 4 - SrcBlock "tau" - SrcPort 1 - Points [60, 0] - Branch { - ZOrder 7 - Points [0, -110] - Branch { - ZOrder 10 - DstBlock "holder\n" - DstPort 1 - } - Branch { - ZOrder 9 - Points [0, -40] - DstBlock "Saturate the Torque Derivative" - DstPort 1 - } - } - Branch { - ZOrder 6 - DstBlock "Switch" - DstPort 3 - } - } - Line { - ZOrder 5 - SrcBlock "Saturate the Torque Derivative" - SrcPort 1 - DstBlock "Switch" - DstPort 1 - } - Line { - ZOrder 8 - SrcBlock "holder\n" - SrcPort 1 - DstBlock "Saturate the Torque Derivative" - DstPort 2 - } - } - } - Line { - ZOrder 20 - SrcBlock "Saturation" - SrcPort 1 - Points [15, 0] - Branch { - ZOrder 207 - Points [0, 60] - DstBlock "Goto" - DstPort 1 - } - Branch { - ZOrder 206 - DstBlock "Set References" - DstPort 1 - } - } - Line { - ZOrder 90 - SrcBlock "Dynamics and Kinematics" - SrcPort 1 - DstBlock "controller_QP" - DstPort 1 - } - Line { - ZOrder 91 - SrcBlock "Dynamics and Kinematics" - SrcPort 2 - DstBlock "controller_QP" - DstPort 2 - } - Line { - ZOrder 92 - SrcBlock "Dynamics and Kinematics" - SrcPort 3 - DstBlock "controller_QP" - DstPort 3 - } - Line { - ZOrder 94 - SrcBlock "Dynamics and Kinematics" - SrcPort 4 - DstBlock "controller_QP" - DstPort 4 - } - Line { - ZOrder 95 - SrcBlock "Dynamics and Kinematics" - SrcPort 5 - DstBlock "controller_QP" - DstPort 5 - } - Line { - ZOrder 97 - SrcBlock "Dynamics and Kinematics" - SrcPort 6 - DstBlock "controller_QP" - DstPort 6 - } - Line { - ZOrder 98 - SrcBlock "Dynamics and Kinematics" - SrcPort 7 - DstBlock "controller_QP" - DstPort 7 - } - Line { - ZOrder 99 - SrcBlock "Dynamics and Kinematics" - SrcPort 8 - DstBlock "controller_QP" - DstPort 8 - } - Line { - ZOrder 100 - SrcBlock "Dynamics and Kinematics" - SrcPort 9 - DstBlock "controller_QP" - DstPort 9 - } - Line { - ZOrder 101 - SrcBlock "Dynamics and Kinematics" - SrcPort 10 - DstBlock "controller_QP" - DstPort 10 - } - Line { - ZOrder 102 - SrcBlock "Dynamics and Kinematics" - SrcPort 11 - DstBlock "controller_QP" - DstPort 11 - } - Line { - ZOrder 119 - SrcBlock "Robot State and References" - SrcPort 1 - Points [48, 0; 0, -260] - DstBlock "Dynamics and Kinematics" - DstPort 1 - } - Line { - ZOrder 120 - SrcBlock "Robot State and References" - SrcPort 2 - Points [69, 0] - Branch { - ZOrder 172 - DstBlock "controller_QP" - DstPort 12 - } - Branch { - ZOrder 124 - Points [0, -180] - Branch { - ZOrder 189 - Points [0, -45] - DstBlock "emergency stop: joint limits" - DstPort 1 - } - Branch { - ZOrder 151 - DstBlock "Dynamics and Kinematics" - DstPort 2 - } - } - } - Line { - ZOrder 121 - SrcBlock "Robot State and References" - SrcPort 3 - Points [94, 0] - Branch { - ZOrder 240 - DstBlock "controller_QP" - DstPort 13 - } - Branch { - ZOrder 126 - Points [0, -100] - DstBlock "Dynamics and Kinematics" - DstPort 3 - } - } - Line { - ZOrder 129 - SrcBlock "Robot State and References" - SrcPort 4 - DstBlock "controller_QP" - DstPort 14 - } - Line { - ZOrder 130 - SrcBlock "Robot State and References" - SrcPort 5 - DstBlock "controller_QP" - DstPort 15 - } - Line { - ZOrder 131 - SrcBlock "Robot State and References" - SrcPort 6 - DstBlock "controller_QP" - DstPort 16 - } - Line { - ZOrder 132 - SrcBlock "Robot State and References" - SrcPort 7 - DstBlock "controller_QP" - DstPort 17 - } - Line { - ZOrder 134 - SrcBlock "Robot State and References" - SrcPort 8 - DstBlock "controller_QP" - DstPort 18 - } - Line { - ZOrder 135 - SrcBlock "Robot State and References" - SrcPort 9 - DstBlock "controller_QP" - DstPort 19 - } - Line { - ZOrder 136 - SrcBlock "Robot State and References" - SrcPort 10 - DstBlock "controller_QP" - DstPort 20 - } - Line { - ZOrder 204 - SrcBlock "controller_QP" - SrcPort 1 - DstBlock "tauDot Saturation" - DstPort 1 - } - Line { - ZOrder 203 - SrcBlock "tauDot Saturation" - SrcPort 1 - DstBlock "Saturation" - DstPort 1 - } - } -} -#Finite State Machines -# -# Stateflow 80000014 -# -# -Stateflow { - machine { - id 1 - name "torqueBalancingYoga" - created "18-Feb-2014 10:14:40" - isLibrary 0 - sfVersion 80000012 - firstTarget 279 - } - chart { - id 2 - machine 1 - name "controller_QP/Compute angular momentum integral/Compute base to fixed link transform/LFoot to base link" - " transform /fromImuToHomogeousTransformFCN" - windowPosition [326.89 491.339 161 328] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.000677131425054] - treeNode [0 3 0 0] - viewObj 2 - ssIdHighWaterMark 88 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 1 - disableImplicitCasting 1 - eml { - name "worldToBaseTransformWithIMUFCN" - } - firstData 4 - firstTransition 13 - firstJunction 12 - } - 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 w_H_b = worldToBaseTransformWithIMUFCN(imu_H_link,imu_H_link_0,link_H_base,inertial_0,in" - "ertial,neck_pos,Config)\n\n w_H_b = wbc.worldToBaseTransformWithIMU(imu_H_link,imu_H_link_0,link_H_base,inert" - "ial_0,inertial,neck_pos,Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 4 - ssIdNumber 82 - name "imu_H_link" - 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 [2 0 5] - } - data { - id 5 - ssIdNumber 83 - name "imu_H_link_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 [2 4 6] - } - data { - id 6 - ssIdNumber 81 - name "link_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 [2 5 7] - } - data { - id 7 - ssIdNumber 85 - name "inertial_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 [2 6 8] - } - data { - id 8 - ssIdNumber 65 - name "inertial" - 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 [2 7 9] - } - data { - id 9 - 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 [2 8 10] - } - data { - id 10 - 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 [2 9 11] - } - data { - id 11 - 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 [2 10 0] - } - junction { - id 12 - position [23.5747 49.5747 7] - chart 2 - subviewer 2 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [2 0 0] - } - transition { - id 13 - 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 12 - 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 14 - machine 1 - name "controller_QP/Compute angular momentum integral/Compute base to fixed link transform/LFoot to base link" - " transform /fromImuToHomogeousTransformFCN" - chart 2 - } - chart { - id 15 - machine 1 - name "Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot to base link transform /fr" - "omImuToHomogeousTransformFCN" - windowPosition [326.89 491.339 161 328] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.000677131425054] - treeNode [0 16 0 0] - viewObj 15 - ssIdHighWaterMark 88 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 2 - disableImplicitCasting 1 - eml { - name "worldToBaseTransformWithIMUFCN" - } - firstData 17 - firstTransition 26 - firstJunction 25 - } - state { - id 16 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 15 - treeNode [15 0 0 0] - superState SUBCHART - subviewer 15 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function w_H_b = worldToBaseTransformWithIMUFCN(imu_H_link,imu_H_link_0,link_H_base,inertial_0,in" - "ertial,neck_pos,Config)\n \n w_H_b = wbc.worldToBaseTransformWithIMU(imu_H_link,imu_H_link_0,link_H_base,i" - "nertial_0,inertial,neck_pos,Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 17 - ssIdNumber 82 - name "imu_H_link" - 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 [15 0 18] - } - data { - id 18 - ssIdNumber 83 - name "imu_H_link_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 [15 17 19] - } - data { - id 19 - ssIdNumber 81 - name "link_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 [15 18 20] - } - data { - id 20 - ssIdNumber 85 - name "inertial_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 [15 19 21] - } - data { - id 21 - ssIdNumber 65 - name "inertial" - 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 [15 20 22] - } - data { - id 22 - 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 [15 21 23] - } - data { - id 23 - 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 [15 22 24] - } - data { - id 24 - 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 [15 23 0] - } - junction { - id 25 - position [23.5747 49.5747 7] - chart 15 - subviewer 15 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [15 0 0] - } - transition { - id 26 - 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 25 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 15 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 15 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [15 0 0] - } - instance { - id 27 - machine 1 - name "Robot State and References/Internal Coordinator/Base to fixed_link/fixedFoot to base link transform /fr" - "omImuToHomogeousTransformFCN" - chart 15 - } - chart { - id 28 - machine 1 - name "Robot State and References/State Machine Yoga/Compute base to fixed link transform/RFoot to base link t" - "ransform/fromImuToHomogeousTransformFCN" - windowPosition [326.89 491.339 161 328] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.000677131425054] - treeNode [0 29 0 0] - viewObj 28 - ssIdHighWaterMark 88 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 3 - disableImplicitCasting 1 - eml { - name "worldToBaseTransformWithIMUFCN" - } - firstData 30 - firstTransition 39 - firstJunction 38 - } - state { - id 29 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 28 - treeNode [28 0 0 0] - superState SUBCHART - subviewer 28 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function w_H_b = worldToBaseTransformWithIMUFCN(imu_H_link,imu_H_link_0,link_H_base,inertial_0,in" - "ertial,neck_pos,Config)\n\n w_H_b = wbc.worldToBaseTransformWithIMU(imu_H_link,imu_H_link_0,link_H_base,inert" - "ial_0,inertial,neck_pos,Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 30 - ssIdNumber 82 - name "imu_H_link" - 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 [28 0 31] - } - data { - id 31 - ssIdNumber 83 - name "imu_H_link_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 [28 30 32] - } - data { - id 32 - ssIdNumber 81 - name "link_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 [28 31 33] - } - data { - id 33 - ssIdNumber 85 - name "inertial_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 [28 32 34] - } - data { - id 34 - ssIdNumber 65 - name "inertial" - 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 [28 33 35] - } - data { - id 35 - 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 [28 34 36] - } - data { - id 36 - 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 [28 35 37] - } - data { - id 37 - 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 [28 36 0] - } - junction { - id 38 - position [23.5747 49.5747 7] - chart 28 - subviewer 28 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [28 0 0] - } - transition { - id 39 - 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 38 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 28 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 28 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [28 0 0] - } - instance { - id 40 - machine 1 - name "Robot State and References/State Machine Yoga/Compute base to fixed link transform/RFoot to base link t" - "ransform/fromImuToHomogeousTransformFCN" - chart 28 - } - chart { - id 41 - machine 1 - name "controller_QP/Compute angular momentum integral/Compute base to fixed link transform/RFoot to base link" - " transform/fromImuToHomogeousTransformFCN" - windowPosition [326.89 491.339 161 328] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.000677131425054] - treeNode [0 42 0 0] - viewObj 41 - ssIdHighWaterMark 88 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 4 - disableImplicitCasting 1 - eml { - name "worldToBaseTransformWithIMUFCN" - } - firstData 43 - firstTransition 52 - firstJunction 51 - } - state { - id 42 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 41 - treeNode [41 0 0 0] - superState SUBCHART - subviewer 41 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function w_H_b = worldToBaseTransformWithIMUFCN(imu_H_link,imu_H_link_0,link_H_base,inertial_0,in" - "ertial,neck_pos,Config)\n\n w_H_b = wbc.worldToBaseTransformWithIMU(imu_H_link,imu_H_link_0,link_H_base,inert" - "ial_0,inertial,neck_pos,Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 43 - ssIdNumber 82 - name "imu_H_link" - 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 [41 0 44] - } - data { - id 44 - ssIdNumber 83 - name "imu_H_link_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 [41 43 45] - } - data { - id 45 - ssIdNumber 81 - name "link_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 [41 44 46] - } - data { - id 46 - ssIdNumber 85 - name "inertial_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 [41 45 47] - } - data { - id 47 - ssIdNumber 65 - name "inertial" - 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 [41 46 48] - } - data { - id 48 - 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 [41 47 49] - } - data { - id 49 - 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 [41 48 50] - } - data { - id 50 - 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 [41 49 0] - } - junction { - id 51 - position [23.5747 49.5747 7] - chart 41 - subviewer 41 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [41 0 0] - } - transition { - id 52 - 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 51 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 41 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 41 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [41 0 0] - } - instance { - id 53 - machine 1 - name "controller_QP/Compute angular momentum integral/Compute base to fixed link transform/RFoot to base link" - " transform/fromImuToHomogeousTransformFCN" - chart 41 - } - chart { - id 54 - machine 1 - name "Robot State and References/Internal Coordinator/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 55 0 0] - viewObj 54 - ssIdHighWaterMark 10 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 5 - disableImplicitCasting 1 - eml { - name "computeBaseVelocityFCN" - } - firstData 56 - firstTransition 63 - firstJunction 62 - } - state { - id 55 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 54 - treeNode [54 0 0 0] - superState SUBCHART - subviewer 54 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function nu_b = computeBaseVelocityFCN(J_LeftFoot, J_RightFoot, feetInContact, qjDot, Reg)\n \n" - " nu_b = wbc.computeBaseVelocity(J_LeftFoot, J_RightFoot, feetInContact, qjDot, Reg);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 56 - ssIdNumber 4 - name "J_LeftFoot" - 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 [54 0 57] - } - data { - id 57 - ssIdNumber 8 - name "J_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 [54 56 58] - } - data { - id 58 - ssIdNumber 9 - name "feetInContact" - 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 [54 57 59] - } - data { - id 59 - 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 [54 58 60] - } - data { - id 60 - ssIdNumber 6 - name "qjDot" - 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 [54 59 61] - } - data { - id 61 - 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 [54 60 0] - } - junction { - id 62 - position [23.5747 49.5747 7] - chart 54 - subviewer 54 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [54 0 0] - } - transition { - id 63 - 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 62 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 54 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 54 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [54 0 0] - } - instance { - id 64 - machine 1 - name "Robot State and References/Internal Coordinator/Compute State Velocity/Compute Base Velocity" - chart 54 - } - chart { - id 65 - machine 1 - name "Dynamics and Kinematics/Dynamics/Add motors reflected inertias/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 66 0 0] - viewObj 65 - ssIdHighWaterMark 6 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 6 - disableImplicitCasting 1 - eml { - name "addMotorsInertiaFCN" - } - firstData 67 - firstTransition 71 - firstJunction 70 - } - state { - id 66 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 65 - treeNode [65 0 0 0] - superState SUBCHART - subviewer 65 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function M_with_inertia = addMotorsInertiaFCN(M,Config)\n\n M_with_inertia = wbc.addMotorsIner" - "tia(M,Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 67 - 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 [65 0 68] - } - data { - id 68 - 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 [65 67 69] - } - data { - id 69 - 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 [65 68 0] - } - junction { - id 70 - position [23.5747 49.5747 7] - chart 65 - subviewer 65 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [65 0 0] - } - transition { - id 71 - 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 70 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 65 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 65 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [65 0 0] - } - instance { - id 72 - machine 1 - name "Dynamics and Kinematics/Dynamics/Add motors reflected inertias/Add motor reflected inertias" - chart 65 - } - chart { - id 73 - machine 1 - name "Robot State and References/State Machine Yoga/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 74 0 0] - viewObj 73 - ssIdHighWaterMark 10 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 7 - disableImplicitCasting 1 - eml { - name "computeBaseVelocityFCN" - } - firstData 75 - firstTransition 82 - firstJunction 81 - } - state { - id 74 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 73 - treeNode [73 0 0 0] - superState SUBCHART - subviewer 73 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function nu_b = computeBaseVelocityFCN(J_LeftFoot, J_RightFoot, feetInContact, qjDot, Reg)\n\n " - " nu_b = wbc.computeBaseVelocity(J_LeftFoot, J_RightFoot, feetInContact, qjDot, Reg);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 75 - ssIdNumber 4 - name "J_LeftFoot" - 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 [73 0 76] - } - data { - id 76 - ssIdNumber 8 - name "J_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 [73 75 77] - } - data { - id 77 - ssIdNumber 9 - name "feetInContact" - 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 [73 76 78] - } - data { - id 78 - 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 [73 77 79] - } - data { - id 79 - ssIdNumber 6 - name "qjDot" - 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 [73 78 80] - } - data { - id 80 - 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 [73 79 0] - } - junction { - id 81 - position [23.5747 49.5747 7] - chart 73 - subviewer 73 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [73 0 0] - } - transition { - id 82 - 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 81 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 73 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 73 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [73 0 0] - } - instance { - id 83 - machine 1 - name "Robot State and References/State Machine Yoga/Compute State Velocity/Compute Base Velocity" - chart 73 - } - chart { - id 84 - machine 1 - name "Robot State and References/State Machine Yoga/Compute base to fixed link transform/LFoot to base link t" - "ransform /fromImuToHomogeousTransformFCN" - windowPosition [326.89 491.339 161 328] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.000677131425054] - treeNode [0 85 0 0] - viewObj 84 - ssIdHighWaterMark 88 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 8 - disableImplicitCasting 1 - eml { - name "worldToBaseTransformWithIMUFCN" - } - firstData 86 - firstTransition 95 - firstJunction 94 - } - state { - id 85 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 84 - treeNode [84 0 0 0] - superState SUBCHART - subviewer 84 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function w_H_b = worldToBaseTransformWithIMUFCN(imu_H_link,imu_H_link_0,link_H_base,inertial_0,in" - "ertial,neck_pos,Config)\n\n w_H_b = wbc.worldToBaseTransformWithIMU(imu_H_link,imu_H_link_0,link_H_base,inert" - "ial_0,inertial,neck_pos,Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 86 - ssIdNumber 82 - name "imu_H_link" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [84 0 87] - } - data { - id 87 - ssIdNumber 83 - name "imu_H_link_0" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [84 86 88] - } - data { - id 88 - ssIdNumber 81 - name "link_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 [84 87 89] - } - data { - id 89 - ssIdNumber 85 - name "inertial_0" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [84 88 90] - } - data { - id 90 - ssIdNumber 65 - name "inertial" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [84 89 91] - } - data { - id 91 - 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 [84 90 92] - } - data { - id 92 - 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 [84 91 93] - } - data { - id 93 - 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 [84 92 0] - } - junction { - id 94 - position [23.5747 49.5747 7] - chart 84 - subviewer 84 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [84 0 0] - } - transition { - id 95 - 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 94 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 84 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 84 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [84 0 0] - } - instance { - id 96 - machine 1 - name "Robot State and References/State Machine Yoga/Compute base to fixed link transform/LFoot to base link t" - "ransform /fromImuToHomogeousTransformFCN" - chart 84 - } - chart { - id 97 - machine 1 - name "controller_QP/Compute joint torques (motor reflected inertia)/(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 98 0 0] - viewObj 97 - ssIdHighWaterMark 5 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 9 - disableImplicitCasting 1 - eml { - name "computeMotorsInertiaFCN" - } - firstData 99 - firstTransition 102 - firstJunction 101 - } - state { - id 98 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 97 - treeNode [97 0 0 0] - superState SUBCHART - subviewer 97 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function reflectedInertia = computeMotorsInertiaFCN(Config)\n \n [reflectedInertia] = wbc." - "computeMotorsInertia(Config);\n\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 99 - 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 [97 0 100] - } - data { - id 100 - 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 [97 99 0] - } - junction { - id 101 - position [23.5747 49.5747 7] - chart 97 - subviewer 97 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [97 0 0] - } - transition { - id 102 - 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 101 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 97 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 97 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [97 0 0] - } - instance { - id 103 - machine 1 - name "controller_QP/Compute joint torques (motor reflected inertia)/(transpose(T*Gamma))^{-1}*I_m*(T*Gamma)^{" - "-1}" - chart 97 - } - chart { - id 104 - machine 1 - name "Robot State and References/Internal Coordinator/Reference Generator CoM" - windowPosition [319.546 -65.92 97 534.4] - viewLimits [0 112 0 601] - screen [1 1 1366 768 1.25] - treeNode [0 105 0 0] - viewObj 104 - ssIdHighWaterMark 19 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 10 - disableImplicitCasting 1 - eml { - name "referenceGeneratorCoMFCN" - } - supportVariableSizing 0 - firstData 106 - firstTransition 111 - firstJunction 110 - } - state { - id 105 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 104 - treeNode [104 0 0 0] - superState SUBCHART - subviewer 104 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function references_CoM = referenceGeneratorCoMFCN(pos_CoM_0, t, Config)\n \n references_Co" - "M = wbc.referenceGeneratorCoM(pos_CoM_0, t, Config);\nend\n" - 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 106 - ssIdNumber 17 - 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 [104 0 107] - } - data { - id 107 - ssIdNumber 13 - name "t" - 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 [104 106 108] - } - data { - id 108 - ssIdNumber 4 - name "references_CoM" - 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 [104 107 109] - } - data { - id 109 - 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 [104 108 0] - } - junction { - id 110 - position [23.5747 49.5747 7] - chart 104 - subviewer 104 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [104 0 0] - } - transition { - id 111 - labelString "{eML_blk_kernel();}" - labelPosition [40.125 31.875 104 16.042] - fontSize 12 - src { - intersection [0 0 1 0 23.5747 14.625 0 0] - } - dst { - id 110 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 104 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 104 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [104 0 0] - } - instance { - id 112 - machine 1 - name "Robot State and References/Internal Coordinator/Reference Generator CoM" - chart 104 - } - chart { - id 113 - machine 1 - name "tauDot Saturation/Saturate the Torque Derivative" - windowPosition [422 539.941 189 413] - viewLimits [0 156.75 0 153.75] - screen [1 1 3600 1200 1.180555555555556] - treeNode [0 114 0 0] - viewObj 113 - ssIdHighWaterMark 7 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 11 - disableImplicitCasting 1 - eml { - name "saturateInputDerivativeFCN" - } - firstData 115 - firstTransition 120 - firstJunction 119 - } - state { - id 114 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 113 - treeNode [113 0 0 0] - superState SUBCHART - subviewer 113 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function uSat = saturateInputDerivativeFCN(u, u_0, Config)\n\n uSat = saturateInputDerivative(" - "u, u_0, Config);\n\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 115 - 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 [113 0 116] - } - data { - id 116 - 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 [113 115 117] - } - data { - id 117 - 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 [113 116 118] - } - data { - id 118 - ssIdNumber 7 - 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 [113 117 0] - } - junction { - id 119 - position [23.5747 49.5747 7] - chart 113 - subviewer 113 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [113 0 0] - } - transition { - id 120 - labelString "{eML_blk_kernel();}" - labelPosition [28.125 13.875 102.544 14.964] - fontSize 12 - src { - intersection [0 0 1 0 23.5747 14.625 0 0] - } - dst { - id 119 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 113 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 113 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [113 0 0] - } - instance { - id 121 - machine 1 - name "tauDot Saturation/Saturate the Torque Derivative" - chart 113 - } - chart { - id 122 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/One Foot/Process QP output" - windowPosition [422 539.941 189 413] - viewLimits [0 156.75 0 153.75] - screen [1 1 3600 1200 1.180555555555556] - treeNode [0 123 0 0] - viewObj 122 - ssIdHighWaterMark 13 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 12 - disableImplicitCasting 1 - eml { - name "processOutputQPFCN" - } - firstData 124 - firstTransition 130 - firstJunction 129 - } - state { - id 123 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 122 - treeNode [122 0 0 0] - superState SUBCHART - subviewer 122 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function f0 = processOutputQPFCN(analyticalSolution,primalSolution,QPStatus,Config)\n\n f0 = " - "wbc.processOutputQP(analyticalSolution,primalSolution,QPStatus,Config);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 124 - 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 [122 0 125] - } - data { - id 125 - 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 [122 124 126] - } - data { - id 126 - 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 [122 125 127] - } - data { - id 127 - ssIdNumber 5 - name "f0" - scope OUTPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_NO - unit { - name "inherit" - } - } - dataType "Inherit: Same as Simulink" - linkNode [122 126 128] - } - data { - id 128 - 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 [122 127 0] - } - junction { - id 129 - position [23.5747 49.5747 7] - chart 122 - subviewer 122 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [122 0 0] - } - transition { - id 130 - 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 129 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 122 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 122 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [122 0 0] - } - instance { - id 131 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/One Foot/Process QP output" - chart 122 - } - chart { - id 132 - machine 1 - name "Robot State and References/State Machine Yoga/stateMachineYogaFCN" - windowPosition [326.89 491.339 161 328] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.000677131425054] - treeNode [0 133 0 0] - viewObj 132 - ssIdHighWaterMark 84 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 13 - disableImplicitCasting 1 - eml { - name "stateMachineYogaFCN" - } - firstData 134 - firstTransition 156 - firstJunction 155 - } - state { - id 133 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 132 - treeNode [132 0 0 0] - superState SUBCHART - subviewer 132 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function [w_H_b, CoM_des, qj_des, constraints, impedances, KPCoM, KDCoM, currentState, jointsSmoo" - "thingTime] = ...\n stateMachineYogaFCN(wrench_rightFoot, wrench_leftFoot, CoM_0, q0, l_sole_CoM, r_sole" - "_CoM, qj, t, l_sole_H_b, r_sole_H_b, Sm, Gain)\n \n [w_H_b, CoM_des, qj_des, constraints, impedances, KPCoM, " - "KDCoM, currentState, jointsSmoothingTime] = ...\n stateMachineYoga(CoM_0, q0, l_sole_CoM, r_sole_CoM, qj," - " t, wrench_rightFoot, wrench_leftFoot, l_sole_H_b, r_sole_H_b, Sm, Gain); \nend\n \n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 134 - 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 [132 0 135] - } - data { - id 135 - 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 [132 134 136] - } - data { - id 136 - 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 [132 135 137] - } - data { - id 137 - ssIdNumber 52 - name "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 [132 136 138] - } - data { - id 138 - ssIdNumber 46 - name "qj_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 [132 137 139] - } - data { - id 139 - ssIdNumber 43 - name "constraints" - 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 [132 138 140] - } - data { - id 140 - ssIdNumber 37 - name "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 [132 139 141] - } - data { - id 141 - ssIdNumber 54 - name "q0" - 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 [132 140 142] - } - data { - id 142 - ssIdNumber 64 - name "l_sole_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 [132 141 143] - } - data { - id 143 - ssIdNumber 77 - name "r_sole_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 [132 142 144] - } - data { - id 144 - ssIdNumber 69 - name "qj" - 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 [132 143 145] - } - data { - id 145 - ssIdNumber 56 - name "t" - 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 [132 144 146] - } - data { - id 146 - 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 [132 145 147] - } - data { - id 147 - 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 [132 146 148] - } - data { - id 148 - ssIdNumber 55 - name "Sm" - 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 [132 147 149] - } - data { - id 149 - ssIdNumber 66 - name "impedances" - 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 [132 148 150] - } - data { - id 150 - ssIdNumber 82 - name "KPCoM" - 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 [132 149 151] - } - data { - id 151 - ssIdNumber 83 - name "KDCoM" - 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 [132 150 152] - } - data { - id 152 - ssIdNumber 67 - name "Gain" - 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 [132 151 153] - } - data { - id 153 - ssIdNumber 68 - name "currentState" - 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 [132 152 154] - } - data { - id 154 - ssIdNumber 81 - name "jointsSmoothingTime" - 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 [132 153 0] - } - junction { - id 155 - position [23.5747 49.5747 7] - chart 132 - subviewer 132 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [132 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 132 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 132 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [132 0 0] - } - instance { - id 157 - machine 1 - name "Robot State and References/State Machine Yoga/stateMachineYogaFCN" - chart 132 - } - chart { - id 158 - machine 1 - name "emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/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 159 0 0] - viewObj 158 - ssIdHighWaterMark 8 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 14 - disableImplicitCasting 1 - eml { - name "checkSpikes" - } - supportVariableSizing 0 - firstData 160 - firstTransition 164 - firstJunction 163 - } - 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 noSpikes = checkSpikes(u, delta_u_max)\n\n noSpikes = wbc.checkSpikes(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 160 - 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 [158 0 161] - } - data { - id 161 - 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 [158 160 162] - } - data { - id 162 - 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 [158 161 0] - } - junction { - id 163 - position [23.5747 49.5747 7] - chart 158 - subviewer 158 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [158 0 0] - } - transition { - id 164 - 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 163 - 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 165 - machine 1 - name "emergency stop: joint limits/STOP IF THERE ARE SPIKES IN THE ENCODERS/MATLAB Function" - chart 158 - } - chart { - id 166 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/ContactsTransition" - windowPosition [422 539.941 189 413] - viewLimits [0 156.75 0 153.75] - screen [1 1 3600 1200 1.180555555555556] - treeNode [0 167 0 0] - viewObj 166 - ssIdHighWaterMark 5 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 15 - disableImplicitCasting 1 - eml { - name "contactsTransitionQPFCN" - } - firstData 168 - firstTransition 171 - firstJunction 170 - } - state { - id 167 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 166 - treeNode [166 0 0 0] - superState SUBCHART - subviewer 166 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function onOneFoot = contactsTransitionQPFCN(LR_FootInContact)\n\n onOneFoot = wbc.contactsTra" - "nsitionQP(LR_FootInContact);\nend\n" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 168 - ssIdNumber 4 - name "LR_FootInContact" - 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 [166 0 169] - } - data { - id 169 - 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 [166 168 0] - } - junction { - id 170 - position [23.5747 49.5747 7] - chart 166 - subviewer 166 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [166 0 0] - } - transition { - id 171 - 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 170 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 166 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 166 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [166 0 0] - } - instance { - id 172 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/ContactsTransition" - chart 166 - } - chart { - id 173 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/Process One Foot Output" - windowPosition [422 539.941 189 413] - viewLimits [0 156.75 0 153.75] - screen [1 1 3600 1200 1.180555555555556] - treeNode [0 174 0 0] - viewObj 173 - ssIdHighWaterMark 6 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 16 - disableImplicitCasting 1 - eml { - name "processOneFootOutputQPFCN" - } - firstData 175 - firstTransition 179 - firstJunction 178 - } - state { - id 174 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 173 - treeNode [173 0 0 0] - superState SUBCHART - subviewer 173 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function f0_oneFoot = processOneFootOutputQPFCN(primalSolution,LR_FootInContact)\n \n f0_on" - "eFoot = wbc.processOneFootOutputQP(primalSolution,LR_FootInContact);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 175 - 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 [173 0 176] - } - data { - id 176 - ssIdNumber 5 - name "f0_oneFoot" - 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 [173 175 177] - } - data { - id 177 - ssIdNumber 6 - name "LR_FootInContact" - 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 [173 176 0] - } - junction { - id 178 - position [23.5747 49.5747 7] - chart 173 - subviewer 173 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [173 0 0] - } - transition { - id 179 - 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 178 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 173 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 173 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [173 0 0] - } - instance { - id 180 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/Process One Foot Output" - chart 173 - } - chart { - id 181 - machine 1 - name "controller_QP/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 182 0 0] - viewObj 181 - ssIdHighWaterMark 82 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 17 - disableImplicitCasting 1 - eml { - name "balancingControllerYogaFCN" - } - firstData 183 - firstTransition 223 - firstJunction 222 - } - state { - id 182 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 181 - treeNode [181 0 0 0] - superState SUBCHART - subviewer 181 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function [tauModel, Sigma, NA, fLdotDesC1C2, ...\n HessianMatrixQP1Foot, gradientQP1Foot" - ", ConstraintsMatrixQP1Foot, bVectorConstraintsQp1Foot, ...\n HessianMatrixQP2Feet, gradientQP2Feet, Con" - "straintsMatrixQP2Feet, bVectorConstraintsQp2Feet, ...\n errorCoM,f_noQP] = ...\n balancing" - "ControllerYogaFCN(constraints, ROBOT_DOF_FOR_SIMULINK, ConstraintsMatrix, bVectorConstraints, ...\n " - " qj, qjDes, nu, M, h, L, intLw, w_H_l_sole, w_H_r_sole, JL, JR, dJLv, dJRv, xCoM, J_CoM, desired_x_dx_ddx_Co" - "M, ...\n gainsPCOM, gainsDCOM, impedances, Gain, Reg)\n \n [tauModel, Sigma, NA, fL" - "dotDesC1C2, ...\n HessianMatrixQP1Foot, gradientQP1Foot, ConstraintsMatrixQP1Foot, bVectorConstraintsQp1Foot" - ", ...\n HessianMatrixQP2Feet, gradientQP2Feet, ConstraintsMatrixQP2Feet, bVectorConstraintsQp2Feet, ...\n " - " errorCoM, f_noQP] = ...\n balancingControllerYoga(constraints, ROBOT_DOF_FOR_SIMULINK, ConstraintsMatr" - "ix, bVectorConstraints, ...\n qj, qjDes, nu, M, h, L, intLw, w_H_l_sole, w_H_r_s" - "ole, JL, JR, dJLv, dJRv, xCoM, J_CoM, desired_x_dx_ddx_CoM, ...\n gainsPCOM, gai" - "nsDCOM, impedances, Reg, Gain);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 183 - 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 [181 0 184] - } - data { - id 184 - 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 [181 183 185] - } - data { - id 185 - ssIdNumber 66 - 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 [181 184 186] - } - data { - id 186 - ssIdNumber 64 - name "fLdotDesC1C2" - 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 [181 185 187] - } - data { - id 187 - ssIdNumber 5 - name "HessianMatrixQP1Foot" - 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 [181 186 188] - } - data { - id 188 - ssIdNumber 52 - name "gradientQP1Foot" - 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 [181 187 189] - } - data { - id 189 - ssIdNumber 69 - name "ConstraintsMatrixQP1Foot" - 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 [181 188 190] - } - data { - id 190 - ssIdNumber 70 - name "bVectorConstraintsQp1Foot" - 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 [181 189 191] - } - data { - id 191 - ssIdNumber 53 - name "HessianMatrixQP2Feet" - 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 [181 190 192] - } - data { - id 192 - ssIdNumber 54 - name "gradientQP2Feet" - 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 [181 191 193] - } - data { - id 193 - ssIdNumber 62 - name "ConstraintsMatrixQP2Feet" - 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 [181 192 194] - } - data { - id 194 - ssIdNumber 63 - name "bVectorConstraintsQp2Feet" - 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 [181 193 195] - } - data { - id 195 - ssIdNumber 13 - name "constraints" - 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 [181 194 196] - } - data { - id 196 - ssIdNumber 14 - name "ROBOT_DOF_FOR_SIMULINK" - 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 [181 195 197] - } - data { - id 197 - 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 [181 196 198] - } - data { - id 198 - 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 [181 197 199] - } - data { - id 199 - ssIdNumber 4 - name "qj" - 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 [181 198 200] - } - data { - id 200 - ssIdNumber 6 - name "qjDes" - 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 [181 199 201] - } - data { - id 201 - 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 [181 200 202] - } - data { - id 202 - 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 [181 201 203] - } - data { - id 203 - 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 [181 202 204] - } - data { - id 204 - 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 [181 203 205] - } - data { - id 205 - ssIdNumber 77 - name "intLw" - 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 [181 204 206] - } - data { - id 206 - 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 [181 205 207] - } - data { - id 207 - 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 [181 206 208] - } - data { - id 208 - ssIdNumber 38 - 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 [181 207 209] - } - data { - id 209 - ssIdNumber 32 - 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 [181 208 210] - } - data { - id 210 - ssIdNumber 33 - name "dJLv" - 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 [181 209 211] - } - data { - id 211 - ssIdNumber 40 - name "dJRv" - 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 [181 210 212] - } - data { - id 212 - ssIdNumber 15 - name "xCoM" - 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 [181 211 213] - } - data { - id 213 - 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 [181 212 214] - } - data { - id 214 - ssIdNumber 17 - name "desired_x_dx_ddx_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 [181 213 215] - } - data { - id 215 - ssIdNumber 81 - name "gainsPCOM" - 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 [181 214 216] - } - data { - id 216 - ssIdNumber 82 - name "gainsDCOM" - 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 [181 215 217] - } - data { - id 217 - ssIdNumber 19 - name "impedances" - 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 [181 216 218] - } - data { - id 218 - ssIdNumber 48 - name "errorCoM" - 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 [181 217 219] - } - data { - id 219 - ssIdNumber 49 - name "f_noQP" - 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 [181 218 220] - } - data { - id 220 - ssIdNumber 47 - name "Gain" - 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 [181 219 221] - } - data { - id 221 - ssIdNumber 20 - name "Reg" - 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 [181 220 0] - } - junction { - id 222 - position [23.5747 49.5747 7] - chart 181 - subviewer 181 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [181 0 0] - } - transition { - id 223 - 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 222 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 181 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 181 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [181 0 0] - } - instance { - id 224 - machine 1 - name "controller_QP/Balancing Controller\n" - chart 181 - } - chart { - id 225 - 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 226 0 0] - viewObj 225 - ssIdHighWaterMark 8 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 18 - disableImplicitCasting 1 - eml { - name "checkRangeFCN" - } - supportVariableSizing 0 - firstData 227 - firstTransition 233 - firstJunction 232 - } - state { - id 226 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 225 - treeNode [225 0 0 0] - superState SUBCHART - subviewer 225 - 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" - 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 227 - 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 [225 0 228] - } - data { - id 228 - 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 [225 227 229] - } - data { - id 229 - 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 [225 228 230] - } - data { - id 230 - 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 [225 229 231] - } - data { - id 231 - 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 [225 230 0] - } - junction { - id 232 - position [23.5747 49.5747 7] - chart 225 - subviewer 225 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [225 0 0] - } - transition { - id 233 - 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 232 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 225 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 225 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [225 0 0] - } - instance { - id 234 - machine 1 - name "emergency stop: joint limits/STOP IF JOINTS HIT THE LIMITS/MATLAB Function" - chart 225 - } - chart { - id 235 - machine 1 - name "controller_QP/Compute angular momentum integral/choose base 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 236 0 0] - viewObj 235 - ssIdHighWaterMark 5 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 19 - disableImplicitCasting 1 - eml { - name "footOnGround" - } - firstData 237 - firstTransition 240 - firstJunction 239 - } - state { - id 236 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 235 - treeNode [235 0 0 0] - superState SUBCHART - subviewer 235 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function 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\n bool" - "eanState = 1;\n\n if state == 9 || state == 10 || state == 11 || state == 12\n\n booleanState = 0;\n " - " end\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 237 - 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 [235 0 238] - } - data { - id 238 - 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 [235 237 0] - } - junction { - id 239 - position [23.5747 49.5747 7] - chart 235 - subviewer 235 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [235 0 0] - } - transition { - id 240 - 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 239 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 235 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 235 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [235 0 0] - } - instance { - id 241 - machine 1 - name "controller_QP/Compute angular momentum integral/choose base to world transform" - chart 235 - } - chart { - id 242 - machine 1 - name "controller_QP/Compute angular momentum integral/References for L" - windowPosition [357.12 483.496 167 391] - viewLimits [0 156.75 0 153.75] - screen [1 1 1280 1024 1.041666666666667] - treeNode [0 243 0 0] - viewObj 242 - ssIdHighWaterMark 10 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 20 - disableImplicitCasting 1 - eml { - name "generateReferencesForH_FCN" - } - firstData 244 - firstTransition 251 - firstJunction 250 - } - state { - id 243 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 242 - treeNode [242 0 0 0] - superState SUBCHART - subviewer 242 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function nu_b_equivalent = generateReferencesForH_FCN(qjErr, JL, JR, Reg, activeFeetConstraints)\n" - "\n if activeFeetConstraints(1) == 1\n \n pinvJb = (JL(:,1:6)'*JL(:,1:6) + Reg.pinvDamp_nu_" - "b*eye(6))\\JL(:,1:6)';\n nu_b_equivalent = -pinvJb*JL(:,7:end)*qjErr;\n else\n \n pinvJb " - " = (JR(:,1:6)'*JR(:,1:6) + Reg.pinvDamp_nu_b*eye(6))\\JR(:,1:6)';\n nu_b_equivalent = -pinvJb*JR(:,7" - ":end)*qjErr;\n end\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 244 - ssIdNumber 4 - name "qjErr" - 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 [242 0 245] - } - data { - id 245 - ssIdNumber 6 - 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 [242 244 246] - } - data { - id 246 - ssIdNumber 9 - 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 [242 245 247] - } - data { - id 247 - ssIdNumber 7 - name "nu_b_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 [242 246 248] - } - data { - id 248 - 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 [242 247 249] - } - data { - id 249 - ssIdNumber 10 - name "activeFeetConstraints" - 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 [242 248 0] - } - junction { - id 250 - position [23.5747 49.5747 7] - chart 242 - subviewer 242 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [242 0 0] - } - transition { - id 251 - 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 250 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 242 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 242 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [242 0 0] - } - instance { - id 252 - machine 1 - name "controller_QP/Compute angular momentum integral/References for L" - chart 242 - } - chart { - id 253 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/Two Feet/Analytical 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 254 0 0] - viewObj 253 - ssIdHighWaterMark 6 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 21 - disableImplicitCasting 1 - eml { - name "analyticalSolutionQPFCN" - } - firstData 255 - firstTransition 259 - firstJunction 258 - } - state { - id 254 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 253 - treeNode [253 0 0 0] - superState SUBCHART - subviewer 253 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function analyticalSolution = analyticalSolutionQPFCN(H,g)\n\n analyticalSolution = wbc.analyt" - "icalSolutionQP(H,g);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 255 - 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 [253 0 256] - } - data { - id 256 - 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 [253 255 257] - } - data { - id 257 - 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 [253 256 0] - } - junction { - id 258 - position [23.5747 49.5747 7] - chart 253 - subviewer 253 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [253 0 0] - } - transition { - id 259 - 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 258 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 253 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 253 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [253 0 0] - } - instance { - id 260 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/Two Feet/Analytical Solution Two Feet (unconstrained)" - chart 253 - } - chart { - id 261 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/One Foot/Analytical Solution 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 262 0 0] - viewObj 261 - ssIdHighWaterMark 6 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 22 - disableImplicitCasting 1 - eml { - name "analyticalSolutionQPFCN" - } - firstData 263 - firstTransition 267 - firstJunction 266 - } - state { - id 262 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 261 - treeNode [261 0 0 0] - superState SUBCHART - subviewer 261 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function analyticalSolution = analyticalSolutionQPFCN(H,g)\n\n analyticalSolution = wbc.analyt" - "icalSolutionQP(H,g);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 263 - 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 [261 0 264] - } - data { - id 264 - 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 [261 263 265] - } - data { - id 265 - 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 [261 264 0] - } - junction { - id 266 - position [23.5747 49.5747 7] - chart 261 - subviewer 261 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [261 0 0] - } - transition { - id 267 - 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 266 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 261 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 261 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [261 0 0] - } - instance { - id 268 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/One Foot/Analytical Solution One Foot (unconstrained)" - chart 261 - } - chart { - id 269 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/Two Feet/Process QP output" - windowPosition [422 539.941 189 413] - viewLimits [0 156.75 0 153.75] - screen [1 1 3600 1200 1.180555555555556] - treeNode [0 270 0 0] - viewObj 269 - ssIdHighWaterMark 13 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 23 - disableImplicitCasting 1 - eml { - name "processOutputQPFCN" - } - firstData 271 - firstTransition 277 - firstJunction 276 - } - state { - id 270 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 269 - treeNode [269 0 0 0] - superState SUBCHART - subviewer 269 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function f0 = processOutputQPFCN(analyticalSolution,primalSolution,QPStatus,Config)\n\n f0 = " - "wbc.processOutputQP(analyticalSolution,primalSolution,QPStatus,Config);\nend" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 271 - 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 [269 0 272] - } - data { - id 272 - 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 [269 271 273] - } - data { - id 273 - 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 [269 272 274] - } - data { - id 274 - ssIdNumber 5 - name "f0" - 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 [269 273 275] - } - data { - id 275 - 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 [269 274 0] - } - junction { - id 276 - position [23.5747 49.5747 7] - chart 269 - subviewer 269 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [269 0 0] - } - transition { - id 277 - 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 276 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 269 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 269 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [269 0 0] - } - instance { - id 278 - machine 1 - name "controller_QP/Compute joint torques/QPSolver/Two Feet/Process QP output" - chart 269 - } - target { - id 279 - machine 1 - name "sfun" - codeFlags "" - linkNode [1 0 280] - } - target { - id 280 - machine 1 - name "rtw" - linkNode [1 279 0] - } -} diff --git a/utilities/README.md b/utilities/README.md index f2dbbc0..ef80274 100644 --- a/utilities/README.md +++ b/utilities/README.md @@ -2,17 +2,16 @@ Simulink models for debugging. -- `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. +- `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. -- `debug_FTMeas.mdl`: This model is used for checking the raw measurements coming from iCub legs, arms and feet FT sensors. **USAGE:** this model is supposed to be used only with `iCubGenova02` and `iCubGenova04` robots. +- `debug_FTMeas.mdl`: this model is used for checking the raw measurements coming from iCub legs, arms and feet FT sensors. **USAGE:** this model is supposed to be used only with `iCubGenova02` and `iCubGenova04` robots. -- `debug_FTMeas_shoes.mdl`: This model is used for checking the raw measurements coming from the sensorized shoes FT sensors. **USAGE:** this model is supposed to be used only with the `sensorized shoes`. +- `debug_FTMeas_shoes.mdl`: this model is used for checking the raw measurements coming from the sensorized shoes FT sensors. **USAGE:** this model is supposed to be used only with the `sensorized shoes`. -- `debug_BoschIMU.mdl`: This model is used for checking if the iCub Bosch IMU (located in the iCub head) is correclty aligned with the gravity. **USAGE:** this model is supposed to be used only with `iCubGenova02` and `iCubGenova04` robots, and only with the robot on the pole. +- `debug_BoschIMU.mdl`: this model is used for checking if the iCub Bosch IMU (located in the iCub head) is correclty aligned with the gravity. **USAGE:** this model is supposed to be used only with `iCubGenova02` and `iCubGenova04` robots, and only with the robot on the pole. -- `debug_xSensIMU.mdl`: This model is used for checking if the iCub xSens IMU (located in the iCub root link) is correclty aligned with the gravity. **USAGE:** this model is supposed to be used only with `iCubGenova04` robot. +- `debug_xSensIMU.mdl`: this model is used for checking if the iCub xSens IMU (located in the iCub root link) is correclty aligned with the gravity. **USAGE:** this model is supposed to be used only with `iCubGenova04` robot on the pole. -- `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`. Run on a terminal `cd PATH/TO/THIS/FOLDER && sh releaseLegStressesWhileStanding.sh`. +- `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 diff --git a/utilities/debug_BoschIMU.mdl b/utilities/debug_BoschIMU.mdl index 9b123db..961e531 100644 --- a/utilities/debug_BoschIMU.mdl +++ b/utilities/debug_BoschIMU.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.34" + ComputedModelVersion "1.39" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -24,8 +24,8 @@ Model { Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_BoschIMU/Yarp Read" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_BoschIMU/YarpRead" SID "17" Type "LIBRARY_BLOCK" } @@ -58,7 +58,7 @@ Model { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [65.0, 24.0, 1301.0, 744.0] + Location [134.0, 55.0, 3706.0, 2105.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -82,9 +82,9 @@ Model { IsActive [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [1263.0, 595.0] - ZoomFactor [3.3378378378378377] - Offset [90.805668016194318, 30.37044534412955] + Extents [3668.0, 1809.0] + ZoomFactor [9.829878422111431] + Offset [93.5757707879165, 25.984619833607681] } Object { $PropName "DockComponentsInfo" @@ -102,13 +102,13 @@ Model { } WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABiAP///wAAAAEAAAAAAAAAAPwCA" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAC6AP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAALAD///8AAAUVAAACkQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "QBjAHQAbwByAAAAAAD/////AAAAOgD///8AAA56AAAHbQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////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/" - "////wEAAAOD/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" + "////wEAAACe/////wAAAAAAAAAA/////wEAAAEM/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGq/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAASj/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" } } HideAutomaticNames on @@ -118,9 +118,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Dec 14 14:42:27 2018" - RTWModifiedTimeStamp 466699347 - ModelVersionFormat "1.%" + LastModifiedDate "Fri Mar 08 16:25:43 2019" + RTWModifiedTimeStamp 473963143 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -996,7 +996,7 @@ Model { } System { Name "debug_BoschIMU" - Location [65, 24, 1366, 768] + Location [134, 55, 3840, 2160] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -1009,14 +1009,14 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "334" + ZoomFactor "983" ReportName "simulink-default.rpt" SIDHighWatermark "17" Block { BlockType Constant Name "Constant" SID "12" - Position [265, 117, 305, 153] + Position [255, 117, 295, 153] ZOrder 33 ShowName off Value "0" @@ -1058,10 +1058,10 @@ Model { ForegroundColor "[0.916667, 0.916667, 0.916667]" BackgroundColor "gray" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" period "0.01" } @@ -1070,7 +1070,7 @@ Model { Name "Selector" SID "11" Ports [1, 1] - Position [265, 66, 305, 104] + Position [255, 66, 295, 104] ZOrder 32 ShowName off InputPortWidth "12" @@ -1080,17 +1080,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read" + Name "YarpRead" SID "17" Ports [0, 1] Position [140, 73, 180, 97] ZOrder 515 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/inertial'" signalSize "12" @@ -1108,7 +1108,7 @@ Model { Name "Bosch IMU" ZOrder 13 Labels [1, 1] - SrcBlock "Yarp Read" + SrcBlock "YarpRead" SrcPort 1 DstBlock "Selector" DstPort 1 diff --git a/utilities/debug_FTExternalForces.mdl b/utilities/debug_FTExternalForces.mdl index 5e8975a..6ec97bc 100644 --- a/utilities/debug_FTExternalForces.mdl +++ b/utilities/debug_FTExternalForces.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.42" + ComputedModelVersion "1.49" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -24,38 +24,38 @@ Model { Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTExternalForces/Yarp Read" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTExternalForces/YarpRead" SID "17" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTExternalForces/Yarp Read1" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTExternalForces/YarpRead1" SID "20" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTExternalForces/Yarp Read2" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTExternalForces/YarpRead2" SID "30" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTExternalForces/Yarp Read3" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTExternalForces/YarpRead3" SID "31" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTExternalForces/Yarp Read4" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTExternalForces/YarpRead4" SID "39" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTExternalForces/Yarp Read5" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTExternalForces/YarpRead5" SID "40" Type "LIBRARY_BLOCK" } @@ -88,7 +88,7 @@ Model { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [65.0, 24.0, 1855.0, 1056.0] + Location [134.0, 55.0, 3706.0, 2105.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -112,9 +112,9 @@ Model { IsActive [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [1821.0, 911.0] - ZoomFactor [1.0499405469678953] - Offset [-1714.6919592298982, -84.334088335220883] + Extents [3668.0, 1809.0] + ZoomFactor [2.117717003567182] + Offset [-1713.050733710495, -77.610892756878172] } Object { $PropName "DockComponentsInfo" @@ -132,12 +132,12 @@ Model { } WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABeAP///wAAAAEAAAAAAAAAAPwCA" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAC6AP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAAKAD///8AAAc/AAADyQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "QBjAHQAbwByAAAAAAD/////AAAAOgD///8AAA56AAAHbQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////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/" + "////wEAAACe/////wAAAAAAAAAA/////wEAAAEM/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGq/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAASj/////wAAAAAAAAAA/" "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" } } @@ -148,9 +148,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Dec 14 14:42:28 2018" - RTWModifiedTimeStamp 466699348 - ModelVersionFormat "1.%" + LastModifiedDate "Fri Mar 08 16:25:44 2019" + RTWModifiedTimeStamp 473963144 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -1106,7 +1106,7 @@ Model { } System { Name "debug_FTExternalForces" - Location [65, 24, 1920, 1080] + Location [134, 55, 3840, 2160] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -1119,9 +1119,9 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "105" + ZoomFactor "212" ReportName "simulink-default.rpt" - SIDHighWatermark "58" + SIDHighWatermark "63" Block { BlockType Demux Name "Demux" @@ -1264,7 +1264,7 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "45" + SIDHighWatermark "57" Block { BlockType Inport Name "u" @@ -1276,20 +1276,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "43::44" + SID "43::56" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 35 + ZOrder 47 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "43::43" + SID "43::55" Tag "Stateflow S-Function debug_FTExternalForces 2" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 34 + ZOrder 46 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1303,9 +1303,9 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "43::45" + SID "43::57" Position [460, 241, 480, 259] - ZOrder 36 + ZOrder 48 } Block { BlockType Outport @@ -1316,7 +1316,7 @@ Model { IconDisplay "Port number" } Line { - ZOrder 37 + ZOrder 53 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1324,7 +1324,7 @@ Model { } Line { Name "y" - ZOrder 38 + ZOrder 54 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1332,14 +1332,14 @@ Model { DstPort 1 } Line { - ZOrder 39 + ZOrder 55 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 40 + ZOrder 56 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1350,10 +1350,10 @@ Model { Block { BlockType SubSystem Name "MATLAB Function1" - SID "44" + SID "59" Ports [1, 1] Position [-905, 432, -835, 478] - ZOrder 546 + ZOrder 561 ErrorFcn "Stateflow.Translate.translate" PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on @@ -1375,11 +1375,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "45" + SIDHighWatermark "57" Block { BlockType Inport Name "u" - SID "44::1" + SID "59::1" Position [20, 101, 40, 119] ZOrder -1 IconDisplay "Port number" @@ -1387,20 +1387,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "44::44" + SID "59::56" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 35 + ZOrder 47 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "44::43" + SID "59::55" Tag "Stateflow S-Function debug_FTExternalForces 1" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 34 + ZOrder 46 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1414,20 +1414,20 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "44::45" + SID "59::57" Position [460, 241, 480, 259] - ZOrder 36 + ZOrder 48 } Block { BlockType Outport Name "y" - SID "44::5" + SID "59::5" Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" } Line { - ZOrder 33 + ZOrder 13 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1435,7 +1435,7 @@ Model { } Line { Name "y" - ZOrder 34 + ZOrder 14 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1443,14 +1443,14 @@ Model { DstPort 1 } Line { - ZOrder 35 + ZOrder 15 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 36 + ZOrder 16 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1461,10 +1461,10 @@ Model { Block { BlockType SubSystem Name "MATLAB Function2" - SID "45" + SID "60" Ports [1, 1] - Position [-905, 627, -835, 673] - ZOrder 547 + Position [-905, 497, -835, 543] + ZOrder 562 ErrorFcn "Stateflow.Translate.translate" PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on @@ -1486,11 +1486,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "45" + SIDHighWatermark "57" Block { BlockType Inport Name "u" - SID "45::1" + SID "60::1" Position [20, 101, 40, 119] ZOrder -1 IconDisplay "Port number" @@ -1498,20 +1498,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "45::44" + SID "60::56" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 35 + ZOrder 47 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "45::43" + SID "60::55" Tag "Stateflow S-Function debug_FTExternalForces 3" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 34 + ZOrder 46 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1525,20 +1525,20 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "45::45" + SID "60::57" Position [460, 241, 480, 259] - ZOrder 36 + ZOrder 48 } Block { BlockType Outport Name "y" - SID "45::5" + SID "60::5" Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" } Line { - ZOrder 33 + ZOrder 13 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1546,7 +1546,7 @@ Model { } Line { Name "y" - ZOrder 34 + ZOrder 14 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1554,14 +1554,14 @@ Model { DstPort 1 } Line { - ZOrder 35 + ZOrder 15 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 36 + ZOrder 16 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1572,10 +1572,10 @@ Model { Block { BlockType SubSystem Name "MATLAB Function3" - SID "46" + SID "61" Ports [1, 1] - Position [-905, 692, -835, 738] - ZOrder 548 + Position [-905, 562, -835, 608] + ZOrder 563 ErrorFcn "Stateflow.Translate.translate" PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on @@ -1597,11 +1597,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "45" + SIDHighWatermark "57" Block { BlockType Inport Name "u" - SID "46::1" + SID "61::1" Position [20, 101, 40, 119] ZOrder -1 IconDisplay "Port number" @@ -1609,20 +1609,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "46::44" + SID "61::56" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 35 + ZOrder 47 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "46::43" + SID "61::55" Tag "Stateflow S-Function debug_FTExternalForces 4" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 34 + ZOrder 46 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1636,20 +1636,20 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "46::45" + SID "61::57" Position [460, 241, 480, 259] - ZOrder 36 + ZOrder 48 } Block { BlockType Outport Name "y" - SID "46::5" + SID "61::5" Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" } Line { - ZOrder 33 + ZOrder 13 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1657,7 +1657,7 @@ Model { } Line { Name "y" - ZOrder 34 + ZOrder 14 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1665,14 +1665,14 @@ Model { DstPort 1 } Line { - ZOrder 35 + ZOrder 15 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 36 + ZOrder 16 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1683,10 +1683,10 @@ Model { Block { BlockType SubSystem Name "MATLAB Function4" - SID "47" + SID "62" Ports [1, 1] - Position [-905, 497, -835, 543] - ZOrder 549 + Position [-905, 627, -835, 673] + ZOrder 564 ErrorFcn "Stateflow.Translate.translate" PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on @@ -1708,11 +1708,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "45" + SIDHighWatermark "57" Block { BlockType Inport Name "u" - SID "47::1" + SID "62::1" Position [20, 101, 40, 119] ZOrder -1 IconDisplay "Port number" @@ -1720,20 +1720,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "47::44" + SID "62::56" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 35 + ZOrder 47 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "47::43" + SID "62::55" Tag "Stateflow S-Function debug_FTExternalForces 5" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 34 + ZOrder 46 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1747,20 +1747,20 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "47::45" + SID "62::57" Position [460, 241, 480, 259] - ZOrder 36 + ZOrder 48 } Block { BlockType Outport Name "y" - SID "47::5" + SID "62::5" Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" } Line { - ZOrder 33 + ZOrder 13 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1768,7 +1768,7 @@ Model { } Line { Name "y" - ZOrder 34 + ZOrder 14 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1776,14 +1776,14 @@ Model { DstPort 1 } Line { - ZOrder 35 + ZOrder 15 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 36 + ZOrder 16 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1794,10 +1794,10 @@ Model { Block { BlockType SubSystem Name "MATLAB Function5" - SID "48" + SID "63" Ports [1, 1] - Position [-905, 562, -835, 608] - ZOrder 550 + Position [-905, 692, -835, 738] + ZOrder 565 ErrorFcn "Stateflow.Translate.translate" PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on @@ -1819,11 +1819,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "45" + SIDHighWatermark "57" Block { BlockType Inport Name "u" - SID "48::1" + SID "63::1" Position [20, 101, 40, 119] ZOrder -1 IconDisplay "Port number" @@ -1831,20 +1831,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "48::44" + SID "63::56" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 35 + ZOrder 47 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "48::43" + SID "63::55" Tag "Stateflow S-Function debug_FTExternalForces 6" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 34 + ZOrder 46 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1858,20 +1858,20 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "48::45" + SID "63::57" Position [460, 241, 480, 259] - ZOrder 36 + ZOrder 48 } Block { BlockType Outport Name "y" - SID "48::5" + SID "63::5" Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" } Line { - ZOrder 33 + ZOrder 13 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1879,7 +1879,7 @@ Model { } Line { Name "y" - ZOrder 34 + ZOrder 14 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1887,14 +1887,14 @@ Model { DstPort 1 } Line { - ZOrder 35 + ZOrder 15 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 36 + ZOrder 16 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1918,34 +1918,32 @@ Model { "66666666667;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.0745098039215686" " 0.650980392156863],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines" - "',3,'LineNames',{{'force left arm:1','force left arm:2','force left arm:3'}},'ShowContent',true,'Placement',1),s" - "truct('MinYLimReal','-0.02821','MaxYLimReal','0.03263','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','1.41" - "356','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',{{}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'torque left" - " arm:1','torque left arm:2','torque left arm:3'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-1.249" - "39','MaxYLimReal','1.11267','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','1.41356','LegendVisibility','of" - "f','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627" - "4509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.41" - "1764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509" - "803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropert" - "iesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'force right arm:1','force right arm:2'" - ",'force right arm:3'}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-0.03433','MaxYLimReal','0.0343'," - "'YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','1.41356','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.160784313725" - "49;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352941176" - "47 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{}},'UserDefinedC" - "hannelNames',{{}},'NumLines',3,'LineNames',{{'torque right arm:1','torque right arm:2','torque right arm:3'}},'S" - "howContent',true,'Placement',4)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickC" - "olor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[0.0745098039215686 0.623529411764706" - " 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" - "n',[409 227 1301 663])" + "',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.02821','MaxYLimReal','0.03263'" + ",'YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','1.41356','LegendVisibility','off','XGrid',true,'YGrid',tru" + "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980392" + "2],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" + "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117" + "647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{}},'UserDefined" + "ChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-1.24" + "939','MaxYLimReal','1.11267','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','1.41356','LegendVisibility','o" + "ff','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862" + "74509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4" + "11764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450" + "9803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProper" + "tiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3)" + ",struct('MinYLimReal','-0.03433','MaxYLimReal','0.0343','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','1.4" + "1356','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.074509803921568" + "6 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',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowC" + "ontent',true,'Placement',4)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor" + "',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[0.0745098039215686 0.623529411764706 1;1" + " 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',[" + "409 227 1301 663])" NumInputPorts "4" Floating off } @@ -1965,34 +1963,32 @@ Model { "66666666667;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.0745098039215686" " 0.650980392156863],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines" - "',3,'LineNames',{{'force left arm:1','force left arm:2','force left arm:3'}},'ShowContent',true,'Placement',1),s" - "truct('MinYLimReal','-0.02821','MaxYLimReal','0.03263','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','1.41" - "356','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',{{}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'torque left" - " arm:1','torque left arm:2','torque left arm:3'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-1.249" - "39','MaxYLimReal','1.11267','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','1.41356','LegendVisibility','of" - "f','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.68627" - "4509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.41" - "1764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509" - "803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropert" - "iesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'force right arm:1','force right arm:2'" - ",'force right arm:3'}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-0.03433','MaxYLimReal','0.0343'," - "'YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','1.41356','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.160784313725" - "49;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.05882352941176" - "47 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{}},'UserDefinedC" - "hannelNames',{{}},'NumLines',3,'LineNames',{{'torque right arm:1','torque right arm:2','torque right arm:3'}},'S" - "howContent',true,'Placement',4)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickC" - "olor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[0.0745098039215686 0.623529411764706" - " 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" - "n',[409 227 1301 663])" + "',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.02821','MaxYLimReal','0.03263'" + ",'YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','1.41356','LegendVisibility','off','XGrid',true,'YGrid',tru" + "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980392" + "2],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" + "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117" + "647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{}},'UserDefined" + "ChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-1.24" + "939','MaxYLimReal','1.11267','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','1.41356','LegendVisibility','o" + "ff','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.6862" + "74509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.4" + "11764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.27450" + "9803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProper" + "tiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3)" + ",struct('MinYLimReal','-0.03433','MaxYLimReal','0.0343','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','1.4" + "1356','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.074509803921568" + "6 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',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowC" + "ontent',true,'Placement',4)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor" + "',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[0.0745098039215686 0.623529411764706 1;1" + " 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',[" + "404 208 1301 663])" NumInputPorts "6" Floating off } @@ -2012,34 +2008,32 @@ Model { "666666666667;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.074509803921568" "6 0.650980392156863],'Title','%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLine" - "s',3,'LineNames',{{'force left foot:1','force left foot:2','force left foot:3'}},'ShowContent',true,'Placement'," - "1),struct('MinYLimReal','-0.48787','MaxYLimReal','0.44951','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','" + "s',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-0.48787','MaxYLimReal','0.44951" + "','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','0.28409','LegendVisibility','On','XGrid',true,'YGrid',tru" + "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980392" + "2],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" + "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117" + "647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{}},'UserDefined" + "ChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-57.0" + "9767','MaxYLimReal','-22.56265','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','0.28409','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.274" + "509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LineProp" + "ertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement'," + "3),struct('MinYLimReal','-2.05544','MaxYLimReal','3.67473','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','" "0.28409','LegendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickCol" "or',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215" "686 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',{{}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'torque l" - "eft foot:1','torque left foot:2','torque left foot:3'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','" - "-57.09767','MaxYLimReal','-22.56265','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','0.28409','LegendVisibi" - "lity','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',{{}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'force right foot:1','force rig" - "ht foot:2','force right foot:3'}},'ShowContent',true,'Placement',3),struct('MinYLimReal','-2.05544','MaxYLimReal" - "','3.67473','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','0.28409','LegendVisibility','On','XGrid',true,'" - "YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.6862" - "74509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1" - "6078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058" - "8235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{}},'U" - "serDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'torque right foot:1','torque right foot:2','torque righ" - "t foot:3'}},'ShowContent',true,'Placement',4)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0" - " 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[0.0745098039215686 0.6" - "23529411764706 1;1 0.411764705882353 0.16078431372549;1 1 0.0666666666666667;0.717647058823529 0.274509803921569" - " 1;0.392156862745098 0.831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039" - "2156863]),'DisplayLayoutDimensions',[2 2],'DisplayContentCache',[],'TimeRangeSamples','60','TimeRangeFrames','60" - "'),extmgr.Configuration('Tools','Plot Navigation',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr.Confi" - "guration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Position',[66 1 1855 1001])" + ",'%','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'Sho" + "wContent',true,'Placement',4)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickCol" + "or',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[0.0745098039215686 0.623529411764706 1" + ";1 0.411764705882353 0.16078431372549;1 1 0.0666666666666667;0.717647058823529 0.274509803921569 1;0.39215686274" + "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])" NumInputPorts "4" Floating off } @@ -2084,7 +2078,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','2018a')),'Version','2018a','Position',[135 16 1865 922])" NumInputPorts "4" Floating off } @@ -2093,31 +2087,31 @@ Model { Name "Real Time Synchronizer" SID "16" Ports [] - Position [-535, 244, -410, 281] + Position [-535, 259, -410, 296] ZOrder 514 ForegroundColor "[0.916667, 0.916667, 0.916667]" BackgroundColor "gray" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" period "0.01" } Block { BlockType Reference - Name "Yarp Read" + Name "YarpRead" SID "17" Ports [0, 1] Position [-1290, -33, -1235, -7] ZOrder 519 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/right_foot/analog:o'" signalSize "6" @@ -2133,17 +2127,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read1" + Name "YarpRead1" SID "20" Ports [0, 1] Position [-1290, -63, -1235, -37] ZOrder 521 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/left_foot/analog:o'" signalSize "6" @@ -2159,17 +2153,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read2" + Name "YarpRead2" SID "30" Ports [0, 1] Position [-1285, 185, -1225, 215] ZOrder 528 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/right_leg/analog:o'" signalSize "6" @@ -2185,17 +2179,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read3" + Name "YarpRead3" SID "31" Ports [0, 1] Position [-1285, 145, -1225, 175] ZOrder 530 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/left_leg/analog:o'" signalSize "6" @@ -2211,17 +2205,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read4" + Name "YarpRead4" SID "39" Ports [0, 1] Position [-1285, 76, -1225, 104] ZOrder 537 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/right_arm/analog:o'" signalSize "6" @@ -2237,17 +2231,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read5" + Name "YarpRead5" SID "40" Ports [0, 1] Position [-1285, 37, -1225, 63] ZOrder 539 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/left_arm/analog:o'" signalSize "6" @@ -2318,19 +2312,19 @@ Model { Line { Name "analog right foot" ZOrder 47 - SrcBlock "Yarp Read" + SrcBlock "YarpRead" SrcPort 1 Points [139, 0] Branch { - ZOrder 310 - Labels [-1, 1] - DstBlock "Demux" + ZOrder 357 + Points [0, 475] + DstBlock "MATLAB Function1" DstPort 1 } Branch { - ZOrder 138 - Points [0, 475] - DstBlock "MATLAB Function1" + ZOrder 310 + Labels [-1, 1] + DstBlock "Demux" DstPort 1 } } @@ -2355,7 +2349,7 @@ Model { Line { Name "analog left foot" ZOrder 40 - SrcBlock "Yarp Read1" + SrcBlock "YarpRead1" SrcPort 1 Points [162, 0] Branch { @@ -2392,19 +2386,19 @@ Model { Line { Name "analog right leg" ZOrder 53 - SrcBlock "Yarp Read2" + SrcBlock "YarpRead2" SrcPort 1 Points [80, 0] Branch { - ZOrder 296 - Labels [-1, 0] - DstBlock "Demux2" + ZOrder 362 + Points [0, 385] + DstBlock "MATLAB Function3" DstPort 1 } Branch { - ZOrder 240 - Points [0, 385] - DstBlock "MATLAB Function5" + ZOrder 296 + Labels [-1, 0] + DstBlock "Demux2" DstPort 1 } } @@ -2429,19 +2423,19 @@ Model { Line { Name "analog left leg" ZOrder 58 - SrcBlock "Yarp Read3" + SrcBlock "YarpRead3" SrcPort 1 Points [104, 0] Branch { - ZOrder 297 - Labels [-1, 1] - DstBlock "Demux3" + ZOrder 359 + Points [0, 360] + DstBlock "MATLAB Function2" DstPort 1 } Branch { - ZOrder 236 - Points [0, 360] - DstBlock "MATLAB Function4" + ZOrder 297 + Labels [-1, 1] + DstBlock "Demux3" DstPort 1 } } @@ -2466,19 +2460,19 @@ Model { Line { Name "analog right arm" ZOrder 78 - SrcBlock "Yarp Read4" + SrcBlock "YarpRead4" SrcPort 1 Points [38, 0] Branch { - ZOrder 294 - Labels [-1, 1] - DstBlock "Demux4" + ZOrder 365 + Points [0, 625] + DstBlock "MATLAB Function5" DstPort 1 } Branch { - ZOrder 221 - Points [0, 625] - DstBlock "MATLAB Function3" + ZOrder 294 + Labels [-1, 1] + DstBlock "Demux4" DstPort 1 } } @@ -2503,19 +2497,19 @@ Model { Line { Name "analog left arm" ZOrder 83 - SrcBlock "Yarp Read5" + SrcBlock "YarpRead5" SrcPort 1 Points [59, 0] Branch { - ZOrder 295 - Labels [-1, 1] - DstBlock "Demux5" + ZOrder 364 + Points [0, 600] + DstBlock "MATLAB Function4" DstPort 1 } Branch { - ZOrder 217 - Points [0, 600] - DstBlock "MATLAB Function2" + ZOrder 295 + Labels [-1, 1] + DstBlock "Demux5" DstPort 1 } } @@ -2538,7 +2532,7 @@ Model { DstPort 2 } Line { - ZOrder 149 + ZOrder 358 SrcBlock "MATLAB Function1" SrcPort 1 Points [96, 0] @@ -2555,8 +2549,8 @@ Model { } } Line { - ZOrder 150 - SrcBlock "MATLAB Function4" + ZOrder 360 + SrcBlock "MATLAB Function2" SrcPort 1 Points [97, 0] Branch { @@ -2572,8 +2566,8 @@ Model { } } Line { - ZOrder 151 - SrcBlock "MATLAB Function5" + ZOrder 361 + SrcBlock "MATLAB Function3" SrcPort 1 Points [98, 0] Branch { @@ -2589,8 +2583,8 @@ Model { } } Line { - ZOrder 152 - SrcBlock "MATLAB Function2" + ZOrder 363 + SrcBlock "MATLAB Function4" SrcPort 1 Points [93, 0] Branch { @@ -2606,8 +2600,8 @@ Model { } } Line { - ZOrder 153 - SrcBlock "MATLAB Function3" + ZOrder 366 + SrcBlock "MATLAB Function5" SrcPort 1 Points [98, 0] Branch { @@ -2670,7 +2664,7 @@ Stateflow { chartFileNumber 1 disableImplicitCasting 1 eml { - name "fcn" + name "normFCN" } firstData 4 firstTransition 7 @@ -2690,7 +2684,7 @@ Stateflow { decomposition CLUSTER_STATE eml { isEML 1 - script "function y = fcn(u)\ny =norm(u);\n" + script "function y = normFCN(u)\n\n y = norm(u);\nend" editorLayout "100 M4x1[10 5 700 500]" } } @@ -2793,7 +2787,7 @@ Stateflow { chartFileNumber 2 disableImplicitCasting 1 eml { - name "fcn" + name "normFCN" } firstData 11 firstTransition 14 @@ -2813,7 +2807,7 @@ Stateflow { decomposition CLUSTER_STATE eml { isEML 1 - script "function y = fcn(u)\ny =norm(u);\n" + script "function y = normFCN(u)\n\n y = norm(u);\nend" editorLayout "100 M4x1[10 5 700 500]" } } @@ -2916,7 +2910,7 @@ Stateflow { chartFileNumber 3 disableImplicitCasting 1 eml { - name "fcn" + name "normFCN" } firstData 18 firstTransition 21 @@ -2936,7 +2930,7 @@ Stateflow { decomposition CLUSTER_STATE eml { isEML 1 - script "function y = fcn(u)\ny =norm(u);\n" + script "function y = normFCN(u)\n\n y = norm(u);\nend" editorLayout "100 M4x1[10 5 700 500]" } } @@ -3039,7 +3033,7 @@ Stateflow { chartFileNumber 4 disableImplicitCasting 1 eml { - name "fcn" + name "normFCN" } firstData 25 firstTransition 28 @@ -3059,7 +3053,7 @@ Stateflow { decomposition CLUSTER_STATE eml { isEML 1 - script "function y = fcn(u)\ny =norm(u);\n" + script "function y = normFCN(u)\n\n y = norm(u);\nend" editorLayout "100 M4x1[10 5 700 500]" } } @@ -3162,7 +3156,7 @@ Stateflow { chartFileNumber 5 disableImplicitCasting 1 eml { - name "fcn" + name "normFCN" } firstData 32 firstTransition 35 @@ -3182,7 +3176,7 @@ Stateflow { decomposition CLUSTER_STATE eml { isEML 1 - script "function y = fcn(u)\ny =norm(u);\n" + script "function y = normFCN(u)\n\n y = norm(u);\nend" editorLayout "100 M4x1[10 5 700 500]" } } @@ -3285,7 +3279,7 @@ Stateflow { chartFileNumber 6 disableImplicitCasting 1 eml { - name "fcn" + name "normFCN" } firstData 39 firstTransition 42 @@ -3305,7 +3299,7 @@ Stateflow { decomposition CLUSTER_STATE eml { isEML 1 - script "function y = fcn(u)\ny =norm(u);\n" + script "function y = normFCN(u)\n\n y = norm(u);\nend" editorLayout "100 M4x1[10 5 700 500]" } } diff --git a/utilities/debug_FTMeas.mdl b/utilities/debug_FTMeas.mdl index e8316a0..42b10fd 100644 --- a/utilities/debug_FTMeas.mdl +++ b/utilities/debug_FTMeas.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.38" + ComputedModelVersion "1.43" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -24,38 +24,38 @@ Model { Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTMeas/Yarp Read" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTMeas/YarpRead" SID "17" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTMeas/Yarp Read1" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTMeas/YarpRead1" SID "20" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTMeas/Yarp Read2" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTMeas/YarpRead2" SID "30" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTMeas/Yarp Read3" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTMeas/YarpRead3" SID "31" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTMeas/Yarp Read4" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTMeas/YarpRead4" SID "39" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTMeas/Yarp Read5" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTMeas/YarpRead5" SID "40" Type "LIBRARY_BLOCK" } @@ -124,7 +124,7 @@ Model { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [106.0, 39.0, 3734.0, 2121.0] + Location [134.0, 55.0, 3706.0, 2105.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -148,9 +148,9 @@ Model { IsActive [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [3696.0, 1916.0] - ZoomFactor [3.1308824412005944] - Offset [-1094.4715827767177, -22.484021435387149] + Extents [3668.0, 1809.0] + ZoomFactor [3.1071690563521175] + Offset [-1094.5057091346152, -7.600993732829636] } Object { $PropName "DockComponentsInfo" @@ -168,12 +168,12 @@ Model { } WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAACVAP///wAAAAEAAAAAAAAAAPwCA" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAC6AP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAANQD///8AAA6WAAAHywAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "QBjAHQAbwByAAAAAAD/////AAAAOgD///8AAA56AAAHbQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACQ/////wAAAAAAAAAA/////wEAAAD6/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGK/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAQg/////wAAAAAAAAAA/" + "////wEAAACe/////wAAAAAAAAAA/////wEAAAEM/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGq/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAASj/////wAAAAAAAAAA/" "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" } } @@ -184,9 +184,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Dec 14 14:42:29 2018" - RTWModifiedTimeStamp 466699349 - ModelVersionFormat "1.%" + LastModifiedDate "Fri Mar 08 16:25:44 2019" + RTWModifiedTimeStamp 473963144 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -1064,7 +1064,7 @@ Model { } System { Name "debug_FTMeas" - Location [106, 39, 3840, 2160] + Location [134, 55, 3840, 2160] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -1077,7 +1077,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "313" + ZoomFactor "311" ReportName "simulink-default.rpt" SIDHighWatermark "42" Block { @@ -1338,15 +1338,15 @@ Model { Name "Real Time Synchronizer" SID "16" Ports [] - Position [-375, 349, -250, 386] + Position [-375, 334, -250, 371] ZOrder 514 ForegroundColor "[0.916667, 0.916667, 0.916667]" BackgroundColor "gray" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" period "0.01" } @@ -1436,17 +1436,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read" + Name "YarpRead" SID "17" Ports [0, 1] Position [-1090, 196, -1030, 244] ZOrder 519 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/right_foot/analog:o'" signalSize "6" @@ -1462,17 +1462,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read1" + Name "YarpRead1" SID "20" Ports [0, 1] Position [-1090, 131, -1030, 179] ZOrder 521 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/left_foot/analog:o'" signalSize "6" @@ -1488,17 +1488,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read2" + Name "YarpRead2" SID "30" Ports [0, 1] Position [-455, 206, -395, 254] ZOrder 528 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/right_leg/analog:o'" signalSize "6" @@ -1514,17 +1514,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read3" + Name "YarpRead3" SID "31" Ports [0, 1] Position [-455, 131, -395, 179] ZOrder 530 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/left_leg/analog:o'" signalSize "6" @@ -1540,17 +1540,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read4" + Name "YarpRead4" SID "39" Ports [0, 1] Position [-1090, 366, -1030, 414] ZOrder 537 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/right_arm/analog:o'" signalSize "6" @@ -1566,17 +1566,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read5" + Name "YarpRead5" SID "40" Ports [0, 1] Position [-1090, 296, -1030, 344] ZOrder 539 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/left_arm/analog:o'" signalSize "6" @@ -1599,10 +1599,10 @@ Model { ZOrder 524 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off } Block { @@ -1614,10 +1614,10 @@ Model { ZOrder 526 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off } Block { @@ -1629,10 +1629,10 @@ Model { ZOrder 533 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off } Block { @@ -1644,10 +1644,10 @@ Model { ZOrder 535 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off } Block { @@ -1659,10 +1659,10 @@ Model { ZOrder 542 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off } Block { @@ -1674,16 +1674,16 @@ Model { ZOrder 544 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off } Line { Name "analog right foot" ZOrder 47 - SrcBlock "Yarp Read" + SrcBlock "YarpRead" SrcPort 1 Points [34, 0; 0, 25] Branch { @@ -1720,7 +1720,7 @@ Model { Line { Name "analog left foot" ZOrder 40 - SrcBlock "Yarp Read1" + SrcBlock "YarpRead1" SrcPort 1 Points [45, 0] Branch { @@ -1788,7 +1788,7 @@ Model { Line { Name "analog right leg" ZOrder 53 - SrcBlock "Yarp Read2" + SrcBlock "YarpRead2" SrcPort 1 Points [29, 0] Branch { @@ -1825,7 +1825,7 @@ Model { Line { Name "analog left leg" ZOrder 58 - SrcBlock "Yarp Read3" + SrcBlock "YarpRead3" SrcPort 1 Points [25, 0] Branch { @@ -1893,7 +1893,7 @@ Model { Line { Name "analog right arm" ZOrder 78 - SrcBlock "Yarp Read4" + SrcBlock "YarpRead4" SrcPort 1 Points [34, 0] Branch { @@ -1930,7 +1930,7 @@ Model { Line { Name "analog left arm" ZOrder 83 - SrcBlock "Yarp Read5" + SrcBlock "YarpRead5" SrcPort 1 Points [30, 0] Branch { diff --git a/utilities/debug_FTMeas_shoes.mdl b/utilities/debug_FTMeas_shoes.mdl index 8eeb5fd..ab5c00d 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.40" + ComputedModelVersion "1.45" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -24,26 +24,26 @@ Model { Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTMeas_shoes/Yarp Read" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTMeas_shoes/YarpRead" SID "17" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTMeas_shoes/Yarp Read1" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTMeas_shoes/YarpRead1" SID "20" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTMeas_shoes/Yarp Read4" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTMeas_shoes/YarpRead4" SID "39" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_FTMeas_shoes/Yarp Read5" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_FTMeas_shoes/YarpRead5" SID "40" Type "LIBRARY_BLOCK" } @@ -100,7 +100,7 @@ Model { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [106.0, 39.0, 3734.0, 2121.0] + Location [134.0, 55.0, 3706.0, 2105.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -124,9 +124,9 @@ Model { IsActive [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [3696.0, 1916.0] - ZoomFactor [5.0079575596816976] - Offset [-1226.0439618644068, 71.204449152542367] + Extents [3668.0, 1809.0] + ZoomFactor [4.8528610354223432] + Offset [-1234.976079976137, 81.115103874227941] } Object { $PropName "DockComponentsInfo" @@ -144,12 +144,12 @@ Model { } WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAACVAP///wAAAAEAAAAAAAAAAPwCA" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAC6AP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAANQD///8AAA6WAAAHywAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "QBjAHQAbwByAAAAAAD/////AAAAOgD///8AAA56AAAHbQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACQ/////wAAAAAAAAAA/////wEAAAD6/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGK/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAQg/////wAAAAAAAAAA/" + "////wEAAACe/////wAAAAAAAAAA/////wEAAAEM/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGq/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAASj/////wAAAAAAAAAA/" "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" } } @@ -160,9 +160,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Dec 14 14:42:29 2018" - RTWModifiedTimeStamp 466699349 - ModelVersionFormat "1.%" + LastModifiedDate "Fri Mar 08 16:25:45 2019" + RTWModifiedTimeStamp 473963145 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -1040,7 +1040,7 @@ Model { } System { Name "debug_FTMeas_shoes" - Location [106, 39, 3840, 2160] + Location [134, 55, 3840, 2160] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -1053,7 +1053,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "501" + ZoomFactor "485" ReportName "simulink-default.rpt" SIDHighWatermark "51" Block { @@ -1276,15 +1276,15 @@ Model { Name "Real Time Synchronizer" SID "16" Ports [] - Position [-700, 79, -575, 116] + Position [-700, 84, -575, 121] ZOrder 514 ForegroundColor "[0.916667, 0.916667, 0.916667]" BackgroundColor "gray" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" period "0.01" } @@ -1346,17 +1346,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read" + Name "YarpRead" SID "17" Ports [0, 1] Position [-1145, 216, -1085, 264] ZOrder 519 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'ft/ftShoe_Right_Front/analog:o'" signalSize "6" @@ -1372,17 +1372,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read1" + Name "YarpRead1" SID "20" Ports [0, 1] Position [-1145, 141, -1085, 189] ZOrder 521 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'ft/ftShoe_Left_Front/analog:o'" signalSize "6" @@ -1398,17 +1398,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read4" + Name "YarpRead4" SID "39" Ports [0, 1] Position [-1145, 381, -1085, 429] ZOrder 537 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'ft/ftShoe_Right_Rear/analog:o'" signalSize "6" @@ -1424,17 +1424,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read5" + Name "YarpRead5" SID "40" Ports [0, 1] Position [-1145, 301, -1085, 349] ZOrder 539 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'ft/ftShoe_Left_Rear/analog:o'" signalSize "6" @@ -1457,10 +1457,10 @@ Model { ZOrder 524 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off } Block { @@ -1472,10 +1472,10 @@ Model { ZOrder 526 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off } Block { @@ -1487,10 +1487,10 @@ Model { ZOrder 542 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off } Block { @@ -1502,17 +1502,17 @@ Model { ZOrder 544 BackgroundColor "[0.537255, 0.721569, 1.000000]" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/holder\n" SourceType "Holder" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off } Line { Name "analog right front" ZOrder 47 Labels [1, 1] - SrcBlock "Yarp Read" + SrcBlock "YarpRead" SrcPort 1 Points [89, 0] Branch { @@ -1549,7 +1549,7 @@ Model { Name "analog left front" ZOrder 40 Labels [1, 1] - SrcBlock "Yarp Read1" + SrcBlock "YarpRead1" SrcPort 1 Points [91, 0] Branch { @@ -1615,7 +1615,7 @@ Model { Name "analog right rear" ZOrder 78 Labels [1, 0] - SrcBlock "Yarp Read4" + SrcBlock "YarpRead4" SrcPort 1 Points [89, 0] Branch { @@ -1652,7 +1652,7 @@ Model { Name "analog left rear" ZOrder 83 Labels [1, 1] - SrcBlock "Yarp Read5" + SrcBlock "YarpRead5" SrcPort 1 Points [85, 0] Branch { diff --git a/utilities/debug_seesawIMU.mdl b/utilities/debug_seesawIMU.mdl index af0c3b6..410d2ca 100644 --- a/utilities/debug_seesawIMU.mdl +++ b/utilities/debug_seesawIMU.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.42" + ComputedModelVersion "1.51" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -24,8 +24,8 @@ Model { Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_seesawIMU/Yarp Read" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_seesawIMU/YarpRead" SID "1" Type "LIBRARY_BLOCK" } @@ -57,7 +57,7 @@ Model { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [106.0, 39.0, 3734.0, 2121.0] + Location [134.0, 55.0, 3706.0, 2105.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -81,9 +81,9 @@ Model { IsActive [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [3696.0, 1916.0] - ZoomFactor [4.3795380769374281] - Offset [101.80331583969468, 160.75546960196294] + Extents [3668.0, 1809.0] + ZoomFactor [4.1612149532710276] + Offset [83.064116454941029, 160.63559797866364] } Object { $PropName "DockComponentsInfo" @@ -101,12 +101,12 @@ Model { } WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAACVAP///wAAAAEAAAAAAAAAAPwCA" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAC6AP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAANQD///8AAA6WAAAHywAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "QBjAHQAbwByAAAAAAD/////AAAAOgD///8AAA56AAAHbQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACQ/////wAAAAAAAAAA/////wEAAAD6/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGK/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAQg/////wAAAAAAAAAA/" + "////wEAAACe/////wAAAAAAAAAA/////wEAAAEM/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGq/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAASj/////wAAAAAAAAAA/" "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" } } @@ -117,9 +117,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Dec 14 14:42:30 2018" - RTWModifiedTimeStamp 466699350 - ModelVersionFormat "1.%" + LastModifiedDate "Fri Mar 08 16:25:45 2019" + RTWModifiedTimeStamp 473963145 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -1095,7 +1095,7 @@ Model { } System { Name "debug_seesawIMU" - Location [106, 39, 3840, 2160] + Location [134, 55, 3840, 2160] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -1108,9 +1108,9 @@ Model { TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "438" + ZoomFactor "416" ReportName "simulink-default.rpt" - SIDHighWatermark "25" + SIDHighWatermark "27" Block { BlockType Selector Name " " @@ -1125,18 +1125,18 @@ Model { } Block { BlockType SubSystem - Name "MATLAB Function" - SID "5" + Name "MATLAB Function1" + SID "26" Ports [1, 1] Position [430, 332, 500, 378] - ZOrder 516 + ZOrder 907 ErrorFcn "Stateflow.Translate.translate" PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" System { - Name "MATLAB Function" + Name "MATLAB Function1" Location [223, 338, 826, 833] Open off PortBlocksUseCompactNotation off @@ -1151,11 +1151,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "57" + SIDHighWatermark "69" Block { BlockType Inport Name "u" - SID "5::1" + SID "26::1" Position [20, 101, 40, 119] ZOrder -1 IconDisplay "Port number" @@ -1163,20 +1163,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "5::56" + SID "26::68" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 47 + ZOrder 59 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "5::55" - Tag "Stateflow S-Function debug_seesawIMU 2" + SID "26::67" + Tag "Stateflow S-Function debug_seesawIMU 1" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 46 + ZOrder 58 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1190,20 +1190,20 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "5::57" + SID "26::69" Position [460, 241, 480, 259] - ZOrder 48 + ZOrder 60 } Block { BlockType Outport Name "y" - SID "5::5" + SID "26::5" Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" } Line { - ZOrder 53 + ZOrder 13 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1211,7 +1211,7 @@ Model { } Line { Name "y" - ZOrder 54 + ZOrder 14 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1219,14 +1219,14 @@ Model { DstPort 1 } Line { - ZOrder 55 + ZOrder 15 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 56 + ZOrder 16 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1236,18 +1236,18 @@ Model { } Block { BlockType SubSystem - Name "MATLAB Function1" - SID "9" + Name "MATLAB Function2" + SID "10" Ports [1, 1] - Position [430, 412, 500, 458] - ZOrder 520 + Position [430, 207, 500, 253] + ZOrder 521 ErrorFcn "Stateflow.Translate.translate" PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" System { - Name "MATLAB Function1" + Name "MATLAB Function2" Location [223, 338, 826, 833] Open off PortBlocksUseCompactNotation off @@ -1262,11 +1262,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "57" + SIDHighWatermark "69" Block { BlockType Inport Name "u" - SID "9::1" + SID "10::1" Position [20, 101, 40, 119] ZOrder -1 IconDisplay "Port number" @@ -1274,20 +1274,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "9::56" + SID "10::68" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 47 + ZOrder 59 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "9::55" - Tag "Stateflow S-Function debug_seesawIMU 1" + SID "10::67" + Tag "Stateflow S-Function debug_seesawIMU 3" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 46 + ZOrder 58 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1301,20 +1301,20 @@ Model { Block { BlockType Terminator Name " Terminator " - SID "9::57" + SID "10::69" Position [460, 241, 480, 259] - ZOrder 48 + ZOrder 60 } Block { BlockType Outport Name "y" - SID "9::5" + SID "10::5" Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" } Line { - ZOrder 49 + ZOrder 65 SrcBlock "u" SrcPort 1 DstBlock " SFunction " @@ -1322,7 +1322,7 @@ Model { } Line { Name "y" - ZOrder 50 + ZOrder 66 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1330,14 +1330,14 @@ Model { DstPort 1 } Line { - ZOrder 51 + ZOrder 67 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 52 + ZOrder 68 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1347,18 +1347,18 @@ Model { } Block { BlockType SubSystem - Name "MATLAB Function2" - SID "10" + Name "MATLAB Function3" + SID "12" Ports [1, 1] - Position [430, 207, 500, 253] - ZOrder 521 + Position [620, 537, 690, 583] + ZOrder 523 ErrorFcn "Stateflow.Translate.translate" PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" System { - Name "MATLAB Function2" + Name "MATLAB Function3" Location [223, 338, 826, 833] Open off PortBlocksUseCompactNotation off @@ -1373,11 +1373,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "57" + SIDHighWatermark "69" Block { BlockType Inport - Name "u" - SID "10::1" + Name "acc" + SID "12::1" Position [20, 101, 40, 119] ZOrder -1 IconDisplay "Port number" @@ -1385,20 +1385,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "10::56" + SID "12::68" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 47 + ZOrder 59 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "10::55" - Tag "Stateflow S-Function debug_seesawIMU 3" + SID "12::67" + Tag "Stateflow S-Function debug_seesawIMU 4" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 46 + ZOrder 58 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1406,49 +1406,49 @@ Model { SFcnIsStateOwnerBlock off Port { PortNumber 2 - Name "y" + Name "theta" } } Block { BlockType Terminator Name " Terminator " - SID "10::57" + SID "12::69" Position [460, 241, 480, 259] - ZOrder 48 + ZOrder 60 } Block { BlockType Outport - Name "y" - SID "10::5" + Name "theta" + SID "12::5" Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" } Line { - ZOrder 49 - SrcBlock "u" + ZOrder 65 + SrcBlock "acc" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - Name "y" - ZOrder 50 + Name "theta" + ZOrder 66 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 - DstBlock "y" + DstBlock "theta" DstPort 1 } Line { - ZOrder 51 + ZOrder 67 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 52 + ZOrder 68 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1458,18 +1458,18 @@ Model { } Block { BlockType SubSystem - Name "MATLAB Function3" - SID "12" + Name "MATLAB Function4" + SID "27" Ports [1, 1] - Position [620, 537, 690, 583] - ZOrder 523 + Position [430, 412, 500, 458] + ZOrder 908 ErrorFcn "Stateflow.Translate.translate" PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" System { - Name "MATLAB Function3" + Name "MATLAB Function4" Location [223, 338, 826, 833] Open off PortBlocksUseCompactNotation off @@ -1484,11 +1484,11 @@ Model { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "57" + SIDHighWatermark "69" Block { BlockType Inport - Name "acc" - SID "12::1" + Name "u" + SID "27::1" Position [20, 101, 40, 119] ZOrder -1 IconDisplay "Port number" @@ -1496,20 +1496,20 @@ Model { Block { BlockType Demux Name " Demux " - SID "12::56" + SID "27::68" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 47 + ZOrder 59 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "12::55" - Tag "Stateflow S-Function debug_seesawIMU 4" + SID "27::67" + Tag "Stateflow S-Function debug_seesawIMU 2" Ports [1, 2] Position [180, 100, 230, 160] - ZOrder 46 + ZOrder 58 FunctionName "sf_sfun" PortCounts "[1 2]" SFunctionDeploymentMode off @@ -1517,49 +1517,49 @@ Model { SFcnIsStateOwnerBlock off Port { PortNumber 2 - Name "theta" + Name "y" } } Block { BlockType Terminator Name " Terminator " - SID "12::57" + SID "27::69" Position [460, 241, 480, 259] - ZOrder 48 + ZOrder 60 } Block { BlockType Outport - Name "theta" - SID "12::5" + Name "y" + SID "27::5" Position [460, 101, 480, 119] ZOrder -5 IconDisplay "Port number" } Line { - ZOrder 49 - SrcBlock "acc" + ZOrder 13 + SrcBlock "u" SrcPort 1 DstBlock " SFunction " DstPort 1 } Line { - Name "theta" - ZOrder 50 + Name "y" + ZOrder 14 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 - DstBlock "theta" + DstBlock "y" DstPort 1 } Line { - ZOrder 51 + ZOrder 15 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 52 + ZOrder 16 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1583,15 +1583,15 @@ Model { Name "Real Time Synchronizer" SID "4" Ports [] - Position [590, 169, 715, 206] + Position [590, 164, 715, 201] ZOrder 515 ForegroundColor "[0.916667, 0.916667, 0.916667]" BackgroundColor "gray" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" period "0.01" } @@ -1633,16 +1633,16 @@ Model { } Block { BlockType Reference - Name "Yarp Read" + Name "YarpRead" SID "1" Ports [0, 1] Position [105, 341, 140, 369] ZOrder 23 ForegroundColor "[0.490196, 0.000000, 0.000000]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/seesaw'" signalSize "9" @@ -1670,7 +1670,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','2018a')),'Version','2018a','Location',[183 400 517 593])" NumInputPorts "1" Floating off } @@ -1736,7 +1736,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','2018a')),'Version','2018a','Location',[188 349 512 588])" NumInputPorts "1" Floating off } @@ -1749,22 +1749,28 @@ Model { ZOrder 904 ScopeSpecificationString "Simulink.scopes.TimeScopeBlockCfg('CurrentConfiguration', extmgr.ConfigurationSet(extm" "gr.Configuration('Core','General UI',true),extmgr.Configuration('Core','Source UI',true),extmgr.Configuration('S" - "ources','WiredSimulink',true,'DataLoggingLimitDataPoints',true,'DataLoggingSaveFormat','Array','DataLoggingDecim" - "ation','1','DataLoggingDecimateData',true),extmgr.Configuration('Visuals','Time Domain',true,'DisplayPropertyDef" - "aults',struct('MinYLimReal','-10','MaxYLimReal','10','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendV" - "isibility','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.6235294117" - "64706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.7176470588" - "23529 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%" - "','LinePropertiesCache',{{}},'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])" + "ources','WiredSimulink',true,'DataLoggingSaveFormat','Array','DataLoggingDecimation','1','DataLoggingDecimateDat" + "a',true),extmgr.Configuration('Visuals','Time Domain',true,'SerializedDisplays',{struct('MinYLimReal','-10','Max" + "YLimReal','10','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisibility','Off','XGrid',true,'YGrid'," + "true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980" + "3922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431" + "372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294" + "117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','LinePropertiesCache',{{}},'UserDefi" + "nedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',1)},'DisplayPropertyDefault" + "s',struct('MinYLimReal','-10','MaxYLimReal','10','YLabelReal','','MinYLimMag','0','MaxYLimMag','10','LegendVisib" + "ility','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.62352941176470" + "6 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.71764705882352" + "9 0.274509803921569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','%','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])" NumInputPorts "1" Floating off } Line { ZOrder 1 - SrcBlock "Yarp Read" + SrcBlock "YarpRead" SrcPort 1 Points [46, 0] Branch { @@ -1786,15 +1792,15 @@ Model { } } Line { - ZOrder 2 + ZOrder 136 SrcBlock "Selector" SrcPort 1 - DstBlock "MATLAB Function" + DstBlock "MATLAB Function1" DstPort 1 } Line { - ZOrder 3 - SrcBlock "MATLAB Function" + ZOrder 135 + SrcBlock "MATLAB Function1" SrcPort 1 Points [37, 0] Branch { @@ -1810,15 +1816,15 @@ Model { } } Line { - ZOrder 6 + ZOrder 134 SrcBlock "Selector1" SrcPort 1 - DstBlock "MATLAB Function1" + DstBlock "MATLAB Function4" DstPort 1 } Line { - ZOrder 9 - SrcBlock "MATLAB Function1" + ZOrder 133 + SrcBlock "MATLAB Function4" SrcPort 1 Points [35, 0] Branch { @@ -1930,7 +1936,7 @@ Stateflow { decomposition CLUSTER_STATE eml { isEML 1 - script "function y = rotate(u)\n\ny = rotx(pi)*rotz(-44/180*pi)*u;\n\nend\n" + script "function y = rotate(u)\n\n y = wbc.rotx(pi)*wbc.rotz(-44/180*pi)*u;\n\nend\n" editorLayout "100 M4x1[10 5 700 500]" } } @@ -2021,7 +2027,7 @@ Stateflow { chart { id 9 machine 1 - name "MATLAB Function" + name "MATLAB Function4" windowPosition [422 539.941 189 413] viewLimits [0 156.75 0 153.75] screen [1 1 3600 1200 1.180555555555556] @@ -2053,7 +2059,7 @@ Stateflow { decomposition CLUSTER_STATE eml { isEML 1 - script "function y = rotate(u)\n\ny = rotx(pi)*rotz(-44/180*pi)*u;\n\nend" + script "function y = rotate(u)\n\n y = wbc.rotx(pi)*wbc.rotz(-44/180*pi)*u;\n\nend\n" editorLayout "100 M4x1[10 5 700 500]" } } @@ -2138,7 +2144,7 @@ Stateflow { instance { id 15 machine 1 - name "MATLAB Function" + name "MATLAB Function4" chart 9 } chart { @@ -2176,7 +2182,7 @@ Stateflow { decomposition CLUSTER_STATE eml { isEML 1 - script "function y = rotate(u)\n\ny = rotx(pi)*rotz(-44/180*pi)*u;\n\nend\n" + script "function y = rotate(u)\n\n y = wbc.rotx(pi)*wbc.rotz(-44/180*pi)*u;\n\nend\n" editorLayout "100 M4x1[10 5 700 500]" } } @@ -2299,7 +2305,7 @@ Stateflow { decomposition CLUSTER_STATE eml { isEML 1 - script "function theta = arctangent(acc)\n\ntheta = atan2(acc(2),acc(3))*180/pi;\n\nend" + script "function theta = arctangent(acc)\n\n theta = atan2(acc(2),acc(3))*180/pi;\nend" editorLayout "100 M4x1[10 5 700 500]" } } diff --git a/utilities/debug_xSensIMU.mdl b/utilities/debug_xSensIMU.mdl index 43da12b..065a924 100644 --- a/utilities/debug_xSensIMU.mdl +++ b/utilities/debug_xSensIMU.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.43" + ComputedModelVersion "1.48" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -30,32 +30,32 @@ Model { Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyStates/Get Measurement" - Path "debug_xSensIMU/Transform inertials into world frame/Get Measurement" + Reference "WBToolboxLibrary/States/GetMeasurement" + Path "debug_xSensIMU/Transform inertials into world frame/GetMeasurement" SID "79" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" Path "debug_xSensIMU/Transform inertials into world frame/Head IMU" SID "28" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" + Reference "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" Path "debug_xSensIMU/Transform inertials into world frame/xsens IMU" SID "35" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_xSensIMU/Yarp Read" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_xSensIMU/YarpRead" SID "17" Type "LIBRARY_BLOCK" } ExternalFileReference { - Reference "WBToolboxLibrary/Utilities/Yarp Read" - Path "debug_xSensIMU/Yarp Read1" + Reference "WBToolboxLibrary/Utilities/YarpRead" + Path "debug_xSensIMU/YarpRead1" SID "27" Type "LIBRARY_BLOCK" } @@ -88,7 +88,7 @@ Model { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [106.0, 39.0, 3734.0, 2121.0] + Location [134.0, 55.0, 3706.0, 2105.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -112,9 +112,9 @@ Model { IsActive [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [3696.0, 1916.0] - ZoomFactor [3.0550161812297736] - Offset [-398.90287341101691, -86.582627118644041] + Extents [3668.0, 1809.0] + ZoomFactor [2.8959349593495936] + Offset [-427.28589100224576, -86.834362717574379] } Object { $PropName "DockComponentsInfo" @@ -132,12 +132,12 @@ Model { } WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAACVAP///wAAAAEAAAAAAAAAAPwCA" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAC6AP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAANQD///8AAA6WAAAHywAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "QBjAHQAbwByAAAAAAD/////AAAAOgD///8AAA56AAAHbQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACQ/////wAAAAAAAAAA/////wEAAAD6/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGK/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAQg/////wAAAAAAAAAA/" + "////wEAAACe/////wAAAAAAAAAA/////wEAAAEM/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGq/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAASj/////wAAAAAAAAAA/" "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" } } @@ -148,9 +148,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "gnava" ModifiedDateFormat "%" - LastModifiedDate "Fri Dec 14 14:42:31 2018" - RTWModifiedTimeStamp 466699350 - ModelVersionFormat "1.%" + LastModifiedDate "Fri Mar 08 16:25:46 2019" + RTWModifiedTimeStamp 473963146 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -1148,7 +1148,7 @@ Model { } System { Name "debug_xSensIMU" - Location [106, 39, 3840, 2160] + Location [134, 55, 3840, 2160] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -1161,7 +1161,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "306" + ZoomFactor "290" ReportName "simulink-default.rpt" SIDHighWatermark "80" Block { @@ -1169,12 +1169,12 @@ Model { Name "Configuration" SID "80" Ports [] - Position [-15, 140, 60, 170] + Position [-15, 155, 60, 185] ZOrder 615 - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/Configuration" SourceType "" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off ConfigSource "Workspace" ConfigObject "'WBTConfigRobot'" @@ -1218,21 +1218,20 @@ Model { "esColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.066" "6666666666667;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.07450980392156" - "86 0.650980392156863],'Title','IMU head','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',3," - "'LineNames',{{'Selector1:1','Selector1:2','Selector1:3'}},'ShowContent',true,'Placement',1),struct('MinYLimReal'" - ",'-33.78282','MaxYLimReal','30.66384','YLabelReal','','MinYLimMag',' 0.00000','MaxYLimMag','137.70681','LegendVi" - "sibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980" - "3922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764" - "706 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','IMU xsens','Line" - "PropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'Selector5:1','Selector5:2','Sel" - "ector5:3'}},'ShowContent',true,'Placement',2)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0" - " 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[0.0745098039215686 0.6" - "23529411764706 1;1 0.411764705882353 0.16078431372549;1 1 0.0666666666666667;0.717647058823529 0.274509803921569" - " 1;0.392156862745098 0.831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039" - "2156863]),'DisplayLayoutDimensions',[2 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigatio" - "n',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version'," - "'2017b')),'Version','2018a','Position',[1 1 1920 947])" + "86 0.650980392156863],'Title','IMU head','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0," + "'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-33.78282','MaxYLimReal','30.66384','" + "YLabelReal','','MinYLimMag',' 0.00000','MaxYLimMag','137.70681','LegendVisibility','On','XGrid',true,'YGrid',tru" + "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980392" + "2],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" + "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117" + "647 1 1;1 0.0745098039215686 0.650980392156863],'Title','IMU xsens','LinePropertiesCache',{{}},'UserDefinedChann" + "elNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2)},'DisplayPropertyDefaults',struc" + "t('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'C" + "olorOrder',[0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;1 1 0.0666666666666667;0" + ".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])" NumInputPorts "2" Floating off } @@ -1252,20 +1251,19 @@ Model { "0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;0.392156862745098 0" ".831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.07450980392" "15686 0.650980392156863],'Title','IMU head','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines'" - ",3,'LineNames',{{'Selector2:1','Selector2:2','Selector2:3'}},'ShowContent',true,'Placement',1),struct('MinYLimRe" - "al','-1.2142','MaxYLimReal','0.56889','YLabelReal','','MinYLimMag',' 0.00000','MaxYLimMag','137.70681','LegendVi" - "sibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980" - "3922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764" - "706 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','IMU xsens','Line" - "PropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'Selector7:1','Selector7:2','Sel" - "ector7:3'}},'ShowContent',true,'Placement',2)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0" - " 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[0.0745098039215686 0.6" - "23529411764706 1;1 0.411764705882353 0.16078431372549;1 1 0.0666666666666667;0.717647058823529 0.274509803921569" - " 1;0.392156862745098 0.831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039" - "2156863]),'DisplayLayoutDimensions',[2 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigatio" - "n',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version'," - "'2017b')),'Version','2018a','Position',[1 1 1920 947])" + ",0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-1.2142','MaxYLimReal','0.56889','" + "YLabelReal','','MinYLimMag',' 0.00000','MaxYLimMag','137.70681','LegendVisibility','On','XGrid',true,'YGrid',tru" + "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980392" + "2],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" + "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117" + "647 1 1;1 0.0745098039215686 0.650980392156863],'Title','IMU xsens','LinePropertiesCache',{{}},'UserDefinedChann" + "elNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2)},'DisplayPropertyDefaults',struc" + "t('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'C" + "olorOrder',[0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;1 1 0.0666666666666667;0" + ".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])" NumInputPorts "2" Floating off } @@ -1284,28 +1282,26 @@ Model { "sColor',[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.831" "372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117647 1 1;1 0.074509803921568" - "6 0.650980392156863],'Title','IMU head','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',4,'" - "LineNames',{{'Selector:1','Selector:2','Selector:3','Constant'}},'ShowContent',true,'Placement',1),struct('MinYL" - "imReal','-4.60026','MaxYLimReal','13.38285','YLabelReal','','MinYLimMag',' 0.00000','MaxYLimMag','137.70681','Le" - "gendVisibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627" - "4509803922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529" - "411764706 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','IMU xsens'" - ",'LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',4,'LineNames',{{'Selector6:1','Selector6:2" - "','Selector6:3','Constant1'}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-4.97519','MaxYLimReal','4" - ".95253','YLabelReal','','MinYLimMag',' 0.00000','MaxYLimMag','137.70681','LegendVisibility','On','XGrid',true,'Y" - "Grid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627" - "4509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16" - "078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588" - "235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','error','LinePropertiesCache',{{}},'UserDefined" - "ChannelNames',{{}},'NumLines',4,'LineNames',{{'Sum1:1','Sum1:2','Sum1:3','Sum1:4'}},'ShowContent',true,'Placemen" - "t',3)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0" - ".686274509803922 0.686274509803922],'ColorOrder',[0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.1" - "6078431372549;1 1 0.0666666666666667;0.717647058823529 0.274509803921569 1;0.392156862745098 0.831372549019608 0" - ".0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863]),'DisplayLayoutDimensions',[3 1" - "],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigation',true,'PreviousAutoscale','XY','OnceAt" - "Stop',false),extmgr.Configuration('Tools','Measurements',true,'Version','2017b')),'Version','2018a','Position',[" - "1 1 1920 947])" + "6 0.650980392156863],'Title','IMU head','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',0,'" + "LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-4.60026','MaxYLimReal','13.38285','YL" + "abelReal','','MinYLimMag',' 0.00000','MaxYLimMag','137.70681','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.1607843137254" + "9;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.058823529411764" + "7 1 1;1 0.0745098039215686 0.650980392156863],'Title','IMU xsens','LinePropertiesCache',{{}},'UserDefinedChannel" + "Names',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2),struct('MinYLimReal','-4.97519','M" + "axYLimReal','4.95253','YLabelReal','','MinYLimMag',' 0.00000','MaxYLimMag','137.70681','LegendVisibility','On','" + "XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509" + "803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764" + "705882353 0.16078431372549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.2745098039" + "21569 1;0.0588235294117647 1 1;1 0.0745098039215686 0.650980392156863],'Title','error','LinePropertiesCache',{{}" + "},'UserDefinedChannelNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',3)},'DisplayProp" + "ertyDefaults',struct('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0." + "686274509803922],'ColorOrder',[0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;1 1 0" + ".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])" NumInputPorts "3" Floating off } @@ -1325,20 +1321,19 @@ Model { "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.074509803921" "5686 0.650980392156863],'Title','IMU head','LinePropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines'," - "3,'LineNames',{{'Selector3:1','Selector3:2','Selector3:3'}},'ShowContent',true,'Placement',1),struct('MinYLimRea" - "l','-29.72159','MaxYLimReal','137.2842','YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','137.2842','LegendVi" - "sibility','On','XGrid',true,'YGrid',true,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.68627450980" - "3922 0.686274509803922 0.686274509803922],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764" - "706 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','IMU xsens','Line" - "PropertiesCache',{{}},'UserDefinedChannelNames',{{}},'NumLines',3,'LineNames',{{'Selector4:1','Selector4:2','Sel" - "ector4:3'}},'ShowContent',true,'Placement',2)},'DisplayPropertyDefaults',struct('YLabelReal','','AxesColor',[0 0" - " 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'ColorOrder',[0.0745098039215686 0.6" - "23529411764706 1;1 0.411764705882353 0.16078431372549;1 1 0.0666666666666667;0.717647058823529 0.274509803921569" - " 1;0.392156862745098 0.831372549019608 0.0745098039215686;0.0588235294117647 1 1;1 0.0745098039215686 0.65098039" - "2156863]),'DisplayLayoutDimensions',[2 1],'DisplayContentCache',[]),extmgr.Configuration('Tools','Plot Navigatio" - "n',true,'PreviousAutoscale','XY','OnceAtStop',false),extmgr.Configuration('Tools','Measurements',true,'Version'," - "'2017b')),'Version','2018a','Position',[1 1 1920 954])" + "0,'LineNames',{{[]}},'ShowContent',true,'Placement',1),struct('MinYLimReal','-29.72159','MaxYLimReal','137.2842'" + ",'YLabelReal','','MinYLimMag','0.00000','MaxYLimMag','137.2842','LegendVisibility','On','XGrid',true,'YGrid',tru" + "e,'PlotMagPhase',false,'AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.68627450980392" + "2],'ColorOrder',[1 1 0.0666666666666667;0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372" + "549;0.392156862745098 0.831372549019608 0.0745098039215686;0.717647058823529 0.274509803921569 1;0.0588235294117" + "647 1 1;1 0.0745098039215686 0.650980392156863],'Title','IMU xsens','LinePropertiesCache',{{}},'UserDefinedChann" + "elNames',{{}},'NumLines',0,'LineNames',{{[]}},'ShowContent',true,'Placement',2)},'DisplayPropertyDefaults',struc" + "t('YLabelReal','','AxesColor',[0 0 0],'AxesTickColor',[0.686274509803922 0.686274509803922 0.686274509803922],'C" + "olorOrder',[0.0745098039215686 0.623529411764706 1;1 0.411764705882353 0.16078431372549;1 1 0.0666666666666667;0" + ".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])" NumInputPorts "2" Floating off } @@ -1369,15 +1364,15 @@ Model { Name "Real Time Synchronizer" SID "16" Ports [] - Position [-40, 74, 85, 111] + Position [-40, 89, 85, 126] ZOrder 514 ForegroundColor "[0.916667, 0.916667, 0.916667]" BackgroundColor "gray" ShowName off - LibraryVersion "1.649" + LibraryVersion "1.675" SourceBlock "WBToolboxLibrary/Utilities/Real Time Synchronizer" SourceType "Real Time Synchronizer" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" period "0.01" } @@ -1509,7 +1504,7 @@ Model { RequestExecContextInheritance off System { Name "Transform inertials into world frame" - Location [65, 24, 1366, 768] + Location [134, 55, 3840, 2160] Open off PortBlocksUseCompactNotation off ModelBrowserVisibility off @@ -1522,7 +1517,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "114" + ZoomFactor "358" Block { BlockType Inport Name "inertial_head" @@ -1572,16 +1567,16 @@ Model { } Block { BlockType Reference - Name "Get Measurement" + Name "GetMeasurement" SID "79" Ports [0, 1] Position [210, -84, 285, -56] ZOrder 634 BackgroundColor "[0.513700, 0.851000, 0.670600]" - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyStates/Get Measurement" - SourceType "Get Measurement" - SourceProductName "Whole Body Toolbox" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/States/GetMeasurement" + SourceType "GetMeasurement" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off measuredType "Joints Position" } @@ -1592,10 +1587,10 @@ Model { Ports [2, 1] Position [380, 160, 530, 240] ZOrder 525 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off frameName "'imu_frame'" } @@ -1758,10 +1753,10 @@ Model { Ports [2, 1] Position [380, -130, 530, -50] ZOrder 531 - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/wholeBodyModel/Kinematics/Forward Kinematics" - SourceType "Forward Kinematics" - SourceProductName "Whole Body Toolbox" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Model/Kinematics/ForwardKinematics" + SourceType "ForwardKinematics" + SourceProductName "WholeBodyToolbox" ContentPreviewEnabled off frameName "'root_link_imu_frame'" } @@ -1784,7 +1779,7 @@ Model { } Line { ZOrder 158 - SrcBlock "Get Measurement" + SrcBlock "GetMeasurement" SrcPort 1 Points [19, 0] Branch { @@ -2044,17 +2039,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read" + Name "YarpRead" SID "17" Ports [0, 1] Position [-270, -12, -185, 2] ZOrder 515 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/xsens_inertial'" signalSize "12" @@ -2070,17 +2065,17 @@ Model { } Block { BlockType Reference - Name "Yarp Read1" + Name "YarpRead1" SID "27" Ports [0, 1] Position [-270, -62, -185, -48] ZOrder 524 ForegroundColor "[0.490196, 0.000000, 0.000000]" ShowName off - LibraryVersion "1.649" - SourceBlock "WBToolboxLibrary/Utilities/Yarp Read" + LibraryVersion "1.675" + SourceBlock "WBToolboxLibrary/Utilities/YarpRead" SourceType "YARP Read" - SourceProductName "Whole Body Toolbox" + SourceProductName "WholeBodyToolbox" MultiThreadCoSim "auto" portName "'/icub/inertial'" signalSize "12" @@ -2126,7 +2121,7 @@ Model { Name "inertial_imu_head" ZOrder 38 Labels [1, 1] - SrcBlock "Yarp Read1" + SrcBlock "YarpRead1" SrcPort 1 DstBlock "Transform inertials into world frame" DstPort 1 @@ -2135,7 +2130,7 @@ Model { Name "inertial_imu_xsens" ZOrder 87 Labels [0, 0] - SrcBlock "Yarp Read" + SrcBlock "YarpRead" SrcPort 1 DstBlock "Transform inertials into world frame" DstPort 2 diff --git a/utilities/releaseLegStressesWhileStanding.sh b/utilities/releaseLegStressesWhileStanding.sh index a6257df..8a86ba0 100644 --- a/utilities/releaseLegStressesWhileStanding.sh +++ b/utilities/releaseLegStressesWhileStanding.sh @@ -1,5 +1,8 @@ #!/bin/bash +# Release the internal stresses when the robot is balancing on two feet, +# due to the closed kinematic chain of the legs. The legs are put in +# `idle` mode for 0.25 seconds. list="4 5" for a in $list; do @@ -28,4 +31,4 @@ for a in $list; do echo "set icmd cmod $a pos" | yarp rpc /icub/left_leg/rpc:i done -yarp clean --timeout 0.2 +yarp clean --timeout 0.2 \ No newline at end of file