Skip to content
Snippets Groups Projects
Select Git revision
  • edc6ce532d47d2da2b3c235d1f0c1d791eb4b88c
  • master default protected
2 results

scaled_teleoperation.mdl

Blame
  • Martin Karlsson's avatar
    Martin Karlsson authored
    edc6ce53
    History
    scaled_teleoperation.mdl 615.21 KiB
    Model {
      Name			  "scaled_teleoperation"
      Version		  7.9
      MdlSubVersion		  0
      GraphicalInterface {
        NumRootInports	    57
        Inport {
          BusObject		      ""
          Name		      "irb2ext.obtaining"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.manualMode"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.controlActive"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].parKp"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].parKv"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].parKi"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].parTrqMin"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].parTrqMax"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].posRawFb"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].posFlt"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].velRaw"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].velFlt"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].velOut"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].trqRaw"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].trqRefFlt"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].posRef"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].velRef"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].trqFfw"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].trqFfwGrav"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].mocgendata.instruction"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].mocgendata.value1"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].mocgendata.value2"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].mocgendata.value3"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].mocgendata.value4"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].mocgendata.value5"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].mocgendata.value6"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].mocgendata.string1[i]"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].mocgendata.string2[i]"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].parKp"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].parKv"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].parKi"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].parTrqMin"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].parTrqMax"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].posRawFb"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].posFlt"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].velFlt"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].velOut"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].trqRaw"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].trqRefFlt"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].posRef"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].velRef"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].trqFfw"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].trqFfwGrav"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].mocgendata.instruction"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].mocgendata.value1"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].mocgendata.value2"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].mocgendata.value3"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].mocgendata.value4"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].mocgendata.value5"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].mocgendata.value6"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].mocgendata.string1[i]"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].mocgendata.string2[i]"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[1].joint[i].posRawAbs"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].posRawAbs"
        }
        Inport {
          BusObject		      ""
          Name		      "irb2ext.robot[0].joint[i].velRaw"
        }
        Inport {
          BusObject		      ""
          Name		      "ati2rob.forcesTorques[i]"
        }
        Inport {
          BusObject		      ""
          Name		      "ati2rob_2.forcesTorques[i]"
        }
        NumRootOutports	    30
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].joint[i].parKp"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].joint[i].parKv"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].joint[i].parKi"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].joint[i].posRef"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].joint[i].velRef"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].joint[i].trqFfw"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].mocgendata.instruction"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].mocgendata.value1"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].mocgendata.value2"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].mocgendata.value3"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].mocgendata.value4"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].mocgendata.value5"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].mocgendata.value6"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].mocgendata.string1[i]"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[1].mocgendata.string2[i]"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].joint[i].parKp"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].joint[i].parKv"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].joint[i].parKi"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].joint[i].posRef"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].joint[i].velRef"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].joint[i].trqFfw"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].mocgendata.instruction"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].mocgendata.value1"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].mocgendata.value2"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].mocgendata.value3"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].mocgendata.value4"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].mocgendata.value5"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].mocgendata.value6"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].mocgendata.string1[i]"
        }
        Outport {
          BusObject		      ""
          BusOutputAsStruct	      "off"
          Name		      "ext2irb.robot[0].mocgendata.string2[i]"
        }
        ParameterArgumentNames  ""
        ComputedModelVersion    "1.2866"
        NumModelReferences	    0
        NumTestPointedSignals   21
        TestPointedSignal {
          SignalName	      "torque1[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque"
        }
        TestPointedSignal {
          SignalName	      "torque2[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque"
          PortIndex		      2
        }
        TestPointedSignal {
          SignalName	      "offset[i]"
          FullBlockPath	      "scaled_teleoperation/Left/leadthroug control/MATLAB Function"
          PortIndex		      6
        }
        TestPointedSignal {
          SignalName	      "sat"
          FullBlockPath	      "scaled_teleoperation/Left/leadthroug control/MATLAB Function"
          PortIndex		      7
        }
        TestPointedSignal {
          SignalName	      "rstill[i]"
          FullBlockPath	      "scaled_teleoperation/Right/leadthroug control/MATLAB Function"
          PortIndex		      6
        }
        TestPointedSignal {
          SignalName	      "cright[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/Gain"
        }
        TestPointedSignal {
          SignalName	      "cleft[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/Gain1"
        }
        TestPointedSignal {
          SignalName	      "w1[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/Gain10"
        }
        TestPointedSignal {
          SignalName	      "w2[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/Gain11"
        }
        TestPointedSignal {
          SignalName	      "help_right[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/Gain2"
        }
        TestPointedSignal {
          SignalName	      "help_left[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/Gain3"
        }
        TestPointedSignal {
          SignalName	      "diagm[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/Gain4"
        }
        TestPointedSignal {
          SignalName	      "e[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/MATLAB Function1"
        }
        TestPointedSignal {
          SignalName	      "pose1[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/MATLAB Function3"
        }
        TestPointedSignal {
          SignalName	      "vel1[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/MATLAB Function4"
        }
        TestPointedSignal {
          SignalName	      "vel2[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/MATLAB Function4"
          PortIndex		      2
        }
        TestPointedSignal {
          SignalName	      "q1_arm[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/haptic_controller"
        }
        TestPointedSignal {
          SignalName	      "lambda[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/haptic_controller"
          PortIndex		      2
        }
        TestPointedSignal {
          SignalName	      "q2_arm[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/haptic_controller"
          PortIndex		      3
        }
        TestPointedSignal {
          SignalName	      "freqDith"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/dither/Constant3"
        }
        TestPointedSignal {
          SignalName	      "dithSignal[i]"
          FullBlockPath	      "scaled_teleoperation/botharms_get_torque/dither/DITHERING"
        }
      }
      SavedCharacterEncoding  "UTF-8"
      SaveDefaultBlockParams  on
      ScopeRefreshTime	  0.035000
      OverrideScopeRefreshTime on
      DisableAllScopes	  off
      DataTypeOverride	  "UseLocalSettings"
      DataTypeOverrideAppliesTo "AllNumericTypes"
      MinMaxOverflowLogging	  "UseLocalSettings"
      MinMaxOverflowArchiveMode "Overwrite"
      FPTRunName		  "Run 1"
      MaxMDLFileLineLength	  120
      Created		  "Tue Apr 02 13:20:51 2002"
      Creator		  "jjwang"
      UpdateHistory		  "UpdateHistoryNever"
      ModifiedByFormat	  "%<Auto>"
      LastModifiedBy	  "martinka"
      ModifiedDateFormat	  "%<Auto>"
      LastModifiedDate	  "Tue Sep 17 20:31:39 2019"
      RTWModifiedTimeStamp	  490651023
      ModelVersionFormat	  "1.%<AutoIncrement:2866>"
      ConfigurationManager	  "none"
      SampleTimeColors	  off
      SampleTimeAnnotations	  off
      LibraryLinkDisplay	  "none"
      WideLines		  off
      ShowLineDimensions	  on
      ShowPortDataTypes	  off
      ShowDesignRanges	  off
      ShowLoopsOnError	  on
      IgnoreBidirectionalLines off
      ShowStorageClass	  off
      ShowTestPointIcons	  on
      ShowSignalResolutionIcons on
      ShowViewerIcons	  on
      SortedOrder		  off
      ExecutionContextIcon	  off
      ShowLinearizationAnnotations on
      BlockNameDataTip	  off
      BlockParametersDataTip  off
      BlockDescriptionStringDataTip	off
      ToolBar		  on
      StatusBar		  on
      BrowserShowLibraryLinks off
      BrowserLookUnderMasks	  off
      SimulationMode	  "external"
      LinearizationMsg	  "none"
      Profile		  off
      ParamWorkspaceSource	  "MATLABWorkspace"
      AccelSystemTargetFile	  "accel.tlc"
      AccelTemplateMakefile	  "accel_default_tmf"
      AccelMakeCommand	  "make_rtw"
      TryForcingSFcnDF	  off
      RecordCoverage	  off
      CovPath		  "/"
      CovSaveName		  "covdata"
      CovMetricSettings	  "dw"
      CovNameIncrementing	  off
      CovHtmlReporting	  on
      CovForceBlockReductionOff on
      covSaveCumulativeToWorkspaceVar on
      CovSaveSingleToWorkspaceVar on
      CovCumulativeVarName	  "covCumulativeData"
      CovCumulativeReport	  off
      CovReportOnPause	  on
      CovModelRefEnable	  "Off"
      CovExternalEMLEnable	  off
      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 off
      TunableVars		  "a_switch,f_switch,highKiActive,keepStill,leftActive,rightActive"
      TunableVarsStorageClass "Auto,Auto,Auto,Auto,Auto,Auto"
      TunableVarsTypeQualifier ",,,,,"
      BufferReuse		  on
      ShowModelReferenceBlockVersion off
      ShowModelReferenceBlockIO off
      Array {
        Type		    "Handle"
        Dimension		    1
        Simulink.ConfigSet {
          $ObjectID		      1
          Version		      "1.12.0"
          Array {
    	Type			"Handle"
    	Dimension		8
    	Simulink.SolverCC {
    	  $ObjectID		  2
    	  Version		  "1.12.0"
    	  StartTime		  "0.0"
    	  StopTime		  "10.0"
    	  AbsTol		  "auto"
    	  FixedStep		  "auto"
    	  InitialStep		  "auto"
    	  MaxNumMinSteps	  "-1"
    	  MaxOrder		  5
    	  ZcThreshold		  "auto"
    	  ConsecutiveZCsStepRelTol "10*128*eps"
    	  MaxConsecutiveZCs	  "1000"
    	  ExtrapolationOrder	  4
    	  NumberNewtonIterations  1
    	  MaxStep		  "auto"
    	  MinStep		  "auto"
    	  MaxConsecutiveMinStep	  "1"
    	  RelTol		  "1e-3"
    	  SolverMode		  "Auto"
    	  EnableConcurrentExecution off
    	  ConcurrentTasks	  off
    	  Solver		  "FixedStepDiscrete"
    	  SolverName		  "FixedStepDiscrete"
    	  SolverJacobianMethodControl "auto"
    	  ShapePreserveControl	  "DisableAll"
    	  ZeroCrossControl	  "UseLocalSettings"
    	  ZeroCrossAlgorithm	  "Nonadaptive"
    	  AlgebraicLoopSolver	  "TrustRegion"
    	  SolverResetMethod	  "Fast"
    	  PositivePriorityOrder	  off
    	  AutoInsertRateTranBlk	  off
    	  SampleTimeConstraint	  "Unconstrained"
    	  InsertRTBMode		  "Whenever possible"
    	}
    	Simulink.DataIOCC {
    	  $ObjectID		  3
    	  Version		  "1.12.0"
    	  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		  off
    	  SaveState		  off
    	  SignalLogging		  on
    	  DSMLogging		  on
    	  InspectSignalLogs	  off
    	  SaveTime		  off
    	  ReturnWorkspaceOutputs  off
    	  StateSaveName		  "xout"
    	  TimeSaveName		  "tout"
    	  OutputSaveName	  "yout"
    	  SignalLoggingName	  "sigsOut"
    	  DSMLoggingName	  "dsmout"
    	  OutputOption		  "RefineOutputTimes"
    	  OutputTimes		  "[]"
    	  ReturnWorkspaceOutputsName "out"
    	  Refine		  "1"
    	}
    	Simulink.OptimizationCC {
    	  $ObjectID		  4
    	  Version		  "1.12.0"
    	  Array {
    	    Type		    "Cell"
    	    Dimension		    8
    	    Cell		    "BooleansAsBitfields"
    	    Cell		    "PassReuseOutputArgsAs"
    	    Cell		    "PassReuseOutputArgsThreshold"
    	    Cell		    "ZeroExternalMemoryAtStartup"
    	    Cell		    "ZeroInternalMemoryAtStartup"
    	    Cell		    "OptimizeModelRefInitCode"
    	    Cell		    "NoFixptDivByZeroProtection"
    	    Cell		    "UseSpecifiedMinMax"
    	    PropName		    "DisabledProps"
    	  }
    	  BlockReduction	  on
    	  BooleanDataType	  off
    	  ConditionallyExecuteInputs on
    	  InlineParams		  on
    	  UseIntDivNetSlope	  off
    	  UseFloatMulNetSlope	  off
    	  UseSpecifiedMinMax	  off
    	  InlineInvariantSignals  off
    	  OptimizeBlockIOStorage  on
    	  BufferReuse		  on
    	  EnhancedBackFolding	  off
    	  StrengthReduction	  off
    	  ExpressionFolding	  on
    	  BooleansAsBitfields	  off
    	  BitfieldContainerType	  "uint_T"
    	  EnableMemcpy		  on
    	  MemcpyThreshold	  64
    	  PassReuseOutputArgsAs	  "Structure reference"
    	  ExpressionDepthLimit	  2147483647
    	  FoldNonRolledExpr	  on
    	  LocalBlockOutputs	  on
    	  RollThreshold		  8
    	  SystemCodeInlineAuto	  off
    	  StateBitsets		  off
    	  DataBitsets		  off
    	  UseTempVars		  off
    	  ZeroExternalMemoryAtStartup on
    	  ZeroInternalMemoryAtStartup on
    	  InitFltsAndDblsToZero	  on
    	  NoFixptDivByZeroProtection off
    	  EfficientFloat2IntCast  off
    	  EfficientMapNaN2IntZero on
    	  OptimizeModelRefInitCode off
    	  LifeSpan		  "inf"
    	  MaxStackSize		  "Inherit from target"
    	  BufferReusableBoundary  on
    	  SimCompilerOptimization "Off"
    	  AccelVerboseBuild	  off
    	  ParallelExecutionInRapidAccelerator on
    	}
    	Simulink.DebuggingCC {
    	  $ObjectID		  5
    	  Version		  "1.12.0"
    	  RTPrefix		  "error"
    	  ConsistencyChecking	  "none"
    	  ArrayBoundsChecking	  "none"
    	  SignalInfNanChecking	  "none"
    	  SignalRangeChecking	  "none"
    	  ReadBeforeWriteMsg	  "UseLocalSettings"
    	  WriteAfterWriteMsg	  "UseLocalSettings"
    	  WriteAfterReadMsg	  "UseLocalSettings"
    	  AlgebraicLoopMsg	  "warning"
    	  ArtificialAlgebraicLoopMsg "warning"
    	  SaveWithDisabledLinksMsg "warning"
    	  SaveWithParameterizedLinksMsg	"none"
    	  CheckSSInitialOutputMsg on
    	  UnderspecifiedInitializationDetection	"Classic"
    	  MergeDetectMultiDrivingBlocksExec "none"
    	  CheckExecutionContextPreStartOutputMsg off
    	  CheckExecutionContextRuntimeOutputMsg	off
    	  SignalResolutionControl "TryResolveAllWithWarning"
    	  BlockPriorityViolationMsg "warning"
    	  MinStepSizeMsg	  "warning"
    	  TimeAdjustmentMsg	  "none"
    	  MaxConsecutiveZCsMsg	  "error"
    	  MaskedZcDiagnostic	  "warning"
    	  IgnoredZcDiagnostic	  "warning"
    	  SolverPrmCheckMsg	  "none"
    	  InheritedTsInSrcMsg	  "warning"
    	  DiscreteInheritContinuousMsg "warning"
    	  MultiTaskDSMMsg	  "warning"
    	  MultiTaskCondExecSysMsg "none"
    	  MultiTaskRateTransMsg	  "error"
    	  SingleTaskRateTransMsg  "none"
    	  TasksWithSamePriorityMsg "warning"
    	  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 "Use local settings"
    	  SignalLabelMismatchMsg  "none"
    	  UnconnectedInputMsg	  "warning"
    	  UnconnectedOutputMsg	  "warning"
    	  UnconnectedLineMsg	  "warning"
    	  SFcnCompatibilityMsg	  "none"
    	  FrameProcessingCompatibilityMsg "warning"
    	  UniqueDataStoreMsg	  "none"
    	  BusObjectLabelMismatch  "warning"
    	  RootOutportRequireBusObject "warning"
    	  AssertControl		  "UseLocalSettings"
    	  EnableOverflowDetection off
    	  ModelReferenceIOMsg	  "none"
    	  ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error"
    	  ModelReferenceVersionMismatchMessage "none"
    	  ModelReferenceIOMismatchMessage "none"
    	  ModelReferenceCSMismatchMessage "none"
    	  UnknownTsInhSupMsg	  "warning"
    	  ModelReferenceDataLoggingMessage "warning"
    	  ModelReferenceSymbolNameMessage "warning"
    	  ModelReferenceExtraNoncontSigs "error"
    	  StateNameClashWarn	  "warning"
    	  SimStateInterfaceChecksumMismatchMsg "warning"
    	  SimStateOlderReleaseMsg "error"
    	  InitInArrayFormatMsg	  "warning"
    	  StrictBusMsg		  "None"
    	  BusNameAdapt		  "WarnAndRepair"
    	  NonBusSignalsTreatedAsBus "none"
    	  LoggingUnavailableSignals "error"
    	  BlockIODiagnostic	  "none"
    	  SFUnusedDataAndEventsDiag "warning"
    	  SFUnexpectedBacktrackingDiag "warning"
    	  SFInvalidInputDataAccessInChartInitDiag "warning"
    	  SFNoUnconditionalDefaultTransitionDiag "warning"
    	  SFTransitionOutsideNaturalParentDiag "warning"
    	  SFUnconditionalTransitionShadowingDiag "warning"
    	}
    	Simulink.HardwareCC {
    	  $ObjectID		  6
    	  Version		  "1.12.0"
    	  ProdBitPerChar	  8
    	  ProdBitPerShort	  16
    	  ProdBitPerInt		  32
    	  ProdBitPerLong	  32
    	  ProdBitPerFloat	  32
    	  ProdBitPerDouble	  64
    	  ProdBitPerPointer	  32
    	  ProdLargestAtomicInteger "Char"
    	  ProdLargestAtomicFloat  "None"
    	  ProdIntDivRoundTo	  "Undefined"
    	  ProdEndianess		  "Unspecified"
    	  ProdWordSize		  32
    	  ProdShiftRightIntArith  on
    	  ProdHWDeviceType	  "Specified"
    	  TargetBitPerChar	  8
    	  TargetBitPerShort	  16
    	  TargetBitPerInt	  32
    	  TargetBitPerLong	  32
    	  TargetBitPerFloat	  32
    	  TargetBitPerDouble	  64
    	  TargetBitPerPointer	  32
    	  TargetLargestAtomicInteger "Char"
    	  TargetLargestAtomicFloat "None"
    	  TargetShiftRightIntArith on
    	  TargetIntDivRoundTo	  "Undefined"
    	  TargetEndianess	  "Unspecified"
    	  TargetWordSize	  32
    	  TargetTypeEmulationWarnSuppressLevel 0
    	  TargetPreprocMaxBitsSint 32
    	  TargetPreprocMaxBitsUint 32
    	  TargetHWDeviceType	  "32-bit Generic"
    	  TargetUnknown		  off
    	  ProdEqTarget		  off
    	}
    	Simulink.ModelReferenceCC {
    	  $ObjectID		  7
    	  Version		  "1.12.0"
    	  UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange"
    	  CheckModelReferenceTargetMessage "error"
    	  EnableParallelModelReferenceBuilds off
    	  ParallelModelReferenceErrorOnInvalidPool on
    	  ParallelModelReferenceMATLABWorkerInit "None"
    	  ModelReferenceNumInstancesAllowed "Multi"
    	  PropagateVarSize	  "Infer from blocks in model"
    	  ModelReferencePassRootInputsByReference on
    	  ModelReferenceMinAlgLoopOccurrences off
    	  PropagateSignalLabelsOutOfModel off
    	  SupportModelReferenceSimTargetCustomCode off
    	}
    	Simulink.SFSimCC {
    	  $ObjectID		  8
    	  Version		  "1.12.0"
    	  SFSimEnableDebug	  on
    	  SFSimOverflowDetection  on
    	  SFSimEcho		  on
    	  SimBlas		  on
    	  SimCtrlC		  on
    	  SimExtrinsic		  on
    	  SimIntegrity		  on
    	  SimUseLocalCustomCode	  off
    	  SimParseCustomCode	  off
    	  SimBuildMode		  "sf_incremental_build"
    	}
    	Simulink.RTWCC {
    	  $BackupClass		  "Simulink.RTWCC"
    	  $ObjectID		  9
    	  Version		  "1.12.0"
    	  Array {
    	    Type		    "Cell"
    	    Dimension		    9
    	    Cell		    "IncludeHyperlinkInReport"
    	    Cell		    "GenerateTraceInfo"
    	    Cell		    "GenerateTraceReport"
    	    Cell		    "GenerateTraceReportSl"
    	    Cell		    "GenerateTraceReportSf"
    	    Cell		    "GenerateTraceReportEml"
    	    Cell		    "GenerateWebview"
    	    Cell		    "GenerateCodeMetricsReport"
    	    Cell		    "GenerateCodeReplacementReport"
    	    PropName		    "DisabledProps"
    	  }
    	  SystemTargetFile	  "rtw2orca.tlc"
    	  GenCodeOnly		  off
    	  MakeCommand		  "make_rtw"
    	  GenerateMakefile	  on
    	  TemplateMakefile	  "rtw2orca.tmf"
    	  Description		  "RTW2ORCA Real-Time Target"
    	  GenerateReport	  off
    	  SaveLog		  off
    	  RTWVerbose		  on
    	  RetainRTWFile		  on
    	  ProfileTLC		  off
    	  TLCDebug		  off
    	  TLCCoverage		  off
    	  TLCAssert		  off
    	  ProcessScriptMode	  "Default"
    	  ConfigurationMode	  "Optimized"
    	  ProcessScript		  "rtw2orca_make_rtw_hook"
    	  ConfigAtBuild		  off
    	  RTWUseLocalCustomCode	  off
    	  RTWUseSimCustomCode	  off
    	  IncludeHyperlinkInReport off
    	  LaunchReport		  off
    	  TargetLang		  "C"
    	  IncludeBusHierarchyInRTWFileBlockHierarchyMap	off
    	  IncludeERTFirstTime	  off
    	  GenerateTraceInfo	  off
    	  GenerateTraceReport	  off
    	  GenerateTraceReportSl	  off
    	  GenerateTraceReportSf	  off
    	  GenerateTraceReportEml  off
    	  GenerateCodeInfo	  off
    	  GenerateWebview	  off
    	  GenerateCodeMetricsReport off
    	  GenerateCodeReplacementReport	off
    	  RTWCompilerOptimization "Off"
    	  CheckMdlBeforeBuild	  "Off"
    	  CustomRebuildMode	  "OnUpdate"
    	  Array {
    	    Type		    "Handle"
    	    Dimension		    2
    	    Simulink.CodeAppCC {
    	      $ObjectID		      10
    	      Version		      "1.12.0"
    	      Array {
    		Type			"Cell"
    		Dimension		21
    		Cell			"IgnoreCustomStorageClasses"
    		Cell			"IgnoreTestpoints"
    		Cell			"InsertBlockDesc"
    		Cell			"InsertPolySpaceComments"
    		Cell			"SFDataObjDesc"
    		Cell			"MATLABFcnDesc"
    		Cell			"SimulinkDataObjDesc"
    		Cell			"DefineNamingRule"
    		Cell			"SignalNamingRule"
    		Cell			"ParamNamingRule"
    		Cell			"InlinedPrmAccess"
    		Cell			"CustomSymbolStr"
    		Cell			"CustomSymbolStrGlobalVar"
    		Cell			"CustomSymbolStrType"
    		Cell			"CustomSymbolStrField"
    		Cell			"CustomSymbolStrFcn"
    		Cell			"CustomSymbolStrFcnArg"
    		Cell			"CustomSymbolStrBlkIO"
    		Cell			"CustomSymbolStrTmpVar"
    		Cell			"CustomSymbolStrMacro"
    		Cell			"ReqsInCode"
    		PropName		"DisabledProps"
    	      }
    	      ForceParamTrailComments on
    	      GenerateComments	      on
    	      IgnoreCustomStorageClasses on
    	      IgnoreTestpoints	      off
    	      IncHierarchyInIds	      off
    	      MaxIdLength	      128
    	      PreserveName	      off
    	      PreserveNameWithParent  off
    	      ShowEliminatedStatement on
    	      IncAutoGenComments      off
    	      SimulinkDataObjDesc     off
    	      SFDataObjDesc	      off
    	      MATLABFcnDesc	      off
    	      IncDataTypeInIds	      off
    	      MangleLength	      1
    	      CustomSymbolStrGlobalVar "$R$N$M"
    	      CustomSymbolStrType     "$N$R$M"
    	      CustomSymbolStrField    "$N$M"
    	      CustomSymbolStrFcn      "$R$N$M$F"
    	      CustomSymbolStrFcnArg   "rt$I$N$M"
    	      CustomSymbolStrBlkIO    "rtb_$N$M"
    	      CustomSymbolStrTmpVar   "$N$M"
    	      CustomSymbolStrMacro    "$R$N$M"
    	      DefineNamingRule	      "None"
    	      ParamNamingRule	      "None"
    	      SignalNamingRule	      "None"
    	      InsertBlockDesc	      off
    	      InsertPolySpaceComments off
    	      SimulinkBlockComments   on
    	      MATLABSourceComments    off
    	      EnableCustomComments    off
    	      InlinedPrmAccess	      "Literals"
    	      ReqsInCode	      off
    	      UseSimReservedNames     off
    	    }
    	    Simulink.STFCustomTargetCC {
    	      $BackupClass	      "Simulink.TargetCC"
    	      $ObjectID		      11
    	      Version		      "1.12.0"
    	      Array {
    		Type			"Cell"
    		Dimension		16
    		Cell			"GeneratePreprocessorConditionals"
    		Cell			"IncludeMdlTerminateFcn"
    		Cell			"CombineOutputUpdateFcns"
    		Cell			"SuppressErrorStatus"
    		Cell			"ERTCustomFileBanners"
    		Cell			"GenerateSampleERTMain"
    		Cell			"GenerateTestInterfaces"
    		Cell			"ModelStepFunctionPrototypeControlCompliant"
    		Cell			"CPPClassGenCompliant"
    		Cell			"MultiInstanceERTCode"
    		Cell			"PurelyIntegerCode"
    		Cell			"SupportComplex"
    		Cell			"SupportAbsoluteTime"
    		Cell			"SupportContinuousTime"
    		Cell			"SupportNonInlinedSFcns"
    		Cell			"PortableWordSizes"
    		PropName		"DisabledProps"
    	      }
    	      TargetFcnLib	      "ansi_tfl_tmw.mat"
    	      TargetLibSuffix	      ""
    	      TargetPreCompLibLocation ""
    	      CodeReplacementLibrary  "ANSI_C"
    	      UtilityFuncGeneration   "Auto"
    	      ERTMultiwordTypeDef     "System defined"
    	      CodeExecutionProfiling  off
    	      ERTMultiwordLength      256
    	      MultiwordLength	      2048
    	      GenerateFullHeader      on
    	      GenerateSampleERTMain   off
    	      GenerateTestInterfaces  off
    	      IsPILTarget	      off
    	      ModelReferenceCompliant off
    	      ParMdlRefBuildCompliant off
    	      CompOptLevelCompliant   off
    	      ConcurrentExecutionCompliant off
    	      IncludeMdlTerminateFcn  on
    	      GeneratePreprocessorConditionals "Disable all"
    	      CombineOutputUpdateFcns off
    	      CombineSignalStateStructs	off
    	      SuppressErrorStatus     off
    	      ERTFirstTimeCompliant   off
    	      IncludeFileDelimiter    "Auto"
    	      ERTCustomFileBanners    off
    	      SupportAbsoluteTime     on
    	      LogVarNameModifier      "rt_"
    	      MatFileLogging	      on
    	      MultiInstanceERTCode    off
    	      SupportNonFinite	      on
    	      SupportComplex	      on
    	      PurelyIntegerCode	      off
    	      SupportContinuousTime   on
    	      SupportNonInlinedSFcns  on
    	      SupportVariableSizeSignals off
    	      EnableShiftOperators    on
    	      ParenthesesLevel	      "Nominal"
    	      PortableWordSizes	      off
    	      ModelStepFunctionPrototypeControlCompliant off
    	      CPPClassGenCompliant    off
    	      AutosarCompliant	      off
    	      GRTInterface	      on
    	      SystemTargetFile	      "rtw2orca.tlc"
    	      DialogCategory	      0
    	      CustomProperty {
    		DataType		"string"
    		Name			"RTW2ORCA_LOCAL"
    		Value			"ext2irb irb2ext"
    	      }
    	      CustomProperty {
    		DataType		"string"
    		Name			"RTW2ORCA_LABCOMM"
    		Value			"extctrl_irc5_main_20110428 ati_netbox_lc ati_netbox2_lc"
    	      }
    	      CustomProperty {
    		DataType		"string"
    		Name			"RTW2ORCA_EXTERNAL_SRCS"
    		Value			"rtw2orca_irc5_main_20110428_xeno labcomm_lth_net_xeno"
    	      }
    	      CustomProperty {
    		DataType		"string"
    		Name			"RTW2ORCA_VPATH"
    		Value			".. /opt/robot/labcomm /opt/robot/orca /opt/robot/extctrl  /home/martinka/frida_dynamics"
    	      }
    	      CustomProperty {
    		DataType		"string"
    		Name			"RTW2ORCA_LIB"
    		Value			"-lrtdm -lnative -lxenomai -lrightfridadyn -lleftfridadyn"
    	      }
    	      CustomProperty {
    		DataType		"string"
    		Name			"RTW2ORCA_INCLUDE"
    		Value			"/opt/robot/include/lth/lth_net /opt/robot/extctrl /usr/xenomai/include /home/martinka/frida_dynamics /home/"
    		"martinka/frida_dynamics/cdyn_defs"
    	      }
    	      CustomProperty {
    		DataType		"string"
    		Name			"RTW2ORCA_LIBRARY"
    		Value			"/home/martinka/frida_dynamics"
    	      }
    	      CustomProperty {
    		DataType		"string"
    		Name			"RTW2ORCA_CFLAGS"
    		Value			" -DROBOT_COUNT=2 -DROBOT_0_JOINT_COUNT=7 -DROBOT_1_JOINT_COUNT=7 -Wno-error -DEXPOSE_DYN_EXTRA -fPIC -O4"
    	      }
    	      CustomProperty {
    		DataType		"string"
    		Name			"RTW2ORCA_POSTCOMMAND"
    		Value			"mkdir -p /tmp/\\$(USER)/rtw2orca ; cp \\$(MODEL) /tmp/\\$(USER)/rtw2orca"
    	      }
    	    }
    	    PropName		    "Components"
    	  }
    	}
    	PropName		"Components"
          }
          Name		      "Configuration"
          ExtraOptions	      "-aGenerateTraceInfo=0 -aIgnoreTestpoints=0 "
          CurrentDlgPage	      "Code Generation/RTW2ORCA"
          ConfigPrmDlgPosition     [ 427, 27, 1493, 1150 ] 
        }
        PropName		    "ConfigurationSets"
      }
      Simulink.ConfigSet {
        $PropName		    "ActiveConfigurationSet"
        $ObjectID		    1
      }
      ExplicitPartitioning	  off
      BlockDefaults {
        ForegroundColor	    "black"
        BackgroundColor	    "white"
        DropShadow		    off
        NamePlacement	    "normal"
        FontName		    "Helvetica"
        FontSize		    10
        FontWeight		    "normal"
        FontAngle		    "normal"
        ShowName		    on
        BlockRotation	    0
        BlockMirror		    off
      }
      AnnotationDefaults {
        HorizontalAlignment	    "center"
        VerticalAlignment	    "middle"
        ForegroundColor	    "black"
        BackgroundColor	    "white"
        DropShadow		    off
        FontName		    "Helvetica"
        FontSize		    10
        FontWeight		    "normal"
        FontAngle		    "normal"
        UseDisplayTextAsClickCallback off
      }
      LineDefaults {
        FontName		    "Helvetica"
        FontSize		    9
        FontWeight		    "normal"
        FontAngle		    "normal"
      }
      BlockParameterDefaults {
        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		      DiscreteFilter
          NumeratorSource	      "Dialog"
          Numerator		      "[1]"
          DenominatorSource	      "Dialog"
          Denominator	      "[1 0.5]"
          InitialStatesSource     "Dialog"
          InitialStates	      "0"
          InputProcessing	      "Elements as channels (sample based)"
          ExternalReset	      "None"
          SampleTime	      "-1"
          a0EqualsOne	      off
          NumCoefMin	      "[]"
          NumCoefMax	      "[]"
          DenCoefMin	      "[]"
          DenCoefMax	      "[]"
          OutMin		      "[]"
          OutMax		      "[]"
          StateDataTypeStr	      "Inherit: Same as input"
          NumCoefDataTypeStr      "Inherit: Inherit via internal rule"
          DenCoefDataTypeStr      "Inherit: Inherit via internal rule"
          NumProductDataTypeStr   "Inherit: Inherit via internal rule"
          DenProductDataTypeStr   "Inherit: Inherit via internal rule"
          NumAccumDataTypeStr     "Inherit: Inherit via internal rule"
          DenAccumDataTypeStr     "Inherit: Inherit via internal rule"
          OutDataTypeStr	      "Inherit: Inherit via internal rule"
          LockScale		      off
          RndMeth		      "Floor"
          SaturateOnIntegerOverflow	off
          StateMustResolveToSignalObject off
          RTWStateStorageClass    "Auto"
        }
        Block {
          BlockType		      From
          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
          IconDisplay	      "Tag"
          TagVisibility	      "local"
        }
        Block {
          BlockType		      Inport
          Port		      "1"
          OutputFunctionCall      off
          OutMin		      "[]"
          OutMax		      "[]"
          OutDataTypeStr	      "Inherit: auto"
          LockScale		      off
          BusOutputAsStruct	      off
          PortDimensions	      "-1"
          VarSizeSig	      "Inherit"
          SampleTime	      "-1"
          SignalType	      "auto"
          SamplingMode	      "auto"
          LatchByDelayingOutsideSignal off
          LatchInputForFeedbackSignals off
          Interpolate	      on
        }
        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
          PortDimensions	      "-1"
          VarSizeSig	      "Inherit"
          SampleTime	      "-1"
          SignalType	      "auto"
          SamplingMode	      "auto"
          SourceOfInitialOutputValue "Dialog"
          OutputWhenDisabled      "held"
          InitialOutput	      "[]"
        }
        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	      "[]"
          SFunctionDeploymentMode off
        }
        Block {
          BlockType		      SubSystem
          ShowPortLabels	      "FromPortIcon"
          Permissions	      "ReadWrite"
          PermitHierarchicalResolution "All"
          TreatAsAtomicUnit	      off
          CheckFcnCallInpInsideContextMsg off
          SystemSampleTime	      "-1"
          RTWFcnNameOpts	      "Auto"
          RTWFileNameOpts	      "Auto"
          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"
          SFBlockType	      "NONE"
          Variant		      off
          GeneratePreprocessorConditionals 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		    "scaled_teleoperation"
        Location		    [0, 92, 1920, 1200]
        Open		    on
        ModelBrowserVisibility  off
        ModelBrowserWidth	    200
        ScreenColor		    "white"
        PaperOrientation	    "landscape"
        PaperPositionMode	    "auto"
        PaperType		    "A4"
        PaperUnits		    "inches"
        TiledPaperMargins	    [0.196850, 0.196850, 0.196850, 0.196850]
        TiledPageScale	    1
        ShowPageBoundaries	    off
        ZoomFactor		    "100"
        ReportName		    "simulink-default.rpt"
        SIDHighWatermark	    "1421"
        Block {
          BlockType		      Inport
          Name		      "irb2ext.obtaining"
          SID		      "51"
          Position		      [95, 58, 125, 72]
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.manualMode"
          SID		      "111"
          Position		      [95, 93, 125, 107]
          Port		      "2"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.controlActive"
          SID		      "112"
          Position		      [95, 128, 125, 142]
          Port		      "3"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].parKp"
          SID		      "115"
          Position		      [95, 348, 125, 362]
          Port		      "4"
          IconDisplay	      "Port number"
          OutDataTypeStr	      "double"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].parKv"
          SID		      "116"
          Position		      [95, 383, 125, 397]
          Port		      "5"
          IconDisplay	      "Port number"
          OutDataTypeStr	      "double"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].parKi"
          SID		      "117"
          Position		      [95, 418, 125, 432]
          Port		      "6"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].parTrqMin"
          SID		      "118"
          Position		      [95, 453, 125, 467]
          Port		      "7"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].parTrqMax"
          SID		      "119"
          Position		      [95, 488, 125, 502]
          Port		      "8"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].posRawFb"
          SID		      "121"
          Position		      [95, 558, 125, 572]
          Port		      "9"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].posFlt"
          SID		      "122"
          Position		      [95, 593, 125, 607]
          Port		      "10"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].velRaw"
          SID		      "123"
          Position		      [635, 118, 665, 132]
          Port		      "11"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].velFlt"
          SID		      "124"
          Position		      [95, 633, 125, 647]
          Port		      "12"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].velOut"
          SID		      "125"
          Position		      [95, 668, 125, 682]
          Port		      "13"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].trqRaw"
          SID		      "126"
          Position		      [95, 703, 125, 717]
          Port		      "14"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].trqRefFlt"
          SID		      "127"
          Position		      [95, 738, 125, 752]
          Port		      "15"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].posRef"
          SID		      "128"
          Position		      [95, 773, 125, 787]
          Port		      "16"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].velRef"
          SID		      "129"
          Position		      [95, 808, 125, 822]
          Port		      "17"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].trqFfw"
          SID		      "130"
          Position		      [95, 843, 125, 857]
          Port		      "18"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].trqFfwGrav"
          SID		      "131"
          Position		      [95, 878, 125, 892]
          Port		      "19"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].mocgendata.instruction"
          SID		      "132"
          Position		      [95, 913, 125, 927]
          Port		      "20"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].mocgendata.value1"
          SID		      "133"
          Position		      [95, 948, 125, 962]
          Port		      "21"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].mocgendata.value2"
          SID		      "134"
          Position		      [95, 983, 125, 997]
          Port		      "22"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].mocgendata.value3"
          SID		      "135"
          Position		      [95, 1018, 125, 1032]
          Port		      "23"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].mocgendata.value4"
          SID		      "136"
          Position		      [95, 1053, 125, 1067]
          Port		      "24"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].mocgendata.value5"
          SID		      "137"
          Position		      [95, 1093, 125, 1107]
          Port		      "25"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].mocgendata.value6"
          SID		      "138"
          Position		      [95, 1128, 125, 1142]
          Port		      "26"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].mocgendata.string1[i]"
          SID		      "139"
          Position		      [85, 1163, 115, 1177]
          Port		      "27"
          IconDisplay	      "Port number"
          PortDimensions	      "20"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].mocgendata.string2[i]"
          SID		      "140"
          Position		      [80, 1203, 110, 1217]
          Port		      "28"
          IconDisplay	      "Port number"
          PortDimensions	      "20"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].parKp"
          SID		      "226"
          Position		      [870, 328, 900, 342]
          Port		      "29"
          IconDisplay	      "Port number"
          OutDataTypeStr	      "double"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].parKv"
          SID		      "227"
          Position		      [890, 383, 920, 397]
          Port		      "30"
          IconDisplay	      "Port number"
          OutDataTypeStr	      "double"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].parKi"
          SID		      "228"
          Position		      [890, 418, 920, 432]
          Port		      "31"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].parTrqMin"
          SID		      "229"
          Position		      [890, 453, 920, 467]
          Port		      "32"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].parTrqMax"
          SID		      "230"
          Position		      [890, 488, 920, 502]
          Port		      "33"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].posRawFb"
          SID		      "232"
          Position		      [910, 558, 940, 572]
          Port		      "34"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].posFlt"
          SID		      "233"
          Position		      [890, 593, 920, 607]
          Port		      "35"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].velFlt"
          SID		      "235"
          Position		      [890, 663, 920, 677]
          Port		      "36"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].velOut"
          SID		      "236"
          Position		      [890, 698, 920, 712]
          Port		      "37"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].trqRaw"
          SID		      "237"
          Position		      [890, 733, 920, 747]
          Port		      "38"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].trqRefFlt"
          SID		      "238"
          Position		      [890, 768, 920, 782]
          Port		      "39"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].posRef"
          SID		      "239"
          Position		      [945, 1328, 975, 1342]
          Port		      "40"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].velRef"
          SID		      "240"
          Position		      [960, 1293, 990, 1307]
          Port		      "41"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].trqFfw"
          SID		      "241"
          Position		      [920, 828, 950, 842]
          Port		      "42"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].trqFfwGrav"
          SID		      "242"
          Position		      [920, 878, 950, 892]
          Port		      "43"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].mocgendata.instruction"
          SID		      "243"
          Position		      [920, 908, 950, 922]
          Port		      "44"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].mocgendata.value1"
          SID		      "244"
          Position		      [920, 948, 950, 962]
          Port		      "45"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].mocgendata.value2"
          SID		      "245"
          Position		      [910, 988, 940, 1002]
          Port		      "46"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].mocgendata.value3"
          SID		      "246"
          Position		      [910, 1028, 940, 1042]
          Port		      "47"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].mocgendata.value4"
          SID		      "247"
          Position		      [910, 1068, 940, 1082]
          Port		      "48"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].mocgendata.value5"
          SID		      "248"
          Position		      [910, 1108, 940, 1122]
          Port		      "49"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].mocgendata.value6"
          SID		      "249"
          Position		      [900, 1148, 930, 1162]
          Port		      "50"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].mocgendata.string1[i]"
          SID		      "250"
          Position		      [915, 1188, 945, 1202]
          Port		      "51"
          IconDisplay	      "Port number"
          PortDimensions	      "20"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].mocgendata.string2[i]"
          SID		      "251"
          Position		      [895, 1228, 925, 1242]
          Port		      "52"
          IconDisplay	      "Port number"
          PortDimensions	      "20"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[1].joint[i].posRawAbs"
          SID		      "120"
          Position		      [635, 88, 665, 102]
          Port		      "53"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].posRawAbs"
          SID		      "231"
          Position		      [635, 28, 665, 42]
          Port		      "54"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "irb2ext.robot[0].joint[i].velRaw"
          SID		      "234"
          Position		      [635, 58, 665, 72]
          Port		      "55"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "ati2rob.forcesTorques[i]"
          SID		      "773"
          Position		      [645, 178, 675, 192]
          Port		      "56"
          IconDisplay	      "Port number"
          PortDimensions	      "6"
          SignalType	      "real"
        }
        Block {
          BlockType		      Inport
          Name		      "ati2rob_2.forcesTorques[i]"
          SID		      "1279"
          Position		      [570, 198, 600, 212]
          Port		      "57"
          IconDisplay	      "Port number"
          PortDimensions	      "6"
          SignalType	      "real"
        }
        Block {
          BlockType		      Constant
          Name		      "Constant1"
          SID		      "1254"
          Position		      [1000, 1265, 1080, 1285]
          ShowName		      off
          Value		      "f_switch"
          VectorParams1D	      off
        }
        Block {
          BlockType		      Constant
          Name		      "Constant9"
          SID		      "378"
          Position		      [45, 1340, 125, 1360]
          ShowName		      off
          Value		      "f_switch"
          VectorParams1D	      off
        }
        Block {
          BlockType		      SubSystem
          Name		      "Left"
          SID		      "557"
          Ports		      [29, 16]
          Position		      [335, 234, 655, 1296]
          LibraryVersion	      "1.762"
          MinAlgLoopOccurrences   off
          PropExecContextOutsideSubsystem off
          RTWSystemCode	      "Auto"
          FunctionWithSeparateData off
          Opaque		      off
          RequestExecContextInheritance off
          MaskHideContents	      off
          System {
    	Name			"Left"
    	Location		[131, -19, 1728, 1033]
    	Open			off
    	ModelBrowserVisibility	off
    	ModelBrowserWidth	200
    	ScreenColor		"white"
    	PaperOrientation	"landscape"
    	PaperPositionMode	"auto"
    	PaperType		"A4"
    	PaperUnits		"centimeters"
    	TiledPaperMargins	[1.270000, 1.270000, 1.270000, 1.270000]
    	TiledPageScale		1
    	ShowPageBoundaries	off
    	ZoomFactor		"100"
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.obtaining"
    	  SID			  "558"
    	  Position		  [825, 968, 855, 982]
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.manualMode"
    	  SID			  "559"
    	  Position		  [65, 73, 95, 87]
    	  Port			  "2"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.controlActive"
    	  SID			  "560"
    	  Position		  [835, 928, 865, 942]
    	  Port			  "3"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].parKp"
    	  SID			  "561"
    	  Position		  [1280, 233, 1310, 247]
    	  Port			  "4"
    	  IconDisplay		  "Port number"
    	  OutDataTypeStr	  "double"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].parKv"
    	  SID			  "562"
    	  Position		  [1280, 268, 1310, 282]
    	  Port			  "5"
    	  IconDisplay		  "Port number"
    	  OutDataTypeStr	  "double"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].parKi"
    	  SID			  "563"
    	  Position		  [1295, 303, 1325, 317]
    	  Port			  "6"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].parTrqMin"
    	  SID			  "564"
    	  Position		  [65, 233, 95, 247]
    	  Port			  "7"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].parTrqMax"
    	  SID			  "565"
    	  Position		  [65, 263, 95, 277]
    	  Port			  "8"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].posRawFb"
    	  SID			  "567"
    	  Position		  [475, 283, 505, 297]
    	  Port			  "9"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].posFlt"
    	  SID			  "568"
    	  Position		  [65, 363, 95, 377]
    	  Port			  "10"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].velFlt"
    	  SID			  "570"
    	  Position		  [1025, 513, 1055, 527]
    	  Port			  "11"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].velOut"
    	  SID			  "571"
    	  Position		  [380, 43, 410, 57]
    	  Port			  "12"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].trqRaw"
    	  SID			  "572"
    	  Position		  [1270, 363, 1300, 377]
    	  Port			  "13"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].trqRefFlt"
    	  SID			  "573"
    	  Position		  [365, 103, 395, 117]
    	  Port			  "14"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].posRef"
    	  SID			  "574"
    	  Position		  [1155, 388, 1185, 402]
    	  NamePlacement		  "alternate"
    	  Port			  "15"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].velRef"
    	  SID			  "575"
    	  Position		  [1630, 863, 1660, 877]
    	  Port			  "16"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].trqFfw"
    	  SID			  "576"
    	  Position		  [90, 148, 120, 162]
    	  Port			  "17"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].trqFfwGrav"
    	  SID			  "577"
    	  Position		  [90, 183, 120, 197]
    	  Port			  "18"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.instruction"
    	  SID			  "578"
    	  Position		  [760, 813, 790, 827]
    	  Port			  "19"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.value1"
    	  SID			  "579"
    	  Position		  [145, 458, 175, 472]
    	  Port			  "20"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.value2"
    	  SID			  "580"
    	  Position		  [145, 493, 175, 507]
    	  Port			  "21"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.value3"
    	  SID			  "581"
    	  Position		  [145, 528, 175, 542]
    	  Port			  "22"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.value4"
    	  SID			  "582"
    	  Position		  [145, 563, 175, 577]
    	  Port			  "23"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.value5"
    	  SID			  "583"
    	  Position		  [145, 598, 175, 612]
    	  Port			  "24"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.value6"
    	  SID			  "584"
    	  Position		  [145, 633, 175, 647]
    	  Port			  "25"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.string1[i]"
    	  SID			  "585"
    	  Position		  [145, 668, 175, 682]
    	  Port			  "26"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "20"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.string2[i]"
    	  SID			  "586"
    	  Position		  [145, 703, 175, 717]
    	  Port			  "27"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "20"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "f_switch"
    	  SID			  "587"
    	  Position		  [1405, 878, 1435, 892]
    	  Port			  "28"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "torque_motor_2"
    	  SID			  "1356"
    	  Position		  [1140, 813, 1170, 827]
    	  Port			  "29"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant"
    	  SID			  "1303"
    	  Position		  [1230, 820, 1260, 850]
    	  ZOrder		  -4
    	  Value			  "0"
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant1"
    	  SID			  "698"
    	  Position		  [1195, 754, 1310, 776]
    	  ShowName		  off
    	  Value			  "maxRateLeft"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant11"
    	  SID			  "736"
    	  Position		  [1120, 875, 1260, 895]
    	  ShowName		  off
    	  Value			  "jointLimitsStartMotorRad"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant13"
    	  SID			  "955"
    	  Position		  [825, 1005, 905, 1025]
    	  ShowName		  off
    	  Value			  "f_switch"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant2"
    	  SID			  "590"
    	  Position		  [1185, 789, 1300, 811]
    	  ShowName		  off
    	  Value			  "highKiActive"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant3"
    	  SID			  "695"
    	  Position		  [1225, 579, 1340, 601]
    	  ShowName		  off
    	  Value			  "keepStill"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant4"
    	  SID			  "699"
    	  Position		  [1200, 719, 1315, 741]
    	  ShowName		  off
    	  Value			  "slidingThresWidthLeft"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant5"
    	  SID			  "591"
    	  Position		  [570, 319, 685, 341]
    	  ShowName		  off
    	  Value			  "x_hat_rob2"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant6"
    	  SID			  "592"
    	  Position		  [570, 399, 685, 421]
    	  ShowName		  off
    	  Value			  "baseAnglesLeft"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant7"
    	  SID			  "700"
    	  Position		  [1195, 684, 1310, 706]
    	  ShowName		  off
    	  Value			  "KiHighThresLeft"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant8"
    	  SID			  "701"
    	  Position		  [1195, 649, 1310, 671]
    	  ShowName		  off
    	  Value			  "percentageTrqFfwLeft"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant9"
    	  SID			  "594"
    	  Position		  [570, 359, 685, 381]
    	  ShowName		  off
    	  Value			  "gearRatio"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  DiscreteFilter
    	  Name			  "Discrete Filter3"
    	  SID			  "595"
    	  Ports			  [1, 1]
    	  Position		  [1140, 502, 1200, 538]
    	  InputPortMap		  "u0"
    	  Numerator		  "Blpfilt"
    	  Denominator		  "Alpfilt"
    	  a0EqualsOne		  on
    	}
    	Block {
    	  BlockType		  From
    	  Name			  "From"
    	  SID			  "733"
    	  Position		  [1235, 431, 1330, 459]
    	  GotoTag		  "f_switch_mod"
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain2"
    	  SID			  "734"
    	  Position		  [1475, 1020, 1505, 1050]
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain3"
    	  SID			  "735"
    	  Position		  [1610, 1020, 1640, 1050]
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	}
    	Block {
    	  BlockType		  Goto
    	  Name			  "Goto"
    	  SID			  "732"
    	  Position		  [1560, 912, 1640, 938]
    	  GotoTag		  "f_switch_mod"
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "MATLAB Function3"
    	  SID			  "1234"
    	  Ports			  [3, 4]
    	  Position		  [1000, 913, 1255, 1037]
    	  ShowName		  off
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "MATLAB Function3"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "68"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "controlActive"
    	      SID		      "1234::66"
    	      Position		      [20, 101, 40, 119]
    	      ZOrder		      18
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "obtaining"
    	      SID		      "1234::67"
    	      Position		      [20, 136, 40, 154]
    	      ZOrder		      19
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "switch_on"
    	      SID		      "1234::68"
    	      Position		      [20, 171, 40, 189]
    	      ZOrder		      20
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1234::62"
    	      Ports		      [1, 1]
    	      Position		      [270, 250, 320, 290]
    	      ZOrder		      14
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1234::61"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 15"
    	      Ports		      [3, 5]
    	      Position		      [180, 108, 230, 252]
    	      ZOrder		      13
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[3 5]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"runExtctrl"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		3
    		Name			"lockOutput"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		4
    		Name			"resetProgram"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		5
    		Name			"state"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1234::64"
    	      Position		      [460, 261, 480, 279]
    	      ZOrder		      16
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "runExtctrl"
    	      SID		      "1234::56"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "lockOutput"
    	      SID		      "1234::5"
    	      Position		      [460, 136, 480, 154]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "resetProgram"
    	      SID		      "1234::45"
    	      Position		      [460, 171, 480, 189]
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "state"
    	      SID		      "1234::65"
    	      Position		      [460, 206, 480, 224]
    	      ZOrder		      17
    	      Port		      "4"
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "controlActive"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "obtaining"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      2
    	    }
    	    Line {
    	      SrcBlock		      "switch_on"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      3
    	    }
    	    Line {
    	      Name		      "runExtctrl"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "runExtctrl"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "lockOutput"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      3
    	      DstBlock		      "lockOutput"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "resetProgram"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      4
    	      DstBlock		      "resetProgram"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "state"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      5
    	      DstBlock		      "state"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  Product
    	  Name			  "Product"
    	  SID			  "731"
    	  Ports			  [2, 1]
    	  Position		  [1495, 907, 1525, 938]
    	  InputSameDT		  off
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator"
    	  SID			  "602"
    	  Position		  [180, 230, 200, 250]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator1"
    	  SID			  "603"
    	  Position		  [220, 260, 240, 280]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator10"
    	  SID			  "604"
    	  Position		  [520, 100, 540, 120]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator11"
    	  SID			  "605"
    	  Position		  [1695, 1025, 1715, 1045]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator12"
    	  SID			  "606"
    	  Position		  [175, 70, 195, 90]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator13"
    	  SID			  "607"
    	  Position		  [1540, 980, 1560, 1000]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator2"
    	  SID			  "610"
    	  Position		  [200, 180, 220, 200]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator4"
    	  SID			  "612"
    	  Position		  [190, 145, 210, 165]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator5"
    	  SID			  "613"
    	  Position		  [180, 360, 200, 380]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator8"
    	  SID			  "616"
    	  Position		  [535, 40, 555, 60]
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "calcGravityTorque"
    	  SID			  "696"
    	  Ports			  [4, 3]
    	  Position		  [730, 265, 910, 435]
    	  AncestorBlock		  "leadthroughLib/calcGravityTorque"
    	  LibraryVersion	  "*1.87"
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  System {
    	    Name		    "calcGravityTorque"
    	    Location		    [581, 363, 1572, 970]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "940"
    	    SIDPrevWatermark	    "939"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "q_motor"
    	      SID		      "696:3"
    	      Position		      [150, 78, 180, 92]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "x_hat"
    	      SID		      "696:8"
    	      Position		      [175, 188, 205, 202]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "gearRatio"
    	      SID		      "696:10"
    	      Position		      [200, 298, 230, 312]
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "baseAngles"
    	      SID		      "696:12"
    	      Position		      [200, 408, 230, 422]
    	      Port		      "4"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      "Terminator1"
    	      SID		      "696:16"
    	      Position		      [675, 75, 695, 95]
    	    }
    	    Block {
    	      BlockType		      SubSystem
    	      Name		      "calcGravTrq"
    	      SID		      "696:1"
    	      Ports		      [4, 4]
    	      Position		      [305, 34, 600, 466]
    	      LibraryVersion	      "1.32"
    	      ErrorFcn		      "Stateflow.Translate.translate"
    	      PermitHierarchicalResolution "ExplicitOnly"
    	      TreatAsAtomicUnit	      on
    	      MinAlgLoopOccurrences   off
    	      PropExecContextOutsideSubsystem off
    	      RTWSystemCode	      "Auto"
    	      FunctionWithSeparateData off
    	      Opaque		      off
    	      RequestExecContextInheritance off
    	      MaskHideContents	      off
    	      SFBlockType	      "MATLAB Function"
    	      MaskType		      "Stateflow"
    	      MaskDescription	      "Embedded MATLAB block"
    	      MaskSelfModifiable      on
    	      MaskDisplay	      "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
    	      "rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
    	      MaskIconFrame	      on
    	      MaskIconOpaque	      off
    	      MaskIconRotate	      "none"
    	      MaskPortRotate	      "default"
    	      MaskIconUnits	      "normalized"
    	      System {
    		Name			"calcGravTrq"
    		Location		[257, 457, 812, 717]
    		Open			off
    		ModelBrowserVisibility	off
    		ModelBrowserWidth	200
    		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	"46"
    		Block {
    		  BlockType		  Inport
    		  Name			  "q_motor"
    		  SID			  "696:1::20"
    		  Position		  [20, 101, 40, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "x_hat"
    		  SID			  "696:1::21"
    		  Position		  [20, 136, 40, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "gearRatio"
    		  SID			  "696:1::22"
    		  Position		  [20, 171, 40, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "baseAngles"
    		  SID			  "696:1::24"
    		  Position		  [20, 206, 40, 224]
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Demux
    		  Name			  " Demux "
    		  SID			  "696:1::43"
    		  Ports			  [1, 1]
    		  Position		  [270, 245, 320, 285]
    		  ZOrder		  6
    		  Outputs		  "1"
    		}
    		Block {
    		  BlockType		  S-Function
    		  Name			  " SFunction "
    		  SID			  "696:1::42"
    		  Tag			  "Stateflow S-Function scaled_teleoperation 2"
    		  Ports			  [4, 5]
    		  Position		  [180, 100, 230, 220]
    		  ZOrder		  5
    		  FunctionName		  "sf_sfun"
    		  PortCounts		  "[4 5]"
    		  EnableBusSupport	  on
    		  Port {
    		    PortNumber		    2
    		    Name		    "gravTrq"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    3
    		    Name		    "gravTrqMotor"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    4
    		    Name		    "coulombWidthMotor"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    5
    		    Name		    "coulombWidth"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		}
    		Block {
    		  BlockType		  Terminator
    		  Name			  " Terminator "
    		  SID			  "696:1::45"
    		  Position		  [460, 256, 480, 274]
    		  ZOrder		  8
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "gravTrq"
    		  SID			  "696:1::25"
    		  Position		  [460, 101, 480, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "gravTrqMotor"
    		  SID			  "696:1::26"
    		  Position		  [460, 136, 480, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "coulombWidthMotor"
    		  SID			  "696:1::27"
    		  Position		  [460, 171, 480, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "coulombWidth"
    		  SID			  "696:1::46"
    		  Position		  [460, 206, 480, 224]
    		  ZOrder		  9
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Line {
    		  SrcBlock		  "q_motor"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  "x_hat"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  2
    		}
    		Line {
    		  SrcBlock		  "gearRatio"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  3
    		}
    		Line {
    		  SrcBlock		  "baseAngles"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  4
    		}
    		Line {
    		  Name			  "gravTrq"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  2
    		  DstBlock		  "gravTrq"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "gravTrqMotor"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  3
    		  DstBlock		  "gravTrqMotor"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "coulombWidthMotor"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  4
    		  DstBlock		  "coulombWidthMotor"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "coulombWidth"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  5
    		  DstBlock		  "coulombWidth"
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " Demux "
    		  SrcPort		  1
    		  DstBlock		  " Terminator "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " SFunction "
    		  SrcPort		  1
    		  DstBlock		  " Demux "
    		  DstPort		  1
    		}
    	      }
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "gravTrqMotor"
    	      SID		      "696:13"
    	      Position		      [685, 188, 715, 202]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "coulombWidthMotor"
    	      SID		      "696:43"
    	      Position		      [695, 298, 725, 312]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "coulombWidthArm"
    	      SID		      "696:940"
    	      Position		      [680, 409, 715, 421]
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "calcGravTrq"
    	      SrcPort		      3
    	      DstBlock		      "coulombWidthMotor"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "calcGravTrq"
    	      SrcPort		      2
    	      DstBlock		      "gravTrqMotor"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "baseAngles"
    	      SrcPort		      1
    	      DstBlock		      "calcGravTrq"
    	      DstPort		      4
    	    }
    	    Line {
    	      SrcBlock		      "gearRatio"
    	      SrcPort		      1
    	      DstBlock		      "calcGravTrq"
    	      DstPort		      3
    	    }
    	    Line {
    	      SrcBlock		      "calcGravTrq"
    	      SrcPort		      1
    	      DstBlock		      "Terminator1"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "x_hat"
    	      SrcPort		      1
    	      DstBlock		      "calcGravTrq"
    	      DstPort		      2
    	    }
    	    Line {
    	      SrcBlock		      "q_motor"
    	      SrcPort		      1
    	      DstBlock		      "calcGravTrq"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "calcGravTrq"
    	      SrcPort		      4
    	      DstBlock		      "coulombWidthArm"
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "leadthroug control"
    	  SID			  "697"
    	  Ports			  [23, 5]
    	  Position		  [1415, 231, 1610, 849]
    	  AncestorBlock		  "leadthroughLib/leadthroug control"
    	  LibraryVersion	  "*1.87"
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  System {
    	    Name		    "leadthroug control"
    	    Location		    [240, 156, 1680, 970]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "942"
    	    SIDPrevWatermark	    "939"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "Kp_in"
    	      SID		      "697:19"
    	      Position		      [225, 58, 255, 72]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "Kv_in"
    	      SID		      "697:20"
    	      Position		      [335, 83, 365, 97]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "Ki_in"
    	      SID		      "697:21"
    	      Position		      [390, 108, 420, 122]
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "gravTrqMotor"
    	      SID		      "697:22"
    	      Position		      [320, 133, 350, 147]
    	      Port		      "4"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "trqRaw"
    	      SID		      "697:24"
    	      Position		      [265, 158, 295, 172]
    	      Port		      "5"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "posRef_in"
    	      SID		      "697:25"
    	      Position		      [225, 183, 255, 197]
    	      Port		      "6"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "posRaw"
    	      SID		      "697:26"
    	      Position		      [300, 208, 330, 222]
    	      Port		      "7"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "f_switch"
    	      SID		      "697:28"
    	      Position		      [355, 233, 385, 247]
    	      Port		      "8"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "velFlt_filtered"
    	      SID		      "697:29"
    	      Position		      [165, 258, 195, 272]
    	      Port		      "9"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "velFlt1"
    	      SID		      "697:30"
    	      Position		      [160, 333, 190, 347]
    	      Port		      "10"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "keepStill"
    	      SID		      "697:32"
    	      Position		      [345, 308, 375, 322]
    	      Port		      "11"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "coulomb_width"
    	      SID		      "697:34"
    	      Position		      [235, 358, 265, 372]
    	      Port		      "12"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "percentageTrqFfw"
    	      SID		      "697:36"
    	      Position		      [305, 383, 335, 397]
    	      Port		      "13"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "KiHighThres"
    	      SID		      "697:37"
    	      Position		      [370, 408, 400, 422]
    	      Port		      "14"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "slidingThresWidth"
    	      SID		      "697:38"
    	      Position		      [430, 433, 460, 447]
    	      Port		      "15"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "maxRate"
    	      SID		      "697:40"
    	      Position		      [235, 458, 265, 472]
    	      Port		      "16"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "highKiActive"
    	      SID		      "697:42"
    	      Position		      [285, 283, 315, 297]
    	      Port		      "17"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "resetPosRef"
    	      SID		      "697:45"
    	      Position		      [370, 493, 400, 507]
    	      Port		      "18"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "jointLimitsStartMotorRad"
    	      SID		      "697:46"
    	      Position		      [370, 528, 400, 542]
    	      Port		      "19"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "extTorque"
    	      SID		      "697:116"
    	      Position		      [370, 618, 400, 632]
    	      Port		      "20"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "sensorOn"
    	      SID		      "697:117"
    	      Position		      [370, 578, 400, 592]
    	      Port		      "21"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "fricHelp"
    	      SID		      "697:118"
    	      Position		      [370, 673, 400, 687]
    	      Port		      "22"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "torque_motor_2"
    	      SID		      "697:942"
    	      Position		      [370, 738, 400, 752]
    	      Port		      "23"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      SubSystem
    	      Name		      "MATLAB Function"
    	      SID		      "697:17"
    	      Ports		      [23, 7]
    	      Position		      [760, 36, 1265, 624]
    	      LibraryVersion	      "1.32"
    	      ErrorFcn		      "Stateflow.Translate.translate"
    	      PermitHierarchicalResolution "ExplicitOnly"
    	      TreatAsAtomicUnit	      on
    	      MinAlgLoopOccurrences   off
    	      PropExecContextOutsideSubsystem off
    	      RTWSystemCode	      "Auto"
    	      FunctionWithSeparateData off
    	      Opaque		      off
    	      RequestExecContextInheritance off
    	      MaskHideContents	      off
    	      SFBlockType	      "MATLAB Function"
    	      MaskType		      "Stateflow"
    	      MaskDescription	      "Embedded MATLAB block"
    	      MaskSelfModifiable      on
    	      MaskDisplay	      "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
    	      "rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
    	      MaskIconFrame	      on
    	      MaskIconOpaque	      off
    	      MaskIconRotate	      "none"
    	      MaskPortRotate	      "default"
    	      MaskIconUnits	      "normalized"
    	      Port {
    		PortNumber		6
    		Name			"offset[i]"
    		PropagatedSignals	"test"
    		TestPoint		on
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		7
    		Name			"sat"
    		PropagatedSignals	"sat"
    		TestPoint		on
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      System {
    		Name			"MATLAB Function"
    		Location		[257, 457, 812, 717]
    		Open			off
    		ModelBrowserVisibility	off
    		ModelBrowserWidth	200
    		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			  "Kp_in"
    		  SID			  "697:17::1"
    		  Position		  [20, 101, 40, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "Kv_in"
    		  SID			  "697:17::18"
    		  Position		  [20, 136, 40, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "Ki_in"
    		  SID			  "697:17::19"
    		  Position		  [20, 171, 40, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "gravTrqMotor"
    		  SID			  "697:17::20"
    		  Position		  [20, 206, 40, 224]
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "trqRaw"
    		  SID			  "697:17::21"
    		  Position		  [20, 246, 40, 264]
    		  Port			  "5"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "posRefIn"
    		  SID			  "697:17::22"
    		  Position		  [20, 281, 40, 299]
    		  Port			  "6"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "posRaw"
    		  SID			  "697:17::23"
    		  Position		  [20, 316, 40, 334]
    		  Port			  "7"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "f_switch"
    		  SID			  "697:17::24"
    		  Position		  [20, 351, 40, 369]
    		  Port			  "8"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "velFlt"
    		  SID			  "697:17::25"
    		  Position		  [20, 386, 40, 404]
    		  Port			  "9"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "highKiActive"
    		  SID			  "697:17::26"
    		  Position		  [20, 426, 40, 444]
    		  Port			  "10"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "keepStill"
    		  SID			  "697:17::27"
    		  Position		  [20, 461, 40, 479]
    		  Port			  "11"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "velFlt_noExtraFilt"
    		  SID			  "697:17::28"
    		  Position		  [20, 496, 40, 514]
    		  Port			  "12"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "coulomb_width"
    		  SID			  "697:17::34"
    		  Position		  [20, 531, 40, 549]
    		  Port			  "13"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "percentageTrqFfw"
    		  SID			  "697:17::35"
    		  Position		  [20, 566, 40, 584]
    		  Port			  "14"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "KiHighThres"
    		  SID			  "697:17::36"
    		  Position		  [20, 606, 40, 624]
    		  Port			  "15"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "slidingThresWidth"
    		  SID			  "697:17::37"
    		  Position		  [20, 641, 40, 659]
    		  Port			  "16"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "maxRate"
    		  SID			  "697:17::38"
    		  Position		  [20, 676, 40, 694]
    		  Port			  "17"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "resetPosRef"
    		  SID			  "697:17::39"
    		  Position		  [20, 711, 40, 729]
    		  Port			  "18"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "jointLimitStartMotorRad"
    		  SID			  "697:17::40"
    		  Position		  [20, 746, 40, 764]
    		  Port			  "19"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "extTorque"
    		  SID			  "697:17::41"
    		  Position		  [20, 786, 40, 804]
    		  Port			  "20"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "sensorOn"
    		  SID			  "697:17::42"
    		  Position		  [20, 821, 40, 839]
    		  Port			  "21"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "fricHelp"
    		  SID			  "697:17::43"
    		  Position		  [20, 856, 40, 874]
    		  Port			  "22"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "qtorque"
    		  SID			  "697:17::84"
    		  Position		  [20, 891, 40, 909]
    		  ZOrder		  11
    		  Port			  "23"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Demux
    		  Name			  " Demux "
    		  SID			  "697:17::79"
    		  Ports			  [1, 1]
    		  Position		  [270, 580, 320, 620]
    		  ZOrder		  6
    		  Outputs		  "1"
    		}
    		Block {
    		  BlockType		  S-Function
    		  Name			  " SFunction "
    		  SID			  "697:17::78"
    		  Tag			  "Stateflow S-Function scaled_teleoperation 12"
    		  Ports			  [23, 8]
    		  Position		  [180, 100, 230, 580]
    		  ZOrder		  5
    		  FunctionName		  "sf_sfun"
    		  PortCounts		  "[23 8]"
    		  EnableBusSupport	  on
    		  Port {
    		    PortNumber		    2
    		    Name		    "Kp"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    3
    		    Name		    "Kv"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    4
    		    Name		    "Ki"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    5
    		    Name		    "trqFfw"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    6
    		    Name		    "posRef"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    7
    		    Name		    "test"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    8
    		    Name		    "sat"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		}
    		Block {
    		  BlockType		  Terminator
    		  Name			  " Terminator "
    		  SID			  "697:17::81"
    		  Position		  [460, 591, 480, 609]
    		  ZOrder		  8
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Kp"
    		  SID			  "697:17::5"
    		  Position		  [460, 101, 480, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Kv"
    		  SID			  "697:17::29"
    		  Position		  [460, 136, 480, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Ki"
    		  SID			  "697:17::30"
    		  Position		  [460, 171, 480, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "trqFfw"
    		  SID			  "697:17::31"
    		  Position		  [460, 206, 480, 224]
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "posRef"
    		  SID			  "697:17::32"
    		  Position		  [460, 246, 480, 264]
    		  Port			  "5"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "test"
    		  SID			  "697:17::82"
    		  Position		  [460, 281, 480, 299]
    		  ZOrder		  9
    		  Port			  "6"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "sat"
    		  SID			  "697:17::83"
    		  Position		  [460, 316, 480, 334]
    		  ZOrder		  10
    		  Port			  "7"
    		  IconDisplay		  "Port number"
    		}
    		Line {
    		  SrcBlock		  "Kp_in"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  "Kv_in"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  2
    		}
    		Line {
    		  SrcBlock		  "Ki_in"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  3
    		}
    		Line {
    		  SrcBlock		  "gravTrqMotor"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  4
    		}
    		Line {
    		  SrcBlock		  "trqRaw"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  5
    		}
    		Line {
    		  SrcBlock		  "posRefIn"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  6
    		}
    		Line {
    		  SrcBlock		  "posRaw"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  7
    		}
    		Line {
    		  SrcBlock		  "f_switch"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  8
    		}
    		Line {
    		  SrcBlock		  "velFlt"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  9
    		}
    		Line {
    		  SrcBlock		  "highKiActive"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  10
    		}
    		Line {
    		  SrcBlock		  "keepStill"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  11
    		}
    		Line {
    		  SrcBlock		  "velFlt_noExtraFilt"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  12
    		}
    		Line {
    		  SrcBlock		  "coulomb_width"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  13
    		}
    		Line {
    		  SrcBlock		  "percentageTrqFfw"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  14
    		}
    		Line {
    		  SrcBlock		  "KiHighThres"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  15
    		}
    		Line {
    		  SrcBlock		  "slidingThresWidth"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  16
    		}
    		Line {
    		  SrcBlock		  "maxRate"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  17
    		}
    		Line {
    		  SrcBlock		  "resetPosRef"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  18
    		}
    		Line {
    		  SrcBlock		  "jointLimitStartMotorRad"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  19
    		}
    		Line {
    		  SrcBlock		  "extTorque"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  20
    		}
    		Line {
    		  SrcBlock		  "sensorOn"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  21
    		}
    		Line {
    		  SrcBlock		  "fricHelp"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  22
    		}
    		Line {
    		  SrcBlock		  "qtorque"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  23
    		}
    		Line {
    		  Name			  "Kp"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  2
    		  DstBlock		  "Kp"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "Kv"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  3
    		  DstBlock		  "Kv"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "Ki"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  4
    		  DstBlock		  "Ki"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "trqFfw"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  5
    		  DstBlock		  "trqFfw"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "posRef"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  6
    		  DstBlock		  "posRef"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "test"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  7
    		  DstBlock		  "test"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "sat"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  8
    		  DstBlock		  "sat"
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " Demux "
    		  SrcPort		  1
    		  DstBlock		  " Terminator "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " SFunction "
    		  SrcPort		  1
    		  DstBlock		  " Demux "
    		  DstPort		  1
    		}
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      "Terminator1"
    	      SID		      "697:941"
    	      Position		      [1435, 655, 1455, 675]
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      "Terminator14"
    	      SID		      "697:940"
    	      Position		      [1430, 555, 1450, 575]
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "Kp"
    	      SID		      "697:23"
    	      Position		      [1515, 73, 1545, 87]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "Kv"
    	      SID		      "697:27"
    	      Position		      [1525, 188, 1555, 202]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "Ki"
    	      SID		      "697:31"
    	      Position		      [1505, 323, 1535, 337]
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "trqFfw"
    	      SID		      "697:35"
    	      Position		      [1375, 398, 1405, 412]
    	      Port		      "4"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "posRef"
    	      SID		      "697:39"
    	      Position		      [1385, 518, 1415, 532]
    	      Port		      "5"
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "fricHelp"
    	      SrcPort		      1
    	      Points		      [270, 0; 0, -100]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      22
    	    }
    	    Line {
    	      SrcBlock		      "extTorque"
    	      SrcPort		      1
    	      Points		      [115, 0; 0, -95]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      20
    	    }
    	    Line {
    	      SrcBlock		      "sensorOn"
    	      SrcPort		      1
    	      Points		      [60, 0; 0, -30]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      21
    	    }
    	    Line {
    	      SrcBlock		      "jointLimitsStartMotorRad"
    	      SrcPort		      1
    	      Points		      [340, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      19
    	    }
    	    Line {
    	      SrcBlock		      "resetPosRef"
    	      SrcPort		      1
    	      Points		      [340, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      18
    	    }
    	    Line {
    	      SrcBlock		      "highKiActive"
    	      SrcPort		      1
    	      Points		      [425, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      10
    	    }
    	    Line {
    	      SrcBlock		      "velFlt1"
    	      SrcPort		      1
    	      Points		      [550, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      12
    	    }
    	    Line {
    	      SrcBlock		      "velFlt_filtered"
    	      SrcPort		      1
    	      Points		      [545, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      9
    	    }
    	    Line {
    	      SrcBlock		      "maxRate"
    	      SrcPort		      1
    	      Points		      [475, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      17
    	    }
    	    Line {
    	      SrcBlock		      "slidingThresWidth"
    	      SrcPort		      1
    	      Points		      [280, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      16
    	    }
    	    Line {
    	      SrcBlock		      "KiHighThres"
    	      SrcPort		      1
    	      Points		      [340, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      15
    	    }
    	    Line {
    	      SrcBlock		      "percentageTrqFfw"
    	      SrcPort		      1
    	      Points		      [405, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      14
    	    }
    	    Line {
    	      SrcBlock		      "coulomb_width"
    	      SrcPort		      1
    	      Points		      [475, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      13
    	    }
    	    Line {
    	      SrcBlock		      "keepStill"
    	      SrcPort		      1
    	      Points		      [365, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      11
    	    }
    	    Line {
    	      SrcBlock		      "MATLAB Function"
    	      SrcPort		      3
    	      Points		      [0, 15]
    	      DstBlock		      "Ki"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "f_switch"
    	      SrcPort		      1
    	      Points		      [355, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      8
    	    }
    	    Line {
    	      SrcBlock		      "MATLAB Function"
    	      SrcPort		      2
    	      Points		      [0, 10]
    	      DstBlock		      "Kv"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "posRaw"
    	      SrcPort		      1
    	      Points		      [410, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      7
    	    }
    	    Line {
    	      SrcBlock		      "posRef_in"
    	      SrcPort		      1
    	      Points		      [485, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      6
    	    }
    	    Line {
    	      SrcBlock		      "trqRaw"
    	      SrcPort		      1
    	      Points		      [445, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      5
    	    }
    	    Line {
    	      SrcBlock		      "MATLAB Function"
    	      SrcPort		      1
    	      Points		      [0, 5]
    	      DstBlock		      "Kp"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "gravTrqMotor"
    	      SrcPort		      1
    	      Points		      [390, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      4
    	    }
    	    Line {
    	      SrcBlock		      "Ki_in"
    	      SrcPort		      1
    	      Points		      [320, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      3
    	    }
    	    Line {
    	      SrcBlock		      "Kv_in"
    	      SrcPort		      1
    	      Points		      [375, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      2
    	    }
    	    Line {
    	      SrcBlock		      "Kp_in"
    	      SrcPort		      1
    	      Points		      [485, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "MATLAB Function"
    	      SrcPort		      4
    	      Points		      [45, 0; 0, 75]
    	      DstBlock		      "trqFfw"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "MATLAB Function"
    	      SrcPort		      5
    	      Points		      [50, 0; 0, 110]
    	      DstBlock		      "posRef"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "offset[i]"
    	      Labels		      [0, 0]
    	      SrcBlock		      "MATLAB Function"
    	      SrcPort		      6
    	      Points		      [0, 50; 145, 0]
    	      DstBlock		      "Terminator14"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "sat"
    	      Labels		      [2, 0]
    	      SrcBlock		      "MATLAB Function"
    	      SrcPort		      7
    	      Points		      [0, 55; 140, 0; 0, 25]
    	      DstBlock		      "Terminator1"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "torque_motor_2"
    	      SrcPort		      1
    	      Points		      [300, 0; 0, -140]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      23
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  Reference
    	  Name			  "leadthroughLockOutput"
    	  SID			  "729"
    	  Ports			  [3, 2]
    	  Position		  [1785, 841, 1960, 899]
    	  ForegroundColor	  "blue"
    	  LibraryVersion	  "1.5"
    	  SourceBlock		  "rapid_extctrl_handover_lib_leadthrough/leadthroughLockOutput"
    	  SourceType		  "Stateflow"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "coulombWidthArmLeftAbb"
    	  SID			  "1363"
    	  Position		  [1025, 398, 1055, 412]
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.joint[i].parKp"
    	  SID			  "618"
    	  Position		  [1715, 308, 1745, 322]
    	  Port			  "2"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.joint[i].parKv"
    	  SID			  "619"
    	  Position		  [1695, 428, 1725, 442]
    	  Port			  "3"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.joint[i].parKi"
    	  SID			  "620"
    	  Position		  [1715, 548, 1745, 562]
    	  Port			  "4"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.joint[i].posRef"
    	  SID			  "621"
    	  Position		  [2070, 848, 2100, 862]
    	  Port			  "5"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.joint[i].velRef"
    	  SID			  "622"
    	  Position		  [2070, 878, 2100, 892]
    	  Port			  "6"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.joint[i].trqFfw"
    	  SID			  "623"
    	  Position		  [1715, 668, 1745, 682]
    	  Port			  "7"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.instruction"
    	  SID			  "624"
    	  Position		  [1040, 813, 1070, 827]
    	  Port			  "8"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.value1"
    	  SID			  "625"
    	  Position		  [380, 458, 410, 472]
    	  Port			  "9"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.value2"
    	  SID			  "626"
    	  Position		  [380, 493, 410, 507]
    	  Port			  "10"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.value3"
    	  SID			  "627"
    	  Position		  [380, 528, 410, 542]
    	  Port			  "11"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.value4"
    	  SID			  "628"
    	  Position		  [380, 563, 410, 577]
    	  Port			  "12"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.value5"
    	  SID			  "629"
    	  Position		  [380, 598, 410, 612]
    	  Port			  "13"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.value6"
    	  SID			  "630"
    	  Position		  [380, 633, 410, 647]
    	  Port			  "14"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.string1[i]"
    	  SID			  "631"
    	  Position		  [380, 668, 410, 682]
    	  Port			  "15"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "20"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.string2[i]"
    	  SID			  "632"
    	  Position		  [380, 703, 410, 717]
    	  Port			  "16"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "20"
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].parTrqMin"
    	  SrcPort		  1
    	  DstBlock		  "Terminator"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].parTrqMax"
    	  SrcPort		  1
    	  DstBlock		  "Terminator1"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].trqFfwGrav"
    	  SrcPort		  1
    	  DstBlock		  "Terminator2"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].posFlt"
    	  SrcPort		  1
    	  DstBlock		  "Terminator5"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].trqRefFlt"
    	  SrcPort		  1
    	  DstBlock		  "Terminator10"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.instruction"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.instruction"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.string1[i]"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.string1[i]"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.string2[i]"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.string2[i]"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.value2"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.value2"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.value3"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.value3"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.value4"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.value4"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.value5"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.value5"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.value6"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.value6"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.value1"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.value1"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.manualMode"
    	  SrcPort		  1
    	  DstBlock		  "Terminator12"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].velOut"
    	  SrcPort		  1
    	  DstBlock		  "Terminator8"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].posRawFb"
    	  SrcPort		  1
    	  Points		  [35, 0]
    	  Branch {
    	    DstBlock		    "calcGravityTorque"
    	    DstPort		    1
    	  }
    	  Branch {
    	    Points		    [0, 160; 40, -5; 10, 0]
    	    DstBlock		    "leadthroug control"
    	    DstPort		    7
    	  }
    	}
    	Line {
    	  SrcBlock		  "calcGravityTorque"
    	  SrcPort		  1
    	  Points		  [340, 0; 0, 45]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  4
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].trqFfw"
    	  SrcPort		  1
    	  DstBlock		  "Terminator4"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Constant5"
    	  SrcPort		  1
    	  DstBlock		  "calcGravityTorque"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "Constant9"
    	  SrcPort		  1
    	  DstBlock		  "calcGravityTorque"
    	  DstPort		  3
    	}
    	Line {
    	  SrcBlock		  "Constant6"
    	  SrcPort		  1
    	  DstBlock		  "calcGravityTorque"
    	  DstPort		  4
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].parKp"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].parKv"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].parKi"
    	  SrcPort		  1
    	  Points		  [70, 0]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  3
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].trqRaw"
    	  SrcPort		  1
    	  Points		  [95, 0]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  5
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].posRef"
    	  SrcPort		  1
    	  Points		  [210, 0]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  6
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].velFlt"
    	  SrcPort		  1
    	  Points		  [40, 0]
    	  Branch {
    	    DstBlock		    "Discrete Filter3"
    	    DstPort		    1
    	  }
    	  Branch {
    	    Points		    [0, 35]
    	    DstBlock		    "leadthroug control"
    	    DstPort		    10
    	  }
    	}
    	Line {
    	  SrcBlock		  "Constant2"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  17
    	}
    	Line {
    	  SrcBlock		  "Constant3"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  11
    	}
    	Line {
    	  SrcBlock		  "leadthroug control"
    	  SrcPort		  4
    	  Points		  [0, 5]
    	  DstBlock		  "ext2irb.joint[i].trqFfw"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "leadthroug control"
    	  SrcPort		  3
    	  Points		  [0, 10]
    	  DstBlock		  "ext2irb.joint[i].parKi"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "leadthroug control"
    	  SrcPort		  2
    	  DstBlock		  "ext2irb.joint[i].parKv"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "leadthroug control"
    	  SrcPort		  1
    	  Points		  [0, 20]
    	  DstBlock		  "ext2irb.joint[i].parKp"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Constant1"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  16
    	}
    	Line {
    	  SrcBlock		  "Constant4"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  15
    	}
    	Line {
    	  SrcBlock		  "Constant7"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  14
    	}
    	Line {
    	  SrcBlock		  "Constant8"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  13
    	}
    	Line {
    	  SrcBlock		  "Discrete Filter3"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  9
    	}
    	Line {
    	  SrcBlock		  "calcGravityTorque"
    	  SrcPort		  2
    	  Points		  [90, 0; 0, 275]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  12
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function3"
    	  SrcPort		  1
    	  DstBlock		  "Product"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "f_switch"
    	  SrcPort		  1
    	  Points		  [20, 0; 0, 30]
    	  DstBlock		  "Product"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Product"
    	  SrcPort		  1
    	  DstBlock		  "Goto"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "From"
    	  SrcPort		  1
    	  Points		  [65, 0]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  8
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function3"
    	  SrcPort		  2
    	  Points		  [55, 0; 0, -5]
    	  Branch {
    	    Points		    [0, -120]
    	    DstBlock		    "leadthroug control"
    	    DstPort		    18
    	  }
    	  Branch {
    	    Points		    [0, 5; 455, 0]
    	    DstBlock		    "leadthroughLockOutput"
    	    DstPort		    3
    	  }
    	}
    	Line {
    	  SrcBlock		  "Gain3"
    	  SrcPort		  1
    	  DstBlock		  "Terminator11"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Gain2"
    	  SrcPort		  1
    	  DstBlock		  "Gain3"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].velRef"
    	  SrcPort		  1
    	  DstBlock		  "leadthroughLockOutput"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "Constant11"
    	  SrcPort		  1
    	  Points		  [100, 0; 0, -75]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  19
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function3"
    	  SrcPort		  3
    	  DstBlock		  "Terminator13"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function3"
    	  SrcPort		  4
    	  Points		  [100, 0; 0, 15]
    	  DstBlock		  "Gain2"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.controlActive"
    	  SrcPort		  1
    	  DstBlock		  "MATLAB Function3"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.obtaining"
    	  SrcPort		  1
    	  DstBlock		  "MATLAB Function3"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "leadthroug control"
    	  SrcPort		  5
    	  Points		  [75, 0; 0, 60]
    	  DstBlock		  "leadthroughLockOutput"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Constant13"
    	  SrcPort		  1
    	  DstBlock		  "MATLAB Function3"
    	  DstPort		  3
    	}
    	Line {
    	  SrcBlock		  "leadthroughLockOutput"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.joint[i].posRef"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "leadthroughLockOutput"
    	  SrcPort		  2
    	  DstBlock		  "ext2irb.joint[i].velRef"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Constant"
    	  SrcPort		  1
    	  Points		  [135, 0]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  22
    	}
    	Line {
    	  SrcBlock		  "torque_motor_2"
    	  SrcPort		  1
    	  Points		  [50, 0; 0, -5]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  23
    	}
    	Line {
    	  SrcBlock		  "calcGravityTorque"
    	  SrcPort		  3
    	  DstBlock		  "coulombWidthArmLeftAbb"
    	  DstPort		  1
    	}
    	Annotation {
    	  SID			  "1355"
    	  Position		  [964, 363]
    	}
          }
        }
        Block {
          BlockType		      SubSystem
          Name		      "MATLAB Function1"
          SID		      "895"
          Ports		      [1, 1]
          Position		      [1545, 1355, 1595, 1395]
          ShowName		      off
          LibraryVersion	      "1.32"
          ErrorFcn		      "Stateflow.Translate.translate"
          PermitHierarchicalResolution "ExplicitOnly"
          TreatAsAtomicUnit	      on
          MinAlgLoopOccurrences   off
          PropExecContextOutsideSubsystem off
          RTWSystemCode	      "Auto"
          FunctionWithSeparateData off
          Opaque		      off
          RequestExecContextInheritance off
          MaskHideContents	      off
          SFBlockType	      "MATLAB Function"
          MaskType		      "Stateflow"
          MaskDescription	      "Embedded MATLAB block"
          MaskSelfModifiable      on
          MaskDisplay	      "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('pr"
          "ivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'nancheck']);"
          MaskIconFrame	      on
          MaskIconOpaque	      off
          MaskIconRotate	      "none"
          MaskPortRotate	      "default"
          MaskIconUnits	      "normalized"
          System {
    	Name			"MATLAB Function1"
    	Location		[257, 457, 812, 717]
    	Open			off
    	ModelBrowserVisibility	off
    	ModelBrowserWidth	200
    	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	"64"
    	Block {
    	  BlockType		  Inport
    	  Name			  "u"
    	  SID			  "895::1"
    	  Position		  [20, 101, 40, 119]
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Demux
    	  Name			  " Demux "
    	  SID			  "895::62"
    	  Ports			  [1, 1]
    	  Position		  [270, 160, 320, 200]
    	  ZOrder		  14
    	  Outputs		  "1"
    	}
    	Block {
    	  BlockType		  S-Function
    	  Name			  " SFunction "
    	  SID			  "895::61"
    	  Tag			  "Stateflow S-Function scaled_teleoperation 7"
    	  Ports			  [1, 2]
    	  Position		  [180, 100, 230, 160]
    	  ZOrder		  13
    	  FunctionName		  "sf_sfun"
    	  PortCounts		  "[1 2]"
    	  EnableBusSupport	  on
    	  Port {
    	    PortNumber		    2
    	    Name		    "y"
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  " Terminator "
    	  SID			  "895::64"
    	  Position		  [460, 171, 480, 189]
    	  ZOrder		  16
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "y"
    	  SID			  "895::56"
    	  Position		  [460, 101, 480, 119]
    	  IconDisplay		  "Port number"
    	}
    	Line {
    	  SrcBlock		  "u"
    	  SrcPort		  1
    	  DstBlock		  " SFunction "
    	  DstPort		  1
    	}
    	Line {
    	  Name			  "y"
    	  Labels		  [0, 0]
    	  SrcBlock		  " SFunction "
    	  SrcPort		  2
    	  DstBlock		  "y"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  " Demux "
    	  SrcPort		  1
    	  DstBlock		  " Terminator "
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  " SFunction "
    	  SrcPort		  1
    	  DstBlock		  " Demux "
    	  DstPort		  1
    	}
          }
        }
        Block {
          BlockType		      SubSystem
          Name		      "MATLAB Function2"
          SID		      "896"
          Ports		      [1, 1]
          Position		      [1625, 530, 1675, 570]
          ShowName		      off
          LibraryVersion	      "1.32"
          ErrorFcn		      "Stateflow.Translate.translate"
          PermitHierarchicalResolution "ExplicitOnly"
          TreatAsAtomicUnit	      on
          MinAlgLoopOccurrences   off
          PropExecContextOutsideSubsystem off
          RTWSystemCode	      "Auto"
          FunctionWithSeparateData off
          Opaque		      off
          RequestExecContextInheritance off
          MaskHideContents	      off
          SFBlockType	      "MATLAB Function"
          MaskType		      "Stateflow"
          MaskDescription	      "Embedded MATLAB block"
          MaskSelfModifiable      on
          MaskDisplay	      "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('pr"
          "ivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'nancheck']);"
          MaskIconFrame	      on
          MaskIconOpaque	      off
          MaskIconRotate	      "none"
          MaskPortRotate	      "default"
          MaskIconUnits	      "normalized"
          System {
    	Name			"MATLAB Function2"
    	Location		[257, 457, 812, 717]
    	Open			off
    	ModelBrowserVisibility	off
    	ModelBrowserWidth	200
    	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	"64"
    	Block {
    	  BlockType		  Inport
    	  Name			  "u"
    	  SID			  "896::1"
    	  Position		  [20, 101, 40, 119]
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Demux
    	  Name			  " Demux "
    	  SID			  "896::62"
    	  Ports			  [1, 1]
    	  Position		  [270, 160, 320, 200]
    	  ZOrder		  14
    	  Outputs		  "1"
    	}
    	Block {
    	  BlockType		  S-Function
    	  Name			  " SFunction "
    	  SID			  "896::61"
    	  Tag			  "Stateflow S-Function scaled_teleoperation 9"
    	  Ports			  [1, 2]
    	  Position		  [180, 100, 230, 160]
    	  ZOrder		  13
    	  FunctionName		  "sf_sfun"
    	  PortCounts		  "[1 2]"
    	  EnableBusSupport	  on
    	  Port {
    	    PortNumber		    2
    	    Name		    "y"
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  " Terminator "
    	  SID			  "896::64"
    	  Position		  [460, 171, 480, 189]
    	  ZOrder		  16
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "y"
    	  SID			  "896::56"
    	  Position		  [460, 101, 480, 119]
    	  IconDisplay		  "Port number"
    	}
    	Line {
    	  SrcBlock		  "u"
    	  SrcPort		  1
    	  DstBlock		  " SFunction "
    	  DstPort		  1
    	}
    	Line {
    	  Name			  "y"
    	  Labels		  [0, 0]
    	  SrcBlock		  " SFunction "
    	  SrcPort		  2
    	  DstBlock		  "y"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  " Demux "
    	  SrcPort		  1
    	  DstBlock		  " Terminator "
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  " SFunction "
    	  SrcPort		  1
    	  DstBlock		  " Demux "
    	  DstPort		  1
    	}
          }
        }
        Block {
          BlockType		      SubSystem
          Name		      "MATLAB Function3"
          SID		      "934"
          Ports		      [1, 1]
          Position		      [695, 520, 745, 560]
          ShowName		      off
          LibraryVersion	      "1.32"
          ErrorFcn		      "Stateflow.Translate.translate"
          PermitHierarchicalResolution "ExplicitOnly"
          TreatAsAtomicUnit	      on
          MinAlgLoopOccurrences   off
          PropExecContextOutsideSubsystem off
          RTWSystemCode	      "Auto"
          FunctionWithSeparateData off
          Opaque		      off
          RequestExecContextInheritance off
          MaskHideContents	      off
          SFBlockType	      "MATLAB Function"
          MaskType		      "Stateflow"
          MaskDescription	      "Embedded MATLAB block"
          MaskSelfModifiable      on
          MaskDisplay	      "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('pr"
          "ivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'nancheck']);"
          MaskIconFrame	      on
          MaskIconOpaque	      off
          MaskIconRotate	      "none"
          MaskPortRotate	      "default"
          MaskIconUnits	      "normalized"
          System {
    	Name			"MATLAB Function3"
    	Location		[257, 457, 812, 717]
    	Open			off
    	ModelBrowserVisibility	off
    	ModelBrowserWidth	200
    	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	"64"
    	Block {
    	  BlockType		  Inport
    	  Name			  "u"
    	  SID			  "934::1"
    	  Position		  [20, 101, 40, 119]
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Demux
    	  Name			  " Demux "
    	  SID			  "934::62"
    	  Ports			  [1, 1]
    	  Position		  [270, 160, 320, 200]
    	  ZOrder		  14
    	  Outputs		  "1"
    	}
    	Block {
    	  BlockType		  S-Function
    	  Name			  " SFunction "
    	  SID			  "934::61"
    	  Tag			  "Stateflow S-Function scaled_teleoperation 6"
    	  Ports			  [1, 2]
    	  Position		  [180, 100, 230, 160]
    	  ZOrder		  13
    	  FunctionName		  "sf_sfun"
    	  PortCounts		  "[1 2]"
    	  EnableBusSupport	  on
    	  Port {
    	    PortNumber		    2
    	    Name		    "y"
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  " Terminator "
    	  SID			  "934::64"
    	  Position		  [460, 171, 480, 189]
    	  ZOrder		  16
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "y"
    	  SID			  "934::56"
    	  Position		  [460, 101, 480, 119]
    	  IconDisplay		  "Port number"
    	}
    	Line {
    	  SrcBlock		  "u"
    	  SrcPort		  1
    	  DstBlock		  " SFunction "
    	  DstPort		  1
    	}
    	Line {
    	  Name			  "y"
    	  Labels		  [0, 0]
    	  SrcBlock		  " SFunction "
    	  SrcPort		  2
    	  DstBlock		  "y"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  " Demux "
    	  SrcPort		  1
    	  DstBlock		  " Terminator "
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  " SFunction "
    	  SrcPort		  1
    	  DstBlock		  " Demux "
    	  DstPort		  1
    	}
          }
        }
        Block {
          BlockType		      SubSystem
          Name		      "MATLAB Function4"
          SID		      "935"
          Ports		      [1, 1]
          Position		      [685, 585, 735, 625]
          ShowName		      off
          LibraryVersion	      "1.32"
          ErrorFcn		      "Stateflow.Translate.translate"
          PermitHierarchicalResolution "ExplicitOnly"
          TreatAsAtomicUnit	      on
          MinAlgLoopOccurrences   off
          PropExecContextOutsideSubsystem off
          RTWSystemCode	      "Auto"
          FunctionWithSeparateData off
          Opaque		      off
          RequestExecContextInheritance off
          MaskHideContents	      off
          SFBlockType	      "MATLAB Function"
          MaskType		      "Stateflow"
          MaskDescription	      "Embedded MATLAB block"
          MaskSelfModifiable      on
          MaskDisplay	      "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('pr"
          "ivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'nancheck']);"
          MaskIconFrame	      on
          MaskIconOpaque	      off
          MaskIconRotate	      "none"
          MaskPortRotate	      "default"
          MaskIconUnits	      "normalized"
          System {
    	Name			"MATLAB Function4"
    	Location		[257, 457, 812, 717]
    	Open			off
    	ModelBrowserVisibility	off
    	ModelBrowserWidth	200
    	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	"64"
    	Block {
    	  BlockType		  Inport
    	  Name			  "u"
    	  SID			  "935::1"
    	  Position		  [20, 101, 40, 119]
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Demux
    	  Name			  " Demux "
    	  SID			  "935::62"
    	  Ports			  [1, 1]
    	  Position		  [270, 160, 320, 200]
    	  ZOrder		  14
    	  Outputs		  "1"
    	}
    	Block {
    	  BlockType		  S-Function
    	  Name			  " SFunction "
    	  SID			  "935::61"
    	  Tag			  "Stateflow S-Function scaled_teleoperation 10"
    	  Ports			  [1, 2]
    	  Position		  [180, 100, 230, 160]
    	  ZOrder		  13
    	  FunctionName		  "sf_sfun"
    	  PortCounts		  "[1 2]"
    	  EnableBusSupport	  on
    	  Port {
    	    PortNumber		    2
    	    Name		    "y"
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  " Terminator "
    	  SID			  "935::64"
    	  Position		  [460, 171, 480, 189]
    	  ZOrder		  16
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "y"
    	  SID			  "935::56"
    	  Position		  [460, 101, 480, 119]
    	  IconDisplay		  "Port number"
    	}
    	Line {
    	  SrcBlock		  "u"
    	  SrcPort		  1
    	  DstBlock		  " SFunction "
    	  DstPort		  1
    	}
    	Line {
    	  Name			  "y"
    	  Labels		  [0, 0]
    	  SrcBlock		  " SFunction "
    	  SrcPort		  2
    	  DstBlock		  "y"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  " Demux "
    	  SrcPort		  1
    	  DstBlock		  " Terminator "
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  " SFunction "
    	  SrcPort		  1
    	  DstBlock		  " Demux "
    	  DstPort		  1
    	}
          }
        }
        Block {
          BlockType		      SubSystem
          Name		      "Right"
          SID		      "633"
          Ports		      [29, 16]
          Position		      [1150, 190, 1480, 1430]
          LibraryVersion	      "1.762"
          MinAlgLoopOccurrences   off
          PropExecContextOutsideSubsystem off
          RTWSystemCode	      "Auto"
          FunctionWithSeparateData off
          Opaque		      off
          RequestExecContextInheritance off
          MaskHideContents	      off
          System {
    	Name			"Right"
    	Location		[0, -28, 1920, 1080]
    	Open			off
    	ModelBrowserVisibility	off
    	ModelBrowserWidth	200
    	ScreenColor		"white"
    	PaperOrientation	"landscape"
    	PaperPositionMode	"auto"
    	PaperType		"A4"
    	PaperUnits		"centimeters"
    	TiledPaperMargins	[1.270000, 1.270000, 1.270000, 1.270000]
    	TiledPageScale		1
    	ShowPageBoundaries	off
    	ZoomFactor		"100"
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.obtaining"
    	  SID			  "634"
    	  Position		  [415, 968, 445, 982]
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.manualMode"
    	  SID			  "635"
    	  Position		  [65, 83, 95, 97]
    	  Port			  "2"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.controlActive"
    	  SID			  "636"
    	  Position		  [415, 928, 445, 942]
    	  Port			  "3"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].parKp"
    	  SID			  "637"
    	  Position		  [820, 58, 850, 72]
    	  Port			  "4"
    	  IconDisplay		  "Port number"
    	  OutDataTypeStr	  "double"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].parKv"
    	  SID			  "638"
    	  Position		  [830, 103, 860, 117]
    	  Port			  "5"
    	  IconDisplay		  "Port number"
    	  OutDataTypeStr	  "double"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].parKi"
    	  SID			  "639"
    	  Position		  [825, 148, 855, 162]
    	  Port			  "6"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].parTrqMin"
    	  SID			  "640"
    	  Position		  [65, 233, 95, 247]
    	  Port			  "7"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].parTrqMax"
    	  SID			  "641"
    	  Position		  [65, 263, 95, 277]
    	  Port			  "8"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].posRawFb"
    	  SID			  "643"
    	  Position		  [320, 183, 350, 197]
    	  Port			  "9"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].posFlt"
    	  SID			  "644"
    	  Position		  [65, 363, 95, 377]
    	  Port			  "10"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].velFlt"
    	  SID			  "646"
    	  Position		  [750, 373, 780, 387]
    	  Port			  "11"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].velOut"
    	  SID			  "647"
    	  Position		  [65, 453, 95, 467]
    	  Port			  "12"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].trqRaw"
    	  SID			  "648"
    	  Position		  [835, 198, 865, 212]
    	  Port			  "13"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].trqRefFlt"
    	  SID			  "649"
    	  Position		  [65, 513, 95, 527]
    	  Port			  "14"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].trqFfw"
    	  SID			  "652"
    	  Position		  [65, 618, 95, 632]
    	  Port			  "15"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].trqFfwGrav"
    	  SID			  "653"
    	  Position		  [65, 653, 95, 667]
    	  Port			  "16"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.instruction"
    	  SID			  "654"
    	  Position		  [75, 778, 105, 792]
    	  Port			  "17"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.value1"
    	  SID			  "655"
    	  Position		  [65, 838, 95, 852]
    	  Port			  "18"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.value2"
    	  SID			  "656"
    	  Position		  [65, 873, 95, 887]
    	  Port			  "19"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.value3"
    	  SID			  "657"
    	  Position		  [65, 908, 95, 922]
    	  Port			  "20"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.value4"
    	  SID			  "658"
    	  Position		  [65, 943, 95, 957]
    	  Port			  "21"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.value5"
    	  SID			  "659"
    	  Position		  [65, 978, 95, 992]
    	  Port			  "22"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.value6"
    	  SID			  "660"
    	  Position		  [65, 1013, 95, 1027]
    	  Port			  "23"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.string1[i]"
    	  SID			  "661"
    	  Position		  [65, 1048, 95, 1062]
    	  Port			  "24"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "20"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.mocgendata.string2[i]"
    	  SID			  "662"
    	  Position		  [65, 1083, 95, 1097]
    	  Port			  "25"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "20"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "f_switch"
    	  SID			  "663"
    	  Position		  [900, 873, 930, 887]
    	  Port			  "26"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].velRef"
    	  SID			  "651"
    	  Position		  [1190, 773, 1220, 787]
    	  Port			  "27"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "irb2ext.joint[i].posRef"
    	  SID			  "1251"
    	  Position		  [755, 298, 785, 312]
    	  NamePlacement		  "alternate"
    	  Port			  "28"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "torque_motor_1"
    	  SID			  "1354"
    	  Position		  [440, 838, 470, 852]
    	  Port			  "29"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	  SignalType		  "real"
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant"
    	  SID			  "1302"
    	  Position		  [695, 865, 725, 895]
    	  ZOrder		  -4
    	  Value			  "0"
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant1"
    	  SID			  "703"
    	  Position		  [745, 729, 860, 751]
    	  ShowName		  off
    	  Value			  "maxRateRight"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant10"
    	  SID			  "1255"
    	  Position		  [390, 1005, 470, 1025]
    	  ShowName		  off
    	  Value			  "f_switch"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant11"
    	  SID			  "737"
    	  Position		  [615, 795, 755, 815]
    	  ShowName		  off
    	  Value			  "jointLimitsStartMotorRad"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant2"
    	  SID			  "704"
    	  Position		  [740, 764, 855, 786]
    	  ShowName		  off
    	  Value			  "highKiActive"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant3"
    	  SID			  "705"
    	  Position		  [770, 504, 885, 526]
    	  ShowName		  off
    	  Value			  "keepStill"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant4"
    	  SID			  "706"
    	  Position		  [745, 684, 860, 706]
    	  ShowName		  off
    	  Value			  "slidingThresWidthRight"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant5"
    	  SID			  "707"
    	  Position		  [290, 219, 405, 241]
    	  ShowName		  off
    	  Value			  "x_hat_rob1"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant6"
    	  SID			  "708"
    	  Position		  [290, 299, 405, 321]
    	  ShowName		  off
    	  Value			  "baseAnglesRight"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant7"
    	  SID			  "709"
    	  Position		  [745, 639, 860, 661]
    	  ShowName		  off
    	  Value			  "KiHighThresRight"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant8"
    	  SID			  "710"
    	  Position		  [710, 579, 825, 601]
    	  ShowName		  off
    	  Value			  "percentageTrqFfwRight"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant9"
    	  SID			  "711"
    	  Position		  [290, 259, 405, 281]
    	  ShowName		  off
    	  Value			  "gearRatio"
    	  VectorParams1D	  off
    	}
    	Block {
    	  BlockType		  DiscreteFilter
    	  Name			  "Discrete Filter3"
    	  SID			  "712"
    	  Ports			  [1, 1]
    	  Position		  [800, 407, 860, 443]
    	  InputPortMap		  "u0"
    	  Numerator		  "Blpfilt"
    	  Denominator		  "Alpfilt"
    	  a0EqualsOne		  on
    	}
    	Block {
    	  BlockType		  From
    	  Name			  "From"
    	  SID			  "726"
    	  Position		  [775, 322, 880, 348]
    	  ShowName		  off
    	  GotoTag		  "f_switch_mod"
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain"
    	  SID			  "722"
    	  Position		  [965, 1020, 995, 1050]
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain1"
    	  SID			  "723"
    	  Position		  [1065, 1020, 1095, 1050]
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	}
    	Block {
    	  BlockType		  Goto
    	  Name			  "Goto"
    	  SID			  "725"
    	  Position		  [1060, 911, 1155, 939]
    	  ShowName		  off
    	  GotoTag		  "f_switch_mod"
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "MATLAB Function3"
    	  SID			  "1253"
    	  Ports			  [3, 4]
    	  Position		  [525, 913, 780, 1037]
    	  ShowName		  off
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "MATLAB Function3"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "68"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "controlActive"
    	      SID		      "1253::66"
    	      Position		      [20, 101, 40, 119]
    	      ZOrder		      18
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "obtaining"
    	      SID		      "1253::67"
    	      Position		      [20, 136, 40, 154]
    	      ZOrder		      19
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "switch_on"
    	      SID		      "1253::68"
    	      Position		      [20, 171, 40, 189]
    	      ZOrder		      20
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1253::62"
    	      Ports		      [1, 1]
    	      Position		      [270, 250, 320, 290]
    	      ZOrder		      14
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1253::61"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 27"
    	      Ports		      [3, 5]
    	      Position		      [180, 108, 230, 252]
    	      ZOrder		      13
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[3 5]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"runExtctrl"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		3
    		Name			"lockOutput"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		4
    		Name			"resetProgram"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		5
    		Name			"state"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1253::64"
    	      Position		      [460, 261, 480, 279]
    	      ZOrder		      16
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "runExtctrl"
    	      SID		      "1253::56"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "lockOutput"
    	      SID		      "1253::5"
    	      Position		      [460, 136, 480, 154]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "resetProgram"
    	      SID		      "1253::45"
    	      Position		      [460, 171, 480, 189]
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "state"
    	      SID		      "1253::65"
    	      Position		      [460, 206, 480, 224]
    	      ZOrder		      17
    	      Port		      "4"
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "controlActive"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "obtaining"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      2
    	    }
    	    Line {
    	      SrcBlock		      "switch_on"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      3
    	    }
    	    Line {
    	      Name		      "runExtctrl"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "runExtctrl"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "lockOutput"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      3
    	      DstBlock		      "lockOutput"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "resetProgram"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      4
    	      DstBlock		      "resetProgram"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "state"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      5
    	      DstBlock		      "state"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  Product
    	  Name			  "Product"
    	  SID			  "724"
    	  Ports			  [2, 1]
    	  Position		  [985, 907, 1015, 938]
    	  InputSameDT		  off
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator"
    	  SID			  "664"
    	  Position		  [180, 230, 200, 250]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator1"
    	  SID			  "665"
    	  Position		  [220, 260, 240, 280]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator10"
    	  SID			  "666"
    	  Position		  [220, 510, 240, 530]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator11"
    	  SID			  "667"
    	  Position		  [220, 80, 240, 100]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator2"
    	  SID			  "674"
    	  Position		  [175, 650, 195, 670]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator4"
    	  SID			  "716"
    	  Position		  [175, 615, 195, 635]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator5"
    	  SID			  "676"
    	  Position		  [180, 360, 200, 380]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator7"
    	  SID			  "721"
    	  Position		  [1130, 1025, 1150, 1045]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator8"
    	  SID			  "728"
    	  Position		  [1020, 990, 1040, 1010]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "aTerminator8"
    	  SID			  "678"
    	  Position		  [220, 450, 240, 470]
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "calcGravityTorque"
    	  SID			  "713"
    	  Ports			  [4, 3]
    	  Position		  [455, 165, 635, 335]
    	  AncestorBlock		  "leadthroughLib/calcGravityTorque"
    	  LibraryVersion	  "*1.87"
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  System {
    	    Name		    "calcGravityTorque"
    	    Location		    [603, 3, 1594, 610]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "940"
    	    SIDPrevWatermark	    "939"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "q_motor"
    	      SID		      "713:3"
    	      Position		      [150, 78, 180, 92]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "x_hat"
    	      SID		      "713:8"
    	      Position		      [175, 188, 205, 202]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "gearRatio"
    	      SID		      "713:10"
    	      Position		      [200, 298, 230, 312]
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "baseAngles"
    	      SID		      "713:12"
    	      Position		      [200, 408, 230, 422]
    	      Port		      "4"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      "Terminator1"
    	      SID		      "713:16"
    	      Position		      [675, 75, 695, 95]
    	    }
    	    Block {
    	      BlockType		      SubSystem
    	      Name		      "calcGravTrq"
    	      SID		      "713:1"
    	      Ports		      [4, 4]
    	      Position		      [305, 34, 600, 466]
    	      LibraryVersion	      "1.32"
    	      ErrorFcn		      "Stateflow.Translate.translate"
    	      PermitHierarchicalResolution "ExplicitOnly"
    	      TreatAsAtomicUnit	      on
    	      MinAlgLoopOccurrences   off
    	      PropExecContextOutsideSubsystem off
    	      RTWSystemCode	      "Auto"
    	      FunctionWithSeparateData off
    	      Opaque		      off
    	      RequestExecContextInheritance off
    	      MaskHideContents	      off
    	      SFBlockType	      "MATLAB Function"
    	      MaskType		      "Stateflow"
    	      MaskDescription	      "Embedded MATLAB block"
    	      MaskSelfModifiable      on
    	      MaskDisplay	      "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
    	      "rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
    	      MaskIconFrame	      on
    	      MaskIconOpaque	      off
    	      MaskIconRotate	      "none"
    	      MaskPortRotate	      "default"
    	      MaskIconUnits	      "normalized"
    	      System {
    		Name			"calcGravTrq"
    		Location		[257, 457, 812, 717]
    		Open			off
    		ModelBrowserVisibility	off
    		ModelBrowserWidth	200
    		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	"46"
    		Block {
    		  BlockType		  Inport
    		  Name			  "q_motor"
    		  SID			  "713:1::20"
    		  Position		  [20, 101, 40, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "x_hat"
    		  SID			  "713:1::21"
    		  Position		  [20, 136, 40, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "gearRatio"
    		  SID			  "713:1::22"
    		  Position		  [20, 171, 40, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "baseAngles"
    		  SID			  "713:1::24"
    		  Position		  [20, 206, 40, 224]
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Demux
    		  Name			  " Demux "
    		  SID			  "713:1::43"
    		  Ports			  [1, 1]
    		  Position		  [270, 245, 320, 285]
    		  ZOrder		  6
    		  Outputs		  "1"
    		}
    		Block {
    		  BlockType		  S-Function
    		  Name			  " SFunction "
    		  SID			  "713:1::42"
    		  Tag			  "Stateflow S-Function scaled_teleoperation 16"
    		  Ports			  [4, 5]
    		  Position		  [180, 100, 230, 220]
    		  ZOrder		  5
    		  FunctionName		  "sf_sfun"
    		  PortCounts		  "[4 5]"
    		  EnableBusSupport	  on
    		  Port {
    		    PortNumber		    2
    		    Name		    "gravTrq"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    3
    		    Name		    "gravTrqMotor"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    4
    		    Name		    "coulombWidthMotor"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    5
    		    Name		    "coulombWidth"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		}
    		Block {
    		  BlockType		  Terminator
    		  Name			  " Terminator "
    		  SID			  "713:1::45"
    		  Position		  [460, 256, 480, 274]
    		  ZOrder		  8
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "gravTrq"
    		  SID			  "713:1::25"
    		  Position		  [460, 101, 480, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "gravTrqMotor"
    		  SID			  "713:1::26"
    		  Position		  [460, 136, 480, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "coulombWidthMotor"
    		  SID			  "713:1::27"
    		  Position		  [460, 171, 480, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "coulombWidth"
    		  SID			  "713:1::46"
    		  Position		  [460, 206, 480, 224]
    		  ZOrder		  9
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Line {
    		  SrcBlock		  "q_motor"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  "x_hat"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  2
    		}
    		Line {
    		  SrcBlock		  "gearRatio"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  3
    		}
    		Line {
    		  SrcBlock		  "baseAngles"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  4
    		}
    		Line {
    		  Name			  "gravTrq"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  2
    		  DstBlock		  "gravTrq"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "gravTrqMotor"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  3
    		  DstBlock		  "gravTrqMotor"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "coulombWidthMotor"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  4
    		  DstBlock		  "coulombWidthMotor"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "coulombWidth"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  5
    		  DstBlock		  "coulombWidth"
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " Demux "
    		  SrcPort		  1
    		  DstBlock		  " Terminator "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " SFunction "
    		  SrcPort		  1
    		  DstBlock		  " Demux "
    		  DstPort		  1
    		}
    	      }
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "gravTrqMotor"
    	      SID		      "713:13"
    	      Position		      [690, 188, 720, 202]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "coulombWidthMotor"
    	      SID		      "713:43"
    	      Position		      [680, 298, 710, 312]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "coulombWidthArm"
    	      SID		      "713:940"
    	      Position		      [690, 408, 720, 422]
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "calcGravTrq"
    	      SrcPort		      3
    	      DstBlock		      "coulombWidthMotor"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "calcGravTrq"
    	      SrcPort		      2
    	      DstBlock		      "gravTrqMotor"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "baseAngles"
    	      SrcPort		      1
    	      DstBlock		      "calcGravTrq"
    	      DstPort		      4
    	    }
    	    Line {
    	      SrcBlock		      "gearRatio"
    	      SrcPort		      1
    	      DstBlock		      "calcGravTrq"
    	      DstPort		      3
    	    }
    	    Line {
    	      SrcBlock		      "calcGravTrq"
    	      SrcPort		      1
    	      DstBlock		      "Terminator1"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "x_hat"
    	      SrcPort		      1
    	      DstBlock		      "calcGravTrq"
    	      DstPort		      2
    	    }
    	    Line {
    	      SrcBlock		      "q_motor"
    	      SrcPort		      1
    	      DstBlock		      "calcGravTrq"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "calcGravTrq"
    	      SrcPort		      4
    	      DstBlock		      "coulombWidthArm"
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "leadthroug control"
    	  SID			  "714"
    	  Ports			  [23, 5]
    	  Position		  [920, 35, 1140, 835]
    	  AncestorBlock		  "leadthroughLib/leadthroug control"
    	  LibraryVersion	  "*1.87"
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  System {
    	    Name		    "leadthroug control"
    	    Location		    [1, 8, 1919, 822]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "941"
    	    SIDPrevWatermark	    "939"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "Kp_in"
    	      SID		      "714:19"
    	      Position		      [225, 58, 255, 72]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "Kv_in"
    	      SID		      "714:20"
    	      Position		      [335, 83, 365, 97]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "Ki_in"
    	      SID		      "714:21"
    	      Position		      [390, 108, 420, 122]
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "gravTrqMotor"
    	      SID		      "714:22"
    	      Position		      [320, 133, 350, 147]
    	      Port		      "4"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "trqRaw"
    	      SID		      "714:24"
    	      Position		      [265, 158, 295, 172]
    	      Port		      "5"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "posRef_in"
    	      SID		      "714:25"
    	      Position		      [225, 183, 255, 197]
    	      Port		      "6"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "posRaw"
    	      SID		      "714:26"
    	      Position		      [300, 208, 330, 222]
    	      Port		      "7"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "f_switch"
    	      SID		      "714:28"
    	      Position		      [355, 233, 385, 247]
    	      Port		      "8"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "velFlt_filtered"
    	      SID		      "714:29"
    	      Position		      [165, 258, 195, 272]
    	      Port		      "9"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "velFlt1"
    	      SID		      "714:30"
    	      Position		      [160, 333, 190, 347]
    	      Port		      "10"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "keepStill"
    	      SID		      "714:32"
    	      Position		      [345, 308, 375, 322]
    	      Port		      "11"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "coulomb_width"
    	      SID		      "714:34"
    	      Position		      [235, 358, 265, 372]
    	      Port		      "12"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "percentageTrqFfw"
    	      SID		      "714:36"
    	      Position		      [305, 383, 335, 397]
    	      Port		      "13"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "KiHighThres"
    	      SID		      "714:37"
    	      Position		      [370, 408, 400, 422]
    	      Port		      "14"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "slidingThresWidth"
    	      SID		      "714:38"
    	      Position		      [430, 433, 460, 447]
    	      Port		      "15"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "maxRate"
    	      SID		      "714:40"
    	      Position		      [235, 458, 265, 472]
    	      Port		      "16"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "highKiActive"
    	      SID		      "714:42"
    	      Position		      [285, 283, 315, 297]
    	      Port		      "17"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "resetPosRef"
    	      SID		      "714:45"
    	      Position		      [370, 493, 400, 507]
    	      Port		      "18"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "jointLimitsStartMotorRad"
    	      SID		      "714:46"
    	      Position		      [370, 528, 400, 542]
    	      Port		      "19"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "extTorque"
    	      SID		      "714:116"
    	      Position		      [370, 618, 400, 632]
    	      Port		      "20"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "sensorOn"
    	      SID		      "714:117"
    	      Position		      [370, 578, 400, 592]
    	      Port		      "21"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "fricHelp"
    	      SID		      "714:118"
    	      Position		      [370, 673, 400, 687]
    	      Port		      "22"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "qtorgue"
    	      SID		      "714:941"
    	      Position		      [360, 728, 390, 742]
    	      Port		      "23"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      SubSystem
    	      Name		      "MATLAB Function"
    	      SID		      "714:17"
    	      Ports		      [23, 6]
    	      Position		      [540, 48, 1050, 572]
    	      LibraryVersion	      "1.32"
    	      ErrorFcn		      "Stateflow.Translate.translate"
    	      PermitHierarchicalResolution "ExplicitOnly"
    	      TreatAsAtomicUnit	      on
    	      MinAlgLoopOccurrences   off
    	      PropExecContextOutsideSubsystem off
    	      RTWSystemCode	      "Auto"
    	      FunctionWithSeparateData off
    	      Opaque		      off
    	      RequestExecContextInheritance off
    	      MaskHideContents	      off
    	      SFBlockType	      "MATLAB Function"
    	      MaskType		      "Stateflow"
    	      MaskDescription	      "Embedded MATLAB block"
    	      MaskSelfModifiable      on
    	      MaskDisplay	      "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
    	      "rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
    	      MaskIconFrame	      on
    	      MaskIconOpaque	      off
    	      MaskIconRotate	      "none"
    	      MaskPortRotate	      "default"
    	      MaskIconUnits	      "normalized"
    	      Port {
    		PortNumber		6
    		Name			"rstill[i]"
    		PropagatedSignals	"rstill"
    		TestPoint		on
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      System {
    		Name			"MATLAB Function"
    		Location		[257, 457, 812, 717]
    		Open			off
    		ModelBrowserVisibility	off
    		ModelBrowserWidth	200
    		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	"83"
    		Block {
    		  BlockType		  Inport
    		  Name			  "Kp_in"
    		  SID			  "714:17::1"
    		  Position		  [20, 101, 40, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "Kv_in"
    		  SID			  "714:17::18"
    		  Position		  [20, 136, 40, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "Ki_in"
    		  SID			  "714:17::19"
    		  Position		  [20, 171, 40, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "gravTrqMotor"
    		  SID			  "714:17::20"
    		  Position		  [20, 206, 40, 224]
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "trqRaw"
    		  SID			  "714:17::21"
    		  Position		  [20, 246, 40, 264]
    		  Port			  "5"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "posRefIn"
    		  SID			  "714:17::22"
    		  Position		  [20, 281, 40, 299]
    		  Port			  "6"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "posRaw"
    		  SID			  "714:17::23"
    		  Position		  [20, 316, 40, 334]
    		  Port			  "7"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "f_switch"
    		  SID			  "714:17::24"
    		  Position		  [20, 351, 40, 369]
    		  Port			  "8"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "velFlt"
    		  SID			  "714:17::25"
    		  Position		  [20, 386, 40, 404]
    		  Port			  "9"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "highKiActive"
    		  SID			  "714:17::26"
    		  Position		  [20, 426, 40, 444]
    		  Port			  "10"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "keepStill"
    		  SID			  "714:17::27"
    		  Position		  [20, 461, 40, 479]
    		  Port			  "11"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "velFlt_noExtraFilt"
    		  SID			  "714:17::28"
    		  Position		  [20, 496, 40, 514]
    		  Port			  "12"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "coulomb_width"
    		  SID			  "714:17::34"
    		  Position		  [20, 531, 40, 549]
    		  Port			  "13"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "percentageTrqFfw"
    		  SID			  "714:17::35"
    		  Position		  [20, 566, 40, 584]
    		  Port			  "14"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "KiHighThres"
    		  SID			  "714:17::36"
    		  Position		  [20, 606, 40, 624]
    		  Port			  "15"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "slidingThresWidth"
    		  SID			  "714:17::37"
    		  Position		  [20, 641, 40, 659]
    		  Port			  "16"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "maxRate"
    		  SID			  "714:17::38"
    		  Position		  [20, 676, 40, 694]
    		  Port			  "17"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "resetPosRef"
    		  SID			  "714:17::39"
    		  Position		  [20, 711, 40, 729]
    		  Port			  "18"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "jointLimitStartMotorRad"
    		  SID			  "714:17::40"
    		  Position		  [20, 746, 40, 764]
    		  Port			  "19"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "extTorque"
    		  SID			  "714:17::41"
    		  Position		  [20, 786, 40, 804]
    		  Port			  "20"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "sensorOn"
    		  SID			  "714:17::42"
    		  Position		  [20, 821, 40, 839]
    		  Port			  "21"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "fricHelp"
    		  SID			  "714:17::43"
    		  Position		  [20, 856, 40, 874]
    		  Port			  "22"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "qtorque"
    		  SID			  "714:17::83"
    		  Position		  [20, 891, 40, 909]
    		  ZOrder		  10
    		  Port			  "23"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Demux
    		  Name			  " Demux "
    		  SID			  "714:17::79"
    		  Ports			  [1, 1]
    		  Position		  [270, 580, 320, 620]
    		  ZOrder		  6
    		  Outputs		  "1"
    		}
    		Block {
    		  BlockType		  S-Function
    		  Name			  " SFunction "
    		  SID			  "714:17::78"
    		  Tag			  "Stateflow S-Function scaled_teleoperation 13"
    		  Ports			  [23, 7]
    		  Position		  [180, 100, 230, 580]
    		  ZOrder		  5
    		  FunctionName		  "sf_sfun"
    		  PortCounts		  "[23 7]"
    		  EnableBusSupport	  on
    		  Port {
    		    PortNumber		    2
    		    Name		    "Kp"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    3
    		    Name		    "Kv"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    4
    		    Name		    "Ki"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    5
    		    Name		    "trqFfw"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    6
    		    Name		    "posRef"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    7
    		    Name		    "rstill"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		}
    		Block {
    		  BlockType		  Terminator
    		  Name			  " Terminator "
    		  SID			  "714:17::81"
    		  Position		  [460, 591, 480, 609]
    		  ZOrder		  8
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Kp"
    		  SID			  "714:17::5"
    		  Position		  [460, 101, 480, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Kv"
    		  SID			  "714:17::29"
    		  Position		  [460, 136, 480, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Ki"
    		  SID			  "714:17::30"
    		  Position		  [460, 171, 480, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "trqFfw"
    		  SID			  "714:17::31"
    		  Position		  [460, 206, 480, 224]
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "posRef"
    		  SID			  "714:17::32"
    		  Position		  [460, 246, 480, 264]
    		  Port			  "5"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "rstill"
    		  SID			  "714:17::82"
    		  Position		  [460, 281, 480, 299]
    		  ZOrder		  9
    		  Port			  "6"
    		  IconDisplay		  "Port number"
    		}
    		Line {
    		  SrcBlock		  "Kp_in"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  "Kv_in"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  2
    		}
    		Line {
    		  SrcBlock		  "Ki_in"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  3
    		}
    		Line {
    		  SrcBlock		  "gravTrqMotor"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  4
    		}
    		Line {
    		  SrcBlock		  "trqRaw"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  5
    		}
    		Line {
    		  SrcBlock		  "posRefIn"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  6
    		}
    		Line {
    		  SrcBlock		  "posRaw"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  7
    		}
    		Line {
    		  SrcBlock		  "f_switch"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  8
    		}
    		Line {
    		  SrcBlock		  "velFlt"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  9
    		}
    		Line {
    		  SrcBlock		  "highKiActive"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  10
    		}
    		Line {
    		  SrcBlock		  "keepStill"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  11
    		}
    		Line {
    		  SrcBlock		  "velFlt_noExtraFilt"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  12
    		}
    		Line {
    		  SrcBlock		  "coulomb_width"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  13
    		}
    		Line {
    		  SrcBlock		  "percentageTrqFfw"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  14
    		}
    		Line {
    		  SrcBlock		  "KiHighThres"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  15
    		}
    		Line {
    		  SrcBlock		  "slidingThresWidth"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  16
    		}
    		Line {
    		  SrcBlock		  "maxRate"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  17
    		}
    		Line {
    		  SrcBlock		  "resetPosRef"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  18
    		}
    		Line {
    		  SrcBlock		  "jointLimitStartMotorRad"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  19
    		}
    		Line {
    		  SrcBlock		  "extTorque"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  20
    		}
    		Line {
    		  SrcBlock		  "sensorOn"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  21
    		}
    		Line {
    		  SrcBlock		  "fricHelp"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  22
    		}
    		Line {
    		  SrcBlock		  "qtorque"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  23
    		}
    		Line {
    		  Name			  "Kp"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  2
    		  DstBlock		  "Kp"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "Kv"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  3
    		  DstBlock		  "Kv"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "Ki"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  4
    		  DstBlock		  "Ki"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "trqFfw"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  5
    		  DstBlock		  "trqFfw"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "posRef"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  6
    		  DstBlock		  "posRef"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "rstill"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  7
    		  DstBlock		  "rstill"
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " Demux "
    		  SrcPort		  1
    		  DstBlock		  " Terminator "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " SFunction "
    		  SrcPort		  1
    		  DstBlock		  " Demux "
    		  DstPort		  1
    		}
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      "Terminator11"
    	      SID		      "714:940"
    	      Position		      [1190, 515, 1210, 535]
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "Kp"
    	      SID		      "714:23"
    	      Position		      [1305, 78, 1335, 92]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "Kv"
    	      SID		      "714:27"
    	      Position		      [1290, 168, 1320, 182]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "Ki"
    	      SID		      "714:31"
    	      Position		      [1300, 258, 1330, 272]
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "trqFfw"
    	      SID		      "714:35"
    	      Position		      [1300, 348, 1330, 362]
    	      Port		      "4"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "posRef"
    	      SID		      "714:39"
    	      Position		      [1300, 438, 1330, 452]
    	      Port		      "5"
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "fricHelp"
    	      SrcPort		      1
    	      Points		      [105, 0; 0, -170]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      22
    	    }
    	    Line {
    	      SrcBlock		      "extTorque"
    	      SrcPort		      1
    	      Points		      [100, 0; 0, -155]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      20
    	    }
    	    Line {
    	      SrcBlock		      "sensorOn"
    	      SrcPort		      1
    	      Points		      [60, 0; 0, -95]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      21
    	    }
    	    Line {
    	      SrcBlock		      "jointLimitsStartMotorRad"
    	      SrcPort		      1
    	      Points		      [60, 0; 0, -85]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      19
    	    }
    	    Line {
    	      SrcBlock		      "resetPosRef"
    	      SrcPort		      1
    	      Points		      [120, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      18
    	    }
    	    Line {
    	      SrcBlock		      "highKiActive"
    	      SrcPort		      1
    	      Points		      [205, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      10
    	    }
    	    Line {
    	      SrcBlock		      "velFlt1"
    	      SrcPort		      1
    	      Points		      [330, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      12
    	    }
    	    Line {
    	      SrcBlock		      "velFlt_filtered"
    	      SrcPort		      1
    	      Points		      [325, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      9
    	    }
    	    Line {
    	      SrcBlock		      "maxRate"
    	      SrcPort		      1
    	      Points		      [255, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      17
    	    }
    	    Line {
    	      SrcBlock		      "MATLAB Function"
    	      SrcPort		      5
    	      DstBlock		      "posRef"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "slidingThresWidth"
    	      SrcPort		      1
    	      Points		      [60, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      16
    	    }
    	    Line {
    	      SrcBlock		      "KiHighThres"
    	      SrcPort		      1
    	      Points		      [120, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      15
    	    }
    	    Line {
    	      SrcBlock		      "percentageTrqFfw"
    	      SrcPort		      1
    	      Points		      [185, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      14
    	    }
    	    Line {
    	      SrcBlock		      "MATLAB Function"
    	      SrcPort		      4
    	      Points		      [0, -5]
    	      DstBlock		      "trqFfw"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "coulomb_width"
    	      SrcPort		      1
    	      Points		      [255, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      13
    	    }
    	    Line {
    	      SrcBlock		      "keepStill"
    	      SrcPort		      1
    	      Points		      [145, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      11
    	    }
    	    Line {
    	      SrcBlock		      "MATLAB Function"
    	      SrcPort		      3
    	      Points		      [0, -10]
    	      DstBlock		      "Ki"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "f_switch"
    	      SrcPort		      1
    	      Points		      [135, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      8
    	    }
    	    Line {
    	      SrcBlock		      "MATLAB Function"
    	      SrcPort		      2
    	      Points		      [0, -15]
    	      DstBlock		      "Kv"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "posRaw"
    	      SrcPort		      1
    	      Points		      [190, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      7
    	    }
    	    Line {
    	      SrcBlock		      "posRef_in"
    	      SrcPort		      1
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      6
    	    }
    	    Line {
    	      SrcBlock		      "trqRaw"
    	      SrcPort		      1
    	      Points		      [225, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      5
    	    }
    	    Line {
    	      SrcBlock		      "MATLAB Function"
    	      SrcPort		      1
    	      Points		      [0, -20]
    	      DstBlock		      "Kp"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "gravTrqMotor"
    	      SrcPort		      1
    	      Points		      [0, 15; 170, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      4
    	    }
    	    Line {
    	      SrcBlock		      "Ki_in"
    	      SrcPort		      1
    	      Points		      [100, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      3
    	    }
    	    Line {
    	      SrcBlock		      "Kv_in"
    	      SrcPort		      1
    	      Points		      [155, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      2
    	    }
    	    Line {
    	      SrcBlock		      "Kp_in"
    	      SrcPort		      1
    	      Points		      [265, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "rstill[i]"
    	      Labels		      [0, 0]
    	      SrcBlock		      "MATLAB Function"
    	      SrcPort		      6
    	      DstBlock		      "Terminator11"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "qtorgue"
    	      SrcPort		      1
    	      Points		      [130, 0]
    	      DstBlock		      "MATLAB Function"
    	      DstPort		      23
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  Reference
    	  Name			  "leadthroughLockOutput"
    	  SID			  "727"
    	  Ports			  [3, 2]
    	  Position		  [1535, 745, 1665, 815]
    	  ForegroundColor	  "blue"
    	  LibraryVersion	  "1.5"
    	  SourceBlock		  "rapid_extctrl_handover_lib_leadthrough/leadthroughLockOutput"
    	  SourceType		  "Stateflow"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "coulombWidthArmRightAbb"
    	  SID			  "1362"
    	  Position		  [670, 298, 700, 312]
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.joint[i].parKp"
    	  SID			  "679"
    	  Position		  [1200, 98, 1230, 112]
    	  Port			  "2"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.joint[i].parKv"
    	  SID			  "680"
    	  Position		  [1200, 258, 1230, 272]
    	  Port			  "3"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.joint[i].parKi"
    	  SID			  "681"
    	  Position		  [1200, 418, 1230, 432]
    	  Port			  "4"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.joint[i].velRef"
    	  SID			  "683"
    	  Position		  [1755, 793, 1785, 807]
    	  Port			  "5"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.joint[i].trqFfw"
    	  SID			  "684"
    	  Position		  [1320, 588, 1350, 602]
    	  Port			  "6"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.instruction"
    	  SID			  "685"
    	  Position		  [235, 778, 265, 792]
    	  Port			  "7"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.value1"
    	  SID			  "686"
    	  Position		  [290, 838, 320, 852]
    	  Port			  "8"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.value2"
    	  SID			  "687"
    	  Position		  [290, 873, 320, 887]
    	  Port			  "9"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.value3"
    	  SID			  "688"
    	  Position		  [290, 908, 320, 922]
    	  Port			  "10"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.value4"
    	  SID			  "689"
    	  Position		  [290, 943, 320, 957]
    	  Port			  "11"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.value5"
    	  SID			  "690"
    	  Position		  [290, 978, 320, 992]
    	  Port			  "12"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.value6"
    	  SID			  "691"
    	  Position		  [290, 1013, 320, 1027]
    	  Port			  "13"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "1"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.string1[i]"
    	  SID			  "692"
    	  Position		  [290, 1048, 320, 1062]
    	  Port			  "14"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "20"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.mocgendata.string2[i]"
    	  SID			  "693"
    	  Position		  [290, 1083, 320, 1097]
    	  Port			  "15"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "20"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "ext2irb.joint[i].posRef"
    	  SID			  "1252"
    	  Position		  [1755, 758, 1785, 772]
    	  Port			  "16"
    	  IconDisplay		  "Port number"
    	  PortDimensions	  "DOF"
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].parTrqMin"
    	  SrcPort		  1
    	  DstBlock		  "Terminator"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].parTrqMax"
    	  SrcPort		  1
    	  DstBlock		  "Terminator1"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].trqFfwGrav"
    	  SrcPort		  1
    	  DstBlock		  "Terminator2"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].posFlt"
    	  SrcPort		  1
    	  DstBlock		  "Terminator5"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].velOut"
    	  SrcPort		  1
    	  DstBlock		  "aTerminator8"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].trqRefFlt"
    	  SrcPort		  1
    	  DstBlock		  "Terminator10"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.instruction"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.instruction"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.string1[i]"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.string1[i]"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.string2[i]"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.string2[i]"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.value2"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.value2"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.value3"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.value3"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.value4"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.value4"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.value5"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.value5"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.value6"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.value6"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.mocgendata.value1"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.mocgendata.value1"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].posRawFb"
    	  SrcPort		  1
    	  Points		  [50, 0]
    	  Branch {
    	    Points		    [0, -65; 310, -10; 15, 125; 175, 5]
    	    DstBlock		    "leadthroug control"
    	    DstPort		    7
    	  }
    	  Branch {
    	    DstBlock		    "calcGravityTorque"
    	    DstPort		    1
    	  }
    	}
    	Line {
    	  SrcBlock		  "Constant6"
    	  SrcPort		  1
    	  DstBlock		  "calcGravityTorque"
    	  DstPort		  4
    	}
    	Line {
    	  SrcBlock		  "Constant9"
    	  SrcPort		  1
    	  DstBlock		  "calcGravityTorque"
    	  DstPort		  3
    	}
    	Line {
    	  SrcBlock		  "Constant5"
    	  SrcPort		  1
    	  DstBlock		  "calcGravityTorque"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].posRef"
    	  SrcPort		  1
    	  Points		  [40, -15]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  6
    	}
    	Line {
    	  SrcBlock		  "calcGravityTorque"
    	  SrcPort		  2
    	  Points		  [105, 0; 0, 310]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  12
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].trqFfw"
    	  SrcPort		  1
    	  DstBlock		  "Terminator4"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Constant2"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  17
    	}
    	Line {
    	  SrcBlock		  "Constant1"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  16
    	}
    	Line {
    	  SrcBlock		  "Constant4"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  15
    	}
    	Line {
    	  SrcBlock		  "Constant7"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  14
    	}
    	Line {
    	  SrcBlock		  "Constant8"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  13
    	}
    	Line {
    	  SrcBlock		  "Discrete Filter3"
    	  SrcPort		  1
    	  Points		  [15, -30]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  9
    	}
    	Line {
    	  SrcBlock		  "leadthroug control"
    	  SrcPort		  3
    	  Points		  [0, -15]
    	  DstBlock		  "ext2irb.joint[i].parKi"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "leadthroug control"
    	  SrcPort		  2
    	  Points		  [0, -15]
    	  DstBlock		  "ext2irb.joint[i].parKv"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "leadthroug control"
    	  SrcPort		  1
    	  Points		  [0, -15]
    	  DstBlock		  "ext2irb.joint[i].parKp"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].parKi"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  3
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].parKv"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].parKp"
    	  SrcPort		  1
    	  Points		  [50, 0]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Constant3"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  11
    	}
    	Line {
    	  SrcBlock		  "Gain1"
    	  SrcPort		  1
    	  DstBlock		  "Terminator7"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function3"
    	  SrcPort		  4
    	  Points		  [0, 15]
    	  DstBlock		  "Gain"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Gain"
    	  SrcPort		  1
    	  DstBlock		  "Gain1"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Product"
    	  SrcPort		  1
    	  DstBlock		  "Goto"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "From"
    	  SrcPort		  1
    	  DstBlock		  "leadthroug control"
    	  DstPort		  8
    	}
    	Line {
    	  SrcBlock		  "leadthroug control"
    	  SrcPort		  5
    	  DstBlock		  "leadthroughLockOutput"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function3"
    	  SrcPort		  2
    	  Points		  [0, 5; 35, 0]
    	  Branch {
    	    Points		    [0, -155]
    	    DstBlock		    "leadthroug control"
    	    DstPort		    18
    	  }
    	  Branch {
    	    Points		    [700, 0]
    	    DstBlock		    "leadthroughLockOutput"
    	    DstPort		    3
    	  }
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function3"
    	  SrcPort		  3
    	  Points		  [0, 10]
    	  DstBlock		  "Terminator8"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Constant11"
    	  SrcPort		  1
    	  Points		  [135, 0; 0, -125]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  19
    	}
    	Line {
    	  SrcBlock		  "irb2ext.manualMode"
    	  SrcPort		  1
    	  DstBlock		  "Terminator11"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function3"
    	  SrcPort		  1
    	  DstBlock		  "Product"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "f_switch"
    	  SrcPort		  1
    	  Points		  [15, 0; 0, 35]
    	  DstBlock		  "Product"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.controlActive"
    	  SrcPort		  1
    	  DstBlock		  "MATLAB Function3"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "irb2ext.obtaining"
    	  SrcPort		  1
    	  DstBlock		  "MATLAB Function3"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "Constant10"
    	  SrcPort		  1
    	  DstBlock		  "MATLAB Function3"
    	  DstPort		  3
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].velRef"
    	  SrcPort		  1
    	  DstBlock		  "leadthroughLockOutput"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "leadthroughLockOutput"
    	  SrcPort		  1
    	  DstBlock		  "ext2irb.joint[i].posRef"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "leadthroughLockOutput"
    	  SrcPort		  2
    	  DstBlock		  "ext2irb.joint[i].velRef"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Constant"
    	  SrcPort		  1
    	  Points		  [170, 0; 0, -95]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  22
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].velFlt"
    	  SrcPort		  1
    	  Points		  [0, -5; 10, 0]
    	  Branch {
    	    Points		    [-10, 0]
    	    DstBlock		    "Discrete Filter3"
    	    DstPort		    1
    	  }
    	  Branch {
    	    Points		    [0, -10]
    	    DstBlock		    "leadthroug control"
    	    DstPort		    10
    	  }
    	}
    	Line {
    	  SrcBlock		  "irb2ext.joint[i].trqRaw"
    	  SrcPort		  1
    	  Points		  [35, 0]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  5
    	}
    	Line {
    	  SrcBlock		  "leadthroug control"
    	  SrcPort		  4
    	  DstBlock		  "ext2irb.joint[i].trqFfw"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "calcGravityTorque"
    	  SrcPort		  1
    	  Points		  [145, 0; 0, -10; 120, 0]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  4
    	}
    	Line {
    	  SrcBlock		  "calcGravityTorque"
    	  SrcPort		  3
    	  DstBlock		  "coulombWidthArmRightAbb"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "torque_motor_1"
    	  SrcPort		  1
    	  Points		  [430, 0]
    	  DstBlock		  "leadthroug control"
    	  DstPort		  23
    	}
          }
        }
        Block {
          BlockType		      SubSystem
          Name		      "botharms_get_torque"
          SID		      "1345"
          Ports		      [10, 2]
          Position		      [800, 22, 1010, 218]
          AncestorBlock	      "frida_library/FRIDA arm acceleration"
          LibraryVersion	      "*1.21"
          MinAlgLoopOccurrences   off
          PropExecContextOutsideSubsystem off
          RTWSystemCode	      "Auto"
          FunctionWithSeparateData off
          Opaque		      off
          RequestExecContextInheritance off
          MaskHideContents	      off
          Port {
    	PortNumber		1
    	Name			"torque1[i]"
    	TestPoint		on
    	RTWStorageClass		"Auto"
    	DataLoggingNameMode	"SignalName"
          }
          Port {
    	PortNumber		2
    	Name			"torque2[i]"
    	TestPoint		on
    	RTWStorageClass		"Auto"
    	DataLoggingNameMode	"SignalName"
          }
          System {
    	Name			"botharms_get_torque"
    	Location		[0, 18, 1920, 1126]
    	Open			on
    	ModelBrowserVisibility	off
    	ModelBrowserWidth	200
    	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	"497"
    	SIDPrevWatermark	"325"
    	Block {
    	  BlockType		  Inport
    	  Name			  "q_motor_1"
    	  SID			  "1345:295"
    	  Position		  [40, 83, 70, 97]
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "qd_motor_1"
    	  SID			  "1345:299"
    	  Position		  [30, 318, 60, 332]
    	  Port			  "2"
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "q_motor_2"
    	  SID			  "1345:409"
    	  Position		  [30, 488, 60, 502]
    	  Port			  "3"
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "qd_motor_2"
    	  SID			  "1345:410"
    	  Position		  [15, 363, 45, 377]
    	  Port			  "4"
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "coulombWidthArmRightAbb"
    	  SID			  "1345:433"
    	  Position		  [1075, 368, 1105, 382]
    	  Port			  "5"
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "coulombWidthArmLeftAbb"
    	  SID			  "1345:434"
    	  Position		  [1235, 673, 1265, 687]
    	  Port			  "6"
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "velFlt1"
    	  SID			  "1345:450"
    	  Position		  [1220, 433, 1250, 447]
    	  Port			  "7"
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "velFlt2"
    	  SID			  "1345:451"
    	  Position		  [1220, 503, 1250, 517]
    	  Port			  "8"
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "wrench1"
    	  SID			  "1345:486"
    	  Position		  [240, 883, 270, 897]
    	  Port			  "9"
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Inport
    	  Name			  "wrench2"
    	  SID			  "1345:487"
    	  Position		  [225, 993, 255, 1007]
    	  Port			  "10"
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant1"
    	  SID			  "1345:412"
    	  Position		  [395, 529, 455, 561]
    	  ZOrder		  -5
    	  Value			  "zeros(7,1)"
    	}
    	Block {
    	  BlockType		  Constant
    	  Name			  "Constant2"
    	  SID			  "1345:377"
    	  Position		  [405, 124, 465, 156]
    	  ZOrder		  -5
    	  Value			  "zeros(7,1)"
    	}
    	Block {
    	  BlockType		  Demux
    	  Name			  "Demux"
    	  SID			  "1345:432"
    	  Ports			  [1, 2]
    	  Position		  [175, 321, 180, 359]
    	  ZOrder		  -7
    	  ShowName		  off
    	  Outputs		  "2"
    	  DisplayOption		  "bar"
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "FRIDA arm acceleration NOT SI"
    	  SID			  "1345:348"
    	  Ports			  [2, 3]
    	  Position		  [335, 200, 440, 290]
    	  AncestorBlock		  "frida_library/FRIDA arm acceleration"
    	  LibraryVersion	  "*1.21"
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  System {
    	    Name		    "FRIDA arm acceleration NOT SI"
    	    Location		    [338, 225, 1510, 661]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "328"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "q_arm1"
    	      SID		      "1345:348:295"
    	      Position		      [120, 83, 150, 97]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "qd_arm1"
    	      SID		      "1345:348:299"
    	      Position		      [120, 168, 150, 182]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Reference
    	      Name		      "Base transformation right arm (rob1)"
    	      SID		      "1345:348:328"
    	      Ports		      [0, 1]
    	      Position		      [340, 16, 495, 54]
    	      ForegroundColor	      "blue"
    	      LibraryVersion	      "1.21"
    	      SourceBlock	      "frida_library/Base transformation right arm (rob1)"
    	      SourceType	      "Stateflow"
    	    }
    	    Block {
    	      BlockType		      Constant
    	      Name		      "T44_flange->tool"
    	      SID		      "1345:348:327"
    	      Position		      [430, 67, 505, 93]
    	      BackgroundColor	      "cyan"
    	      Value		      "[1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1]'"
    	      VectorParams1D	      off
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      "Terminator"
    	      SID		      "1345:348:290"
    	      Position		      [410, 150, 430, 170]
    	      ShowName		      off
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      "Terminator1"
    	      SID		      "1345:348:303"
    	      Position		      [740, 140, 760, 160]
    	      ShowName		      off
    	    }
    	    Block {
    	      BlockType		      SubSystem
    	      Name		      "base_tool_transform"
    	      SID		      "1345:348:289"
    	      Ports		      [6, 4]
    	      Position		      [555, 30, 710, 220]
    	      LibraryVersion	      "1.32"
    	      ErrorFcn		      "Stateflow.Translate.translate"
    	      PermitHierarchicalResolution "ExplicitOnly"
    	      TreatAsAtomicUnit	      on
    	      MinAlgLoopOccurrences   off
    	      PropExecContextOutsideSubsystem off
    	      RTWSystemCode	      "Auto"
    	      FunctionWithSeparateData off
    	      Opaque		      off
    	      RequestExecContextInheritance off
    	      MaskHideContents	      off
    	      SFBlockType	      "MATLAB Function"
    	      MaskType		      "Stateflow"
    	      MaskDescription	      "Embedded MATLAB block"
    	      MaskSelfModifiable      on
    	      MaskDisplay	      "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
    	      "rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'base_tool_transform']"
    	      ");"
    	      MaskIconFrame	      on
    	      MaskIconOpaque	      off
    	      MaskIconRotate	      "none"
    	      MaskPortRotate	      "default"
    	      MaskIconUnits	      "normalized"
    	      System {
    		Name			"base_tool_transform"
    		Location		[257, 457, 812, 717]
    		Open			off
    		ModelBrowserVisibility	off
    		ModelBrowserWidth	200
    		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	"33"
    		Block {
    		  BlockType		  Inport
    		  Name			  "T44_world2base"
    		  SID			  "1345:348:289::1"
    		  Position		  [20, 101, 40, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "T44_flange2tool"
    		  SID			  "1345:348:289::18"
    		  Position		  [20, 136, 40, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "T44_in"
    		  SID			  "1345:348:289::19"
    		  Position		  [20, 171, 40, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "J0_in"
    		  SID			  "1345:348:289::20"
    		  Position		  [20, 206, 40, 224]
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "qd"
    		  SID			  "1345:348:289::21"
    		  Position		  [20, 246, 40, 264]
    		  Port			  "5"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "Jdot0_in"
    		  SID			  "1345:348:289::22"
    		  Position		  [20, 281, 40, 299]
    		  Port			  "6"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Demux
    		  Name			  " Demux "
    		  SID			  "1345:348:289::31"
    		  Ports			  [1, 1]
    		  Position		  [270, 245, 320, 285]
    		  ZOrder		  6
    		  Outputs		  "1"
    		}
    		Block {
    		  BlockType		  S-Function
    		  Name			  " SFunction "
    		  SID			  "1345:348:289::30"
    		  Tag			  "Stateflow S-Function scaled_teleoperation 8"
    		  Ports			  [6, 5]
    		  Position		  [180, 102, 230, 243]
    		  ZOrder		  5
    		  FunctionName		  "sf_sfun"
    		  PortCounts		  "[6 5]"
    		  EnableBusSupport	  on
    		  Port {
    		    PortNumber		    2
    		    Name		    "T44_out"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    3
    		    Name		    "J0_out"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    4
    		    Name		    "Jn_out"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    5
    		    Name		    "Jdot0_out"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		}
    		Block {
    		  BlockType		  Terminator
    		  Name			  " Terminator "
    		  SID			  "1345:348:289::33"
    		  Position		  [460, 256, 480, 274]
    		  ZOrder		  8
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "T44_out"
    		  SID			  "1345:348:289::5"
    		  Position		  [460, 101, 480, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "J0_out"
    		  SID			  "1345:348:289::23"
    		  Position		  [460, 136, 480, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Jn_out"
    		  SID			  "1345:348:289::24"
    		  Position		  [460, 171, 480, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Jdot0_out"
    		  SID			  "1345:348:289::25"
    		  Position		  [460, 206, 480, 224]
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Line {
    		  SrcBlock		  "T44_world2base"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  "T44_flange2tool"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  2
    		}
    		Line {
    		  SrcBlock		  "T44_in"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  3
    		}
    		Line {
    		  SrcBlock		  "J0_in"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  4
    		}
    		Line {
    		  SrcBlock		  "qd"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  5
    		}
    		Line {
    		  SrcBlock		  "Jdot0_in"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  6
    		}
    		Line {
    		  Name			  "T44_out"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  2
    		  DstBlock		  "T44_out"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "J0_out"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  3
    		  DstBlock		  "J0_out"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "Jn_out"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  4
    		  DstBlock		  "Jn_out"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "Jdot0_out"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  5
    		  DstBlock		  "Jdot0_out"
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " Demux "
    		  SrcPort		  1
    		  DstBlock		  " Terminator "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " SFunction "
    		  SrcPort		  1
    		  DstBlock		  " Demux "
    		  DstPort		  1
    		}
    	      }
    	    }
    	    Block {
    	      BlockType		      SubSystem
    	      Name		      "frida_kin"
    	      SID		      "1345:348:288"
    	      Ports		      [2, 4]
    	      Position		      [275, 56, 385, 224]
    	      LibraryVersion	      "1.32"
    	      ErrorFcn		      "Stateflow.Translate.translate"
    	      PermitHierarchicalResolution "ExplicitOnly"
    	      TreatAsAtomicUnit	      on
    	      MinAlgLoopOccurrences   off
    	      PropExecContextOutsideSubsystem off
    	      RTWSystemCode	      "Auto"
    	      FunctionWithSeparateData off
    	      Opaque		      off
    	      RequestExecContextInheritance off
    	      MaskHideContents	      off
    	      SFBlockType	      "MATLAB Function"
    	      MaskType		      "Stateflow"
    	      MaskDescription	      "Embedded MATLAB block"
    	      MaskSelfModifiable      on
    	      MaskDisplay	      "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
    	      "rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'frida_kinematics']);"
    	      MaskIconFrame	      on
    	      MaskIconOpaque	      off
    	      MaskIconRotate	      "none"
    	      MaskPortRotate	      "default"
    	      MaskIconUnits	      "normalized"
    	      System {
    		Name			"frida_kin"
    		Location		[257, 457, 812, 717]
    		Open			off
    		ModelBrowserVisibility	off
    		ModelBrowserWidth	200
    		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	"29"
    		Block {
    		  BlockType		  Inport
    		  Name			  "q"
    		  SID			  "1345:348:288::1"
    		  Position		  [20, 101, 40, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "qd"
    		  SID			  "1345:348:288::18"
    		  Position		  [20, 136, 40, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Demux
    		  Name			  " Demux "
    		  SID			  "1345:348:288::27"
    		  Ports			  [1, 1]
    		  Position		  [270, 245, 320, 285]
    		  ZOrder		  6
    		  Outputs		  "1"
    		}
    		Block {
    		  BlockType		  S-Function
    		  Name			  " SFunction "
    		  SID			  "1345:348:288::26"
    		  Tag			  "Stateflow S-Function scaled_teleoperation 1"
    		  Ports			  [2, 5]
    		  Position		  [180, 100, 230, 220]
    		  ZOrder		  5
    		  FunctionName		  "sf_sfun"
    		  PortCounts		  "[2 5]"
    		  EnableBusSupport	  on
    		  Port {
    		    PortNumber		    2
    		    Name		    "T44_fkine"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    3
    		    Name		    "J0"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    4
    		    Name		    "Jn"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    5
    		    Name		    "Jdot0"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		}
    		Block {
    		  BlockType		  Terminator
    		  Name			  " Terminator "
    		  SID			  "1345:348:288::29"
    		  Position		  [460, 256, 480, 274]
    		  ZOrder		  8
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "T44_fkine"
    		  SID			  "1345:348:288::5"
    		  Position		  [460, 101, 480, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "J0"
    		  SID			  "1345:348:288::19"
    		  Position		  [460, 136, 480, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Jn"
    		  SID			  "1345:348:288::20"
    		  Position		  [460, 171, 480, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Jdot0"
    		  SID			  "1345:348:288::21"
    		  Position		  [460, 206, 480, 224]
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Line {
    		  SrcBlock		  "q"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  "qd"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  2
    		}
    		Line {
    		  Name			  "T44_fkine"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  2
    		  DstBlock		  "T44_fkine"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "J0"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  3
    		  DstBlock		  "J0"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "Jn"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  4
    		  DstBlock		  "Jn"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "Jdot0"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  5
    		  DstBlock		  "Jdot0"
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " Demux "
    		  SrcPort		  1
    		  DstBlock		  " Terminator "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " SFunction "
    		  SrcPort		  1
    		  DstBlock		  " Demux "
    		  DstPort		  1
    		}
    	      }
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "T44_FK"
    	      SID		      "1345:348:293"
    	      Position		      [735, 53, 765, 67]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "J0"
    	      SID		      "1345:348:296"
    	      Position		      [735, 98, 765, 112]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "Jdot0"
    	      SID		      "1345:348:300"
    	      Position		      [735, 188, 765, 202]
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "base_tool_transform"
    	      SrcPort		      4
    	      DstBlock		      "Jdot0"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "qd_arm1"
    	      SrcPort		      1
    	      Points		      [75, 0]
    	      Branch {
    		Points			[30, 0]
    		DstBlock		"frida_kin"
    		DstPort			2
    	      }
    	      Branch {
    		Points			[0, 80; 205, 0; 0, -85]
    		DstBlock		"base_tool_transform"
    		DstPort			5
    	      }
    	    }
    	    Line {
    	      SrcBlock		      "base_tool_transform"
    	      SrcPort		      3
    	      DstBlock		      "Terminator1"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "base_tool_transform"
    	      SrcPort		      2
    	      DstBlock		      "J0"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "q_arm1"
    	      SrcPort		      1
    	      Points		      [105, 0]
    	      DstBlock		      "frida_kin"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "base_tool_transform"
    	      SrcPort		      1
    	      DstBlock		      "T44_FK"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "Base transformation right arm (rob1)"
    	      SrcPort		      1
    	      Points		      [0, 15]
    	      DstBlock		      "base_tool_transform"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "frida_kin"
    	      SrcPort		      4
    	      DstBlock		      "base_tool_transform"
    	      DstPort		      6
    	    }
    	    Line {
    	      SrcBlock		      "frida_kin"
    	      SrcPort		      3
    	      DstBlock		      "Terminator"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "frida_kin"
    	      SrcPort		      2
    	      Points		      [55, 0; 0, 20]
    	      DstBlock		      "base_tool_transform"
    	      DstPort		      4
    	    }
    	    Line {
    	      SrcBlock		      "frida_kin"
    	      SrcPort		      1
    	      Points		      [25, 0; 0, 30]
    	      DstBlock		      "base_tool_transform"
    	      DstPort		      3
    	    }
    	    Line {
    	      SrcBlock		      "T44_flange->tool"
    	      SrcPort		      1
    	      DstBlock		      "base_tool_transform"
    	      DstPort		      2
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "FRIDA arm acceleration NOT SI1"
    	  SID			  "1345:413"
    	  Ports			  [2, 3]
    	  Position		  [350, 605, 455, 695]
    	  AncestorBlock		  "frida_library/FRIDA arm acceleration"
    	  LibraryVersion	  "*1.21"
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  System {
    	    Name		    "FRIDA arm acceleration NOT SI1"
    	    Location		    [134, 23, 1077, 362]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "329"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "q_arm1"
    	      SID		      "1345:413:295"
    	      Position		      [120, 83, 150, 97]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "qd_arm1"
    	      SID		      "1345:413:299"
    	      Position		      [120, 168, 150, 182]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Reference
    	      Name		      "Base transformation left arm (rob2)"
    	      SID		      "1345:413:329"
    	      Ports		      [0, 1]
    	      Position		      [305, 15, 455, 55]
    	      ForegroundColor	      "blue"
    	      LibraryVersion	      "1.21"
    	      SourceBlock	      "frida_library/Base transformation left arm (rob2)"
    	      SourceType	      "Stateflow"
    	    }
    	    Block {
    	      BlockType		      Constant
    	      Name		      "T44_flange->tool"
    	      SID		      "1345:413:327"
    	      Position		      [430, 67, 505, 93]
    	      BackgroundColor	      "cyan"
    	      Value		      "[1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1]'"
    	      VectorParams1D	      off
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      "Terminator"
    	      SID		      "1345:413:290"
    	      Position		      [410, 150, 430, 170]
    	      ShowName		      off
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      "Terminator1"
    	      SID		      "1345:413:303"
    	      Position		      [740, 140, 760, 160]
    	      ShowName		      off
    	    }
    	    Block {
    	      BlockType		      SubSystem
    	      Name		      "base_tool_transform"
    	      SID		      "1345:413:289"
    	      Ports		      [6, 4]
    	      Position		      [555, 30, 710, 220]
    	      LibraryVersion	      "1.32"
    	      ErrorFcn		      "Stateflow.Translate.translate"
    	      PermitHierarchicalResolution "ExplicitOnly"
    	      TreatAsAtomicUnit	      on
    	      MinAlgLoopOccurrences   off
    	      PropExecContextOutsideSubsystem off
    	      RTWSystemCode	      "Auto"
    	      FunctionWithSeparateData off
    	      Opaque		      off
    	      RequestExecContextInheritance off
    	      MaskHideContents	      off
    	      SFBlockType	      "MATLAB Function"
    	      MaskType		      "Stateflow"
    	      MaskDescription	      "Embedded MATLAB block"
    	      MaskSelfModifiable      on
    	      MaskDisplay	      "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
    	      "rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'base_tool_transform']"
    	      ");"
    	      MaskIconFrame	      on
    	      MaskIconOpaque	      off
    	      MaskIconRotate	      "none"
    	      MaskPortRotate	      "default"
    	      MaskIconUnits	      "normalized"
    	      System {
    		Name			"base_tool_transform"
    		Location		[257, 457, 812, 717]
    		Open			off
    		ModelBrowserVisibility	off
    		ModelBrowserWidth	200
    		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	"33"
    		Block {
    		  BlockType		  Inport
    		  Name			  "T44_world2base"
    		  SID			  "1345:413:289::1"
    		  Position		  [20, 101, 40, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "T44_flange2tool"
    		  SID			  "1345:413:289::18"
    		  Position		  [20, 136, 40, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "T44_in"
    		  SID			  "1345:413:289::19"
    		  Position		  [20, 171, 40, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "J0_in"
    		  SID			  "1345:413:289::20"
    		  Position		  [20, 206, 40, 224]
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "qd"
    		  SID			  "1345:413:289::21"
    		  Position		  [20, 246, 40, 264]
    		  Port			  "5"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "Jdot0_in"
    		  SID			  "1345:413:289::22"
    		  Position		  [20, 281, 40, 299]
    		  Port			  "6"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Demux
    		  Name			  " Demux "
    		  SID			  "1345:413:289::31"
    		  Ports			  [1, 1]
    		  Position		  [270, 245, 320, 285]
    		  ZOrder		  6
    		  Outputs		  "1"
    		}
    		Block {
    		  BlockType		  S-Function
    		  Name			  " SFunction "
    		  SID			  "1345:413:289::30"
    		  Tag			  "Stateflow S-Function scaled_teleoperation 29"
    		  Ports			  [6, 5]
    		  Position		  [180, 102, 230, 243]
    		  ZOrder		  5
    		  FunctionName		  "sf_sfun"
    		  PortCounts		  "[6 5]"
    		  EnableBusSupport	  on
    		  Port {
    		    PortNumber		    2
    		    Name		    "T44_out"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    3
    		    Name		    "J0_out"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    4
    		    Name		    "Jn_out"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    5
    		    Name		    "Jdot0_out"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		}
    		Block {
    		  BlockType		  Terminator
    		  Name			  " Terminator "
    		  SID			  "1345:413:289::33"
    		  Position		  [460, 256, 480, 274]
    		  ZOrder		  8
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "T44_out"
    		  SID			  "1345:413:289::5"
    		  Position		  [460, 101, 480, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "J0_out"
    		  SID			  "1345:413:289::23"
    		  Position		  [460, 136, 480, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Jn_out"
    		  SID			  "1345:413:289::24"
    		  Position		  [460, 171, 480, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Jdot0_out"
    		  SID			  "1345:413:289::25"
    		  Position		  [460, 206, 480, 224]
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Line {
    		  SrcBlock		  "T44_world2base"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  "T44_flange2tool"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  2
    		}
    		Line {
    		  SrcBlock		  "T44_in"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  3
    		}
    		Line {
    		  SrcBlock		  "J0_in"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  4
    		}
    		Line {
    		  SrcBlock		  "qd"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  5
    		}
    		Line {
    		  SrcBlock		  "Jdot0_in"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  6
    		}
    		Line {
    		  Name			  "T44_out"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  2
    		  DstBlock		  "T44_out"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "J0_out"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  3
    		  DstBlock		  "J0_out"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "Jn_out"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  4
    		  DstBlock		  "Jn_out"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "Jdot0_out"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  5
    		  DstBlock		  "Jdot0_out"
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " Demux "
    		  SrcPort		  1
    		  DstBlock		  " Terminator "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " SFunction "
    		  SrcPort		  1
    		  DstBlock		  " Demux "
    		  DstPort		  1
    		}
    	      }
    	    }
    	    Block {
    	      BlockType		      SubSystem
    	      Name		      "frida_kin"
    	      SID		      "1345:413:288"
    	      Ports		      [2, 4]
    	      Position		      [275, 56, 385, 224]
    	      LibraryVersion	      "1.32"
    	      ErrorFcn		      "Stateflow.Translate.translate"
    	      PermitHierarchicalResolution "ExplicitOnly"
    	      TreatAsAtomicUnit	      on
    	      MinAlgLoopOccurrences   off
    	      PropExecContextOutsideSubsystem off
    	      RTWSystemCode	      "Auto"
    	      FunctionWithSeparateData off
    	      Opaque		      off
    	      RequestExecContextInheritance off
    	      MaskHideContents	      off
    	      SFBlockType	      "MATLAB Function"
    	      MaskType		      "Stateflow"
    	      MaskDescription	      "Embedded MATLAB block"
    	      MaskSelfModifiable      on
    	      MaskDisplay	      "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
    	      "rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'frida_kinematics']);"
    	      MaskIconFrame	      on
    	      MaskIconOpaque	      off
    	      MaskIconRotate	      "none"
    	      MaskPortRotate	      "default"
    	      MaskIconUnits	      "normalized"
    	      System {
    		Name			"frida_kin"
    		Location		[257, 457, 812, 717]
    		Open			off
    		ModelBrowserVisibility	off
    		ModelBrowserWidth	200
    		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	"29"
    		Block {
    		  BlockType		  Inport
    		  Name			  "q"
    		  SID			  "1345:413:288::1"
    		  Position		  [20, 101, 40, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "qd"
    		  SID			  "1345:413:288::18"
    		  Position		  [20, 136, 40, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Demux
    		  Name			  " Demux "
    		  SID			  "1345:413:288::27"
    		  Ports			  [1, 1]
    		  Position		  [270, 245, 320, 285]
    		  ZOrder		  6
    		  Outputs		  "1"
    		}
    		Block {
    		  BlockType		  S-Function
    		  Name			  " SFunction "
    		  SID			  "1345:413:288::26"
    		  Tag			  "Stateflow S-Function scaled_teleoperation 30"
    		  Ports			  [2, 5]
    		  Position		  [180, 100, 230, 220]
    		  ZOrder		  5
    		  FunctionName		  "sf_sfun"
    		  PortCounts		  "[2 5]"
    		  EnableBusSupport	  on
    		  Port {
    		    PortNumber		    2
    		    Name		    "T44_fkine"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    3
    		    Name		    "J0"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    4
    		    Name		    "Jn"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		  Port {
    		    PortNumber		    5
    		    Name		    "Jdot0"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		}
    		Block {
    		  BlockType		  Terminator
    		  Name			  " Terminator "
    		  SID			  "1345:413:288::29"
    		  Position		  [460, 256, 480, 274]
    		  ZOrder		  8
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "T44_fkine"
    		  SID			  "1345:413:288::5"
    		  Position		  [460, 101, 480, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "J0"
    		  SID			  "1345:413:288::19"
    		  Position		  [460, 136, 480, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Jn"
    		  SID			  "1345:413:288::20"
    		  Position		  [460, 171, 480, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "Jdot0"
    		  SID			  "1345:413:288::21"
    		  Position		  [460, 206, 480, 224]
    		  Port			  "4"
    		  IconDisplay		  "Port number"
    		}
    		Line {
    		  SrcBlock		  "q"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  "qd"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  2
    		}
    		Line {
    		  Name			  "T44_fkine"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  2
    		  DstBlock		  "T44_fkine"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "J0"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  3
    		  DstBlock		  "J0"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "Jn"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  4
    		  DstBlock		  "Jn"
    		  DstPort		  1
    		}
    		Line {
    		  Name			  "Jdot0"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  5
    		  DstBlock		  "Jdot0"
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " Demux "
    		  SrcPort		  1
    		  DstBlock		  " Terminator "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " SFunction "
    		  SrcPort		  1
    		  DstBlock		  " Demux "
    		  DstPort		  1
    		}
    	      }
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "T44_FK"
    	      SID		      "1345:413:293"
    	      Position		      [735, 53, 765, 67]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "J0"
    	      SID		      "1345:413:296"
    	      Position		      [735, 98, 765, 112]
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "Jdot0"
    	      SID		      "1345:413:300"
    	      Position		      [735, 188, 765, 202]
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "base_tool_transform"
    	      SrcPort		      4
    	      DstBlock		      "Jdot0"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "qd_arm1"
    	      SrcPort		      1
    	      Points		      [75, 0]
    	      Branch {
    		Points			[30, 0]
    		DstBlock		"frida_kin"
    		DstPort			2
    	      }
    	      Branch {
    		Points			[0, 80; 205, 0; 0, -85]
    		DstBlock		"base_tool_transform"
    		DstPort			5
    	      }
    	    }
    	    Line {
    	      SrcBlock		      "base_tool_transform"
    	      SrcPort		      3
    	      DstBlock		      "Terminator1"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "base_tool_transform"
    	      SrcPort		      2
    	      DstBlock		      "J0"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "q_arm1"
    	      SrcPort		      1
    	      Points		      [105, 0]
    	      DstBlock		      "frida_kin"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "base_tool_transform"
    	      SrcPort		      1
    	      DstBlock		      "T44_FK"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "Base transformation left arm (rob2)"
    	      SrcPort		      1
    	      Points		      [25, 0; 0, 15]
    	      DstBlock		      "base_tool_transform"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "frida_kin"
    	      SrcPort		      4
    	      DstBlock		      "base_tool_transform"
    	      DstPort		      6
    	    }
    	    Line {
    	      SrcBlock		      "frida_kin"
    	      SrcPort		      3
    	      DstBlock		      "Terminator"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "frida_kin"
    	      SrcPort		      2
    	      Points		      [55, 0; 0, 20]
    	      DstBlock		      "base_tool_transform"
    	      DstPort		      4
    	    }
    	    Line {
    	      SrcBlock		      "frida_kin"
    	      SrcPort		      1
    	      Points		      [25, 0; 0, 30]
    	      DstBlock		      "base_tool_transform"
    	      DstPort		      3
    	    }
    	    Line {
    	      SrcBlock		      "T44_flange->tool"
    	      SrcPort		      1
    	      DstBlock		      "base_tool_transform"
    	      DstPort		      2
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  Reference
    	  Name			  "FRIDA motor2arm"
    	  SID			  "1345:335"
    	  Ports			  [1, 1]
    	  Position		  [210, 73, 280, 107]
    	  LibraryVersion	  "1.21"
    	  SourceBlock		  "frida_library/FRIDA motor2arm"
    	  SourceType		  "SubSystem"
    	}
    	Block {
    	  BlockType		  Reference
    	  Name			  "FRIDA motor2arm1"
    	  SID			  "1345:336"
    	  Ports			  [1, 1]
    	  Position		  [210, 160, 280, 190]
    	  LibraryVersion	  "1.21"
    	  SourceBlock		  "frida_library/FRIDA motor2arm"
    	  SourceType		  "SubSystem"
    	}
    	Block {
    	  BlockType		  Reference
    	  Name			  "FRIDA motor2arm2"
    	  SID			  "1345:337"
    	  Ports			  [1, 1]
    	  Position		  [1625, 92, 1720, 128]
    	  LibraryVersion	  "1.21"
    	  SourceBlock		  "frida_library/FRIDA motor2arm"
    	  SourceType		  "SubSystem"
    	}
    	Block {
    	  BlockType		  Reference
    	  Name			  "FRIDA motor2arm3"
    	  SID			  "1345:406"
    	  Ports			  [1, 1]
    	  Position		  [1490, 757, 1585, 793]
    	  LibraryVersion	  "1.21"
    	  SourceBlock		  "frida_library/FRIDA motor2arm"
    	  SourceType		  "SubSystem"
    	}
    	Block {
    	  BlockType		  Reference
    	  Name			  "FRIDA motor2arm4"
    	  SID			  "1345:414"
    	  Ports			  [1, 1]
    	  Position		  [200, 478, 270, 512]
    	  LibraryVersion	  "1.21"
    	  SourceBlock		  "frida_library/FRIDA motor2arm"
    	  SourceType		  "SubSystem"
    	}
    	Block {
    	  BlockType		  Reference
    	  Name			  "FRIDA motor2arm5"
    	  SID			  "1345:415"
    	  Ports			  [1, 1]
    	  Position		  [200, 565, 270, 595]
    	  LibraryVersion	  "1.21"
    	  SourceBlock		  "frida_library/FRIDA motor2arm"
    	  SourceType		  "SubSystem"
    	}
    	Block {
    	  BlockType		  Reference
    	  Name			  "FRIDA motor2arm6"
    	  SID			  "1345:452"
    	  Ports			  [1, 1]
    	  Position		  [1270, 428, 1305, 452]
    	  LibraryVersion	  "1.21"
    	  SourceBlock		  "frida_library/FRIDA motor2arm"
    	  SourceType		  "SubSystem"
    	}
    	Block {
    	  BlockType		  Reference
    	  Name			  "FRIDA motor2arm7"
    	  SID			  "1345:453"
    	  Ports			  [1, 1]
    	  Position		  [1270, 499, 1305, 521]
    	  LibraryVersion	  "1.21"
    	  SourceBlock		  "frida_library/FRIDA motor2arm"
    	  SourceType		  "SubSystem"
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain"
    	  SID			  "1345:439"
    	  Position		  [1135, 360, 1165, 390]
    	  ZOrder		  -8
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	  Port {
    	    PortNumber		    1
    	    Name		    "cright[i]"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain1"
    	  SID			  "1345:440"
    	  Position		  [1295, 665, 1325, 695]
    	  ZOrder		  -8
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	  Port {
    	    PortNumber		    1
    	    Name		    "cleft[i]"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain10"
    	  SID			  "1345:496"
    	  Position		  [640, 895, 670, 925]
    	  ZOrder		  -8
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	  Port {
    	    PortNumber		    1
    	    Name		    "w1[i]"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain11"
    	  SID			  "1345:497"
    	  Position		  [640, 960, 670, 990]
    	  ZOrder		  -8
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	  Port {
    	    PortNumber		    1
    	    Name		    "w2[i]"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain2"
    	  SID			  "1345:441"
    	  Position		  [1555, 405, 1585, 435]
    	  ZOrder		  -8
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	  Port {
    	    PortNumber		    1
    	    Name		    "help_right[i]"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain3"
    	  SID			  "1345:442"
    	  Position		  [1605, 575, 1635, 605]
    	  ZOrder		  -8
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	  Port {
    	    PortNumber		    1
    	    Name		    "help_left[i]"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain4"
    	  SID			  "1345:455"
    	  Position		  [1235, 815, 1265, 845]
    	  ZOrder		  -8
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	  Port {
    	    PortNumber		    1
    	    Name		    "diagm[i]"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain5"
    	  SID			  "1345:457"
    	  Position		  [1375, 95, 1405, 125]
    	  ZOrder		  -8
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain6"
    	  SID			  "1345:458"
    	  Position		  [1330, 755, 1360, 785]
    	  ZOrder		  -8
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain7"
    	  SID			  "1345:459"
    	  Position		  [1355, 815, 1385, 845]
    	  ZOrder		  -8
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain8"
    	  SID			  "1345:461"
    	  Position		  [1825, 95, 1850, 125]
    	  ZOrder		  -8
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	}
    	Block {
    	  BlockType		  Gain
    	  Name			  "Gain9"
    	  SID			  "1345:462"
    	  Position		  [1725, 760, 1755, 790]
    	  ZOrder		  -8
    	  ParamDataTypeStr	  "Inherit: Inherit via internal rule"
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "MATLAB Function1"
    	  SID			  "1345:350"
    	  Ports			  [2, 2]
    	  Position		  [740, 818, 790, 922]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  Port {
    	    PortNumber		    1
    	    Name		    "e[i]"
    	    PropagatedSignals	    "e"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	  System {
    	    Name		    "MATLAB Function1"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "27"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "T44_FK1"
    	      SID		      "1345:350::24"
    	      Position		      [20, 101, 40, 119]
    	      ZOrder		      10
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "T44_FK2"
    	      SID		      "1345:350::26"
    	      Position		      [20, 136, 40, 154]
    	      ZOrder		      12
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:350::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 205, 320, 245]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:350::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 11"
    	      Ports		      [2, 3]
    	      Position		      [180, 104, 230, 206]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[2 3]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"e"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		3
    		Name			"scale"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:350::22"
    	      Position		      [460, 216, 480, 234]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "e"
    	      SID		      "1345:350::25"
    	      Position		      [460, 101, 480, 119]
    	      ZOrder		      11
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "scale"
    	      SID		      "1345:350::27"
    	      Position		      [460, 136, 480, 154]
    	      ZOrder		      13
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "T44_FK1"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "T44_FK2"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      2
    	    }
    	    Line {
    	      Name		      "e"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "e"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "scale"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      3
    	      DstBlock		      "scale"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "MATLAB Function10"
    	  SID			  "1345:418"
    	  Ports			  [1, 1]
    	  Position		  [395, 446, 460, 474]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'abb2logical']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "MATLAB Function10"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "22"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "abb"
    	      SID		      "1345:418::1"
    	      Position		      [20, 101, 40, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:418::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 160, 320, 200]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:418::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 31"
    	      Ports		      [1, 2]
    	      Position		      [180, 100, 230, 160]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[1 2]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"logical"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:418::22"
    	      Position		      [460, 171, 480, 189]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "logical"
    	      SID		      "1345:418::5"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "abb"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "logical"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "logical"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "MATLAB Function2"
    	  SID			  "1345:390"
    	  Ports			  [1, 1]
    	  Position		  [405, 56, 470, 84]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'abb2logical']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "MATLAB Function2"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "22"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "abb"
    	      SID		      "1345:390::1"
    	      Position		      [20, 101, 40, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:390::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 160, 320, 200]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:390::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 3"
    	      Ports		      [1, 2]
    	      Position		      [180, 100, 230, 160]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[1 2]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"logical"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:390::22"
    	      Position		      [460, 171, 480, 189]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "logical"
    	      SID		      "1345:390::5"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "abb"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "logical"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "logical"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "MATLAB Function3"
    	  SID			  "1345:470"
    	  Ports			  [1, 1]
    	  Position		  [540, 284, 605, 366]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  Port {
    	    PortNumber		    1
    	    Name		    "pose1[i]"
    	    PropagatedSignals	    "pose"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	  System {
    	    Name		    "MATLAB Function3"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "33"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "T44_FK"
    	      SID		      "1345:470::24"
    	      Position		      [20, 101, 40, 119]
    	      ZOrder		      10
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:470::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 255, 320, 295]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:470::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 17"
    	      Ports		      [1, 2]
    	      Position		      [180, 113, 230, 257]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[1 2]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"pose"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:470::22"
    	      Position		      [460, 266, 480, 284]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "pose"
    	      SID		      "1345:470::25"
    	      Position		      [460, 101, 480, 119]
    	      ZOrder		      11
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "T44_FK"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "pose"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "pose"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "MATLAB Function4"
    	  SID			  "1345:476"
    	  Ports			  [3, 2]
    	  Position		  [1680, 179, 1745, 261]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  Port {
    	    PortNumber		    1
    	    Name		    "vel1[i]"
    	    PropagatedSignals	    "vel1"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	  Port {
    	    PortNumber		    2
    	    Name		    "vel2[i]"
    	    PropagatedSignals	    "vel2"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	  System {
    	    Name		    "MATLAB Function4"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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		      "J1"
    	      SID		      "1345:476::24"
    	      Position		      [20, 101, 40, 119]
    	      ZOrder		      10
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "J2"
    	      SID		      "1345:476::34"
    	      Position		      [20, 136, 40, 154]
    	      ZOrder		      12
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "q_dot"
    	      SID		      "1345:476::35"
    	      Position		      [20, 171, 40, 189]
    	      ZOrder		      13
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:476::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 255, 320, 295]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:476::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 18"
    	      Ports		      [3, 3]
    	      Position		      [180, 113, 230, 257]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[3 3]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"vel1"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		3
    		Name			"vel2"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:476::22"
    	      Position		      [460, 266, 480, 284]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "vel1"
    	      SID		      "1345:476::25"
    	      Position		      [460, 101, 480, 119]
    	      ZOrder		      11
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "vel2"
    	      SID		      "1345:476::36"
    	      Position		      [460, 136, 480, 154]
    	      ZOrder		      14
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "J1"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "J2"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      2
    	    }
    	    Line {
    	      SrcBlock		      "q_dot"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      3
    	    }
    	    Line {
    	      Name		      "vel1"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "vel1"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "vel2"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      3
    	      DstBlock		      "vel2"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "MATLAB Function5"
    	  SID			  "1345:394"
    	  Ports			  [1, 1]
    	  Position		  [1755, 90, 1805, 130]
    	  ShowName		  off
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'nancheck']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "MATLAB Function5"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "64"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "u"
    	      SID		      "1345:394::1"
    	      Position		      [20, 101, 40, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:394::62"
    	      Ports		      [1, 1]
    	      Position		      [270, 160, 320, 200]
    	      ZOrder		      14
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:394::61"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 5"
    	      Ports		      [1, 2]
    	      Position		      [180, 100, 230, 160]
    	      ZOrder		      13
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[1 2]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"y"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:394::64"
    	      Position		      [460, 171, 480, 189]
    	      ZOrder		      16
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "y"
    	      SID		      "1345:394::56"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "u"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "y"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "y"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "MATLAB Function6"
    	  SID			  "1345:404"
    	  Ports			  [1, 1]
    	  Position		  [470, 91, 535, 119]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'abb2logical']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "MATLAB Function6"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "22"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "abb"
    	      SID		      "1345:404::1"
    	      Position		      [20, 101, 40, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:404::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 160, 320, 200]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:404::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 24"
    	      Ports		      [1, 2]
    	      Position		      [180, 100, 230, 160]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[1 2]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"logical"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:404::22"
    	      Position		      [460, 171, 480, 189]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "logical"
    	      SID		      "1345:404::5"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "abb"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "logical"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "logical"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "MATLAB Function7"
    	  SID			  "1345:407"
    	  Ports			  [1, 1]
    	  Position		  [1595, 755, 1645, 795]
    	  ShowName		  off
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'nancheck']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "MATLAB Function7"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "64"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "u"
    	      SID		      "1345:407::1"
    	      Position		      [20, 101, 40, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:407::62"
    	      Ports		      [1, 1]
    	      Position		      [270, 160, 320, 200]
    	      ZOrder		      14
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:407::61"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 28"
    	      Ports		      [1, 2]
    	      Position		      [180, 100, 230, 160]
    	      ZOrder		      13
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[1 2]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"y"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:407::64"
    	      Position		      [460, 171, 480, 189]
    	      ZOrder		      16
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "y"
    	      SID		      "1345:407::56"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "u"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "y"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "y"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "MATLAB Function9"
    	  SID			  "1345:417"
    	  Ports			  [1, 1]
    	  Position		  [395, 481, 460, 509]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'abb2logical']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "MATLAB Function9"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "22"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "abb"
    	      SID		      "1345:417::1"
    	      Position		      [20, 101, 40, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:417::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 160, 320, 200]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:417::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 36"
    	      Ports		      [1, 2]
    	      Position		      [180, 100, 230, 160]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[1 2]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"logical"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:417::22"
    	      Position		      [460, 171, 480, 189]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "logical"
    	      SID		      "1345:417::5"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "abb"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "logical"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "logical"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  Mux
    	  Name			  "Mux"
    	  SID			  "1345:431"
    	  Ports			  [2, 1]
    	  Position		  [85, 321, 90, 359]
    	  ZOrder		  -16
    	  ShowName		  off
    	  Inputs		  "2"
    	  DisplayOption		  "bar"
    	}
    	Block {
    	  BlockType		  Mux
    	  Name			  "Mux2"
    	  SID			  "1345:449"
    	  Ports			  [2, 1]
    	  Position		  [1340, 456, 1345, 494]
    	  ZOrder		  -16
    	  ShowName		  off
    	  Inputs		  "2"
    	  DisplayOption		  "bar"
    	}
    	Block {
    	  BlockType		  S-Function
    	  Name			  "S-Function1"
    	  SID			  "1345:367"
    	  Ports			  [3, 4]
    	  Position		  [580, 49, 650, 156]
    	  ZOrder		  -5
    	  FunctionName		  "s_right_dyn"
    	  EnableBusSupport	  off
    	}
    	Block {
    	  BlockType		  S-Function
    	  Name			  "S-Function2"
    	  SID			  "1345:422"
    	  Ports			  [3, 4]
    	  Position		  [565, 439, 635, 546]
    	  ZOrder		  -5
    	  FunctionName		  "s_left_dyn"
    	  EnableBusSupport	  off
    	}
    	Block {
    	  BlockType		  Sum
    	  Name			  "Sum"
    	  SID			  "1345:437"
    	  Ports			  [2, 1]
    	  Position		  [1430, 765, 1450, 785]
    	  ZOrder		  -18
    	  ShowName		  off
    	  IconShape		  "round"
    	  Inputs		  "++|"
    	  InputSameDT		  off
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	}
    	Block {
    	  BlockType		  Sum
    	  Name			  "Sum1"
    	  SID			  "1345:438"
    	  Ports			  [2, 1]
    	  Position		  [1570, 100, 1590, 120]
    	  ZOrder		  -18
    	  ShowName		  off
    	  IconShape		  "round"
    	  Inputs		  "|++"
    	  InputSameDT		  off
    	  OutDataTypeStr	  "Inherit: Inherit via internal rule"
    	  SaturateOnIntegerOverflow off
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator1"
    	  SID			  "1345:429"
    	  Position		  [1295, 280, 1315, 300]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator10"
    	  SID			  "1345:478"
    	  Position		  [1810, 230, 1830, 250]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator11"
    	  SID			  "1345:491"
    	  Position		  [710, 965, 730, 985]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator2"
    	  SID			  "1345:456"
    	  Position		  [1440, 820, 1460, 840]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator3"
    	  SID			  "1345:471"
    	  Position		  [680, 295, 700, 315]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator4"
    	  SID			  "1345:425"
    	  Position		  [670, 520, 690, 540]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator5"
    	  SID			  "1345:354"
    	  Position		  [700, 130, 720, 150]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator6"
    	  SID			  "1345:360"
    	  Position		  [715, 105, 735, 125]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator7"
    	  SID			  "1345:426"
    	  Position		  [670, 495, 690, 515]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator8"
    	  SID			  "1345:490"
    	  Position		  [710, 900, 730, 920]
    	}
    	Block {
    	  BlockType		  Terminator
    	  Name			  "Terminator9"
    	  SID			  "1345:477"
    	  Position		  [1810, 190, 1830, 210]
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "coulFric"
    	  SID			  "1345:436"
    	  Ports			  [4, 2]
    	  Position		  [1420, 345, 1495, 690]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'coulFric']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "coulFric"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "27"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "coulRight"
    	      SID		      "1345:436::1"
    	      Position		      [20, 101, 40, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "q_dot"
    	      SID		      "1345:436::26"
    	      Position		      [20, 136, 40, 154]
    	      ZOrder		      12
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "coulLeft"
    	      SID		      "1345:436::24"
    	      Position		      [20, 171, 40, 189]
    	      ZOrder		      10
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "dith"
    	      SID		      "1345:436::27"
    	      Position		      [20, 206, 40, 224]
    	      ZOrder		      13
    	      Port		      "4"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:436::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 220, 320, 260]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:436::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 14"
    	      Ports		      [4, 3]
    	      Position		      [180, 100, 230, 220]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[4 3]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"trqRight"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		3
    		Name			"trqLeft"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:436::22"
    	      Position		      [460, 231, 480, 249]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "trqRight"
    	      SID		      "1345:436::5"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "trqLeft"
    	      SID		      "1345:436::25"
    	      Position		      [460, 136, 480, 154]
    	      ZOrder		      11
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "coulRight"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "q_dot"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      2
    	    }
    	    Line {
    	      SrcBlock		      "coulLeft"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      3
    	    }
    	    Line {
    	      SrcBlock		      "dith"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      4
    	    }
    	    Line {
    	      Name		      "trqRight"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "trqRight"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "trqLeft"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      3
    	      DstBlock		      "trqLeft"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "coulFric1"
    	  SID			  "1345:488"
    	  Ports			  [4, 2]
    	  Position		  [505, 873, 535, 1012]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'forcelog']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "coulFric1"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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		      "T1"
    	      SID		      "1345:488::1"
    	      Position		      [20, 101, 40, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "T2"
    	      SID		      "1345:488::26"
    	      Position		      [20, 136, 40, 154]
    	      ZOrder		      12
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "wrench1"
    	      SID		      "1345:488::28"
    	      Position		      [20, 171, 40, 189]
    	      ZOrder		      14
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "wrench2"
    	      SID		      "1345:488::29"
    	      Position		      [20, 206, 40, 224]
    	      ZOrder		      15
    	      Port		      "4"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:488::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 220, 320, 260]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:488::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 21"
    	      Ports		      [4, 3]
    	      Position		      [180, 100, 230, 220]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[4 3]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"wrench1_out"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		3
    		Name			"wrench2_out"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:488::22"
    	      Position		      [460, 231, 480, 249]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "wrench1_out"
    	      SID		      "1345:488::5"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "wrench2_out"
    	      SID		      "1345:488::30"
    	      Position		      [460, 136, 480, 154]
    	      ZOrder		      16
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "T1"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "T2"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      2
    	    }
    	    Line {
    	      SrcBlock		      "wrench1"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      3
    	    }
    	    Line {
    	      SrcBlock		      "wrench2"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      4
    	    }
    	    Line {
    	      Name		      "wrench1_out"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "wrench1_out"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "wrench2_out"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      3
    	      DstBlock		      "wrench2_out"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "dither"
    	  SID			  "1345:463"
    	  Ports			  [1, 1]
    	  Position		  [1250, 882, 1345, 988]
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  System {
    	    Name		    "dither"
    	    Location		    [685, 72, 1726, 625]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "coulRight"
    	      SID		      "1345:469"
    	      Position		      [55, 158, 85, 172]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Constant
    	      Name		      "Constant2"
    	      SID		      "1345:465"
    	      Position		      [80, 294, 145, 326]
    	      Value		      "AmpDithering"
    	    }
    	    Block {
    	      BlockType		      Constant
    	      Name		      "Constant3"
    	      SID		      "1345:466"
    	      Position		      [80, 249, 145, 281]
    	      Value		      "FreqDithering"
    	      Port {
    		PortNumber		1
    		Name			"freqDith"
    		TestPoint		on
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      SubSystem
    	      Name		      "DITHERING"
    	      SID		      "1345:467"
    	      Ports		      [3, 1]
    	      Position		      [275, 236, 385, 294]
    	      LibraryVersion	      "1.32"
    	      ErrorFcn		      "Stateflow.Translate.translate"
    	      PermitHierarchicalResolution "ExplicitOnly"
    	      TreatAsAtomicUnit	      on
    	      MinAlgLoopOccurrences   off
    	      PropExecContextOutsideSubsystem off
    	      RTWSystemCode	      "Auto"
    	      FunctionWithSeparateData off
    	      Opaque		      off
    	      RequestExecContextInheritance off
    	      MaskHideContents	      off
    	      SFBlockType	      "MATLAB Function"
    	      MaskType		      "Stateflow"
    	      MaskDescription	      "Embedded MATLAB block"
    	      MaskSelfModifiable      on
    	      MaskDisplay	      "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
    	      "rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
    	      MaskIconFrame	      on
    	      MaskIconOpaque	      off
    	      MaskIconRotate	      "none"
    	      MaskPortRotate	      "default"
    	      MaskIconUnits	      "normalized"
    	      Port {
    		PortNumber		1
    		Name			"dithSignal[i]"
    		PropagatedSignals	"dithering"
    		TestPoint		on
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      System {
    		Name			"DITHERING"
    		Location		[257, 457, 812, 717]
    		Open			off
    		ModelBrowserVisibility	off
    		ModelBrowserWidth	200
    		ScreenColor		"white"
    		PaperOrientation	"landscape"
    		PaperPositionMode	"auto"
    		PaperType		"usletter"
    		PaperUnits		"inches"
    		TiledPaperMargins	[0.500000, 0.500000, 0.500000, 0.500000]
    		TiledPageScale		1
    		ShowPageBoundaries	off
    		ZoomFactor		"100"
    		SIDHighWatermark	"23"
    		Block {
    		  BlockType		  Inport
    		  Name			  "coulomb"
    		  SID			  "1345:467::19"
    		  Position		  [20, 101, 40, 119]
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "Fdith"
    		  SID			  "1345:467::1"
    		  Position		  [20, 136, 40, 154]
    		  Port			  "2"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Inport
    		  Name			  "amp"
    		  SID			  "1345:467::18"
    		  Position		  [20, 171, 40, 189]
    		  Port			  "3"
    		  IconDisplay		  "Port number"
    		}
    		Block {
    		  BlockType		  Demux
    		  Name			  " Demux "
    		  SID			  "1345:467::21"
    		  Ports			  [1, 1]
    		  Position		  [270, 180, 320, 220]
    		  ZOrder		  2
    		  Outputs		  "1"
    		}
    		Block {
    		  BlockType		  S-Function
    		  Name			  " SFunction "
    		  SID			  "1345:467::20"
    		  Tag			  "Stateflow S-Function scaled_teleoperation 20"
    		  Ports			  [3, 2]
    		  Position		  [180, 100, 230, 180]
    		  ZOrder		  1
    		  FunctionName		  "sf_sfun"
    		  PortCounts		  "[3 2]"
    		  EnableBusSupport	  on
    		  Port {
    		    PortNumber		    2
    		    Name		    "dithering"
    		    RTWStorageClass	    "Auto"
    		    DataLoggingNameMode	    "SignalName"
    		  }
    		}
    		Block {
    		  BlockType		  Terminator
    		  Name			  " Terminator "
    		  SID			  "1345:467::23"
    		  Position		  [460, 191, 480, 209]
    		  ZOrder		  4
    		}
    		Block {
    		  BlockType		  Outport
    		  Name			  "dithering"
    		  SID			  "1345:467::5"
    		  Position		  [460, 101, 480, 119]
    		  IconDisplay		  "Port number"
    		}
    		Line {
    		  SrcBlock		  "coulomb"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  "Fdith"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  2
    		}
    		Line {
    		  SrcBlock		  "amp"
    		  SrcPort		  1
    		  DstBlock		  " SFunction "
    		  DstPort		  3
    		}
    		Line {
    		  Name			  "dithering"
    		  Labels		  [0, 0]
    		  SrcBlock		  " SFunction "
    		  SrcPort		  2
    		  DstBlock		  "dithering"
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " Demux "
    		  SrcPort		  1
    		  DstBlock		  " Terminator "
    		  DstPort		  1
    		}
    		Line {
    		  SrcBlock		  " SFunction "
    		  SrcPort		  1
    		  DstBlock		  " Demux "
    		  DstPort		  1
    		}
    	      }
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "dith"
    	      SID		      "1345:468"
    	      Position		      [725, 258, 755, 272]
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "coulRight"
    	      SrcPort		      1
    	      Points		      [160, 0; 0, 80]
    	      DstBlock		      "DITHERING"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "Constant2"
    	      SrcPort		      1
    	      Points		      [0, -25]
    	      DstBlock		      "DITHERING"
    	      DstPort		      3
    	    }
    	    Line {
    	      Name		      "freqDith"
    	      Labels		      [0, 0]
    	      SrcBlock		      "Constant3"
    	      SrcPort		      1
    	      DstBlock		      "DITHERING"
    	      DstPort		      2
    	    }
    	    Line {
    	      Name		      "dithSignal[i]"
    	      Labels		      [1, 0]
    	      SrcBlock		      "DITHERING"
    	      SrcPort		      1
    	      DstBlock		      "dith"
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "haptic_controller"
    	  SID			  "1345:416"
    	  Ports			  [12, 4]
    	  Position		  [830, 16, 975, 929]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  Port {
    	    PortNumber		    1
    	    Name		    "q1_arm[i]"
    	    PropagatedSignals	    "Q1"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	  Port {
    	    PortNumber		    2
    	    Name		    "lambda[i]"
    	    PropagatedSignals	    "lambda"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	  Port {
    	    PortNumber		    3
    	    Name		    "q2_arm[i]"
    	    PropagatedSignals	    "Q2"
    	    TestPoint		    on
    	    RTWStorageClass	    "Auto"
    	    DataLoggingNameMode	    "SignalName"
    	  }
    	  System {
    	    Name		    "haptic_controller"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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		      "B1"
    	      SID		      "1345:416::29"
    	      Position		      [20, 101, 40, 119]
    	      ZOrder		      15
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "C1"
    	      SID		      "1345:416::30"
    	      Position		      [20, 136, 40, 154]
    	      ZOrder		      16
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "J1"
    	      SID		      "1345:416::24"
    	      Position		      [20, 171, 40, 189]
    	      ZOrder		      10
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "J1_dot"
    	      SID		      "1345:416::26"
    	      Position		      [20, 206, 40, 224]
    	      ZOrder		      12
    	      Port		      "4"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "q1_dot"
    	      SID		      "1345:416::27"
    	      Position		      [20, 246, 40, 264]
    	      ZOrder		      13
    	      Port		      "5"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "B2"
    	      SID		      "1345:416::35"
    	      Position		      [20, 281, 40, 299]
    	      ZOrder		      17
    	      Port		      "6"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "C2"
    	      SID		      "1345:416::36"
    	      Position		      [20, 316, 40, 334]
    	      ZOrder		      18
    	      Port		      "7"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "J2"
    	      SID		      "1345:416::37"
    	      Position		      [20, 351, 40, 369]
    	      ZOrder		      19
    	      Port		      "8"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "J2_dot"
    	      SID		      "1345:416::38"
    	      Position		      [20, 386, 40, 404]
    	      ZOrder		      20
    	      Port		      "9"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "q2_dot"
    	      SID		      "1345:416::39"
    	      Position		      [20, 426, 40, 444]
    	      ZOrder		      21
    	      Port		      "10"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "e"
    	      SID		      "1345:416::1"
    	      Position		      [20, 461, 40, 479]
    	      Port		      "11"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Inport
    	      Name		      "scale"
    	      SID		      "1345:416::45"
    	      Position		      [20, 496, 40, 514]
    	      ZOrder		      25
    	      Port		      "12"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:416::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 380, 320, 420]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:416::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 35"
    	      Ports		      [12, 5]
    	      Position		      [180, 120, 230, 380]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[12 5]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"Q1"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		3
    		Name			"lambda"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		4
    		Name			"Q2"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	      Port {
    		PortNumber		5
    		Name			"diagm"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:416::22"
    	      Position		      [460, 391, 480, 409]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "Q1"
    	      SID		      "1345:416::5"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "lambda"
    	      SID		      "1345:416::41"
    	      Position		      [460, 136, 480, 154]
    	      ZOrder		      22
    	      Port		      "2"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "Q2"
    	      SID		      "1345:416::25"
    	      Position		      [460, 171, 480, 189]
    	      ZOrder		      11
    	      Port		      "3"
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "diagm"
    	      SID		      "1345:416::44"
    	      Position		      [460, 206, 480, 224]
    	      ZOrder		      24
    	      Port		      "4"
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "B1"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      "C1"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      2
    	    }
    	    Line {
    	      SrcBlock		      "J1"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      3
    	    }
    	    Line {
    	      SrcBlock		      "J1_dot"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      4
    	    }
    	    Line {
    	      SrcBlock		      "q1_dot"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      5
    	    }
    	    Line {
    	      SrcBlock		      "B2"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      6
    	    }
    	    Line {
    	      SrcBlock		      "C2"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      7
    	    }
    	    Line {
    	      SrcBlock		      "J2"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      8
    	    }
    	    Line {
    	      SrcBlock		      "J2_dot"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      9
    	    }
    	    Line {
    	      SrcBlock		      "q2_dot"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      10
    	    }
    	    Line {
    	      SrcBlock		      "e"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      11
    	    }
    	    Line {
    	      SrcBlock		      "scale"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      12
    	    }
    	    Line {
    	      Name		      "Q1"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "Q1"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "lambda"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      3
    	      DstBlock		      "lambda"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "Q2"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      4
    	      DstBlock		      "Q2"
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "diagm"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      5
    	      DstBlock		      "diagm"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "lp"
    	  SID			  "1345:430"
    	  Ports			  [1, 1]
    	  Position		  [110, 309, 165, 371]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'lp']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "lp"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "22"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "v_in"
    	      SID		      "1345:430::1"
    	      Position		      [20, 101, 40, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:430::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 160, 320, 200]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:430::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 4"
    	      Ports		      [1, 2]
    	      Position		      [180, 100, 230, 160]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[1 2]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"v_out"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:430::22"
    	      Position		      [460, 171, 480, 189]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "v_out"
    	      SID		      "1345:430::5"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "v_in"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "v_out"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "v_out"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "lp1"
    	  SID			  "1345:485"
    	  Ports			  [1, 1]
    	  Position		  [1430, 214, 1485, 276]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'lp']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "lp1"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "22"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "v_in"
    	      SID		      "1345:485::1"
    	      Position		      [20, 101, 40, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:485::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 160, 320, 200]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:485::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 19"
    	      Ports		      [1, 2]
    	      Position		      [180, 100, 230, 160]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[1 2]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"v_out"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:485::22"
    	      Position		      [460, 171, 480, 189]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "v_out"
    	      SID		      "1345:485::5"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "v_in"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "v_out"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "v_out"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "lp2"
    	  SID			  "1345:492"
    	  Ports			  [1, 1]
    	  Position		  [565, 890, 605, 930]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'lp']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "lp2"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "22"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "v_in"
    	      SID		      "1345:492::1"
    	      Position		      [20, 101, 40, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:492::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 160, 320, 200]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:492::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 22"
    	      Ports		      [1, 2]
    	      Position		      [180, 100, 230, 160]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[1 2]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"v_out"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:492::22"
    	      Position		      [460, 171, 480, 189]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "v_out"
    	      SID		      "1345:492::5"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "v_in"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "v_out"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "v_out"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  SubSystem
    	  Name			  "lp3"
    	  SID			  "1345:494"
    	  Ports			  [1, 1]
    	  Position		  [565, 955, 605, 995]
    	  ZOrder		  -4
    	  LibraryVersion	  "1.32"
    	  ErrorFcn		  "Stateflow.Translate.translate"
    	  PermitHierarchicalResolution "ExplicitOnly"
    	  TreatAsAtomicUnit	  on
    	  MinAlgLoopOccurrences	  off
    	  PropExecContextOutsideSubsystem off
    	  RTWSystemCode		  "Auto"
    	  FunctionWithSeparateData off
    	  Opaque		  off
    	  RequestExecContextInheritance	off
    	  MaskHideContents	  off
    	  SFBlockType		  "MATLAB Function"
    	  MaskType		  "Stateflow"
    	  MaskDescription	  "Embedded MATLAB block"
    	  MaskSelfModifiable	  on
    	  MaskDisplay		  "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
    	  "eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'lp']);"
    	  MaskIconFrame		  on
    	  MaskIconOpaque	  off
    	  MaskIconRotate	  "none"
    	  MaskPortRotate	  "default"
    	  MaskIconUnits		  "normalized"
    	  System {
    	    Name		    "lp3"
    	    Location		    [257, 457, 812, 717]
    	    Open		    off
    	    ModelBrowserVisibility  off
    	    ModelBrowserWidth	    200
    	    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	    "22"
    	    Block {
    	      BlockType		      Inport
    	      Name		      "v_in"
    	      SID		      "1345:494::1"
    	      Position		      [20, 101, 40, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Block {
    	      BlockType		      Demux
    	      Name		      " Demux "
    	      SID		      "1345:494::20"
    	      Ports		      [1, 1]
    	      Position		      [270, 160, 320, 200]
    	      ZOrder		      6
    	      Outputs		      "1"
    	    }
    	    Block {
    	      BlockType		      S-Function
    	      Name		      " SFunction "
    	      SID		      "1345:494::19"
    	      Tag		      "Stateflow S-Function scaled_teleoperation 23"
    	      Ports		      [1, 2]
    	      Position		      [180, 100, 230, 160]
    	      ZOrder		      5
    	      FunctionName	      "sf_sfun"
    	      PortCounts	      "[1 2]"
    	      EnableBusSupport	      on
    	      Port {
    		PortNumber		2
    		Name			"v_out"
    		RTWStorageClass		"Auto"
    		DataLoggingNameMode	"SignalName"
    	      }
    	    }
    	    Block {
    	      BlockType		      Terminator
    	      Name		      " Terminator "
    	      SID		      "1345:494::22"
    	      Position		      [460, 171, 480, 189]
    	      ZOrder		      8
    	    }
    	    Block {
    	      BlockType		      Outport
    	      Name		      "v_out"
    	      SID		      "1345:494::5"
    	      Position		      [460, 101, 480, 119]
    	      IconDisplay	      "Port number"
    	    }
    	    Line {
    	      SrcBlock		      "v_in"
    	      SrcPort		      1
    	      DstBlock		      " SFunction "
    	      DstPort		      1
    	    }
    	    Line {
    	      Name		      "v_out"
    	      Labels		      [0, 0]
    	      SrcBlock		      " SFunction "
    	      SrcPort		      2
    	      DstBlock		      "v_out"
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " Demux "
    	      SrcPort		      1
    	      DstBlock		      " Terminator "
    	      DstPort		      1
    	    }
    	    Line {
    	      SrcBlock		      " SFunction "
    	      SrcPort		      1
    	      DstBlock		      " Demux "
    	      DstPort		      1
    	    }
    	  }
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "torque_motor_1"
    	  SID			  "1345:293"
    	  Position		  [1870, 103, 1900, 117]
    	  IconDisplay		  "Port number"
    	}
    	Block {
    	  BlockType		  Outport
    	  Name			  "torque_motor_2"
    	  SID			  "1345:408"
    	  Position		  [1805, 758, 1835, 772]
    	  Port			  "2"
    	  IconDisplay		  "Port number"
    	}
    	Line {
    	  SrcBlock		  "q_motor_1"
    	  SrcPort		  1
    	  DstBlock		  "FRIDA motor2arm"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "FRIDA arm acceleration NOT SI"
    	  SrcPort		  1
    	  Points		  [45, 0]
    	  Branch {
    	    Points		    [40, 0]
    	    Branch {
    	      Points		      [195, 0]
    	      DstBlock		      "MATLAB Function1"
    	      DstPort		      1
    	    }
    	    Branch {
    	      Points		      [-5, 0]
    	      DstBlock		      "MATLAB Function3"
    	      DstPort		      1
    	    }
    	  }
    	  Branch {
    	    DstBlock		    "coulFric1"
    	    DstPort		    1
    	  }
    	}
    	Line {
    	  SrcBlock		  "FRIDA motor2arm1"
    	  SrcPort		  1
    	  Points		  [15, 0]
    	  Branch {
    	    Points		    [0, -70]
    	    DstBlock		    "MATLAB Function6"
    	    DstPort		    1
    	  }
    	  Branch {
    	    Points		    [5, 0; 0, 100]
    	    Branch {
    	      Points		      [15, 0]
    	      DstBlock		      "FRIDA arm acceleration NOT SI"
    	      DstPort		      2
    	    }
    	    Branch {
    	      Points		      [0, 85]
    	      DstBlock		      "haptic_controller"
    	      DstPort		      5
    	    }
    	  }
    	}
    	Line {
    	  SrcBlock		  "FRIDA motor2arm"
    	  SrcPort		  1
    	  Points		  [30, 0; 0, 5]
    	  Branch {
    	    Points		    [0, 130]
    	    DstBlock		    "FRIDA arm acceleration NOT SI"
    	    DstPort		    1
    	  }
    	  Branch {
    	    Points		    [0, -25]
    	    DstBlock		    "MATLAB Function2"
    	    DstPort		    1
    	  }
    	}
    	Line {
    	  SrcBlock		  "S-Function1"
    	  SrcPort		  4
    	  DstBlock		  "Terminator5"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Constant2"
    	  SrcPort		  1
    	  DstBlock		  "S-Function1"
    	  DstPort		  3
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function2"
    	  SrcPort		  1
    	  DstBlock		  "S-Function1"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function5"
    	  SrcPort		  1
    	  DstBlock		  "Gain8"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function6"
    	  SrcPort		  1
    	  DstBlock		  "S-Function1"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function7"
    	  SrcPort		  1
    	  DstBlock		  "Gain9"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "q_motor_2"
    	  SrcPort		  1
    	  DstBlock		  "FRIDA motor2arm4"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "FRIDA arm acceleration NOT SI1"
    	  SrcPort		  1
    	  Points		  [10, 0]
    	  Branch {
    	    Points		    [195, 0; 0, 275]
    	    DstBlock		    "MATLAB Function1"
    	    DstPort		    2
    	  }
    	  Branch {
    	    Points		    [0, 305]
    	    DstBlock		    "coulFric1"
    	    DstPort		    2
    	  }
    	}
    	Line {
    	  SrcBlock		  "FRIDA motor2arm5"
    	  SrcPort		  1
    	  Points		  [10, 0]
    	  Branch {
    	    Points		    [0, -70; 95, 0]
    	    DstBlock		    "MATLAB Function9"
    	    DstPort		    1
    	  }
    	  Branch {
    	    Points		    [10, 0; 0, 100]
    	    Branch {
    	      Points		      [40, 0]
    	      DstBlock		      "FRIDA arm acceleration NOT SI1"
    	      DstPort		      2
    	    }
    	    Branch {
    	      Points		      [0, 55]
    	      DstBlock		      "haptic_controller"
    	      DstPort		      10
    	    }
    	  }
    	}
    	Line {
    	  SrcBlock		  "FRIDA motor2arm4"
    	  SrcPort		  1
    	  Points		  [30, 0; 0, 5]
    	  Branch {
    	    Points		    [0, 130]
    	    DstBlock		    "FRIDA arm acceleration NOT SI1"
    	    DstPort		    1
    	  }
    	  Branch {
    	    Points		    [0, -40]
    	    DstBlock		    "MATLAB Function10"
    	    DstPort		    1
    	  }
    	}
    	Line {
    	  SrcBlock		  "Constant1"
    	  SrcPort		  1
    	  Points		  [90, 0]
    	  DstBlock		  "S-Function2"
    	  DstPort		  3
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function10"
    	  SrcPort		  1
    	  DstBlock		  "S-Function2"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "MATLAB Function9"
    	  SrcPort		  1
    	  DstBlock		  "S-Function2"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "S-Function1"
    	  SrcPort		  1
    	  Points		  [160, 0]
    	  DstBlock		  "haptic_controller"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "S-Function2"
    	  SrcPort		  1
    	  Points		  [160, 0; 0, -20]
    	  DstBlock		  "haptic_controller"
    	  DstPort		  6
    	}
    	Line {
    	  SrcBlock		  "S-Function1"
    	  SrcPort		  2
    	  Points		  [160, 0]
    	  DstBlock		  "haptic_controller"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "FRIDA arm acceleration NOT SI"
    	  SrcPort		  3
    	  Points		  [330, 0; 0, 10]
    	  DstBlock		  "haptic_controller"
    	  DstPort		  4
    	}
    	Line {
    	  SrcBlock		  "S-Function2"
    	  SrcPort		  2
    	  Points		  [150, 0; 0, 30]
    	  DstBlock		  "haptic_controller"
    	  DstPort		  7
    	}
    	Line {
    	  SrcBlock		  "FRIDA arm acceleration NOT SI1"
    	  SrcPort		  2
    	  Points		  [150, 0; 0, 5; 170, 0]
    	  Branch {
    	    Points		    [35, 0]
    	    DstBlock		    "haptic_controller"
    	    DstPort		    8
    	  }
    	  Branch {
    	    Points		    [0, -435]
    	    DstBlock		    "MATLAB Function4"
    	    DstPort		    2
    	  }
    	}
    	Line {
    	  SrcBlock		  "FRIDA arm acceleration NOT SI1"
    	  SrcPort		  3
    	  Points		  [350, 0; 0, -20]
    	  DstBlock		  "haptic_controller"
    	  DstPort		  9
    	}
    	Line {
    	  Name			  "e[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "MATLAB Function1"
    	  SrcPort		  1
    	  Points		  [20, 0]
    	  DstBlock		  "haptic_controller"
    	  DstPort		  11
    	}
    	Line {
    	  SrcBlock		  "FRIDA arm acceleration NOT SI"
    	  SrcPort		  2
    	  Points		  [310, 0]
    	  Branch {
    	    Points		    [60, 0]
    	    DstBlock		    "haptic_controller"
    	    DstPort		    3
    	  }
    	  Branch {
    	    Points		    [0, -50]
    	    DstBlock		    "MATLAB Function4"
    	    DstPort		    1
    	  }
    	}
    	Line {
    	  SrcBlock		  "S-Function1"
    	  SrcPort		  3
    	  DstBlock		  "Terminator6"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "S-Function2"
    	  SrcPort		  3
    	  DstBlock		  "Terminator7"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "S-Function2"
    	  SrcPort		  4
    	  DstBlock		  "Terminator4"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "lp"
    	  SrcPort		  1
    	  DstBlock		  "Demux"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Mux"
    	  SrcPort		  1
    	  DstBlock		  "lp"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "qd_motor_1"
    	  SrcPort		  1
    	  Points		  [0, 5]
    	  DstBlock		  "Mux"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "qd_motor_2"
    	  SrcPort		  1
    	  Points		  [10, 0; 0, -20]
    	  DstBlock		  "Mux"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "Demux"
    	  SrcPort		  1
    	  Points		  [10, 0]
    	  DstBlock		  "FRIDA motor2arm1"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Demux"
    	  SrcPort		  2
    	  DstBlock		  "FRIDA motor2arm5"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "coulombWidthArmRightAbb"
    	  SrcPort		  1
    	  DstBlock		  "Gain"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "FRIDA motor2arm2"
    	  SrcPort		  1
    	  DstBlock		  "MATLAB Function5"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "FRIDA motor2arm3"
    	  SrcPort		  1
    	  DstBlock		  "MATLAB Function7"
    	  DstPort		  1
    	}
    	Line {
    	  Name			  "q2_arm[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "haptic_controller"
    	  SrcPort		  3
    	  Points		  [230, 0; 0, 180]
    	  DstBlock		  "Gain6"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Sum"
    	  SrcPort		  1
    	  DstBlock		  "FRIDA motor2arm3"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "coulombWidthArmLeftAbb"
    	  SrcPort		  1
    	  DstBlock		  "Gain1"
    	  DstPort		  1
    	}
    	Line {
    	  Name			  "lambda[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "haptic_controller"
    	  SrcPort		  2
    	  Points		  [0, -70]
    	  DstBlock		  "Terminator1"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Sum1"
    	  SrcPort		  1
    	  DstBlock		  "FRIDA motor2arm2"
    	  DstPort		  1
    	}
    	Line {
    	  Name			  "q1_arm[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "haptic_controller"
    	  SrcPort		  1
    	  Points		  [0, -20]
    	  DstBlock		  "Gain5"
    	  DstPort		  1
    	}
    	Line {
    	  Name			  "cright[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "Gain"
    	  SrcPort		  1
    	  Points		  [20, 0]
    	  Branch {
    	    Points		    [215, 0]
    	    DstBlock		    "coulFric"
    	    DstPort		    1
    	  }
    	  Branch {
    	    Points		    [0, 560]
    	    DstBlock		    "dither"
    	    DstPort		    1
    	  }
    	}
    	Line {
    	  Name			  "cleft[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "Gain1"
    	  SrcPort		  1
    	  Points		  [55, 0; 0, -35]
    	  DstBlock		  "coulFric"
    	  DstPort		  3
    	}
    	Line {
    	  Name			  "help_right[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "Gain2"
    	  SrcPort		  1
    	  Points		  [40, 0; 0, -285]
    	  DstBlock		  "Sum1"
    	  DstPort		  2
    	}
    	Line {
    	  Name			  "help_left[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "Gain3"
    	  SrcPort		  1
    	  Points		  [5, 0; 0, 135; -205, 0]
    	  DstBlock		  "Sum"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "coulFric"
    	  SrcPort		  1
    	  Points		  [15, 0; 0, -15]
    	  DstBlock		  "Gain2"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "coulFric"
    	  SrcPort		  2
    	  Points		  [5, 0; 0, -15]
    	  DstBlock		  "Gain3"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Mux2"
    	  SrcPort		  1
    	  Points		  [5, 0]
    	  Branch {
    	    DstBlock		    "coulFric"
    	    DstPort		    2
    	  }
    	  Branch {
    	    Points		    [-30, -230]
    	    DstBlock		    "lp1"
    	    DstPort		    1
    	  }
    	}
    	Line {
    	  SrcBlock		  "velFlt1"
    	  SrcPort		  1
    	  DstBlock		  "FRIDA motor2arm6"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "velFlt2"
    	  SrcPort		  1
    	  DstBlock		  "FRIDA motor2arm7"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "FRIDA motor2arm6"
    	  SrcPort		  1
    	  DstBlock		  "Mux2"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "FRIDA motor2arm7"
    	  SrcPort		  1
    	  Points		  [0, -25]
    	  DstBlock		  "Mux2"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "haptic_controller"
    	  SrcPort		  4
    	  Points		  [0, 10]
    	  DstBlock		  "Gain4"
    	  DstPort		  1
    	}
    	Line {
    	  Name			  "diagm[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "Gain4"
    	  SrcPort		  1
    	  DstBlock		  "Gain7"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Gain5"
    	  SrcPort		  1
    	  DstBlock		  "Sum1"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Gain6"
    	  SrcPort		  1
    	  Points		  [50, 0]
    	  DstBlock		  "Sum"
    	  DstPort		  2
    	}
    	Line {
    	  SrcBlock		  "Gain7"
    	  SrcPort		  1
    	  DstBlock		  "Terminator2"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Gain8"
    	  SrcPort		  1
    	  DstBlock		  "torque_motor_1"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "Gain9"
    	  SrcPort		  1
    	  Points		  [30, 0]
    	  DstBlock		  "torque_motor_2"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "dither"
    	  SrcPort		  1
    	  Points		  [55, 0]
    	  DstBlock		  "coulFric"
    	  DstPort		  4
    	}
    	Line {
    	  Name			  "pose1[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "MATLAB Function3"
    	  SrcPort		  1
    	  Points		  [0, -20]
    	  DstBlock		  "Terminator3"
    	  DstPort		  1
    	}
    	Line {
    	  Name			  "vel1[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "MATLAB Function4"
    	  SrcPort		  1
    	  DstBlock		  "Terminator9"
    	  DstPort		  1
    	}
    	Line {
    	  Name			  "vel2[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "MATLAB Function4"
    	  SrcPort		  2
    	  DstBlock		  "Terminator10"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "lp1"
    	  SrcPort		  1
    	  DstBlock		  "MATLAB Function4"
    	  DstPort		  3
    	}
    	Line {
    	  SrcBlock		  "wrench2"
    	  SrcPort		  1
    	  Points		  [0, -5]
    	  DstBlock		  "coulFric1"
    	  DstPort		  4
    	}
    	Line {
    	  SrcBlock		  "wrench1"
    	  SrcPort		  1
    	  Points		  [0, 70]
    	  DstBlock		  "coulFric1"
    	  DstPort		  3
    	}
    	Line {
    	  SrcBlock		  "coulFric1"
    	  SrcPort		  1
    	  DstBlock		  "lp2"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "coulFric1"
    	  SrcPort		  2
    	  Points		  [0, -5]
    	  DstBlock		  "lp3"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "lp2"
    	  SrcPort		  1
    	  DstBlock		  "Gain10"
    	  DstPort		  1
    	}
    	Line {
    	  SrcBlock		  "lp3"
    	  SrcPort		  1
    	  DstBlock		  "Gain11"
    	  DstPort		  1
    	}
    	Line {
    	  Name			  "w1[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "Gain10"
    	  SrcPort		  1
    	  DstBlock		  "Terminator8"
    	  DstPort		  1
    	}
    	Line {
    	  Name			  "w2[i]"
    	  Labels		  [0, 0]
    	  SrcBlock		  "Gain11"
    	  SrcPort		  1
    	  DstBlock		  "Terminator11"
    	  DstPort		  1
    	}
          }
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].joint[i].parKp"
          SID		      "211"
          Position		      [725, 338, 755, 352]
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].joint[i].parKv"
          SID		      "212"
          Position		      [715, 403, 745, 417]
          Port		      "2"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].joint[i].parKi"
          SID		      "213"
          Position		      [695, 468, 725, 482]
          Port		      "3"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].joint[i].posRef"
          SID		      "214"
          Position		      [765, 533, 795, 547]
          Port		      "4"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].joint[i].velRef"
          SID		      "215"
          Position		      [765, 598, 795, 612]
          Port		      "5"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].joint[i].trqFfw"
          SID		      "216"
          Position		      [720, 663, 750, 677]
          Port		      "6"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].mocgendata.instruction"
          SID		      "217"
          Position		      [710, 728, 740, 742]
          Port		      "7"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].mocgendata.value1"
          SID		      "218"
          Position		      [735, 793, 765, 807]
          Port		      "8"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].mocgendata.value2"
          SID		      "219"
          Position		      [735, 858, 765, 872]
          Port		      "9"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].mocgendata.value3"
          SID		      "220"
          Position		      [735, 923, 765, 937]
          Port		      "10"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].mocgendata.value4"
          SID		      "221"
          Position		      [725, 988, 755, 1002]
          Port		      "11"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].mocgendata.value5"
          SID		      "222"
          Position		      [725, 1053, 755, 1067]
          Port		      "12"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].mocgendata.value6"
          SID		      "223"
          Position		      [735, 1118, 765, 1132]
          Port		      "13"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].mocgendata.string1[i]"
          SID		      "224"
          Position		      [735, 1183, 765, 1197]
          Port		      "14"
          IconDisplay	      "Port number"
          PortDimensions	      "20"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[1].mocgendata.string2[i]"
          SID		      "225"
          Position		      [715, 1253, 745, 1267]
          Port		      "15"
          IconDisplay	      "Port number"
          PortDimensions	      "20"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].joint[i].parKp"
          SID		      "322"
          Position		      [1530, 318, 1560, 332]
          Port		      "16"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].joint[i].parKv"
          SID		      "323"
          Position		      [1570, 393, 1600, 407]
          Port		      "17"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].joint[i].parKi"
          SID		      "324"
          Position		      [1590, 468, 1620, 482]
          Port		      "18"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].joint[i].posRef"
          SID		      "325"
          Position		      [1665, 1368, 1695, 1382]
          Port		      "19"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].joint[i].velRef"
          SID		      "326"
          Position		      [1755, 543, 1785, 557]
          Port		      "20"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].joint[i].trqFfw"
          SID		      "327"
          Position		      [1570, 618, 1600, 632]
          Port		      "21"
          IconDisplay	      "Port number"
          PortDimensions	      "DOF"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].mocgendata.instruction"
          SID		      "328"
          Position		      [1615, 693, 1645, 707]
          Port		      "22"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].mocgendata.value1"
          SID		      "329"
          Position		      [1595, 768, 1625, 782]
          Port		      "23"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].mocgendata.value2"
          SID		      "330"
          Position		      [1605, 843, 1635, 857]
          Port		      "24"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].mocgendata.value3"
          SID		      "331"
          Position		      [1605, 918, 1635, 932]
          Port		      "25"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].mocgendata.value4"
          SID		      "332"
          Position		      [1595, 993, 1625, 1007]
          Port		      "26"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].mocgendata.value5"
          SID		      "333"
          Position		      [1600, 1068, 1630, 1082]
          Port		      "27"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].mocgendata.value6"
          SID		      "334"
          Position		      [1585, 1143, 1615, 1157]
          Port		      "28"
          IconDisplay	      "Port number"
          PortDimensions	      "1"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].mocgendata.string1[i]"
          SID		      "335"
          Position		      [1570, 1218, 1600, 1232]
          Port		      "29"
          IconDisplay	      "Port number"
          PortDimensions	      "20"
        }
        Block {
          BlockType		      Outport
          Name		      "ext2irb.robot[0].mocgendata.string2[i]"
          SID		      "336"
          Position		      [1595, 1293, 1625, 1307]
          Port		      "30"
          IconDisplay	      "Port number"
          PortDimensions	      "20"
        }
        Line {
          SrcBlock		      "irb2ext.obtaining"
          SrcPort		      1
          Points		      [80, 0; 0, 160]
          Branch {
    	Points			[925, 0]
    	DstBlock		"Right"
    	DstPort			1
          }
          Branch {
    	Points			[0, 50]
    	DstBlock		"Left"
    	DstPort			1
          }
        }
        Line {
          SrcBlock		      "irb2ext.manualMode"
          SrcPort		      1
          Points		      [110, 0; 0, 165]
          Branch {
    	Points			[850, 0; 0, 25]
    	DstBlock		"Right"
    	DstPort			2
          }
          Branch {
    	Points			[0, 45]
    	DstBlock		"Left"
    	DstPort			2
          }
        }
        Line {
          SrcBlock		      "irb2ext.controlActive"
          SrcPort		      1
          Points		      [50, 0]
          Branch {
    	Points			[0, 95; 835, 0; 0, 100]
    	DstBlock		"Right"
    	DstPort			3
          }
          Branch {
    	Points			[0, 210]
    	DstBlock		"Left"
    	DstPort			3
          }
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].mocgendata.string2[i]"
          SrcPort		      1
          Points		      [205, 0]
          DstBlock		      "Left"
          DstPort		      27
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].mocgendata.string1[i]"
          SrcPort		      1
          Points		      [200, 0]
          DstBlock		      "Left"
          DstPort		      26
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].mocgendata.value6"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      25
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].mocgendata.value5"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      24
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].parKp"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      4
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].parKv"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      5
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].parKi"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      6
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].parTrqMin"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      7
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].parTrqMax"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      8
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].posRawFb"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      9
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].posFlt"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      10
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].velFlt"
          SrcPort		      1
          Points		      [115, 0]
          Branch {
    	Points			[75, 0]
    	DstBlock		"Left"
    	DstPort			11
          }
          Branch {
    	Points			[0, -470]
    	DstBlock		"botharms_get_torque"
    	DstPort			8
          }
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].velOut"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      12
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].trqRaw"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      13
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].trqRefFlt"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      14
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].posRef"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      15
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].velRef"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      16
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].trqFfw"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      17
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].trqFfwGrav"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      18
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].mocgendata.instruction"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      19
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].mocgendata.value1"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      20
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].mocgendata.value2"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      21
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].mocgendata.value3"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      22
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].mocgendata.value4"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Left"
          DstPort		      23
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      2
          DstBlock		      "ext2irb.robot[1].joint[i].parKp"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      3
          DstBlock		      "ext2irb.robot[1].joint[i].parKv"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      4
          DstBlock		      "ext2irb.robot[1].joint[i].parKi"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      5
          DstBlock		      "MATLAB Function3"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      6
          DstBlock		      "MATLAB Function4"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      7
          DstBlock		      "ext2irb.robot[1].joint[i].trqFfw"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      8
          DstBlock		      "ext2irb.robot[1].mocgendata.instruction"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      9
          DstBlock		      "ext2irb.robot[1].mocgendata.value1"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      10
          DstBlock		      "ext2irb.robot[1].mocgendata.value2"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      11
          DstBlock		      "ext2irb.robot[1].mocgendata.value3"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      12
          DstBlock		      "ext2irb.robot[1].mocgendata.value4"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      13
          DstBlock		      "ext2irb.robot[1].mocgendata.value5"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      14
          DstBlock		      "ext2irb.robot[1].mocgendata.value6"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      15
          DstBlock		      "ext2irb.robot[1].mocgendata.string1[i]"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      16
          Points		      [0, 5]
          DstBlock		      "ext2irb.robot[1].mocgendata.string2[i]"
          DstPort		      1
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].mocgendata.string2[i]"
          SrcPort		      1
          Points		      [205, 0]
          DstBlock		      "Right"
          DstPort		      25
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].mocgendata.string1[i]"
          SrcPort		      1
          Points		      [185, 0]
          DstBlock		      "Right"
          DstPort		      24
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].mocgendata.value6"
          SrcPort		      1
          Points		      [200, 0]
          DstBlock		      "Right"
          DstPort		      23
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].mocgendata.value5"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Right"
          DstPort		      22
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].parKp"
          SrcPort		      1
          Points		      [230, 0]
          DstBlock		      "Right"
          DstPort		      4
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].parKv"
          SrcPort		      1
          Points		      [210, 0]
          DstBlock		      "Right"
          DstPort		      5
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].parKi"
          SrcPort		      1
          Points		      [210, 0]
          DstBlock		      "Right"
          DstPort		      6
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].parTrqMin"
          SrcPort		      1
          Points		      [210, 0]
          DstBlock		      "Right"
          DstPort		      7
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].parTrqMax"
          SrcPort		      1
          Points		      [210, 0]
          DstBlock		      "Right"
          DstPort		      8
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].posRawFb"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Right"
          DstPort		      9
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].posFlt"
          SrcPort		      1
          Points		      [210, 0]
          DstBlock		      "Right"
          DstPort		      10
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].velFlt"
          SrcPort		      1
          Points		      [95, 0]
          Branch {
    	Points			[115, 0]
    	DstBlock		"Right"
    	DstPort			11
          }
          Branch {
    	Points			[0, -30; -235, 0]
    	DstBlock		"botharms_get_torque"
    	DstPort			7
          }
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].velOut"
          SrcPort		      1
          Points		      [210, 0]
          DstBlock		      "Right"
          DstPort		      12
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].trqRaw"
          SrcPort		      1
          Points		      [210, 0]
          DstBlock		      "Right"
          DstPort		      13
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].trqRefFlt"
          SrcPort		      1
          Points		      [210, 0]
          DstBlock		      "Right"
          DstPort		      14
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].trqFfw"
          SrcPort		      1
          Points		      [180, 0]
          DstBlock		      "Right"
          DstPort		      15
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].trqFfwGrav"
          SrcPort		      1
          Points		      [180, 0]
          DstBlock		      "Right"
          DstPort		      16
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].mocgendata.instruction"
          SrcPort		      1
          Points		      [180, 0]
          DstBlock		      "Right"
          DstPort		      17
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].mocgendata.value1"
          SrcPort		      1
          Points		      [180, 0]
          DstBlock		      "Right"
          DstPort		      18
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].mocgendata.value2"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Right"
          DstPort		      19
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].mocgendata.value3"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Right"
          DstPort		      20
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].mocgendata.value4"
          SrcPort		      1
          Points		      [190, 0]
          DstBlock		      "Right"
          DstPort		      21
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      2
          DstBlock		      "ext2irb.robot[0].joint[i].parKp"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      3
          DstBlock		      "ext2irb.robot[0].joint[i].parKv"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      6
          DstBlock		      "ext2irb.robot[0].joint[i].trqFfw"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      9
          DstBlock		      "ext2irb.robot[0].mocgendata.value2"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      10
          DstBlock		      "ext2irb.robot[0].mocgendata.value3"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      11
          DstBlock		      "ext2irb.robot[0].mocgendata.value4"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      12
          DstBlock		      "ext2irb.robot[0].mocgendata.value5"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      15
          DstBlock		      "ext2irb.robot[0].mocgendata.string2[i]"
          DstPort		      1
        }
        Line {
          SrcBlock		      "MATLAB Function2"
          SrcPort		      1
          DstBlock		      "ext2irb.robot[0].joint[i].velRef"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      5
          DstBlock		      "MATLAB Function2"
          DstPort		      1
        }
        Line {
          SrcBlock		      "MATLAB Function4"
          SrcPort		      1
          DstBlock		      "ext2irb.robot[1].joint[i].velRef"
          DstPort		      1
        }
        Line {
          SrcBlock		      "MATLAB Function3"
          SrcPort		      1
          DstBlock		      "ext2irb.robot[1].joint[i].posRef"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      7
          DstBlock		      "ext2irb.robot[0].mocgendata.instruction"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Constant9"
          SrcPort		      1
          Points		      [135, 0; 0, -130]
          DstBlock		      "Left"
          DstPort		      28
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      8
          DstBlock		      "ext2irb.robot[0].mocgendata.value1"
          DstPort		      1
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].posRef"
          SrcPort		      1
          Points		      [155, 0]
          DstBlock		      "Right"
          DstPort		      28
        }
        Line {
          SrcBlock		      "MATLAB Function1"
          SrcPort		      1
          DstBlock		      "ext2irb.robot[0].joint[i].posRef"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      16
          DstBlock		      "MATLAB Function1"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Constant1"
          SrcPort		      1
          Points		      [50, 0]
          DstBlock		      "Right"
          DstPort		      26
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].velRef"
          SrcPort		      1
          Points		      [140, 0]
          DstBlock		      "Right"
          DstPort		      27
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      4
          DstBlock		      "ext2irb.robot[0].joint[i].parKi"
          DstPort		      1
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].posRawAbs"
          SrcPort		      1
          Points		      [115, 0]
          DstBlock		      "botharms_get_torque"
          DstPort		      3
        }
        Line {
          SrcBlock		      "irb2ext.robot[1].joint[i].velRaw"
          SrcPort		      1
          Points		      [0, 15; 115, 0]
          DstBlock		      "botharms_get_torque"
          DstPort		      4
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].posRawAbs"
          SrcPort		      1
          Points		      [115, 0]
          DstBlock		      "botharms_get_torque"
          DstPort		      1
        }
        Line {
          SrcBlock		      "irb2ext.robot[0].joint[i].velRaw"
          SrcPort		      1
          Points		      [115, 0]
          DstBlock		      "botharms_get_torque"
          DstPort		      2
        }
        Line {
          Name		      "torque1[i]"
          Labels		      [0, 0]
          SrcBlock		      "botharms_get_torque"
          SrcPort		      1
          Points		      [100, 0; 0, 1300]
          DstBlock		      "Right"
          DstPort		      29
        }
        Line {
          Name		      "torque2[i]"
          Labels		      [0, 0]
          SrcBlock		      "botharms_get_torque"
          SrcPort		      2
          Points		      [35, 0; 0, 50; -1035, 0; 0, 1035]
          DstBlock		      "Left"
          DstPort		      29
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      14
          DstBlock		      "ext2irb.robot[0].mocgendata.string1[i]"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      13
          DstBlock		      "ext2irb.robot[0].mocgendata.value6"
          DstPort		      1
        }
        Line {
          SrcBlock		      "Right"
          SrcPort		      1
          Points		      [45, 0; 0, -245; -775, 0; 0, 105]
          DstBlock		      "botharms_get_torque"
          DstPort		      5
        }
        Line {
          SrcBlock		      "Left"
          SrcPort		      1
          Points		      [110, 0; 0, -150]
          DstBlock		      "botharms_get_torque"
          DstPort		      6
        }
        Line {
          SrcBlock		      "ati2rob.forcesTorques[i]"
          SrcPort		      1
          Points		      [0, 5]
          DstBlock		      "botharms_get_torque"
          DstPort		      9
        }
        Line {
          SrcBlock		      "ati2rob_2.forcesTorques[i]"
          SrcPort		      1
          Points		      [0, 5]
          DstBlock		      "botharms_get_torque"
          DstPort		      10
        }
        Annotation {
          SID		      "930"
          Name		      "addpath /opt/robot/matlab;\naddpath /home/martinka/Projects/SARAFun/demopert;\naddpath /opt/robot/m"
          "atlab/irb/mex;\naddpath /opt/robot/matlab/irb/mex/mex_ESMERALDA; \naddpath /home/martinka/Projects/SARAFun/bp-ar"
          "-hmm/dmp/leadthrough;\naddpath /home/martinka/frida_dynamics;\n\nDOF=7;\nh=0.004032;\nhighKiActive = 0;\nkeepSti"
          "ll = 0;\nrightActive = 1;\nleftActive = 1;\n\nfriction = 0.7;\ncontrol = 1;\nAmpDithering=0.3;\nFreqDithering=15"
          ";\nadmit_CTRL=0;\n\n\n[B_lpfilt,A_lpfilt] = butter(1,0.01);\n\nparametersLeadthrough;\n\nf_switch = 1;\na_switch"
          " = 0;\ndisp('Init done');"
          Position		      [1762, 173]
          ForegroundColor	      "red"
          DropShadow	      on
          ClickFcn		      "addpath /opt/robot/matlab;\naddpath /home/martinka/Projects/SARAFun/demopert;\naddpath /opt/rob"
          "ot/matlab/irb/mex;\naddpath /opt/robot/matlab/irb/mex/mex_ESMERALDA; \naddpath /home/martinka/Projects/SARAFun/b"
          "p-ar-hmm/dmp/leadthrough;\naddpath /home/martinka/frida_dynamics;\n\nDOF=7;\nh=0.004032;\nhighKiActive = 0;\nkee"
          "pStill = 0;\nrightActive = 1;\nleftActive = 1;\n\nfriction = 0.7;\ncontrol = 1;\nAmpDithering=0.3;\nFreqDitherin"
          "g=15;\nadmit_CTRL=0;\n\n\n[B_lpfilt,A_lpfilt] = butter(1,0.01);\n\nparametersLeadthrough;\n\nf_switch = 1;\na_sw"
          "itch = 0;\ndisp('Init done');"
          UseDisplayTextAsClickCallback on
        }
      }
    }
    # Finite State Machines
    #
    #    Stateflow Version 7.6 (R2011b) dated Jan 25 2012, 13:50:40
    #
    #
    
    
    Stateflow {
      machine {
        id			    1
        name		    "scaled_teleoperation"
        created		    "06-Feb-2013 13:26:16"
        isLibrary		    0
        firstTarget		    349
        sfVersion		    76014001.00040001
      }
      chart {
        id			    2
        name		    "botharms_get_torque/FRIDA arm acceleration NOT SI/frida_kin"
        windowPosition	    [270.375 98.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 3 0 0]
        firstTransition	    5
        firstJunction	    4
        viewObj		    2
        machine		    1
        ssIdHighWaterMark	    9
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    6
        chartFileNumber	    1
        disableImplicitCasting  1
        eml {
          name		      "frida_kinematics"
        }
      }
      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 [T44_fkine, J0, Jn, Jdot0] = frida_kinematics(q,qd)\n% Calculation of forward kinematics"
          ", jacobian in base frame and flange\n% frame, and Jacobian time derivative (in base frame)\n\nn_joints = 7;\n\nq"
          "_offs = [0;0;pi/2;0;0;0;0];\nq = q+q_offs;\n\nshoulderLen = 0.110*1000;\nshoulderOffs = -0.030*1000;\nelbowOffs "
          "= 0.0405*1000;\nwristOffs = 0.0135*1000;\nhandOffs = -0.027*1000;\nupArmLen = 0.2465*1000;\nlowArmLen = 0.265*10"
          "00;\nhandLen = 0.032*1000;\n\n\n%          alpha        a        theta       d       Rev=0/Pris=1\ndhpar = [ -pi"
          "/2   shoulderOffs   q(1)   shoulderLen   0\n           pi/2  -shoulderOffs   q(2)        0        0\n          -"
          "pi/2   elbowOffs      q(7)    upArmLen     0\n           pi/2  -elbowOffs      q(3)        0        0\n         "
          " -pi/2   wristOffs      q(4)    lowArmLen    0\n           pi/2   handOffs       q(5)        0        0\n       "
          "    0      0              q(6)     handLen     0 ];\n        \nbase = eye(4);\n     \ntrans = zeros(4,4,n_joints"
          ");\n        \nfor j=1:n_joints\n    t = [ cos(dhpar(j,3))   -sin(dhpar(j,3))*cos(dhpar(j,1))    sin(dhpar(j,3))*"
          "sin(dhpar(j,1))    dhpar(j,2)*cos(dhpar(j,3))\n          sin(dhpar(j,3))   cos(dhpar(j,3))*cos(dhpar(j,1))     -"
          "cos(dhpar(j,3))*sin(dhpar(j,1))   dhpar(j,2)*sin(dhpar(j,3))\n          0                 sin(dhpar(j,1))       "
          "              cos(dhpar(j,1))                    dhpar(j,4)\n          0                 0                      "
          "             0                                  1                          ];\n    trans(:,:,j) = t;\nend\n\n%%%"
          "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% fkine\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\nT = base;\nfor i=1:n_joints,\n	"
          "T = T * trans(:,:,i);\nend\n\nT44_fkine = reshape(T',16,1);\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% tr2rot\n%%"
          "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\nR = T(1:3,1:3);\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% J0 + Jdot0\n%%%%%%"
          "%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\nJ0_tmp = zeros(6,n_joints);\n\nJ_dot = zeros(6,n_joints);\nomega = zeros(3,n_joi"
          "nts);\nnonZeroOmega = zeros(n_joints,1);\n\nT44_FK = eye(4);\n\nfor k=1:n_joints\n\n    if dhpar(j,5) == 0\n		% "
          "revolute axis\n        omega(:,k) = T44_FK(1:3,3);\n        nonZeroOmega(k) = 1;\n        J0_tmp(4:6,k) = omega("
          ":,k);\n    end\n    p_prev = T44_FK(1:3,4);\n    T44_FK = T44_FK*trans(:,:,k);\n    r_tilde = T44_FK(1:3,4)-p_pr"
          "ev;\n    \n    for i=1:n_joints\n        if nonZeroOmega(i)\n            J0_tmp(1:3,i) = J0_tmp(1:3,i) + cross(o"
          "mega(:,i),r_tilde);\n            for j=1:n_joints\n                if nonZeroOmega(j)\n                    contr"
          "ibution = cross(omega(:,i),cross(omega(:,j)*qd(j),r_tilde));\n                    J_dot(1:3,i) = J_dot(1:3,i) + "
          "contribution;\n                end\n            end\n        end\n    end\n\n    if dhpar(j,5) == 1\n		% prismat"
          "ic axis\n        prismAxis = T44_FK(1:3,3);\n        J0_tmp(1:3,k) = prismAxis;\n        for i=1:n_joints\n     "
          "       contribution = cross(omega(:,i),prismAxis);\n            J_dot(1:3,k) = J_dot(1:3,k) + contribution*qd(i)"
          ";\n            J_dot(1:3,i) = J_dot(1:3,i) + contribution*qd(k);\n        end\n    end\nend\nJ0_tmp = J0_tmp(:,["
          "1 2 4 5 6 7 3]);  % Switch to wierd joint order\nJ0 = reshape(J0_tmp', 6*n_joints,1);\n\nJdot0 = reshape(J_dot',"
          " 6*n_joints,1);\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% Jn\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n\nJn_tmp = ["
          "R' zeros(3);zeros(3) R']*J0_tmp;\nJn = reshape(Jn_tmp', 6*n_joints,1);\n"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    4
        position		    [23.5747 49.5747 7]
        chart		    2
        linkNode		    [2 0 0]
        subviewer		    2
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    5
        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		      4
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    2
        linkNode		    [2 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    2
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    6
        ssIdNumber		    4
        name		    "q"
        linkNode		    [2 0 7]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    7
        ssIdNumber		    5
        name		    "T44_fkine"
        linkNode		    [2 6 8]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    8
        ssIdNumber		    6
        name		    "qd"
        linkNode		    [2 7 9]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    9
        ssIdNumber		    7
        name		    "J0"
        linkNode		    [2 8 10]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    10
        ssIdNumber		    8
        name		    "Jn"
        linkNode		    [2 9 11]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    11
        ssIdNumber		    9
        name		    "Jdot0"
        linkNode		    [2 10 0]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    12
        name		    "botharms_get_torque/FRIDA arm acceleration NOT SI/frida_kin"
        machine		    1
        chart		    2
      }
      chart {
        id			    13
        name		    "Left/calcGravityTorque/calcGravTrq"
        windowPosition	    [390.75 188.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 14 0 0]
        firstTransition	    16
        firstJunction	    15
        viewObj		    13
        machine		    1
        ssIdHighWaterMark	    16
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    17
        chartFileNumber	    2
        disableImplicitCasting  1
        eml {
          name		      "fcn"
        }
      }
      state {
        id			    14
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    13
        treeNode		    [13 0 0 0]
        superState		    SUBCHART
        subviewer		    13
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function [gravTrq,gravTrqMotor,coulombWidthMotor, coulombWidth]  = fcn(q_motor,x_hat,gearRatio,ba"
          "seAngles)\n    \n%     gearRatio = [100;100;-100;100;-101;100;100];   \n    \n    q_arm_ABBorder = q_motor./gear"
          "Ratio;\n    \n\n    \n    gravTrqLogicalOrder = calcTrqFromParams( x_hat,q_arm_ABBorder,baseAngles);\n    %logic"
          "al ordering\n    \n    gravTrq = [gravTrqLogicalOrder(1:2) ; gravTrqLogicalOrder(4:7) ; gravTrqLogicalOrder(3)];"
          "\n    %ABB-order\n    \n%     gearRatio = [100;100;-100;100;-101;100;100];\n    \n    gravTrqMotor = gravTrq./ge"
          "arRatio;\n    \n    coulombWidthLogicalOrder = x_hat(29:35);\n    coulombWidth = [coulombWidthLogicalOrder(1:2) "
          "; coulombWidthLogicalOrder(4:7) ; coulombWidthLogicalOrder(3)];\n    coulombWidthMotor = coulombWidth./abs(gearR"
          "atio);\n\nend\n\nfunction [ trq ] = calcTrqFromParams( x_hat,q,baseAngles)\n%CALCTRQFROMPARAMS Summary of this f"
          "unction goes here\n%   Detailed explanation goes here\n\n    A = calcA_FRIDA(q,-9.82,baseAngles);\n    \n    \n "
          "   trq = A*x_hat(1:28,1);\n\nend\n\nfunction [A] = calcA_FRIDA(q,g,baseAngles)\n    % q should be given in ABB-o"
          "rder!!!\n    % Offsets are already handled in the equations!!!\n\n    c1 = cos(q(1));\n    s1 = sin(q(1));\n    "
          "c2 = cos(q(2));\n    s2 = sin(q(2));\n    c3 = cos(q(7));\n    s3 = sin(q(7));\n    c4 = cos(q(3));\n    s4 = si"
          "n(q(3));\n    c5 = cos(q(4));\n    s5 = sin(q(4));\n    c6 = cos(q(5));\n    s6 = sin(q(5));\n    c7 = cos(q(6))"
          ";\n    s7 = sin(q(6));\n\n    sb1 = sin(baseAngles(1));\n    cb1 = cos(baseAngles(1));\n    sb2 = sin(baseAngles"
          "(2));\n    cb2 = cos(baseAngles(2));\n    sb3 = sin(baseAngles(3));\n    cb3 = cos(baseAngles(3));\n\n    should"
          "erLen =   0.110;\n    shoulderOffs = -0.030;\n    elbowOffs =     0.0405;\n    wristOffs =     0.0135;\n    hand"
          "Offs =     -0.027;\n    upArmLen =      0.2465;\n    lowArmLen =     0.265;\n    handLen =       0.032;\n\n    A"
          " = zeros(7,28);\n\n    p0=g*cb1;\n    p1=g*sb1;\n    p2=sb2*p0;\n    p3=s1*cb3;\n    p4=c2*c3;\n    p5=cb2*p0;\n"
          "    p6=c1*cb3;\n    p7=c1*sb3;\n    p8=s1*sb3;\n    p9=s4*p1;\n    p10=s4*p2;\n    p11=c5*c6;\n    p12=c4*p1;\n "
          "   p13=s1*sb3;\n    p14=c4*p2;\n    p15=s3*c2;\n    p16=c7^2;\n    p17=s7^2;\n    p18=s5*c6;\n    p19=s2*s6;\n  "
          "  p20=s3*s6;\n    p21=s2*c3;\n    p22=c6^2;\n    p23=s6*p4;\n    p24=p3*p9;\n    p25=p6*p10;\n    p26=s4*p5;\n  "
          "  p27=c3*p1;\n    p28=p7*p9;\n    p29=p8*p10;\n    p30=p3*p12;\n    p31=p6*p14;\n    p32=c4*p5;\n    p33=c3*p2;\n"
          "    p34=p1*p15;\n    p35=s6^2;\n    p36=p7*p12;\n    p37=p8*p14;\n    p38=c5*wristOffs;\n    p39=p2*p15;\n    p4"
          "0=s2*s3;\n    p41=p3*p10;\n    p42=p6*p9;\n    p43=p7*p10;\n    p44=c7*p11;\n    p45=s3*s5;\n    p46=p3*p14;\n  "
          "  p47=c5*p19;\n    p48=p5*p40;\n    p49=p9*p13;\n    p50=s7*p11;\n    p51=p6*p12;\n    p52=p7*p14;\n    p53=s5*s"
          "6;\n    p54=p1*p3;\n    p55=c3*p19;\n    p56=p8*p12;\n    p57=handOffs*p11;\n    p58=s5*p4;\n    p59=s6*c2;\n   "
          " p60=p1*p7;\n    p61=p2*p6;\n    p62=p3*p33;\n    p63=p2*p8;\n    p64=s2*s5;\n    p65=c5*c7;\n    p66=elbowOffs*"
          "p4;\n    p67=p6*p27;\n    p68=p7*p33;\n    p69=c2*p32;\n    p70=s7*c5;\n    p71=c7*p18;\n    p72=s2*c6;\n    p73"
          "=s3*elbowOffs;\n    p74=p8*p27;\n    p75=p1*p6;\n    p76=p2*p3;\n    p77=s5*wristOffs;\n    p78=c2*shoulderOffs;"
          "\n    p79=s7*p18;\n    p80=c6*p4;\n    p81=p11*p17;\n    p82=p1*p8;\n    p83=p2*p7;\n    p84=p3*p34;\n    p85=p6"
          "*p39;\n    p86=c5*p23;\n    p87=elbowOffs*p21;\n    p88=s5*c7;\n    p89=wristOffs*p45;\n    p90=handOffs*p18;\n "
          "   p91=s6*c6;\n    p92=p11*p16;\n    p93=p7*p34;\n    p94=p8*p39;\n    p95=c5*p20;\n    p96=s5*s7;\n    p97=p21*"
          "p26;\n    p98=s2*elbowOffs;\n    p99=wristOffs*p58;\n    p100=p3*p27;\n    p101=s3*c6;\n    p102=s6*c5;\n    p10"
          "3=p16*p18;\n    p104=c2*p26;\n    p105=handOffs*p35;\n    p106=s2*p30;\n    p107=p21*p32;\n    p108=p7*p27;\n   "
          " p109=p6*p34;\n    p110=p3*p39;\n    p111=p17*p18;\n    p112=c5*p4;\n    p113=s2*p36;\n    p114=p8*p34;\n    p11"
          "5=p6*p33;\n    p116=p7*p39;\n    p117=p26*p40;\n    p118=p32*p55;\n    p119=s3*c5;\n    p120=s2*p31;\n    p121=p"
          "4*p24;\n    p122=p16*p20;\n    p123=p8*p33;\n    p124=p40*p54;\n    p125=s2*p37;\n    p126=p4*p25;\n    p127=s3*"
          "p42;\n    p128=p17*p20;\n    p129=p16*p19;\n    p130=p15*p24;\n    p131=c5*p59;\n    p132=p40*p60;\n    p133=s2*"
          "upArmLen;\n    p134=s3*p49;\n    p135=p4*p28;\n    p136=p17*p19;\n    p137=c3*p42;\n    p138=s5*p20;\n    p139=p"
          "40*p61;\n    p140=p26*p59;\n    p141=s2*shoulderOffs;\n    p142=s3*p41;\n    p143=p4*p29;\n    p144=p17*p23;\n  "
          "  p145=c3*p49;\n    p146=p15*p28;\n    p147=handOffs*p19;\n    p148=c2*upArmLen;\n    p149=c6*p11;\n    p150=p40"
          "*p63;\n    p151=p26*p55;\n    p152=s3*p43;\n    p153=p16*p23;\n    p154=s2*p38;\n    p155=c3*p41;\n    p156=p15*"
          "p25;\n    p157=c2*p30;\n    p158=handOffs*p22;\n    p159=c6*p57;\n    p160=p17*p50;\n    p161=c7*p64;\n    p162="
          "p4*p38;\n    p163=c3*p43;\n    p164=p15*p29;\n    p165=s5*p23;\n    p166=c2*p36;\n    p167=p22*p92;\n    p168=p1"
          "6*p44;\n    p169=s7*p64;\n    p170=c7*p45;\n    p171=s2*lowArmLen;\n    p172=s3*p38;\n    p173=c3*p47;\n    p174"
          "=c2*p31;\n    p175=p22*p81;\n    p176=p16*p50;\n    p177=p17*p44;\n    p178=handOffs*p23;\n    p179=s7*p45;\n   "
          " p180=c7*p58;\n    p181=lowArmLen*p4;\n    p182=s6*c3;\n    p183=c2*p37;\n    p184=s7*p58;\n    p185=handOffs*p2"
          "0;\n    p186=p19*p45;\n    p187=s6*p15;\n    p188=s7*p19;\n    p189=p5*p15;\n    p190=s6*p92;\n    p191=c7*p19;\n"
          "    p192=p5*p21;\n    p193=s6*p81;\n    p194=p21*p24;\n    p195=p21*p28;\n    p196=p21*p25;\n    p197=p21*p29;\n"
          "    A(1,1) = -shoulderOffs*p75+shoulderOffs*p82-shoulderOffs*p76-shoulderOffs*p83;\n    A(1,2) = -shoulderOffs*p"
          "75+shoulderOffs*p82+p78*p75-p78*p82-shoulderOffs*p76-shoulderOffs*p83+p78*p76+p78*p83;\n    A(1,3) = -shoulderOf"
          "fs*p75+shoulderOffs*p82+p78*p75+p54*p73+p60*p73-p75*p133-p78*p82+p82*p133-shoulderOffs*p76-shoulderOffs*p83+p66*"
          "p82-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p133-p66*p75-p66*p76-p66*p83;\n    A(1,4) = -shoulderOffs*p75+s"
          "houlderOffs*p82+p78*p75+p54*p73+p60*p73-p75*p133-p78*p82+p82*p133-shoulderOffs*p76-shoulderOffs*p83+p66*p82+p24*"
          "p73+p56*p98+p28*p73-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p133-p66*p75-p51*p98-p66*p76-p66*p83-p46*p98-p2"
          "5*p73-p52*p98+p29*p73-p42*p66+p49*p66-p41*p66-p43*p66;\n    A(1,5) = -shoulderOffs*p75+shoulderOffs*p82+p78*p75+"
          "p54*p73+p60*p73-p75*p133-p78*p82+p82*p133-shoulderOffs*p76-shoulderOffs*p83+p66*p82+p42*p171+s3*lowArmLen*p30+s3"
          "*lowArmLen*p36+p24*p73+p56*p98+p28*p73+p77*p100+p77*p108-p49*p171-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p"
          "133-p66*p75-p51*p98-p24*p172-p56*p154-p28*p172-p77*p114-p66*p76-p66*p83-s3*lowArmLen*p31-p46*p98-p25*p73-p52*p98"
          "-p77*p115+p41*p171+p43*p171+s3*lowArmLen*p37+p29*p73+p77*p123-p51*p181-p42*p66+p51*p154+p56*p181+p49*p66+p77*p10"
          "9-p46*p181-p52*p181-p41*p66-p43*p66+p46*p154+p25*p172+p52*p154+p77*p110+p77*p116-p29*p172+p42*p162-p49*p162+p41*"
          "p162+p43*p162;\n    A(1,6) = -shoulderOffs*p75+shoulderOffs*p82+p78*p75+p54*p73+p60*p73-p75*p133-p78*p82+p82*p13"
          "3-shoulderOffs*p76-shoulderOffs*p83+p66*p82+p42*p171+s3*lowArmLen*p30+s3*lowArmLen*p36+p24*p73+p56*p98+p28*p73+p"
          "77*p100+p77*p108-p49*p171-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p133-p66*p75-p51*p98-p24*p172-p56*p154-p2"
          "8*p172-p77*p114+p49*p147-p66*p76-p66*p83-s3*lowArmLen*p31-p46*p98-p25*p73-p52*p98-p77*p115+p41*p171+p43*p171+s3*"
          "lowArmLen*p37+p29*p73+p77*p123-p51*p181-p42*p66+p51*p154+p100*p90+p90*p108+p56*p181+p49*p66+p77*p109-p42*p147-p3"
          "0*p185-p36*p185-p90*p115-p46*p181-p52*p181-p41*p66-p43*p66+p31*p185+p46*p154+p25*p172+p52*p154+p90*p123+p77*p110"
          "+p77*p116-p41*p147-p43*p147-p37*p185-p29*p172+p51*p178+s2*p51*p57+p42*p162+p90*p109-s3*p24*p57-p56*p178-s2*p57*p"
          "56-s3*p28*p57-p49*p162-p90*p114+p46*p178+s2*p46*p57+s3*p25*p57+p52*p178+s2*p52*p57+p41*p162+p43*p162+p90*p110+p9"
          "0*p116-s3*p29*p57+p4*p42*p57-p4*p49*p57+p4*p41*p57+p4*p43*p57;\n    A(1,7) = -p75+p82-p76-p83;\n    A(1,9) = +p5"
          "4+p60-p61+p63;\n    A(1,10) = -c2*p75+c2*p82-c2*p76-c2*p83;\n    A(1,11) = +p54+p60-p61+p63;\n    A(1,12) = -s2*"
          "p75+s2*p82-s2*p76-s2*p83;\n    A(1,13) = +s3*p54+s3*p60+p4*p82-s3*p61+s3*p63-p4*p75-p4*p76-p4*p83;\n    A(1,14) "
          "= +s2*p75-s2*p82+s2*p76+s2*p83;\n    A(1,15) = +p100+p108+p109-p114-p115+p123+p110+p116;\n    A(1,16) = +s2*p51-"
          "s3*p24-s2*p56-s3*p28+s2*p46+s3*p25+s2*p52-s3*p29+p4*p42-p4*p49+p4*p41+p4*p43;\n    A(1,17) = +p100+p108+p109-p11"
          "4-p115+p123+p110+p116;\n    A(1,18) = +s2*p42+s3*p30+s3*p36-s2*p49-s3*p31+s2*p41+s2*p43+s3*p37-p4*p51+p4*p56-p4*"
          "p46-p4*p52;\n    A(1,19) = +s5*p100+s5*p108-s5*p114-s5*p115+s5*p123+s2*c5*p51+s5*p109-p24*p119-s2*c5*p56-p28*p11"
          "9+s2*c5*p46+p25*p119+s2*c5*p52+s5*p110+s5*p116-p29*p119+p42*p112-p49*p112+p41*p112+p43*p112;\n    A(1,20) = -s2*"
          "p42-s3*p30-s3*p36+s2*p49+s3*p31-s2*p41-s2*p43-s3*p37+p4*p51-p4*p56+p4*p46+p4*p52;\n    A(1,21) = +c5*p100+c5*p10"
          "8+p24*p45+p56*p64+p28*p45-c5*p115+c5*p123+c5*p109-p51*p64-c5*p114+c5*p110+c5*p116-p46*p64-p25*p45-p52*p64+p29*p4"
          "5-p42*p58+p49*p58-p41*p58-p43*p58;\n    A(1,22) = +p19*p49+p18*p100+p18*p108-p19*p42-p20*p30-p20*p36-p18*p115+p2"
          "0*p31+p18*p123-p19*p41-p19*p43-p20*p37+p23*p51+s2*p11*p51+p18*p109-s3*p11*p24-p23*p56-s2*p11*p56-s3*p11*p28-p18*"
          "p114+p23*p46+s2*p11*p46+s3*p11*p25+p23*p52+s2*p11*p52+p18*p110+p18*p116-s3*p11*p29+p4*p11*p42-p4*p11*p49+p4*p11*"
          "p41+p4*p11*p43;\n    A(1,23) = +c5*p100+c5*p108+p24*p45+p56*p64+p28*p45-c5*p115+c5*p123+c5*p109-p51*p64-c5*p114+"
          "c5*p110+c5*p116-p46*p64-p25*p45-p52*p64+p29*p45-p42*p58+p49*p58-p41*p58-p43*p58;\n    A(1,24) = -p49*p72+p42*p72"
          "+p30*p101+p36*p101+p53*p100+p53*p108-p31*p101-p53*p115+p41*p72+p43*p72+p37*p101+p53*p123-p51*p80+p47*p51+p56*p80"
          "+p53*p109-p24*p95-p47*p56-p28*p95-p53*p114-p46*p80-p52*p80+p47*p46+p25*p95+p47*p52+p53*p110+p53*p116-p29*p95+p42"
          "*p86-p49*p86+p41*p86+p43*p86;\n    A(1,25) = +p70*p100+p70*p108-p70*p115+p70*p123+p70*p109+p71*p100+p71*p108-p51"
          "*p169-p42*p191-c7*p20*p30-c7*p20*p36-p70*p114+p24*p179+p56*p169+p28*p179+p49*p191-p71*p115+c7*p20*p31+p70*p110+p"
          "70*p116+p71*p123-p46*p169-p25*p179-p41*p191-p52*p169-p43*p191-c7*p20*p37+p29*p179+c7*p23*p51+s2*p44*p51+p71*p109"
          "-p42*p184-s3*p24*p44-c7*p23*p56-s2*p44*p56-s3*p28*p44-p71*p114+p49*p184+p71*p110+p71*p116-p41*p184-p43*p184-s3*p"
          "29*p44+p4*p44*p42-p4*p44*p49+c7*p23*p46+s2*p46*p44+s3*p25*p44+c7*p23*p52+s2*p44*p52+p4*p41*p44+p4*p44*p43;\n    "
          "A(1,26) = +p65*p100+p65*p108-p49*p188-p65*p115+p65*p123+p65*p109-p51*p161-p79*p100-p65*p114-p79*p108+p42*p188+p2"
          "4*p170+s7*p20*p30+p56*p161+p28*p170+s7*p20*p36+p65*p110+p79*p115+p65*p116-p46*p161-p25*p170-s7*p20*p31-p52*p161-"
          "p79*p123+p41*p188+p43*p188+p29*p170+s7*p20*p37-p42*p180-s7*p23*p51-s2*p51*p50-p79*p109+s3*p24*p50+p49*p180+s7*p2"
          "3*p56+s2*p50*p56+s3*p28*p50+p79*p114-p41*p180-s7*p23*p46-s2*p46*p50-s3*p25*p50-p43*p180-s7*p23*p52-s2*p52*p50-p7"
          "9*p110-p79*p116+s3*p29*p50-p4*p42*p50+p4*p50*p49-p4*p41*p50-p4*p43*p50;\n    A(1,27) = -p49*p72+p42*p72+p30*p101"
          "+p36*p101+p53*p100+p53*p108-p31*p101-p53*p115+p41*p72+p43*p72+p37*p101+p53*p123-p51*p80+p47*p51+p56*p80+p53*p109"
          "-p24*p95-p47*p56-p28*p95-p53*p114-p46*p80-p52*p80+p47*p46+p25*p95+p47*p52+p53*p110+p53*p116-p29*p95+p42*p86-p49*"
          "p86+p41*p86+p43*p86;\n\n    A(2,2) = -p5*p78-p54*p141-p60*p141+p61*p141-p63*p141;\n    A(2,3) = -p5*p78+p5*p133+"
          "p5*p66-p54*p148-p60*p148-p54*p141-p60*p141+p54*p87+p60*p87+p61*p148+p61*p141-p63*p148-p63*p141-p61*p87+p63*p87;\n"
          "    A(2,4) = -p5*p78+p5*p133+p5*p66+p32*p98-p54*p148-p60*p148-p54*p141-p60*p141+p54*p87+p60*p87+p61*p148+p61*p14"
          "1-p63*p148-p63*p141+p26*p66-elbowOffs*p157-elbowOffs*p166+elbowOffs*p174-p61*p87-elbowOffs*p183+p63*p87+p24*p87+"
          "p28*p87-p25*p87+p29*p87;\n    A(2,5) = -p5*p78+p5*p133+p5*p66+p32*p98-p26*p171-p54*p148-p60*p148-p54*p141-p60*p1"
          "41+p54*p87+p60*p87-p77*p189+c2*lowArmLen*p24+c2*lowArmLen*p28+p61*p148+p61*p141-p63*p148-p63*p141+p32*p181+p26*p"
          "66-elbowOffs*p157-elbowOffs*p166-p32*p154-p77*p124-p77*p132+elbowOffs*p174-p61*p87-c2*lowArmLen*p25-elbowOffs*p1"
          "83+p63*p87+c2*lowArmLen*p29-p26*p162+p38*p157+p38*p166+lowArmLen*p21*p30+lowArmLen*p21*p36+p24*p87+p28*p87-p38*p"
          "174-lowArmLen*p21*p31-p25*p87+p38*p183+lowArmLen*p21*p37+p29*p87+p77*p139-p77*p150-p38*p194-p38*p195+p38*p196-p3"
          "8*p197;\n    A(2,6) = -p5*p78+p5*p133+p5*p66+p32*p98-p26*p171-p54*p148-p60*p148-p54*p141-p60*p141+p54*p87+p60*p8"
          "7-p77*p189+p26*p147+c2*lowArmLen*p24+c2*lowArmLen*p28+p61*p148+p61*p141-p63*p148-p63*p141+p32*p181+p26*p66-elbow"
          "Offs*p157-elbowOffs*p166-p32*p154-p77*p124-p77*p132+elbowOffs*p174-p61*p87-c2*lowArmLen*p25-elbowOffs*p183+p63*p"
          "87+c2*lowArmLen*p29-p32*p178-s2*p32*p57-p26*p162-p90*p189+p38*p157+p38*p166+lowArmLen*p21*p30+lowArmLen*p21*p36+"
          "p24*p87+p28*p87-handOffs*p24*p59-handOffs*p28*p59-p38*p174-lowArmLen*p21*p31-p25*p87+handOffs*p25*p59+p38*p183+l"
          "owArmLen*p21*p37+p29*p87+p77*p139-handOffs*p29*p59-p77*p150-p4*p26*p57+p57*p157+p57*p166-handOffs*p30*p55-handOf"
          "fs*p36*p55-p38*p194-p38*p195-p90*p124-p90*p132+handOffs*p31*p55+p57*p183+p38*p196+p90*p139-handOffs*p37*p55-p38*"
          "p197-p90*p150-p57*p194-p57*p195-p57*p174-p57*p197+p57*p196;\n    A(2,10) = +c2*p5+s2*p54+s2*p60-s2*p61+s2*p63;\n"
          "    A(2,12) = +s2*p5-c2*p54-c2*p60+c2*p61-c2*p63;\n    A(2,13) = +p5*p4+p21*p54+p21*p60-p21*p61+p21*p63;\n    A("
          "2,14) = -s2*p5+c2*p54+c2*p60-c2*p61+c2*p63;\n    A(2,15) = -p189-p124-p132+p139-p150;\n    A(2,16) = -s2*p32+p15"
          "7+p166-p4*p26-p174+p183-p194-p195+p196-p197;\n    A(2,17) = -p189-p124-p132+p139-p150;\n    A(2,18) = -s2*p26+c2"
          "*p24+c2*p28+p4*p32-c2*p25+c2*p29+p21*p30+p21*p36-p21*p31+p21*p37;\n    A(2,19) = -s5*p189-s2*c5*p32-s5*p124-s5*p"
          "132-p26*p112+c5*p157+c5*p166-c5*p174+c5*p183+s5*p139-s5*p150-c5*p194-c5*p195+c5*p196-c5*p197;\n    A(2,20) = +s2"
          "*p26-c2*p24-c2*p28-p4*p32+c2*p25-c2*p29-p21*p30-p21*p36+p21*p31-p21*p37;\n    A(2,21) = +p32*p64-c5*p189+p26*p58"
          "-s5*p157-s5*p166-c5*p124-c5*p132+s5*p174+c5*p139-s5*p183-c5*p150+s5*p194+s5*p195-s5*p196+s5*p197;\n    A(2,22) ="
          " +p19*p26-p23*p32-s2*p11*p32-p18*p189-p24*p59-p28*p59+p25*p59-p29*p59-p4*p11*p26+p11*p157+p11*p166-p30*p55-p36*p"
          "55-p18*p124-p18*p132-p11*p174+p31*p55+p11*p183+p18*p139-p37*p55-p18*p150-p11*p194-p11*p195-p11*p197+p11*p196;\n "
          "   A(2,23) = +p32*p64-c5*p189+p26*p58-s5*p157-s5*p166-c5*p124-c5*p132+s5*p174+c5*p139-s5*p183-c5*p150+s5*p194+s5"
          "*p195-s5*p196+s5*p197;\n    A(2,24) = -p26*p72+p32*p80-p32*p47-p53*p189+c2*c6*p24+c2*c6*p28-c2*c6*p25+c2*c6*p29-"
          "p26*p86+c6*p21*p30+p30*p131+c6*p21*p36+p36*p131-p54*p186-p60*p186-c6*p21*p31-p31*p131+c6*p21*p37+p37*p131+p61*p1"
          "86-p63*p186-p24*p173-p28*p173+p25*p173-p29*p173;\n    A(2,25) = -p70*p189+p32*p169+p26*p191-c7*p23*p32-s2*p32*p4"
          "4-p71*p189+p26*p184-p96*p157-c7*p24*p59-p96*p166-c7*p28*p59-p70*p124-p70*p132+p96*p174+c7*p25*p59+p70*p139-p96*p"
          "183-c7*p29*p59-p70*p150-p4*p26*p44+p44*p157+p44*p166-c7*p30*p55-c7*p36*p55-p71*p124-p71*p132+p96*p194+p96*p195+p"
          "71*p139-p96*p196-c7*p37*p55-p71*p150+p96*p197-p44*p194-p44*p195-p44*p174+c7*p31*p55+p44*p183+p44*p196-p44*p197;\n"
          "    A(2,26) = -p65*p189+p32*p161-p26*p188+p26*p180+s7*p23*p32+s2*p32*p50+p79*p189-p88*p157-p88*p166-p65*p124-p65"
          "*p132+s7*p24*p59+s7*p28*p59+p88*p174+p65*p139-s7*p25*p59-p88*p183-p65*p150+s7*p29*p59+p4*p26*p50-p50*p157-p50*p1"
          "66+p88*p194+s7*p30*p55+p88*p195+s7*p36*p55+p79*p124+p79*p132-p88*p196-s7*p31*p55-p50*p183-p79*p139+p88*p197+s7*p"
          "37*p55+p79*p150+p50*p194+p50*p195+p50*p174-p50*p196+p50*p197;\n    A(2,27) = -p26*p72+p32*p80-p32*p47-p53*p189+c"
          "2*c6*p24+c2*c6*p28-c2*c6*p25+c2*c6*p29-p26*p86+c6*p21*p30+p30*p131+c6*p21*p36+p36*p131-p54*p186-p60*p186-c6*p21*"
          "p31-p31*p131+c6*p21*p37+p37*p131+p61*p186-p63*p186-p24*p173-p28*p173+p25*p173-p29*p173;\n\n    A(3,3) = -elbowOf"
          "fs*p48-elbowOffs*p67+elbowOffs*p74+elbowOffs*p84+elbowOffs*p93-elbowOffs*p62-elbowOffs*p68-elbowOffs*p85+elbowOf"
          "fs*p94;\n    A(3,4) = -elbowOffs*p67-elbowOffs*p48+elbowOffs*p74-elbowOffs*p117+elbowOffs*p84+elbowOffs*p93+elbo"
          "wOffs*p145-elbowOffs*p62-elbowOffs*p68-elbowOffs*p137-elbowOffs*p85-elbowOffs*p155-elbowOffs*p163+elbowOffs*p94+"
          "elbowOffs*p130+elbowOffs*p146-elbowOffs*p156+elbowOffs*p164;\n    A(3,5) = -elbowOffs*p67-elbowOffs*p48+elbowOff"
          "s*p74-lowArmLen*p32*p40-elbowOffs*p117+elbowOffs*p84+elbowOffs*p93-p77*p192+c3*lowArmLen*p56+elbowOffs*p145+p75*"
          "p89-p89*p82-elbowOffs*p62-elbowOffs*p68-c3*lowArmLen*p51-elbowOffs*p137-p38*p145-elbowOffs*p85-c3*lowArmLen*p46-"
          "c3*lowArmLen*p52-elbowOffs*p155-elbowOffs*p163+elbowOffs*p94+p76*p89+p89*p83+p38*p137+lowArmLen*p15*p30+lowArmLe"
          "n*p15*p36+elbowOffs*p130+elbowOffs*p146+p38*p117+p54*p99+p60*p99-lowArmLen*p15*p31-elbowOffs*p156-p61*p99+p38*p1"
          "55+p38*p163+lowArmLen*p15*p37+elbowOffs*p164+p63*p99-p38*p130-p38*p146+p38*p156-p38*p164;\n    A(3,6) = -elbowOf"
          "fs*p67-elbowOffs*p48+elbowOffs*p74-lowArmLen*p32*p40-elbowOffs*p117+elbowOffs*p84+elbowOffs*p93-p77*p192+c3*lowA"
          "rmLen*p56+elbowOffs*p145+p75*p89-p89*p82-elbowOffs*p62-elbowOffs*p68-c3*lowArmLen*p51-elbowOffs*p137-p38*p145-s3"
          "*p82*p90-elbowOffs*p85-c3*lowArmLen*p46-c3*lowArmLen*p52-elbowOffs*p155-elbowOffs*p163+elbowOffs*p94+p76*p89+p89"
          "*p83-p90*p192+handOffs*p51*p182+p38*p137+s3*p32*p147+s3*p75*p90+lowArmLen*p15*p30+lowArmLen*p15*p36+elbowOffs*p1"
          "30+elbowOffs*p146+p38*p117+p54*p99+p60*p99-handOffs*p56*p182-lowArmLen*p15*p31-elbowOffs*p156-p61*p99+handOffs*p"
          "46*p182+handOffs*p52*p182+p38*p155+p38*p163+s3*p76*p90+s3*p83*p90+lowArmLen*p15*p37+elbowOffs*p164+p63*p99+p57*p"
          "137+p57*p117+p4*p54*p90+p4*p60*p90-handOffs*p30*p187-handOffs*p36*p187-p57*p145-p38*p130-p38*p146-p4*p61*p90+han"
          "dOffs*p31*p187+p57*p155+p57*p163+p38*p156+p4*p63*p90-handOffs*p37*p187-p38*p164-p57*p130-p57*p146-p57*p164+p57*p"
          "156;\n    A(3,13) = -p67-p48+p74+p84+p93-p62-p68-p85+p94;\n    A(3,15) = +s3*p75-p192-s3*p82+p4*p54+p4*p60+s3*p7"
          "6+s3*p83-p4*p61+p4*p63;\n    A(3,16) = +p137+p117-p145+p155+p163-p130-p146+p156-p164;\n    A(3,17) = -p192+s3*p7"
          "5-s3*p82+p4*p54+p4*p60+s3*p76+s3*p83-p4*p61+p4*p63;\n    A(3,18) = -p32*p40+c3*p56-c3*p51-c3*p46-c3*p52+p15*p30+"
          "p15*p36-p15*p31+p15*p37;\n    A(3,19) = -s5*p192+p45*p75-p45*p82+p45*p76+p45*p83+c5*p137+c5*p117+p54*p58+p60*p58"
          "-c5*p145-p61*p58+c5*p155+c5*p163+p58*p63-c5*p130-c5*p146+c5*p156-c5*p164;\n    A(3,20) = +p32*p40-c3*p56+c3*p51+"
          "c3*p46+c3*p52-p15*p30-p15*p36+p15*p31-p15*p37;\n    A(3,21) = +p75*p119-p82*p119-c5*p192+s5*p145+p76*p119+p83*p1"
          "19+p54*p112+p60*p112-s5*p137-s5*p117-p61*p112+p63*p112-s5*p155-s5*p163+s5*p130+s5*p146-s5*p156+s5*p164;\n    A(3"
          ",22) = -s3*p18*p82-p18*p192+p51*p182+s3*p19*p32+s3*p18*p75-p56*p182+p46*p182+p52*p182+s3*p18*p76+s3*p18*p83+p11*"
          "p137+p11*p117+p4*p18*p54+p4*p18*p60-p30*p187-p36*p187-p11*p145-p4*p18*p61+p31*p187+p11*p155+p11*p163+p4*p18*p63-"
          "p37*p187-p11*p130-p11*p146-p11*p164+p11*p156;\n    A(3,23) = +p75*p119-p82*p119-c5*p192+s5*p145+p76*p119+p83*p11"
          "9+p54*p112+p60*p112-s5*p137-s5*p117-p61*p112+p63*p112-s5*p155-s5*p163+s5*p130+s5*p146-s5*p156+s5*p164;\n    A(3,"
          "24) = -p82*p138-c3*c6*p51-c6*p32*p40-s5*p5*p55+c3*c6*p56+p75*p138-c3*c6*p46-c3*c6*p52+p76*p138+p83*p138+c6*p15*p"
          "30+p102*p137+c6*p15*p36+s3*p26*p47+p54*p165+p60*p165-p102*p145-c6*p15*p31-p61*p165+p102*p155+c6*p15*p37+p102*p16"
          "3+p63*p165-p102*p130-p102*p146+p102*p156-p102*p164;\n    A(3,25) = -s3*p70*p82-p70*p192+s3*p75*p70+s3*p76*p70+s3"
          "*p70*p83-p71*p192+c7*p51*p182+s3*p32*p191+p4*p54*p70+s3*p75*p71+p4*p60*p70-p96*p137-c7*p56*p182-p96*p117-s3*p71*"
          "p82+p96*p145-p4*p61*p70+c7*p46*p182+c7*p52*p182+s3*p76*p71+p4*p63*p70+s3*p71*p83-p96*p155-p96*p163+p44*p137+p44*"
          "p117+p4*p54*p71+p4*p60*p71-c7*p30*p187-c7*p36*p187-p44*p145+p96*p130+p96*p146+p4*p63*p71-p96*p156-c7*p37*p187+p9"
          "6*p164-p44*p130-p44*p146-p4*p61*p71+c7*p31*p187+p44*p155+p44*p163+p44*p156-p44*p164;\n    A(3,26) = -p65*p192+s3"
          "*p65*p75-s3*p65*p82+s3*p65*p76+s3*p65*p83+p4*p54*p65+p4*p60*p65+p79*p192-p88*p137-s7*p51*p182-p88*p117-s3*p32*p1"
          "88-s3*p75*p79+p88*p145+s7*p56*p182+s3*p82*p79-p4*p61*p65+p4*p65*p63-p88*p155-s7*p46*p182-p88*p163-s7*p52*p182-s3"
          "*p76*p79-s3*p83*p79-p50*p137-p50*p117-p4*p54*p79-p4*p60*p79+p88*p130+s7*p30*p187+p88*p146+s7*p36*p187+p50*p145-p"
          "88*p156-s7*p31*p187-p50*p155-p50*p163-p4*p63*p79+p88*p164+s7*p37*p187+p50*p130+p50*p146+p4*p61*p79-p50*p156+p50*"
          "p164;\n    A(3,27) = -p82*p138-c3*c6*p51-c6*p32*p40-s5*p5*p55+c3*c6*p56+p75*p138-c3*c6*p46-c3*c6*p52+p76*p138+p8"
          "3*p138+c6*p15*p30+p102*p137+c6*p15*p36+s3*p26*p47+p54*p165+p60*p165-p102*p145-c6*p15*p31-p61*p165+p102*p155+c6*p"
          "15*p37+p102*p163+p63*p165-p102*p130-p102*p146+p102*p156-p102*p164;\n\n    A(4,4) = +elbowOffs*p104+p24*p98+p28*p"
          "98+p56*p73+p32*p87-p51*p73-p25*p98-p46*p73-p52*p73+p29*p98-p30*p66-p36*p66+p31*p66-p37*p66;\n    A(4,5) = +lowAr"
          "mLen*p69+elbowOffs*p104-lowArmLen*p97+lowArmLen*p106+lowArmLen*p127+lowArmLen*p113+p24*p98+p28*p98+p56*p73-lowAr"
          "mLen*p134+p32*p87-p51*p73-p38*p104-p24*p154-p28*p154-p56*p172-lowArmLen*p120-p25*p98-p46*p73-p52*p73+lowArmLen*p"
          "142+lowArmLen*p125+lowArmLen*p152+p29*p98-p30*p66-p36*p66-p38*p107+p51*p172+lowArmLen*p121+lowArmLen*p135+p31*p6"
          "6-lowArmLen*p126-p37*p66+p25*p154+p46*p172+p52*p172+lowArmLen*p143-p29*p154+p30*p162+p36*p162-p31*p162+p37*p162;"
          "\n    A(4,6) = +lowArmLen*p69+elbowOffs*p104-lowArmLen*p97+lowArmLen*p106+lowArmLen*p127+lowArmLen*p113+p24*p98+"
          "p28*p98+p56*p73-lowArmLen*p134+p32*p87-handOffs*p32*p59-p51*p73-p38*p104-p24*p154-p28*p154-p56*p172+p49*p185-low"
          "ArmLen*p120-p25*p98-p46*p73-p52*p73+lowArmLen*p142+lowArmLen*p125+lowArmLen*p152+p29*p98-p57*p104-p30*p66-p36*p6"
          "6-p38*p107+p51*p172+handOffs*p151+lowArmLen*p121+lowArmLen*p135-p30*p147-p42*p185-p36*p147+p31*p66-lowArmLen*p12"
          "6-p37*p66+p31*p147+p25*p154+p46*p172+p52*p172+lowArmLen*p143-p41*p185-p37*p147-p43*p185-p29*p154-p57*p107+s3*p51"
          "*p57+p30*p162+p36*p162-p24*p178-s2*p24*p57-p28*p178-s2*p28*p57-s3*p57*p56-p31*p162+p25*p178+s2*p25*p57+s3*p46*p5"
          "7+s3*p52*p57+p37*p162-p29*p178-s2*p29*p57+p4*p30*p57+p4*p36*p57-p4*p31*p57+p4*p37*p57;\n    A(4,16) = -p104+s3*p"
          "51-s2*p24-s2*p28-s3*p56-p107+s2*p25+s3*p46+s3*p52-s2*p29+p4*p30+p4*p36-p4*p31+p4*p37;\n    A(4,18) = +p69-p97+p1"
          "06+p127+p113-p134-p120+p142+p125+p152+p121+p135-p126+p143;\n    A(4,19) = -c5*p104-c5*p107+p51*p119-s2*c5*p24-s2"
          "*c5*p28-p56*p119+s2*c5*p25+p46*p119+p52*p119-s2*c5*p29+p30*p112+p36*p112-p31*p112+p37*p112;\n    A(4,20) = -p69+"
          "p97-p106-p127-p113+p134+p120-p142-p125-p152-p121-p135+p126-p143;\n    A(4,21) = +s5*p104+p24*p64+p28*p64+p45*p56"
          "+s5*p107-p45*p51-p25*p64-p46*p45-p45*p52+p29*p64-p30*p58-p36*p58+p31*p58-p37*p58;\n    A(4,22) = -p32*p59+p20*p4"
          "9-p11*p104+p151-p19*p30-p20*p42-p19*p36+p19*p31-p20*p41-p19*p37-p20*p43-p11*p107+s3*p11*p51-p24*p23-s2*p11*p24-p"
          "23*p28-s2*p11*p28-s3*p11*p56+p25*p23+s2*p11*p25+s3*p11*p46+s3*p11*p52-p23*p29-s2*p11*p29+p4*p11*p30+p4*p11*p36-p"
          "4*p11*p31+p4*p11*p37;\n    A(4,23) = +s5*p104+p24*p64+p28*p64+p45*p56+s5*p107-p45*p51-p25*p64-p46*p45-p45*p52+p2"
          "9*p64-p30*p58-p36*p58+p31*p58-p37*p58;\n    A(4,24) = +c6*p69-p49*p101-c6*p97-p26*p131+p30*p72+p42*p101+p36*p72-"
          "p31*p72+p41*p101+p37*p72+p43*p101-p32*p173+p24*p80+p51*p95+p28*p80-p24*p47-p28*p47-p56*p95-p25*p80+p25*p47+p46*p"
          "95+p29*p80+p52*p95-p29*p47+p30*p86+p36*p86+p37*p86-p31*p86;\n    A(4,25) = -c7*p32*p59+p96*p104-p44*p104+p96*p10"
          "7+c7*p151-p51*p179-p30*p191-c7*p20*p42-p36*p191+p24*p169+p28*p169+p56*p179+c7*p20*p49+p31*p191-p25*p169-p46*p179"
          "-c7*p20*p41-p52*p179-p37*p191-c7*p20*p43+p29*p169-p44*p107+s3*p44*p51-p30*p184-c7*p24*p23-s2*p24*p44-p36*p184-c7"
          "*p23*p28-s2*p28*p44-s3*p44*p56-p37*p184-c7*p23*p29-s2*p29*p44+p4*p30*p44+p4*p36*p44+p31*p184+c7*p25*p23+s2*p25*p"
          "44+s3*p46*p44+s3*p44*p52-p4*p31*p44+p4*p37*p44;\n    A(4,26) = +p88*p104+s7*p32*p59-s7*p20*p49+p88*p107+p50*p104"
          "-p51*p170-s7*p151+p24*p161+p30*p188+s7*p20*p42+p28*p161+p36*p188+p56*p170-p25*p161-p31*p188-p46*p170-p52*p170+s7"
          "*p20*p41+p29*p161+p37*p188+s7*p20*p43+p50*p107-p30*p180-s3*p51*p50-p36*p180+s7*p24*p23+s2*p24*p50+s7*p23*p28+s2*"
          "p28*p50+s3*p50*p56-s7*p25*p23-s2*p25*p50-s3*p46*p50-p37*p180-s3*p52*p50+s7*p23*p29+s2*p29*p50-p4*p30*p50-p4*p36*"
          "p50+p31*p180+p4*p31*p50-p4*p37*p50;\n    A(4,27) = +c6*p69-p49*p101-c6*p97-p26*p131+p30*p72+p42*p101+p36*p72-p31"
          "*p72+p41*p101+p37*p72+p43*p101-p32*p173+p24*p80+p51*p95+p28*p80-p24*p47-p28*p47-p56*p95-p25*p80+p25*p47+p46*p95+"
          "p29*p80+p52*p95-p29*p47+p30*p86+p36*p86+p37*p86-p31*p86;\n\n    A(5,5) = -p48*p38+p38*p74-p69*p77-p38*p67-wristO"
          "ffs*p30*p64-p42*p89-wristOffs*p36*p64+p49*p89-p38*p62-p38*p68+p77*p97+p38*p84+p38*p93-p38*p85+wristOffs*p31*p64+"
          "p38*p94-p41*p89-wristOffs*p37*p64-p43*p89-p24*p99-p28*p99+p25*p99-p29*p99;\n    A(5,6) = -p48*p38+p38*p74-p69*p7"
          "7-p38*p67-wristOffs*p30*p64-p42*p89-wristOffs*p36*p64+p49*p89-p38*p62-p38*p68-p69*p90-p57*p67-p48*p57+p57*p74+p7"
          "7*p97+p38*p84+p38*p93-p62*p57-p57*p68-p38*p85+wristOffs*p31*p64+p38*p94-p41*p89-wristOffs*p37*p64-p43*p89+p97*p9"
          "0+p57*p84+p57*p93-p90*p106-p90*p127-p90*p113-p24*p99-p28*p99+p90*p134-p57*p85+p90*p120+p25*p99+p57*p94-p90*p142-"
          "p90*p125-p90*p152-p29*p99-p90*p121-p90*p135-p90*p143+p90*p126;\n    A(5,19) = -c5*p48+c5*p74-s5*p69-c5*p67+p45*p"
          "49-c5*p62-c5*p68+s5*p97+c5*p84+c5*p93-p30*p64-p45*p42-p36*p64-c5*p85+p31*p64+c5*p94-p41*p45-p37*p64-p45*p43-p24*"
          "p58-p28*p58+p25*p58-p29*p58;\n    A(5,20) = 0;\n    A(5,21) = +s5*p67+s5*p48-s5*p74-c5*p69+p49*p119+s5*p62+s5*p6"
          "8+c5*p97-c5*p106-p42*p119-c5*p113-s5*p84-s5*p93+c5*p120+s5*p85-p41*p119-c5*p125-p43*p119-s5*p94-p24*p112-p28*p11"
          "2+p25*p112-p29*p112;\n    A(5,22) = -p18*p69-p11*p67-p11*p48+p11*p74-p11*p62-p11*p68+p18*p97+p11*p84+p11*p93-p18"
          "*p106-p18*p127-p18*p113+p18*p134-p11*p85+p18*p120+p11*p94-p18*p142-p18*p125-p18*p152-p18*p121-p18*p135-p18*p143+"
          "p18*p126;\n    A(5,23) = +s5*p67+s5*p48-s5*p74-c5*p69+p49*p119+s5*p62+s5*p68+c5*p97-c5*p106-p42*p119-c5*p113-s5*"
          "p84-s5*p93+c5*p120+s5*p85-p41*p119-c5*p125-p43*p119-s5*p94-p24*p112-p28*p112+p25*p112-p29*p112;\n    A(5,24) = -"
          "p53*p69-p67*p102-s3*p5*p47+p74*p102+p49*p138-p62*p102-p68*p102+s5*p151+p84*p102+p93*p102-s5*p19*p30-p42*p138-s5*"
          "p19*p36-p85*p102+s5*p19*p31+p94*p102-p41*p138-s5*p19*p37-p43*p138-p24*p165-p28*p165+p25*p165-p29*p165;\n    A(5,"
          "25) = -p74*p96-p69*p70+p67*p96+p48*p96+p62*p96+p68*p96-p69*p71-p44*p67-p48*p44+p70*p97+p44*p74-p70*p106-p70*p127"
          "-p70*p113-p84*p96-p96*p93+p70*p134-p44*p62-p44*p68+p70*p120+p85*p96-p70*p142-p70*p125-p70*p152-p96*p94+p71*p97+p"
          "44*p84+p44*p93-p70*p121-p71*p106-p71*p127-p70*p135-p71*p113+p71*p134+p44*p94-p71*p142-p70*p143-p71*p125-p71*p152"
          "-p71*p121-p71*p135-p44*p85+p70*p126+p71*p120+p71*p126-p71*p143;\n    A(5,26) = -p69*p65+p67*p88+p48*p88-p74*p88+"
          "p62*p88+p68*p88+p65*p97+p69*p79+p50*p67+p48*p50-p65*p106-p65*p127-p65*p113-p88*p84-p88*p93-p50*p74+p65*p134+p65*"
          "p120+p88*p85+p50*p62+p50*p68-p65*p142-p65*p125-p65*p152-p88*p94-p65*p121-p65*p135-p79*p97-p50*p84-p50*p93+p79*p1"
          "06+p79*p127+p79*p113-p79*p134-p79*p120-p65*p143-p50*p94+p79*p142+p79*p125+p79*p152+p79*p121+p79*p135+p65*p126+p5"
          "0*p85-p79*p126+p79*p143;\n    A(5,27) = -p53*p69-p67*p102-s3*p5*p47+p74*p102+p49*p138-p62*p102-p68*p102+s5*p151+"
          "p84*p102+p93*p102-s5*p19*p30-p42*p138-s5*p19*p36-p85*p102+s5*p19*p31+p94*p102-p41*p138-s5*p19*p37-p43*p138-p24*p"
          "165-p28*p165+p25*p165-p29*p165;\n\n    A(6,6) = -c6*p104*p158-c6*p107*p158-p32*p105*p131-c6*p105*p104+p51*p101*p"
          "158+p53*p67*p105+p5*p20*p53*p147-p24*p72*p158-p28*p72*p158-p56*p101*p158-p53*p74*p105-handOffs*p91*p118+p30*p80*"
          "p158+p36*p80*p158+p26*p105*p173+p51*p91*p185+p67*p91*p90+p5*p22*p45*p147-p30*p47*p105-p42*p95*p105-p36*p47*p105-"
          "p53*p84*p105-p24*p91*p147-p53*p93*p105-p28*p91*p147-p56*p91*p185-p74*p91*p90+p49*p95*p105+p25*p72*p158+p46*p101*"
          "p158+p52*p101*p158+p53*p62*p105+p53*p68*p105-p29*p72*p158-p32*p59*p159-p84*p91*p90-p91*p93*p90+p20*p49*p159-p31*"
          "p80*p158+p31*p47*p105+p37*p80*p158+p53*p85*p105+p25*p91*p147+p46*p91*p185+p62*p91*p90+p52*p91*p185+p68*p91*p90-p"
          "41*p95*p105-p37*p47*p105-p43*p95*p105-p53*p94*p105-p29*p91*p147+p151*p159+p30*p91*p178+p36*p91*p178-p24*p86*p105"
          "-p19*p30*p159-p20*p42*p159-p28*p86*p105-p19*p36*p159-p31*p91*p178+p25*p86*p105+p19*p31*p159+p85*p91*p90+p37*p91*"
          "p178-p20*p41*p159-p29*p86*p105-p19*p37*p159-p20*p43*p159-p91*p94*p90-p23*p24*p159-p23*p28*p159+p23*p25*p159-p23*"
          "p29*p159;\n    A(6,22) = +c6*p22*p104-c6*p104-c6*p104-p53*p74-p53*p74+c6*p22*p107+p32*p35*p131+c6*p35*p104-p22*p"
          "51*p101-p35*p53*p67-p5*p19*p20*p53+p22*p24*p72+p22*p28*p72+p22*p56*p101+p35*p53*p74-c6*p107-c6*p107-p32*p131-p32"
          "*p131+p51*p101+p51*p101+p53*p67+p53*p67+p5*p186+p5*p186-p24*p72-p24*p72-p28*p72-p56*p101-p28*p72-p56*p101+p32*p5"
          "9*p149+p91*p118-p22*p30*p80-p22*p36*p80-p26*p35*p173-p20*p51*p91-p18*p67*p91-p5*p22*p186+p30*p35*p47+p35*p42*p95"
          "+p35*p36*p47+p35*p53*p84+p19*p24*p91+p35*p53*p93+p19*p28*p91+p20*p56*p91+p18*p74*p91+p25*p72+p46*p101+p25*p72+p4"
          "6*p101+p52*p101+p52*p101-p35*p49*p95+p53*p62+p53*p62+p53*p68+p53*p68-p29*p72-p29*p72-p22*p25*p72-p22*p46*p101-p2"
          "2*p52*p101+p30*p80+p30*p80+p36*p80+p36*p80-p35*p53*p62-p35*p53*p68+p26*p173+p26*p173+p22*p29*p72-p30*p47-p42*p95"
          "-p30*p47-p42*p95-p36*p47-p36*p47-p53*p84-p53*p84-p53*p93-p53*p93+p49*p95+p49*p95-p31*p80-p31*p80+p24*p35*p86+p19"
          "*p30*p149+p20*p42*p149+p28*p35*p86+p19*p36*p149+p18*p84*p91+p18*p93*p91+p31*p47+p31*p47+p37*p80+p37*p80-p20*p49*"
          "p149+p53*p85+p53*p85-p41*p95-p41*p95-p37*p47-p43*p95-p37*p47-p43*p95-p53*p94-p53*p94+p22*p31*p80-p31*p35*p47-p22"
          "*p37*p80-p35*p53*p85-p19*p25*p91-p20*p46*p91-p18*p62*p91-p20*p52*p91-p18*p68*p91+p35*p41*p95+p35*p37*p47+p35*p43"
          "*p95-p24*p86-p24*p86-p28*p86-p28*p86+p35*p53*p94+p19*p29*p91-p151*p149-p23*p30*p91-p23*p36*p91-p29*p86-p29*p86+p"
          "23*p31*p91-p25*p35*p86-p19*p31*p149-p18*p85*p91-p23*p37*p91+p20*p41*p149+p29*p35*p86+p19*p37*p149+p20*p43*p149+p"
          "18*p94*p91+p23*p24*p149+p23*p28*p149+p25*p86+p25*p86-p23*p25*p149+p23*p29*p149;\n    A(6,24) = -p140-p140-p19*p2"
          "4-p19*p24-p19*p28-p20*p56-p19*p28-p20*p56+p16*p35*p140+p17*p35*p140+p11*p69+p11*p69-p118-p118-p18*p67-p18*p67-p1"
          "8*p48-p18*p48+p20*p51+p20*p51+p18*p74+p18*p74-p69*p167-p18*p62-p18*p62-p18*p68-p18*p68-p69*p175+p19*p25+p20*p46+"
          "p19*p25+p20*p46+p20*p52+p20*p52+p16*p35*p118+p16*p22*p140+p22*p67*p103+p17*p35*p118+p22*p48*p103+p17*p22*p140-p3"
          "5*p51*p122+p22*p67*p111-p19*p29-p19*p29+p22*p48*p111-p35*p51*p128-p22*p74*p103+p24*p35*p129+p28*p35*p129+p35*p56"
          "*p122-p22*p74*p111+p24*p35*p136+p28*p35*p136+p35*p56*p128-p11*p97-p11*p97+p23*p30+p23*p30+p11*p106+p11*p127+p11*"
          "p106+p11*p127+p23*p36+p23*p36+p11*p113+p11*p113+p18*p84+p18*p84+p18*p93+p18*p93-p11*p134-p11*p134-p25*p35*p136-p"
          "35*p46*p128-p35*p52*p128+p29*p35*p129-p23*p31-p23*p31-p11*p120-p11*p120+p29*p35*p136-p18*p85-p18*p85-p35*p69*p92"
          "+p11*p142+p11*p142+p23*p37+p23*p37+p11*p125+p11*p152+p11*p125+p11*p152+p16*p22*p118+p97*p167-p35*p69*p81+p18*p94"
          "+p18*p94+p17*p22*p118+p97*p175-p30*p35*p153-p106*p167-p22*p51*p122-p127*p167+p35*p67*p103-p35*p36*p153-p113*p167"
          "+p5*p19*p20*p103-p30*p35*p144-p22*p84*p103-p106*p175-p22*p51*p128-p127*p175+p35*p67*p111-p35*p36*p144-p22*p93*p1"
          "03-p113*p175+p5*p19*p20*p111-p22*p84*p111+p22*p24*p129-p22*p93*p111+p22*p28*p129+p22*p56*p122+p134*p167-p35*p74*"
          "p103+p22*p24*p136+p22*p28*p136+p22*p56*p128+p134*p175-p35*p74*p111+p11*p121+p11*p121+p11*p135+p11*p135+p22*p62*p"
          "103+p22*p68*p103-p25*p35*p129-p35*p46*p122+p22*p62*p111-p35*p52*p122+p22*p68*p111+p11*p143+p11*p143+p151*p190-p2"
          "2*p30*p153-p121*p167-p22*p36*p153-p135*p167+p151*p193-p22*p30*p144-p121*p175-p19*p30*p190-p20*p42*p190-p22*p36*p"
          "144-p135*p175-p19*p36*p190-p20*p54*p59*p103-p19*p30*p193-p20*p42*p193-p20*p59*p60*p103-p19*p36*p193-p20*p54*p59*"
          "p111-p20*p59*p60*p111+p20*p49*p190+p20*p49*p193+p31*p35*p153+p120*p167+p31*p35*p144+p22*p85*p103+p120*p175+p22*p"
          "85*p111-p22*p25*p129-p22*p46*p122-p142*p167+p35*p62*p103-p35*p37*p153-p125*p167-p22*p52*p122-p152*p167+p35*p68*p"
          "103-p22*p25*p136-p22*p46*p128-p142*p175+p35*p62*p111-p35*p37*p144-p22*p94*p103-p125*p175-p22*p52*p128-p152*p175+"
          "p35*p68*p111-p22*p94*p111+p22*p29*p129-p11*p126-p11*p126+p22*p29*p136-p35*p92*p121-p23*p28*p190-p35*p81*p121-p23"
          "*p28*p193+p22*p31*p153+p126*p167+p22*p31*p144+p126*p175+p19*p31*p190+p20*p59*p61*p103+p19*p31*p193-p22*p37*p153-"
          "p143*p167+p20*p59*p61*p111-p20*p41*p190-p22*p37*p144-p143*p175-p19*p37*p190-p20*p43*p190-p20*p41*p193-p20*p59*p6"
          "3*p103-p19*p37*p193-p20*p43*p193-p20*p59*p63*p111+p35*p92*p126+p35*p81*p126-p23*p29*p190-p23*p29*p193;\n    A(6,"
          "25) = -c6*c7*p104-c6*c7*p107-p32*p59*p65+c7*p51*p101+c7*p53*p67+p5*p19*p170-c7*p24*p72-c7*p28*p72-c7*p56*p101-c7"
          "*p53*p74+c7*p25*p72+c7*p46*p101+c7*p52*p101+c7*p53*p62+c7*p53*p68-c7*p29*p72+c7*p30*p80+c7*p36*p80+c7*p26*p173-c"
          "7*p30*p47-p20*p42*p65-c7*p36*p47-c7*p53*p84-c7*p53*p93+p20*p49*p65+c7*p53*p85-p20*p41*p65-c7*p37*p47-p20*p43*p65"
          "-c7*p53*p94-p23*p24*p65-p23*p28*p65-c7*p31*p80+c7*p31*p47+c7*p37*p80+p23*p25*p65-p23*p29*p65;\n    A(6,26) = +s7"
          "*c6*p104+s7*c6*p107+p32*p59*p70-s7*p51*p101-s7*p53*p67-p5*p19*p179+s7*p24*p72+s7*p28*p72+s7*p56*p101+s7*p53*p74-"
          "s7*p25*p72-s7*p46*p101-s7*p52*p101-s7*p53*p62-s7*p53*p68+s7*p29*p72-s7*p30*p80-s7*p36*p80-s7*p26*p173+s7*p30*p47"
          "+p20*p42*p70+s7*p36*p47+s7*p53*p84+s7*p53*p93-p20*p49*p70-s7*p31*p47-s7*p37*p80-s7*p53*p85+p20*p41*p70+s7*p37*p4"
          "7+p20*p43*p70+s7*p53*p94+p23*p24*p70+p23*p28*p70+s7*p31*p80-p23*p25*p70+p23*p29*p70;\n    A(6,27) = -p16*p140-p1"
          "7*p140+p69*p81-p16*p118-p67*p103-p17*p118-p48*p103+p51*p122-p67*p111-p48*p111+p51*p128+p74*p103-p24*p129-p28*p12"
          "9-p56*p122+p74*p111-p24*p136-p28*p136-p56*p128+p69*p92+p30*p144+p84*p103+p81*p106+p81*p127+p36*p144+p93*p103+p81"
          "*p113+p84*p111+p93*p111-p92*p134-p81*p134-p62*p103-p68*p103+p25*p129+p46*p122-p62*p111+p52*p122-p68*p111+p25*p13"
          "6+p46*p128+p52*p128-p29*p129-p29*p136-p97*p92-p81*p97+p30*p153+p92*p106+p92*p127+p36*p153+p92*p113-p31*p153-p92*"
          "p120-p31*p144-p85*p103-p81*p120-p85*p111+p92*p142+p37*p153+p92*p125+p92*p152+p81*p142+p37*p144+p94*p103+p81*p125"
          "+p81*p152+p94*p111+p92*p121+p92*p135+p81*p121+p81*p135-p92*p126-p81*p126+p92*p143+p81*p143;\n\n    A(7,25) = +s7"
          "*p17*p140-p69*p88-p67*p65-p48*p65+p65*p74-p69*p160+s7*p17*p118+s7*p16*p140+p17*p67*p79+p17*p48*p79-s7*p51*p128-p"
          "62*p65-p68*p65-p17*p74*p79+s7*p24*p136+s7*p28*p136+s7*p56*p128+p88*p97+p65*p84+p65*p93-p30*p161-p42*p170-p36*p16"
          "1+p49*p170+p16*p48*p79-s7*p30*p144-p106*p160-p127*p160-s7*p51*p122-s7*p36*p144-p113*p160-p65*p85-p17*p84*p79-p17"
          "*p79*p93-p16*p74*p79+p31*p161+s7*p24*p129+s7*p28*p129+p134*p160+s7*p56*p122+p65*p94-p41*p170-p37*p161-p43*p170+p"
          "17*p62*p79+p17*p68*p79-s7*p25*p136-s7*p46*p128-s7*p52*p128-p24*p180-p28*p180+s7*p29*p136-p69*p176+p97*p160+s7*p1"
          "6*p118+p16*p67*p79-p29*p180+s7*p31*p144+p120*p160+p17*p85*p79+p16*p62*p79+p16*p68*p79-s7*p25*p129-p142*p160-s7*p"
          "46*p122-s7*p37*p144-p125*p160-p160*p152-s7*p52*p122-p17*p79*p94+s7*p29*p129+p97*p176-p121*p160-s7*p30*p153-p106*"
          "p176-p127*p176-p135*p160-s7*p36*p153-p113*p176-p16*p84*p79-p16*p79*p93+p25*p180+p134*p176+p126*p160+s7*p31*p153+"
          "p120*p176+p16*p85*p79-p142*p176-p143*p160-s7*p37*p153-p125*p176-p152*p176-p16*p79*p94-p121*p176-p135*p176+p126*p"
          "176-p143*p176;\n    A(7,26) = +c7*p16*p140+p69*p96+p67*p70+p48*p70-p74*p70-p49*p179+c7*p16*p118+p16*p67*p71+p16*"
          "p48*p71+c7*p17*p140-c7*p51*p122-p16*p74*p71+c7*p24*p129+c7*p28*p129+c7*p56*p122+p62*p70+p68*p70-p97*p96-p70*p84-"
          "p70*p93+p30*p169+p42*p179+p36*p169-p69*p168+p17*p48*p71-p16*p71*p84-c7*p51*p128-p16*p71*p93+p134*p168-p17*p74*p7"
          "1+p70*p85+c7*p24*p136+c7*p28*p136+c7*p56*p128-p31*p169-p70*p94+p41*p179+p37*p169+p43*p179+p16*p62*p71+p16*p68*p7"
          "1-c7*p25*p129-c7*p46*p122-c7*p52*p122+c7*p29*p129+p24*p184+p28*p184-p69*p177+p97*p168+c7*p17*p118-c7*p30*p153-p1"
          "06*p168-p127*p168+p17*p67*p71-c7*p36*p153-p113*p168-p25*p184+p29*p184+c7*p31*p153+p120*p168+p16*p71*p85-p142*p16"
          "8+p17*p62*p71-c7*p37*p153-p125*p168-p152*p168+p17*p68*p71-c7*p25*p136-c7*p46*p128-p16*p71*p94-c7*p52*p128+c7*p29"
          "*p136+p97*p177-p121*p168-p135*p168-c7*p30*p144-p106*p177-p127*p177-c7*p36*p144-p113*p177-p17*p71*p84-p17*p71*p93"
          "+p134*p177+p126*p168+c7*p31*p144+p120*p177+p17*p71*p85-p143*p168-p142*p177-c7*p37*p144-p125*p177-p152*p177-p17*p"
          "71*p94-p121*p177-p135*p177+p126*p177-p143*p177;\n\nend"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    15
        position		    [23.5747 49.5747 7]
        chart		    13
        linkNode		    [13 0 0]
        subviewer		    13
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    16
        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		      15
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    13
        linkNode		    [13 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    13
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    17
        ssIdNumber		    8
        name		    "q_motor"
        linkNode		    [13 0 18]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    18
        ssIdNumber		    9
        name		    "x_hat"
        linkNode		    [13 17 19]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    19
        ssIdNumber		    10
        name		    "gearRatio"
        linkNode		    [13 18 20]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    20
        ssIdNumber		    12
        name		    "baseAngles"
        linkNode		    [13 19 21]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    21
        ssIdNumber		    13
        name		    "gravTrq"
        linkNode		    [13 20 22]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    22
        ssIdNumber		    14
        name		    "gravTrqMotor"
        linkNode		    [13 21 23]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    23
        ssIdNumber		    15
        name		    "coulombWidthMotor"
        linkNode		    [13 22 24]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    24
        ssIdNumber		    16
        name		    "coulombWidth"
        linkNode		    [13 23 0]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    25
        name		    "Left/calcGravityTorque/calcGravTrq"
        machine		    1
        chart		    13
      }
      chart {
        id			    26
        name		    "botharms_get_torque/MATLAB Function2"
        windowPosition	    [255.375 98.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 27 0 0]
        firstTransition	    29
        firstJunction	    28
        viewObj		    26
        machine		    1
        toolbarMode		    LIBRARY_TOOLBAR
        ssIdHighWaterMark	    5
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    30
        chartFileNumber	    3
        disableImplicitCasting  1
        eml {
          name		      "abb2logical"
        }
      }
      state {
        id			    27
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    26
        treeNode		    [26 0 0 0]
        superState		    SUBCHART
        subviewer		    26
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function logical = abb2logical(abb)\n%#codegen\n\nlogical = [abb(1:2); abb(7); abb(3:6)];"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    28
        position		    [23.5747 49.5747 7]
        chart		    26
        linkNode		    [26 0 0]
        subviewer		    26
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    29
        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		      28
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    26
        linkNode		    [26 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    26
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    30
        ssIdNumber		    4
        name		    "abb"
        linkNode		    [26 0 31]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    31
        ssIdNumber		    5
        name		    "logical"
        linkNode		    [26 30 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    32
        name		    "botharms_get_torque/MATLAB Function2"
        machine		    1
        chart		    26
      }
      chart {
        id			    33
        name		    "botharms_get_torque/lp"
        windowPosition	    [150.188 68.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 34 0 0]
        firstTransition	    36
        firstJunction	    35
        viewObj		    33
        machine		    1
        toolbarMode		    LIBRARY_TOOLBAR
        ssIdHighWaterMark	    5
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    37
        chartFileNumber	    4
        disableImplicitCasting  1
        eml {
          name		      "lp"
        }
      }
      state {
        id			    34
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    33
        treeNode		    [33 0 0 0]
        superState		    SUBCHART
        subviewer		    33
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function v_out = lp(v_in)\n%#codegen\n\n\npersistent v_flt\nif isempty(v_flt)\n    v_flt = v_in;\n"
          "end\nT = 0.016;\ndt = 0.004;\n\nv_dot = (v_in - v_flt)/T;\n\nv_flt = v_flt + dt*v_dot;\n\nv_out = v_flt;\n"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    35
        position		    [23.5747 49.5747 7]
        chart		    33
        linkNode		    [33 0 0]
        subviewer		    33
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    36
        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		      35
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    33
        linkNode		    [33 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    33
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    37
        ssIdNumber		    4
        name		    "v_in"
        linkNode		    [33 0 38]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    38
        ssIdNumber		    5
        name		    "v_out"
        linkNode		    [33 37 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    39
        name		    "botharms_get_torque/lp"
        machine		    1
        chart		    33
      }
      chart {
        id			    40
        name		    "botharms_get_torque/MATLAB Function5"
        windowPosition	    [375.375 -21.75 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 41 0 0]
        firstTransition	    43
        firstJunction	    42
        viewObj		    40
        machine		    1
        ssIdHighWaterMark	    28
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    44
        updateMethod	    DISCRETE
        sampleTime		    "0.004"
        chartFileNumber	    5
        disableImplicitCasting  1
        eml {
          name		      "nancheck"
        }
      }
      state {
        id			    41
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    40
        treeNode		    [40 0 0 0]
        superState		    SUBCHART
        subviewer		    40
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function y   = nancheck(u)\n\n\ny = u;\n\n\nif any(isnan(u)) || any(isinf(u))\n    y = zeros(size"
          "(u));\nend\n\nend"
          editorLayout	      "100 M4x1[772 19 980 1190]"
        }
      }
      junction {
        id			    42
        position		    [23.5747 49.5747 7]
        chart		    40
        linkNode		    [40 0 0]
        subviewer		    40
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    43
        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		      42
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    40
        linkNode		    [40 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    40
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    44
        ssIdNumber		    28
        name		    "y"
        linkNode		    [40 0 45]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    45
        ssIdNumber		    4
        name		    "u"
        linkNode		    [40 44 0]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    46
        name		    "botharms_get_torque/MATLAB Function5"
        machine		    1
        chart		    40
      }
      chart {
        id			    47
        name		    "MATLAB Function3"
        windowPosition	    [330.375 23.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 48 0 0]
        firstTransition	    50
        firstJunction	    49
        viewObj		    47
        machine		    1
        ssIdHighWaterMark	    28
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    51
        updateMethod	    DISCRETE
        sampleTime		    "0.004"
        chartFileNumber	    6
        disableImplicitCasting  1
        eml {
          name		      "nancheck"
        }
      }
      state {
        id			    48
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    47
        treeNode		    [47 0 0 0]
        superState		    SUBCHART
        subviewer		    47
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function y   = nancheck(u)\n\n\ny = u;\n\n\nif any(isnan(u)) || any(isinf(u))\n    y = zeros(size"
          "(u));\nend\n\nend"
          editorLayout	      "100 M4x1[278 256 700 500]"
        }
      }
      junction {
        id			    49
        position		    [23.5747 49.5747 7]
        chart		    47
        linkNode		    [47 0 0]
        subviewer		    47
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    50
        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		      49
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    47
        linkNode		    [47 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    47
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    51
        ssIdNumber		    28
        name		    "y"
        linkNode		    [47 0 52]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    52
        ssIdNumber		    4
        name		    "u"
        linkNode		    [47 51 0]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    53
        name		    "MATLAB Function3"
        machine		    1
        chart		    47
      }
      chart {
        id			    54
        name		    "MATLAB Function1"
        windowPosition	    [315.375 38.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 55 0 0]
        firstTransition	    57
        firstJunction	    56
        viewObj		    54
        machine		    1
        ssIdHighWaterMark	    28
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    58
        updateMethod	    DISCRETE
        sampleTime		    "0.004"
        chartFileNumber	    7
        disableImplicitCasting  1
        eml {
          name		      "nancheck"
        }
      }
      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 y   = nancheck(u)\n\n\ny = u;\n\n\nif any(isnan(u)) || any(isinf(u))\n    y = zeros(size"
          "(u));\nend\n\nend"
          editorLayout	      "100 M4x1[278 256 700 500]"
        }
      }
      junction {
        id			    56
        position		    [23.5747 49.5747 7]
        chart		    54
        linkNode		    [54 0 0]
        subviewer		    54
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    57
        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		      56
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    54
        linkNode		    [54 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    54
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    58
        ssIdNumber		    28
        name		    "y"
        linkNode		    [54 0 59]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    59
        ssIdNumber		    4
        name		    "u"
        linkNode		    [54 58 0]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    60
        name		    "MATLAB Function1"
        machine		    1
        chart		    54
      }
      chart {
        id			    61
        name		    "botharms_get_torque/FRIDA arm acceleration NOT SI/base_tool_transform"
        windowPosition	    [270.375 98.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 62 0 0]
        firstTransition	    64
        firstJunction	    63
        viewObj		    61
        machine		    1
        ssIdHighWaterMark	    13
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    65
        chartFileNumber	    8
        disableImplicitCasting  1
        eml {
          name		      "base_tool_transform"
        }
      }
      state {
        id			    62
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    61
        treeNode		    [61 0 0 0]
        superState		    SUBCHART
        subviewer		    61
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function [T44_out, J0_out, Jn_out, Jdot0_out] = base_tool_transform(T44_world2base, T44_flange2to"
          "ol, T44_in, J0_in, qd, Jdot0_in)\n\nT44_world2base_m = reshape(T44_world2base, 4, 4)';\nT44_flange2tool_m = resh"
          "ape(T44_flange2tool, 4, 4)';\nT44_in_m = reshape(T44_in, 4, 4)';\nJ0_in_m = reshape(J0_in, [], 6)';\nJdot0_in_m "
          "= reshape(Jdot0_in, [], 6)';\n\n% Forward kinematics\nT44_out_m = T44_world2base_m*T44_in_m*T44_flange2tool_m;\n"
          "\n% Rotation of base\nRbase = T44_world2base_m(1:3,1:3);\nTbase = [Rbase zeros(3);zeros(3) Rbase];\nJ0_m = Tbase"
          "*J0_in_m;\nJdot0_m = Tbase*Jdot0_in_m;\n\n% Handle tool transformation\nT44_baseToFlange = T44_world2base_m*T44_"
          "in_m;\nT44_baseToTcp = T44_out_m;\nr = T44_baseToTcp(1:3,4) - T44_baseToFlange(1:3,4);\nomega = J0_m(4:6,:);\n\n"
          "for i=1:size(Jdot0_m,2)\n    jContribution = cross(omega(:,i),r);\n    J0_m(1:3,i) = J0_m(1:3,i) + jContribution"
          ";\n    for j=1:size(Jdot0_m,2)\n        jdotContribution = cross(omega(:,i),cross(omega(:,j)*qd(j),r));\n       "
          " Jdot0_m(1:3,i) = Jdot0_m(1:3,i) + jdotContribution;\n    end\nend\n\n% Jacobian in tool frame\nJn_m = [T44_out_"
          "m(1:3,1:3)' zeros(3);zeros(3) T44_out_m(1:3,1:3)']*J0_m;\n\n       \nT44_out = reshape(T44_out_m', 16, 1);\nJ0_o"
          "ut = reshape(J0_m', [], 1);\nJn_out = reshape(Jn_m', [], 1);\nJdot0_out = reshape(Jdot0_m',[],1);\n\n\n"
          editorLayout	      "100 M4x1[205 227 1080 733]"
        }
      }
      junction {
        id			    63
        position		    [23.5747 49.5747 7]
        chart		    61
        linkNode		    [61 0 0]
        subviewer		    61
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    64
        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		      63
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    61
        linkNode		    [61 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    61
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    65
        ssIdNumber		    4
        name		    "T44_world2base"
        linkNode		    [61 0 66]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    66
        ssIdNumber		    5
        name		    "T44_out"
        linkNode		    [61 65 67]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    67
        ssIdNumber		    6
        name		    "T44_flange2tool"
        linkNode		    [61 66 68]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    68
        ssIdNumber		    7
        name		    "T44_in"
        linkNode		    [61 67 69]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    69
        ssIdNumber		    8
        name		    "J0_in"
        linkNode		    [61 68 70]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    70
        ssIdNumber		    9
        name		    "qd"
        linkNode		    [61 69 71]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    71
        ssIdNumber		    10
        name		    "Jdot0_in"
        linkNode		    [61 70 72]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    72
        ssIdNumber		    11
        name		    "J0_out"
        linkNode		    [61 71 73]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    73
        ssIdNumber		    12
        name		    "Jn_out"
        linkNode		    [61 72 74]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    74
        ssIdNumber		    13
        name		    "Jdot0_out"
        linkNode		    [61 73 0]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    75
        name		    "botharms_get_torque/FRIDA arm acceleration NOT SI/base_tool_transform"
        machine		    1
        chart		    61
      }
      chart {
        id			    76
        name		    "MATLAB Function2"
        windowPosition	    [315.375 38.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 77 0 0]
        firstTransition	    79
        firstJunction	    78
        viewObj		    76
        machine		    1
        ssIdHighWaterMark	    28
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    80
        updateMethod	    DISCRETE
        sampleTime		    "0.004"
        chartFileNumber	    9
        disableImplicitCasting  1
        eml {
          name		      "nancheck"
        }
      }
      state {
        id			    77
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    76
        treeNode		    [76 0 0 0]
        superState		    SUBCHART
        subviewer		    76
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function y   = nancheck(u)\n\n\ny = u;\n\n\nif any(isnan(u)) || any(isinf(u))\n    y = zeros(size"
          "(u));\nend\n\nend"
          editorLayout	      "100 M4x1[772 19 980 1190]"
        }
      }
      junction {
        id			    78
        position		    [23.5747 49.5747 7]
        chart		    76
        linkNode		    [76 0 0]
        subviewer		    76
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    79
        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		      78
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    76
        linkNode		    [76 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    76
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    80
        ssIdNumber		    28
        name		    "y"
        linkNode		    [76 0 81]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    81
        ssIdNumber		    4
        name		    "u"
        linkNode		    [76 80 0]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    82
        name		    "MATLAB Function2"
        machine		    1
        chart		    76
      }
      chart {
        id			    83
        name		    "MATLAB Function4"
        windowPosition	    [330.375 23.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 84 0 0]
        firstTransition	    86
        firstJunction	    85
        viewObj		    83
        machine		    1
        ssIdHighWaterMark	    28
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    87
        updateMethod	    DISCRETE
        sampleTime		    "0.004"
        chartFileNumber	    10
        disableImplicitCasting  1
        eml {
          name		      "nancheck"
        }
      }
      state {
        id			    84
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    83
        treeNode		    [83 0 0 0]
        superState		    SUBCHART
        subviewer		    83
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function y   = nancheck(u)\n\n\ny = u;\n\n\nif any(isnan(u)) || any(isinf(u))\n    y = zeros(size"
          "(u));\nend\n\nend"
          editorLayout	      "100 M4x1[278 256 700 500]"
        }
      }
      junction {
        id			    85
        position		    [23.5747 49.5747 7]
        chart		    83
        linkNode		    [83 0 0]
        subviewer		    83
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    86
        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		      85
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    83
        linkNode		    [83 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    83
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    87
        ssIdNumber		    28
        name		    "y"
        linkNode		    [83 0 88]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    88
        ssIdNumber		    4
        name		    "u"
        linkNode		    [83 87 0]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    89
        name		    "MATLAB Function4"
        machine		    1
        chart		    83
      }
      chart {
        id			    90
        name		    "botharms_get_torque/MATLAB Function1"
        windowPosition	    [270.375 98.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 91 0 0]
        firstTransition	    93
        firstJunction	    92
        viewObj		    90
        machine		    1
        ssIdHighWaterMark	    12
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    94
        chartFileNumber	    11
        disableImplicitCasting  1
        eml {
          name		      "fcn"
        }
      }
      state {
        id			    91
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    90
        treeNode		    [90 0 0 0]
        superState		    SUBCHART
        subviewer		    90
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function [e, scale]   = fcn(T44_FK1, T44_FK2)\n%#codegen\n\n% Set scale:\nscale = 0.5;\n\n% Resha"
          "ping:\nT441 = reshape(T44_FK1, 4,4)';\nT442 = reshape(T44_FK2, 4,4)';\n\nR1 = T441(1:3,1:3);\nR2 = T442(1:3,1:3)"
          ";\n\np1 = T441(1:3,4);\np2 = scale*T442(1:3,4);\n\n% Position:\npersistent delta_p\nif isempty(delta_p)\n    del"
          "ta_p =  p2 - p1;\nend\nep = p2 - (p1 + delta_p);\n    \n\n% Rotation:\npersistent delta_R\nif isempty(delta_R)\n"
          "    delta_R = R1'*R2;\nend\nR = R2*(R1*delta_R)';\n\n\nRxx = R(1,1); Rxy = R(1,2); Rxz = R(1,3);\nRyx = R(2,1); "
          "Ryy = R(2,2); Ryz = R(2,3);\nRzx = R(3,1); Rzy = R(3,2); Rzz = R(3,3);\n\nw = sqrt( trace( R ) + 1 ) / 2;\n\n% c"
          "heck if w is real. Otherwise, zero it.\nif( imag( w ) > 0 )\n     w = 0;\nend\n\nx1 = ( Rzy - Ryz ) / (4*w);\ny1"
          " = ( Rxz - Rzx ) / (4*w);\nz1 = ( Ryx - Rxy ) / (4*w);\nq1 = [x1; y1; z1];\n\nif any(isnan(q1)) || any(isinf(q1)"
          ")\n    q1 = zeros(size(q1));\nend\n\npersistent q_old\nif isempty(q_old)\n    q_old = q1;\nend\n\nq2 = -q1;\nif "
          "(norm(q_old - q1) < norm(q_old-q2)) || norm(q1)<.1\n    q_out = q1;\nelse\n    q_out = q2;\nend\n\n\nq_old = q_o"
          "ut;\n\ne = [ep ; q_out];\n\n"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    92
        position		    [23.5747 49.5747 7]
        chart		    90
        linkNode		    [90 0 0]
        subviewer		    90
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    93
        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		      92
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    90
        linkNode		    [90 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    90
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    94
        ssIdNumber		    7
        name		    "T44_FK1"
        linkNode		    [90 0 95]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    95
        ssIdNumber		    4
        name		    "e"
        linkNode		    [90 94 96]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    96
        ssIdNumber		    11
        name		    "T44_FK2"
        linkNode		    [90 95 97]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    97
        ssIdNumber		    12
        name		    "scale"
        linkNode		    [90 96 0]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    98
        name		    "botharms_get_torque/MATLAB Function1"
        machine		    1
        chart		    90
      }
      chart {
        id			    99
        name		    "Left/leadthroug control/MATLAB Function"
        windowPosition	    [390.75 188.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 100 0 0]
        firstTransition	    102
        firstJunction	    101
        viewObj		    99
        machine		    1
        ssIdHighWaterMark	    34
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    103
        chartFileNumber	    12
        disableImplicitCasting  1
        eml {
          name		      "fcn"
        }
      }
      state {
        id			    100
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    99
        treeNode		    [99 0 0 0]
        superState		    SUBCHART
        subviewer		    99
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function [Kp,Kv,Ki,trqFfw,posRef,test,sat] = fcn(Kp_in,Kv_in,Ki_in,gravTrqMotor,trqRaw,posRefIn,p"
          "osRaw,f_switch,velFlt,highKiActive,keepStill,velFlt_noExtraFilt,coulomb_width,percentageTrqFfw,KiHighThres,slidi"
          "ngThresWidth,maxRate,resetPosRef,jointLimitStartMotorRad,extTorque,sensorOn,fricHelp,qtorque)\n%#codegen\n% All "
          "vectors are given in ABB-order\n\npersistent torqueFeedforward;\n           % Used to account for the accumulate"
          "d torqe feedforward (size 7x1)\npersistent posRefOffset;\n           % The offset from the incoming position ref"
          "erence, needs to be\n           % updated to the current position to avoid the safety system of\n           % th"
          "e ABB controller to give errors\npersistent trqUpdateAddition;\n           % The torque feedforward addition is "
          "saved in this variable. The\n           % first column is the total torque feedforward, the second\n           %"
          " column the \"feedforward\" part, i.e., the desired torque which\n           % has to be corrected with a feedba"
          "ck part to handle\n           % disturbances. The last column contains a counter that is used\n           % to a"
          "void using feedback 3 samples after any feedforward change\n           % (3 samples is the delay from the feedfo"
          "rward signal is set\n           % until a change can be seen in trqRaw)\npersistent trqRawFlt;\n           % Fil"
          "tered torque signal\npersistent keepKiClosed; \n           % Variable of size 7x1 (one for each joint). If zero,"
          " Ki should \n           % be active for the respective joints. Also works as a counter, \n           % it is set"
          " to a value when a joint is moving, and then counted \n           % down when the joint is still (below a thresh"
          "old)\npersistent closeKiCounter;  \n           % Variable of size 7x2. It is used to make sure that a detected\n"
          "           % torque will have time to make an impact. When a torque is\n           % detected with Ki active, cl"
          "oseKiCounter is set to a value that\n           % is then counted down. While closeKiCounter is non-zero, Ki\n  "
          "         % will not be active and a torque feedforward will be used. The\n           % first column is used if a"
          " negative torque is detected, and the\n           % second column if a positive torque is detected\n           \n"
          "persistent startOffset;\n           % Variable used to handle the offset torque present for each\n           % j"
          "oint. The first column is used to save this torque, the\n           % second column to tell if it has been set o"
          "r not\npersistent extraTorqueOut;\n           % Variable to handle ramping of the extra feedforward torque\npers"
          "istent trqAddHistory\n           % The history of added feedforward torques. It is used to\n           % predict"
          " trqRaw during the delay that is present.\n           \npersistent saturated;           \n\n\n\nlength_mean = [8"
          "0;80;80;80;80;80;80];\ndelay_thres = [60;60;60;60;60;60;60];% length_mean has to be larger than delay thres\nrec"
          "overyFactor = [1.5;1.5;1.5;1.5;1.5;1.5;1.5];\n% slidingThresWidth = [0.006;0.006;0.003;0.00045;0.0006;0.0004;0.0"
          "03;];\n           % These are parameters for detection of torque when Ki are\n           % active\n           \n"
          "trqFiltConstant = [0.8;0.8;0.8;0.8;0.8;0.8;0.8];\n           % The filter constant for the torque low pass filte"
          "r\n\n% KiHighThres = [0.1 1;0.1 1;0.1 1;0.3 1;0.1 1;0.1 1;0.1 1]; The first\n% column is the threshold for the f"
          "iltered version of velFlt, and the\n% second column the threshold for the non-filtered version of velFlt\nKiHigh"
          "Delay = [30 ; 30 ; 30 ; 30 ; 30 ; 30 ; 30];\n           % The standard delay for counting down keepKiClosed when"
          " the\n           % measured velocity is below KiHighThres\n           \nhighVelThres = [5;5;5;5;5;5;5];\nlowVelT"
          "hres = [2;2;2;2;2;2;2];\n           % Thresholds for high and low velocities. Full torque\n           % feedforw"
          "ard is active when the velocity is above highVelThres,\n           % and reduced feedforward when above lowVelTh"
          "res (proportional\n           % to velocity)\n\n           \nuseKp = [0;0;0;0;0;0;0]; %Never use Kp\nuseKv = [1;"
          "1;1;0;0;0;1]; %Kv needs to be active for the base joints (1,2,3, and 7)\n           % When Ki is active, these v"
          "ariables control if also Kp and Kv\n           % should be active\n%TESTING:\npercentageTrqFfw = 0.9*ones(7,1);\n"
          "percentageFfwPos = percentageTrqFfw;\npercentageFfwNeg = percentageTrqFfw;\nposFric = coulomb_width;\nnegFric = "
          "coulomb_width;\n           % Variables to control the amount of torque feedforward\n           \nK = 1/3;\n    %"
          " The proportional feedback parameter for the feedback torque control\n    % loop\n\ngearRatio = [100;100;-100;10"
          "0;-101;100;100];\n\npersistent sampleIndex sampleIndexMod trqFltVec trqFltDelayVec;\nif isempty(sampleIndexMod) "
          "|| f_switch==0\n    sampleIndex = zeros(7,1);\n    sampleIndexMod = zeros(7,1); % obs! zero indexed, add 1 when "
          "used as index\n    trqFltVec = repmat(trqRaw',max(length_mean),1);\n    trqFltDelayVec = trqFltVec;\n    trqRawF"
          "lt = trqRaw;\nend\nfor k=1:7\n    if abs(velFlt(k))>KiHighThres(k,1) || abs(velFlt_noExtraFilt(k))>KiHighThres(k"
          ",2)\n        sampleIndex(k) = 0;\n        sampleIndexMod(k) = 0; % obs! zero indexed, add 1 when used as index\n"
          "        trqFltVec(:,k) = ones(max(length_mean),1)*trqRaw(k);\n        trqFltDelayVec(:,k) = trqFltVec(:,k);\n   "
          "     trqRawFlt(k) = trqRaw(k);\n    end\nend        % Reset the variables for detection of torque when Ki is act"
          "ive,\n           % either when f_switch is zero or the velocity is above the\n           % thresholds. Also used"
          " for initialization\n\n\nif isempty(saturated)\n    saturated = 0;\nend\n\nif isempty(trqAddHistory)\n    trqAdd"
          "History = zeros(7,3);\nend\n\nif isempty(extraTorqueOut)\n    extraTorqueOut = zeros(7,1);\nend\n\nif isempty(st"
          "artOffset)\n    startOffset = zeros(7,2);\nend\n\nif isempty(closeKiCounter)\n    closeKiCounter = zeros(7,2);\n"
          "end\n\nif isempty(torqueFeedforward)\n    torqueFeedforward = zeros(7,1);\nend\n\nif isempty(posRefOffset)\n    "
          "posRefOffset = zeros(7,1);\nend\n\nif isempty(trqUpdateAddition)\n    trqUpdateAddition = zeros(7,3);\nend\n\nif"
          " isempty(keepKiClosed)\n    keepKiClosed = KiHighDelay; % Initialize to standard delay (the amount of\nend      "
          "                       % samples that needs to be counted down to\n                                % allow for s"
          "etting Ki active\n\n                                \nif fricHelp==0\n    highKiActive = 0;\nend\n\njointsActive"
          " = [1;1;1;1;1;1;1]; %ABB-order\n           % The joints where leadthrough should be activated\n\n% maxRate = 0.2"
          "*[0.05 ; 0.05 ; 0.05 ; 0.005 ; 0.005 ; 0.005 ; 0.025];\n           % The maximum rate with which the torque feed"
          "forw\n\nKp = Kp_in;\nKv = Kv_in;\nKi = Ki_in;\n\njointLimitGain = [1.0;1.0;1.0;0.3;0.3;0.3;0.7].*coulomb_width;\n"
          "\nif f_switch==1\n    \n    trqRawFlt = trqFiltConstant.*trqRawFlt + (1-trqFiltConstant).*trqRaw;\n    % Filteri"
          "ng of the raw torque signal\n    \n    extraTorque = zeros(7,1);\n        % The extra torque feedforward (i.e., "
          "all trqFfw except the gravity\n        % torque)\n    \n    for k=1:7\n        if jointsActive(k)\n            \n"
          "            if startOffset(k,2)==0\n                startOffset(k,1) = trqRawFlt(k);\n                startOffse"
          "t(k,2) = 1;\n            end % If unset, save the torque offset\n            \n%             if (~(highKiActive)"
          " && abs(velFlt_noExtraFilt(k))>KiHighThres(k,2)) || (highKiActive && keepKiClosed(k)==0 && abs(velFlt_noExtraFil"
          "t(k))>KiHighThres(k,2))\n            if (keepStill && abs(velFlt_noExtraFilt(k))>KiHighThres(k,2)) || (highKiAct"
          "ive && keepKiClosed(k)==0 && abs(velFlt_noExtraFilt(k))>KiHighThres(k,2))\n                keepKiClosed(k) = 100"
          ";\n            end     % Safety stop of active Ki if the velocity exceeds the threshold\n                    % ("
          "setting keepKiClosed to 100 means that it will take\n                    % at least 100 samples until Ki will be"
          " active again)\n                \n            if (keepStill==1 && keepKiClosed(k)==0) || (highKiActive==1 && abs"
          "(velFlt(k))<KiHighThres(k,1) && keepKiClosed(k)==0 && max(closeKiCounter(k,:))==0)\n                Kp(k) = useK"
          "p(k)*Kp_in(k); % Activate Ki if either\n                Kv(k) = useKv(k)*Kv_in(k); % 1) keepStill is active (for"
          " tuning) and keepKiClosed==0\n                Ki(k) = 100*Ki_in(k);      % 2) highKiActive is active, the veloci"
          "ty is close to zero, keepKiClosed is zero, and closeKiCounter is zero\n                \n                startOf"
          "fset(k,2) = 0;\n                    % Reset the torque offset when the controller is\n                    % acti"
          "vated\n            else\n                Kp(k) = 0;\n                Kv(k) = 0; \n                Ki(k) = 0; \n "
          "           end\n            \n            if abs(velFlt(k))>highVelThres(k)\n                closeKiCounter(k,:)"
          " = [0 0];\n            end % Reset closeKiCounter if the velocity is high, as the detected force now has led to "
          "a high velocity, it does not need any priority\n            \n            if fricHelp==1\n                if vel"
          "Flt(k)>highVelThres(k) || closeKiCounter(k,1)>0\n                    extraTorque(k) = percentageFfwPos(k)*posFri"
          "c(k);\n                elseif velFlt(k)>lowVelThres(k)\n                    extraTorque(k) = percentageFfwPos(k)"
          "*posFric(k)*(velFlt(k)-lowVelThres(k))/(highVelThres(k)-lowVelThres(k));\n                elseif velFlt(k)<-high"
          "VelThres(k) || closeKiCounter(k,2)>0\n                    extraTorque(k) = -percentageFfwNeg(k)*negFric(k);\n   "
          "             elseif velFlt(k)<-lowVelThres(k)\n                    extraTorque(k) = -percentageFfwNeg(k)*negFric"
          "(k)*(-velFlt(k)-lowVelThres(k))/(highVelThres(k)-lowVelThres(k));\n                end\n            end\n       "
          "     % If the velocity exceeds the thresholds or that a torque has\n            % been detected when Ki was acti"
          "ve (closeKiCounter>0), then add\n            % an extra torque feedforward to make it easier to move the\n      "
          "      % robot\n        end\n    end\n    \n    \n    sliding_thres = zeros(7,2);\n    trqFltDelay = zeros(7,1);\n"
          "    for k=1:7\n        trqFltVec(sampleIndexMod(k)+1,k) = trqRaw(k);\n        trqFlt = mean(trqFltVec(1:length_m"
          "ean(k),k));\n        trqFltDelayVec(sampleIndexMod(k)+1,k) = trqFlt;\n        tmp = trqFltDelayVec(mod(sampleInd"
          "exMod(k)-delay_thres(k),length_mean(k))+1,k);\n        \n        if sampleIndex(k)<=length_mean(k)\n            "
          "sliding_thres(k,1) = gravTrqMotor(k) + coulomb_width(k)*1;\n            sliding_thres(k,2) = gravTrqMotor(k) + c"
          "oulomb_width(k)*-1;\n            trqFltDelay(k) = trqFlt;\n        else\n            tmpCount = min(sampleIndex("
          "k)-length_mean(k),length_mean(k));\n            trqFltDelay(k) = (tmpCount*tmp+(length_mean(k)-tmpCount)*trqFlt)"
          "/length_mean(k);\n            multFactor = max(1-1/(recoveryFactor(k)*length_mean(k))*(sampleIndex(k)-length_mea"
          "n(k)),0);\n            sliding_thres(k,:) = trqFltDelay(k) + [1 -1]*(slidingThresWidth(k)*(1-multFactor)+coulomb"
          "_width(k)*multFactor);\n        end\n        \n        sampleIndex(k) = sampleIndex(k) + 1;\n        sampleIndex"
          "Mod(k) = mod(sampleIndex(k),length_mean(k));\n    end\n    % Calculation of quantities that is used to detect to"
          "rques when Ki is\n    % active\n    \n    if highKiActive==1 && ~keepStill\n        for k=1:7\n            if jo"
          "intsActive(k)\n                if abs(velFlt(k))<KiHighThres(k,1) && keepKiClosed(k)==0 && max(closeKiCounter(k,"
          ":))==0\n                    if (trqRawFlt(k) > sliding_thres(k,1)) || (trqRawFlt(k)>(gravTrqMotor(k)+posFric(k))"
          ") \n                        extraTorque(k) = -percentageFfwNeg(k)*negFric(k);\n                        Ki(k) = 0"
          "*Ki_in(k);\n                        closeKiCounter(k,2) = 30;\n                    elseif (trqRawFlt(k) < slidin"
          "g_thres(k,2)) || (trqRawFlt(k)<(gravTrqMotor(k)-negFric(k))) \n                        extraTorque(k) = percenta"
          "geFfwPos(k)*posFric(k);\n                        Ki(k) = 0*Ki_in(k);\n                        closeKiCounter(k,1"
          ") = 30;\n                    end\n                    \n                end\n            end\n        end\n    e"
          "nd\n    % Detection of torques when Ki is active. If a detection is made, Ki is\n    % set to zero and closeKiCo"
          "unter is set to 30, and Ki will not be\n    % active before these 30 samples has been counted down\n    \n    if"
          " sensorOn == 1\n        deadZone = 0.0001;\n        for k=1:7\n            extMotorTorque = extTorque(k)/gearRat"
          "io(k);\n            if extMotorTorque>deadZone\n                extraTorque(k) = percentageFfwPos(k)*posFric(k) "
          "+ extMotorTorque-deadZone;\n                closeKiCounter(k,1) = 30;\n                Ki(k) = 0*Ki_in(k);\n    "
          "        elseif extMotorTorque<(-deadZone)\n                extraTorque(k) = -percentageFfwNeg(k)*negFric(k) + ex"
          "tMotorTorque+deadZone;\n                closeKiCounter(k,2) = 30;\n                Ki(k) = 0*Ki_in(k);\n        "
          "    end\n        end\n    end\n    % Add extra torque due to forces measured by force sensor    \n    % TESTING:"
          "\n%     extraTorque = extraTorque + qtorque;\n    maxExtraTorqueRate = maxRate/17;\n%     maxExtraTorqueRate = m"
          "axRate/4;\n        % The rate with which the torque is allowed to change\n    extraTorqueOut = extraTorqueOut + "
          "max(-maxExtraTorqueRate,min(maxExtraTorqueRate,extraTorque-extraTorqueOut));\n        % A rate limited version o"
          "f extraTorque is saved in extraTorqueOut\n    yMeas = trqRaw + sum(trqAddHistory,2);\n        % A predicition of"
          " the trq\n        \n    for k=1:7\n        \n            % Joint limit avoidance\n        jointLimitTorque = 0;\n"
          "        if jointsActive(k)      \n            if posRaw(k)<jointLimitStartMotorRad(k,1)\n                jointLi"
          "mitTorque = -1*jointLimitGain(k)*(posRaw(k)-jointLimitStartMotorRad(k,1));\n            elseif posRaw(k)>jointLi"
          "mitStartMotorRad(k,2)\n                jointLimitTorque = -1*jointLimitGain(k)*(posRaw(k)-jointLimitStartMotorRa"
          "d(k,2));\n            end\n        end\n        \n        % TESTING TESTING\n        if 1 || jointsActive(k) && "
          "(abs(velFlt(k))>KiHighThres(k,1) || keepKiClosed(k)>0 || max(closeKiCounter(k,:))>0)\n                \n        "
          "    ffwAdd = (gravTrqMotor(k)+extraTorqueOut(k)+jointLimitTorque)-trqUpdateAddition(k,2) + qtorque(k);\n        "
          "    % Update of the feedforward torque\n            fbAdd = K*(gravTrqMotor(k)+extraTorqueOut(k)+jointLimitTorqu"
          "e - yMeas(k)) + qtorque(k);\n            % Update of the feedback torque\n            \n            ffwAdd = max"
          "(-maxRate(k),min(maxRate(k),ffwAdd));\n            % Limit the rate of the feedforward update\n            \n   "
          "         trqUpdateAddition(k,2) = trqUpdateAddition(k,2) + ffwAdd;\n            % Update the accumulated feedfor"
          "ward torque\n            \n            %if abs(ffwAdd)>0\n            %    trqUpdateAddition(k,3) = 3;\n        "
          "    %else\n            %    trqUpdateAddition(k,3) = max(0,trqUpdateAddition(k,3)-1);\n            %end % Set th"
          "e counter to 3 if the feedforward torque addition\n            % is nonzero, otherwise count it down\n          "
          "  \n            magFbAdd = maxRate(k); % Allowed magnitude for the feedback torque\n            %if trqUpdateAdd"
          "ition(k,3)>0\n            %    magFbAdd = 0;\n            %end % Set the magnitude to zero if a feedforward torq"
          "ue change\n            % was made in the last three samples\n            \n            fbAdd = max(-magFbAdd,min"
          "(magFbAdd,fbAdd));\n            \n            trqUpdateAddition(k,1) = ffwAdd+fbAdd;\n                   \n     "
          "   else\n            trqUpdateAddition(k,1:3) = [0 0 0];\n            % Reset if joint is not active\n        en"
          "d\n    end\n    \n    if ~keepStill\n        torqueFeedforward = torqueFeedforward + trqUpdateAddition(:,1);\n  "
          "      % add the torque feedforward addition\n    end\n    \n    for k=1:7\n        closeKiCounter(k,1) = max(clo"
          "seKiCounter(k,1)-1,0);\n        closeKiCounter(k,2) = max(closeKiCounter(k,2)-1,0);\n        % Count down closeK"
          "iCounter\n        if abs(velFlt(k))<KiHighThres(k,1)\n            keepKiClosed(k) = max(keepKiClosed(k)-1,0);\n "
          "           % If the velocity is close to zero, then count down the value\n            % of keepKiClosed\n       "
          " else\n            keepKiClosed(k) = max(keepKiClosed(k),KiHighDelay(k));\n            % If the velocity exceeds"
          " the threshold, then reset the counter\n            % in keepKiClosed\n        end\n    end\n    \n    \n    for"
          " k=1:7\n        if useKp(k)==0\n            posRefOffset(k) = jointsActive(k)*(posRaw(k) - posRefIn(k));\n      "
          "      % Make sure that the position reference follows the actual\n            % position of the robot (to avoid "
          "errors in the underlyiong\n            % ABB-controller)\n        end\n    end\nelse\n    startOffset(:,2) = zer"
          "os(7,1);\n    \n    if resetPosRef\n        posRefOffset = zeros(7,1);\n    end\nend\n\n\njointLimitsDeg = [-170"
          " 170\n                  -140  45\n                  -125  80\n                  -290 290\n                  -95 "
          "130\n                  -180 180\n                  -170 170];\nzoneDeg = 10;\n\n\njointLimitsMotorrad = jointLim"
          "itsDeg*pi/180.*[gearRatio gearRatio];\nzoneMotorrad = zoneDeg*pi/180*abs(gearRatio);\n\nvec1 = (posRaw-(min(join"
          "tLimitsMotorrad,[],2))-zoneMotorrad)<0;\nvec2 = ((max(jointLimitsMotorrad,[],2))-posRaw-zoneMotorrad)<0;\n      "
          "          \nif  f_switch==1 && (saturated==1 || any(vec1) || any(vec2))  && 0%|| max(abs(torqueFeedforward))>max"
          "Trq\n    torqueFeedforward = zeros(7,1);\n    \n    posRawInside = min(max(posRaw,min(jointLimitsMotorrad,[],2)+"
          "zoneMotorrad),max(jointLimitsMotorrad,[],2)-zoneMotorrad);\n    posRefOffset = (posRawInside - posRefIn);\n    s"
          "aturated = 1;\n    \n    Kp = Kp_in;\n    Kv = Kv_in;\n    Ki = Ki_in;\nend\n\ntrqAddHistory(:,2:3) = trqAddHist"
          "ory(:,1:2);\ntrqAddHistory(:,1) = trqUpdateAddition(:,1);\n\nposRef = posRefIn + posRefOffset;\n\ntrqFfw = torqu"
          "eFeedforward;\ntest = posRefOffset;\nsat = saturated;\n"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    101
        position		    [23.5747 49.5747 7]
        chart		    99
        linkNode		    [99 0 0]
        subviewer		    99
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      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	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    99
        linkNode		    [99 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    99
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    103
        ssIdNumber		    4
        name		    "Kp_in"
        linkNode		    [99 0 104]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    104
        ssIdNumber		    5
        name		    "Kp"
        linkNode		    [99 103 105]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    105
        ssIdNumber		    6
        name		    "Kv_in"
        linkNode		    [99 104 106]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    106
        ssIdNumber		    7
        name		    "Ki_in"
        linkNode		    [99 105 107]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    107
        ssIdNumber		    8
        name		    "gravTrqMotor"
        linkNode		    [99 106 108]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    108
        ssIdNumber		    9
        name		    "trqRaw"
        linkNode		    [99 107 109]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    109
        ssIdNumber		    10
        name		    "posRefIn"
        linkNode		    [99 108 110]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    110
        ssIdNumber		    11
        name		    "posRaw"
        linkNode		    [99 109 111]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    111
        ssIdNumber		    12
        name		    "f_switch"
        linkNode		    [99 110 112]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    112
        ssIdNumber		    13
        name		    "velFlt"
        linkNode		    [99 111 113]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    113
        ssIdNumber		    14
        name		    "highKiActive"
        linkNode		    [99 112 114]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    114
        ssIdNumber		    15
        name		    "keepStill"
        linkNode		    [99 113 115]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    115
        ssIdNumber		    16
        name		    "velFlt_noExtraFilt"
        linkNode		    [99 114 116]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    116
        ssIdNumber		    17
        name		    "Kv"
        linkNode		    [99 115 117]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    117
        ssIdNumber		    18
        name		    "Ki"
        linkNode		    [99 116 118]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    118
        ssIdNumber		    19
        name		    "trqFfw"
        linkNode		    [99 117 119]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    119
        ssIdNumber		    20
        name		    "posRef"
        linkNode		    [99 118 120]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    120
        ssIdNumber		    22
        name		    "coulomb_width"
        linkNode		    [99 119 121]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    121
        ssIdNumber		    23
        name		    "percentageTrqFfw"
        linkNode		    [99 120 122]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    122
        ssIdNumber		    24
        name		    "KiHighThres"
        linkNode		    [99 121 123]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    123
        ssIdNumber		    25
        name		    "slidingThresWidth"
        linkNode		    [99 122 124]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    124
        ssIdNumber		    26
        name		    "maxRate"
        linkNode		    [99 123 125]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    125
        ssIdNumber		    27
        name		    "resetPosRef"
        linkNode		    [99 124 126]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    126
        ssIdNumber		    28
        name		    "jointLimitStartMotorRad"
        linkNode		    [99 125 127]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    127
        ssIdNumber		    29
        name		    "extTorque"
        linkNode		    [99 126 128]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    128
        ssIdNumber		    30
        name		    "sensorOn"
        linkNode		    [99 127 129]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    129
        ssIdNumber		    31
        name		    "fricHelp"
        linkNode		    [99 128 130]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    130
        ssIdNumber		    32
        name		    "test"
        linkNode		    [99 129 131]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    131
        ssIdNumber		    33
        name		    "sat"
        linkNode		    [99 130 132]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    132
        ssIdNumber		    34
        name		    "qtorque"
        linkNode		    [99 131 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    133
        name		    "Left/leadthroug control/MATLAB Function"
        machine		    1
        chart		    99
      }
      chart {
        id			    134
        name		    "Right/leadthroug control/MATLAB Function"
        windowPosition	    [390.75 188.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 135 0 0]
        firstTransition	    137
        firstJunction	    136
        viewObj		    134
        machine		    1
        ssIdHighWaterMark	    33
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    138
        chartFileNumber	    13
        disableImplicitCasting  1
        eml {
          name		      "fcn"
        }
      }
      state {
        id			    135
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    134
        treeNode		    [134 0 0 0]
        superState		    SUBCHART
        subviewer		    134
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function [Kp,Kv,Ki,trqFfw,posRef,rstill] = fcn(Kp_in,Kv_in,Ki_in,gravTrqMotor,trqRaw,posRefIn,pos"
          "Raw,f_switch,velFlt,highKiActive,keepStill,velFlt_noExtraFilt,coulomb_width,percentageTrqFfw,KiHighThres,sliding"
          "ThresWidth,maxRate,resetPosRef,jointLimitStartMotorRad,extTorque,sensorOn,fricHelp,qtorque)\n%#codegen\n% All ve"
          "ctors are given in ABB-order\n\npersistent torqueFeedforward;\n           % Used to account for the accumulated "
          "torqe feedforward (size 7x1)\npersistent posRefOffset;\n           % The offset from the incoming position refer"
          "ence, needs to be\n           % updated to the current position to avoid the safety system of\n           % the "
          "ABB controller to give errors\npersistent trqUpdateAddition;\n           % The torque feedforward addition is sa"
          "ved in this variable. The\n           % first column is the total torque feedforward, the second\n           % c"
          "olumn the \"feedforward\" part, i.e., the desired torque which\n           % has to be corrected with a feedback"
          " part to handle\n           % disturbances. The last column contains a counter that is used\n           % to avo"
          "id using feedback 3 samples after any feedforward change\n           % (3 samples is the delay from the feedforw"
          "ard signal is set\n           % until a change can be seen in trqRaw)\npersistent trqRawFlt;\n           % Filte"
          "red torque signal\npersistent keepKiClosed; \n           % Variable of size 7x1 (one for each joint). If zero, K"
          "i should \n           % be active for the respective joints. Also works as a counter, \n           % it is set t"
          "o a value when a joint is moving, and then counted \n           % down when the joint is still (below a threshol"
          "d)\npersistent closeKiCounter;  \n           % Variable of size 7x2. It is used to make sure that a detected\n  "
          "         % torque will have time to make an impact. When a torque is\n           % detected with Ki active, clos"
          "eKiCounter is set to a value that\n           % is then counted down. While closeKiCounter is non-zero, Ki\n    "
          "       % will not be active and a torque feedforward will be used. The\n           % first column is used if a n"
          "egative torque is detected, and the\n           % second column if a positive torque is detected\n           \np"
          "ersistent startOffset;\n           % Variable used to handle the offset torque present for each\n           % jo"
          "int. The first column is used to save this torque, the\n           % second column to tell if it has been set or"
          " not\npersistent extraTorqueOut;\n           % Variable to handle ramping of the extra feedforward torque\npersi"
          "stent trqAddHistory\n           % The history of added feedforward torques. It is used to\n           % predict "
          "trqRaw during the delay that is present.\n           \npersistent saturated;           \n\nrstill = zeros(7,1);\n"
          "\nlength_mean = [80;80;80;80;80;80;80];\ndelay_thres = [60;60;60;60;60;60;60];% length_mean has to be larger tha"
          "n delay thres\nrecoveryFactor = [1.5;1.5;1.5;1.5;1.5;1.5;1.5];\n% slidingThresWidth = [0.006;0.006;0.003;0.00045"
          ";0.0006;0.0004;0.003;];\n           % These are parameters for detection of torque when Ki are\n           % act"
          "ive\n           \ntrqFiltConstant = [0.8;0.8;0.8;0.8;0.8;0.8;0.8];\n           % The filter constant for the tor"
          "que low pass filter\n\n% KiHighThres = [0.1 1;0.1 1;0.1 1;0.3 1;0.1 1;0.1 1;0.1 1]; The first\n% column is the t"
          "hreshold for the filtered version of velFlt, and the\n% second column the threshold for the non-filtered version"
          " of velFlt\nKiHighDelay = [30 ; 30 ; 30 ; 30 ; 30 ; 30 ; 30];\n           % The standard delay for counting down"
          " keepKiClosed when the\n           % measured velocity is below KiHighThres\n           \nhighVelThres = [5;5;5;"
          "5;5;5;5];\nlowVelThres = [2;2;2;2;2;2;2];\n           % Thresholds for high and low velocities. Full torque\n   "
          "        % feedforward is active when the velocity is above highVelThres,\n           % and reduced feedforward w"
          "hen above lowVelThres (proportional\n           % to velocity)\n\n           \nuseKp = [0;0;0;0;0;0;0]; %Never u"
          "se Kp\nuseKv = [1;1;1;0;0;0;1]; %Kv needs to be active for the base joints (1,2,3, and 7)\n           % When Ki "
          "is active, these variables control if also Kp and Kv\n           % should be active\n%TESTING:\npercentageTrqFfw"
          " = 0.9*ones(7,1);\npercentageFfwPos = percentageTrqFfw;\npercentageFfwNeg = percentageTrqFfw;\nposFric = coulomb"
          "_width;\nnegFric = coulomb_width;\n           % Variables to control the amount of torque feedforward\n         "
          "  \nK = 1/3;\n    % The proportional feedback parameter for the feedback torque control\n    % loop\n\ngearRatio"
          " = [100;100;-100;100;-101;100;100];\n\npersistent sampleIndex sampleIndexMod trqFltVec trqFltDelayVec;\nif isemp"
          "ty(sampleIndexMod) || f_switch==0\n    sampleIndex = zeros(7,1);\n    sampleIndexMod = zeros(7,1); % obs! zero i"
          "ndexed, add 1 when used as index\n    trqFltVec = repmat(trqRaw',max(length_mean),1);\n    trqFltDelayVec = trqF"
          "ltVec;\n    trqRawFlt = trqRaw;\nend\nfor k=1:7\n    if abs(velFlt(k))>KiHighThres(k,1) || abs(velFlt_noExtraFil"
          "t(k))>KiHighThres(k,2)\n        sampleIndex(k) = 0;\n        sampleIndexMod(k) = 0; % obs! zero indexed, add 1 w"
          "hen used as index\n        trqFltVec(:,k) = ones(max(length_mean),1)*trqRaw(k);\n        trqFltDelayVec(:,k) = t"
          "rqFltVec(:,k);\n        trqRawFlt(k) = trqRaw(k);\n    end\nend        % Reset the variables for detection of to"
          "rque when Ki is active,\n           % either when f_switch is zero or the velocity is above the\n           % th"
          "resholds. Also used for initialization\n\n\nif isempty(saturated)\n    saturated = 0;\nend\n\nif isempty(trqAddH"
          "istory)\n    trqAddHistory = zeros(7,3);\nend\n\nif isempty(extraTorqueOut)\n    extraTorqueOut = zeros(7,1);\ne"
          "nd\n\nif isempty(startOffset)\n    startOffset = zeros(7,2);\nend\n\nif isempty(closeKiCounter)\n    closeKiCoun"
          "ter = zeros(7,2);\nend\n\nif isempty(torqueFeedforward)\n    torqueFeedforward = zeros(7,1);\nend\n\nif isempty("
          "posRefOffset)\n    posRefOffset = zeros(7,1);\nend\n\nif isempty(trqUpdateAddition)\n    trqUpdateAddition = zer"
          "os(7,3);\nend\n\nif isempty(keepKiClosed)\n    keepKiClosed = KiHighDelay; % Initialize to standard delay (the a"
          "mount of\nend                             % samples that needs to be counted down to\n                          "
          "      % allow for setting Ki active\n\n                                \nif fricHelp==0\n    highKiActive = 0;\n"
          "end\n\njointsActive = [1;1;1;1;1;1;1]; %ABB-order\n           % The joints where leadthrough should be activated"
          "\n\n% maxRate = 0.2*[0.05 ; 0.05 ; 0.05 ; 0.005 ; 0.005 ; 0.005 ; 0.025];\n           % The maximum rate with wh"
          "ich the torque feedforw\n\nKp = Kp_in;\nKv = Kv_in;\nKi = Ki_in;\n\njointLimitGain = [1.0;1.0;1.0;0.3;0.3;0.3;0."
          "7].*coulomb_width;\n\nif f_switch==1\n    \n    trqRawFlt = trqFiltConstant.*trqRawFlt + (1-trqFiltConstant).*tr"
          "qRaw;\n    % Filtering of the raw torque signal\n    \n    extraTorque = zeros(7,1);\n        % The extra torque"
          " feedforward (i.e., all trqFfw except the gravity\n        % torque)\n    \n    for k=1:7\n        if jointsActi"
          "ve(k)\n            \n            if startOffset(k,2)==0\n                startOffset(k,1) = trqRawFlt(k);\n     "
          "           startOffset(k,2) = 1;\n            end % If unset, save the torque offset\n            \n%           "
          "  if (~(highKiActive) && abs(velFlt_noExtraFilt(k))>KiHighThres(k,2)) || (highKiActive && keepKiClosed(k)==0 && "
          "abs(velFlt_noExtraFilt(k))>KiHighThres(k,2))\n            if (keepStill && abs(velFlt_noExtraFilt(k))>KiHighThre"
          "s(k,2)) || (highKiActive && keepKiClosed(k)==0 && abs(velFlt_noExtraFilt(k))>KiHighThres(k,2))\n                "
          "keepKiClosed(k) = 100;\n            end     % Safety stop of active Ki if the velocity exceeds the threshold\n  "
          "                  % (setting keepKiClosed to 100 means that it will take\n                    % at least 100 sam"
          "ples until Ki will be active again)\n                \n            if (keepStill==1 && keepKiClosed(k)==0) || (h"
          "ighKiActive==1 && abs(velFlt(k))<KiHighThres(k,1) && keepKiClosed(k)==0 && max(closeKiCounter(k,:))==0)\n       "
          "         Kp(k) = useKp(k)*Kp_in(k); % Activate Ki if either\n                Kv(k) = useKv(k)*Kv_in(k); % 1) kee"
          "pStill is active (for tuning) and keepKiClosed==0\n                Ki(k) = 100*Ki_in(k);      % 2) highKiActive "
          "is active, the velocity is close to zero, keepKiClosed is zero, and closeKiCounter is zero\n                \n  "
          "              startOffset(k,2) = 0;\n                    % Reset the torque offset when the controller is\n     "
          "               % activated\n            else\n                Kp(k) = 0;\n                Kv(k) = 0; \n         "
          "       Ki(k) = 0; \n            end\n            \n            if abs(velFlt(k))>highVelThres(k)\n              "
          "  closeKiCounter(k,:) = [0 0];\n            end % Reset closeKiCounter if the velocity is high, as the detected "
          "force now has led to a high velocity, it does not need any priority\n            \n            if fricHelp==1\n "
          "               if velFlt(k)>highVelThres(k) || closeKiCounter(k,1)>0\n                    extraTorque(k) = perce"
          "ntageFfwPos(k)*posFric(k);\n                elseif velFlt(k)>lowVelThres(k)\n                    extraTorque(k) "
          "= percentageFfwPos(k)*posFric(k)*(velFlt(k)-lowVelThres(k))/(highVelThres(k)-lowVelThres(k));\n                e"
          "lseif velFlt(k)<-highVelThres(k) || closeKiCounter(k,2)>0\n                    extraTorque(k) = -percentageFfwNe"
          "g(k)*negFric(k);\n                elseif velFlt(k)<-lowVelThres(k)\n                    extraTorque(k) = -percen"
          "tageFfwNeg(k)*negFric(k)*(-velFlt(k)-lowVelThres(k))/(highVelThres(k)-lowVelThres(k));\n                end\n   "
          "         end\n            % If the velocity exceeds the thresholds or that a torque has\n            % been dete"
          "cted when Ki was active (closeKiCounter>0), then add\n            % an extra torque feedforward to make it easie"
          "r to move the\n            % robot\n        end\n    end\n    \n    \n    sliding_thres = zeros(7,2);\n    trqFl"
          "tDelay = zeros(7,1);\n    for k=1:7\n        trqFltVec(sampleIndexMod(k)+1,k) = trqRaw(k);\n        trqFlt = mea"
          "n(trqFltVec(1:length_mean(k),k));\n        trqFltDelayVec(sampleIndexMod(k)+1,k) = trqFlt;\n        tmp = trqFlt"
          "DelayVec(mod(sampleIndexMod(k)-delay_thres(k),length_mean(k))+1,k);\n        \n        if sampleIndex(k)<=length"
          "_mean(k)\n            sliding_thres(k,1) = gravTrqMotor(k) + coulomb_width(k)*1;\n            sliding_thres(k,2)"
          " = gravTrqMotor(k) + coulomb_width(k)*-1;\n            trqFltDelay(k) = trqFlt;\n        else\n            tmpCo"
          "unt = min(sampleIndex(k)-length_mean(k),length_mean(k));\n            trqFltDelay(k) = (tmpCount*tmp+(length_mea"
          "n(k)-tmpCount)*trqFlt)/length_mean(k);\n            multFactor = max(1-1/(recoveryFactor(k)*length_mean(k))*(sam"
          "pleIndex(k)-length_mean(k)),0);\n            sliding_thres(k,:) = trqFltDelay(k) + [1 -1]*(slidingThresWidth(k)*"
          "(1-multFactor)+coulomb_width(k)*multFactor);\n        end\n        \n        sampleIndex(k) = sampleIndex(k) + 1"
          ";\n        sampleIndexMod(k) = mod(sampleIndex(k),length_mean(k));\n    end\n    % Calculation of quantities tha"
          "t is used to detect torques when Ki is\n    % active\n    \n    if highKiActive==1 && ~keepStill\n        for k="
          "1:7\n            if jointsActive(k)\n                if abs(velFlt(k))<KiHighThres(k,1) && keepKiClosed(k)==0 &&"
          " max(closeKiCounter(k,:))==0\n                    if (trqRawFlt(k) > sliding_thres(k,1)) || (trqRawFlt(k)>(gravT"
          "rqMotor(k)+posFric(k))) \n                        extraTorque(k) = -percentageFfwNeg(k)*negFric(k);\n           "
          "             Ki(k) = 0*Ki_in(k);\n                        closeKiCounter(k,2) = 30;\n                    elseif "
          "(trqRawFlt(k) < sliding_thres(k,2)) || (trqRawFlt(k)<(gravTrqMotor(k)-negFric(k))) \n                        ext"
          "raTorque(k) = percentageFfwPos(k)*posFric(k);\n                        Ki(k) = 0*Ki_in(k);\n                    "
          "    closeKiCounter(k,1) = 30;\n                    end\n                    \n                end\n            e"
          "nd\n        end\n    end\n    % Detection of torques when Ki is active. If a detection is made, Ki is\n    % set"
          " to zero and closeKiCounter is set to 30, and Ki will not be\n    % active before these 30 samples has been coun"
          "ted down\n    \n    if sensorOn == 1\n        deadZone = 0.0001;\n        for k=1:7\n            extMotorTorque "
          "= extTorque(k)/gearRatio(k);\n            if extMotorTorque>deadZone\n                extraTorque(k) = percentag"
          "eFfwPos(k)*posFric(k) + extMotorTorque-deadZone;\n                closeKiCounter(k,1) = 30;\n                Ki("
          "k) = 0*Ki_in(k);\n            elseif extMotorTorque<(-deadZone)\n                extraTorque(k) = -percentageFfw"
          "Neg(k)*negFric(k) + extMotorTorque+deadZone;\n                closeKiCounter(k,2) = 30;\n                Ki(k) ="
          " 0*Ki_in(k);\n            end\n        end\n    end\n    % Add extra torque due to forces measured by force sens"
          "or    \n    % TESTING:\n%     extraTorque = extraTorque + qtorque;\n    maxExtraTorqueRate = maxRate/17;\n%     "
          "maxExtraTorqueRate = maxRate/4;\n        % The rate with which the torque is allowed to change\n    extraTorqueO"
          "ut = extraTorqueOut + max(-maxExtraTorqueRate,min(maxExtraTorqueRate,extraTorque-extraTorqueOut));\n        % A "
          "rate limited version of extraTorque is saved in extraTorqueOut\n    yMeas = trqRaw + sum(trqAddHistory,2);\n    "
          "    % A predicition of the trq\n        \n    for k=1:7\n        \n            % Joint limit avoidance\n        "
          "jointLimitTorque = 0;\n        if jointsActive(k)      \n            if posRaw(k)<jointLimitStartMotorRad(k,1)\n"
          "                jointLimitTorque = -1*jointLimitGain(k)*(posRaw(k)-jointLimitStartMotorRad(k,1));\n            e"
          "lseif posRaw(k)>jointLimitStartMotorRad(k,2)\n                jointLimitTorque = -1*jointLimitGain(k)*(posRaw(k)"
          "-jointLimitStartMotorRad(k,2));\n            end\n        end\n        \n        % TESTING TESTING:\n        if "
          "1 || jointsActive(k) && (abs(velFlt(k))>KiHighThres(k,1) || keepKiClosed(k)>0 || max(closeKiCounter(k,:))>0)\n  "
          "              \n                \n            ffwAdd = (gravTrqMotor(k)+extraTorqueOut(k)+jointLimitTorque)-trqU"
          "pdateAddition(k,2) + qtorque(k);\n            % Update of the feedforward torque\n            fbAdd = K*(gravTrq"
          "Motor(k)+extraTorqueOut(k)+jointLimitTorque - yMeas(k)) + qtorque(k);\n            % Update of the feedback torq"
          "ue\n            \n            ffwAdd = max(-maxRate(k),min(maxRate(k),ffwAdd));\n            % Limit the rate of"
          " the feedforward update\n            \n            trqUpdateAddition(k,2) = trqUpdateAddition(k,2) + ffwAdd;\n  "
          "          % Update the accumulated feedforward torque\n            \n            %if abs(ffwAdd)>0\n            "
          "%    trqUpdateAddition(k,3) = 3;\n            %else\n            %    trqUpdateAddition(k,3) = max(0,trqUpdateAd"
          "dition(k,3)-1);\n            %end % Set the counter to 3 if the feedforward torque addition\n            % is no"
          "nzero, otherwise count it down\n            \n            magFbAdd = maxRate(k); % Allowed magnitude for the fee"
          "dback torque\n            %if trqUpdateAddition(k,3)>0\n            %    magFbAdd = 0;\n            %end % Set t"
          "he magnitude to zero if a feedforward torque change\n            % was made in the last three samples\n         "
          "   \n            fbAdd = max(-magFbAdd,min(magFbAdd,fbAdd));\n            \n            trqUpdateAddition(k,1) ="
          " ffwAdd+fbAdd;\n                   \n        else\n            rstill(k) = 1;\n            trqUpdateAddition(k,1"
          ":3) = [0 0 0];\n            % Reset if joint is not active\n        end\n    end\n    \n    if ~keepStill\n     "
          "   torqueFeedforward = torqueFeedforward + trqUpdateAddition(:,1);\n        % add the torque feedforward additio"
          "n\n    end\n    \n    for k=1:7\n        closeKiCounter(k,1) = max(closeKiCounter(k,1)-1,0);\n        closeKiCou"
          "nter(k,2) = max(closeKiCounter(k,2)-1,0);\n        % Count down closeKiCounter\n        if abs(velFlt(k))<KiHigh"
          "Thres(k,1)\n            keepKiClosed(k) = max(keepKiClosed(k)-1,0);\n            % If the velocity is close to z"
          "ero, then count down the value\n            % of keepKiClosed\n        else\n            keepKiClosed(k) = max(k"
          "eepKiClosed(k),KiHighDelay(k));\n            % If the velocity exceeds the threshold, then reset the counter\n  "
          "          % in keepKiClosed\n        end\n    end\n    \n    \n    for k=1:7\n        if useKp(k)==0\n          "
          "  posRefOffset(k) = jointsActive(k)*(posRaw(k) - posRefIn(k));\n            % Make sure that the position refere"
          "nce follows the actual\n            % position of the robot (to avoid errors in the underlyiong\n            % A"
          "BB-controller)\n        end\n    end\nelse\n    startOffset(:,2) = zeros(7,1);\n    \n    if resetPosRef\n      "
          "  posRefOffset = zeros(7,1);\n    end\nend\n\n\njointLimitsDeg = [-170 170\n                  -140  45\n        "
          "          -125  80\n                  -290 290\n                  -95 130\n                  -180 180\n         "
          "         -170 170];\nzoneDeg = 10;\n\n\njointLimitsMotorrad = jointLimitsDeg*pi/180.*[gearRatio gearRatio];\nzon"
          "eMotorrad = zoneDeg*pi/180*abs(gearRatio);\n\nvec1 = (posRaw-(min(jointLimitsMotorrad,[],2))-zoneMotorrad)<0;\nv"
          "ec2 = ((max(jointLimitsMotorrad,[],2))-posRaw-zoneMotorrad)<0;\n                \nif  f_switch==1 && (saturated="
          "=1 || any(vec1) || any(vec2)) %|| max(abs(torqueFeedforward))>maxTrq\n    torqueFeedforward = zeros(7,1);\n    \n"
          "    posRawInside = min(max(posRaw,min(jointLimitsMotorrad,[],2)+zoneMotorrad),max(jointLimitsMotorrad,[],2)-zone"
          "Motorrad);\n    posRefOffset = (posRawInside - posRefIn);\n    saturated = 1;\n    \n    Kp = Kp_in;\n    Kv = K"
          "v_in;\n    Ki = Ki_in;\nend\n\ntrqAddHistory(:,2:3) = trqAddHistory(:,1:2);\ntrqAddHistory(:,1) = trqUpdateAddit"
          "ion(:,1);\n\nposRef = posRefIn + posRefOffset;\n\ntrqFfw = torqueFeedforward;"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    136
        position		    [23.5747 49.5747 7]
        chart		    134
        linkNode		    [134 0 0]
        subviewer		    134
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    137
        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		      136
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    134
        linkNode		    [134 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    134
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    138
        ssIdNumber		    4
        name		    "Kp_in"
        linkNode		    [134 0 139]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    139
        ssIdNumber		    5
        name		    "Kp"
        linkNode		    [134 138 140]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    140
        ssIdNumber		    6
        name		    "Kv_in"
        linkNode		    [134 139 141]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    141
        ssIdNumber		    7
        name		    "Ki_in"
        linkNode		    [134 140 142]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    142
        ssIdNumber		    8
        name		    "gravTrqMotor"
        linkNode		    [134 141 143]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    143
        ssIdNumber		    9
        name		    "trqRaw"
        linkNode		    [134 142 144]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    144
        ssIdNumber		    10
        name		    "posRefIn"
        linkNode		    [134 143 145]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    145
        ssIdNumber		    11
        name		    "posRaw"
        linkNode		    [134 144 146]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    146
        ssIdNumber		    12
        name		    "f_switch"
        linkNode		    [134 145 147]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    147
        ssIdNumber		    13
        name		    "velFlt"
        linkNode		    [134 146 148]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    148
        ssIdNumber		    14
        name		    "highKiActive"
        linkNode		    [134 147 149]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    149
        ssIdNumber		    15
        name		    "keepStill"
        linkNode		    [134 148 150]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    150
        ssIdNumber		    16
        name		    "velFlt_noExtraFilt"
        linkNode		    [134 149 151]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    151
        ssIdNumber		    17
        name		    "Kv"
        linkNode		    [134 150 152]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    152
        ssIdNumber		    18
        name		    "Ki"
        linkNode		    [134 151 153]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    153
        ssIdNumber		    19
        name		    "trqFfw"
        linkNode		    [134 152 154]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    154
        ssIdNumber		    20
        name		    "posRef"
        linkNode		    [134 153 155]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    155
        ssIdNumber		    22
        name		    "coulomb_width"
        linkNode		    [134 154 156]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    156
        ssIdNumber		    23
        name		    "percentageTrqFfw"
        linkNode		    [134 155 157]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    157
        ssIdNumber		    24
        name		    "KiHighThres"
        linkNode		    [134 156 158]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    158
        ssIdNumber		    25
        name		    "slidingThresWidth"
        linkNode		    [134 157 159]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    159
        ssIdNumber		    26
        name		    "maxRate"
        linkNode		    [134 158 160]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    160
        ssIdNumber		    27
        name		    "resetPosRef"
        linkNode		    [134 159 161]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    161
        ssIdNumber		    28
        name		    "jointLimitStartMotorRad"
        linkNode		    [134 160 162]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    162
        ssIdNumber		    29
        name		    "extTorque"
        linkNode		    [134 161 163]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    163
        ssIdNumber		    30
        name		    "sensorOn"
        linkNode		    [134 162 164]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    164
        ssIdNumber		    31
        name		    "fricHelp"
        linkNode		    [134 163 165]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    165
        ssIdNumber		    32
        name		    "rstill"
        linkNode		    [134 164 166]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    166
        ssIdNumber		    33
        name		    "qtorque"
        linkNode		    [134 165 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    167
        name		    "Right/leadthroug control/MATLAB Function"
        machine		    1
        chart		    134
      }
      chart {
        id			    168
        name		    "botharms_get_torque/coulFric"
        windowPosition	    [375.75 203.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 169 0 0]
        firstTransition	    171
        firstJunction	    170
        viewObj		    168
        machine		    1
        toolbarMode		    LIBRARY_TOOLBAR
        ssIdHighWaterMark	    10
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    172
        chartFileNumber	    14
        disableImplicitCasting  1
        eml {
          name		      "coulFric"
        }
      }
      state {
        id			    169
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    168
        treeNode		    [168 0 0 0]
        superState		    SUBCHART
        subviewer		    168
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function [trqRight, trqLeft] = coulFric(coulRight, q_dot, coulLeft,dith)\n\n\ndithScale = 0.75;\n"
          "\nfullThres = 0.1;\nslowThres = fullThres/2;\n\ntrqThres = 0.1;\n\nhelpRate = 0.30;\n\ntrqRight = zeros(7,1);\nt"
          "rqLeft = zeros(7,1);\n\n\n% RIGHT:\nfor k = 1:7\n    if abs(q_dot(k)) > fullThres\n        trqRight(k) = coulRig"
          "ht(k)*sign(q_dot(k))*helpRate;\n    elseif abs(q_dot(k)) > slowThres\n        trqRight(k) = coulRight(k)*(abs(q_"
          "dot(k))-slowThres)/(fullThres-slowThres)*sign(q_dot(k))*helpRate;\n    else\n        trqRight(k) = dith(k)*dithS"
          "cale;\n    end\nend\n\n\n% LEFT:\nq_dot_l = q_dot(8:14);\nfor k = 1:7\n    if abs(q_dot_l(k)) > fullThres\n     "
          "   trqLeft(k) = coulLeft(k)*sign(q_dot_l(k))*helpRate;\n    elseif abs(q_dot_l(k)) > slowThres\n        trqLeft("
          "k) = coulLeft(k)*(abs(q_dot_l(k))-slowThres)/(fullThres-slowThres)*sign(q_dot_l(k))*helpRate;\n    else\n       "
          " trqLeft(k) = dith(k)*dithScale;\n    end\nend\n\n"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    170
        position		    [23.5747 49.5747 7]
        chart		    168
        linkNode		    [168 0 0]
        subviewer		    168
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    171
        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		      170
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    168
        linkNode		    [168 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    168
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    172
        ssIdNumber		    4
        name		    "coulRight"
        linkNode		    [168 0 173]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    173
        ssIdNumber		    9
        name		    "q_dot"
        linkNode		    [168 172 174]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    174
        ssIdNumber		    5
        name		    "trqRight"
        linkNode		    [168 173 175]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    175
        ssIdNumber		    7
        name		    "coulLeft"
        linkNode		    [168 174 176]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    176
        ssIdNumber		    8
        name		    "trqLeft"
        linkNode		    [168 175 177]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    177
        ssIdNumber		    10
        name		    "dith"
        linkNode		    [168 176 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    178
        name		    "botharms_get_torque/coulFric"
        machine		    1
        chart		    168
      }
      chart {
        id			    179
        name		    "Left/MATLAB Function3"
        windowPosition	    [300.375 53.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 180 0 0]
        firstTransition	    182
        firstJunction	    181
        viewObj		    179
        machine		    1
        ssIdHighWaterMark	    32
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    183
        updateMethod	    DISCRETE
        sampleTime		    "0.004"
        chartFileNumber	    15
        disableImplicitCasting  1
        eml {
          name		      "fcn"
        }
      }
      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 [runExtctrl , lockOutput, resetProgram, state] = fcn(controlActive, obtaining, switch_on"
          ")\n\n\nleadthrough_on = switch_on && obtaining;\n%obtaining = 1;\n% state variables\npersistent internalState;\n"
          "\n% internal variables\npersistent runExtctrlInternal;\npersistent lockOutputInternal;\npersistent resetProgramI"
          "nternal;\n\nif isempty(internalState)\n    internalState = 1;\nend\n\nif isempty(runExtctrlInternal)\n    runExt"
          "ctrlInternal = 0;\nend\nif isempty(lockOutputInternal)\n    lockOutputInternal = 0;\nend\nif isempty(resetProgra"
          "mInternal)\n    resetProgramInternal = 1;\nend\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n% number       internalS"
          "tate\n%   1          Extctrl_Idle\n%   2          Extctrl_Running\n%   3          Recovery_Phase\n\n%%%%%%%%%%%%"
          "%%%%%%%%%%%%%%%%%%%%%%\n\n% State transitions + entry/exit-actions\nif internalState==1\n    if (leadthrough_on)"
          " && obtaining\n        %Exit-actions\n        \n        %New state\n        internalState = 2;\n        \n      "
          "  %Entry-actions\n        runExtctrlInternal = 1;\n        lockOutputInternal = 0;\n        resetProgramInternal"
          " = 0;\n    end\nelseif internalState==2\n    if ~controlActive\n        %Exit-actions\n        \n        %New st"
          "ate\n        internalState = 1;\n        \n        %Entry-actions\n        runExtctrlInternal = 0;\n        lock"
          "OutputInternal = 0;\n        resetProgramInternal = 1;\n    elseif (~leadthrough_on) && controlActive\n        %"
          "Exit-actions\n        \n        %New state\n        internalState = 3;\n        \n        %Entry-actions\n      "
          "  runExtctrlInternal = 0;\n        lockOutputInternal = 1;\n        resetProgramInternal = 0;     \n    end\nels"
          "eif internalState==3\n    if (~leadthrough_on) || ~controlActive\n        %Exit-actions\n        \n        %New "
          "state\n        internalState = 1;\n        \n        %Entry-actions\n        runExtctrlInternal = 0;\n        lo"
          "ckOutputInternal = 0;\n        resetProgramInternal = 1;       \n    end\nend\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"
          "%%%\n\n% Continous state actions\n\n%Set outputs\nrunExtctrl = runExtctrlInternal;\nlockOutput = lockOutputInter"
          "nal;\nresetProgram = resetProgramInternal;\nstate = internalState;"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    181
        position		    [23.5747 49.5747 7]
        chart		    179
        linkNode		    [179 0 0]
        subviewer		    179
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    182
        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		      181
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    179
        linkNode		    [179 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    179
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    183
        ssIdNumber		    28
        name		    "runExtctrl"
        linkNode		    [179 0 184]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    184
        ssIdNumber		    5
        name		    "lockOutput"
        linkNode		    [179 183 185]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    185
        ssIdNumber		    27
        name		    "resetProgram"
        linkNode		    [179 184 186]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    186
        ssIdNumber		    29
        name		    "state"
        linkNode		    [179 185 187]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    187
        ssIdNumber		    30
        name		    "controlActive"
        linkNode		    [179 186 188]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    188
        ssIdNumber		    31
        name		    "obtaining"
        linkNode		    [179 187 189]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    189
        ssIdNumber		    32
        name		    "switch_on"
        linkNode		    [179 188 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    190
        name		    "Left/MATLAB Function3"
        machine		    1
        chart		    179
      }
      chart {
        id			    191
        name		    "Right/calcGravityTorque/calcGravTrq"
        windowPosition	    [390.75 188.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 192 0 0]
        firstTransition	    194
        firstJunction	    193
        viewObj		    191
        machine		    1
        ssIdHighWaterMark	    16
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    195
        chartFileNumber	    16
        disableImplicitCasting  1
        eml {
          name		      "fcn"
        }
      }
      state {
        id			    192
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    191
        treeNode		    [191 0 0 0]
        superState		    SUBCHART
        subviewer		    191
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function [gravTrq,gravTrqMotor,coulombWidthMotor, coulombWidth]  = fcn(q_motor,x_hat,gearRatio,ba"
          "seAngles)\n    \n%     gearRatio = [100;100;-100;100;-101;100;100];   \n    \n    q_arm_ABBorder = q_motor./gear"
          "Ratio;\n    \n\n    \n    gravTrqLogicalOrder = calcTrqFromParams( x_hat,q_arm_ABBorder,baseAngles);\n    %logic"
          "al ordering\n    \n    gravTrq = [gravTrqLogicalOrder(1:2) ; gravTrqLogicalOrder(4:7) ; gravTrqLogicalOrder(3)];"
          "\n    %ABB-order\n    \n%     gearRatio = [100;100;-100;100;-101;100;100];\n    \n    gravTrqMotor = gravTrq./ge"
          "arRatio;\n    \n    coulombWidthLogicalOrder = x_hat(29:35);\n    coulombWidth = [coulombWidthLogicalOrder(1:2) "
          "; coulombWidthLogicalOrder(4:7) ; coulombWidthLogicalOrder(3)];\n    coulombWidthMotor = coulombWidth./abs(gearR"
          "atio);\n\nend\n\nfunction [ trq ] = calcTrqFromParams( x_hat,q,baseAngles)\n%CALCTRQFROMPARAMS Summary of this f"
          "unction goes here\n%   Detailed explanation goes here\n\n    A = calcA_FRIDA(q,-9.82,baseAngles);\n    \n    \n "
          "   trq = A*x_hat(1:28,1);\n\nend\n\nfunction [A] = calcA_FRIDA(q,g,baseAngles)\n    % q should be given in ABB-o"
          "rder!!!\n    % Offsets are already handled in the equations!!!\n\n    c1 = cos(q(1));\n    s1 = sin(q(1));\n    "
          "c2 = cos(q(2));\n    s2 = sin(q(2));\n    c3 = cos(q(7));\n    s3 = sin(q(7));\n    c4 = cos(q(3));\n    s4 = si"
          "n(q(3));\n    c5 = cos(q(4));\n    s5 = sin(q(4));\n    c6 = cos(q(5));\n    s6 = sin(q(5));\n    c7 = cos(q(6))"
          ";\n    s7 = sin(q(6));\n\n    sb1 = sin(baseAngles(1));\n    cb1 = cos(baseAngles(1));\n    sb2 = sin(baseAngles"
          "(2));\n    cb2 = cos(baseAngles(2));\n    sb3 = sin(baseAngles(3));\n    cb3 = cos(baseAngles(3));\n\n    should"
          "erLen =   0.110;\n    shoulderOffs = -0.030;\n    elbowOffs =     0.0405;\n    wristOffs =     0.0135;\n    hand"
          "Offs =     -0.027;\n    upArmLen =      0.2465;\n    lowArmLen =     0.265;\n    handLen =       0.032;\n\n    A"
          " = zeros(7,28);\n\n    p0=g*cb1;\n    p1=g*sb1;\n    p2=sb2*p0;\n    p3=s1*cb3;\n    p4=c2*c3;\n    p5=cb2*p0;\n"
          "    p6=c1*cb3;\n    p7=c1*sb3;\n    p8=s1*sb3;\n    p9=s4*p1;\n    p10=s4*p2;\n    p11=c5*c6;\n    p12=c4*p1;\n "
          "   p13=s1*sb3;\n    p14=c4*p2;\n    p15=s3*c2;\n    p16=c7^2;\n    p17=s7^2;\n    p18=s5*c6;\n    p19=s2*s6;\n  "
          "  p20=s3*s6;\n    p21=s2*c3;\n    p22=c6^2;\n    p23=s6*p4;\n    p24=p3*p9;\n    p25=p6*p10;\n    p26=s4*p5;\n  "
          "  p27=c3*p1;\n    p28=p7*p9;\n    p29=p8*p10;\n    p30=p3*p12;\n    p31=p6*p14;\n    p32=c4*p5;\n    p33=c3*p2;\n"
          "    p34=p1*p15;\n    p35=s6^2;\n    p36=p7*p12;\n    p37=p8*p14;\n    p38=c5*wristOffs;\n    p39=p2*p15;\n    p4"
          "0=s2*s3;\n    p41=p3*p10;\n    p42=p6*p9;\n    p43=p7*p10;\n    p44=c7*p11;\n    p45=s3*s5;\n    p46=p3*p14;\n  "
          "  p47=c5*p19;\n    p48=p5*p40;\n    p49=p9*p13;\n    p50=s7*p11;\n    p51=p6*p12;\n    p52=p7*p14;\n    p53=s5*s"
          "6;\n    p54=p1*p3;\n    p55=c3*p19;\n    p56=p8*p12;\n    p57=handOffs*p11;\n    p58=s5*p4;\n    p59=s6*c2;\n   "
          " p60=p1*p7;\n    p61=p2*p6;\n    p62=p3*p33;\n    p63=p2*p8;\n    p64=s2*s5;\n    p65=c5*c7;\n    p66=elbowOffs*"
          "p4;\n    p67=p6*p27;\n    p68=p7*p33;\n    p69=c2*p32;\n    p70=s7*c5;\n    p71=c7*p18;\n    p72=s2*c6;\n    p73"
          "=s3*elbowOffs;\n    p74=p8*p27;\n    p75=p1*p6;\n    p76=p2*p3;\n    p77=s5*wristOffs;\n    p78=c2*shoulderOffs;"
          "\n    p79=s7*p18;\n    p80=c6*p4;\n    p81=p11*p17;\n    p82=p1*p8;\n    p83=p2*p7;\n    p84=p3*p34;\n    p85=p6"
          "*p39;\n    p86=c5*p23;\n    p87=elbowOffs*p21;\n    p88=s5*c7;\n    p89=wristOffs*p45;\n    p90=handOffs*p18;\n "
          "   p91=s6*c6;\n    p92=p11*p16;\n    p93=p7*p34;\n    p94=p8*p39;\n    p95=c5*p20;\n    p96=s5*s7;\n    p97=p21*"
          "p26;\n    p98=s2*elbowOffs;\n    p99=wristOffs*p58;\n    p100=p3*p27;\n    p101=s3*c6;\n    p102=s6*c5;\n    p10"
          "3=p16*p18;\n    p104=c2*p26;\n    p105=handOffs*p35;\n    p106=s2*p30;\n    p107=p21*p32;\n    p108=p7*p27;\n   "
          " p109=p6*p34;\n    p110=p3*p39;\n    p111=p17*p18;\n    p112=c5*p4;\n    p113=s2*p36;\n    p114=p8*p34;\n    p11"
          "5=p6*p33;\n    p116=p7*p39;\n    p117=p26*p40;\n    p118=p32*p55;\n    p119=s3*c5;\n    p120=s2*p31;\n    p121=p"
          "4*p24;\n    p122=p16*p20;\n    p123=p8*p33;\n    p124=p40*p54;\n    p125=s2*p37;\n    p126=p4*p25;\n    p127=s3*"
          "p42;\n    p128=p17*p20;\n    p129=p16*p19;\n    p130=p15*p24;\n    p131=c5*p59;\n    p132=p40*p60;\n    p133=s2*"
          "upArmLen;\n    p134=s3*p49;\n    p135=p4*p28;\n    p136=p17*p19;\n    p137=c3*p42;\n    p138=s5*p20;\n    p139=p"
          "40*p61;\n    p140=p26*p59;\n    p141=s2*shoulderOffs;\n    p142=s3*p41;\n    p143=p4*p29;\n    p144=p17*p23;\n  "
          "  p145=c3*p49;\n    p146=p15*p28;\n    p147=handOffs*p19;\n    p148=c2*upArmLen;\n    p149=c6*p11;\n    p150=p40"
          "*p63;\n    p151=p26*p55;\n    p152=s3*p43;\n    p153=p16*p23;\n    p154=s2*p38;\n    p155=c3*p41;\n    p156=p15*"
          "p25;\n    p157=c2*p30;\n    p158=handOffs*p22;\n    p159=c6*p57;\n    p160=p17*p50;\n    p161=c7*p64;\n    p162="
          "p4*p38;\n    p163=c3*p43;\n    p164=p15*p29;\n    p165=s5*p23;\n    p166=c2*p36;\n    p167=p22*p92;\n    p168=p1"
          "6*p44;\n    p169=s7*p64;\n    p170=c7*p45;\n    p171=s2*lowArmLen;\n    p172=s3*p38;\n    p173=c3*p47;\n    p174"
          "=c2*p31;\n    p175=p22*p81;\n    p176=p16*p50;\n    p177=p17*p44;\n    p178=handOffs*p23;\n    p179=s7*p45;\n   "
          " p180=c7*p58;\n    p181=lowArmLen*p4;\n    p182=s6*c3;\n    p183=c2*p37;\n    p184=s7*p58;\n    p185=handOffs*p2"
          "0;\n    p186=p19*p45;\n    p187=s6*p15;\n    p188=s7*p19;\n    p189=p5*p15;\n    p190=s6*p92;\n    p191=c7*p19;\n"
          "    p192=p5*p21;\n    p193=s6*p81;\n    p194=p21*p24;\n    p195=p21*p28;\n    p196=p21*p25;\n    p197=p21*p29;\n"
          "    A(1,1) = -shoulderOffs*p75+shoulderOffs*p82-shoulderOffs*p76-shoulderOffs*p83;\n    A(1,2) = -shoulderOffs*p"
          "75+shoulderOffs*p82+p78*p75-p78*p82-shoulderOffs*p76-shoulderOffs*p83+p78*p76+p78*p83;\n    A(1,3) = -shoulderOf"
          "fs*p75+shoulderOffs*p82+p78*p75+p54*p73+p60*p73-p75*p133-p78*p82+p82*p133-shoulderOffs*p76-shoulderOffs*p83+p66*"
          "p82-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p133-p66*p75-p66*p76-p66*p83;\n    A(1,4) = -shoulderOffs*p75+s"
          "houlderOffs*p82+p78*p75+p54*p73+p60*p73-p75*p133-p78*p82+p82*p133-shoulderOffs*p76-shoulderOffs*p83+p66*p82+p24*"
          "p73+p56*p98+p28*p73-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p133-p66*p75-p51*p98-p66*p76-p66*p83-p46*p98-p2"
          "5*p73-p52*p98+p29*p73-p42*p66+p49*p66-p41*p66-p43*p66;\n    A(1,5) = -shoulderOffs*p75+shoulderOffs*p82+p78*p75+"
          "p54*p73+p60*p73-p75*p133-p78*p82+p82*p133-shoulderOffs*p76-shoulderOffs*p83+p66*p82+p42*p171+s3*lowArmLen*p30+s3"
          "*lowArmLen*p36+p24*p73+p56*p98+p28*p73+p77*p100+p77*p108-p49*p171-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p"
          "133-p66*p75-p51*p98-p24*p172-p56*p154-p28*p172-p77*p114-p66*p76-p66*p83-s3*lowArmLen*p31-p46*p98-p25*p73-p52*p98"
          "-p77*p115+p41*p171+p43*p171+s3*lowArmLen*p37+p29*p73+p77*p123-p51*p181-p42*p66+p51*p154+p56*p181+p49*p66+p77*p10"
          "9-p46*p181-p52*p181-p41*p66-p43*p66+p46*p154+p25*p172+p52*p154+p77*p110+p77*p116-p29*p172+p42*p162-p49*p162+p41*"
          "p162+p43*p162;\n    A(1,6) = -shoulderOffs*p75+shoulderOffs*p82+p78*p75+p54*p73+p60*p73-p75*p133-p78*p82+p82*p13"
          "3-shoulderOffs*p76-shoulderOffs*p83+p66*p82+p42*p171+s3*lowArmLen*p30+s3*lowArmLen*p36+p24*p73+p56*p98+p28*p73+p"
          "77*p100+p77*p108-p49*p171-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p133-p66*p75-p51*p98-p24*p172-p56*p154-p2"
          "8*p172-p77*p114+p49*p147-p66*p76-p66*p83-s3*lowArmLen*p31-p46*p98-p25*p73-p52*p98-p77*p115+p41*p171+p43*p171+s3*"
          "lowArmLen*p37+p29*p73+p77*p123-p51*p181-p42*p66+p51*p154+p100*p90+p90*p108+p56*p181+p49*p66+p77*p109-p42*p147-p3"
          "0*p185-p36*p185-p90*p115-p46*p181-p52*p181-p41*p66-p43*p66+p31*p185+p46*p154+p25*p172+p52*p154+p90*p123+p77*p110"
          "+p77*p116-p41*p147-p43*p147-p37*p185-p29*p172+p51*p178+s2*p51*p57+p42*p162+p90*p109-s3*p24*p57-p56*p178-s2*p57*p"
          "56-s3*p28*p57-p49*p162-p90*p114+p46*p178+s2*p46*p57+s3*p25*p57+p52*p178+s2*p52*p57+p41*p162+p43*p162+p90*p110+p9"
          "0*p116-s3*p29*p57+p4*p42*p57-p4*p49*p57+p4*p41*p57+p4*p43*p57;\n    A(1,7) = -p75+p82-p76-p83;\n    A(1,9) = +p5"
          "4+p60-p61+p63;\n    A(1,10) = -c2*p75+c2*p82-c2*p76-c2*p83;\n    A(1,11) = +p54+p60-p61+p63;\n    A(1,12) = -s2*"
          "p75+s2*p82-s2*p76-s2*p83;\n    A(1,13) = +s3*p54+s3*p60+p4*p82-s3*p61+s3*p63-p4*p75-p4*p76-p4*p83;\n    A(1,14) "
          "= +s2*p75-s2*p82+s2*p76+s2*p83;\n    A(1,15) = +p100+p108+p109-p114-p115+p123+p110+p116;\n    A(1,16) = +s2*p51-"
          "s3*p24-s2*p56-s3*p28+s2*p46+s3*p25+s2*p52-s3*p29+p4*p42-p4*p49+p4*p41+p4*p43;\n    A(1,17) = +p100+p108+p109-p11"
          "4-p115+p123+p110+p116;\n    A(1,18) = +s2*p42+s3*p30+s3*p36-s2*p49-s3*p31+s2*p41+s2*p43+s3*p37-p4*p51+p4*p56-p4*"
          "p46-p4*p52;\n    A(1,19) = +s5*p100+s5*p108-s5*p114-s5*p115+s5*p123+s2*c5*p51+s5*p109-p24*p119-s2*c5*p56-p28*p11"
          "9+s2*c5*p46+p25*p119+s2*c5*p52+s5*p110+s5*p116-p29*p119+p42*p112-p49*p112+p41*p112+p43*p112;\n    A(1,20) = -s2*"
          "p42-s3*p30-s3*p36+s2*p49+s3*p31-s2*p41-s2*p43-s3*p37+p4*p51-p4*p56+p4*p46+p4*p52;\n    A(1,21) = +c5*p100+c5*p10"
          "8+p24*p45+p56*p64+p28*p45-c5*p115+c5*p123+c5*p109-p51*p64-c5*p114+c5*p110+c5*p116-p46*p64-p25*p45-p52*p64+p29*p4"
          "5-p42*p58+p49*p58-p41*p58-p43*p58;\n    A(1,22) = +p19*p49+p18*p100+p18*p108-p19*p42-p20*p30-p20*p36-p18*p115+p2"
          "0*p31+p18*p123-p19*p41-p19*p43-p20*p37+p23*p51+s2*p11*p51+p18*p109-s3*p11*p24-p23*p56-s2*p11*p56-s3*p11*p28-p18*"
          "p114+p23*p46+s2*p11*p46+s3*p11*p25+p23*p52+s2*p11*p52+p18*p110+p18*p116-s3*p11*p29+p4*p11*p42-p4*p11*p49+p4*p11*"
          "p41+p4*p11*p43;\n    A(1,23) = +c5*p100+c5*p108+p24*p45+p56*p64+p28*p45-c5*p115+c5*p123+c5*p109-p51*p64-c5*p114+"
          "c5*p110+c5*p116-p46*p64-p25*p45-p52*p64+p29*p45-p42*p58+p49*p58-p41*p58-p43*p58;\n    A(1,24) = -p49*p72+p42*p72"
          "+p30*p101+p36*p101+p53*p100+p53*p108-p31*p101-p53*p115+p41*p72+p43*p72+p37*p101+p53*p123-p51*p80+p47*p51+p56*p80"
          "+p53*p109-p24*p95-p47*p56-p28*p95-p53*p114-p46*p80-p52*p80+p47*p46+p25*p95+p47*p52+p53*p110+p53*p116-p29*p95+p42"
          "*p86-p49*p86+p41*p86+p43*p86;\n    A(1,25) = +p70*p100+p70*p108-p70*p115+p70*p123+p70*p109+p71*p100+p71*p108-p51"
          "*p169-p42*p191-c7*p20*p30-c7*p20*p36-p70*p114+p24*p179+p56*p169+p28*p179+p49*p191-p71*p115+c7*p20*p31+p70*p110+p"
          "70*p116+p71*p123-p46*p169-p25*p179-p41*p191-p52*p169-p43*p191-c7*p20*p37+p29*p179+c7*p23*p51+s2*p44*p51+p71*p109"
          "-p42*p184-s3*p24*p44-c7*p23*p56-s2*p44*p56-s3*p28*p44-p71*p114+p49*p184+p71*p110+p71*p116-p41*p184-p43*p184-s3*p"
          "29*p44+p4*p44*p42-p4*p44*p49+c7*p23*p46+s2*p46*p44+s3*p25*p44+c7*p23*p52+s2*p44*p52+p4*p41*p44+p4*p44*p43;\n    "
          "A(1,26) = +p65*p100+p65*p108-p49*p188-p65*p115+p65*p123+p65*p109-p51*p161-p79*p100-p65*p114-p79*p108+p42*p188+p2"
          "4*p170+s7*p20*p30+p56*p161+p28*p170+s7*p20*p36+p65*p110+p79*p115+p65*p116-p46*p161-p25*p170-s7*p20*p31-p52*p161-"
          "p79*p123+p41*p188+p43*p188+p29*p170+s7*p20*p37-p42*p180-s7*p23*p51-s2*p51*p50-p79*p109+s3*p24*p50+p49*p180+s7*p2"
          "3*p56+s2*p50*p56+s3*p28*p50+p79*p114-p41*p180-s7*p23*p46-s2*p46*p50-s3*p25*p50-p43*p180-s7*p23*p52-s2*p52*p50-p7"
          "9*p110-p79*p116+s3*p29*p50-p4*p42*p50+p4*p50*p49-p4*p41*p50-p4*p43*p50;\n    A(1,27) = -p49*p72+p42*p72+p30*p101"
          "+p36*p101+p53*p100+p53*p108-p31*p101-p53*p115+p41*p72+p43*p72+p37*p101+p53*p123-p51*p80+p47*p51+p56*p80+p53*p109"
          "-p24*p95-p47*p56-p28*p95-p53*p114-p46*p80-p52*p80+p47*p46+p25*p95+p47*p52+p53*p110+p53*p116-p29*p95+p42*p86-p49*"
          "p86+p41*p86+p43*p86;\n\n    A(2,2) = -p5*p78-p54*p141-p60*p141+p61*p141-p63*p141;\n    A(2,3) = -p5*p78+p5*p133+"
          "p5*p66-p54*p148-p60*p148-p54*p141-p60*p141+p54*p87+p60*p87+p61*p148+p61*p141-p63*p148-p63*p141-p61*p87+p63*p87;\n"
          "    A(2,4) = -p5*p78+p5*p133+p5*p66+p32*p98-p54*p148-p60*p148-p54*p141-p60*p141+p54*p87+p60*p87+p61*p148+p61*p14"
          "1-p63*p148-p63*p141+p26*p66-elbowOffs*p157-elbowOffs*p166+elbowOffs*p174-p61*p87-elbowOffs*p183+p63*p87+p24*p87+"
          "p28*p87-p25*p87+p29*p87;\n    A(2,5) = -p5*p78+p5*p133+p5*p66+p32*p98-p26*p171-p54*p148-p60*p148-p54*p141-p60*p1"
          "41+p54*p87+p60*p87-p77*p189+c2*lowArmLen*p24+c2*lowArmLen*p28+p61*p148+p61*p141-p63*p148-p63*p141+p32*p181+p26*p"
          "66-elbowOffs*p157-elbowOffs*p166-p32*p154-p77*p124-p77*p132+elbowOffs*p174-p61*p87-c2*lowArmLen*p25-elbowOffs*p1"
          "83+p63*p87+c2*lowArmLen*p29-p26*p162+p38*p157+p38*p166+lowArmLen*p21*p30+lowArmLen*p21*p36+p24*p87+p28*p87-p38*p"
          "174-lowArmLen*p21*p31-p25*p87+p38*p183+lowArmLen*p21*p37+p29*p87+p77*p139-p77*p150-p38*p194-p38*p195+p38*p196-p3"
          "8*p197;\n    A(2,6) = -p5*p78+p5*p133+p5*p66+p32*p98-p26*p171-p54*p148-p60*p148-p54*p141-p60*p141+p54*p87+p60*p8"
          "7-p77*p189+p26*p147+c2*lowArmLen*p24+c2*lowArmLen*p28+p61*p148+p61*p141-p63*p148-p63*p141+p32*p181+p26*p66-elbow"
          "Offs*p157-elbowOffs*p166-p32*p154-p77*p124-p77*p132+elbowOffs*p174-p61*p87-c2*lowArmLen*p25-elbowOffs*p183+p63*p"
          "87+c2*lowArmLen*p29-p32*p178-s2*p32*p57-p26*p162-p90*p189+p38*p157+p38*p166+lowArmLen*p21*p30+lowArmLen*p21*p36+"
          "p24*p87+p28*p87-handOffs*p24*p59-handOffs*p28*p59-p38*p174-lowArmLen*p21*p31-p25*p87+handOffs*p25*p59+p38*p183+l"
          "owArmLen*p21*p37+p29*p87+p77*p139-handOffs*p29*p59-p77*p150-p4*p26*p57+p57*p157+p57*p166-handOffs*p30*p55-handOf"
          "fs*p36*p55-p38*p194-p38*p195-p90*p124-p90*p132+handOffs*p31*p55+p57*p183+p38*p196+p90*p139-handOffs*p37*p55-p38*"
          "p197-p90*p150-p57*p194-p57*p195-p57*p174-p57*p197+p57*p196;\n    A(2,10) = +c2*p5+s2*p54+s2*p60-s2*p61+s2*p63;\n"
          "    A(2,12) = +s2*p5-c2*p54-c2*p60+c2*p61-c2*p63;\n    A(2,13) = +p5*p4+p21*p54+p21*p60-p21*p61+p21*p63;\n    A("
          "2,14) = -s2*p5+c2*p54+c2*p60-c2*p61+c2*p63;\n    A(2,15) = -p189-p124-p132+p139-p150;\n    A(2,16) = -s2*p32+p15"
          "7+p166-p4*p26-p174+p183-p194-p195+p196-p197;\n    A(2,17) = -p189-p124-p132+p139-p150;\n    A(2,18) = -s2*p26+c2"
          "*p24+c2*p28+p4*p32-c2*p25+c2*p29+p21*p30+p21*p36-p21*p31+p21*p37;\n    A(2,19) = -s5*p189-s2*c5*p32-s5*p124-s5*p"
          "132-p26*p112+c5*p157+c5*p166-c5*p174+c5*p183+s5*p139-s5*p150-c5*p194-c5*p195+c5*p196-c5*p197;\n    A(2,20) = +s2"
          "*p26-c2*p24-c2*p28-p4*p32+c2*p25-c2*p29-p21*p30-p21*p36+p21*p31-p21*p37;\n    A(2,21) = +p32*p64-c5*p189+p26*p58"
          "-s5*p157-s5*p166-c5*p124-c5*p132+s5*p174+c5*p139-s5*p183-c5*p150+s5*p194+s5*p195-s5*p196+s5*p197;\n    A(2,22) ="
          " +p19*p26-p23*p32-s2*p11*p32-p18*p189-p24*p59-p28*p59+p25*p59-p29*p59-p4*p11*p26+p11*p157+p11*p166-p30*p55-p36*p"
          "55-p18*p124-p18*p132-p11*p174+p31*p55+p11*p183+p18*p139-p37*p55-p18*p150-p11*p194-p11*p195-p11*p197+p11*p196;\n "
          "   A(2,23) = +p32*p64-c5*p189+p26*p58-s5*p157-s5*p166-c5*p124-c5*p132+s5*p174+c5*p139-s5*p183-c5*p150+s5*p194+s5"
          "*p195-s5*p196+s5*p197;\n    A(2,24) = -p26*p72+p32*p80-p32*p47-p53*p189+c2*c6*p24+c2*c6*p28-c2*c6*p25+c2*c6*p29-"
          "p26*p86+c6*p21*p30+p30*p131+c6*p21*p36+p36*p131-p54*p186-p60*p186-c6*p21*p31-p31*p131+c6*p21*p37+p37*p131+p61*p1"
          "86-p63*p186-p24*p173-p28*p173+p25*p173-p29*p173;\n    A(2,25) = -p70*p189+p32*p169+p26*p191-c7*p23*p32-s2*p32*p4"
          "4-p71*p189+p26*p184-p96*p157-c7*p24*p59-p96*p166-c7*p28*p59-p70*p124-p70*p132+p96*p174+c7*p25*p59+p70*p139-p96*p"
          "183-c7*p29*p59-p70*p150-p4*p26*p44+p44*p157+p44*p166-c7*p30*p55-c7*p36*p55-p71*p124-p71*p132+p96*p194+p96*p195+p"
          "71*p139-p96*p196-c7*p37*p55-p71*p150+p96*p197-p44*p194-p44*p195-p44*p174+c7*p31*p55+p44*p183+p44*p196-p44*p197;\n"
          "    A(2,26) = -p65*p189+p32*p161-p26*p188+p26*p180+s7*p23*p32+s2*p32*p50+p79*p189-p88*p157-p88*p166-p65*p124-p65"
          "*p132+s7*p24*p59+s7*p28*p59+p88*p174+p65*p139-s7*p25*p59-p88*p183-p65*p150+s7*p29*p59+p4*p26*p50-p50*p157-p50*p1"
          "66+p88*p194+s7*p30*p55+p88*p195+s7*p36*p55+p79*p124+p79*p132-p88*p196-s7*p31*p55-p50*p183-p79*p139+p88*p197+s7*p"
          "37*p55+p79*p150+p50*p194+p50*p195+p50*p174-p50*p196+p50*p197;\n    A(2,27) = -p26*p72+p32*p80-p32*p47-p53*p189+c"
          "2*c6*p24+c2*c6*p28-c2*c6*p25+c2*c6*p29-p26*p86+c6*p21*p30+p30*p131+c6*p21*p36+p36*p131-p54*p186-p60*p186-c6*p21*"
          "p31-p31*p131+c6*p21*p37+p37*p131+p61*p186-p63*p186-p24*p173-p28*p173+p25*p173-p29*p173;\n\n    A(3,3) = -elbowOf"
          "fs*p48-elbowOffs*p67+elbowOffs*p74+elbowOffs*p84+elbowOffs*p93-elbowOffs*p62-elbowOffs*p68-elbowOffs*p85+elbowOf"
          "fs*p94;\n    A(3,4) = -elbowOffs*p67-elbowOffs*p48+elbowOffs*p74-elbowOffs*p117+elbowOffs*p84+elbowOffs*p93+elbo"
          "wOffs*p145-elbowOffs*p62-elbowOffs*p68-elbowOffs*p137-elbowOffs*p85-elbowOffs*p155-elbowOffs*p163+elbowOffs*p94+"
          "elbowOffs*p130+elbowOffs*p146-elbowOffs*p156+elbowOffs*p164;\n    A(3,5) = -elbowOffs*p67-elbowOffs*p48+elbowOff"
          "s*p74-lowArmLen*p32*p40-elbowOffs*p117+elbowOffs*p84+elbowOffs*p93-p77*p192+c3*lowArmLen*p56+elbowOffs*p145+p75*"
          "p89-p89*p82-elbowOffs*p62-elbowOffs*p68-c3*lowArmLen*p51-elbowOffs*p137-p38*p145-elbowOffs*p85-c3*lowArmLen*p46-"
          "c3*lowArmLen*p52-elbowOffs*p155-elbowOffs*p163+elbowOffs*p94+p76*p89+p89*p83+p38*p137+lowArmLen*p15*p30+lowArmLe"
          "n*p15*p36+elbowOffs*p130+elbowOffs*p146+p38*p117+p54*p99+p60*p99-lowArmLen*p15*p31-elbowOffs*p156-p61*p99+p38*p1"
          "55+p38*p163+lowArmLen*p15*p37+elbowOffs*p164+p63*p99-p38*p130-p38*p146+p38*p156-p38*p164;\n    A(3,6) = -elbowOf"
          "fs*p67-elbowOffs*p48+elbowOffs*p74-lowArmLen*p32*p40-elbowOffs*p117+elbowOffs*p84+elbowOffs*p93-p77*p192+c3*lowA"
          "rmLen*p56+elbowOffs*p145+p75*p89-p89*p82-elbowOffs*p62-elbowOffs*p68-c3*lowArmLen*p51-elbowOffs*p137-p38*p145-s3"
          "*p82*p90-elbowOffs*p85-c3*lowArmLen*p46-c3*lowArmLen*p52-elbowOffs*p155-elbowOffs*p163+elbowOffs*p94+p76*p89+p89"
          "*p83-p90*p192+handOffs*p51*p182+p38*p137+s3*p32*p147+s3*p75*p90+lowArmLen*p15*p30+lowArmLen*p15*p36+elbowOffs*p1"
          "30+elbowOffs*p146+p38*p117+p54*p99+p60*p99-handOffs*p56*p182-lowArmLen*p15*p31-elbowOffs*p156-p61*p99+handOffs*p"
          "46*p182+handOffs*p52*p182+p38*p155+p38*p163+s3*p76*p90+s3*p83*p90+lowArmLen*p15*p37+elbowOffs*p164+p63*p99+p57*p"
          "137+p57*p117+p4*p54*p90+p4*p60*p90-handOffs*p30*p187-handOffs*p36*p187-p57*p145-p38*p130-p38*p146-p4*p61*p90+han"
          "dOffs*p31*p187+p57*p155+p57*p163+p38*p156+p4*p63*p90-handOffs*p37*p187-p38*p164-p57*p130-p57*p146-p57*p164+p57*p"
          "156;\n    A(3,13) = -p67-p48+p74+p84+p93-p62-p68-p85+p94;\n    A(3,15) = +s3*p75-p192-s3*p82+p4*p54+p4*p60+s3*p7"
          "6+s3*p83-p4*p61+p4*p63;\n    A(3,16) = +p137+p117-p145+p155+p163-p130-p146+p156-p164;\n    A(3,17) = -p192+s3*p7"
          "5-s3*p82+p4*p54+p4*p60+s3*p76+s3*p83-p4*p61+p4*p63;\n    A(3,18) = -p32*p40+c3*p56-c3*p51-c3*p46-c3*p52+p15*p30+"
          "p15*p36-p15*p31+p15*p37;\n    A(3,19) = -s5*p192+p45*p75-p45*p82+p45*p76+p45*p83+c5*p137+c5*p117+p54*p58+p60*p58"
          "-c5*p145-p61*p58+c5*p155+c5*p163+p58*p63-c5*p130-c5*p146+c5*p156-c5*p164;\n    A(3,20) = +p32*p40-c3*p56+c3*p51+"
          "c3*p46+c3*p52-p15*p30-p15*p36+p15*p31-p15*p37;\n    A(3,21) = +p75*p119-p82*p119-c5*p192+s5*p145+p76*p119+p83*p1"
          "19+p54*p112+p60*p112-s5*p137-s5*p117-p61*p112+p63*p112-s5*p155-s5*p163+s5*p130+s5*p146-s5*p156+s5*p164;\n    A(3"
          ",22) = -s3*p18*p82-p18*p192+p51*p182+s3*p19*p32+s3*p18*p75-p56*p182+p46*p182+p52*p182+s3*p18*p76+s3*p18*p83+p11*"
          "p137+p11*p117+p4*p18*p54+p4*p18*p60-p30*p187-p36*p187-p11*p145-p4*p18*p61+p31*p187+p11*p155+p11*p163+p4*p18*p63-"
          "p37*p187-p11*p130-p11*p146-p11*p164+p11*p156;\n    A(3,23) = +p75*p119-p82*p119-c5*p192+s5*p145+p76*p119+p83*p11"
          "9+p54*p112+p60*p112-s5*p137-s5*p117-p61*p112+p63*p112-s5*p155-s5*p163+s5*p130+s5*p146-s5*p156+s5*p164;\n    A(3,"
          "24) = -p82*p138-c3*c6*p51-c6*p32*p40-s5*p5*p55+c3*c6*p56+p75*p138-c3*c6*p46-c3*c6*p52+p76*p138+p83*p138+c6*p15*p"
          "30+p102*p137+c6*p15*p36+s3*p26*p47+p54*p165+p60*p165-p102*p145-c6*p15*p31-p61*p165+p102*p155+c6*p15*p37+p102*p16"
          "3+p63*p165-p102*p130-p102*p146+p102*p156-p102*p164;\n    A(3,25) = -s3*p70*p82-p70*p192+s3*p75*p70+s3*p76*p70+s3"
          "*p70*p83-p71*p192+c7*p51*p182+s3*p32*p191+p4*p54*p70+s3*p75*p71+p4*p60*p70-p96*p137-c7*p56*p182-p96*p117-s3*p71*"
          "p82+p96*p145-p4*p61*p70+c7*p46*p182+c7*p52*p182+s3*p76*p71+p4*p63*p70+s3*p71*p83-p96*p155-p96*p163+p44*p137+p44*"
          "p117+p4*p54*p71+p4*p60*p71-c7*p30*p187-c7*p36*p187-p44*p145+p96*p130+p96*p146+p4*p63*p71-p96*p156-c7*p37*p187+p9"
          "6*p164-p44*p130-p44*p146-p4*p61*p71+c7*p31*p187+p44*p155+p44*p163+p44*p156-p44*p164;\n    A(3,26) = -p65*p192+s3"
          "*p65*p75-s3*p65*p82+s3*p65*p76+s3*p65*p83+p4*p54*p65+p4*p60*p65+p79*p192-p88*p137-s7*p51*p182-p88*p117-s3*p32*p1"
          "88-s3*p75*p79+p88*p145+s7*p56*p182+s3*p82*p79-p4*p61*p65+p4*p65*p63-p88*p155-s7*p46*p182-p88*p163-s7*p52*p182-s3"
          "*p76*p79-s3*p83*p79-p50*p137-p50*p117-p4*p54*p79-p4*p60*p79+p88*p130+s7*p30*p187+p88*p146+s7*p36*p187+p50*p145-p"
          "88*p156-s7*p31*p187-p50*p155-p50*p163-p4*p63*p79+p88*p164+s7*p37*p187+p50*p130+p50*p146+p4*p61*p79-p50*p156+p50*"
          "p164;\n    A(3,27) = -p82*p138-c3*c6*p51-c6*p32*p40-s5*p5*p55+c3*c6*p56+p75*p138-c3*c6*p46-c3*c6*p52+p76*p138+p8"
          "3*p138+c6*p15*p30+p102*p137+c6*p15*p36+s3*p26*p47+p54*p165+p60*p165-p102*p145-c6*p15*p31-p61*p165+p102*p155+c6*p"
          "15*p37+p102*p163+p63*p165-p102*p130-p102*p146+p102*p156-p102*p164;\n\n    A(4,4) = +elbowOffs*p104+p24*p98+p28*p"
          "98+p56*p73+p32*p87-p51*p73-p25*p98-p46*p73-p52*p73+p29*p98-p30*p66-p36*p66+p31*p66-p37*p66;\n    A(4,5) = +lowAr"
          "mLen*p69+elbowOffs*p104-lowArmLen*p97+lowArmLen*p106+lowArmLen*p127+lowArmLen*p113+p24*p98+p28*p98+p56*p73-lowAr"
          "mLen*p134+p32*p87-p51*p73-p38*p104-p24*p154-p28*p154-p56*p172-lowArmLen*p120-p25*p98-p46*p73-p52*p73+lowArmLen*p"
          "142+lowArmLen*p125+lowArmLen*p152+p29*p98-p30*p66-p36*p66-p38*p107+p51*p172+lowArmLen*p121+lowArmLen*p135+p31*p6"
          "6-lowArmLen*p126-p37*p66+p25*p154+p46*p172+p52*p172+lowArmLen*p143-p29*p154+p30*p162+p36*p162-p31*p162+p37*p162;"
          "\n    A(4,6) = +lowArmLen*p69+elbowOffs*p104-lowArmLen*p97+lowArmLen*p106+lowArmLen*p127+lowArmLen*p113+p24*p98+"
          "p28*p98+p56*p73-lowArmLen*p134+p32*p87-handOffs*p32*p59-p51*p73-p38*p104-p24*p154-p28*p154-p56*p172+p49*p185-low"
          "ArmLen*p120-p25*p98-p46*p73-p52*p73+lowArmLen*p142+lowArmLen*p125+lowArmLen*p152+p29*p98-p57*p104-p30*p66-p36*p6"
          "6-p38*p107+p51*p172+handOffs*p151+lowArmLen*p121+lowArmLen*p135-p30*p147-p42*p185-p36*p147+p31*p66-lowArmLen*p12"
          "6-p37*p66+p31*p147+p25*p154+p46*p172+p52*p172+lowArmLen*p143-p41*p185-p37*p147-p43*p185-p29*p154-p57*p107+s3*p51"
          "*p57+p30*p162+p36*p162-p24*p178-s2*p24*p57-p28*p178-s2*p28*p57-s3*p57*p56-p31*p162+p25*p178+s2*p25*p57+s3*p46*p5"
          "7+s3*p52*p57+p37*p162-p29*p178-s2*p29*p57+p4*p30*p57+p4*p36*p57-p4*p31*p57+p4*p37*p57;\n    A(4,16) = -p104+s3*p"
          "51-s2*p24-s2*p28-s3*p56-p107+s2*p25+s3*p46+s3*p52-s2*p29+p4*p30+p4*p36-p4*p31+p4*p37;\n    A(4,18) = +p69-p97+p1"
          "06+p127+p113-p134-p120+p142+p125+p152+p121+p135-p126+p143;\n    A(4,19) = -c5*p104-c5*p107+p51*p119-s2*c5*p24-s2"
          "*c5*p28-p56*p119+s2*c5*p25+p46*p119+p52*p119-s2*c5*p29+p30*p112+p36*p112-p31*p112+p37*p112;\n    A(4,20) = -p69+"
          "p97-p106-p127-p113+p134+p120-p142-p125-p152-p121-p135+p126-p143;\n    A(4,21) = +s5*p104+p24*p64+p28*p64+p45*p56"
          "+s5*p107-p45*p51-p25*p64-p46*p45-p45*p52+p29*p64-p30*p58-p36*p58+p31*p58-p37*p58;\n    A(4,22) = -p32*p59+p20*p4"
          "9-p11*p104+p151-p19*p30-p20*p42-p19*p36+p19*p31-p20*p41-p19*p37-p20*p43-p11*p107+s3*p11*p51-p24*p23-s2*p11*p24-p"
          "23*p28-s2*p11*p28-s3*p11*p56+p25*p23+s2*p11*p25+s3*p11*p46+s3*p11*p52-p23*p29-s2*p11*p29+p4*p11*p30+p4*p11*p36-p"
          "4*p11*p31+p4*p11*p37;\n    A(4,23) = +s5*p104+p24*p64+p28*p64+p45*p56+s5*p107-p45*p51-p25*p64-p46*p45-p45*p52+p2"
          "9*p64-p30*p58-p36*p58+p31*p58-p37*p58;\n    A(4,24) = +c6*p69-p49*p101-c6*p97-p26*p131+p30*p72+p42*p101+p36*p72-"
          "p31*p72+p41*p101+p37*p72+p43*p101-p32*p173+p24*p80+p51*p95+p28*p80-p24*p47-p28*p47-p56*p95-p25*p80+p25*p47+p46*p"
          "95+p29*p80+p52*p95-p29*p47+p30*p86+p36*p86+p37*p86-p31*p86;\n    A(4,25) = -c7*p32*p59+p96*p104-p44*p104+p96*p10"
          "7+c7*p151-p51*p179-p30*p191-c7*p20*p42-p36*p191+p24*p169+p28*p169+p56*p179+c7*p20*p49+p31*p191-p25*p169-p46*p179"
          "-c7*p20*p41-p52*p179-p37*p191-c7*p20*p43+p29*p169-p44*p107+s3*p44*p51-p30*p184-c7*p24*p23-s2*p24*p44-p36*p184-c7"
          "*p23*p28-s2*p28*p44-s3*p44*p56-p37*p184-c7*p23*p29-s2*p29*p44+p4*p30*p44+p4*p36*p44+p31*p184+c7*p25*p23+s2*p25*p"
          "44+s3*p46*p44+s3*p44*p52-p4*p31*p44+p4*p37*p44;\n    A(4,26) = +p88*p104+s7*p32*p59-s7*p20*p49+p88*p107+p50*p104"
          "-p51*p170-s7*p151+p24*p161+p30*p188+s7*p20*p42+p28*p161+p36*p188+p56*p170-p25*p161-p31*p188-p46*p170-p52*p170+s7"
          "*p20*p41+p29*p161+p37*p188+s7*p20*p43+p50*p107-p30*p180-s3*p51*p50-p36*p180+s7*p24*p23+s2*p24*p50+s7*p23*p28+s2*"
          "p28*p50+s3*p50*p56-s7*p25*p23-s2*p25*p50-s3*p46*p50-p37*p180-s3*p52*p50+s7*p23*p29+s2*p29*p50-p4*p30*p50-p4*p36*"
          "p50+p31*p180+p4*p31*p50-p4*p37*p50;\n    A(4,27) = +c6*p69-p49*p101-c6*p97-p26*p131+p30*p72+p42*p101+p36*p72-p31"
          "*p72+p41*p101+p37*p72+p43*p101-p32*p173+p24*p80+p51*p95+p28*p80-p24*p47-p28*p47-p56*p95-p25*p80+p25*p47+p46*p95+"
          "p29*p80+p52*p95-p29*p47+p30*p86+p36*p86+p37*p86-p31*p86;\n\n    A(5,5) = -p48*p38+p38*p74-p69*p77-p38*p67-wristO"
          "ffs*p30*p64-p42*p89-wristOffs*p36*p64+p49*p89-p38*p62-p38*p68+p77*p97+p38*p84+p38*p93-p38*p85+wristOffs*p31*p64+"
          "p38*p94-p41*p89-wristOffs*p37*p64-p43*p89-p24*p99-p28*p99+p25*p99-p29*p99;\n    A(5,6) = -p48*p38+p38*p74-p69*p7"
          "7-p38*p67-wristOffs*p30*p64-p42*p89-wristOffs*p36*p64+p49*p89-p38*p62-p38*p68-p69*p90-p57*p67-p48*p57+p57*p74+p7"
          "7*p97+p38*p84+p38*p93-p62*p57-p57*p68-p38*p85+wristOffs*p31*p64+p38*p94-p41*p89-wristOffs*p37*p64-p43*p89+p97*p9"
          "0+p57*p84+p57*p93-p90*p106-p90*p127-p90*p113-p24*p99-p28*p99+p90*p134-p57*p85+p90*p120+p25*p99+p57*p94-p90*p142-"
          "p90*p125-p90*p152-p29*p99-p90*p121-p90*p135-p90*p143+p90*p126;\n    A(5,19) = -c5*p48+c5*p74-s5*p69-c5*p67+p45*p"
          "49-c5*p62-c5*p68+s5*p97+c5*p84+c5*p93-p30*p64-p45*p42-p36*p64-c5*p85+p31*p64+c5*p94-p41*p45-p37*p64-p45*p43-p24*"
          "p58-p28*p58+p25*p58-p29*p58;\n    A(5,20) = 0;\n    A(5,21) = +s5*p67+s5*p48-s5*p74-c5*p69+p49*p119+s5*p62+s5*p6"
          "8+c5*p97-c5*p106-p42*p119-c5*p113-s5*p84-s5*p93+c5*p120+s5*p85-p41*p119-c5*p125-p43*p119-s5*p94-p24*p112-p28*p11"
          "2+p25*p112-p29*p112;\n    A(5,22) = -p18*p69-p11*p67-p11*p48+p11*p74-p11*p62-p11*p68+p18*p97+p11*p84+p11*p93-p18"
          "*p106-p18*p127-p18*p113+p18*p134-p11*p85+p18*p120+p11*p94-p18*p142-p18*p125-p18*p152-p18*p121-p18*p135-p18*p143+"
          "p18*p126;\n    A(5,23) = +s5*p67+s5*p48-s5*p74-c5*p69+p49*p119+s5*p62+s5*p68+c5*p97-c5*p106-p42*p119-c5*p113-s5*"
          "p84-s5*p93+c5*p120+s5*p85-p41*p119-c5*p125-p43*p119-s5*p94-p24*p112-p28*p112+p25*p112-p29*p112;\n    A(5,24) = -"
          "p53*p69-p67*p102-s3*p5*p47+p74*p102+p49*p138-p62*p102-p68*p102+s5*p151+p84*p102+p93*p102-s5*p19*p30-p42*p138-s5*"
          "p19*p36-p85*p102+s5*p19*p31+p94*p102-p41*p138-s5*p19*p37-p43*p138-p24*p165-p28*p165+p25*p165-p29*p165;\n    A(5,"
          "25) = -p74*p96-p69*p70+p67*p96+p48*p96+p62*p96+p68*p96-p69*p71-p44*p67-p48*p44+p70*p97+p44*p74-p70*p106-p70*p127"
          "-p70*p113-p84*p96-p96*p93+p70*p134-p44*p62-p44*p68+p70*p120+p85*p96-p70*p142-p70*p125-p70*p152-p96*p94+p71*p97+p"
          "44*p84+p44*p93-p70*p121-p71*p106-p71*p127-p70*p135-p71*p113+p71*p134+p44*p94-p71*p142-p70*p143-p71*p125-p71*p152"
          "-p71*p121-p71*p135-p44*p85+p70*p126+p71*p120+p71*p126-p71*p143;\n    A(5,26) = -p69*p65+p67*p88+p48*p88-p74*p88+"
          "p62*p88+p68*p88+p65*p97+p69*p79+p50*p67+p48*p50-p65*p106-p65*p127-p65*p113-p88*p84-p88*p93-p50*p74+p65*p134+p65*"
          "p120+p88*p85+p50*p62+p50*p68-p65*p142-p65*p125-p65*p152-p88*p94-p65*p121-p65*p135-p79*p97-p50*p84-p50*p93+p79*p1"
          "06+p79*p127+p79*p113-p79*p134-p79*p120-p65*p143-p50*p94+p79*p142+p79*p125+p79*p152+p79*p121+p79*p135+p65*p126+p5"
          "0*p85-p79*p126+p79*p143;\n    A(5,27) = -p53*p69-p67*p102-s3*p5*p47+p74*p102+p49*p138-p62*p102-p68*p102+s5*p151+"
          "p84*p102+p93*p102-s5*p19*p30-p42*p138-s5*p19*p36-p85*p102+s5*p19*p31+p94*p102-p41*p138-s5*p19*p37-p43*p138-p24*p"
          "165-p28*p165+p25*p165-p29*p165;\n\n    A(6,6) = -c6*p104*p158-c6*p107*p158-p32*p105*p131-c6*p105*p104+p51*p101*p"
          "158+p53*p67*p105+p5*p20*p53*p147-p24*p72*p158-p28*p72*p158-p56*p101*p158-p53*p74*p105-handOffs*p91*p118+p30*p80*"
          "p158+p36*p80*p158+p26*p105*p173+p51*p91*p185+p67*p91*p90+p5*p22*p45*p147-p30*p47*p105-p42*p95*p105-p36*p47*p105-"
          "p53*p84*p105-p24*p91*p147-p53*p93*p105-p28*p91*p147-p56*p91*p185-p74*p91*p90+p49*p95*p105+p25*p72*p158+p46*p101*"
          "p158+p52*p101*p158+p53*p62*p105+p53*p68*p105-p29*p72*p158-p32*p59*p159-p84*p91*p90-p91*p93*p90+p20*p49*p159-p31*"
          "p80*p158+p31*p47*p105+p37*p80*p158+p53*p85*p105+p25*p91*p147+p46*p91*p185+p62*p91*p90+p52*p91*p185+p68*p91*p90-p"
          "41*p95*p105-p37*p47*p105-p43*p95*p105-p53*p94*p105-p29*p91*p147+p151*p159+p30*p91*p178+p36*p91*p178-p24*p86*p105"
          "-p19*p30*p159-p20*p42*p159-p28*p86*p105-p19*p36*p159-p31*p91*p178+p25*p86*p105+p19*p31*p159+p85*p91*p90+p37*p91*"
          "p178-p20*p41*p159-p29*p86*p105-p19*p37*p159-p20*p43*p159-p91*p94*p90-p23*p24*p159-p23*p28*p159+p23*p25*p159-p23*"
          "p29*p159;\n    A(6,22) = +c6*p22*p104-c6*p104-c6*p104-p53*p74-p53*p74+c6*p22*p107+p32*p35*p131+c6*p35*p104-p22*p"
          "51*p101-p35*p53*p67-p5*p19*p20*p53+p22*p24*p72+p22*p28*p72+p22*p56*p101+p35*p53*p74-c6*p107-c6*p107-p32*p131-p32"
          "*p131+p51*p101+p51*p101+p53*p67+p53*p67+p5*p186+p5*p186-p24*p72-p24*p72-p28*p72-p56*p101-p28*p72-p56*p101+p32*p5"
          "9*p149+p91*p118-p22*p30*p80-p22*p36*p80-p26*p35*p173-p20*p51*p91-p18*p67*p91-p5*p22*p186+p30*p35*p47+p35*p42*p95"
          "+p35*p36*p47+p35*p53*p84+p19*p24*p91+p35*p53*p93+p19*p28*p91+p20*p56*p91+p18*p74*p91+p25*p72+p46*p101+p25*p72+p4"
          "6*p101+p52*p101+p52*p101-p35*p49*p95+p53*p62+p53*p62+p53*p68+p53*p68-p29*p72-p29*p72-p22*p25*p72-p22*p46*p101-p2"
          "2*p52*p101+p30*p80+p30*p80+p36*p80+p36*p80-p35*p53*p62-p35*p53*p68+p26*p173+p26*p173+p22*p29*p72-p30*p47-p42*p95"
          "-p30*p47-p42*p95-p36*p47-p36*p47-p53*p84-p53*p84-p53*p93-p53*p93+p49*p95+p49*p95-p31*p80-p31*p80+p24*p35*p86+p19"
          "*p30*p149+p20*p42*p149+p28*p35*p86+p19*p36*p149+p18*p84*p91+p18*p93*p91+p31*p47+p31*p47+p37*p80+p37*p80-p20*p49*"
          "p149+p53*p85+p53*p85-p41*p95-p41*p95-p37*p47-p43*p95-p37*p47-p43*p95-p53*p94-p53*p94+p22*p31*p80-p31*p35*p47-p22"
          "*p37*p80-p35*p53*p85-p19*p25*p91-p20*p46*p91-p18*p62*p91-p20*p52*p91-p18*p68*p91+p35*p41*p95+p35*p37*p47+p35*p43"
          "*p95-p24*p86-p24*p86-p28*p86-p28*p86+p35*p53*p94+p19*p29*p91-p151*p149-p23*p30*p91-p23*p36*p91-p29*p86-p29*p86+p"
          "23*p31*p91-p25*p35*p86-p19*p31*p149-p18*p85*p91-p23*p37*p91+p20*p41*p149+p29*p35*p86+p19*p37*p149+p20*p43*p149+p"
          "18*p94*p91+p23*p24*p149+p23*p28*p149+p25*p86+p25*p86-p23*p25*p149+p23*p29*p149;\n    A(6,24) = -p140-p140-p19*p2"
          "4-p19*p24-p19*p28-p20*p56-p19*p28-p20*p56+p16*p35*p140+p17*p35*p140+p11*p69+p11*p69-p118-p118-p18*p67-p18*p67-p1"
          "8*p48-p18*p48+p20*p51+p20*p51+p18*p74+p18*p74-p69*p167-p18*p62-p18*p62-p18*p68-p18*p68-p69*p175+p19*p25+p20*p46+"
          "p19*p25+p20*p46+p20*p52+p20*p52+p16*p35*p118+p16*p22*p140+p22*p67*p103+p17*p35*p118+p22*p48*p103+p17*p22*p140-p3"
          "5*p51*p122+p22*p67*p111-p19*p29-p19*p29+p22*p48*p111-p35*p51*p128-p22*p74*p103+p24*p35*p129+p28*p35*p129+p35*p56"
          "*p122-p22*p74*p111+p24*p35*p136+p28*p35*p136+p35*p56*p128-p11*p97-p11*p97+p23*p30+p23*p30+p11*p106+p11*p127+p11*"
          "p106+p11*p127+p23*p36+p23*p36+p11*p113+p11*p113+p18*p84+p18*p84+p18*p93+p18*p93-p11*p134-p11*p134-p25*p35*p136-p"
          "35*p46*p128-p35*p52*p128+p29*p35*p129-p23*p31-p23*p31-p11*p120-p11*p120+p29*p35*p136-p18*p85-p18*p85-p35*p69*p92"
          "+p11*p142+p11*p142+p23*p37+p23*p37+p11*p125+p11*p152+p11*p125+p11*p152+p16*p22*p118+p97*p167-p35*p69*p81+p18*p94"
          "+p18*p94+p17*p22*p118+p97*p175-p30*p35*p153-p106*p167-p22*p51*p122-p127*p167+p35*p67*p103-p35*p36*p153-p113*p167"
          "+p5*p19*p20*p103-p30*p35*p144-p22*p84*p103-p106*p175-p22*p51*p128-p127*p175+p35*p67*p111-p35*p36*p144-p22*p93*p1"
          "03-p113*p175+p5*p19*p20*p111-p22*p84*p111+p22*p24*p129-p22*p93*p111+p22*p28*p129+p22*p56*p122+p134*p167-p35*p74*"
          "p103+p22*p24*p136+p22*p28*p136+p22*p56*p128+p134*p175-p35*p74*p111+p11*p121+p11*p121+p11*p135+p11*p135+p22*p62*p"
          "103+p22*p68*p103-p25*p35*p129-p35*p46*p122+p22*p62*p111-p35*p52*p122+p22*p68*p111+p11*p143+p11*p143+p151*p190-p2"
          "2*p30*p153-p121*p167-p22*p36*p153-p135*p167+p151*p193-p22*p30*p144-p121*p175-p19*p30*p190-p20*p42*p190-p22*p36*p"
          "144-p135*p175-p19*p36*p190-p20*p54*p59*p103-p19*p30*p193-p20*p42*p193-p20*p59*p60*p103-p19*p36*p193-p20*p54*p59*"
          "p111-p20*p59*p60*p111+p20*p49*p190+p20*p49*p193+p31*p35*p153+p120*p167+p31*p35*p144+p22*p85*p103+p120*p175+p22*p"
          "85*p111-p22*p25*p129-p22*p46*p122-p142*p167+p35*p62*p103-p35*p37*p153-p125*p167-p22*p52*p122-p152*p167+p35*p68*p"
          "103-p22*p25*p136-p22*p46*p128-p142*p175+p35*p62*p111-p35*p37*p144-p22*p94*p103-p125*p175-p22*p52*p128-p152*p175+"
          "p35*p68*p111-p22*p94*p111+p22*p29*p129-p11*p126-p11*p126+p22*p29*p136-p35*p92*p121-p23*p28*p190-p35*p81*p121-p23"
          "*p28*p193+p22*p31*p153+p126*p167+p22*p31*p144+p126*p175+p19*p31*p190+p20*p59*p61*p103+p19*p31*p193-p22*p37*p153-"
          "p143*p167+p20*p59*p61*p111-p20*p41*p190-p22*p37*p144-p143*p175-p19*p37*p190-p20*p43*p190-p20*p41*p193-p20*p59*p6"
          "3*p103-p19*p37*p193-p20*p43*p193-p20*p59*p63*p111+p35*p92*p126+p35*p81*p126-p23*p29*p190-p23*p29*p193;\n    A(6,"
          "25) = -c6*c7*p104-c6*c7*p107-p32*p59*p65+c7*p51*p101+c7*p53*p67+p5*p19*p170-c7*p24*p72-c7*p28*p72-c7*p56*p101-c7"
          "*p53*p74+c7*p25*p72+c7*p46*p101+c7*p52*p101+c7*p53*p62+c7*p53*p68-c7*p29*p72+c7*p30*p80+c7*p36*p80+c7*p26*p173-c"
          "7*p30*p47-p20*p42*p65-c7*p36*p47-c7*p53*p84-c7*p53*p93+p20*p49*p65+c7*p53*p85-p20*p41*p65-c7*p37*p47-p20*p43*p65"
          "-c7*p53*p94-p23*p24*p65-p23*p28*p65-c7*p31*p80+c7*p31*p47+c7*p37*p80+p23*p25*p65-p23*p29*p65;\n    A(6,26) = +s7"
          "*c6*p104+s7*c6*p107+p32*p59*p70-s7*p51*p101-s7*p53*p67-p5*p19*p179+s7*p24*p72+s7*p28*p72+s7*p56*p101+s7*p53*p74-"
          "s7*p25*p72-s7*p46*p101-s7*p52*p101-s7*p53*p62-s7*p53*p68+s7*p29*p72-s7*p30*p80-s7*p36*p80-s7*p26*p173+s7*p30*p47"
          "+p20*p42*p70+s7*p36*p47+s7*p53*p84+s7*p53*p93-p20*p49*p70-s7*p31*p47-s7*p37*p80-s7*p53*p85+p20*p41*p70+s7*p37*p4"
          "7+p20*p43*p70+s7*p53*p94+p23*p24*p70+p23*p28*p70+s7*p31*p80-p23*p25*p70+p23*p29*p70;\n    A(6,27) = -p16*p140-p1"
          "7*p140+p69*p81-p16*p118-p67*p103-p17*p118-p48*p103+p51*p122-p67*p111-p48*p111+p51*p128+p74*p103-p24*p129-p28*p12"
          "9-p56*p122+p74*p111-p24*p136-p28*p136-p56*p128+p69*p92+p30*p144+p84*p103+p81*p106+p81*p127+p36*p144+p93*p103+p81"
          "*p113+p84*p111+p93*p111-p92*p134-p81*p134-p62*p103-p68*p103+p25*p129+p46*p122-p62*p111+p52*p122-p68*p111+p25*p13"
          "6+p46*p128+p52*p128-p29*p129-p29*p136-p97*p92-p81*p97+p30*p153+p92*p106+p92*p127+p36*p153+p92*p113-p31*p153-p92*"
          "p120-p31*p144-p85*p103-p81*p120-p85*p111+p92*p142+p37*p153+p92*p125+p92*p152+p81*p142+p37*p144+p94*p103+p81*p125"
          "+p81*p152+p94*p111+p92*p121+p92*p135+p81*p121+p81*p135-p92*p126-p81*p126+p92*p143+p81*p143;\n\n    A(7,25) = +s7"
          "*p17*p140-p69*p88-p67*p65-p48*p65+p65*p74-p69*p160+s7*p17*p118+s7*p16*p140+p17*p67*p79+p17*p48*p79-s7*p51*p128-p"
          "62*p65-p68*p65-p17*p74*p79+s7*p24*p136+s7*p28*p136+s7*p56*p128+p88*p97+p65*p84+p65*p93-p30*p161-p42*p170-p36*p16"
          "1+p49*p170+p16*p48*p79-s7*p30*p144-p106*p160-p127*p160-s7*p51*p122-s7*p36*p144-p113*p160-p65*p85-p17*p84*p79-p17"
          "*p79*p93-p16*p74*p79+p31*p161+s7*p24*p129+s7*p28*p129+p134*p160+s7*p56*p122+p65*p94-p41*p170-p37*p161-p43*p170+p"
          "17*p62*p79+p17*p68*p79-s7*p25*p136-s7*p46*p128-s7*p52*p128-p24*p180-p28*p180+s7*p29*p136-p69*p176+p97*p160+s7*p1"
          "6*p118+p16*p67*p79-p29*p180+s7*p31*p144+p120*p160+p17*p85*p79+p16*p62*p79+p16*p68*p79-s7*p25*p129-p142*p160-s7*p"
          "46*p122-s7*p37*p144-p125*p160-p160*p152-s7*p52*p122-p17*p79*p94+s7*p29*p129+p97*p176-p121*p160-s7*p30*p153-p106*"
          "p176-p127*p176-p135*p160-s7*p36*p153-p113*p176-p16*p84*p79-p16*p79*p93+p25*p180+p134*p176+p126*p160+s7*p31*p153+"
          "p120*p176+p16*p85*p79-p142*p176-p143*p160-s7*p37*p153-p125*p176-p152*p176-p16*p79*p94-p121*p176-p135*p176+p126*p"
          "176-p143*p176;\n    A(7,26) = +c7*p16*p140+p69*p96+p67*p70+p48*p70-p74*p70-p49*p179+c7*p16*p118+p16*p67*p71+p16*"
          "p48*p71+c7*p17*p140-c7*p51*p122-p16*p74*p71+c7*p24*p129+c7*p28*p129+c7*p56*p122+p62*p70+p68*p70-p97*p96-p70*p84-"
          "p70*p93+p30*p169+p42*p179+p36*p169-p69*p168+p17*p48*p71-p16*p71*p84-c7*p51*p128-p16*p71*p93+p134*p168-p17*p74*p7"
          "1+p70*p85+c7*p24*p136+c7*p28*p136+c7*p56*p128-p31*p169-p70*p94+p41*p179+p37*p169+p43*p179+p16*p62*p71+p16*p68*p7"
          "1-c7*p25*p129-c7*p46*p122-c7*p52*p122+c7*p29*p129+p24*p184+p28*p184-p69*p177+p97*p168+c7*p17*p118-c7*p30*p153-p1"
          "06*p168-p127*p168+p17*p67*p71-c7*p36*p153-p113*p168-p25*p184+p29*p184+c7*p31*p153+p120*p168+p16*p71*p85-p142*p16"
          "8+p17*p62*p71-c7*p37*p153-p125*p168-p152*p168+p17*p68*p71-c7*p25*p136-c7*p46*p128-p16*p71*p94-c7*p52*p128+c7*p29"
          "*p136+p97*p177-p121*p168-p135*p168-c7*p30*p144-p106*p177-p127*p177-c7*p36*p144-p113*p177-p17*p71*p84-p17*p71*p93"
          "+p134*p177+p126*p168+c7*p31*p144+p120*p177+p17*p71*p85-p143*p168-p142*p177-c7*p37*p144-p125*p177-p152*p177-p17*p"
          "71*p94-p121*p177-p135*p177+p126*p177-p143*p177;\n\nend"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    193
        position		    [23.5747 49.5747 7]
        chart		    191
        linkNode		    [191 0 0]
        subviewer		    191
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    194
        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		      193
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    191
        linkNode		    [191 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    191
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    195
        ssIdNumber		    8
        name		    "q_motor"
        linkNode		    [191 0 196]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    196
        ssIdNumber		    9
        name		    "x_hat"
        linkNode		    [191 195 197]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    197
        ssIdNumber		    10
        name		    "gearRatio"
        linkNode		    [191 196 198]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    198
        ssIdNumber		    12
        name		    "baseAngles"
        linkNode		    [191 197 199]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    199
        ssIdNumber		    13
        name		    "gravTrq"
        linkNode		    [191 198 200]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    200
        ssIdNumber		    14
        name		    "gravTrqMotor"
        linkNode		    [191 199 201]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    201
        ssIdNumber		    15
        name		    "coulombWidthMotor"
        linkNode		    [191 200 202]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    202
        ssIdNumber		    16
        name		    "coulombWidth"
        linkNode		    [191 201 0]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    203
        name		    "Right/calcGravityTorque/calcGravTrq"
        machine		    1
        chart		    191
      }
      chart {
        id			    204
        name		    "botharms_get_torque/MATLAB Function3"
        windowPosition	    [142.688 83.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 205 0 0]
        firstTransition	    207
        firstJunction	    206
        viewObj		    204
        machine		    1
        ssIdHighWaterMark	    18
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    208
        chartFileNumber	    17
        disableImplicitCasting  1
        eml {
          name		      "fcn"
        }
      }
      state {
        id			    205
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    204
        treeNode		    [204 0 0 0]
        superState		    SUBCHART
        subviewer		    204
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function pose    = fcn(T44_FK)\n%#codegen\n\n% Reshaping:\nT44 = reshape(T44_FK, 4,4)';\n\n\n\n\n"
          "% Position:\np1 = T44(1:3,4)./1000;\n\n \n\n% Orientation\nR = T44(1:3,1:3);\n\nRxx = R(1,1); Rxy = R(1,2); Rxz "
          "= R(1,3);\nRyx = R(2,1); Ryy = R(2,2); Ryz = R(2,3);\nRzx = R(3,1); Rzy = R(3,2); Rzz = R(3,3);\n\nw = sqrt( tra"
          "ce( R ) + 1 ) / 2;\n\n% check if w is real. Otherwise, zero it.\nif( imag( w ) > 0 )\n     w = 0;\nend\n\nx1 = ("
          " Rzy - Ryz ) / (4*w);\ny1 = ( Rxz - Rzx ) / (4*w);\nz1 = ( Ryx - Rxy ) / (4*w);\nq1 = [x1; y1; z1];\n\nif any(is"
          "nan(q1)) || any(isinf(q1))\n    q1 = zeros(size(q1));\nend\n\npersistent q_old\nif isempty(q_old)\n    q_old = q"
          "1;\nend\n\nq2 = -q1;\nif (norm(q_old - q1) < norm(q_old-q2)) || norm(q1)<.1\n    q_out = q1;\nelse\n    q_out = "
          "q2;\nend\n\n\nq_old = q_out;\n\npose = [p1 ; q_out];\n\n"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    206
        position		    [23.5747 49.5747 7]
        chart		    204
        linkNode		    [204 0 0]
        subviewer		    204
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    207
        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		      206
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    204
        linkNode		    [204 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    204
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    208
        ssIdNumber		    7
        name		    "T44_FK"
        linkNode		    [204 0 209]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    209
        ssIdNumber		    4
        name		    "pose"
        linkNode		    [204 208 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    210
        name		    "botharms_get_torque/MATLAB Function3"
        machine		    1
        chart		    204
      }
      chart {
        id			    211
        name		    "botharms_get_torque/MATLAB Function4"
        windowPosition	    [157.688 68.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 212 0 0]
        firstTransition	    214
        firstJunction	    213
        viewObj		    211
        machine		    1
        ssIdHighWaterMark	    21
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    215
        chartFileNumber	    18
        disableImplicitCasting  1
        eml {
          name		      "fcn"
        }
      }
      state {
        id			    212
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    211
        treeNode		    [211 0 0 0]
        superState		    SUBCHART
        subviewer		    211
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function [vel1,vel2]    = fcn(J1,J2,q_dot)\n%#codegen\n\nJ1 = reshape(J1,7,6)';\nJ1(1:3,:) = J1(1"
          ":3,:)/1000; % Convert to m/s\n\nJ2 = reshape(J2,7,6)';\nJ2(1:3,:) = J2(1:3,:)/1000; % Convert to m/s\n\nqd1 = q_"
          "dot(1:7);\nqd2 = q_dot(8:end);\n\n\nvel1 = J1*qd1;\nvel2 = J2*qd2;"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    213
        position		    [23.5747 49.5747 7]
        chart		    211
        linkNode		    [211 0 0]
        subviewer		    211
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    214
        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		      213
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    211
        linkNode		    [211 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    211
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    215
        ssIdNumber		    7
        name		    "J1"
        linkNode		    [211 0 216]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    216
        ssIdNumber		    4
        name		    "vel1"
        linkNode		    [211 215 217]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    217
        ssIdNumber		    19
        name		    "J2"
        linkNode		    [211 216 218]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    218
        ssIdNumber		    20
        name		    "q_dot"
        linkNode		    [211 217 219]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    219
        ssIdNumber		    21
        name		    "vel2"
        linkNode		    [211 218 0]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    220
        name		    "botharms_get_torque/MATLAB Function4"
        machine		    1
        chart		    211
      }
      chart {
        id			    221
        name		    "botharms_get_torque/lp1"
        windowPosition	    [165.188 53.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 222 0 0]
        firstTransition	    224
        firstJunction	    223
        viewObj		    221
        machine		    1
        toolbarMode		    LIBRARY_TOOLBAR
        ssIdHighWaterMark	    5
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    225
        chartFileNumber	    19
        disableImplicitCasting  1
        eml {
          name		      "lp"
        }
      }
      state {
        id			    222
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    221
        treeNode		    [221 0 0 0]
        superState		    SUBCHART
        subviewer		    221
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function v_out = lp(v_in)\n%#codegen\n\n\npersistent v_flt\nif isempty(v_flt)\n    v_flt = v_in;\n"
          "end\nT = 0.500;\ndt = 0.004;\n\nv_dot = (v_in - v_flt)/T;\n\nv_flt = v_flt + dt*v_dot;\n\nv_out = v_flt;\n"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    223
        position		    [23.5747 49.5747 7]
        chart		    221
        linkNode		    [221 0 0]
        subviewer		    221
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    224
        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		      223
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    221
        linkNode		    [221 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    221
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    225
        ssIdNumber		    4
        name		    "v_in"
        linkNode		    [221 0 226]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    226
        ssIdNumber		    5
        name		    "v_out"
        linkNode		    [221 225 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    227
        name		    "botharms_get_torque/lp1"
        machine		    1
        chart		    221
      }
      chart {
        id			    228
        name		    "botharms_get_torque/dither/DITHERING"
        windowPosition	    [495.75 83.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 229 0 0]
        firstTransition	    231
        firstJunction	    230
        viewObj		    228
        machine		    1
        toolbarMode		    LIBRARY_TOOLBAR
        ssIdHighWaterMark	    7
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    232
        chartFileNumber	    20
        disableImplicitCasting  1
        eml {
          name		      "fcn"
        }
      }
      state {
        id			    229
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    228
        treeNode		    [228 0 0 0]
        superState		    SUBCHART
        subviewer		    228
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function dithering = fcn(coulomb,Fdith,amp)\n\npersistent cont\nif isempty(cont)\n    cont=0;\nen"
          "d\n\npersistent out\nif isempty(out)\n    out=0;\nend\n\n\ntc=1/250; % sempling period\nT=1/Fdith; % period of t"
          "he dithering signal\nN=round(T/tc); % number of sample in a period of the dithering signal\n\n\n\nif cont>=0 && "
          "cont<N/2 % first half period\n    out=amp;\n    cont=cont+1;\nend\nif cont >= N/2 && cont<=N % second half perio"
          "d\n        out=-amp;\n        cont=cont+1;\nend\n\nif  cont>=N % reset cont \n    cont=0;\nend\n\ndithering=out."
          "*coulomb;"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    230
        position		    [23.5747 49.5747 7]
        chart		    228
        linkNode		    [228 0 0]
        subviewer		    228
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    231
        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		      230
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    228
        linkNode		    [228 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    228
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    232
        ssIdNumber		    7
        name		    "coulomb"
        linkNode		    [228 0 233]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    233
        ssIdNumber		    4
        name		    "Fdith"
        linkNode		    [228 232 234]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    234
        ssIdNumber		    5
        name		    "dithering"
        linkNode		    [228 233 235]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    235
        ssIdNumber		    6
        name		    "amp"
        linkNode		    [228 234 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    236
        name		    "botharms_get_torque/dither/DITHERING"
        machine		    1
        chart		    228
      }
      chart {
        id			    237
        name		    "botharms_get_torque/coulFric1"
        windowPosition	    [195.375 188.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 238 0 0]
        firstTransition	    240
        firstJunction	    239
        viewObj		    237
        machine		    1
        toolbarMode		    LIBRARY_TOOLBAR
        ssIdHighWaterMark	    13
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    241
        chartFileNumber	    21
        disableImplicitCasting  1
        eml {
          name		      "forcelog"
        }
      }
      state {
        id			    238
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    237
        treeNode		    [237 0 0 0]
        superState		    SUBCHART
        subviewer		    237
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function [wrench1_out, wrench2_out] = forcelog(T1,T2, wrench1, wrench2)\n\n% Rotations\nrotz = -p"
          "i/4;\ntf2t = [cos(rotz) -sin(rotz) 0;\n    sin(rotz) cos(rotz) 0;\n    0 0 1];\n\nrotz2 = -pi/2;\ntf2t2 = [cos(r"
          "otz2) -sin(rotz2) 0;\n    sin(rotz2) cos(rotz2) 0;\n    0 0 1];\n\npersistent w1offs\nif isempty(w1offs)\n    w1"
          "offs = wrench1;\nend\nif w1offs(1) == 0 && wrench1(1) ~= 0\n    w1offs = wrench1;\nend\nwrench1 = wrench1 - w1of"
          "fs;\n\n\n\n\npersistent w2offs\nif isempty(w2offs)\n    w2offs = wrench2;\nend\nif w2offs(1) == 0 && wrench2(1) "
          "~= 0\n    w2offs = wrench2;\nend\nwrench2 = wrench2 - w2offs;\n\n\n\nT1 = reshape(T1, 4,4)';\nR1 = T1(1:3,1:3);\n"
          "wrench1_out = [R1*tf2t*wrench1(1:3); R1*tf2t*wrench1(1:3)];\n\n\nT2 = reshape(T2, 4,4)';\nR2 = T2(1:3,1:3);\nwre"
          "nch2_out = [R2*tf2t2*wrench2(1:3); R2*tf2t2*wrench2(1:3)];\n"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    239
        position		    [23.5747 49.5747 7]
        chart		    237
        linkNode		    [237 0 0]
        subviewer		    237
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    240
        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		      239
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    237
        linkNode		    [237 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    237
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    241
        ssIdNumber		    4
        name		    "T1"
        linkNode		    [237 0 242]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    242
        ssIdNumber		    9
        name		    "T2"
        linkNode		    [237 241 243]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    243
        ssIdNumber		    5
        name		    "wrench1_out"
        linkNode		    [237 242 244]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    244
        ssIdNumber		    11
        name		    "wrench1"
        linkNode		    [237 243 245]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    245
        ssIdNumber		    12
        name		    "wrench2"
        linkNode		    [237 244 246]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    246
        ssIdNumber		    13
        name		    "wrench2_out"
        linkNode		    [237 245 0]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    247
        name		    "botharms_get_torque/coulFric1"
        machine		    1
        chart		    237
      }
      chart {
        id			    248
        name		    "botharms_get_torque/lp2"
        windowPosition	    [195.188 23.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 249 0 0]
        firstTransition	    251
        firstJunction	    250
        viewObj		    248
        machine		    1
        toolbarMode		    LIBRARY_TOOLBAR
        ssIdHighWaterMark	    5
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    252
        chartFileNumber	    22
        disableImplicitCasting  1
        eml {
          name		      "lp"
        }
      }
      state {
        id			    249
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    248
        treeNode		    [248 0 0 0]
        superState		    SUBCHART
        subviewer		    248
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function v_out = lp(v_in)\n%#codegen\n\n\npersistent v_flt\nif isempty(v_flt)\n    v_flt = v_in;\n"
          "end\nT = 0.100;\ndt = 0.004;\n\nv_dot = (v_in - v_flt)/T;\n\nv_flt = v_flt + dt*v_dot;\n\nv_out = v_flt;\n"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    250
        position		    [23.5747 49.5747 7]
        chart		    248
        linkNode		    [248 0 0]
        subviewer		    248
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    251
        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		      250
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    248
        linkNode		    [248 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    248
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    252
        ssIdNumber		    4
        name		    "v_in"
        linkNode		    [248 0 253]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    253
        ssIdNumber		    5
        name		    "v_out"
        linkNode		    [248 252 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    254
        name		    "botharms_get_torque/lp2"
        machine		    1
        chart		    248
      }
      chart {
        id			    255
        name		    "botharms_get_torque/lp3"
        windowPosition	    [195.188 23.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 256 0 0]
        firstTransition	    258
        firstJunction	    257
        viewObj		    255
        machine		    1
        toolbarMode		    LIBRARY_TOOLBAR
        ssIdHighWaterMark	    5
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    259
        chartFileNumber	    23
        disableImplicitCasting  1
        eml {
          name		      "lp"
        }
      }
      state {
        id			    256
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    255
        treeNode		    [255 0 0 0]
        superState		    SUBCHART
        subviewer		    255
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function v_out = lp(v_in)\n%#codegen\n\n\npersistent v_flt\nif isempty(v_flt)\n    v_flt = v_in;\n"
          "end\nT = 0.100;\ndt = 0.004;\n\nv_dot = (v_in - v_flt)/T;\n\nv_flt = v_flt + dt*v_dot;\n\nv_out = v_flt;\n"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    257
        position		    [23.5747 49.5747 7]
        chart		    255
        linkNode		    [255 0 0]
        subviewer		    255
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    258
        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		      257
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    255
        linkNode		    [255 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    255
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    259
        ssIdNumber		    4
        name		    "v_in"
        linkNode		    [255 0 260]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    260
        ssIdNumber		    5
        name		    "v_out"
        linkNode		    [255 259 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    261
        name		    "botharms_get_torque/lp3"
        machine		    1
        chart		    255
      }
      chart {
        id			    262
        name		    "botharms_get_torque/MATLAB Function6"
        windowPosition	    [270.375 83.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 263 0 0]
        firstTransition	    265
        firstJunction	    264
        viewObj		    262
        machine		    1
        toolbarMode		    LIBRARY_TOOLBAR
        ssIdHighWaterMark	    5
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    266
        chartFileNumber	    24
        disableImplicitCasting  1
        eml {
          name		      "abb2logical"
        }
      }
      state {
        id			    263
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    262
        treeNode		    [262 0 0 0]
        superState		    SUBCHART
        subviewer		    262
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function logical = abb2logical(abb)\n%#codegen\n\nlogical = [abb(1:2); abb(7); abb(3:6)];"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    264
        position		    [23.5747 49.5747 7]
        chart		    262
        linkNode		    [262 0 0]
        subviewer		    262
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    265
        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		      264
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    262
        linkNode		    [262 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    262
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    266
        ssIdNumber		    4
        name		    "abb"
        linkNode		    [262 0 267]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    267
        ssIdNumber		    5
        name		    "logical"
        linkNode		    [262 266 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    268
        name		    "botharms_get_torque/MATLAB Function6"
        machine		    1
        chart		    262
      }
      chart {
        id			    269
        name		    "Right/MATLAB Function3"
        windowPosition	    [315.375 38.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 1920 1200 1.25]
        treeNode		    [0 270 0 0]
        firstTransition	    272
        firstJunction	    271
        viewObj		    269
        machine		    1
        ssIdHighWaterMark	    32
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    273
        updateMethod	    DISCRETE
        sampleTime		    "0.004"
        chartFileNumber	    27
        disableImplicitCasting  1
        eml {
          name		      "fcn"
        }
      }
      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 [runExtctrl , lockOutput, resetProgram, state] = fcn(controlActive, obtaining, switch_on"
          ")\n\n\nleadthrough_on =switch_on && obtaining;\n%obtaining = 1;\n% state variables\npersistent internalState;\n\n"
          "% internal variables\npersistent runExtctrlInternal;\npersistent lockOutputInternal;\npersistent resetProgramInt"
          "ernal;\n\nif isempty(internalState)\n    internalState = 1;\nend\n\nif isempty(runExtctrlInternal)\n    runExtct"
          "rlInternal = 0;\nend\nif isempty(lockOutputInternal)\n    lockOutputInternal = 0;\nend\nif isempty(resetProgramI"
          "nternal)\n    resetProgramInternal = 1;\nend\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n% number       internalSta"
          "te\n%   1          Extctrl_Idle\n%   2          Extctrl_Running\n%   3          Recovery_Phase\n\n%%%%%%%%%%%%%%"
          "%%%%%%%%%%%%%%%%%%%%\n\n% State transitions + entry/exit-actions\nif internalState==1\n    if (leadthrough_on) &"
          "& obtaining\n        %Exit-actions\n        \n        %New state\n        internalState = 2;\n        \n        "
          "%Entry-actions\n        runExtctrlInternal = 1;\n        lockOutputInternal = 0;\n        resetProgramInternal ="
          " 0;\n    end\nelseif internalState==2\n    if ~controlActive\n        %Exit-actions\n        \n        %New stat"
          "e\n        internalState = 1;\n        \n        %Entry-actions\n        runExtctrlInternal = 0;\n        lockOu"
          "tputInternal = 0;\n        resetProgramInternal = 1;\n    elseif (~leadthrough_on) && controlActive\n        %Ex"
          "it-actions\n        \n        %New state\n        internalState = 3;\n        \n        %Entry-actions\n        "
          "runExtctrlInternal = 0;\n        lockOutputInternal = 1;\n        resetProgramInternal = 0;     \n    end\nelsei"
          "f internalState==3\n    if (~leadthrough_on) || ~controlActive\n        %Exit-actions\n        \n        %New st"
          "ate\n        internalState = 1;\n        \n        %Entry-actions\n        runExtctrlInternal = 0;\n        lock"
          "OutputInternal = 0;\n        resetProgramInternal = 1;       \n    end\nend\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"
          "%\n\n% Continous state actions\n\n%Set outputs\nrunExtctrl = runExtctrlInternal;\nlockOutput = lockOutputInterna"
          "l;\nresetProgram = resetProgramInternal;\nstate = internalState;"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    271
        position		    [23.5747 49.5747 7]
        chart		    269
        linkNode		    [269 0 0]
        subviewer		    269
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    272
        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		      271
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    269
        linkNode		    [269 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    269
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    273
        ssIdNumber		    28
        name		    "runExtctrl"
        linkNode		    [269 0 274]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    274
        ssIdNumber		    5
        name		    "lockOutput"
        linkNode		    [269 273 275]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    275
        ssIdNumber		    27
        name		    "resetProgram"
        linkNode		    [269 274 276]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    276
        ssIdNumber		    29
        name		    "state"
        linkNode		    [269 275 277]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    277
        ssIdNumber		    30
        name		    "controlActive"
        linkNode		    [269 276 278]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    278
        ssIdNumber		    31
        name		    "obtaining"
        linkNode		    [269 277 279]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    279
        ssIdNumber		    32
        name		    "switch_on"
        linkNode		    [269 278 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    280
        name		    "Right/MATLAB Function3"
        machine		    1
        chart		    269
      }
      chart {
        id			    281
        name		    "botharms_get_torque/MATLAB Function7"
        windowPosition	    [390.375 -36.75 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 282 0 0]
        firstTransition	    284
        firstJunction	    283
        viewObj		    281
        machine		    1
        ssIdHighWaterMark	    28
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    285
        updateMethod	    DISCRETE
        sampleTime		    "0.004"
        chartFileNumber	    28
        disableImplicitCasting  1
        eml {
          name		      "nancheck"
        }
      }
      state {
        id			    282
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    281
        treeNode		    [281 0 0 0]
        superState		    SUBCHART
        subviewer		    281
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function y   = nancheck(u)\n\n\ny = u;\n\n\nif any(isnan(u)) || any(isinf(u))\n    y = zeros(size"
          "(u));\nend\n\nend"
          editorLayout	      "100 M4x1[772 19 980 1190]"
        }
      }
      junction {
        id			    283
        position		    [23.5747 49.5747 7]
        chart		    281
        linkNode		    [281 0 0]
        subviewer		    281
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    284
        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		      283
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    281
        linkNode		    [281 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    281
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    285
        ssIdNumber		    28
        name		    "y"
        linkNode		    [281 0 286]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    286
        ssIdNumber		    4
        name		    "u"
        linkNode		    [281 285 0]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    287
        name		    "botharms_get_torque/MATLAB Function7"
        machine		    1
        chart		    281
      }
      chart {
        id			    288
        name		    "botharms_get_torque/FRIDA arm acceleration NOT SI1/base_tool_transform"
        windowPosition	    [187.688 53.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 289 0 0]
        firstTransition	    291
        firstJunction	    290
        viewObj		    288
        machine		    1
        ssIdHighWaterMark	    13
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    292
        chartFileNumber	    29
        disableImplicitCasting  1
        eml {
          name		      "base_tool_transform"
        }
      }
      state {
        id			    289
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    288
        treeNode		    [288 0 0 0]
        superState		    SUBCHART
        subviewer		    288
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function [T44_out, J0_out, Jn_out, Jdot0_out] = base_tool_transform(T44_world2base, T44_flange2to"
          "ol, T44_in, J0_in, qd, Jdot0_in)\n\nT44_world2base_m = reshape(T44_world2base, 4, 4)';\nT44_flange2tool_m = resh"
          "ape(T44_flange2tool, 4, 4)';\nT44_in_m = reshape(T44_in, 4, 4)';\nJ0_in_m = reshape(J0_in, [], 6)';\nJdot0_in_m "
          "= reshape(Jdot0_in, [], 6)';\n\n% Forward kinematics\nT44_out_m = T44_world2base_m*T44_in_m*T44_flange2tool_m;\n"
          "\n% Rotation of base\nRbase = T44_world2base_m(1:3,1:3);\nTbase = [Rbase zeros(3);zeros(3) Rbase];\nJ0_m = Tbase"
          "*J0_in_m;\nJdot0_m = Tbase*Jdot0_in_m;\n\n% Handle tool transformation\nT44_baseToFlange = T44_world2base_m*T44_"
          "in_m;\nT44_baseToTcp = T44_out_m;\nr = T44_baseToTcp(1:3,4) - T44_baseToFlange(1:3,4);\nomega = J0_m(4:6,:);\n\n"
          "for i=1:size(Jdot0_m,2)\n    jContribution = cross(omega(:,i),r);\n    J0_m(1:3,i) = J0_m(1:3,i) + jContribution"
          ";\n    for j=1:size(Jdot0_m,2)\n        jdotContribution = cross(omega(:,i),cross(omega(:,j)*qd(j),r));\n       "
          " Jdot0_m(1:3,i) = Jdot0_m(1:3,i) + jdotContribution;\n    end\nend\n\n% Jacobian in tool frame\nJn_m = [T44_out_"
          "m(1:3,1:3)' zeros(3);zeros(3) T44_out_m(1:3,1:3)']*J0_m;\n\n       \nT44_out = reshape(T44_out_m', 16, 1);\nJ0_o"
          "ut = reshape(J0_m', [], 1);\nJn_out = reshape(Jn_m', [], 1);\nJdot0_out = reshape(Jdot0_m',[],1);\n\n\n"
          editorLayout	      "100 M4x1[205 227 1080 733]"
        }
      }
      junction {
        id			    290
        position		    [23.5747 49.5747 7]
        chart		    288
        linkNode		    [288 0 0]
        subviewer		    288
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    291
        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		      290
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    288
        linkNode		    [288 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    288
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    292
        ssIdNumber		    4
        name		    "T44_world2base"
        linkNode		    [288 0 293]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    293
        ssIdNumber		    5
        name		    "T44_out"
        linkNode		    [288 292 294]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    294
        ssIdNumber		    6
        name		    "T44_flange2tool"
        linkNode		    [288 293 295]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    295
        ssIdNumber		    7
        name		    "T44_in"
        linkNode		    [288 294 296]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    296
        ssIdNumber		    8
        name		    "J0_in"
        linkNode		    [288 295 297]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    297
        ssIdNumber		    9
        name		    "qd"
        linkNode		    [288 296 298]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    298
        ssIdNumber		    10
        name		    "Jdot0_in"
        linkNode		    [288 297 299]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    299
        ssIdNumber		    11
        name		    "J0_out"
        linkNode		    [288 298 300]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    300
        ssIdNumber		    12
        name		    "Jn_out"
        linkNode		    [288 299 301]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    301
        ssIdNumber		    13
        name		    "Jdot0_out"
        linkNode		    [288 300 0]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    302
        name		    "botharms_get_torque/FRIDA arm acceleration NOT SI1/base_tool_transform"
        machine		    1
        chart		    288
      }
      chart {
        id			    303
        name		    "botharms_get_torque/FRIDA arm acceleration NOT SI1/frida_kin"
        windowPosition	    [187.688 53.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 304 0 0]
        firstTransition	    306
        firstJunction	    305
        viewObj		    303
        machine		    1
        ssIdHighWaterMark	    9
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    307
        chartFileNumber	    30
        disableImplicitCasting  1
        eml {
          name		      "frida_kinematics"
        }
      }
      state {
        id			    304
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    303
        treeNode		    [303 0 0 0]
        superState		    SUBCHART
        subviewer		    303
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function [T44_fkine, J0, Jn, Jdot0] = frida_kinematics(q,qd)\n% Calculation of forward kinematics"
          ", jacobian in base frame and flange\n% frame, and Jacobian time derivative (in base frame)\n\nn_joints = 7;\n\nq"
          "_offs = [0;0;pi/2;0;0;0;0];\nq = q+q_offs;\n\nshoulderLen = 0.110*1000;\nshoulderOffs = -0.030*1000;\nelbowOffs "
          "= 0.0405*1000;\nwristOffs = 0.0135*1000;\nhandOffs = -0.027*1000;\nupArmLen = 0.2465*1000;\nlowArmLen = 0.265*10"
          "00;\nhandLen = 0.032*1000;\n\n\n%          alpha        a        theta       d       Rev=0/Pris=1\ndhpar = [ -pi"
          "/2   shoulderOffs   q(1)   shoulderLen   0\n           pi/2  -shoulderOffs   q(2)        0        0\n          -"
          "pi/2   elbowOffs      q(7)    upArmLen     0\n           pi/2  -elbowOffs      q(3)        0        0\n         "
          " -pi/2   wristOffs      q(4)    lowArmLen    0\n           pi/2   handOffs       q(5)        0        0\n       "
          "    0      0              q(6)     handLen     0 ];\n        \nbase = eye(4);\n     \ntrans = zeros(4,4,n_joints"
          ");\n        \nfor j=1:n_joints\n    t = [ cos(dhpar(j,3))   -sin(dhpar(j,3))*cos(dhpar(j,1))    sin(dhpar(j,3))*"
          "sin(dhpar(j,1))    dhpar(j,2)*cos(dhpar(j,3))\n          sin(dhpar(j,3))   cos(dhpar(j,3))*cos(dhpar(j,1))     -"
          "cos(dhpar(j,3))*sin(dhpar(j,1))   dhpar(j,2)*sin(dhpar(j,3))\n          0                 sin(dhpar(j,1))       "
          "              cos(dhpar(j,1))                    dhpar(j,4)\n          0                 0                      "
          "             0                                  1                          ];\n    trans(:,:,j) = t;\nend\n\n%%%"
          "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% fkine\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\nT = base;\nfor i=1:n_joints,\n	"
          "T = T * trans(:,:,i);\nend\n\nT44_fkine = reshape(T',16,1);\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% tr2rot\n%%"
          "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\nR = T(1:3,1:3);\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% J0 + Jdot0\n%%%%%%"
          "%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\nJ0_tmp = zeros(6,n_joints);\n\nJ_dot = zeros(6,n_joints);\nomega = zeros(3,n_joi"
          "nts);\nnonZeroOmega = zeros(n_joints,1);\n\nT44_FK = eye(4);\n\nfor k=1:n_joints\n\n    if dhpar(j,5) == 0\n		% "
          "revolute axis\n        omega(:,k) = T44_FK(1:3,3);\n        nonZeroOmega(k) = 1;\n        J0_tmp(4:6,k) = omega("
          ":,k);\n    end\n    p_prev = T44_FK(1:3,4);\n    T44_FK = T44_FK*trans(:,:,k);\n    r_tilde = T44_FK(1:3,4)-p_pr"
          "ev;\n    \n    for i=1:n_joints\n        if nonZeroOmega(i)\n            J0_tmp(1:3,i) = J0_tmp(1:3,i) + cross(o"
          "mega(:,i),r_tilde);\n            for j=1:n_joints\n                if nonZeroOmega(j)\n                    contr"
          "ibution = cross(omega(:,i),cross(omega(:,j)*qd(j),r_tilde));\n                    J_dot(1:3,i) = J_dot(1:3,i) + "
          "contribution;\n                end\n            end\n        end\n    end\n\n    if dhpar(j,5) == 1\n		% prismat"
          "ic axis\n        prismAxis = T44_FK(1:3,3);\n        J0_tmp(1:3,k) = prismAxis;\n        for i=1:n_joints\n     "
          "       contribution = cross(omega(:,i),prismAxis);\n            J_dot(1:3,k) = J_dot(1:3,k) + contribution*qd(i)"
          ";\n            J_dot(1:3,i) = J_dot(1:3,i) + contribution*qd(k);\n        end\n    end\nend\nJ0_tmp = J0_tmp(:,["
          "1 2 4 5 6 7 3]);  % Switch to wierd joint order\nJ0 = reshape(J0_tmp', 6*n_joints,1);\n\nJdot0 = reshape(J_dot',"
          " 6*n_joints,1);\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% Jn\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n\nJn_tmp = ["
          "R' zeros(3);zeros(3) R']*J0_tmp;\nJn = reshape(Jn_tmp', 6*n_joints,1);\n"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    305
        position		    [23.5747 49.5747 7]
        chart		    303
        linkNode		    [303 0 0]
        subviewer		    303
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    306
        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		      305
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    303
        linkNode		    [303 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    303
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    307
        ssIdNumber		    4
        name		    "q"
        linkNode		    [303 0 308]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    308
        ssIdNumber		    5
        name		    "T44_fkine"
        linkNode		    [303 307 309]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    309
        ssIdNumber		    6
        name		    "qd"
        linkNode		    [303 308 310]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    310
        ssIdNumber		    7
        name		    "J0"
        linkNode		    [303 309 311]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    311
        ssIdNumber		    8
        name		    "Jn"
        linkNode		    [303 310 312]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    312
        ssIdNumber		    9
        name		    "Jdot0"
        linkNode		    [303 311 0]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    313
        name		    "botharms_get_torque/FRIDA arm acceleration NOT SI1/frida_kin"
        machine		    1
        chart		    303
      }
      chart {
        id			    314
        name		    "botharms_get_torque/MATLAB Function10"
        windowPosition	    [292.875 53.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 315 0 0]
        firstTransition	    317
        firstJunction	    316
        viewObj		    314
        machine		    1
        toolbarMode		    LIBRARY_TOOLBAR
        ssIdHighWaterMark	    5
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    318
        chartFileNumber	    31
        disableImplicitCasting  1
        eml {
          name		      "abb2logical"
        }
      }
      state {
        id			    315
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    314
        treeNode		    [314 0 0 0]
        superState		    SUBCHART
        subviewer		    314
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function logical = abb2logical(abb)\n%#codegen\n\nlogical = [abb(1:2); abb(7); abb(3:6)];"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    316
        position		    [23.5747 49.5747 7]
        chart		    314
        linkNode		    [314 0 0]
        subviewer		    314
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    317
        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		      316
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    314
        linkNode		    [314 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    314
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    318
        ssIdNumber		    4
        name		    "abb"
        linkNode		    [314 0 319]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    319
        ssIdNumber		    5
        name		    "logical"
        linkNode		    [314 318 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    320
        name		    "botharms_get_torque/MATLAB Function10"
        machine		    1
        chart		    314
      }
      chart {
        id			    321
        name		    "botharms_get_torque/haptic_controller"
        windowPosition	    [183.938 68.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 322 0 0]
        firstTransition	    324
        firstJunction	    323
        viewObj		    321
        machine		    1
        ssIdHighWaterMark	    28
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    325
        chartFileNumber	    35
        disableImplicitCasting  1
        eml {
          name		      "fcn"
        }
      }
      state {
        id			    322
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    321
        treeNode		    [321 0 0 0]
        superState		    SUBCHART
        subviewer		    321
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function [Q1,lambda, Q2,diagm] = fcn(B1,C1,J1,J1_dot, q1_dot, B2,C2,J2, J2_dot, q2_dot, e, scale)"
          "\n%#codegen\n\n\n% Reshape before paper method:\n\nJ1 = reshape(J1,7,6)';\nJ1(1:3,:) = J1(1:3,:)/1000; % Convert"
          " to m/s\nJ2 = reshape(J2,7,6)';\nJ2(1:3,:) = J2(1:3,:)/1000; % Convert to m/s\n\ne(1:3) = e(1:3)/1000;\n\nJ1_dot"
          " = reshape(J1_dot,7,6)';\nJ1_dot(1:3,:) = J1_dot(1:3,:)/1000; % Convert to m/s\nJ2_dot = reshape(J2_dot,7,6)';\n"
          "J2_dot(1:3,:) = J2_dot(1:3,:)/1000; % Convert to m/s\n\n    % Convert B1, B2 to abb matrices\nB1 = reshape(B1,7,"
          "7)';\nB1 = [B1(1:2,:); B1(4:7,:); B1(3,:)];\nB1 = [B1(:,1:2) B1(:,4:7) B1(:,3)];\n\nB2 = reshape(B2,7,7)';\nB2 ="
          " [B2(1:2,:); B2(4:7,:); B2(3,:)];\nB2 = [B2(:,1:2) B2(:,4:7) B2(:,3)];\n\n\n    % Convert C1, C2 to abb matrices"
          "\nC1 = reshape(C1,7,7)';\nC1 = [C1(1:2,:); C1(4:7,:); C1(3,:)];\nC1 = [C1(:,1:2) C1(:,4:7) C1(:,3)];\n\nC2 = res"
          "hape(C2,7,7)';\nC2 = [C2(1:2,:); C2(4:7,:); C2(3,:)];\nC2 = [C2(:,1:2) C2(:,4:7) C2(:,3)];\n\nq_dot = [q1_dot; q"
          "2_dot];\n\n% Control gains:\n\n% kp_dist = 225; % Can be increased later\nkp_dist = 250;\nkd_dist = 2*sqrt(kp_di"
          "st); % Nominal. Real negative double pole.\n% kp_quat = 160; % Can be increased later\nkp_quat = 250; % 200\nkd_"
          "quat = 2*sqrt(kp_quat); % Derivative gain\n\nkp = [kp_dist*ones(3,1); kp_quat*ones(3,1)];\nkd = [kd_dist*ones(3,"
          "1); kd_quat*ones(3,1)]; % Derivative gain\n\n\n% Scaling;\nJ2(1:3,:) = scale*J2(1:3,:);\nJ2_dot(1:3,:) = scale*J"
          "2_dot(1:3,:);\n\n% Implementation according to paper:\nG = [-J1 J2];\n\n\ny_dot = G*q_dot;\nG_dot = [-J1_dot J2_"
          "dot];\n\n\nM = blkdiag(B1,B2);\nM = M + 0.005*eye(14);\n\n\nC = blkdiag(C1,C2);\n\nGamma = G*pinv(M)*G';\n\n\n\n"
          "\nustar = pinv(Gamma)*(G*pinv(M)*(C*q_dot)-G_dot*q_dot);\nlambda = ustar - pinv(Gamma)*(kd.*y_dot + kp.*e);\n\n\n"
          "\nQ1 = -J1'*lambda;\n\n% Undo previous scaling, and scale acceleration.\nJ2(1:3,:) = J2(1:3,:)./scale^2;\nQ2 = J"
          "2'*lambda;\n\n\n\n\ndiagm = diag(M);\n% % %  Nice for debugging:\n% Q1(1:3)=0;\n% Q1(7) = 0;\n% Q2(1:3)=0;\n% Q2"
          "(7) = 0;"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    323
        position		    [23.5747 49.5747 7]
        chart		    321
        linkNode		    [321 0 0]
        subviewer		    321
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    324
        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		      323
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    321
        linkNode		    [321 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    321
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    325
        ssIdNumber		    12
        name		    "B1"
        linkNode		    [321 0 326]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    326
        ssIdNumber		    13
        name		    "C1"
        linkNode		    [321 325 327]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    327
        ssIdNumber		    7
        name		    "J1"
        linkNode		    [321 326 328]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    328
        ssIdNumber		    9
        name		    "J1_dot"
        linkNode		    [321 327 329]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    329
        ssIdNumber		    10
        name		    "q1_dot"
        linkNode		    [321 328 330]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    330
        ssIdNumber		    18
        name		    "B2"
        linkNode		    [321 329 331]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    331
        ssIdNumber		    19
        name		    "C2"
        linkNode		    [321 330 332]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    332
        ssIdNumber		    20
        name		    "J2"
        linkNode		    [321 331 333]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    333
        ssIdNumber		    21
        name		    "J2_dot"
        linkNode		    [321 332 334]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    334
        ssIdNumber		    22
        name		    "q2_dot"
        linkNode		    [321 333 335]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
    	isSigned		1
    	wordLength		"16"
          }
          complexity	      SF_COMPLEX_INHERITED
          frame		      SF_FRAME_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    335
        ssIdNumber		    4
        name		    "e"
        linkNode		    [321 334 336]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    336
        ssIdNumber		    5
        name		    "Q1"
        linkNode		    [321 335 337]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    337
        ssIdNumber		    24
        name		    "lambda"
        linkNode		    [321 336 338]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    338
        ssIdNumber		    8
        name		    "Q2"
        linkNode		    [321 337 339]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    339
        ssIdNumber		    27
        name		    "diagm"
        linkNode		    [321 338 340]
        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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    340
        ssIdNumber		    28
        name		    "scale"
        linkNode		    [321 339 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    341
        name		    "botharms_get_torque/haptic_controller"
        machine		    1
        chart		    321
      }
      chart {
        id			    342
        name		    "botharms_get_torque/MATLAB Function9"
        windowPosition	    [307.875 38.25 200.25 189.75]
        viewLimits		    [0 156.75 0 153.75]
        screen		    [1 1 3840 1200 1.25]
        treeNode		    [0 343 0 0]
        firstTransition	    345
        firstJunction	    344
        viewObj		    342
        machine		    1
        toolbarMode		    LIBRARY_TOOLBAR
        ssIdHighWaterMark	    5
        decomposition	    CLUSTER_CHART
        type		    EML_CHART
        firstData		    346
        chartFileNumber	    36
        disableImplicitCasting  1
        eml {
          name		      "abb2logical"
        }
      }
      state {
        id			    343
        labelString		    "eML_blk_kernel()"
        position		    [18 64.5 118 66]
        fontSize		    12
        chart		    342
        treeNode		    [342 0 0 0]
        superState		    SUBCHART
        subviewer		    342
        ssIdNumber		    1
        type		    FUNC_STATE
        decomposition	    CLUSTER_STATE
        eml {
          isEML		      1
          script		      "function logical = abb2logical(abb)\n%#codegen\n\nlogical = [abb(1:2); abb(7); abb(3:6)];"
          editorLayout	      "100 M4x1[10 5 700 500]"
        }
      }
      junction {
        id			    344
        position		    [23.5747 49.5747 7]
        chart		    342
        linkNode		    [342 0 0]
        subviewer		    342
        ssIdNumber		    3
        type		    CONNECTIVE_JUNCTION
      }
      transition {
        id			    345
        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		      344
          intersection	      [7 0 -1 -1 23.5747 42.5747 0 0]
        }
        midPoint		    [23.5747 24.9468]
        chart		    342
        linkNode		    [342 0 0]
        dataLimits		    [23.575 23.575 14.625 34.575]
        subviewer		    342
        drawStyle		    SMART
        executionOrder	    1
        ssIdNumber		    2
      }
      data {
        id			    346
        ssIdNumber		    4
        name		    "abb"
        linkNode		    [342 0 347]
        scope		    INPUT_DATA
        machine		    1
        props {
          array {
    	size			"-1"
          }
          type {
    	method			SF_INHERITED_TYPE
    	primitive		SF_DOUBLE_TYPE
          }
          complexity	      SF_COMPLEX_INHERITED
        }
        dataType		    "Inherit: Same as Simulink"
      }
      data {
        id			    347
        ssIdNumber		    5
        name		    "logical"
        linkNode		    [342 346 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
        }
        dataType		    "Inherit: Same as Simulink"
      }
      instance {
        id			    348
        name		    "botharms_get_torque/MATLAB Function9"
        machine		    1
        chart		    342
      }
      target {
        id			    349
        name		    "sfun"
        description		    "Default Simulink S-Function Target."
        machine		    1
        linkNode		    [1 0 350]
      }
      target {
        id			    350
        name		    "rtw"
        machine		    1
        linkNode		    [1 349 0]
      }
    }