From 4675f318512b782589db62c29e83cfc92cc3f1be Mon Sep 17 00:00:00 2001
From: Martin Karlsson <cont-mkr@ulund.org>
Date: Fri, 27 Sep 2019 13:54:13 +0200
Subject: [PATCH] scaled position and force independently
---
scaled_force_teleoperation.mdl | 21719 +++++++++++++++++++++++++++++++
1 file changed, 21719 insertions(+)
create mode 100644 scaled_force_teleoperation.mdl
diff --git a/scaled_force_teleoperation.mdl b/scaled_force_teleoperation.mdl
new file mode 100644
index 0000000..bf87203
--- /dev/null
+++ b/scaled_force_teleoperation.mdl
@@ -0,0 +1,21719 @@
+Model {
+ Name "scaled_force_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.2904"
+ NumModelReferences 0
+ NumTestPointedSignals 21
+ TestPointedSignal {
+ SignalName "torque1[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque"
+ }
+ TestPointedSignal {
+ SignalName "torque2[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque"
+ PortIndex 2
+ }
+ TestPointedSignal {
+ SignalName "offset[i]"
+ FullBlockPath "scaled_force_teleoperation/Left/leadthroug control/MATLAB Function"
+ PortIndex 6
+ }
+ TestPointedSignal {
+ SignalName "sat"
+ FullBlockPath "scaled_force_teleoperation/Left/leadthroug control/MATLAB Function"
+ PortIndex 7
+ }
+ TestPointedSignal {
+ SignalName "rstill[i]"
+ FullBlockPath "scaled_force_teleoperation/Right/leadthroug control/MATLAB Function"
+ PortIndex 6
+ }
+ TestPointedSignal {
+ SignalName "cright[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/Gain"
+ }
+ TestPointedSignal {
+ SignalName "cleft[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/Gain1"
+ }
+ TestPointedSignal {
+ SignalName "w1[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/Gain10"
+ }
+ TestPointedSignal {
+ SignalName "w2[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/Gain11"
+ }
+ TestPointedSignal {
+ SignalName "help_right[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/Gain2"
+ }
+ TestPointedSignal {
+ SignalName "help_left[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/Gain3"
+ }
+ TestPointedSignal {
+ SignalName "diagm[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/Gain4"
+ }
+ TestPointedSignal {
+ SignalName "e[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/MATLAB Function1"
+ }
+ TestPointedSignal {
+ SignalName "pose1[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/MATLAB Function3"
+ }
+ TestPointedSignal {
+ SignalName "vel1[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/MATLAB Function4"
+ }
+ TestPointedSignal {
+ SignalName "vel2[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/MATLAB Function4"
+ PortIndex 2
+ }
+ TestPointedSignal {
+ SignalName "q1_arm[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/haptic_controller"
+ }
+ TestPointedSignal {
+ SignalName "lambda[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/haptic_controller"
+ PortIndex 2
+ }
+ TestPointedSignal {
+ SignalName "q2_arm[i]"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/haptic_controller"
+ PortIndex 3
+ }
+ TestPointedSignal {
+ SignalName "freqDith"
+ FullBlockPath "scaled_force_teleoperation/botharms_get_torque/dither/Constant3"
+ }
+ TestPointedSignal {
+ SignalName "dithSignal[i]"
+ FullBlockPath "scaled_force_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 "Fri Sep 27 13:49:39 2019"
+ RTWModifiedTimeStamp 491492473
+ ModelVersionFormat "1.%<AutoIncrement:2904>"
+ 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,positionScale,forceScale"
+ TunableVarsStorageClass "Auto,Auto,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 "Optimization/Signals and Parameters"
+ ConfigPrmDlgPosition [ 427, 64, 1493, 1187 ]
+ }
+ 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_force_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 "1424"
+ 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_force_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_force_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_force_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_force_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_force_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_force_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_force_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, 46, 1920, 1154]
+ 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_force_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_force_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_force_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, -19, 1920, 1089]
+ 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 "499"
+ 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 Constant
+ Name "Constant3"
+ SID "1345:498"
+ Position [600, 1029, 715, 1051]
+ ShowName off
+ Value "positionScale"
+ VectorParams1D off
+ }
+ Block {
+ BlockType Constant
+ Name "Constant4"
+ SID "1345:499"
+ Position [600, 1064, 715, 1086]
+ ShowName off
+ Value "forceScale"
+ VectorParams1D off
+ }
+ 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_force_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_force_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_force_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_force_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 [3, 1]
+ Position [750, 818, 800, 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 "29"
+ 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 Inport
+ Name "scale"
+ SID "1345:350::28"
+ Position [20, 171, 40, 189]
+ ZOrder 14
+ Port "3"
+ 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_force_teleoperation 11"
+ Ports [3, 2]
+ Position [180, 104, 230, 206]
+ ZOrder 5
+ FunctionName "sf_sfun"
+ PortCounts "[3 2]"
+ EnableBusSupport on
+ Port {
+ PortNumber 2
+ Name "e"
+ 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"
+ }
+ Line {
+ SrcBlock "T44_FK1"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 1
+ }
+ Line {
+ SrcBlock "T44_FK2"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 2
+ }
+ Line {
+ SrcBlock "scale"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 3
+ }
+ Line {
+ Name "e"
+ Labels [0, 0]
+ SrcBlock " SFunction "
+ SrcPort 2
+ DstBlock "e"
+ 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_force_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_force_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_force_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_force_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_force_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_force_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_force_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_force_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_force_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_force_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_force_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 [13, 4]
+ Position [850, 23, 995, 937]
+ 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 "46"
+ 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 Inport
+ Name "force_scale"
+ SID "1345:416::46"
+ Position [20, 531, 40, 549]
+ ZOrder 26
+ Port "13"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Demux
+ Name " Demux "
+ SID "1345:416::20"
+ Ports [1, 1]
+ Position [270, 405, 320, 445]
+ ZOrder 6
+ Outputs "1"
+ }
+ Block {
+ BlockType S-Function
+ Name " SFunction "
+ SID "1345:416::19"
+ Tag "Stateflow S-Function scaled_force_teleoperation 35"
+ Ports [13, 5]
+ Position [180, 124, 230, 406]
+ ZOrder 5
+ FunctionName "sf_sfun"
+ PortCounts "[13 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, 416, 480, 434]
+ 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 {
+ SrcBlock "force_scale"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 13
+ }
+ 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_force_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_force_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_force_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_force_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 [205, 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, 65]
+ 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, 250]
+ 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, 10]
+ 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 [180, 0]
+ DstBlock "haptic_controller"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "S-Function2"
+ SrcPort 1
+ Points [160, 0; 0, -45]
+ DstBlock "haptic_controller"
+ DstPort 6
+ }
+ Line {
+ SrcBlock "S-Function1"
+ SrcPort 2
+ Points [180, 0]
+ DstBlock "haptic_controller"
+ DstPort 2
+ }
+ Line {
+ SrcBlock "FRIDA arm acceleration NOT SI"
+ SrcPort 3
+ Points [330, 0; 0, -5]
+ DstBlock "haptic_controller"
+ DstPort 4
+ }
+ Line {
+ SrcBlock "S-Function2"
+ SrcPort 2
+ DstBlock "haptic_controller"
+ DstPort 7
+ }
+ Line {
+ SrcBlock "FRIDA arm acceleration NOT SI1"
+ SrcPort 2
+ Points [150, 0; 0, 5; 170, 0]
+ Branch {
+ Points [55, 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, -60]
+ DstBlock "haptic_controller"
+ DstPort 9
+ }
+ Line {
+ Name "e[i]"
+ Labels [0, 0]
+ SrcBlock "MATLAB Function1"
+ SrcPort 1
+ Points [30, 0]
+ DstBlock "haptic_controller"
+ DstPort 11
+ }
+ Line {
+ SrcBlock "FRIDA arm acceleration NOT SI"
+ SrcPort 2
+ Points [310, 0]
+ Branch {
+ Points [80, 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 [210, 0; 0, 175]
+ 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, -75]
+ 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, -25]
+ 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, 5]
+ 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
+ }
+ Line {
+ SrcBlock "Constant3"
+ SrcPort 1
+ Points [20, 0; 0, -20]
+ Branch {
+ Points [0, -115]
+ DstBlock "MATLAB Function1"
+ DstPort 3
+ }
+ Branch {
+ Points [70, 0; 0, -190]
+ DstBlock "haptic_controller"
+ DstPort 12
+ }
+ }
+ Line {
+ SrcBlock "Constant4"
+ SrcPort 1
+ Points [115, 0]
+ DstBlock "haptic_controller"
+ DstPort 13
+ }
+ }
+ }
+ 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;\npositionScale=1;\nforceScale=1;\n\nfriction = 0.7;\ncontrol = 1;\nAm"
+ "pDithering=0.3;\nFreqDithering=15;\nadmit_CTRL=0;\n\n\n[B_lpfilt,A_lpfilt] = butter(1,0.01);\n\nparametersLeadth"
+ "rough;\n\nf_switch = 1;\na_switch = 0;\ndisp('Init done');"
+ Position [1762, 183]
+ 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;\npositionScale=1;\nforceScale=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\nparametersLe"
+ "adthrough;\n\nf_switch = 1;\na_switch = 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_force_teleoperation"
+ created "06-Feb-2013 13:26:16"
+ isLibrary 0
+ firstTarget 350
+ 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 13
+ 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 = fcn(T44_FK1, T44_FK2, scale)\n%#codegen\n\npersistent old_scale\nif isempty(old_s"
+ "cale)\n old_scale = scale;\nend\n\n\n\n% Reshaping:\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:\n"
+ "persistent delta_p\nif isempty(delta_p) || scale ~= old_scale\n delta_p = p2 - p1;\nend\nold_scale = scale;\n"
+ "\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% check if w is real. O"
+ "therwise, 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(si"
+ "ze(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_out;\n\ne = [ep ; q_o"
+ "ut];\n\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 INPUT_DATA
+ machine 1
+ 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 29
+ 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,"
+ " force_scale)\n%#codegen\n\n\n% Reshape before paper method:\n\nJ1 = reshape(J1,7,6)';\nJ1(1:3,:) = J1(1:3,:)/10"
+ "00; % 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)/10"
+ "00;\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)';\nJ2_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 = reshape(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; q2_dot];\n\n% Control gains:\n\n% kp_dist = 225; % Can be increased later\nkp_dist = 250;\nkd_dist ="
+ " 2*sqrt(kp_dist); % Nominal. Real negative double pole.\n% kp_quat = 160; % Can be increased later\nkp_quat = 25"
+ "0; % 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\n% Implementation according to paper:\n\nscaled_J2 "
+ "= [scale*J2(1:3,:);\n J2(4:6,:)];\nG = [-J1 scaled_J2];\n\n\ny_dot = G*q_dot;\nscaled_J2_dot = [scale"
+ "*J2_dot(1:3,:);\n J2_dot(4:6,:)];\nG_dot = [-J1_dot scaled_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\nforce_sca"
+ "led_J2 = [force_scale*J2(1:3,:);\n J2(4:6,:)];\nQ2 = force_scaled_J2'*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 341]
+ scope INPUT_DATA
+ machine 1
+ props {
+ 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 341
+ ssIdNumber 29
+ name "force_scale"
+ linkNode [321 340 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 342
+ name "botharms_get_torque/haptic_controller"
+ machine 1
+ chart 321
+ }
+ chart {
+ id 343
+ 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 344 0 0]
+ firstTransition 346
+ firstJunction 345
+ viewObj 343
+ machine 1
+ toolbarMode LIBRARY_TOOLBAR
+ ssIdHighWaterMark 5
+ decomposition CLUSTER_CHART
+ type EML_CHART
+ firstData 347
+ chartFileNumber 36
+ disableImplicitCasting 1
+ eml {
+ name "abb2logical"
+ }
+ }
+ state {
+ id 344
+ labelString "eML_blk_kernel()"
+ position [18 64.5 118 66]
+ fontSize 12
+ chart 343
+ treeNode [343 0 0 0]
+ superState SUBCHART
+ subviewer 343
+ 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 345
+ position [23.5747 49.5747 7]
+ chart 343
+ linkNode [343 0 0]
+ subviewer 343
+ ssIdNumber 3
+ type CONNECTIVE_JUNCTION
+ }
+ transition {
+ id 346
+ 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 345
+ intersection [7 0 -1 -1 23.5747 42.5747 0 0]
+ }
+ midPoint [23.5747 24.9468]
+ chart 343
+ linkNode [343 0 0]
+ dataLimits [23.575 23.575 14.625 34.575]
+ subviewer 343
+ drawStyle SMART
+ executionOrder 1
+ ssIdNumber 2
+ }
+ data {
+ id 347
+ ssIdNumber 4
+ name "abb"
+ linkNode [343 0 348]
+ 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 348
+ ssIdNumber 5
+ name "logical"
+ linkNode [343 347 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 349
+ name "botharms_get_torque/MATLAB Function9"
+ machine 1
+ chart 343
+ }
+ target {
+ id 350
+ name "sfun"
+ description "Default Simulink S-Function Target."
+ machine 1
+ linkNode [1 0 351]
+ }
+ target {
+ id 351
+ name "rtw"
+ machine 1
+ linkNode [1 350 0]
+ }
+}
--
GitLab