Select Git revision
scaled_teleoperation.mdl
Martin Karlsson authored
scaled_teleoperation.mdl 615.21 KiB
Model {
Name "scaled_teleoperation"
Version 7.9
MdlSubVersion 0
GraphicalInterface {
NumRootInports 57
Inport {
BusObject ""
Name "irb2ext.obtaining"
}
Inport {
BusObject ""
Name "irb2ext.manualMode"
}
Inport {
BusObject ""
Name "irb2ext.controlActive"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].parKp"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].parKv"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].parKi"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].parTrqMin"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].parTrqMax"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].posRawFb"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].posFlt"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].velRaw"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].velFlt"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].velOut"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].trqRaw"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].trqRefFlt"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].posRef"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].velRef"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].trqFfw"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].trqFfwGrav"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].mocgendata.instruction"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].mocgendata.value1"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].mocgendata.value2"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].mocgendata.value3"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].mocgendata.value4"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].mocgendata.value5"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].mocgendata.value6"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].mocgendata.string1[i]"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].mocgendata.string2[i]"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].parKp"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].parKv"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].parKi"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].parTrqMin"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].parTrqMax"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].posRawFb"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].posFlt"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].velFlt"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].velOut"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].trqRaw"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].trqRefFlt"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].posRef"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].velRef"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].trqFfw"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].trqFfwGrav"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].mocgendata.instruction"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].mocgendata.value1"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].mocgendata.value2"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].mocgendata.value3"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].mocgendata.value4"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].mocgendata.value5"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].mocgendata.value6"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].mocgendata.string1[i]"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].mocgendata.string2[i]"
}
Inport {
BusObject ""
Name "irb2ext.robot[1].joint[i].posRawAbs"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].posRawAbs"
}
Inport {
BusObject ""
Name "irb2ext.robot[0].joint[i].velRaw"
}
Inport {
BusObject ""
Name "ati2rob.forcesTorques[i]"
}
Inport {
BusObject ""
Name "ati2rob_2.forcesTorques[i]"
}
NumRootOutports 30
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].joint[i].parKp"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].joint[i].parKv"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].joint[i].parKi"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].joint[i].posRef"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].joint[i].velRef"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].joint[i].trqFfw"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].mocgendata.instruction"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].mocgendata.value1"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].mocgendata.value2"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].mocgendata.value3"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].mocgendata.value4"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].mocgendata.value5"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].mocgendata.value6"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].mocgendata.string1[i]"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[1].mocgendata.string2[i]"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].joint[i].parKp"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].joint[i].parKv"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].joint[i].parKi"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].joint[i].posRef"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].joint[i].velRef"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].joint[i].trqFfw"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].mocgendata.instruction"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].mocgendata.value1"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].mocgendata.value2"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].mocgendata.value3"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].mocgendata.value4"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].mocgendata.value5"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].mocgendata.value6"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].mocgendata.string1[i]"
}
Outport {
BusObject ""
BusOutputAsStruct "off"
Name "ext2irb.robot[0].mocgendata.string2[i]"
}
ParameterArgumentNames ""
ComputedModelVersion "1.2866"
NumModelReferences 0
NumTestPointedSignals 21
TestPointedSignal {
SignalName "torque1[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque"
}
TestPointedSignal {
SignalName "torque2[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque"
PortIndex 2
}
TestPointedSignal {
SignalName "offset[i]"
FullBlockPath "scaled_teleoperation/Left/leadthroug control/MATLAB Function"
PortIndex 6
}
TestPointedSignal {
SignalName "sat"
FullBlockPath "scaled_teleoperation/Left/leadthroug control/MATLAB Function"
PortIndex 7
}
TestPointedSignal {
SignalName "rstill[i]"
FullBlockPath "scaled_teleoperation/Right/leadthroug control/MATLAB Function"
PortIndex 6
}
TestPointedSignal {
SignalName "cright[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/Gain"
}
TestPointedSignal {
SignalName "cleft[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/Gain1"
}
TestPointedSignal {
SignalName "w1[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/Gain10"
}
TestPointedSignal {
SignalName "w2[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/Gain11"
}
TestPointedSignal {
SignalName "help_right[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/Gain2"
}
TestPointedSignal {
SignalName "help_left[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/Gain3"
}
TestPointedSignal {
SignalName "diagm[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/Gain4"
}
TestPointedSignal {
SignalName "e[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/MATLAB Function1"
}
TestPointedSignal {
SignalName "pose1[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/MATLAB Function3"
}
TestPointedSignal {
SignalName "vel1[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/MATLAB Function4"
}
TestPointedSignal {
SignalName "vel2[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/MATLAB Function4"
PortIndex 2
}
TestPointedSignal {
SignalName "q1_arm[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/haptic_controller"
}
TestPointedSignal {
SignalName "lambda[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/haptic_controller"
PortIndex 2
}
TestPointedSignal {
SignalName "q2_arm[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/haptic_controller"
PortIndex 3
}
TestPointedSignal {
SignalName "freqDith"
FullBlockPath "scaled_teleoperation/botharms_get_torque/dither/Constant3"
}
TestPointedSignal {
SignalName "dithSignal[i]"
FullBlockPath "scaled_teleoperation/botharms_get_torque/dither/DITHERING"
}
}
SavedCharacterEncoding "UTF-8"
SaveDefaultBlockParams on
ScopeRefreshTime 0.035000
OverrideScopeRefreshTime on
DisableAllScopes off
DataTypeOverride "UseLocalSettings"
DataTypeOverrideAppliesTo "AllNumericTypes"
MinMaxOverflowLogging "UseLocalSettings"
MinMaxOverflowArchiveMode "Overwrite"
FPTRunName "Run 1"
MaxMDLFileLineLength 120
Created "Tue Apr 02 13:20:51 2002"
Creator "jjwang"
UpdateHistory "UpdateHistoryNever"
ModifiedByFormat "%<Auto>"
LastModifiedBy "martinka"
ModifiedDateFormat "%<Auto>"
LastModifiedDate "Tue Sep 17 20:31:39 2019"
RTWModifiedTimeStamp 490651023
ModelVersionFormat "1.%<AutoIncrement:2866>"
ConfigurationManager "none"
SampleTimeColors off
SampleTimeAnnotations off
LibraryLinkDisplay "none"
WideLines off
ShowLineDimensions on
ShowPortDataTypes off
ShowDesignRanges off
ShowLoopsOnError on
IgnoreBidirectionalLines off
ShowStorageClass off
ShowTestPointIcons on
ShowSignalResolutionIcons on
ShowViewerIcons on
SortedOrder off
ExecutionContextIcon off
ShowLinearizationAnnotations on
BlockNameDataTip off
BlockParametersDataTip off
BlockDescriptionStringDataTip off
ToolBar on
StatusBar on
BrowserShowLibraryLinks off
BrowserLookUnderMasks off
SimulationMode "external"
LinearizationMsg "none"
Profile off
ParamWorkspaceSource "MATLABWorkspace"
AccelSystemTargetFile "accel.tlc"
AccelTemplateMakefile "accel_default_tmf"
AccelMakeCommand "make_rtw"
TryForcingSFcnDF off
RecordCoverage off
CovPath "/"
CovSaveName "covdata"
CovMetricSettings "dw"
CovNameIncrementing off
CovHtmlReporting on
CovForceBlockReductionOff on
covSaveCumulativeToWorkspaceVar on
CovSaveSingleToWorkspaceVar on
CovCumulativeVarName "covCumulativeData"
CovCumulativeReport off
CovReportOnPause on
CovModelRefEnable "Off"
CovExternalEMLEnable off
ExtModeBatchMode off
ExtModeEnableFloating on
ExtModeTrigType "manual"
ExtModeTrigMode "normal"
ExtModeTrigPort "1"
ExtModeTrigElement "any"
ExtModeTrigDuration 1000
ExtModeTrigDurationFloating "auto"
ExtModeTrigHoldOff 0
ExtModeTrigDelay 0
ExtModeTrigDirection "rising"
ExtModeTrigLevel 0
ExtModeArchiveMode "off"
ExtModeAutoIncOneShot off
ExtModeIncDirWhenArm off
ExtModeAddSuffixToVar off
ExtModeWriteAllDataToWs off
ExtModeArmWhenConnect on
ExtModeSkipDownloadWhenConnect off
ExtModeLogAll on
ExtModeAutoUpdateStatusClock off
TunableVars "a_switch,f_switch,highKiActive,keepStill,leftActive,rightActive"
TunableVarsStorageClass "Auto,Auto,Auto,Auto,Auto,Auto"
TunableVarsTypeQualifier ",,,,,"
BufferReuse on
ShowModelReferenceBlockVersion off
ShowModelReferenceBlockIO off
Array {
Type "Handle"
Dimension 1
Simulink.ConfigSet {
$ObjectID 1
Version "1.12.0"
Array {
Type "Handle"
Dimension 8
Simulink.SolverCC {
$ObjectID 2
Version "1.12.0"
StartTime "0.0"
StopTime "10.0"
AbsTol "auto"
FixedStep "auto"
InitialStep "auto"
MaxNumMinSteps "-1"
MaxOrder 5
ZcThreshold "auto"
ConsecutiveZCsStepRelTol "10*128*eps"
MaxConsecutiveZCs "1000"
ExtrapolationOrder 4
NumberNewtonIterations 1
MaxStep "auto"
MinStep "auto"
MaxConsecutiveMinStep "1"
RelTol "1e-3"
SolverMode "Auto"
EnableConcurrentExecution off
ConcurrentTasks off
Solver "FixedStepDiscrete"
SolverName "FixedStepDiscrete"
SolverJacobianMethodControl "auto"
ShapePreserveControl "DisableAll"
ZeroCrossControl "UseLocalSettings"
ZeroCrossAlgorithm "Nonadaptive"
AlgebraicLoopSolver "TrustRegion"
SolverResetMethod "Fast"
PositivePriorityOrder off
AutoInsertRateTranBlk off
SampleTimeConstraint "Unconstrained"
InsertRTBMode "Whenever possible"
}
Simulink.DataIOCC {
$ObjectID 3
Version "1.12.0"
Decimation "1"
ExternalInput "[t, u]"
FinalStateName "xFinal"
InitialState "xInitial"
LimitDataPoints on
MaxDataPoints "1000"
LoadExternalInput off
LoadInitialState off
SaveFinalState off
SaveCompleteFinalSimState off
SaveFormat "Array"
SignalLoggingSaveFormat "ModelDataLogs"
SaveOutput off
SaveState off
SignalLogging on
DSMLogging on
InspectSignalLogs off
SaveTime off
ReturnWorkspaceOutputs off
StateSaveName "xout"
TimeSaveName "tout"
OutputSaveName "yout"
SignalLoggingName "sigsOut"
DSMLoggingName "dsmout"
OutputOption "RefineOutputTimes"
OutputTimes "[]"
ReturnWorkspaceOutputsName "out"
Refine "1"
}
Simulink.OptimizationCC {
$ObjectID 4
Version "1.12.0"
Array {
Type "Cell"
Dimension 8
Cell "BooleansAsBitfields"
Cell "PassReuseOutputArgsAs"
Cell "PassReuseOutputArgsThreshold"
Cell "ZeroExternalMemoryAtStartup"
Cell "ZeroInternalMemoryAtStartup"
Cell "OptimizeModelRefInitCode"
Cell "NoFixptDivByZeroProtection"
Cell "UseSpecifiedMinMax"
PropName "DisabledProps"
}
BlockReduction on
BooleanDataType off
ConditionallyExecuteInputs on
InlineParams on
UseIntDivNetSlope off
UseFloatMulNetSlope off
UseSpecifiedMinMax off
InlineInvariantSignals off
OptimizeBlockIOStorage on
BufferReuse on
EnhancedBackFolding off
StrengthReduction off
ExpressionFolding on
BooleansAsBitfields off
BitfieldContainerType "uint_T"
EnableMemcpy on
MemcpyThreshold 64
PassReuseOutputArgsAs "Structure reference"
ExpressionDepthLimit 2147483647
FoldNonRolledExpr on
LocalBlockOutputs on
RollThreshold 8
SystemCodeInlineAuto off
StateBitsets off
DataBitsets off
UseTempVars off
ZeroExternalMemoryAtStartup on
ZeroInternalMemoryAtStartup on
InitFltsAndDblsToZero on
NoFixptDivByZeroProtection off
EfficientFloat2IntCast off
EfficientMapNaN2IntZero on
OptimizeModelRefInitCode off
LifeSpan "inf"
MaxStackSize "Inherit from target"
BufferReusableBoundary on
SimCompilerOptimization "Off"
AccelVerboseBuild off
ParallelExecutionInRapidAccelerator on
}
Simulink.DebuggingCC {
$ObjectID 5
Version "1.12.0"
RTPrefix "error"
ConsistencyChecking "none"
ArrayBoundsChecking "none"
SignalInfNanChecking "none"
SignalRangeChecking "none"
ReadBeforeWriteMsg "UseLocalSettings"
WriteAfterWriteMsg "UseLocalSettings"
WriteAfterReadMsg "UseLocalSettings"
AlgebraicLoopMsg "warning"
ArtificialAlgebraicLoopMsg "warning"
SaveWithDisabledLinksMsg "warning"
SaveWithParameterizedLinksMsg "none"
CheckSSInitialOutputMsg on
UnderspecifiedInitializationDetection "Classic"
MergeDetectMultiDrivingBlocksExec "none"
CheckExecutionContextPreStartOutputMsg off
CheckExecutionContextRuntimeOutputMsg off
SignalResolutionControl "TryResolveAllWithWarning"
BlockPriorityViolationMsg "warning"
MinStepSizeMsg "warning"
TimeAdjustmentMsg "none"
MaxConsecutiveZCsMsg "error"
MaskedZcDiagnostic "warning"
IgnoredZcDiagnostic "warning"
SolverPrmCheckMsg "none"
InheritedTsInSrcMsg "warning"
DiscreteInheritContinuousMsg "warning"
MultiTaskDSMMsg "warning"
MultiTaskCondExecSysMsg "none"
MultiTaskRateTransMsg "error"
SingleTaskRateTransMsg "none"
TasksWithSamePriorityMsg "warning"
SigSpecEnsureSampleTimeMsg "warning"
CheckMatrixSingularityMsg "none"
IntegerOverflowMsg "warning"
Int32ToFloatConvMsg "warning"
ParameterDowncastMsg "error"
ParameterOverflowMsg "error"
ParameterUnderflowMsg "none"
ParameterPrecisionLossMsg "warning"
ParameterTunabilityLossMsg "warning"
FixptConstUnderflowMsg "none"
FixptConstOverflowMsg "none"
FixptConstPrecisionLossMsg "none"
UnderSpecifiedDataTypeMsg "none"
UnnecessaryDatatypeConvMsg "none"
VectorMatrixConversionMsg "none"
InvalidFcnCallConnMsg "error"
FcnCallInpInsideContextMsg "Use local settings"
SignalLabelMismatchMsg "none"
UnconnectedInputMsg "warning"
UnconnectedOutputMsg "warning"
UnconnectedLineMsg "warning"
SFcnCompatibilityMsg "none"
FrameProcessingCompatibilityMsg "warning"
UniqueDataStoreMsg "none"
BusObjectLabelMismatch "warning"
RootOutportRequireBusObject "warning"
AssertControl "UseLocalSettings"
EnableOverflowDetection off
ModelReferenceIOMsg "none"
ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error"
ModelReferenceVersionMismatchMessage "none"
ModelReferenceIOMismatchMessage "none"
ModelReferenceCSMismatchMessage "none"
UnknownTsInhSupMsg "warning"
ModelReferenceDataLoggingMessage "warning"
ModelReferenceSymbolNameMessage "warning"
ModelReferenceExtraNoncontSigs "error"
StateNameClashWarn "warning"
SimStateInterfaceChecksumMismatchMsg "warning"
SimStateOlderReleaseMsg "error"
InitInArrayFormatMsg "warning"
StrictBusMsg "None"
BusNameAdapt "WarnAndRepair"
NonBusSignalsTreatedAsBus "none"
LoggingUnavailableSignals "error"
BlockIODiagnostic "none"
SFUnusedDataAndEventsDiag "warning"
SFUnexpectedBacktrackingDiag "warning"
SFInvalidInputDataAccessInChartInitDiag "warning"
SFNoUnconditionalDefaultTransitionDiag "warning"
SFTransitionOutsideNaturalParentDiag "warning"
SFUnconditionalTransitionShadowingDiag "warning"
}
Simulink.HardwareCC {
$ObjectID 6
Version "1.12.0"
ProdBitPerChar 8
ProdBitPerShort 16
ProdBitPerInt 32
ProdBitPerLong 32
ProdBitPerFloat 32
ProdBitPerDouble 64
ProdBitPerPointer 32
ProdLargestAtomicInteger "Char"
ProdLargestAtomicFloat "None"
ProdIntDivRoundTo "Undefined"
ProdEndianess "Unspecified"
ProdWordSize 32
ProdShiftRightIntArith on
ProdHWDeviceType "Specified"
TargetBitPerChar 8
TargetBitPerShort 16
TargetBitPerInt 32
TargetBitPerLong 32
TargetBitPerFloat 32
TargetBitPerDouble 64
TargetBitPerPointer 32
TargetLargestAtomicInteger "Char"
TargetLargestAtomicFloat "None"
TargetShiftRightIntArith on
TargetIntDivRoundTo "Undefined"
TargetEndianess "Unspecified"
TargetWordSize 32
TargetTypeEmulationWarnSuppressLevel 0
TargetPreprocMaxBitsSint 32
TargetPreprocMaxBitsUint 32
TargetHWDeviceType "32-bit Generic"
TargetUnknown off
ProdEqTarget off
}
Simulink.ModelReferenceCC {
$ObjectID 7
Version "1.12.0"
UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange"
CheckModelReferenceTargetMessage "error"
EnableParallelModelReferenceBuilds off
ParallelModelReferenceErrorOnInvalidPool on
ParallelModelReferenceMATLABWorkerInit "None"
ModelReferenceNumInstancesAllowed "Multi"
PropagateVarSize "Infer from blocks in model"
ModelReferencePassRootInputsByReference on
ModelReferenceMinAlgLoopOccurrences off
PropagateSignalLabelsOutOfModel off
SupportModelReferenceSimTargetCustomCode off
}
Simulink.SFSimCC {
$ObjectID 8
Version "1.12.0"
SFSimEnableDebug on
SFSimOverflowDetection on
SFSimEcho on
SimBlas on
SimCtrlC on
SimExtrinsic on
SimIntegrity on
SimUseLocalCustomCode off
SimParseCustomCode off
SimBuildMode "sf_incremental_build"
}
Simulink.RTWCC {
$BackupClass "Simulink.RTWCC"
$ObjectID 9
Version "1.12.0"
Array {
Type "Cell"
Dimension 9
Cell "IncludeHyperlinkInReport"
Cell "GenerateTraceInfo"
Cell "GenerateTraceReport"
Cell "GenerateTraceReportSl"
Cell "GenerateTraceReportSf"
Cell "GenerateTraceReportEml"
Cell "GenerateWebview"
Cell "GenerateCodeMetricsReport"
Cell "GenerateCodeReplacementReport"
PropName "DisabledProps"
}
SystemTargetFile "rtw2orca.tlc"
GenCodeOnly off
MakeCommand "make_rtw"
GenerateMakefile on
TemplateMakefile "rtw2orca.tmf"
Description "RTW2ORCA Real-Time Target"
GenerateReport off
SaveLog off
RTWVerbose on
RetainRTWFile on
ProfileTLC off
TLCDebug off
TLCCoverage off
TLCAssert off
ProcessScriptMode "Default"
ConfigurationMode "Optimized"
ProcessScript "rtw2orca_make_rtw_hook"
ConfigAtBuild off
RTWUseLocalCustomCode off
RTWUseSimCustomCode off
IncludeHyperlinkInReport off
LaunchReport off
TargetLang "C"
IncludeBusHierarchyInRTWFileBlockHierarchyMap off
IncludeERTFirstTime off
GenerateTraceInfo off
GenerateTraceReport off
GenerateTraceReportSl off
GenerateTraceReportSf off
GenerateTraceReportEml off
GenerateCodeInfo off
GenerateWebview off
GenerateCodeMetricsReport off
GenerateCodeReplacementReport off
RTWCompilerOptimization "Off"
CheckMdlBeforeBuild "Off"
CustomRebuildMode "OnUpdate"
Array {
Type "Handle"
Dimension 2
Simulink.CodeAppCC {
$ObjectID 10
Version "1.12.0"
Array {
Type "Cell"
Dimension 21
Cell "IgnoreCustomStorageClasses"
Cell "IgnoreTestpoints"
Cell "InsertBlockDesc"
Cell "InsertPolySpaceComments"
Cell "SFDataObjDesc"
Cell "MATLABFcnDesc"
Cell "SimulinkDataObjDesc"
Cell "DefineNamingRule"
Cell "SignalNamingRule"
Cell "ParamNamingRule"
Cell "InlinedPrmAccess"
Cell "CustomSymbolStr"
Cell "CustomSymbolStrGlobalVar"
Cell "CustomSymbolStrType"
Cell "CustomSymbolStrField"
Cell "CustomSymbolStrFcn"
Cell "CustomSymbolStrFcnArg"
Cell "CustomSymbolStrBlkIO"
Cell "CustomSymbolStrTmpVar"
Cell "CustomSymbolStrMacro"
Cell "ReqsInCode"
PropName "DisabledProps"
}
ForceParamTrailComments on
GenerateComments on
IgnoreCustomStorageClasses on
IgnoreTestpoints off
IncHierarchyInIds off
MaxIdLength 128
PreserveName off
PreserveNameWithParent off
ShowEliminatedStatement on
IncAutoGenComments off
SimulinkDataObjDesc off
SFDataObjDesc off
MATLABFcnDesc off
IncDataTypeInIds off
MangleLength 1
CustomSymbolStrGlobalVar "$R$N$M"
CustomSymbolStrType "$N$R$M"
CustomSymbolStrField "$N$M"
CustomSymbolStrFcn "$R$N$M$F"
CustomSymbolStrFcnArg "rt$I$N$M"
CustomSymbolStrBlkIO "rtb_$N$M"
CustomSymbolStrTmpVar "$N$M"
CustomSymbolStrMacro "$R$N$M"
DefineNamingRule "None"
ParamNamingRule "None"
SignalNamingRule "None"
InsertBlockDesc off
InsertPolySpaceComments off
SimulinkBlockComments on
MATLABSourceComments off
EnableCustomComments off
InlinedPrmAccess "Literals"
ReqsInCode off
UseSimReservedNames off
}
Simulink.STFCustomTargetCC {
$BackupClass "Simulink.TargetCC"
$ObjectID 11
Version "1.12.0"
Array {
Type "Cell"
Dimension 16
Cell "GeneratePreprocessorConditionals"
Cell "IncludeMdlTerminateFcn"
Cell "CombineOutputUpdateFcns"
Cell "SuppressErrorStatus"
Cell "ERTCustomFileBanners"
Cell "GenerateSampleERTMain"
Cell "GenerateTestInterfaces"
Cell "ModelStepFunctionPrototypeControlCompliant"
Cell "CPPClassGenCompliant"
Cell "MultiInstanceERTCode"
Cell "PurelyIntegerCode"
Cell "SupportComplex"
Cell "SupportAbsoluteTime"
Cell "SupportContinuousTime"
Cell "SupportNonInlinedSFcns"
Cell "PortableWordSizes"
PropName "DisabledProps"
}
TargetFcnLib "ansi_tfl_tmw.mat"
TargetLibSuffix ""
TargetPreCompLibLocation ""
CodeReplacementLibrary "ANSI_C"
UtilityFuncGeneration "Auto"
ERTMultiwordTypeDef "System defined"
CodeExecutionProfiling off
ERTMultiwordLength 256
MultiwordLength 2048
GenerateFullHeader on
GenerateSampleERTMain off
GenerateTestInterfaces off
IsPILTarget off
ModelReferenceCompliant off
ParMdlRefBuildCompliant off
CompOptLevelCompliant off
ConcurrentExecutionCompliant off
IncludeMdlTerminateFcn on
GeneratePreprocessorConditionals "Disable all"
CombineOutputUpdateFcns off
CombineSignalStateStructs off
SuppressErrorStatus off
ERTFirstTimeCompliant off
IncludeFileDelimiter "Auto"
ERTCustomFileBanners off
SupportAbsoluteTime on
LogVarNameModifier "rt_"
MatFileLogging on
MultiInstanceERTCode off
SupportNonFinite on
SupportComplex on
PurelyIntegerCode off
SupportContinuousTime on
SupportNonInlinedSFcns on
SupportVariableSizeSignals off
EnableShiftOperators on
ParenthesesLevel "Nominal"
PortableWordSizes off
ModelStepFunctionPrototypeControlCompliant off
CPPClassGenCompliant off
AutosarCompliant off
GRTInterface on
SystemTargetFile "rtw2orca.tlc"
DialogCategory 0
CustomProperty {
DataType "string"
Name "RTW2ORCA_LOCAL"
Value "ext2irb irb2ext"
}
CustomProperty {
DataType "string"
Name "RTW2ORCA_LABCOMM"
Value "extctrl_irc5_main_20110428 ati_netbox_lc ati_netbox2_lc"
}
CustomProperty {
DataType "string"
Name "RTW2ORCA_EXTERNAL_SRCS"
Value "rtw2orca_irc5_main_20110428_xeno labcomm_lth_net_xeno"
}
CustomProperty {
DataType "string"
Name "RTW2ORCA_VPATH"
Value ".. /opt/robot/labcomm /opt/robot/orca /opt/robot/extctrl /home/martinka/frida_dynamics"
}
CustomProperty {
DataType "string"
Name "RTW2ORCA_LIB"
Value "-lrtdm -lnative -lxenomai -lrightfridadyn -lleftfridadyn"
}
CustomProperty {
DataType "string"
Name "RTW2ORCA_INCLUDE"
Value "/opt/robot/include/lth/lth_net /opt/robot/extctrl /usr/xenomai/include /home/martinka/frida_dynamics /home/"
"martinka/frida_dynamics/cdyn_defs"
}
CustomProperty {
DataType "string"
Name "RTW2ORCA_LIBRARY"
Value "/home/martinka/frida_dynamics"
}
CustomProperty {
DataType "string"
Name "RTW2ORCA_CFLAGS"
Value " -DROBOT_COUNT=2 -DROBOT_0_JOINT_COUNT=7 -DROBOT_1_JOINT_COUNT=7 -Wno-error -DEXPOSE_DYN_EXTRA -fPIC -O4"
}
CustomProperty {
DataType "string"
Name "RTW2ORCA_POSTCOMMAND"
Value "mkdir -p /tmp/\\$(USER)/rtw2orca ; cp \\$(MODEL) /tmp/\\$(USER)/rtw2orca"
}
}
PropName "Components"
}
}
PropName "Components"
}
Name "Configuration"
ExtraOptions "-aGenerateTraceInfo=0 -aIgnoreTestpoints=0 "
CurrentDlgPage "Code Generation/RTW2ORCA"
ConfigPrmDlgPosition [ 427, 27, 1493, 1150 ]
}
PropName "ConfigurationSets"
}
Simulink.ConfigSet {
$PropName "ActiveConfigurationSet"
$ObjectID 1
}
ExplicitPartitioning off
BlockDefaults {
ForegroundColor "black"
BackgroundColor "white"
DropShadow off
NamePlacement "normal"
FontName "Helvetica"
FontSize 10
FontWeight "normal"
FontAngle "normal"
ShowName on
BlockRotation 0
BlockMirror off
}
AnnotationDefaults {
HorizontalAlignment "center"
VerticalAlignment "middle"
ForegroundColor "black"
BackgroundColor "white"
DropShadow off
FontName "Helvetica"
FontSize 10
FontWeight "normal"
FontAngle "normal"
UseDisplayTextAsClickCallback off
}
LineDefaults {
FontName "Helvetica"
FontSize 9
FontWeight "normal"
FontAngle "normal"
}
BlockParameterDefaults {
Block {
BlockType Constant
Value "1"
VectorParams1D on
SamplingMode "Sample based"
OutMin "[]"
OutMax "[]"
OutDataTypeStr "Inherit: Inherit from 'Constant value'"
LockScale off
SampleTime "inf"
FramePeriod "inf"
PreserveConstantTs off
}
Block {
BlockType Demux
Outputs "4"
DisplayOption "none"
BusSelectionMode off
}
Block {
BlockType DiscreteFilter
NumeratorSource "Dialog"
Numerator "[1]"
DenominatorSource "Dialog"
Denominator "[1 0.5]"
InitialStatesSource "Dialog"
InitialStates "0"
InputProcessing "Elements as channels (sample based)"
ExternalReset "None"
SampleTime "-1"
a0EqualsOne off
NumCoefMin "[]"
NumCoefMax "[]"
DenCoefMin "[]"
DenCoefMax "[]"
OutMin "[]"
OutMax "[]"
StateDataTypeStr "Inherit: Same as input"
NumCoefDataTypeStr "Inherit: Inherit via internal rule"
DenCoefDataTypeStr "Inherit: Inherit via internal rule"
NumProductDataTypeStr "Inherit: Inherit via internal rule"
DenProductDataTypeStr "Inherit: Inherit via internal rule"
NumAccumDataTypeStr "Inherit: Inherit via internal rule"
DenAccumDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
LockScale off
RndMeth "Floor"
SaturateOnIntegerOverflow off
StateMustResolveToSignalObject off
RTWStateStorageClass "Auto"
}
Block {
BlockType From
IconDisplay "Tag"
TagVisibility "local"
}
Block {
BlockType Gain
Gain "1"
Multiplication "Element-wise(K.*u)"
ParamMin "[]"
ParamMax "[]"
ParamDataTypeStr "Inherit: Same as input"
OutMin "[]"
OutMax "[]"
OutDataTypeStr "Inherit: Same as input"
LockScale off
RndMeth "Floor"
SaturateOnIntegerOverflow on
SampleTime "-1"
}
Block {
BlockType Goto
IconDisplay "Tag"
TagVisibility "local"
}
Block {
BlockType Inport
Port "1"
OutputFunctionCall off
OutMin "[]"
OutMax "[]"
OutDataTypeStr "Inherit: auto"
LockScale off
BusOutputAsStruct off
PortDimensions "-1"
VarSizeSig "Inherit"
SampleTime "-1"
SignalType "auto"
SamplingMode "auto"
LatchByDelayingOutsideSignal off
LatchInputForFeedbackSignals off
Interpolate on
}
Block {
BlockType Mux
Inputs "4"
DisplayOption "none"
UseBusObject off
BusObject "BusObject"
NonVirtualBus off
}
Block {
BlockType Outport
Port "1"
OutMin "[]"
OutMax "[]"
OutDataTypeStr "Inherit: auto"
LockScale off
BusOutputAsStruct off
PortDimensions "-1"
VarSizeSig "Inherit"
SampleTime "-1"
SignalType "auto"
SamplingMode "auto"
SourceOfInitialOutputValue "Dialog"
OutputWhenDisabled "held"
InitialOutput "[]"
}
Block {
BlockType Product
Inputs "2"
Multiplication "Element-wise(.*)"
CollapseMode "All dimensions"
CollapseDim "1"
InputSameDT on
OutMin "[]"
OutMax "[]"
OutDataTypeStr "Inherit: Same as first input"
LockScale off
RndMeth "Zero"
SaturateOnIntegerOverflow on
SampleTime "-1"
}
Block {
BlockType S-Function
FunctionName "system"
SFunctionModules "''"
PortCounts "[]"
SFunctionDeploymentMode off
}
Block {
BlockType SubSystem
ShowPortLabels "FromPortIcon"
Permissions "ReadWrite"
PermitHierarchicalResolution "All"
TreatAsAtomicUnit off
CheckFcnCallInpInsideContextMsg off
SystemSampleTime "-1"
RTWFcnNameOpts "Auto"
RTWFileNameOpts "Auto"
RTWMemSecFuncInitTerm "Inherit from model"
RTWMemSecFuncExecute "Inherit from model"
RTWMemSecDataConstants "Inherit from model"
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
SimViewingDevice off
DataTypeOverride "UseLocalSettings"
DataTypeOverrideAppliesTo "AllNumericTypes"
MinMaxOverflowLogging "UseLocalSettings"
SFBlockType "NONE"
Variant off
GeneratePreprocessorConditionals off
}
Block {
BlockType Sum
IconShape "rectangular"
Inputs "++"
CollapseMode "All dimensions"
CollapseDim "1"
InputSameDT on
AccumDataTypeStr "Inherit: Inherit via internal rule"
OutMin "[]"
OutMax "[]"
OutDataTypeStr "Inherit: Same as first input"
LockScale off
RndMeth "Floor"
SaturateOnIntegerOverflow on
SampleTime "-1"
}
Block {
BlockType Terminator
}
}
System {
Name "scaled_teleoperation"
Location [0, 92, 1920, 1200]
Open on
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "A4"
PaperUnits "inches"
TiledPaperMargins [0.196850, 0.196850, 0.196850, 0.196850]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
ReportName "simulink-default.rpt"
SIDHighWatermark "1421"
Block {
BlockType Inport
Name "irb2ext.obtaining"
SID "51"
Position [95, 58, 125, 72]
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.manualMode"
SID "111"
Position [95, 93, 125, 107]
Port "2"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.controlActive"
SID "112"
Position [95, 128, 125, 142]
Port "3"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].parKp"
SID "115"
Position [95, 348, 125, 362]
Port "4"
IconDisplay "Port number"
OutDataTypeStr "double"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].parKv"
SID "116"
Position [95, 383, 125, 397]
Port "5"
IconDisplay "Port number"
OutDataTypeStr "double"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].parKi"
SID "117"
Position [95, 418, 125, 432]
Port "6"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].parTrqMin"
SID "118"
Position [95, 453, 125, 467]
Port "7"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].parTrqMax"
SID "119"
Position [95, 488, 125, 502]
Port "8"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].posRawFb"
SID "121"
Position [95, 558, 125, 572]
Port "9"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].posFlt"
SID "122"
Position [95, 593, 125, 607]
Port "10"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].velRaw"
SID "123"
Position [635, 118, 665, 132]
Port "11"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].velFlt"
SID "124"
Position [95, 633, 125, 647]
Port "12"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].velOut"
SID "125"
Position [95, 668, 125, 682]
Port "13"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].trqRaw"
SID "126"
Position [95, 703, 125, 717]
Port "14"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].trqRefFlt"
SID "127"
Position [95, 738, 125, 752]
Port "15"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].posRef"
SID "128"
Position [95, 773, 125, 787]
Port "16"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].velRef"
SID "129"
Position [95, 808, 125, 822]
Port "17"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].trqFfw"
SID "130"
Position [95, 843, 125, 857]
Port "18"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].trqFfwGrav"
SID "131"
Position [95, 878, 125, 892]
Port "19"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].mocgendata.instruction"
SID "132"
Position [95, 913, 125, 927]
Port "20"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].mocgendata.value1"
SID "133"
Position [95, 948, 125, 962]
Port "21"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].mocgendata.value2"
SID "134"
Position [95, 983, 125, 997]
Port "22"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].mocgendata.value3"
SID "135"
Position [95, 1018, 125, 1032]
Port "23"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].mocgendata.value4"
SID "136"
Position [95, 1053, 125, 1067]
Port "24"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].mocgendata.value5"
SID "137"
Position [95, 1093, 125, 1107]
Port "25"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].mocgendata.value6"
SID "138"
Position [95, 1128, 125, 1142]
Port "26"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].mocgendata.string1[i]"
SID "139"
Position [85, 1163, 115, 1177]
Port "27"
IconDisplay "Port number"
PortDimensions "20"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].mocgendata.string2[i]"
SID "140"
Position [80, 1203, 110, 1217]
Port "28"
IconDisplay "Port number"
PortDimensions "20"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].parKp"
SID "226"
Position [870, 328, 900, 342]
Port "29"
IconDisplay "Port number"
OutDataTypeStr "double"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].parKv"
SID "227"
Position [890, 383, 920, 397]
Port "30"
IconDisplay "Port number"
OutDataTypeStr "double"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].parKi"
SID "228"
Position [890, 418, 920, 432]
Port "31"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].parTrqMin"
SID "229"
Position [890, 453, 920, 467]
Port "32"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].parTrqMax"
SID "230"
Position [890, 488, 920, 502]
Port "33"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].posRawFb"
SID "232"
Position [910, 558, 940, 572]
Port "34"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].posFlt"
SID "233"
Position [890, 593, 920, 607]
Port "35"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].velFlt"
SID "235"
Position [890, 663, 920, 677]
Port "36"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].velOut"
SID "236"
Position [890, 698, 920, 712]
Port "37"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].trqRaw"
SID "237"
Position [890, 733, 920, 747]
Port "38"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].trqRefFlt"
SID "238"
Position [890, 768, 920, 782]
Port "39"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].posRef"
SID "239"
Position [945, 1328, 975, 1342]
Port "40"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].velRef"
SID "240"
Position [960, 1293, 990, 1307]
Port "41"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].trqFfw"
SID "241"
Position [920, 828, 950, 842]
Port "42"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].trqFfwGrav"
SID "242"
Position [920, 878, 950, 892]
Port "43"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].mocgendata.instruction"
SID "243"
Position [920, 908, 950, 922]
Port "44"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].mocgendata.value1"
SID "244"
Position [920, 948, 950, 962]
Port "45"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].mocgendata.value2"
SID "245"
Position [910, 988, 940, 1002]
Port "46"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].mocgendata.value3"
SID "246"
Position [910, 1028, 940, 1042]
Port "47"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].mocgendata.value4"
SID "247"
Position [910, 1068, 940, 1082]
Port "48"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].mocgendata.value5"
SID "248"
Position [910, 1108, 940, 1122]
Port "49"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].mocgendata.value6"
SID "249"
Position [900, 1148, 930, 1162]
Port "50"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].mocgendata.string1[i]"
SID "250"
Position [915, 1188, 945, 1202]
Port "51"
IconDisplay "Port number"
PortDimensions "20"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].mocgendata.string2[i]"
SID "251"
Position [895, 1228, 925, 1242]
Port "52"
IconDisplay "Port number"
PortDimensions "20"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[1].joint[i].posRawAbs"
SID "120"
Position [635, 88, 665, 102]
Port "53"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].posRawAbs"
SID "231"
Position [635, 28, 665, 42]
Port "54"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.robot[0].joint[i].velRaw"
SID "234"
Position [635, 58, 665, 72]
Port "55"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "ati2rob.forcesTorques[i]"
SID "773"
Position [645, 178, 675, 192]
Port "56"
IconDisplay "Port number"
PortDimensions "6"
SignalType "real"
}
Block {
BlockType Inport
Name "ati2rob_2.forcesTorques[i]"
SID "1279"
Position [570, 198, 600, 212]
Port "57"
IconDisplay "Port number"
PortDimensions "6"
SignalType "real"
}
Block {
BlockType Constant
Name "Constant1"
SID "1254"
Position [1000, 1265, 1080, 1285]
ShowName off
Value "f_switch"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant9"
SID "378"
Position [45, 1340, 125, 1360]
ShowName off
Value "f_switch"
VectorParams1D off
}
Block {
BlockType SubSystem
Name "Left"
SID "557"
Ports [29, 16]
Position [335, 234, 655, 1296]
LibraryVersion "1.762"
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
System {
Name "Left"
Location [131, -19, 1728, 1033]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "A4"
PaperUnits "centimeters"
TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
Block {
BlockType Inport
Name "irb2ext.obtaining"
SID "558"
Position [825, 968, 855, 982]
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.manualMode"
SID "559"
Position [65, 73, 95, 87]
Port "2"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.controlActive"
SID "560"
Position [835, 928, 865, 942]
Port "3"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].parKp"
SID "561"
Position [1280, 233, 1310, 247]
Port "4"
IconDisplay "Port number"
OutDataTypeStr "double"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].parKv"
SID "562"
Position [1280, 268, 1310, 282]
Port "5"
IconDisplay "Port number"
OutDataTypeStr "double"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].parKi"
SID "563"
Position [1295, 303, 1325, 317]
Port "6"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].parTrqMin"
SID "564"
Position [65, 233, 95, 247]
Port "7"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].parTrqMax"
SID "565"
Position [65, 263, 95, 277]
Port "8"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].posRawFb"
SID "567"
Position [475, 283, 505, 297]
Port "9"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].posFlt"
SID "568"
Position [65, 363, 95, 377]
Port "10"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].velFlt"
SID "570"
Position [1025, 513, 1055, 527]
Port "11"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].velOut"
SID "571"
Position [380, 43, 410, 57]
Port "12"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].trqRaw"
SID "572"
Position [1270, 363, 1300, 377]
Port "13"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].trqRefFlt"
SID "573"
Position [365, 103, 395, 117]
Port "14"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].posRef"
SID "574"
Position [1155, 388, 1185, 402]
NamePlacement "alternate"
Port "15"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].velRef"
SID "575"
Position [1630, 863, 1660, 877]
Port "16"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].trqFfw"
SID "576"
Position [90, 148, 120, 162]
Port "17"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].trqFfwGrav"
SID "577"
Position [90, 183, 120, 197]
Port "18"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.instruction"
SID "578"
Position [760, 813, 790, 827]
Port "19"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.value1"
SID "579"
Position [145, 458, 175, 472]
Port "20"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.value2"
SID "580"
Position [145, 493, 175, 507]
Port "21"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.value3"
SID "581"
Position [145, 528, 175, 542]
Port "22"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.value4"
SID "582"
Position [145, 563, 175, 577]
Port "23"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.value5"
SID "583"
Position [145, 598, 175, 612]
Port "24"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.value6"
SID "584"
Position [145, 633, 175, 647]
Port "25"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.string1[i]"
SID "585"
Position [145, 668, 175, 682]
Port "26"
IconDisplay "Port number"
PortDimensions "20"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.string2[i]"
SID "586"
Position [145, 703, 175, 717]
Port "27"
IconDisplay "Port number"
PortDimensions "20"
SignalType "real"
}
Block {
BlockType Inport
Name "f_switch"
SID "587"
Position [1405, 878, 1435, 892]
Port "28"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "torque_motor_2"
SID "1356"
Position [1140, 813, 1170, 827]
Port "29"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Constant
Name "Constant"
SID "1303"
Position [1230, 820, 1260, 850]
ZOrder -4
Value "0"
}
Block {
BlockType Constant
Name "Constant1"
SID "698"
Position [1195, 754, 1310, 776]
ShowName off
Value "maxRateLeft"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant11"
SID "736"
Position [1120, 875, 1260, 895]
ShowName off
Value "jointLimitsStartMotorRad"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant13"
SID "955"
Position [825, 1005, 905, 1025]
ShowName off
Value "f_switch"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant2"
SID "590"
Position [1185, 789, 1300, 811]
ShowName off
Value "highKiActive"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant3"
SID "695"
Position [1225, 579, 1340, 601]
ShowName off
Value "keepStill"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant4"
SID "699"
Position [1200, 719, 1315, 741]
ShowName off
Value "slidingThresWidthLeft"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant5"
SID "591"
Position [570, 319, 685, 341]
ShowName off
Value "x_hat_rob2"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant6"
SID "592"
Position [570, 399, 685, 421]
ShowName off
Value "baseAnglesLeft"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant7"
SID "700"
Position [1195, 684, 1310, 706]
ShowName off
Value "KiHighThresLeft"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant8"
SID "701"
Position [1195, 649, 1310, 671]
ShowName off
Value "percentageTrqFfwLeft"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant9"
SID "594"
Position [570, 359, 685, 381]
ShowName off
Value "gearRatio"
VectorParams1D off
}
Block {
BlockType DiscreteFilter
Name "Discrete Filter3"
SID "595"
Ports [1, 1]
Position [1140, 502, 1200, 538]
InputPortMap "u0"
Numerator "Blpfilt"
Denominator "Alpfilt"
a0EqualsOne on
}
Block {
BlockType From
Name "From"
SID "733"
Position [1235, 431, 1330, 459]
GotoTag "f_switch_mod"
}
Block {
BlockType Gain
Name "Gain2"
SID "734"
Position [1475, 1020, 1505, 1050]
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
BlockType Gain
Name "Gain3"
SID "735"
Position [1610, 1020, 1640, 1050]
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
BlockType Goto
Name "Goto"
SID "732"
Position [1560, 912, 1640, 938]
GotoTag "f_switch_mod"
}
Block {
BlockType SubSystem
Name "MATLAB Function3"
SID "1234"
Ports [3, 4]
Position [1000, 913, 1255, 1037]
ShowName off
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "MATLAB Function3"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "68"
Block {
BlockType Inport
Name "controlActive"
SID "1234::66"
Position [20, 101, 40, 119]
ZOrder 18
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "obtaining"
SID "1234::67"
Position [20, 136, 40, 154]
ZOrder 19
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "switch_on"
SID "1234::68"
Position [20, 171, 40, 189]
ZOrder 20
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1234::62"
Ports [1, 1]
Position [270, 250, 320, 290]
ZOrder 14
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1234::61"
Tag "Stateflow S-Function scaled_teleoperation 15"
Ports [3, 5]
Position [180, 108, 230, 252]
ZOrder 13
FunctionName "sf_sfun"
PortCounts "[3 5]"
EnableBusSupport on
Port {
PortNumber 2
Name "runExtctrl"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "lockOutput"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 4
Name "resetProgram"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 5
Name "state"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1234::64"
Position [460, 261, 480, 279]
ZOrder 16
}
Block {
BlockType Outport
Name "runExtctrl"
SID "1234::56"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "lockOutput"
SID "1234::5"
Position [460, 136, 480, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "resetProgram"
SID "1234::45"
Position [460, 171, 480, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "state"
SID "1234::65"
Position [460, 206, 480, 224]
ZOrder 17
Port "4"
IconDisplay "Port number"
}
Line {
SrcBlock "controlActive"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "obtaining"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
SrcBlock "switch_on"
SrcPort 1
DstBlock " SFunction "
DstPort 3
}
Line {
Name "runExtctrl"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "runExtctrl"
DstPort 1
}
Line {
Name "lockOutput"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "lockOutput"
DstPort 1
}
Line {
Name "resetProgram"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 4
DstBlock "resetProgram"
DstPort 1
}
Line {
Name "state"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 5
DstBlock "state"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType Product
Name "Product"
SID "731"
Ports [2, 1]
Position [1495, 907, 1525, 938]
InputSameDT off
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
BlockType Terminator
Name "Terminator"
SID "602"
Position [180, 230, 200, 250]
}
Block {
BlockType Terminator
Name "Terminator1"
SID "603"
Position [220, 260, 240, 280]
}
Block {
BlockType Terminator
Name "Terminator10"
SID "604"
Position [520, 100, 540, 120]
}
Block {
BlockType Terminator
Name "Terminator11"
SID "605"
Position [1695, 1025, 1715, 1045]
}
Block {
BlockType Terminator
Name "Terminator12"
SID "606"
Position [175, 70, 195, 90]
}
Block {
BlockType Terminator
Name "Terminator13"
SID "607"
Position [1540, 980, 1560, 1000]
}
Block {
BlockType Terminator
Name "Terminator2"
SID "610"
Position [200, 180, 220, 200]
}
Block {
BlockType Terminator
Name "Terminator4"
SID "612"
Position [190, 145, 210, 165]
}
Block {
BlockType Terminator
Name "Terminator5"
SID "613"
Position [180, 360, 200, 380]
}
Block {
BlockType Terminator
Name "Terminator8"
SID "616"
Position [535, 40, 555, 60]
}
Block {
BlockType SubSystem
Name "calcGravityTorque"
SID "696"
Ports [4, 3]
Position [730, 265, 910, 435]
AncestorBlock "leadthroughLib/calcGravityTorque"
LibraryVersion "*1.87"
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
System {
Name "calcGravityTorque"
Location [581, 363, 1572, 970]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "940"
SIDPrevWatermark "939"
Block {
BlockType Inport
Name "q_motor"
SID "696:3"
Position [150, 78, 180, 92]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "x_hat"
SID "696:8"
Position [175, 188, 205, 202]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "gearRatio"
SID "696:10"
Position [200, 298, 230, 312]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "baseAngles"
SID "696:12"
Position [200, 408, 230, 422]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Terminator
Name "Terminator1"
SID "696:16"
Position [675, 75, 695, 95]
}
Block {
BlockType SubSystem
Name "calcGravTrq"
SID "696:1"
Ports [4, 4]
Position [305, 34, 600, 466]
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
"rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "calcGravTrq"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "46"
Block {
BlockType Inport
Name "q_motor"
SID "696:1::20"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "x_hat"
SID "696:1::21"
Position [20, 136, 40, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "gearRatio"
SID "696:1::22"
Position [20, 171, 40, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "baseAngles"
SID "696:1::24"
Position [20, 206, 40, 224]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "696:1::43"
Ports [1, 1]
Position [270, 245, 320, 285]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "696:1::42"
Tag "Stateflow S-Function scaled_teleoperation 2"
Ports [4, 5]
Position [180, 100, 230, 220]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[4 5]"
EnableBusSupport on
Port {
PortNumber 2
Name "gravTrq"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "gravTrqMotor"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 4
Name "coulombWidthMotor"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 5
Name "coulombWidth"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "696:1::45"
Position [460, 256, 480, 274]
ZOrder 8
}
Block {
BlockType Outport
Name "gravTrq"
SID "696:1::25"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "gravTrqMotor"
SID "696:1::26"
Position [460, 136, 480, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "coulombWidthMotor"
SID "696:1::27"
Position [460, 171, 480, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "coulombWidth"
SID "696:1::46"
Position [460, 206, 480, 224]
ZOrder 9
Port "4"
IconDisplay "Port number"
}
Line {
SrcBlock "q_motor"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "x_hat"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
SrcBlock "gearRatio"
SrcPort 1
DstBlock " SFunction "
DstPort 3
}
Line {
SrcBlock "baseAngles"
SrcPort 1
DstBlock " SFunction "
DstPort 4
}
Line {
Name "gravTrq"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "gravTrq"
DstPort 1
}
Line {
Name "gravTrqMotor"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "gravTrqMotor"
DstPort 1
}
Line {
Name "coulombWidthMotor"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 4
DstBlock "coulombWidthMotor"
DstPort 1
}
Line {
Name "coulombWidth"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 5
DstBlock "coulombWidth"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType Outport
Name "gravTrqMotor"
SID "696:13"
Position [685, 188, 715, 202]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "coulombWidthMotor"
SID "696:43"
Position [695, 298, 725, 312]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "coulombWidthArm"
SID "696:940"
Position [680, 409, 715, 421]
Port "3"
IconDisplay "Port number"
}
Line {
SrcBlock "calcGravTrq"
SrcPort 3
DstBlock "coulombWidthMotor"
DstPort 1
}
Line {
SrcBlock "calcGravTrq"
SrcPort 2
DstBlock "gravTrqMotor"
DstPort 1
}
Line {
SrcBlock "baseAngles"
SrcPort 1
DstBlock "calcGravTrq"
DstPort 4
}
Line {
SrcBlock "gearRatio"
SrcPort 1
DstBlock "calcGravTrq"
DstPort 3
}
Line {
SrcBlock "calcGravTrq"
SrcPort 1
DstBlock "Terminator1"
DstPort 1
}
Line {
SrcBlock "x_hat"
SrcPort 1
DstBlock "calcGravTrq"
DstPort 2
}
Line {
SrcBlock "q_motor"
SrcPort 1
DstBlock "calcGravTrq"
DstPort 1
}
Line {
SrcBlock "calcGravTrq"
SrcPort 4
DstBlock "coulombWidthArm"
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "leadthroug control"
SID "697"
Ports [23, 5]
Position [1415, 231, 1610, 849]
AncestorBlock "leadthroughLib/leadthroug control"
LibraryVersion "*1.87"
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
System {
Name "leadthroug control"
Location [240, 156, 1680, 970]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "942"
SIDPrevWatermark "939"
Block {
BlockType Inport
Name "Kp_in"
SID "697:19"
Position [225, 58, 255, 72]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "Kv_in"
SID "697:20"
Position [335, 83, 365, 97]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "Ki_in"
SID "697:21"
Position [390, 108, 420, 122]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "gravTrqMotor"
SID "697:22"
Position [320, 133, 350, 147]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "trqRaw"
SID "697:24"
Position [265, 158, 295, 172]
Port "5"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "posRef_in"
SID "697:25"
Position [225, 183, 255, 197]
Port "6"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "posRaw"
SID "697:26"
Position [300, 208, 330, 222]
Port "7"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "f_switch"
SID "697:28"
Position [355, 233, 385, 247]
Port "8"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "velFlt_filtered"
SID "697:29"
Position [165, 258, 195, 272]
Port "9"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "velFlt1"
SID "697:30"
Position [160, 333, 190, 347]
Port "10"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "keepStill"
SID "697:32"
Position [345, 308, 375, 322]
Port "11"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "coulomb_width"
SID "697:34"
Position [235, 358, 265, 372]
Port "12"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "percentageTrqFfw"
SID "697:36"
Position [305, 383, 335, 397]
Port "13"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "KiHighThres"
SID "697:37"
Position [370, 408, 400, 422]
Port "14"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "slidingThresWidth"
SID "697:38"
Position [430, 433, 460, 447]
Port "15"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "maxRate"
SID "697:40"
Position [235, 458, 265, 472]
Port "16"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "highKiActive"
SID "697:42"
Position [285, 283, 315, 297]
Port "17"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "resetPosRef"
SID "697:45"
Position [370, 493, 400, 507]
Port "18"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "jointLimitsStartMotorRad"
SID "697:46"
Position [370, 528, 400, 542]
Port "19"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "extTorque"
SID "697:116"
Position [370, 618, 400, 632]
Port "20"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "sensorOn"
SID "697:117"
Position [370, 578, 400, 592]
Port "21"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "fricHelp"
SID "697:118"
Position [370, 673, 400, 687]
Port "22"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "torque_motor_2"
SID "697:942"
Position [370, 738, 400, 752]
Port "23"
IconDisplay "Port number"
}
Block {
BlockType SubSystem
Name "MATLAB Function"
SID "697:17"
Ports [23, 7]
Position [760, 36, 1265, 624]
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
"rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
Port {
PortNumber 6
Name "offset[i]"
PropagatedSignals "test"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 7
Name "sat"
PropagatedSignals "sat"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
System {
Name "MATLAB Function"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "84"
Block {
BlockType Inport
Name "Kp_in"
SID "697:17::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "Kv_in"
SID "697:17::18"
Position [20, 136, 40, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "Ki_in"
SID "697:17::19"
Position [20, 171, 40, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "gravTrqMotor"
SID "697:17::20"
Position [20, 206, 40, 224]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "trqRaw"
SID "697:17::21"
Position [20, 246, 40, 264]
Port "5"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "posRefIn"
SID "697:17::22"
Position [20, 281, 40, 299]
Port "6"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "posRaw"
SID "697:17::23"
Position [20, 316, 40, 334]
Port "7"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "f_switch"
SID "697:17::24"
Position [20, 351, 40, 369]
Port "8"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "velFlt"
SID "697:17::25"
Position [20, 386, 40, 404]
Port "9"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "highKiActive"
SID "697:17::26"
Position [20, 426, 40, 444]
Port "10"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "keepStill"
SID "697:17::27"
Position [20, 461, 40, 479]
Port "11"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "velFlt_noExtraFilt"
SID "697:17::28"
Position [20, 496, 40, 514]
Port "12"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "coulomb_width"
SID "697:17::34"
Position [20, 531, 40, 549]
Port "13"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "percentageTrqFfw"
SID "697:17::35"
Position [20, 566, 40, 584]
Port "14"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "KiHighThres"
SID "697:17::36"
Position [20, 606, 40, 624]
Port "15"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "slidingThresWidth"
SID "697:17::37"
Position [20, 641, 40, 659]
Port "16"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "maxRate"
SID "697:17::38"
Position [20, 676, 40, 694]
Port "17"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "resetPosRef"
SID "697:17::39"
Position [20, 711, 40, 729]
Port "18"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "jointLimitStartMotorRad"
SID "697:17::40"
Position [20, 746, 40, 764]
Port "19"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "extTorque"
SID "697:17::41"
Position [20, 786, 40, 804]
Port "20"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "sensorOn"
SID "697:17::42"
Position [20, 821, 40, 839]
Port "21"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "fricHelp"
SID "697:17::43"
Position [20, 856, 40, 874]
Port "22"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "qtorque"
SID "697:17::84"
Position [20, 891, 40, 909]
ZOrder 11
Port "23"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "697:17::79"
Ports [1, 1]
Position [270, 580, 320, 620]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "697:17::78"
Tag "Stateflow S-Function scaled_teleoperation 12"
Ports [23, 8]
Position [180, 100, 230, 580]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[23 8]"
EnableBusSupport on
Port {
PortNumber 2
Name "Kp"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "Kv"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 4
Name "Ki"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 5
Name "trqFfw"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 6
Name "posRef"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 7
Name "test"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 8
Name "sat"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "697:17::81"
Position [460, 591, 480, 609]
ZOrder 8
}
Block {
BlockType Outport
Name "Kp"
SID "697:17::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Kv"
SID "697:17::29"
Position [460, 136, 480, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Ki"
SID "697:17::30"
Position [460, 171, 480, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "trqFfw"
SID "697:17::31"
Position [460, 206, 480, 224]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "posRef"
SID "697:17::32"
Position [460, 246, 480, 264]
Port "5"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "test"
SID "697:17::82"
Position [460, 281, 480, 299]
ZOrder 9
Port "6"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "sat"
SID "697:17::83"
Position [460, 316, 480, 334]
ZOrder 10
Port "7"
IconDisplay "Port number"
}
Line {
SrcBlock "Kp_in"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "Kv_in"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
SrcBlock "Ki_in"
SrcPort 1
DstBlock " SFunction "
DstPort 3
}
Line {
SrcBlock "gravTrqMotor"
SrcPort 1
DstBlock " SFunction "
DstPort 4
}
Line {
SrcBlock "trqRaw"
SrcPort 1
DstBlock " SFunction "
DstPort 5
}
Line {
SrcBlock "posRefIn"
SrcPort 1
DstBlock " SFunction "
DstPort 6
}
Line {
SrcBlock "posRaw"
SrcPort 1
DstBlock " SFunction "
DstPort 7
}
Line {
SrcBlock "f_switch"
SrcPort 1
DstBlock " SFunction "
DstPort 8
}
Line {
SrcBlock "velFlt"
SrcPort 1
DstBlock " SFunction "
DstPort 9
}
Line {
SrcBlock "highKiActive"
SrcPort 1
DstBlock " SFunction "
DstPort 10
}
Line {
SrcBlock "keepStill"
SrcPort 1
DstBlock " SFunction "
DstPort 11
}
Line {
SrcBlock "velFlt_noExtraFilt"
SrcPort 1
DstBlock " SFunction "
DstPort 12
}
Line {
SrcBlock "coulomb_width"
SrcPort 1
DstBlock " SFunction "
DstPort 13
}
Line {
SrcBlock "percentageTrqFfw"
SrcPort 1
DstBlock " SFunction "
DstPort 14
}
Line {
SrcBlock "KiHighThres"
SrcPort 1
DstBlock " SFunction "
DstPort 15
}
Line {
SrcBlock "slidingThresWidth"
SrcPort 1
DstBlock " SFunction "
DstPort 16
}
Line {
SrcBlock "maxRate"
SrcPort 1
DstBlock " SFunction "
DstPort 17
}
Line {
SrcBlock "resetPosRef"
SrcPort 1
DstBlock " SFunction "
DstPort 18
}
Line {
SrcBlock "jointLimitStartMotorRad"
SrcPort 1
DstBlock " SFunction "
DstPort 19
}
Line {
SrcBlock "extTorque"
SrcPort 1
DstBlock " SFunction "
DstPort 20
}
Line {
SrcBlock "sensorOn"
SrcPort 1
DstBlock " SFunction "
DstPort 21
}
Line {
SrcBlock "fricHelp"
SrcPort 1
DstBlock " SFunction "
DstPort 22
}
Line {
SrcBlock "qtorque"
SrcPort 1
DstBlock " SFunction "
DstPort 23
}
Line {
Name "Kp"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "Kp"
DstPort 1
}
Line {
Name "Kv"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "Kv"
DstPort 1
}
Line {
Name "Ki"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 4
DstBlock "Ki"
DstPort 1
}
Line {
Name "trqFfw"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 5
DstBlock "trqFfw"
DstPort 1
}
Line {
Name "posRef"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 6
DstBlock "posRef"
DstPort 1
}
Line {
Name "test"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 7
DstBlock "test"
DstPort 1
}
Line {
Name "sat"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 8
DstBlock "sat"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType Terminator
Name "Terminator1"
SID "697:941"
Position [1435, 655, 1455, 675]
}
Block {
BlockType Terminator
Name "Terminator14"
SID "697:940"
Position [1430, 555, 1450, 575]
}
Block {
BlockType Outport
Name "Kp"
SID "697:23"
Position [1515, 73, 1545, 87]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Kv"
SID "697:27"
Position [1525, 188, 1555, 202]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Ki"
SID "697:31"
Position [1505, 323, 1535, 337]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "trqFfw"
SID "697:35"
Position [1375, 398, 1405, 412]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "posRef"
SID "697:39"
Position [1385, 518, 1415, 532]
Port "5"
IconDisplay "Port number"
}
Line {
SrcBlock "fricHelp"
SrcPort 1
Points [270, 0; 0, -100]
DstBlock "MATLAB Function"
DstPort 22
}
Line {
SrcBlock "extTorque"
SrcPort 1
Points [115, 0; 0, -95]
DstBlock "MATLAB Function"
DstPort 20
}
Line {
SrcBlock "sensorOn"
SrcPort 1
Points [60, 0; 0, -30]
DstBlock "MATLAB Function"
DstPort 21
}
Line {
SrcBlock "jointLimitsStartMotorRad"
SrcPort 1
Points [340, 0]
DstBlock "MATLAB Function"
DstPort 19
}
Line {
SrcBlock "resetPosRef"
SrcPort 1
Points [340, 0]
DstBlock "MATLAB Function"
DstPort 18
}
Line {
SrcBlock "highKiActive"
SrcPort 1
Points [425, 0]
DstBlock "MATLAB Function"
DstPort 10
}
Line {
SrcBlock "velFlt1"
SrcPort 1
Points [550, 0]
DstBlock "MATLAB Function"
DstPort 12
}
Line {
SrcBlock "velFlt_filtered"
SrcPort 1
Points [545, 0]
DstBlock "MATLAB Function"
DstPort 9
}
Line {
SrcBlock "maxRate"
SrcPort 1
Points [475, 0]
DstBlock "MATLAB Function"
DstPort 17
}
Line {
SrcBlock "slidingThresWidth"
SrcPort 1
Points [280, 0]
DstBlock "MATLAB Function"
DstPort 16
}
Line {
SrcBlock "KiHighThres"
SrcPort 1
Points [340, 0]
DstBlock "MATLAB Function"
DstPort 15
}
Line {
SrcBlock "percentageTrqFfw"
SrcPort 1
Points [405, 0]
DstBlock "MATLAB Function"
DstPort 14
}
Line {
SrcBlock "coulomb_width"
SrcPort 1
Points [475, 0]
DstBlock "MATLAB Function"
DstPort 13
}
Line {
SrcBlock "keepStill"
SrcPort 1
Points [365, 0]
DstBlock "MATLAB Function"
DstPort 11
}
Line {
SrcBlock "MATLAB Function"
SrcPort 3
Points [0, 15]
DstBlock "Ki"
DstPort 1
}
Line {
SrcBlock "f_switch"
SrcPort 1
Points [355, 0]
DstBlock "MATLAB Function"
DstPort 8
}
Line {
SrcBlock "MATLAB Function"
SrcPort 2
Points [0, 10]
DstBlock "Kv"
DstPort 1
}
Line {
SrcBlock "posRaw"
SrcPort 1
Points [410, 0]
DstBlock "MATLAB Function"
DstPort 7
}
Line {
SrcBlock "posRef_in"
SrcPort 1
Points [485, 0]
DstBlock "MATLAB Function"
DstPort 6
}
Line {
SrcBlock "trqRaw"
SrcPort 1
Points [445, 0]
DstBlock "MATLAB Function"
DstPort 5
}
Line {
SrcBlock "MATLAB Function"
SrcPort 1
Points [0, 5]
DstBlock "Kp"
DstPort 1
}
Line {
SrcBlock "gravTrqMotor"
SrcPort 1
Points [390, 0]
DstBlock "MATLAB Function"
DstPort 4
}
Line {
SrcBlock "Ki_in"
SrcPort 1
Points [320, 0]
DstBlock "MATLAB Function"
DstPort 3
}
Line {
SrcBlock "Kv_in"
SrcPort 1
Points [375, 0]
DstBlock "MATLAB Function"
DstPort 2
}
Line {
SrcBlock "Kp_in"
SrcPort 1
Points [485, 0]
DstBlock "MATLAB Function"
DstPort 1
}
Line {
SrcBlock "MATLAB Function"
SrcPort 4
Points [45, 0; 0, 75]
DstBlock "trqFfw"
DstPort 1
}
Line {
SrcBlock "MATLAB Function"
SrcPort 5
Points [50, 0; 0, 110]
DstBlock "posRef"
DstPort 1
}
Line {
Name "offset[i]"
Labels [0, 0]
SrcBlock "MATLAB Function"
SrcPort 6
Points [0, 50; 145, 0]
DstBlock "Terminator14"
DstPort 1
}
Line {
Name "sat"
Labels [2, 0]
SrcBlock "MATLAB Function"
SrcPort 7
Points [0, 55; 140, 0; 0, 25]
DstBlock "Terminator1"
DstPort 1
}
Line {
SrcBlock "torque_motor_2"
SrcPort 1
Points [300, 0; 0, -140]
DstBlock "MATLAB Function"
DstPort 23
}
}
}
Block {
BlockType Reference
Name "leadthroughLockOutput"
SID "729"
Ports [3, 2]
Position [1785, 841, 1960, 899]
ForegroundColor "blue"
LibraryVersion "1.5"
SourceBlock "rapid_extctrl_handover_lib_leadthrough/leadthroughLockOutput"
SourceType "Stateflow"
}
Block {
BlockType Outport
Name "coulombWidthArmLeftAbb"
SID "1363"
Position [1025, 398, 1055, 412]
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.joint[i].parKp"
SID "618"
Position [1715, 308, 1745, 322]
Port "2"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.joint[i].parKv"
SID "619"
Position [1695, 428, 1725, 442]
Port "3"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.joint[i].parKi"
SID "620"
Position [1715, 548, 1745, 562]
Port "4"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.joint[i].posRef"
SID "621"
Position [2070, 848, 2100, 862]
Port "5"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.joint[i].velRef"
SID "622"
Position [2070, 878, 2100, 892]
Port "6"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.joint[i].trqFfw"
SID "623"
Position [1715, 668, 1745, 682]
Port "7"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.instruction"
SID "624"
Position [1040, 813, 1070, 827]
Port "8"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.value1"
SID "625"
Position [380, 458, 410, 472]
Port "9"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.value2"
SID "626"
Position [380, 493, 410, 507]
Port "10"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.value3"
SID "627"
Position [380, 528, 410, 542]
Port "11"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.value4"
SID "628"
Position [380, 563, 410, 577]
Port "12"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.value5"
SID "629"
Position [380, 598, 410, 612]
Port "13"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.value6"
SID "630"
Position [380, 633, 410, 647]
Port "14"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.string1[i]"
SID "631"
Position [380, 668, 410, 682]
Port "15"
IconDisplay "Port number"
PortDimensions "20"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.string2[i]"
SID "632"
Position [380, 703, 410, 717]
Port "16"
IconDisplay "Port number"
PortDimensions "20"
}
Line {
SrcBlock "irb2ext.joint[i].parTrqMin"
SrcPort 1
DstBlock "Terminator"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].parTrqMax"
SrcPort 1
DstBlock "Terminator1"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].trqFfwGrav"
SrcPort 1
DstBlock "Terminator2"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].posFlt"
SrcPort 1
DstBlock "Terminator5"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].trqRefFlt"
SrcPort 1
DstBlock "Terminator10"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.instruction"
SrcPort 1
DstBlock "ext2irb.mocgendata.instruction"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.string1[i]"
SrcPort 1
DstBlock "ext2irb.mocgendata.string1[i]"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.string2[i]"
SrcPort 1
DstBlock "ext2irb.mocgendata.string2[i]"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.value2"
SrcPort 1
DstBlock "ext2irb.mocgendata.value2"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.value3"
SrcPort 1
DstBlock "ext2irb.mocgendata.value3"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.value4"
SrcPort 1
DstBlock "ext2irb.mocgendata.value4"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.value5"
SrcPort 1
DstBlock "ext2irb.mocgendata.value5"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.value6"
SrcPort 1
DstBlock "ext2irb.mocgendata.value6"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.value1"
SrcPort 1
DstBlock "ext2irb.mocgendata.value1"
DstPort 1
}
Line {
SrcBlock "irb2ext.manualMode"
SrcPort 1
DstBlock "Terminator12"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].velOut"
SrcPort 1
DstBlock "Terminator8"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].posRawFb"
SrcPort 1
Points [35, 0]
Branch {
DstBlock "calcGravityTorque"
DstPort 1
}
Branch {
Points [0, 160; 40, -5; 10, 0]
DstBlock "leadthroug control"
DstPort 7
}
}
Line {
SrcBlock "calcGravityTorque"
SrcPort 1
Points [340, 0; 0, 45]
DstBlock "leadthroug control"
DstPort 4
}
Line {
SrcBlock "irb2ext.joint[i].trqFfw"
SrcPort 1
DstBlock "Terminator4"
DstPort 1
}
Line {
SrcBlock "Constant5"
SrcPort 1
DstBlock "calcGravityTorque"
DstPort 2
}
Line {
SrcBlock "Constant9"
SrcPort 1
DstBlock "calcGravityTorque"
DstPort 3
}
Line {
SrcBlock "Constant6"
SrcPort 1
DstBlock "calcGravityTorque"
DstPort 4
}
Line {
SrcBlock "irb2ext.joint[i].parKp"
SrcPort 1
DstBlock "leadthroug control"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].parKv"
SrcPort 1
DstBlock "leadthroug control"
DstPort 2
}
Line {
SrcBlock "irb2ext.joint[i].parKi"
SrcPort 1
Points [70, 0]
DstBlock "leadthroug control"
DstPort 3
}
Line {
SrcBlock "irb2ext.joint[i].trqRaw"
SrcPort 1
Points [95, 0]
DstBlock "leadthroug control"
DstPort 5
}
Line {
SrcBlock "irb2ext.joint[i].posRef"
SrcPort 1
Points [210, 0]
DstBlock "leadthroug control"
DstPort 6
}
Line {
SrcBlock "irb2ext.joint[i].velFlt"
SrcPort 1
Points [40, 0]
Branch {
DstBlock "Discrete Filter3"
DstPort 1
}
Branch {
Points [0, 35]
DstBlock "leadthroug control"
DstPort 10
}
}
Line {
SrcBlock "Constant2"
SrcPort 1
DstBlock "leadthroug control"
DstPort 17
}
Line {
SrcBlock "Constant3"
SrcPort 1
DstBlock "leadthroug control"
DstPort 11
}
Line {
SrcBlock "leadthroug control"
SrcPort 4
Points [0, 5]
DstBlock "ext2irb.joint[i].trqFfw"
DstPort 1
}
Line {
SrcBlock "leadthroug control"
SrcPort 3
Points [0, 10]
DstBlock "ext2irb.joint[i].parKi"
DstPort 1
}
Line {
SrcBlock "leadthroug control"
SrcPort 2
DstBlock "ext2irb.joint[i].parKv"
DstPort 1
}
Line {
SrcBlock "leadthroug control"
SrcPort 1
Points [0, 20]
DstBlock "ext2irb.joint[i].parKp"
DstPort 1
}
Line {
SrcBlock "Constant1"
SrcPort 1
DstBlock "leadthroug control"
DstPort 16
}
Line {
SrcBlock "Constant4"
SrcPort 1
DstBlock "leadthroug control"
DstPort 15
}
Line {
SrcBlock "Constant7"
SrcPort 1
DstBlock "leadthroug control"
DstPort 14
}
Line {
SrcBlock "Constant8"
SrcPort 1
DstBlock "leadthroug control"
DstPort 13
}
Line {
SrcBlock "Discrete Filter3"
SrcPort 1
DstBlock "leadthroug control"
DstPort 9
}
Line {
SrcBlock "calcGravityTorque"
SrcPort 2
Points [90, 0; 0, 275]
DstBlock "leadthroug control"
DstPort 12
}
Line {
SrcBlock "MATLAB Function3"
SrcPort 1
DstBlock "Product"
DstPort 2
}
Line {
SrcBlock "f_switch"
SrcPort 1
Points [20, 0; 0, 30]
DstBlock "Product"
DstPort 1
}
Line {
SrcBlock "Product"
SrcPort 1
DstBlock "Goto"
DstPort 1
}
Line {
SrcBlock "From"
SrcPort 1
Points [65, 0]
DstBlock "leadthroug control"
DstPort 8
}
Line {
SrcBlock "MATLAB Function3"
SrcPort 2
Points [55, 0; 0, -5]
Branch {
Points [0, -120]
DstBlock "leadthroug control"
DstPort 18
}
Branch {
Points [0, 5; 455, 0]
DstBlock "leadthroughLockOutput"
DstPort 3
}
}
Line {
SrcBlock "Gain3"
SrcPort 1
DstBlock "Terminator11"
DstPort 1
}
Line {
SrcBlock "Gain2"
SrcPort 1
DstBlock "Gain3"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].velRef"
SrcPort 1
DstBlock "leadthroughLockOutput"
DstPort 2
}
Line {
SrcBlock "Constant11"
SrcPort 1
Points [100, 0; 0, -75]
DstBlock "leadthroug control"
DstPort 19
}
Line {
SrcBlock "MATLAB Function3"
SrcPort 3
DstBlock "Terminator13"
DstPort 1
}
Line {
SrcBlock "MATLAB Function3"
SrcPort 4
Points [100, 0; 0, 15]
DstBlock "Gain2"
DstPort 1
}
Line {
SrcBlock "irb2ext.controlActive"
SrcPort 1
DstBlock "MATLAB Function3"
DstPort 1
}
Line {
SrcBlock "irb2ext.obtaining"
SrcPort 1
DstBlock "MATLAB Function3"
DstPort 2
}
Line {
SrcBlock "leadthroug control"
SrcPort 5
Points [75, 0; 0, 60]
DstBlock "leadthroughLockOutput"
DstPort 1
}
Line {
SrcBlock "Constant13"
SrcPort 1
DstBlock "MATLAB Function3"
DstPort 3
}
Line {
SrcBlock "leadthroughLockOutput"
SrcPort 1
DstBlock "ext2irb.joint[i].posRef"
DstPort 1
}
Line {
SrcBlock "leadthroughLockOutput"
SrcPort 2
DstBlock "ext2irb.joint[i].velRef"
DstPort 1
}
Line {
SrcBlock "Constant"
SrcPort 1
Points [135, 0]
DstBlock "leadthroug control"
DstPort 22
}
Line {
SrcBlock "torque_motor_2"
SrcPort 1
Points [50, 0; 0, -5]
DstBlock "leadthroug control"
DstPort 23
}
Line {
SrcBlock "calcGravityTorque"
SrcPort 3
DstBlock "coulombWidthArmLeftAbb"
DstPort 1
}
Annotation {
SID "1355"
Position [964, 363]
}
}
}
Block {
BlockType SubSystem
Name "MATLAB Function1"
SID "895"
Ports [1, 1]
Position [1545, 1355, 1595, 1395]
ShowName off
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('pr"
"ivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'nancheck']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "MATLAB Function1"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "64"
Block {
BlockType Inport
Name "u"
SID "895::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "895::62"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 14
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "895::61"
Tag "Stateflow S-Function scaled_teleoperation 7"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 13
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "y"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "895::64"
Position [460, 171, 480, 189]
ZOrder 16
}
Block {
BlockType Outport
Name "y"
SID "895::56"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "u"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "y"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "y"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "MATLAB Function2"
SID "896"
Ports [1, 1]
Position [1625, 530, 1675, 570]
ShowName off
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('pr"
"ivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'nancheck']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "MATLAB Function2"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "64"
Block {
BlockType Inport
Name "u"
SID "896::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "896::62"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 14
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "896::61"
Tag "Stateflow S-Function scaled_teleoperation 9"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 13
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "y"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "896::64"
Position [460, 171, 480, 189]
ZOrder 16
}
Block {
BlockType Outport
Name "y"
SID "896::56"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "u"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "y"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "y"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "MATLAB Function3"
SID "934"
Ports [1, 1]
Position [695, 520, 745, 560]
ShowName off
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('pr"
"ivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'nancheck']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "MATLAB Function3"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "64"
Block {
BlockType Inport
Name "u"
SID "934::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "934::62"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 14
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "934::61"
Tag "Stateflow S-Function scaled_teleoperation 6"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 13
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "y"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "934::64"
Position [460, 171, 480, 189]
ZOrder 16
}
Block {
BlockType Outport
Name "y"
SID "934::56"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "u"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "y"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "y"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "MATLAB Function4"
SID "935"
Ports [1, 1]
Position [685, 585, 735, 625]
ShowName off
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('pr"
"ivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'nancheck']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "MATLAB Function4"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "64"
Block {
BlockType Inport
Name "u"
SID "935::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "935::62"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 14
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "935::61"
Tag "Stateflow S-Function scaled_teleoperation 10"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 13
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "y"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "935::64"
Position [460, 171, 480, 189]
ZOrder 16
}
Block {
BlockType Outport
Name "y"
SID "935::56"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "u"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "y"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "y"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "Right"
SID "633"
Ports [29, 16]
Position [1150, 190, 1480, 1430]
LibraryVersion "1.762"
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
System {
Name "Right"
Location [0, -28, 1920, 1080]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "A4"
PaperUnits "centimeters"
TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
Block {
BlockType Inport
Name "irb2ext.obtaining"
SID "634"
Position [415, 968, 445, 982]
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.manualMode"
SID "635"
Position [65, 83, 95, 97]
Port "2"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.controlActive"
SID "636"
Position [415, 928, 445, 942]
Port "3"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].parKp"
SID "637"
Position [820, 58, 850, 72]
Port "4"
IconDisplay "Port number"
OutDataTypeStr "double"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].parKv"
SID "638"
Position [830, 103, 860, 117]
Port "5"
IconDisplay "Port number"
OutDataTypeStr "double"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].parKi"
SID "639"
Position [825, 148, 855, 162]
Port "6"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].parTrqMin"
SID "640"
Position [65, 233, 95, 247]
Port "7"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].parTrqMax"
SID "641"
Position [65, 263, 95, 277]
Port "8"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].posRawFb"
SID "643"
Position [320, 183, 350, 197]
Port "9"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].posFlt"
SID "644"
Position [65, 363, 95, 377]
Port "10"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].velFlt"
SID "646"
Position [750, 373, 780, 387]
Port "11"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].velOut"
SID "647"
Position [65, 453, 95, 467]
Port "12"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].trqRaw"
SID "648"
Position [835, 198, 865, 212]
Port "13"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].trqRefFlt"
SID "649"
Position [65, 513, 95, 527]
Port "14"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].trqFfw"
SID "652"
Position [65, 618, 95, 632]
Port "15"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].trqFfwGrav"
SID "653"
Position [65, 653, 95, 667]
Port "16"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.instruction"
SID "654"
Position [75, 778, 105, 792]
Port "17"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.value1"
SID "655"
Position [65, 838, 95, 852]
Port "18"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.value2"
SID "656"
Position [65, 873, 95, 887]
Port "19"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.value3"
SID "657"
Position [65, 908, 95, 922]
Port "20"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.value4"
SID "658"
Position [65, 943, 95, 957]
Port "21"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.value5"
SID "659"
Position [65, 978, 95, 992]
Port "22"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.value6"
SID "660"
Position [65, 1013, 95, 1027]
Port "23"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.string1[i]"
SID "661"
Position [65, 1048, 95, 1062]
Port "24"
IconDisplay "Port number"
PortDimensions "20"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.mocgendata.string2[i]"
SID "662"
Position [65, 1083, 95, 1097]
Port "25"
IconDisplay "Port number"
PortDimensions "20"
SignalType "real"
}
Block {
BlockType Inport
Name "f_switch"
SID "663"
Position [900, 873, 930, 887]
Port "26"
IconDisplay "Port number"
PortDimensions "1"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].velRef"
SID "651"
Position [1190, 773, 1220, 787]
Port "27"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "irb2ext.joint[i].posRef"
SID "1251"
Position [755, 298, 785, 312]
NamePlacement "alternate"
Port "28"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Inport
Name "torque_motor_1"
SID "1354"
Position [440, 838, 470, 852]
Port "29"
IconDisplay "Port number"
PortDimensions "DOF"
SignalType "real"
}
Block {
BlockType Constant
Name "Constant"
SID "1302"
Position [695, 865, 725, 895]
ZOrder -4
Value "0"
}
Block {
BlockType Constant
Name "Constant1"
SID "703"
Position [745, 729, 860, 751]
ShowName off
Value "maxRateRight"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant10"
SID "1255"
Position [390, 1005, 470, 1025]
ShowName off
Value "f_switch"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant11"
SID "737"
Position [615, 795, 755, 815]
ShowName off
Value "jointLimitsStartMotorRad"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant2"
SID "704"
Position [740, 764, 855, 786]
ShowName off
Value "highKiActive"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant3"
SID "705"
Position [770, 504, 885, 526]
ShowName off
Value "keepStill"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant4"
SID "706"
Position [745, 684, 860, 706]
ShowName off
Value "slidingThresWidthRight"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant5"
SID "707"
Position [290, 219, 405, 241]
ShowName off
Value "x_hat_rob1"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant6"
SID "708"
Position [290, 299, 405, 321]
ShowName off
Value "baseAnglesRight"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant7"
SID "709"
Position [745, 639, 860, 661]
ShowName off
Value "KiHighThresRight"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant8"
SID "710"
Position [710, 579, 825, 601]
ShowName off
Value "percentageTrqFfwRight"
VectorParams1D off
}
Block {
BlockType Constant
Name "Constant9"
SID "711"
Position [290, 259, 405, 281]
ShowName off
Value "gearRatio"
VectorParams1D off
}
Block {
BlockType DiscreteFilter
Name "Discrete Filter3"
SID "712"
Ports [1, 1]
Position [800, 407, 860, 443]
InputPortMap "u0"
Numerator "Blpfilt"
Denominator "Alpfilt"
a0EqualsOne on
}
Block {
BlockType From
Name "From"
SID "726"
Position [775, 322, 880, 348]
ShowName off
GotoTag "f_switch_mod"
}
Block {
BlockType Gain
Name "Gain"
SID "722"
Position [965, 1020, 995, 1050]
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
BlockType Gain
Name "Gain1"
SID "723"
Position [1065, 1020, 1095, 1050]
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
BlockType Goto
Name "Goto"
SID "725"
Position [1060, 911, 1155, 939]
ShowName off
GotoTag "f_switch_mod"
}
Block {
BlockType SubSystem
Name "MATLAB Function3"
SID "1253"
Ports [3, 4]
Position [525, 913, 780, 1037]
ShowName off
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "MATLAB Function3"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "68"
Block {
BlockType Inport
Name "controlActive"
SID "1253::66"
Position [20, 101, 40, 119]
ZOrder 18
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "obtaining"
SID "1253::67"
Position [20, 136, 40, 154]
ZOrder 19
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "switch_on"
SID "1253::68"
Position [20, 171, 40, 189]
ZOrder 20
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1253::62"
Ports [1, 1]
Position [270, 250, 320, 290]
ZOrder 14
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1253::61"
Tag "Stateflow S-Function scaled_teleoperation 27"
Ports [3, 5]
Position [180, 108, 230, 252]
ZOrder 13
FunctionName "sf_sfun"
PortCounts "[3 5]"
EnableBusSupport on
Port {
PortNumber 2
Name "runExtctrl"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "lockOutput"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 4
Name "resetProgram"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 5
Name "state"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1253::64"
Position [460, 261, 480, 279]
ZOrder 16
}
Block {
BlockType Outport
Name "runExtctrl"
SID "1253::56"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "lockOutput"
SID "1253::5"
Position [460, 136, 480, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "resetProgram"
SID "1253::45"
Position [460, 171, 480, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "state"
SID "1253::65"
Position [460, 206, 480, 224]
ZOrder 17
Port "4"
IconDisplay "Port number"
}
Line {
SrcBlock "controlActive"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "obtaining"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
SrcBlock "switch_on"
SrcPort 1
DstBlock " SFunction "
DstPort 3
}
Line {
Name "runExtctrl"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "runExtctrl"
DstPort 1
}
Line {
Name "lockOutput"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "lockOutput"
DstPort 1
}
Line {
Name "resetProgram"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 4
DstBlock "resetProgram"
DstPort 1
}
Line {
Name "state"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 5
DstBlock "state"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType Product
Name "Product"
SID "724"
Ports [2, 1]
Position [985, 907, 1015, 938]
InputSameDT off
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
BlockType Terminator
Name "Terminator"
SID "664"
Position [180, 230, 200, 250]
}
Block {
BlockType Terminator
Name "Terminator1"
SID "665"
Position [220, 260, 240, 280]
}
Block {
BlockType Terminator
Name "Terminator10"
SID "666"
Position [220, 510, 240, 530]
}
Block {
BlockType Terminator
Name "Terminator11"
SID "667"
Position [220, 80, 240, 100]
}
Block {
BlockType Terminator
Name "Terminator2"
SID "674"
Position [175, 650, 195, 670]
}
Block {
BlockType Terminator
Name "Terminator4"
SID "716"
Position [175, 615, 195, 635]
}
Block {
BlockType Terminator
Name "Terminator5"
SID "676"
Position [180, 360, 200, 380]
}
Block {
BlockType Terminator
Name "Terminator7"
SID "721"
Position [1130, 1025, 1150, 1045]
}
Block {
BlockType Terminator
Name "Terminator8"
SID "728"
Position [1020, 990, 1040, 1010]
}
Block {
BlockType Terminator
Name "aTerminator8"
SID "678"
Position [220, 450, 240, 470]
}
Block {
BlockType SubSystem
Name "calcGravityTorque"
SID "713"
Ports [4, 3]
Position [455, 165, 635, 335]
AncestorBlock "leadthroughLib/calcGravityTorque"
LibraryVersion "*1.87"
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
System {
Name "calcGravityTorque"
Location [603, 3, 1594, 610]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "940"
SIDPrevWatermark "939"
Block {
BlockType Inport
Name "q_motor"
SID "713:3"
Position [150, 78, 180, 92]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "x_hat"
SID "713:8"
Position [175, 188, 205, 202]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "gearRatio"
SID "713:10"
Position [200, 298, 230, 312]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "baseAngles"
SID "713:12"
Position [200, 408, 230, 422]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Terminator
Name "Terminator1"
SID "713:16"
Position [675, 75, 695, 95]
}
Block {
BlockType SubSystem
Name "calcGravTrq"
SID "713:1"
Ports [4, 4]
Position [305, 34, 600, 466]
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
"rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "calcGravTrq"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "46"
Block {
BlockType Inport
Name "q_motor"
SID "713:1::20"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "x_hat"
SID "713:1::21"
Position [20, 136, 40, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "gearRatio"
SID "713:1::22"
Position [20, 171, 40, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "baseAngles"
SID "713:1::24"
Position [20, 206, 40, 224]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "713:1::43"
Ports [1, 1]
Position [270, 245, 320, 285]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "713:1::42"
Tag "Stateflow S-Function scaled_teleoperation 16"
Ports [4, 5]
Position [180, 100, 230, 220]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[4 5]"
EnableBusSupport on
Port {
PortNumber 2
Name "gravTrq"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "gravTrqMotor"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 4
Name "coulombWidthMotor"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 5
Name "coulombWidth"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "713:1::45"
Position [460, 256, 480, 274]
ZOrder 8
}
Block {
BlockType Outport
Name "gravTrq"
SID "713:1::25"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "gravTrqMotor"
SID "713:1::26"
Position [460, 136, 480, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "coulombWidthMotor"
SID "713:1::27"
Position [460, 171, 480, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "coulombWidth"
SID "713:1::46"
Position [460, 206, 480, 224]
ZOrder 9
Port "4"
IconDisplay "Port number"
}
Line {
SrcBlock "q_motor"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "x_hat"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
SrcBlock "gearRatio"
SrcPort 1
DstBlock " SFunction "
DstPort 3
}
Line {
SrcBlock "baseAngles"
SrcPort 1
DstBlock " SFunction "
DstPort 4
}
Line {
Name "gravTrq"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "gravTrq"
DstPort 1
}
Line {
Name "gravTrqMotor"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "gravTrqMotor"
DstPort 1
}
Line {
Name "coulombWidthMotor"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 4
DstBlock "coulombWidthMotor"
DstPort 1
}
Line {
Name "coulombWidth"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 5
DstBlock "coulombWidth"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType Outport
Name "gravTrqMotor"
SID "713:13"
Position [690, 188, 720, 202]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "coulombWidthMotor"
SID "713:43"
Position [680, 298, 710, 312]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "coulombWidthArm"
SID "713:940"
Position [690, 408, 720, 422]
Port "3"
IconDisplay "Port number"
}
Line {
SrcBlock "calcGravTrq"
SrcPort 3
DstBlock "coulombWidthMotor"
DstPort 1
}
Line {
SrcBlock "calcGravTrq"
SrcPort 2
DstBlock "gravTrqMotor"
DstPort 1
}
Line {
SrcBlock "baseAngles"
SrcPort 1
DstBlock "calcGravTrq"
DstPort 4
}
Line {
SrcBlock "gearRatio"
SrcPort 1
DstBlock "calcGravTrq"
DstPort 3
}
Line {
SrcBlock "calcGravTrq"
SrcPort 1
DstBlock "Terminator1"
DstPort 1
}
Line {
SrcBlock "x_hat"
SrcPort 1
DstBlock "calcGravTrq"
DstPort 2
}
Line {
SrcBlock "q_motor"
SrcPort 1
DstBlock "calcGravTrq"
DstPort 1
}
Line {
SrcBlock "calcGravTrq"
SrcPort 4
DstBlock "coulombWidthArm"
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "leadthroug control"
SID "714"
Ports [23, 5]
Position [920, 35, 1140, 835]
AncestorBlock "leadthroughLib/leadthroug control"
LibraryVersion "*1.87"
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
System {
Name "leadthroug control"
Location [1, 8, 1919, 822]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "941"
SIDPrevWatermark "939"
Block {
BlockType Inport
Name "Kp_in"
SID "714:19"
Position [225, 58, 255, 72]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "Kv_in"
SID "714:20"
Position [335, 83, 365, 97]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "Ki_in"
SID "714:21"
Position [390, 108, 420, 122]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "gravTrqMotor"
SID "714:22"
Position [320, 133, 350, 147]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "trqRaw"
SID "714:24"
Position [265, 158, 295, 172]
Port "5"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "posRef_in"
SID "714:25"
Position [225, 183, 255, 197]
Port "6"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "posRaw"
SID "714:26"
Position [300, 208, 330, 222]
Port "7"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "f_switch"
SID "714:28"
Position [355, 233, 385, 247]
Port "8"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "velFlt_filtered"
SID "714:29"
Position [165, 258, 195, 272]
Port "9"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "velFlt1"
SID "714:30"
Position [160, 333, 190, 347]
Port "10"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "keepStill"
SID "714:32"
Position [345, 308, 375, 322]
Port "11"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "coulomb_width"
SID "714:34"
Position [235, 358, 265, 372]
Port "12"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "percentageTrqFfw"
SID "714:36"
Position [305, 383, 335, 397]
Port "13"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "KiHighThres"
SID "714:37"
Position [370, 408, 400, 422]
Port "14"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "slidingThresWidth"
SID "714:38"
Position [430, 433, 460, 447]
Port "15"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "maxRate"
SID "714:40"
Position [235, 458, 265, 472]
Port "16"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "highKiActive"
SID "714:42"
Position [285, 283, 315, 297]
Port "17"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "resetPosRef"
SID "714:45"
Position [370, 493, 400, 507]
Port "18"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "jointLimitsStartMotorRad"
SID "714:46"
Position [370, 528, 400, 542]
Port "19"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "extTorque"
SID "714:116"
Position [370, 618, 400, 632]
Port "20"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "sensorOn"
SID "714:117"
Position [370, 578, 400, 592]
Port "21"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "fricHelp"
SID "714:118"
Position [370, 673, 400, 687]
Port "22"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "qtorgue"
SID "714:941"
Position [360, 728, 390, 742]
Port "23"
IconDisplay "Port number"
}
Block {
BlockType SubSystem
Name "MATLAB Function"
SID "714:17"
Ports [23, 6]
Position [540, 48, 1050, 572]
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
"rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
Port {
PortNumber 6
Name "rstill[i]"
PropagatedSignals "rstill"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
System {
Name "MATLAB Function"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "83"
Block {
BlockType Inport
Name "Kp_in"
SID "714:17::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "Kv_in"
SID "714:17::18"
Position [20, 136, 40, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "Ki_in"
SID "714:17::19"
Position [20, 171, 40, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "gravTrqMotor"
SID "714:17::20"
Position [20, 206, 40, 224]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "trqRaw"
SID "714:17::21"
Position [20, 246, 40, 264]
Port "5"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "posRefIn"
SID "714:17::22"
Position [20, 281, 40, 299]
Port "6"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "posRaw"
SID "714:17::23"
Position [20, 316, 40, 334]
Port "7"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "f_switch"
SID "714:17::24"
Position [20, 351, 40, 369]
Port "8"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "velFlt"
SID "714:17::25"
Position [20, 386, 40, 404]
Port "9"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "highKiActive"
SID "714:17::26"
Position [20, 426, 40, 444]
Port "10"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "keepStill"
SID "714:17::27"
Position [20, 461, 40, 479]
Port "11"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "velFlt_noExtraFilt"
SID "714:17::28"
Position [20, 496, 40, 514]
Port "12"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "coulomb_width"
SID "714:17::34"
Position [20, 531, 40, 549]
Port "13"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "percentageTrqFfw"
SID "714:17::35"
Position [20, 566, 40, 584]
Port "14"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "KiHighThres"
SID "714:17::36"
Position [20, 606, 40, 624]
Port "15"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "slidingThresWidth"
SID "714:17::37"
Position [20, 641, 40, 659]
Port "16"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "maxRate"
SID "714:17::38"
Position [20, 676, 40, 694]
Port "17"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "resetPosRef"
SID "714:17::39"
Position [20, 711, 40, 729]
Port "18"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "jointLimitStartMotorRad"
SID "714:17::40"
Position [20, 746, 40, 764]
Port "19"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "extTorque"
SID "714:17::41"
Position [20, 786, 40, 804]
Port "20"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "sensorOn"
SID "714:17::42"
Position [20, 821, 40, 839]
Port "21"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "fricHelp"
SID "714:17::43"
Position [20, 856, 40, 874]
Port "22"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "qtorque"
SID "714:17::83"
Position [20, 891, 40, 909]
ZOrder 10
Port "23"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "714:17::79"
Ports [1, 1]
Position [270, 580, 320, 620]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "714:17::78"
Tag "Stateflow S-Function scaled_teleoperation 13"
Ports [23, 7]
Position [180, 100, 230, 580]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[23 7]"
EnableBusSupport on
Port {
PortNumber 2
Name "Kp"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "Kv"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 4
Name "Ki"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 5
Name "trqFfw"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 6
Name "posRef"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 7
Name "rstill"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "714:17::81"
Position [460, 591, 480, 609]
ZOrder 8
}
Block {
BlockType Outport
Name "Kp"
SID "714:17::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Kv"
SID "714:17::29"
Position [460, 136, 480, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Ki"
SID "714:17::30"
Position [460, 171, 480, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "trqFfw"
SID "714:17::31"
Position [460, 206, 480, 224]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "posRef"
SID "714:17::32"
Position [460, 246, 480, 264]
Port "5"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "rstill"
SID "714:17::82"
Position [460, 281, 480, 299]
ZOrder 9
Port "6"
IconDisplay "Port number"
}
Line {
SrcBlock "Kp_in"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "Kv_in"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
SrcBlock "Ki_in"
SrcPort 1
DstBlock " SFunction "
DstPort 3
}
Line {
SrcBlock "gravTrqMotor"
SrcPort 1
DstBlock " SFunction "
DstPort 4
}
Line {
SrcBlock "trqRaw"
SrcPort 1
DstBlock " SFunction "
DstPort 5
}
Line {
SrcBlock "posRefIn"
SrcPort 1
DstBlock " SFunction "
DstPort 6
}
Line {
SrcBlock "posRaw"
SrcPort 1
DstBlock " SFunction "
DstPort 7
}
Line {
SrcBlock "f_switch"
SrcPort 1
DstBlock " SFunction "
DstPort 8
}
Line {
SrcBlock "velFlt"
SrcPort 1
DstBlock " SFunction "
DstPort 9
}
Line {
SrcBlock "highKiActive"
SrcPort 1
DstBlock " SFunction "
DstPort 10
}
Line {
SrcBlock "keepStill"
SrcPort 1
DstBlock " SFunction "
DstPort 11
}
Line {
SrcBlock "velFlt_noExtraFilt"
SrcPort 1
DstBlock " SFunction "
DstPort 12
}
Line {
SrcBlock "coulomb_width"
SrcPort 1
DstBlock " SFunction "
DstPort 13
}
Line {
SrcBlock "percentageTrqFfw"
SrcPort 1
DstBlock " SFunction "
DstPort 14
}
Line {
SrcBlock "KiHighThres"
SrcPort 1
DstBlock " SFunction "
DstPort 15
}
Line {
SrcBlock "slidingThresWidth"
SrcPort 1
DstBlock " SFunction "
DstPort 16
}
Line {
SrcBlock "maxRate"
SrcPort 1
DstBlock " SFunction "
DstPort 17
}
Line {
SrcBlock "resetPosRef"
SrcPort 1
DstBlock " SFunction "
DstPort 18
}
Line {
SrcBlock "jointLimitStartMotorRad"
SrcPort 1
DstBlock " SFunction "
DstPort 19
}
Line {
SrcBlock "extTorque"
SrcPort 1
DstBlock " SFunction "
DstPort 20
}
Line {
SrcBlock "sensorOn"
SrcPort 1
DstBlock " SFunction "
DstPort 21
}
Line {
SrcBlock "fricHelp"
SrcPort 1
DstBlock " SFunction "
DstPort 22
}
Line {
SrcBlock "qtorque"
SrcPort 1
DstBlock " SFunction "
DstPort 23
}
Line {
Name "Kp"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "Kp"
DstPort 1
}
Line {
Name "Kv"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "Kv"
DstPort 1
}
Line {
Name "Ki"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 4
DstBlock "Ki"
DstPort 1
}
Line {
Name "trqFfw"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 5
DstBlock "trqFfw"
DstPort 1
}
Line {
Name "posRef"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 6
DstBlock "posRef"
DstPort 1
}
Line {
Name "rstill"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 7
DstBlock "rstill"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType Terminator
Name "Terminator11"
SID "714:940"
Position [1190, 515, 1210, 535]
}
Block {
BlockType Outport
Name "Kp"
SID "714:23"
Position [1305, 78, 1335, 92]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Kv"
SID "714:27"
Position [1290, 168, 1320, 182]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Ki"
SID "714:31"
Position [1300, 258, 1330, 272]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "trqFfw"
SID "714:35"
Position [1300, 348, 1330, 362]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "posRef"
SID "714:39"
Position [1300, 438, 1330, 452]
Port "5"
IconDisplay "Port number"
}
Line {
SrcBlock "fricHelp"
SrcPort 1
Points [105, 0; 0, -170]
DstBlock "MATLAB Function"
DstPort 22
}
Line {
SrcBlock "extTorque"
SrcPort 1
Points [100, 0; 0, -155]
DstBlock "MATLAB Function"
DstPort 20
}
Line {
SrcBlock "sensorOn"
SrcPort 1
Points [60, 0; 0, -95]
DstBlock "MATLAB Function"
DstPort 21
}
Line {
SrcBlock "jointLimitsStartMotorRad"
SrcPort 1
Points [60, 0; 0, -85]
DstBlock "MATLAB Function"
DstPort 19
}
Line {
SrcBlock "resetPosRef"
SrcPort 1
Points [120, 0]
DstBlock "MATLAB Function"
DstPort 18
}
Line {
SrcBlock "highKiActive"
SrcPort 1
Points [205, 0]
DstBlock "MATLAB Function"
DstPort 10
}
Line {
SrcBlock "velFlt1"
SrcPort 1
Points [330, 0]
DstBlock "MATLAB Function"
DstPort 12
}
Line {
SrcBlock "velFlt_filtered"
SrcPort 1
Points [325, 0]
DstBlock "MATLAB Function"
DstPort 9
}
Line {
SrcBlock "maxRate"
SrcPort 1
Points [255, 0]
DstBlock "MATLAB Function"
DstPort 17
}
Line {
SrcBlock "MATLAB Function"
SrcPort 5
DstBlock "posRef"
DstPort 1
}
Line {
SrcBlock "slidingThresWidth"
SrcPort 1
Points [60, 0]
DstBlock "MATLAB Function"
DstPort 16
}
Line {
SrcBlock "KiHighThres"
SrcPort 1
Points [120, 0]
DstBlock "MATLAB Function"
DstPort 15
}
Line {
SrcBlock "percentageTrqFfw"
SrcPort 1
Points [185, 0]
DstBlock "MATLAB Function"
DstPort 14
}
Line {
SrcBlock "MATLAB Function"
SrcPort 4
Points [0, -5]
DstBlock "trqFfw"
DstPort 1
}
Line {
SrcBlock "coulomb_width"
SrcPort 1
Points [255, 0]
DstBlock "MATLAB Function"
DstPort 13
}
Line {
SrcBlock "keepStill"
SrcPort 1
Points [145, 0]
DstBlock "MATLAB Function"
DstPort 11
}
Line {
SrcBlock "MATLAB Function"
SrcPort 3
Points [0, -10]
DstBlock "Ki"
DstPort 1
}
Line {
SrcBlock "f_switch"
SrcPort 1
Points [135, 0]
DstBlock "MATLAB Function"
DstPort 8
}
Line {
SrcBlock "MATLAB Function"
SrcPort 2
Points [0, -15]
DstBlock "Kv"
DstPort 1
}
Line {
SrcBlock "posRaw"
SrcPort 1
Points [190, 0]
DstBlock "MATLAB Function"
DstPort 7
}
Line {
SrcBlock "posRef_in"
SrcPort 1
DstBlock "MATLAB Function"
DstPort 6
}
Line {
SrcBlock "trqRaw"
SrcPort 1
Points [225, 0]
DstBlock "MATLAB Function"
DstPort 5
}
Line {
SrcBlock "MATLAB Function"
SrcPort 1
Points [0, -20]
DstBlock "Kp"
DstPort 1
}
Line {
SrcBlock "gravTrqMotor"
SrcPort 1
Points [0, 15; 170, 0]
DstBlock "MATLAB Function"
DstPort 4
}
Line {
SrcBlock "Ki_in"
SrcPort 1
Points [100, 0]
DstBlock "MATLAB Function"
DstPort 3
}
Line {
SrcBlock "Kv_in"
SrcPort 1
Points [155, 0]
DstBlock "MATLAB Function"
DstPort 2
}
Line {
SrcBlock "Kp_in"
SrcPort 1
Points [265, 0]
DstBlock "MATLAB Function"
DstPort 1
}
Line {
Name "rstill[i]"
Labels [0, 0]
SrcBlock "MATLAB Function"
SrcPort 6
DstBlock "Terminator11"
DstPort 1
}
Line {
SrcBlock "qtorgue"
SrcPort 1
Points [130, 0]
DstBlock "MATLAB Function"
DstPort 23
}
}
}
Block {
BlockType Reference
Name "leadthroughLockOutput"
SID "727"
Ports [3, 2]
Position [1535, 745, 1665, 815]
ForegroundColor "blue"
LibraryVersion "1.5"
SourceBlock "rapid_extctrl_handover_lib_leadthrough/leadthroughLockOutput"
SourceType "Stateflow"
}
Block {
BlockType Outport
Name "coulombWidthArmRightAbb"
SID "1362"
Position [670, 298, 700, 312]
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.joint[i].parKp"
SID "679"
Position [1200, 98, 1230, 112]
Port "2"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.joint[i].parKv"
SID "680"
Position [1200, 258, 1230, 272]
Port "3"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.joint[i].parKi"
SID "681"
Position [1200, 418, 1230, 432]
Port "4"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.joint[i].velRef"
SID "683"
Position [1755, 793, 1785, 807]
Port "5"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.joint[i].trqFfw"
SID "684"
Position [1320, 588, 1350, 602]
Port "6"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.instruction"
SID "685"
Position [235, 778, 265, 792]
Port "7"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.value1"
SID "686"
Position [290, 838, 320, 852]
Port "8"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.value2"
SID "687"
Position [290, 873, 320, 887]
Port "9"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.value3"
SID "688"
Position [290, 908, 320, 922]
Port "10"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.value4"
SID "689"
Position [290, 943, 320, 957]
Port "11"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.value5"
SID "690"
Position [290, 978, 320, 992]
Port "12"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.value6"
SID "691"
Position [290, 1013, 320, 1027]
Port "13"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.string1[i]"
SID "692"
Position [290, 1048, 320, 1062]
Port "14"
IconDisplay "Port number"
PortDimensions "20"
}
Block {
BlockType Outport
Name "ext2irb.mocgendata.string2[i]"
SID "693"
Position [290, 1083, 320, 1097]
Port "15"
IconDisplay "Port number"
PortDimensions "20"
}
Block {
BlockType Outport
Name "ext2irb.joint[i].posRef"
SID "1252"
Position [1755, 758, 1785, 772]
Port "16"
IconDisplay "Port number"
PortDimensions "DOF"
}
Line {
SrcBlock "irb2ext.joint[i].parTrqMin"
SrcPort 1
DstBlock "Terminator"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].parTrqMax"
SrcPort 1
DstBlock "Terminator1"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].trqFfwGrav"
SrcPort 1
DstBlock "Terminator2"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].posFlt"
SrcPort 1
DstBlock "Terminator5"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].velOut"
SrcPort 1
DstBlock "aTerminator8"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].trqRefFlt"
SrcPort 1
DstBlock "Terminator10"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.instruction"
SrcPort 1
DstBlock "ext2irb.mocgendata.instruction"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.string1[i]"
SrcPort 1
DstBlock "ext2irb.mocgendata.string1[i]"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.string2[i]"
SrcPort 1
DstBlock "ext2irb.mocgendata.string2[i]"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.value2"
SrcPort 1
DstBlock "ext2irb.mocgendata.value2"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.value3"
SrcPort 1
DstBlock "ext2irb.mocgendata.value3"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.value4"
SrcPort 1
DstBlock "ext2irb.mocgendata.value4"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.value5"
SrcPort 1
DstBlock "ext2irb.mocgendata.value5"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.value6"
SrcPort 1
DstBlock "ext2irb.mocgendata.value6"
DstPort 1
}
Line {
SrcBlock "irb2ext.mocgendata.value1"
SrcPort 1
DstBlock "ext2irb.mocgendata.value1"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].posRawFb"
SrcPort 1
Points [50, 0]
Branch {
Points [0, -65; 310, -10; 15, 125; 175, 5]
DstBlock "leadthroug control"
DstPort 7
}
Branch {
DstBlock "calcGravityTorque"
DstPort 1
}
}
Line {
SrcBlock "Constant6"
SrcPort 1
DstBlock "calcGravityTorque"
DstPort 4
}
Line {
SrcBlock "Constant9"
SrcPort 1
DstBlock "calcGravityTorque"
DstPort 3
}
Line {
SrcBlock "Constant5"
SrcPort 1
DstBlock "calcGravityTorque"
DstPort 2
}
Line {
SrcBlock "irb2ext.joint[i].posRef"
SrcPort 1
Points [40, -15]
DstBlock "leadthroug control"
DstPort 6
}
Line {
SrcBlock "calcGravityTorque"
SrcPort 2
Points [105, 0; 0, 310]
DstBlock "leadthroug control"
DstPort 12
}
Line {
SrcBlock "irb2ext.joint[i].trqFfw"
SrcPort 1
DstBlock "Terminator4"
DstPort 1
}
Line {
SrcBlock "Constant2"
SrcPort 1
DstBlock "leadthroug control"
DstPort 17
}
Line {
SrcBlock "Constant1"
SrcPort 1
DstBlock "leadthroug control"
DstPort 16
}
Line {
SrcBlock "Constant4"
SrcPort 1
DstBlock "leadthroug control"
DstPort 15
}
Line {
SrcBlock "Constant7"
SrcPort 1
DstBlock "leadthroug control"
DstPort 14
}
Line {
SrcBlock "Constant8"
SrcPort 1
DstBlock "leadthroug control"
DstPort 13
}
Line {
SrcBlock "Discrete Filter3"
SrcPort 1
Points [15, -30]
DstBlock "leadthroug control"
DstPort 9
}
Line {
SrcBlock "leadthroug control"
SrcPort 3
Points [0, -15]
DstBlock "ext2irb.joint[i].parKi"
DstPort 1
}
Line {
SrcBlock "leadthroug control"
SrcPort 2
Points [0, -15]
DstBlock "ext2irb.joint[i].parKv"
DstPort 1
}
Line {
SrcBlock "leadthroug control"
SrcPort 1
Points [0, -15]
DstBlock "ext2irb.joint[i].parKp"
DstPort 1
}
Line {
SrcBlock "irb2ext.joint[i].parKi"
SrcPort 1
DstBlock "leadthroug control"
DstPort 3
}
Line {
SrcBlock "irb2ext.joint[i].parKv"
SrcPort 1
DstBlock "leadthroug control"
DstPort 2
}
Line {
SrcBlock "irb2ext.joint[i].parKp"
SrcPort 1
Points [50, 0]
DstBlock "leadthroug control"
DstPort 1
}
Line {
SrcBlock "Constant3"
SrcPort 1
DstBlock "leadthroug control"
DstPort 11
}
Line {
SrcBlock "Gain1"
SrcPort 1
DstBlock "Terminator7"
DstPort 1
}
Line {
SrcBlock "MATLAB Function3"
SrcPort 4
Points [0, 15]
DstBlock "Gain"
DstPort 1
}
Line {
SrcBlock "Gain"
SrcPort 1
DstBlock "Gain1"
DstPort 1
}
Line {
SrcBlock "Product"
SrcPort 1
DstBlock "Goto"
DstPort 1
}
Line {
SrcBlock "From"
SrcPort 1
DstBlock "leadthroug control"
DstPort 8
}
Line {
SrcBlock "leadthroug control"
SrcPort 5
DstBlock "leadthroughLockOutput"
DstPort 1
}
Line {
SrcBlock "MATLAB Function3"
SrcPort 2
Points [0, 5; 35, 0]
Branch {
Points [0, -155]
DstBlock "leadthroug control"
DstPort 18
}
Branch {
Points [700, 0]
DstBlock "leadthroughLockOutput"
DstPort 3
}
}
Line {
SrcBlock "MATLAB Function3"
SrcPort 3
Points [0, 10]
DstBlock "Terminator8"
DstPort 1
}
Line {
SrcBlock "Constant11"
SrcPort 1
Points [135, 0; 0, -125]
DstBlock "leadthroug control"
DstPort 19
}
Line {
SrcBlock "irb2ext.manualMode"
SrcPort 1
DstBlock "Terminator11"
DstPort 1
}
Line {
SrcBlock "MATLAB Function3"
SrcPort 1
DstBlock "Product"
DstPort 2
}
Line {
SrcBlock "f_switch"
SrcPort 1
Points [15, 0; 0, 35]
DstBlock "Product"
DstPort 1
}
Line {
SrcBlock "irb2ext.controlActive"
SrcPort 1
DstBlock "MATLAB Function3"
DstPort 1
}
Line {
SrcBlock "irb2ext.obtaining"
SrcPort 1
DstBlock "MATLAB Function3"
DstPort 2
}
Line {
SrcBlock "Constant10"
SrcPort 1
DstBlock "MATLAB Function3"
DstPort 3
}
Line {
SrcBlock "irb2ext.joint[i].velRef"
SrcPort 1
DstBlock "leadthroughLockOutput"
DstPort 2
}
Line {
SrcBlock "leadthroughLockOutput"
SrcPort 1
DstBlock "ext2irb.joint[i].posRef"
DstPort 1
}
Line {
SrcBlock "leadthroughLockOutput"
SrcPort 2
DstBlock "ext2irb.joint[i].velRef"
DstPort 1
}
Line {
SrcBlock "Constant"
SrcPort 1
Points [170, 0; 0, -95]
DstBlock "leadthroug control"
DstPort 22
}
Line {
SrcBlock "irb2ext.joint[i].velFlt"
SrcPort 1
Points [0, -5; 10, 0]
Branch {
Points [-10, 0]
DstBlock "Discrete Filter3"
DstPort 1
}
Branch {
Points [0, -10]
DstBlock "leadthroug control"
DstPort 10
}
}
Line {
SrcBlock "irb2ext.joint[i].trqRaw"
SrcPort 1
Points [35, 0]
DstBlock "leadthroug control"
DstPort 5
}
Line {
SrcBlock "leadthroug control"
SrcPort 4
DstBlock "ext2irb.joint[i].trqFfw"
DstPort 1
}
Line {
SrcBlock "calcGravityTorque"
SrcPort 1
Points [145, 0; 0, -10; 120, 0]
DstBlock "leadthroug control"
DstPort 4
}
Line {
SrcBlock "calcGravityTorque"
SrcPort 3
DstBlock "coulombWidthArmRightAbb"
DstPort 1
}
Line {
SrcBlock "torque_motor_1"
SrcPort 1
Points [430, 0]
DstBlock "leadthroug control"
DstPort 23
}
}
}
Block {
BlockType SubSystem
Name "botharms_get_torque"
SID "1345"
Ports [10, 2]
Position [800, 22, 1010, 218]
AncestorBlock "frida_library/FRIDA arm acceleration"
LibraryVersion "*1.21"
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
Port {
PortNumber 1
Name "torque1[i]"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 2
Name "torque2[i]"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
System {
Name "botharms_get_torque"
Location [0, 18, 1920, 1126]
Open on
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "497"
SIDPrevWatermark "325"
Block {
BlockType Inport
Name "q_motor_1"
SID "1345:295"
Position [40, 83, 70, 97]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "qd_motor_1"
SID "1345:299"
Position [30, 318, 60, 332]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "q_motor_2"
SID "1345:409"
Position [30, 488, 60, 502]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "qd_motor_2"
SID "1345:410"
Position [15, 363, 45, 377]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "coulombWidthArmRightAbb"
SID "1345:433"
Position [1075, 368, 1105, 382]
Port "5"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "coulombWidthArmLeftAbb"
SID "1345:434"
Position [1235, 673, 1265, 687]
Port "6"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "velFlt1"
SID "1345:450"
Position [1220, 433, 1250, 447]
Port "7"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "velFlt2"
SID "1345:451"
Position [1220, 503, 1250, 517]
Port "8"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "wrench1"
SID "1345:486"
Position [240, 883, 270, 897]
Port "9"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "wrench2"
SID "1345:487"
Position [225, 993, 255, 1007]
Port "10"
IconDisplay "Port number"
}
Block {
BlockType Constant
Name "Constant1"
SID "1345:412"
Position [395, 529, 455, 561]
ZOrder -5
Value "zeros(7,1)"
}
Block {
BlockType Constant
Name "Constant2"
SID "1345:377"
Position [405, 124, 465, 156]
ZOrder -5
Value "zeros(7,1)"
}
Block {
BlockType Demux
Name "Demux"
SID "1345:432"
Ports [1, 2]
Position [175, 321, 180, 359]
ZOrder -7
ShowName off
Outputs "2"
DisplayOption "bar"
}
Block {
BlockType SubSystem
Name "FRIDA arm acceleration NOT SI"
SID "1345:348"
Ports [2, 3]
Position [335, 200, 440, 290]
AncestorBlock "frida_library/FRIDA arm acceleration"
LibraryVersion "*1.21"
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
System {
Name "FRIDA arm acceleration NOT SI"
Location [338, 225, 1510, 661]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "328"
Block {
BlockType Inport
Name "q_arm1"
SID "1345:348:295"
Position [120, 83, 150, 97]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "qd_arm1"
SID "1345:348:299"
Position [120, 168, 150, 182]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Reference
Name "Base transformation right arm (rob1)"
SID "1345:348:328"
Ports [0, 1]
Position [340, 16, 495, 54]
ForegroundColor "blue"
LibraryVersion "1.21"
SourceBlock "frida_library/Base transformation right arm (rob1)"
SourceType "Stateflow"
}
Block {
BlockType Constant
Name "T44_flange->tool"
SID "1345:348:327"
Position [430, 67, 505, 93]
BackgroundColor "cyan"
Value "[1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1]'"
VectorParams1D off
}
Block {
BlockType Terminator
Name "Terminator"
SID "1345:348:290"
Position [410, 150, 430, 170]
ShowName off
}
Block {
BlockType Terminator
Name "Terminator1"
SID "1345:348:303"
Position [740, 140, 760, 160]
ShowName off
}
Block {
BlockType SubSystem
Name "base_tool_transform"
SID "1345:348:289"
Ports [6, 4]
Position [555, 30, 710, 220]
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
"rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'base_tool_transform']"
");"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "base_tool_transform"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "33"
Block {
BlockType Inport
Name "T44_world2base"
SID "1345:348:289::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "T44_flange2tool"
SID "1345:348:289::18"
Position [20, 136, 40, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "T44_in"
SID "1345:348:289::19"
Position [20, 171, 40, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "J0_in"
SID "1345:348:289::20"
Position [20, 206, 40, 224]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "qd"
SID "1345:348:289::21"
Position [20, 246, 40, 264]
Port "5"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "Jdot0_in"
SID "1345:348:289::22"
Position [20, 281, 40, 299]
Port "6"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:348:289::31"
Ports [1, 1]
Position [270, 245, 320, 285]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:348:289::30"
Tag "Stateflow S-Function scaled_teleoperation 8"
Ports [6, 5]
Position [180, 102, 230, 243]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[6 5]"
EnableBusSupport on
Port {
PortNumber 2
Name "T44_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "J0_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 4
Name "Jn_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 5
Name "Jdot0_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:348:289::33"
Position [460, 256, 480, 274]
ZOrder 8
}
Block {
BlockType Outport
Name "T44_out"
SID "1345:348:289::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "J0_out"
SID "1345:348:289::23"
Position [460, 136, 480, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Jn_out"
SID "1345:348:289::24"
Position [460, 171, 480, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Jdot0_out"
SID "1345:348:289::25"
Position [460, 206, 480, 224]
Port "4"
IconDisplay "Port number"
}
Line {
SrcBlock "T44_world2base"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "T44_flange2tool"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
SrcBlock "T44_in"
SrcPort 1
DstBlock " SFunction "
DstPort 3
}
Line {
SrcBlock "J0_in"
SrcPort 1
DstBlock " SFunction "
DstPort 4
}
Line {
SrcBlock "qd"
SrcPort 1
DstBlock " SFunction "
DstPort 5
}
Line {
SrcBlock "Jdot0_in"
SrcPort 1
DstBlock " SFunction "
DstPort 6
}
Line {
Name "T44_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "T44_out"
DstPort 1
}
Line {
Name "J0_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "J0_out"
DstPort 1
}
Line {
Name "Jn_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 4
DstBlock "Jn_out"
DstPort 1
}
Line {
Name "Jdot0_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 5
DstBlock "Jdot0_out"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "frida_kin"
SID "1345:348:288"
Ports [2, 4]
Position [275, 56, 385, 224]
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
"rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'frida_kinematics']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "frida_kin"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "29"
Block {
BlockType Inport
Name "q"
SID "1345:348:288::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "qd"
SID "1345:348:288::18"
Position [20, 136, 40, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:348:288::27"
Ports [1, 1]
Position [270, 245, 320, 285]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:348:288::26"
Tag "Stateflow S-Function scaled_teleoperation 1"
Ports [2, 5]
Position [180, 100, 230, 220]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[2 5]"
EnableBusSupport on
Port {
PortNumber 2
Name "T44_fkine"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "J0"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 4
Name "Jn"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 5
Name "Jdot0"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:348:288::29"
Position [460, 256, 480, 274]
ZOrder 8
}
Block {
BlockType Outport
Name "T44_fkine"
SID "1345:348:288::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "J0"
SID "1345:348:288::19"
Position [460, 136, 480, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Jn"
SID "1345:348:288::20"
Position [460, 171, 480, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Jdot0"
SID "1345:348:288::21"
Position [460, 206, 480, 224]
Port "4"
IconDisplay "Port number"
}
Line {
SrcBlock "q"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "qd"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
Name "T44_fkine"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "T44_fkine"
DstPort 1
}
Line {
Name "J0"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "J0"
DstPort 1
}
Line {
Name "Jn"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 4
DstBlock "Jn"
DstPort 1
}
Line {
Name "Jdot0"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 5
DstBlock "Jdot0"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType Outport
Name "T44_FK"
SID "1345:348:293"
Position [735, 53, 765, 67]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "J0"
SID "1345:348:296"
Position [735, 98, 765, 112]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Jdot0"
SID "1345:348:300"
Position [735, 188, 765, 202]
Port "3"
IconDisplay "Port number"
}
Line {
SrcBlock "base_tool_transform"
SrcPort 4
DstBlock "Jdot0"
DstPort 1
}
Line {
SrcBlock "qd_arm1"
SrcPort 1
Points [75, 0]
Branch {
Points [30, 0]
DstBlock "frida_kin"
DstPort 2
}
Branch {
Points [0, 80; 205, 0; 0, -85]
DstBlock "base_tool_transform"
DstPort 5
}
}
Line {
SrcBlock "base_tool_transform"
SrcPort 3
DstBlock "Terminator1"
DstPort 1
}
Line {
SrcBlock "base_tool_transform"
SrcPort 2
DstBlock "J0"
DstPort 1
}
Line {
SrcBlock "q_arm1"
SrcPort 1
Points [105, 0]
DstBlock "frida_kin"
DstPort 1
}
Line {
SrcBlock "base_tool_transform"
SrcPort 1
DstBlock "T44_FK"
DstPort 1
}
Line {
SrcBlock "Base transformation right arm (rob1)"
SrcPort 1
Points [0, 15]
DstBlock "base_tool_transform"
DstPort 1
}
Line {
SrcBlock "frida_kin"
SrcPort 4
DstBlock "base_tool_transform"
DstPort 6
}
Line {
SrcBlock "frida_kin"
SrcPort 3
DstBlock "Terminator"
DstPort 1
}
Line {
SrcBlock "frida_kin"
SrcPort 2
Points [55, 0; 0, 20]
DstBlock "base_tool_transform"
DstPort 4
}
Line {
SrcBlock "frida_kin"
SrcPort 1
Points [25, 0; 0, 30]
DstBlock "base_tool_transform"
DstPort 3
}
Line {
SrcBlock "T44_flange->tool"
SrcPort 1
DstBlock "base_tool_transform"
DstPort 2
}
}
}
Block {
BlockType SubSystem
Name "FRIDA arm acceleration NOT SI1"
SID "1345:413"
Ports [2, 3]
Position [350, 605, 455, 695]
AncestorBlock "frida_library/FRIDA arm acceleration"
LibraryVersion "*1.21"
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
System {
Name "FRIDA arm acceleration NOT SI1"
Location [134, 23, 1077, 362]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "329"
Block {
BlockType Inport
Name "q_arm1"
SID "1345:413:295"
Position [120, 83, 150, 97]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "qd_arm1"
SID "1345:413:299"
Position [120, 168, 150, 182]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Reference
Name "Base transformation left arm (rob2)"
SID "1345:413:329"
Ports [0, 1]
Position [305, 15, 455, 55]
ForegroundColor "blue"
LibraryVersion "1.21"
SourceBlock "frida_library/Base transformation left arm (rob2)"
SourceType "Stateflow"
}
Block {
BlockType Constant
Name "T44_flange->tool"
SID "1345:413:327"
Position [430, 67, 505, 93]
BackgroundColor "cyan"
Value "[1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1]'"
VectorParams1D off
}
Block {
BlockType Terminator
Name "Terminator"
SID "1345:413:290"
Position [410, 150, 430, 170]
ShowName off
}
Block {
BlockType Terminator
Name "Terminator1"
SID "1345:413:303"
Position [740, 140, 760, 160]
ShowName off
}
Block {
BlockType SubSystem
Name "base_tool_transform"
SID "1345:413:289"
Ports [6, 4]
Position [555, 30, 710, 220]
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
"rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'base_tool_transform']"
");"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "base_tool_transform"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "33"
Block {
BlockType Inport
Name "T44_world2base"
SID "1345:413:289::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "T44_flange2tool"
SID "1345:413:289::18"
Position [20, 136, 40, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "T44_in"
SID "1345:413:289::19"
Position [20, 171, 40, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "J0_in"
SID "1345:413:289::20"
Position [20, 206, 40, 224]
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "qd"
SID "1345:413:289::21"
Position [20, 246, 40, 264]
Port "5"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "Jdot0_in"
SID "1345:413:289::22"
Position [20, 281, 40, 299]
Port "6"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:413:289::31"
Ports [1, 1]
Position [270, 245, 320, 285]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:413:289::30"
Tag "Stateflow S-Function scaled_teleoperation 29"
Ports [6, 5]
Position [180, 102, 230, 243]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[6 5]"
EnableBusSupport on
Port {
PortNumber 2
Name "T44_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "J0_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 4
Name "Jn_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 5
Name "Jdot0_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:413:289::33"
Position [460, 256, 480, 274]
ZOrder 8
}
Block {
BlockType Outport
Name "T44_out"
SID "1345:413:289::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "J0_out"
SID "1345:413:289::23"
Position [460, 136, 480, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Jn_out"
SID "1345:413:289::24"
Position [460, 171, 480, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Jdot0_out"
SID "1345:413:289::25"
Position [460, 206, 480, 224]
Port "4"
IconDisplay "Port number"
}
Line {
SrcBlock "T44_world2base"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "T44_flange2tool"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
SrcBlock "T44_in"
SrcPort 1
DstBlock " SFunction "
DstPort 3
}
Line {
SrcBlock "J0_in"
SrcPort 1
DstBlock " SFunction "
DstPort 4
}
Line {
SrcBlock "qd"
SrcPort 1
DstBlock " SFunction "
DstPort 5
}
Line {
SrcBlock "Jdot0_in"
SrcPort 1
DstBlock " SFunction "
DstPort 6
}
Line {
Name "T44_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "T44_out"
DstPort 1
}
Line {
Name "J0_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "J0_out"
DstPort 1
}
Line {
Name "Jn_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 4
DstBlock "Jn_out"
DstPort 1
}
Line {
Name "Jdot0_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 5
DstBlock "Jdot0_out"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "frida_kin"
SID "1345:413:288"
Ports [2, 4]
Position [275, 56, 385, 224]
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
"rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'frida_kinematics']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "frida_kin"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "29"
Block {
BlockType Inport
Name "q"
SID "1345:413:288::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "qd"
SID "1345:413:288::18"
Position [20, 136, 40, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:413:288::27"
Ports [1, 1]
Position [270, 245, 320, 285]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:413:288::26"
Tag "Stateflow S-Function scaled_teleoperation 30"
Ports [2, 5]
Position [180, 100, 230, 220]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[2 5]"
EnableBusSupport on
Port {
PortNumber 2
Name "T44_fkine"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "J0"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 4
Name "Jn"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 5
Name "Jdot0"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:413:288::29"
Position [460, 256, 480, 274]
ZOrder 8
}
Block {
BlockType Outport
Name "T44_fkine"
SID "1345:413:288::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "J0"
SID "1345:413:288::19"
Position [460, 136, 480, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Jn"
SID "1345:413:288::20"
Position [460, 171, 480, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Jdot0"
SID "1345:413:288::21"
Position [460, 206, 480, 224]
Port "4"
IconDisplay "Port number"
}
Line {
SrcBlock "q"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "qd"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
Name "T44_fkine"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "T44_fkine"
DstPort 1
}
Line {
Name "J0"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "J0"
DstPort 1
}
Line {
Name "Jn"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 4
DstBlock "Jn"
DstPort 1
}
Line {
Name "Jdot0"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 5
DstBlock "Jdot0"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType Outport
Name "T44_FK"
SID "1345:413:293"
Position [735, 53, 765, 67]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "J0"
SID "1345:413:296"
Position [735, 98, 765, 112]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Jdot0"
SID "1345:413:300"
Position [735, 188, 765, 202]
Port "3"
IconDisplay "Port number"
}
Line {
SrcBlock "base_tool_transform"
SrcPort 4
DstBlock "Jdot0"
DstPort 1
}
Line {
SrcBlock "qd_arm1"
SrcPort 1
Points [75, 0]
Branch {
Points [30, 0]
DstBlock "frida_kin"
DstPort 2
}
Branch {
Points [0, 80; 205, 0; 0, -85]
DstBlock "base_tool_transform"
DstPort 5
}
}
Line {
SrcBlock "base_tool_transform"
SrcPort 3
DstBlock "Terminator1"
DstPort 1
}
Line {
SrcBlock "base_tool_transform"
SrcPort 2
DstBlock "J0"
DstPort 1
}
Line {
SrcBlock "q_arm1"
SrcPort 1
Points [105, 0]
DstBlock "frida_kin"
DstPort 1
}
Line {
SrcBlock "base_tool_transform"
SrcPort 1
DstBlock "T44_FK"
DstPort 1
}
Line {
SrcBlock "Base transformation left arm (rob2)"
SrcPort 1
Points [25, 0; 0, 15]
DstBlock "base_tool_transform"
DstPort 1
}
Line {
SrcBlock "frida_kin"
SrcPort 4
DstBlock "base_tool_transform"
DstPort 6
}
Line {
SrcBlock "frida_kin"
SrcPort 3
DstBlock "Terminator"
DstPort 1
}
Line {
SrcBlock "frida_kin"
SrcPort 2
Points [55, 0; 0, 20]
DstBlock "base_tool_transform"
DstPort 4
}
Line {
SrcBlock "frida_kin"
SrcPort 1
Points [25, 0; 0, 30]
DstBlock "base_tool_transform"
DstPort 3
}
Line {
SrcBlock "T44_flange->tool"
SrcPort 1
DstBlock "base_tool_transform"
DstPort 2
}
}
}
Block {
BlockType Reference
Name "FRIDA motor2arm"
SID "1345:335"
Ports [1, 1]
Position [210, 73, 280, 107]
LibraryVersion "1.21"
SourceBlock "frida_library/FRIDA motor2arm"
SourceType "SubSystem"
}
Block {
BlockType Reference
Name "FRIDA motor2arm1"
SID "1345:336"
Ports [1, 1]
Position [210, 160, 280, 190]
LibraryVersion "1.21"
SourceBlock "frida_library/FRIDA motor2arm"
SourceType "SubSystem"
}
Block {
BlockType Reference
Name "FRIDA motor2arm2"
SID "1345:337"
Ports [1, 1]
Position [1625, 92, 1720, 128]
LibraryVersion "1.21"
SourceBlock "frida_library/FRIDA motor2arm"
SourceType "SubSystem"
}
Block {
BlockType Reference
Name "FRIDA motor2arm3"
SID "1345:406"
Ports [1, 1]
Position [1490, 757, 1585, 793]
LibraryVersion "1.21"
SourceBlock "frida_library/FRIDA motor2arm"
SourceType "SubSystem"
}
Block {
BlockType Reference
Name "FRIDA motor2arm4"
SID "1345:414"
Ports [1, 1]
Position [200, 478, 270, 512]
LibraryVersion "1.21"
SourceBlock "frida_library/FRIDA motor2arm"
SourceType "SubSystem"
}
Block {
BlockType Reference
Name "FRIDA motor2arm5"
SID "1345:415"
Ports [1, 1]
Position [200, 565, 270, 595]
LibraryVersion "1.21"
SourceBlock "frida_library/FRIDA motor2arm"
SourceType "SubSystem"
}
Block {
BlockType Reference
Name "FRIDA motor2arm6"
SID "1345:452"
Ports [1, 1]
Position [1270, 428, 1305, 452]
LibraryVersion "1.21"
SourceBlock "frida_library/FRIDA motor2arm"
SourceType "SubSystem"
}
Block {
BlockType Reference
Name "FRIDA motor2arm7"
SID "1345:453"
Ports [1, 1]
Position [1270, 499, 1305, 521]
LibraryVersion "1.21"
SourceBlock "frida_library/FRIDA motor2arm"
SourceType "SubSystem"
}
Block {
BlockType Gain
Name "Gain"
SID "1345:439"
Position [1135, 360, 1165, 390]
ZOrder -8
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
Port {
PortNumber 1
Name "cright[i]"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Gain
Name "Gain1"
SID "1345:440"
Position [1295, 665, 1325, 695]
ZOrder -8
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
Port {
PortNumber 1
Name "cleft[i]"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Gain
Name "Gain10"
SID "1345:496"
Position [640, 895, 670, 925]
ZOrder -8
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
Port {
PortNumber 1
Name "w1[i]"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Gain
Name "Gain11"
SID "1345:497"
Position [640, 960, 670, 990]
ZOrder -8
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
Port {
PortNumber 1
Name "w2[i]"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Gain
Name "Gain2"
SID "1345:441"
Position [1555, 405, 1585, 435]
ZOrder -8
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
Port {
PortNumber 1
Name "help_right[i]"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Gain
Name "Gain3"
SID "1345:442"
Position [1605, 575, 1635, 605]
ZOrder -8
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
Port {
PortNumber 1
Name "help_left[i]"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Gain
Name "Gain4"
SID "1345:455"
Position [1235, 815, 1265, 845]
ZOrder -8
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
Port {
PortNumber 1
Name "diagm[i]"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Gain
Name "Gain5"
SID "1345:457"
Position [1375, 95, 1405, 125]
ZOrder -8
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
BlockType Gain
Name "Gain6"
SID "1345:458"
Position [1330, 755, 1360, 785]
ZOrder -8
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
BlockType Gain
Name "Gain7"
SID "1345:459"
Position [1355, 815, 1385, 845]
ZOrder -8
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
BlockType Gain
Name "Gain8"
SID "1345:461"
Position [1825, 95, 1850, 125]
ZOrder -8
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
BlockType Gain
Name "Gain9"
SID "1345:462"
Position [1725, 760, 1755, 790]
ZOrder -8
ParamDataTypeStr "Inherit: Inherit via internal rule"
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
BlockType SubSystem
Name "MATLAB Function1"
SID "1345:350"
Ports [2, 2]
Position [740, 818, 790, 922]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
Port {
PortNumber 1
Name "e[i]"
PropagatedSignals "e"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
System {
Name "MATLAB Function1"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "27"
Block {
BlockType Inport
Name "T44_FK1"
SID "1345:350::24"
Position [20, 101, 40, 119]
ZOrder 10
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "T44_FK2"
SID "1345:350::26"
Position [20, 136, 40, 154]
ZOrder 12
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:350::20"
Ports [1, 1]
Position [270, 205, 320, 245]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:350::19"
Tag "Stateflow S-Function scaled_teleoperation 11"
Ports [2, 3]
Position [180, 104, 230, 206]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[2 3]"
EnableBusSupport on
Port {
PortNumber 2
Name "e"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "scale"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:350::22"
Position [460, 216, 480, 234]
ZOrder 8
}
Block {
BlockType Outport
Name "e"
SID "1345:350::25"
Position [460, 101, 480, 119]
ZOrder 11
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "scale"
SID "1345:350::27"
Position [460, 136, 480, 154]
ZOrder 13
Port "2"
IconDisplay "Port number"
}
Line {
SrcBlock "T44_FK1"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "T44_FK2"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
Name "e"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "e"
DstPort 1
}
Line {
Name "scale"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "scale"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "MATLAB Function10"
SID "1345:418"
Ports [1, 1]
Position [395, 446, 460, 474]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'abb2logical']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "MATLAB Function10"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "22"
Block {
BlockType Inport
Name "abb"
SID "1345:418::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:418::20"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:418::19"
Tag "Stateflow S-Function scaled_teleoperation 31"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "logical"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:418::22"
Position [460, 171, 480, 189]
ZOrder 8
}
Block {
BlockType Outport
Name "logical"
SID "1345:418::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "abb"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "logical"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "logical"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "MATLAB Function2"
SID "1345:390"
Ports [1, 1]
Position [405, 56, 470, 84]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'abb2logical']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "MATLAB Function2"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "22"
Block {
BlockType Inport
Name "abb"
SID "1345:390::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:390::20"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:390::19"
Tag "Stateflow S-Function scaled_teleoperation 3"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "logical"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:390::22"
Position [460, 171, 480, 189]
ZOrder 8
}
Block {
BlockType Outport
Name "logical"
SID "1345:390::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "abb"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "logical"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "logical"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "MATLAB Function3"
SID "1345:470"
Ports [1, 1]
Position [540, 284, 605, 366]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
Port {
PortNumber 1
Name "pose1[i]"
PropagatedSignals "pose"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
System {
Name "MATLAB Function3"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "33"
Block {
BlockType Inport
Name "T44_FK"
SID "1345:470::24"
Position [20, 101, 40, 119]
ZOrder 10
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:470::20"
Ports [1, 1]
Position [270, 255, 320, 295]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:470::19"
Tag "Stateflow S-Function scaled_teleoperation 17"
Ports [1, 2]
Position [180, 113, 230, 257]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "pose"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:470::22"
Position [460, 266, 480, 284]
ZOrder 8
}
Block {
BlockType Outport
Name "pose"
SID "1345:470::25"
Position [460, 101, 480, 119]
ZOrder 11
IconDisplay "Port number"
}
Line {
SrcBlock "T44_FK"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "pose"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "pose"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "MATLAB Function4"
SID "1345:476"
Ports [3, 2]
Position [1680, 179, 1745, 261]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
Port {
PortNumber 1
Name "vel1[i]"
PropagatedSignals "vel1"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 2
Name "vel2[i]"
PropagatedSignals "vel2"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
System {
Name "MATLAB Function4"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "36"
Block {
BlockType Inport
Name "J1"
SID "1345:476::24"
Position [20, 101, 40, 119]
ZOrder 10
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "J2"
SID "1345:476::34"
Position [20, 136, 40, 154]
ZOrder 12
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "q_dot"
SID "1345:476::35"
Position [20, 171, 40, 189]
ZOrder 13
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:476::20"
Ports [1, 1]
Position [270, 255, 320, 295]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:476::19"
Tag "Stateflow S-Function scaled_teleoperation 18"
Ports [3, 3]
Position [180, 113, 230, 257]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[3 3]"
EnableBusSupport on
Port {
PortNumber 2
Name "vel1"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "vel2"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:476::22"
Position [460, 266, 480, 284]
ZOrder 8
}
Block {
BlockType Outport
Name "vel1"
SID "1345:476::25"
Position [460, 101, 480, 119]
ZOrder 11
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "vel2"
SID "1345:476::36"
Position [460, 136, 480, 154]
ZOrder 14
Port "2"
IconDisplay "Port number"
}
Line {
SrcBlock "J1"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "J2"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
SrcBlock "q_dot"
SrcPort 1
DstBlock " SFunction "
DstPort 3
}
Line {
Name "vel1"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "vel1"
DstPort 1
}
Line {
Name "vel2"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "vel2"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "MATLAB Function5"
SID "1345:394"
Ports [1, 1]
Position [1755, 90, 1805, 130]
ShowName off
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'nancheck']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "MATLAB Function5"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "64"
Block {
BlockType Inport
Name "u"
SID "1345:394::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:394::62"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 14
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:394::61"
Tag "Stateflow S-Function scaled_teleoperation 5"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 13
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "y"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:394::64"
Position [460, 171, 480, 189]
ZOrder 16
}
Block {
BlockType Outport
Name "y"
SID "1345:394::56"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "u"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "y"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "y"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "MATLAB Function6"
SID "1345:404"
Ports [1, 1]
Position [470, 91, 535, 119]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'abb2logical']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "MATLAB Function6"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "22"
Block {
BlockType Inport
Name "abb"
SID "1345:404::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:404::20"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:404::19"
Tag "Stateflow S-Function scaled_teleoperation 24"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "logical"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:404::22"
Position [460, 171, 480, 189]
ZOrder 8
}
Block {
BlockType Outport
Name "logical"
SID "1345:404::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "abb"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "logical"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "logical"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "MATLAB Function7"
SID "1345:407"
Ports [1, 1]
Position [1595, 755, 1645, 795]
ShowName off
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'nancheck']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "MATLAB Function7"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "64"
Block {
BlockType Inport
Name "u"
SID "1345:407::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:407::62"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 14
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:407::61"
Tag "Stateflow S-Function scaled_teleoperation 28"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 13
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "y"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:407::64"
Position [460, 171, 480, 189]
ZOrder 16
}
Block {
BlockType Outport
Name "y"
SID "1345:407::56"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "u"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "y"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "y"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "MATLAB Function9"
SID "1345:417"
Ports [1, 1]
Position [395, 481, 460, 509]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'abb2logical']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "MATLAB Function9"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "22"
Block {
BlockType Inport
Name "abb"
SID "1345:417::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:417::20"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:417::19"
Tag "Stateflow S-Function scaled_teleoperation 36"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "logical"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:417::22"
Position [460, 171, 480, 189]
ZOrder 8
}
Block {
BlockType Outport
Name "logical"
SID "1345:417::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "abb"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "logical"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "logical"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType Mux
Name "Mux"
SID "1345:431"
Ports [2, 1]
Position [85, 321, 90, 359]
ZOrder -16
ShowName off
Inputs "2"
DisplayOption "bar"
}
Block {
BlockType Mux
Name "Mux2"
SID "1345:449"
Ports [2, 1]
Position [1340, 456, 1345, 494]
ZOrder -16
ShowName off
Inputs "2"
DisplayOption "bar"
}
Block {
BlockType S-Function
Name "S-Function1"
SID "1345:367"
Ports [3, 4]
Position [580, 49, 650, 156]
ZOrder -5
FunctionName "s_right_dyn"
EnableBusSupport off
}
Block {
BlockType S-Function
Name "S-Function2"
SID "1345:422"
Ports [3, 4]
Position [565, 439, 635, 546]
ZOrder -5
FunctionName "s_left_dyn"
EnableBusSupport off
}
Block {
BlockType Sum
Name "Sum"
SID "1345:437"
Ports [2, 1]
Position [1430, 765, 1450, 785]
ZOrder -18
ShowName off
IconShape "round"
Inputs "++|"
InputSameDT off
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
BlockType Sum
Name "Sum1"
SID "1345:438"
Ports [2, 1]
Position [1570, 100, 1590, 120]
ZOrder -18
ShowName off
IconShape "round"
Inputs "|++"
InputSameDT off
OutDataTypeStr "Inherit: Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
BlockType Terminator
Name "Terminator1"
SID "1345:429"
Position [1295, 280, 1315, 300]
}
Block {
BlockType Terminator
Name "Terminator10"
SID "1345:478"
Position [1810, 230, 1830, 250]
}
Block {
BlockType Terminator
Name "Terminator11"
SID "1345:491"
Position [710, 965, 730, 985]
}
Block {
BlockType Terminator
Name "Terminator2"
SID "1345:456"
Position [1440, 820, 1460, 840]
}
Block {
BlockType Terminator
Name "Terminator3"
SID "1345:471"
Position [680, 295, 700, 315]
}
Block {
BlockType Terminator
Name "Terminator4"
SID "1345:425"
Position [670, 520, 690, 540]
}
Block {
BlockType Terminator
Name "Terminator5"
SID "1345:354"
Position [700, 130, 720, 150]
}
Block {
BlockType Terminator
Name "Terminator6"
SID "1345:360"
Position [715, 105, 735, 125]
}
Block {
BlockType Terminator
Name "Terminator7"
SID "1345:426"
Position [670, 495, 690, 515]
}
Block {
BlockType Terminator
Name "Terminator8"
SID "1345:490"
Position [710, 900, 730, 920]
}
Block {
BlockType Terminator
Name "Terminator9"
SID "1345:477"
Position [1810, 190, 1830, 210]
}
Block {
BlockType SubSystem
Name "coulFric"
SID "1345:436"
Ports [4, 2]
Position [1420, 345, 1495, 690]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'coulFric']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "coulFric"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "27"
Block {
BlockType Inport
Name "coulRight"
SID "1345:436::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "q_dot"
SID "1345:436::26"
Position [20, 136, 40, 154]
ZOrder 12
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "coulLeft"
SID "1345:436::24"
Position [20, 171, 40, 189]
ZOrder 10
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "dith"
SID "1345:436::27"
Position [20, 206, 40, 224]
ZOrder 13
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:436::20"
Ports [1, 1]
Position [270, 220, 320, 260]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:436::19"
Tag "Stateflow S-Function scaled_teleoperation 14"
Ports [4, 3]
Position [180, 100, 230, 220]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[4 3]"
EnableBusSupport on
Port {
PortNumber 2
Name "trqRight"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "trqLeft"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:436::22"
Position [460, 231, 480, 249]
ZOrder 8
}
Block {
BlockType Outport
Name "trqRight"
SID "1345:436::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "trqLeft"
SID "1345:436::25"
Position [460, 136, 480, 154]
ZOrder 11
Port "2"
IconDisplay "Port number"
}
Line {
SrcBlock "coulRight"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "q_dot"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
SrcBlock "coulLeft"
SrcPort 1
DstBlock " SFunction "
DstPort 3
}
Line {
SrcBlock "dith"
SrcPort 1
DstBlock " SFunction "
DstPort 4
}
Line {
Name "trqRight"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "trqRight"
DstPort 1
}
Line {
Name "trqLeft"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "trqLeft"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "coulFric1"
SID "1345:488"
Ports [4, 2]
Position [505, 873, 535, 1012]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'forcelog']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "coulFric1"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "30"
Block {
BlockType Inport
Name "T1"
SID "1345:488::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "T2"
SID "1345:488::26"
Position [20, 136, 40, 154]
ZOrder 12
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "wrench1"
SID "1345:488::28"
Position [20, 171, 40, 189]
ZOrder 14
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "wrench2"
SID "1345:488::29"
Position [20, 206, 40, 224]
ZOrder 15
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:488::20"
Ports [1, 1]
Position [270, 220, 320, 260]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:488::19"
Tag "Stateflow S-Function scaled_teleoperation 21"
Ports [4, 3]
Position [180, 100, 230, 220]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[4 3]"
EnableBusSupport on
Port {
PortNumber 2
Name "wrench1_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "wrench2_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:488::22"
Position [460, 231, 480, 249]
ZOrder 8
}
Block {
BlockType Outport
Name "wrench1_out"
SID "1345:488::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "wrench2_out"
SID "1345:488::30"
Position [460, 136, 480, 154]
ZOrder 16
Port "2"
IconDisplay "Port number"
}
Line {
SrcBlock "T1"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "T2"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
SrcBlock "wrench1"
SrcPort 1
DstBlock " SFunction "
DstPort 3
}
Line {
SrcBlock "wrench2"
SrcPort 1
DstBlock " SFunction "
DstPort 4
}
Line {
Name "wrench1_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "wrench1_out"
DstPort 1
}
Line {
Name "wrench2_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "wrench2_out"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "dither"
SID "1345:463"
Ports [1, 1]
Position [1250, 882, 1345, 988]
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
System {
Name "dither"
Location [685, 72, 1726, 625]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
Block {
BlockType Inport
Name "coulRight"
SID "1345:469"
Position [55, 158, 85, 172]
IconDisplay "Port number"
}
Block {
BlockType Constant
Name "Constant2"
SID "1345:465"
Position [80, 294, 145, 326]
Value "AmpDithering"
}
Block {
BlockType Constant
Name "Constant3"
SID "1345:466"
Position [80, 249, 145, 281]
Value "FreqDithering"
Port {
PortNumber 1
Name "freqDith"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType SubSystem
Name "DITHERING"
SID "1345:467"
Ports [3, 1]
Position [275, 236, 385, 294]
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p"
"rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
Port {
PortNumber 1
Name "dithSignal[i]"
PropagatedSignals "dithering"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
System {
Name "DITHERING"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "23"
Block {
BlockType Inport
Name "coulomb"
SID "1345:467::19"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "Fdith"
SID "1345:467::1"
Position [20, 136, 40, 154]
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "amp"
SID "1345:467::18"
Position [20, 171, 40, 189]
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:467::21"
Ports [1, 1]
Position [270, 180, 320, 220]
ZOrder 2
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:467::20"
Tag "Stateflow S-Function scaled_teleoperation 20"
Ports [3, 2]
Position [180, 100, 230, 180]
ZOrder 1
FunctionName "sf_sfun"
PortCounts "[3 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "dithering"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:467::23"
Position [460, 191, 480, 209]
ZOrder 4
}
Block {
BlockType Outport
Name "dithering"
SID "1345:467::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "coulomb"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "Fdith"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
SrcBlock "amp"
SrcPort 1
DstBlock " SFunction "
DstPort 3
}
Line {
Name "dithering"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "dithering"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType Outport
Name "dith"
SID "1345:468"
Position [725, 258, 755, 272]
IconDisplay "Port number"
}
Line {
SrcBlock "coulRight"
SrcPort 1
Points [160, 0; 0, 80]
DstBlock "DITHERING"
DstPort 1
}
Line {
SrcBlock "Constant2"
SrcPort 1
Points [0, -25]
DstBlock "DITHERING"
DstPort 3
}
Line {
Name "freqDith"
Labels [0, 0]
SrcBlock "Constant3"
SrcPort 1
DstBlock "DITHERING"
DstPort 2
}
Line {
Name "dithSignal[i]"
Labels [1, 0]
SrcBlock "DITHERING"
SrcPort 1
DstBlock "dith"
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "haptic_controller"
SID "1345:416"
Ports [12, 4]
Position [830, 16, 975, 929]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
Port {
PortNumber 1
Name "q1_arm[i]"
PropagatedSignals "Q1"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 2
Name "lambda[i]"
PropagatedSignals "lambda"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "q2_arm[i]"
PropagatedSignals "Q2"
TestPoint on
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
System {
Name "haptic_controller"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "45"
Block {
BlockType Inport
Name "B1"
SID "1345:416::29"
Position [20, 101, 40, 119]
ZOrder 15
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "C1"
SID "1345:416::30"
Position [20, 136, 40, 154]
ZOrder 16
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "J1"
SID "1345:416::24"
Position [20, 171, 40, 189]
ZOrder 10
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "J1_dot"
SID "1345:416::26"
Position [20, 206, 40, 224]
ZOrder 12
Port "4"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "q1_dot"
SID "1345:416::27"
Position [20, 246, 40, 264]
ZOrder 13
Port "5"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "B2"
SID "1345:416::35"
Position [20, 281, 40, 299]
ZOrder 17
Port "6"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "C2"
SID "1345:416::36"
Position [20, 316, 40, 334]
ZOrder 18
Port "7"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "J2"
SID "1345:416::37"
Position [20, 351, 40, 369]
ZOrder 19
Port "8"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "J2_dot"
SID "1345:416::38"
Position [20, 386, 40, 404]
ZOrder 20
Port "9"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "q2_dot"
SID "1345:416::39"
Position [20, 426, 40, 444]
ZOrder 21
Port "10"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "e"
SID "1345:416::1"
Position [20, 461, 40, 479]
Port "11"
IconDisplay "Port number"
}
Block {
BlockType Inport
Name "scale"
SID "1345:416::45"
Position [20, 496, 40, 514]
ZOrder 25
Port "12"
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:416::20"
Ports [1, 1]
Position [270, 380, 320, 420]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:416::19"
Tag "Stateflow S-Function scaled_teleoperation 35"
Ports [12, 5]
Position [180, 120, 230, 380]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[12 5]"
EnableBusSupport on
Port {
PortNumber 2
Name "Q1"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 3
Name "lambda"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 4
Name "Q2"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
Port {
PortNumber 5
Name "diagm"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:416::22"
Position [460, 391, 480, 409]
ZOrder 8
}
Block {
BlockType Outport
Name "Q1"
SID "1345:416::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "lambda"
SID "1345:416::41"
Position [460, 136, 480, 154]
ZOrder 22
Port "2"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "Q2"
SID "1345:416::25"
Position [460, 171, 480, 189]
ZOrder 11
Port "3"
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "diagm"
SID "1345:416::44"
Position [460, 206, 480, 224]
ZOrder 24
Port "4"
IconDisplay "Port number"
}
Line {
SrcBlock "B1"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
SrcBlock "C1"
SrcPort 1
DstBlock " SFunction "
DstPort 2
}
Line {
SrcBlock "J1"
SrcPort 1
DstBlock " SFunction "
DstPort 3
}
Line {
SrcBlock "J1_dot"
SrcPort 1
DstBlock " SFunction "
DstPort 4
}
Line {
SrcBlock "q1_dot"
SrcPort 1
DstBlock " SFunction "
DstPort 5
}
Line {
SrcBlock "B2"
SrcPort 1
DstBlock " SFunction "
DstPort 6
}
Line {
SrcBlock "C2"
SrcPort 1
DstBlock " SFunction "
DstPort 7
}
Line {
SrcBlock "J2"
SrcPort 1
DstBlock " SFunction "
DstPort 8
}
Line {
SrcBlock "J2_dot"
SrcPort 1
DstBlock " SFunction "
DstPort 9
}
Line {
SrcBlock "q2_dot"
SrcPort 1
DstBlock " SFunction "
DstPort 10
}
Line {
SrcBlock "e"
SrcPort 1
DstBlock " SFunction "
DstPort 11
}
Line {
SrcBlock "scale"
SrcPort 1
DstBlock " SFunction "
DstPort 12
}
Line {
Name "Q1"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "Q1"
DstPort 1
}
Line {
Name "lambda"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 3
DstBlock "lambda"
DstPort 1
}
Line {
Name "Q2"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 4
DstBlock "Q2"
DstPort 1
}
Line {
Name "diagm"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 5
DstBlock "diagm"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "lp"
SID "1345:430"
Ports [1, 1]
Position [110, 309, 165, 371]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'lp']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "lp"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "22"
Block {
BlockType Inport
Name "v_in"
SID "1345:430::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:430::20"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:430::19"
Tag "Stateflow S-Function scaled_teleoperation 4"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "v_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:430::22"
Position [460, 171, 480, 189]
ZOrder 8
}
Block {
BlockType Outport
Name "v_out"
SID "1345:430::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "v_in"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "v_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "v_out"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "lp1"
SID "1345:485"
Ports [1, 1]
Position [1430, 214, 1485, 276]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'lp']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "lp1"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "22"
Block {
BlockType Inport
Name "v_in"
SID "1345:485::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:485::20"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:485::19"
Tag "Stateflow S-Function scaled_teleoperation 19"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "v_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:485::22"
Position [460, 171, 480, 189]
ZOrder 8
}
Block {
BlockType Outport
Name "v_out"
SID "1345:485::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "v_in"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "v_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "v_out"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "lp2"
SID "1345:492"
Ports [1, 1]
Position [565, 890, 605, 930]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'lp']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "lp2"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "22"
Block {
BlockType Inport
Name "v_in"
SID "1345:492::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:492::20"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:492::19"
Tag "Stateflow S-Function scaled_teleoperation 22"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "v_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:492::22"
Position [460, 171, 480, 189]
ZOrder 8
}
Block {
BlockType Outport
Name "v_out"
SID "1345:492::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "v_in"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "v_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "v_out"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType SubSystem
Name "lp3"
SID "1345:494"
Ports [1, 1]
Position [565, 955, 605, 995]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
PermitHierarchicalResolution "ExplicitOnly"
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
PropExecContextOutsideSubsystem off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Opaque off
RequestExecContextInheritance off
MaskHideContents off
SFBlockType "MATLAB Function"
MaskType "Stateflow"
MaskDescription "Embedded MATLAB block"
MaskSelfModifiable on
MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('private/"
"eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'lp']);"
MaskIconFrame on
MaskIconOpaque off
MaskIconRotate "none"
MaskPortRotate "default"
MaskIconUnits "normalized"
System {
Name "lp3"
Location [257, 457, 812, 717]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
PaperOrientation "landscape"
PaperPositionMode "auto"
PaperType "usletter"
PaperUnits "inches"
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "22"
Block {
BlockType Inport
Name "v_in"
SID "1345:494::1"
Position [20, 101, 40, 119]
IconDisplay "Port number"
}
Block {
BlockType Demux
Name " Demux "
SID "1345:494::20"
Ports [1, 1]
Position [270, 160, 320, 200]
ZOrder 6
Outputs "1"
}
Block {
BlockType S-Function
Name " SFunction "
SID "1345:494::19"
Tag "Stateflow S-Function scaled_teleoperation 23"
Ports [1, 2]
Position [180, 100, 230, 160]
ZOrder 5
FunctionName "sf_sfun"
PortCounts "[1 2]"
EnableBusSupport on
Port {
PortNumber 2
Name "v_out"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Terminator
Name " Terminator "
SID "1345:494::22"
Position [460, 171, 480, 189]
ZOrder 8
}
Block {
BlockType Outport
Name "v_out"
SID "1345:494::5"
Position [460, 101, 480, 119]
IconDisplay "Port number"
}
Line {
SrcBlock "v_in"
SrcPort 1
DstBlock " SFunction "
DstPort 1
}
Line {
Name "v_out"
Labels [0, 0]
SrcBlock " SFunction "
SrcPort 2
DstBlock "v_out"
DstPort 1
}
Line {
SrcBlock " Demux "
SrcPort 1
DstBlock " Terminator "
DstPort 1
}
Line {
SrcBlock " SFunction "
SrcPort 1
DstBlock " Demux "
DstPort 1
}
}
}
Block {
BlockType Outport
Name "torque_motor_1"
SID "1345:293"
Position [1870, 103, 1900, 117]
IconDisplay "Port number"
}
Block {
BlockType Outport
Name "torque_motor_2"
SID "1345:408"
Position [1805, 758, 1835, 772]
Port "2"
IconDisplay "Port number"
}
Line {
SrcBlock "q_motor_1"
SrcPort 1
DstBlock "FRIDA motor2arm"
DstPort 1
}
Line {
SrcBlock "FRIDA arm acceleration NOT SI"
SrcPort 1
Points [45, 0]
Branch {
Points [40, 0]
Branch {
Points [195, 0]
DstBlock "MATLAB Function1"
DstPort 1
}
Branch {
Points [-5, 0]
DstBlock "MATLAB Function3"
DstPort 1
}
}
Branch {
DstBlock "coulFric1"
DstPort 1
}
}
Line {
SrcBlock "FRIDA motor2arm1"
SrcPort 1
Points [15, 0]
Branch {
Points [0, -70]
DstBlock "MATLAB Function6"
DstPort 1
}
Branch {
Points [5, 0; 0, 100]
Branch {
Points [15, 0]
DstBlock "FRIDA arm acceleration NOT SI"
DstPort 2
}
Branch {
Points [0, 85]
DstBlock "haptic_controller"
DstPort 5
}
}
}
Line {
SrcBlock "FRIDA motor2arm"
SrcPort 1
Points [30, 0; 0, 5]
Branch {
Points [0, 130]
DstBlock "FRIDA arm acceleration NOT SI"
DstPort 1
}
Branch {
Points [0, -25]
DstBlock "MATLAB Function2"
DstPort 1
}
}
Line {
SrcBlock "S-Function1"
SrcPort 4
DstBlock "Terminator5"
DstPort 1
}
Line {
SrcBlock "Constant2"
SrcPort 1
DstBlock "S-Function1"
DstPort 3
}
Line {
SrcBlock "MATLAB Function2"
SrcPort 1
DstBlock "S-Function1"
DstPort 1
}
Line {
SrcBlock "MATLAB Function5"
SrcPort 1
DstBlock "Gain8"
DstPort 1
}
Line {
SrcBlock "MATLAB Function6"
SrcPort 1
DstBlock "S-Function1"
DstPort 2
}
Line {
SrcBlock "MATLAB Function7"
SrcPort 1
DstBlock "Gain9"
DstPort 1
}
Line {
SrcBlock "q_motor_2"
SrcPort 1
DstBlock "FRIDA motor2arm4"
DstPort 1
}
Line {
SrcBlock "FRIDA arm acceleration NOT SI1"
SrcPort 1
Points [10, 0]
Branch {
Points [195, 0; 0, 275]
DstBlock "MATLAB Function1"
DstPort 2
}
Branch {
Points [0, 305]
DstBlock "coulFric1"
DstPort 2
}
}
Line {
SrcBlock "FRIDA motor2arm5"
SrcPort 1
Points [10, 0]
Branch {
Points [0, -70; 95, 0]
DstBlock "MATLAB Function9"
DstPort 1
}
Branch {
Points [10, 0; 0, 100]
Branch {
Points [40, 0]
DstBlock "FRIDA arm acceleration NOT SI1"
DstPort 2
}
Branch {
Points [0, 55]
DstBlock "haptic_controller"
DstPort 10
}
}
}
Line {
SrcBlock "FRIDA motor2arm4"
SrcPort 1
Points [30, 0; 0, 5]
Branch {
Points [0, 130]
DstBlock "FRIDA arm acceleration NOT SI1"
DstPort 1
}
Branch {
Points [0, -40]
DstBlock "MATLAB Function10"
DstPort 1
}
}
Line {
SrcBlock "Constant1"
SrcPort 1
Points [90, 0]
DstBlock "S-Function2"
DstPort 3
}
Line {
SrcBlock "MATLAB Function10"
SrcPort 1
DstBlock "S-Function2"
DstPort 1
}
Line {
SrcBlock "MATLAB Function9"
SrcPort 1
DstBlock "S-Function2"
DstPort 2
}
Line {
SrcBlock "S-Function1"
SrcPort 1
Points [160, 0]
DstBlock "haptic_controller"
DstPort 1
}
Line {
SrcBlock "S-Function2"
SrcPort 1
Points [160, 0; 0, -20]
DstBlock "haptic_controller"
DstPort 6
}
Line {
SrcBlock "S-Function1"
SrcPort 2
Points [160, 0]
DstBlock "haptic_controller"
DstPort 2
}
Line {
SrcBlock "FRIDA arm acceleration NOT SI"
SrcPort 3
Points [330, 0; 0, 10]
DstBlock "haptic_controller"
DstPort 4
}
Line {
SrcBlock "S-Function2"
SrcPort 2
Points [150, 0; 0, 30]
DstBlock "haptic_controller"
DstPort 7
}
Line {
SrcBlock "FRIDA arm acceleration NOT SI1"
SrcPort 2
Points [150, 0; 0, 5; 170, 0]
Branch {
Points [35, 0]
DstBlock "haptic_controller"
DstPort 8
}
Branch {
Points [0, -435]
DstBlock "MATLAB Function4"
DstPort 2
}
}
Line {
SrcBlock "FRIDA arm acceleration NOT SI1"
SrcPort 3
Points [350, 0; 0, -20]
DstBlock "haptic_controller"
DstPort 9
}
Line {
Name "e[i]"
Labels [0, 0]
SrcBlock "MATLAB Function1"
SrcPort 1
Points [20, 0]
DstBlock "haptic_controller"
DstPort 11
}
Line {
SrcBlock "FRIDA arm acceleration NOT SI"
SrcPort 2
Points [310, 0]
Branch {
Points [60, 0]
DstBlock "haptic_controller"
DstPort 3
}
Branch {
Points [0, -50]
DstBlock "MATLAB Function4"
DstPort 1
}
}
Line {
SrcBlock "S-Function1"
SrcPort 3
DstBlock "Terminator6"
DstPort 1
}
Line {
SrcBlock "S-Function2"
SrcPort 3
DstBlock "Terminator7"
DstPort 1
}
Line {
SrcBlock "S-Function2"
SrcPort 4
DstBlock "Terminator4"
DstPort 1
}
Line {
SrcBlock "lp"
SrcPort 1
DstBlock "Demux"
DstPort 1
}
Line {
SrcBlock "Mux"
SrcPort 1
DstBlock "lp"
DstPort 1
}
Line {
SrcBlock "qd_motor_1"
SrcPort 1
Points [0, 5]
DstBlock "Mux"
DstPort 1
}
Line {
SrcBlock "qd_motor_2"
SrcPort 1
Points [10, 0; 0, -20]
DstBlock "Mux"
DstPort 2
}
Line {
SrcBlock "Demux"
SrcPort 1
Points [10, 0]
DstBlock "FRIDA motor2arm1"
DstPort 1
}
Line {
SrcBlock "Demux"
SrcPort 2
DstBlock "FRIDA motor2arm5"
DstPort 1
}
Line {
SrcBlock "coulombWidthArmRightAbb"
SrcPort 1
DstBlock "Gain"
DstPort 1
}
Line {
SrcBlock "FRIDA motor2arm2"
SrcPort 1
DstBlock "MATLAB Function5"
DstPort 1
}
Line {
SrcBlock "FRIDA motor2arm3"
SrcPort 1
DstBlock "MATLAB Function7"
DstPort 1
}
Line {
Name "q2_arm[i]"
Labels [0, 0]
SrcBlock "haptic_controller"
SrcPort 3
Points [230, 0; 0, 180]
DstBlock "Gain6"
DstPort 1
}
Line {
SrcBlock "Sum"
SrcPort 1
DstBlock "FRIDA motor2arm3"
DstPort 1
}
Line {
SrcBlock "coulombWidthArmLeftAbb"
SrcPort 1
DstBlock "Gain1"
DstPort 1
}
Line {
Name "lambda[i]"
Labels [0, 0]
SrcBlock "haptic_controller"
SrcPort 2
Points [0, -70]
DstBlock "Terminator1"
DstPort 1
}
Line {
SrcBlock "Sum1"
SrcPort 1
DstBlock "FRIDA motor2arm2"
DstPort 1
}
Line {
Name "q1_arm[i]"
Labels [0, 0]
SrcBlock "haptic_controller"
SrcPort 1
Points [0, -20]
DstBlock "Gain5"
DstPort 1
}
Line {
Name "cright[i]"
Labels [0, 0]
SrcBlock "Gain"
SrcPort 1
Points [20, 0]
Branch {
Points [215, 0]
DstBlock "coulFric"
DstPort 1
}
Branch {
Points [0, 560]
DstBlock "dither"
DstPort 1
}
}
Line {
Name "cleft[i]"
Labels [0, 0]
SrcBlock "Gain1"
SrcPort 1
Points [55, 0; 0, -35]
DstBlock "coulFric"
DstPort 3
}
Line {
Name "help_right[i]"
Labels [0, 0]
SrcBlock "Gain2"
SrcPort 1
Points [40, 0; 0, -285]
DstBlock "Sum1"
DstPort 2
}
Line {
Name "help_left[i]"
Labels [0, 0]
SrcBlock "Gain3"
SrcPort 1
Points [5, 0; 0, 135; -205, 0]
DstBlock "Sum"
DstPort 1
}
Line {
SrcBlock "coulFric"
SrcPort 1
Points [15, 0; 0, -15]
DstBlock "Gain2"
DstPort 1
}
Line {
SrcBlock "coulFric"
SrcPort 2
Points [5, 0; 0, -15]
DstBlock "Gain3"
DstPort 1
}
Line {
SrcBlock "Mux2"
SrcPort 1
Points [5, 0]
Branch {
DstBlock "coulFric"
DstPort 2
}
Branch {
Points [-30, -230]
DstBlock "lp1"
DstPort 1
}
}
Line {
SrcBlock "velFlt1"
SrcPort 1
DstBlock "FRIDA motor2arm6"
DstPort 1
}
Line {
SrcBlock "velFlt2"
SrcPort 1
DstBlock "FRIDA motor2arm7"
DstPort 1
}
Line {
SrcBlock "FRIDA motor2arm6"
SrcPort 1
DstBlock "Mux2"
DstPort 1
}
Line {
SrcBlock "FRIDA motor2arm7"
SrcPort 1
Points [0, -25]
DstBlock "Mux2"
DstPort 2
}
Line {
SrcBlock "haptic_controller"
SrcPort 4
Points [0, 10]
DstBlock "Gain4"
DstPort 1
}
Line {
Name "diagm[i]"
Labels [0, 0]
SrcBlock "Gain4"
SrcPort 1
DstBlock "Gain7"
DstPort 1
}
Line {
SrcBlock "Gain5"
SrcPort 1
DstBlock "Sum1"
DstPort 1
}
Line {
SrcBlock "Gain6"
SrcPort 1
Points [50, 0]
DstBlock "Sum"
DstPort 2
}
Line {
SrcBlock "Gain7"
SrcPort 1
DstBlock "Terminator2"
DstPort 1
}
Line {
SrcBlock "Gain8"
SrcPort 1
DstBlock "torque_motor_1"
DstPort 1
}
Line {
SrcBlock "Gain9"
SrcPort 1
Points [30, 0]
DstBlock "torque_motor_2"
DstPort 1
}
Line {
SrcBlock "dither"
SrcPort 1
Points [55, 0]
DstBlock "coulFric"
DstPort 4
}
Line {
Name "pose1[i]"
Labels [0, 0]
SrcBlock "MATLAB Function3"
SrcPort 1
Points [0, -20]
DstBlock "Terminator3"
DstPort 1
}
Line {
Name "vel1[i]"
Labels [0, 0]
SrcBlock "MATLAB Function4"
SrcPort 1
DstBlock "Terminator9"
DstPort 1
}
Line {
Name "vel2[i]"
Labels [0, 0]
SrcBlock "MATLAB Function4"
SrcPort 2
DstBlock "Terminator10"
DstPort 1
}
Line {
SrcBlock "lp1"
SrcPort 1
DstBlock "MATLAB Function4"
DstPort 3
}
Line {
SrcBlock "wrench2"
SrcPort 1
Points [0, -5]
DstBlock "coulFric1"
DstPort 4
}
Line {
SrcBlock "wrench1"
SrcPort 1
Points [0, 70]
DstBlock "coulFric1"
DstPort 3
}
Line {
SrcBlock "coulFric1"
SrcPort 1
DstBlock "lp2"
DstPort 1
}
Line {
SrcBlock "coulFric1"
SrcPort 2
Points [0, -5]
DstBlock "lp3"
DstPort 1
}
Line {
SrcBlock "lp2"
SrcPort 1
DstBlock "Gain10"
DstPort 1
}
Line {
SrcBlock "lp3"
SrcPort 1
DstBlock "Gain11"
DstPort 1
}
Line {
Name "w1[i]"
Labels [0, 0]
SrcBlock "Gain10"
SrcPort 1
DstBlock "Terminator8"
DstPort 1
}
Line {
Name "w2[i]"
Labels [0, 0]
SrcBlock "Gain11"
SrcPort 1
DstBlock "Terminator11"
DstPort 1
}
}
}
Block {
BlockType Outport
Name "ext2irb.robot[1].joint[i].parKp"
SID "211"
Position [725, 338, 755, 352]
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].joint[i].parKv"
SID "212"
Position [715, 403, 745, 417]
Port "2"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].joint[i].parKi"
SID "213"
Position [695, 468, 725, 482]
Port "3"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].joint[i].posRef"
SID "214"
Position [765, 533, 795, 547]
Port "4"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].joint[i].velRef"
SID "215"
Position [765, 598, 795, 612]
Port "5"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].joint[i].trqFfw"
SID "216"
Position [720, 663, 750, 677]
Port "6"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].mocgendata.instruction"
SID "217"
Position [710, 728, 740, 742]
Port "7"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].mocgendata.value1"
SID "218"
Position [735, 793, 765, 807]
Port "8"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].mocgendata.value2"
SID "219"
Position [735, 858, 765, 872]
Port "9"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].mocgendata.value3"
SID "220"
Position [735, 923, 765, 937]
Port "10"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].mocgendata.value4"
SID "221"
Position [725, 988, 755, 1002]
Port "11"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].mocgendata.value5"
SID "222"
Position [725, 1053, 755, 1067]
Port "12"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].mocgendata.value6"
SID "223"
Position [735, 1118, 765, 1132]
Port "13"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].mocgendata.string1[i]"
SID "224"
Position [735, 1183, 765, 1197]
Port "14"
IconDisplay "Port number"
PortDimensions "20"
}
Block {
BlockType Outport
Name "ext2irb.robot[1].mocgendata.string2[i]"
SID "225"
Position [715, 1253, 745, 1267]
Port "15"
IconDisplay "Port number"
PortDimensions "20"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].joint[i].parKp"
SID "322"
Position [1530, 318, 1560, 332]
Port "16"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].joint[i].parKv"
SID "323"
Position [1570, 393, 1600, 407]
Port "17"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].joint[i].parKi"
SID "324"
Position [1590, 468, 1620, 482]
Port "18"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].joint[i].posRef"
SID "325"
Position [1665, 1368, 1695, 1382]
Port "19"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].joint[i].velRef"
SID "326"
Position [1755, 543, 1785, 557]
Port "20"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].joint[i].trqFfw"
SID "327"
Position [1570, 618, 1600, 632]
Port "21"
IconDisplay "Port number"
PortDimensions "DOF"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].mocgendata.instruction"
SID "328"
Position [1615, 693, 1645, 707]
Port "22"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].mocgendata.value1"
SID "329"
Position [1595, 768, 1625, 782]
Port "23"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].mocgendata.value2"
SID "330"
Position [1605, 843, 1635, 857]
Port "24"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].mocgendata.value3"
SID "331"
Position [1605, 918, 1635, 932]
Port "25"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].mocgendata.value4"
SID "332"
Position [1595, 993, 1625, 1007]
Port "26"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].mocgendata.value5"
SID "333"
Position [1600, 1068, 1630, 1082]
Port "27"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].mocgendata.value6"
SID "334"
Position [1585, 1143, 1615, 1157]
Port "28"
IconDisplay "Port number"
PortDimensions "1"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].mocgendata.string1[i]"
SID "335"
Position [1570, 1218, 1600, 1232]
Port "29"
IconDisplay "Port number"
PortDimensions "20"
}
Block {
BlockType Outport
Name "ext2irb.robot[0].mocgendata.string2[i]"
SID "336"
Position [1595, 1293, 1625, 1307]
Port "30"
IconDisplay "Port number"
PortDimensions "20"
}
Line {
SrcBlock "irb2ext.obtaining"
SrcPort 1
Points [80, 0; 0, 160]
Branch {
Points [925, 0]
DstBlock "Right"
DstPort 1
}
Branch {
Points [0, 50]
DstBlock "Left"
DstPort 1
}
}
Line {
SrcBlock "irb2ext.manualMode"
SrcPort 1
Points [110, 0; 0, 165]
Branch {
Points [850, 0; 0, 25]
DstBlock "Right"
DstPort 2
}
Branch {
Points [0, 45]
DstBlock "Left"
DstPort 2
}
}
Line {
SrcBlock "irb2ext.controlActive"
SrcPort 1
Points [50, 0]
Branch {
Points [0, 95; 835, 0; 0, 100]
DstBlock "Right"
DstPort 3
}
Branch {
Points [0, 210]
DstBlock "Left"
DstPort 3
}
}
Line {
SrcBlock "irb2ext.robot[1].mocgendata.string2[i]"
SrcPort 1
Points [205, 0]
DstBlock "Left"
DstPort 27
}
Line {
SrcBlock "irb2ext.robot[1].mocgendata.string1[i]"
SrcPort 1
Points [200, 0]
DstBlock "Left"
DstPort 26
}
Line {
SrcBlock "irb2ext.robot[1].mocgendata.value6"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 25
}
Line {
SrcBlock "irb2ext.robot[1].mocgendata.value5"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 24
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].parKp"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 4
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].parKv"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 5
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].parKi"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 6
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].parTrqMin"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 7
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].parTrqMax"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 8
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].posRawFb"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 9
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].posFlt"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 10
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].velFlt"
SrcPort 1
Points [115, 0]
Branch {
Points [75, 0]
DstBlock "Left"
DstPort 11
}
Branch {
Points [0, -470]
DstBlock "botharms_get_torque"
DstPort 8
}
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].velOut"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 12
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].trqRaw"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 13
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].trqRefFlt"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 14
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].posRef"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 15
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].velRef"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 16
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].trqFfw"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 17
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].trqFfwGrav"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 18
}
Line {
SrcBlock "irb2ext.robot[1].mocgendata.instruction"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 19
}
Line {
SrcBlock "irb2ext.robot[1].mocgendata.value1"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 20
}
Line {
SrcBlock "irb2ext.robot[1].mocgendata.value2"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 21
}
Line {
SrcBlock "irb2ext.robot[1].mocgendata.value3"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 22
}
Line {
SrcBlock "irb2ext.robot[1].mocgendata.value4"
SrcPort 1
Points [190, 0]
DstBlock "Left"
DstPort 23
}
Line {
SrcBlock "Left"
SrcPort 2
DstBlock "ext2irb.robot[1].joint[i].parKp"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 3
DstBlock "ext2irb.robot[1].joint[i].parKv"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 4
DstBlock "ext2irb.robot[1].joint[i].parKi"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 5
DstBlock "MATLAB Function3"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 6
DstBlock "MATLAB Function4"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 7
DstBlock "ext2irb.robot[1].joint[i].trqFfw"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 8
DstBlock "ext2irb.robot[1].mocgendata.instruction"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 9
DstBlock "ext2irb.robot[1].mocgendata.value1"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 10
DstBlock "ext2irb.robot[1].mocgendata.value2"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 11
DstBlock "ext2irb.robot[1].mocgendata.value3"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 12
DstBlock "ext2irb.robot[1].mocgendata.value4"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 13
DstBlock "ext2irb.robot[1].mocgendata.value5"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 14
DstBlock "ext2irb.robot[1].mocgendata.value6"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 15
DstBlock "ext2irb.robot[1].mocgendata.string1[i]"
DstPort 1
}
Line {
SrcBlock "Left"
SrcPort 16
Points [0, 5]
DstBlock "ext2irb.robot[1].mocgendata.string2[i]"
DstPort 1
}
Line {
SrcBlock "irb2ext.robot[0].mocgendata.string2[i]"
SrcPort 1
Points [205, 0]
DstBlock "Right"
DstPort 25
}
Line {
SrcBlock "irb2ext.robot[0].mocgendata.string1[i]"
SrcPort 1
Points [185, 0]
DstBlock "Right"
DstPort 24
}
Line {
SrcBlock "irb2ext.robot[0].mocgendata.value6"
SrcPort 1
Points [200, 0]
DstBlock "Right"
DstPort 23
}
Line {
SrcBlock "irb2ext.robot[0].mocgendata.value5"
SrcPort 1
Points [190, 0]
DstBlock "Right"
DstPort 22
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].parKp"
SrcPort 1
Points [230, 0]
DstBlock "Right"
DstPort 4
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].parKv"
SrcPort 1
Points [210, 0]
DstBlock "Right"
DstPort 5
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].parKi"
SrcPort 1
Points [210, 0]
DstBlock "Right"
DstPort 6
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].parTrqMin"
SrcPort 1
Points [210, 0]
DstBlock "Right"
DstPort 7
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].parTrqMax"
SrcPort 1
Points [210, 0]
DstBlock "Right"
DstPort 8
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].posRawFb"
SrcPort 1
Points [190, 0]
DstBlock "Right"
DstPort 9
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].posFlt"
SrcPort 1
Points [210, 0]
DstBlock "Right"
DstPort 10
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].velFlt"
SrcPort 1
Points [95, 0]
Branch {
Points [115, 0]
DstBlock "Right"
DstPort 11
}
Branch {
Points [0, -30; -235, 0]
DstBlock "botharms_get_torque"
DstPort 7
}
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].velOut"
SrcPort 1
Points [210, 0]
DstBlock "Right"
DstPort 12
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].trqRaw"
SrcPort 1
Points [210, 0]
DstBlock "Right"
DstPort 13
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].trqRefFlt"
SrcPort 1
Points [210, 0]
DstBlock "Right"
DstPort 14
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].trqFfw"
SrcPort 1
Points [180, 0]
DstBlock "Right"
DstPort 15
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].trqFfwGrav"
SrcPort 1
Points [180, 0]
DstBlock "Right"
DstPort 16
}
Line {
SrcBlock "irb2ext.robot[0].mocgendata.instruction"
SrcPort 1
Points [180, 0]
DstBlock "Right"
DstPort 17
}
Line {
SrcBlock "irb2ext.robot[0].mocgendata.value1"
SrcPort 1
Points [180, 0]
DstBlock "Right"
DstPort 18
}
Line {
SrcBlock "irb2ext.robot[0].mocgendata.value2"
SrcPort 1
Points [190, 0]
DstBlock "Right"
DstPort 19
}
Line {
SrcBlock "irb2ext.robot[0].mocgendata.value3"
SrcPort 1
Points [190, 0]
DstBlock "Right"
DstPort 20
}
Line {
SrcBlock "irb2ext.robot[0].mocgendata.value4"
SrcPort 1
Points [190, 0]
DstBlock "Right"
DstPort 21
}
Line {
SrcBlock "Right"
SrcPort 2
DstBlock "ext2irb.robot[0].joint[i].parKp"
DstPort 1
}
Line {
SrcBlock "Right"
SrcPort 3
DstBlock "ext2irb.robot[0].joint[i].parKv"
DstPort 1
}
Line {
SrcBlock "Right"
SrcPort 6
DstBlock "ext2irb.robot[0].joint[i].trqFfw"
DstPort 1
}
Line {
SrcBlock "Right"
SrcPort 9
DstBlock "ext2irb.robot[0].mocgendata.value2"
DstPort 1
}
Line {
SrcBlock "Right"
SrcPort 10
DstBlock "ext2irb.robot[0].mocgendata.value3"
DstPort 1
}
Line {
SrcBlock "Right"
SrcPort 11
DstBlock "ext2irb.robot[0].mocgendata.value4"
DstPort 1
}
Line {
SrcBlock "Right"
SrcPort 12
DstBlock "ext2irb.robot[0].mocgendata.value5"
DstPort 1
}
Line {
SrcBlock "Right"
SrcPort 15
DstBlock "ext2irb.robot[0].mocgendata.string2[i]"
DstPort 1
}
Line {
SrcBlock "MATLAB Function2"
SrcPort 1
DstBlock "ext2irb.robot[0].joint[i].velRef"
DstPort 1
}
Line {
SrcBlock "Right"
SrcPort 5
DstBlock "MATLAB Function2"
DstPort 1
}
Line {
SrcBlock "MATLAB Function4"
SrcPort 1
DstBlock "ext2irb.robot[1].joint[i].velRef"
DstPort 1
}
Line {
SrcBlock "MATLAB Function3"
SrcPort 1
DstBlock "ext2irb.robot[1].joint[i].posRef"
DstPort 1
}
Line {
SrcBlock "Right"
SrcPort 7
DstBlock "ext2irb.robot[0].mocgendata.instruction"
DstPort 1
}
Line {
SrcBlock "Constant9"
SrcPort 1
Points [135, 0; 0, -130]
DstBlock "Left"
DstPort 28
}
Line {
SrcBlock "Right"
SrcPort 8
DstBlock "ext2irb.robot[0].mocgendata.value1"
DstPort 1
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].posRef"
SrcPort 1
Points [155, 0]
DstBlock "Right"
DstPort 28
}
Line {
SrcBlock "MATLAB Function1"
SrcPort 1
DstBlock "ext2irb.robot[0].joint[i].posRef"
DstPort 1
}
Line {
SrcBlock "Right"
SrcPort 16
DstBlock "MATLAB Function1"
DstPort 1
}
Line {
SrcBlock "Constant1"
SrcPort 1
Points [50, 0]
DstBlock "Right"
DstPort 26
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].velRef"
SrcPort 1
Points [140, 0]
DstBlock "Right"
DstPort 27
}
Line {
SrcBlock "Right"
SrcPort 4
DstBlock "ext2irb.robot[0].joint[i].parKi"
DstPort 1
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].posRawAbs"
SrcPort 1
Points [115, 0]
DstBlock "botharms_get_torque"
DstPort 3
}
Line {
SrcBlock "irb2ext.robot[1].joint[i].velRaw"
SrcPort 1
Points [0, 15; 115, 0]
DstBlock "botharms_get_torque"
DstPort 4
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].posRawAbs"
SrcPort 1
Points [115, 0]
DstBlock "botharms_get_torque"
DstPort 1
}
Line {
SrcBlock "irb2ext.robot[0].joint[i].velRaw"
SrcPort 1
Points [115, 0]
DstBlock "botharms_get_torque"
DstPort 2
}
Line {
Name "torque1[i]"
Labels [0, 0]
SrcBlock "botharms_get_torque"
SrcPort 1
Points [100, 0; 0, 1300]
DstBlock "Right"
DstPort 29
}
Line {
Name "torque2[i]"
Labels [0, 0]
SrcBlock "botharms_get_torque"
SrcPort 2
Points [35, 0; 0, 50; -1035, 0; 0, 1035]
DstBlock "Left"
DstPort 29
}
Line {
SrcBlock "Right"
SrcPort 14
DstBlock "ext2irb.robot[0].mocgendata.string1[i]"
DstPort 1
}
Line {
SrcBlock "Right"
SrcPort 13
DstBlock "ext2irb.robot[0].mocgendata.value6"
DstPort 1
}
Line {
SrcBlock "Right"
SrcPort 1
Points [45, 0; 0, -245; -775, 0; 0, 105]
DstBlock "botharms_get_torque"
DstPort 5
}
Line {
SrcBlock "Left"
SrcPort 1
Points [110, 0; 0, -150]
DstBlock "botharms_get_torque"
DstPort 6
}
Line {
SrcBlock "ati2rob.forcesTorques[i]"
SrcPort 1
Points [0, 5]
DstBlock "botharms_get_torque"
DstPort 9
}
Line {
SrcBlock "ati2rob_2.forcesTorques[i]"
SrcPort 1
Points [0, 5]
DstBlock "botharms_get_torque"
DstPort 10
}
Annotation {
SID "930"
Name "addpath /opt/robot/matlab;\naddpath /home/martinka/Projects/SARAFun/demopert;\naddpath /opt/robot/m"
"atlab/irb/mex;\naddpath /opt/robot/matlab/irb/mex/mex_ESMERALDA; \naddpath /home/martinka/Projects/SARAFun/bp-ar"
"-hmm/dmp/leadthrough;\naddpath /home/martinka/frida_dynamics;\n\nDOF=7;\nh=0.004032;\nhighKiActive = 0;\nkeepSti"
"ll = 0;\nrightActive = 1;\nleftActive = 1;\n\nfriction = 0.7;\ncontrol = 1;\nAmpDithering=0.3;\nFreqDithering=15"
";\nadmit_CTRL=0;\n\n\n[B_lpfilt,A_lpfilt] = butter(1,0.01);\n\nparametersLeadthrough;\n\nf_switch = 1;\na_switch"
" = 0;\ndisp('Init done');"
Position [1762, 173]
ForegroundColor "red"
DropShadow on
ClickFcn "addpath /opt/robot/matlab;\naddpath /home/martinka/Projects/SARAFun/demopert;\naddpath /opt/rob"
"ot/matlab/irb/mex;\naddpath /opt/robot/matlab/irb/mex/mex_ESMERALDA; \naddpath /home/martinka/Projects/SARAFun/b"
"p-ar-hmm/dmp/leadthrough;\naddpath /home/martinka/frida_dynamics;\n\nDOF=7;\nh=0.004032;\nhighKiActive = 0;\nkee"
"pStill = 0;\nrightActive = 1;\nleftActive = 1;\n\nfriction = 0.7;\ncontrol = 1;\nAmpDithering=0.3;\nFreqDitherin"
"g=15;\nadmit_CTRL=0;\n\n\n[B_lpfilt,A_lpfilt] = butter(1,0.01);\n\nparametersLeadthrough;\n\nf_switch = 1;\na_sw"
"itch = 0;\ndisp('Init done');"
UseDisplayTextAsClickCallback on
}
}
}
# Finite State Machines
#
# Stateflow Version 7.6 (R2011b) dated Jan 25 2012, 13:50:40
#
#
Stateflow {
machine {
id 1
name "scaled_teleoperation"
created "06-Feb-2013 13:26:16"
isLibrary 0
firstTarget 349
sfVersion 76014001.00040001
}
chart {
id 2
name "botharms_get_torque/FRIDA arm acceleration NOT SI/frida_kin"
windowPosition [270.375 98.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 3 0 0]
firstTransition 5
firstJunction 4
viewObj 2
machine 1
ssIdHighWaterMark 9
decomposition CLUSTER_CHART
type EML_CHART
firstData 6
chartFileNumber 1
disableImplicitCasting 1
eml {
name "frida_kinematics"
}
}
state {
id 3
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 2
treeNode [2 0 0 0]
superState SUBCHART
subviewer 2
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [T44_fkine, J0, Jn, Jdot0] = frida_kinematics(q,qd)\n% Calculation of forward kinematics"
", jacobian in base frame and flange\n% frame, and Jacobian time derivative (in base frame)\n\nn_joints = 7;\n\nq"
"_offs = [0;0;pi/2;0;0;0;0];\nq = q+q_offs;\n\nshoulderLen = 0.110*1000;\nshoulderOffs = -0.030*1000;\nelbowOffs "
"= 0.0405*1000;\nwristOffs = 0.0135*1000;\nhandOffs = -0.027*1000;\nupArmLen = 0.2465*1000;\nlowArmLen = 0.265*10"
"00;\nhandLen = 0.032*1000;\n\n\n% alpha a theta d Rev=0/Pris=1\ndhpar = [ -pi"
"/2 shoulderOffs q(1) shoulderLen 0\n pi/2 -shoulderOffs q(2) 0 0\n -"
"pi/2 elbowOffs q(7) upArmLen 0\n pi/2 -elbowOffs q(3) 0 0\n "
" -pi/2 wristOffs q(4) lowArmLen 0\n pi/2 handOffs q(5) 0 0\n "
" 0 0 q(6) handLen 0 ];\n \nbase = eye(4);\n \ntrans = zeros(4,4,n_joints"
");\n \nfor j=1:n_joints\n t = [ cos(dhpar(j,3)) -sin(dhpar(j,3))*cos(dhpar(j,1)) sin(dhpar(j,3))*"
"sin(dhpar(j,1)) dhpar(j,2)*cos(dhpar(j,3))\n sin(dhpar(j,3)) cos(dhpar(j,3))*cos(dhpar(j,1)) -"
"cos(dhpar(j,3))*sin(dhpar(j,1)) dhpar(j,2)*sin(dhpar(j,3))\n 0 sin(dhpar(j,1)) "
" cos(dhpar(j,1)) dhpar(j,4)\n 0 0 "
" 0 1 ];\n trans(:,:,j) = t;\nend\n\n%%%"
"%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% fkine\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\nT = base;\nfor i=1:n_joints,\n "
"T = T * trans(:,:,i);\nend\n\nT44_fkine = reshape(T',16,1);\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% tr2rot\n%%"
"%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\nR = T(1:3,1:3);\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% J0 + Jdot0\n%%%%%%"
"%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\nJ0_tmp = zeros(6,n_joints);\n\nJ_dot = zeros(6,n_joints);\nomega = zeros(3,n_joi"
"nts);\nnonZeroOmega = zeros(n_joints,1);\n\nT44_FK = eye(4);\n\nfor k=1:n_joints\n\n if dhpar(j,5) == 0\n % "
"revolute axis\n omega(:,k) = T44_FK(1:3,3);\n nonZeroOmega(k) = 1;\n J0_tmp(4:6,k) = omega("
":,k);\n end\n p_prev = T44_FK(1:3,4);\n T44_FK = T44_FK*trans(:,:,k);\n r_tilde = T44_FK(1:3,4)-p_pr"
"ev;\n \n for i=1:n_joints\n if nonZeroOmega(i)\n J0_tmp(1:3,i) = J0_tmp(1:3,i) + cross(o"
"mega(:,i),r_tilde);\n for j=1:n_joints\n if nonZeroOmega(j)\n contr"
"ibution = cross(omega(:,i),cross(omega(:,j)*qd(j),r_tilde));\n J_dot(1:3,i) = J_dot(1:3,i) + "
"contribution;\n end\n end\n end\n end\n\n if dhpar(j,5) == 1\n % prismat"
"ic axis\n prismAxis = T44_FK(1:3,3);\n J0_tmp(1:3,k) = prismAxis;\n for i=1:n_joints\n "
" contribution = cross(omega(:,i),prismAxis);\n J_dot(1:3,k) = J_dot(1:3,k) + contribution*qd(i)"
";\n J_dot(1:3,i) = J_dot(1:3,i) + contribution*qd(k);\n end\n end\nend\nJ0_tmp = J0_tmp(:,["
"1 2 4 5 6 7 3]); % Switch to wierd joint order\nJ0 = reshape(J0_tmp', 6*n_joints,1);\n\nJdot0 = reshape(J_dot',"
" 6*n_joints,1);\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% Jn\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n\nJn_tmp = ["
"R' zeros(3);zeros(3) R']*J0_tmp;\nJn = reshape(Jn_tmp', 6*n_joints,1);\n"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 4
position [23.5747 49.5747 7]
chart 2
linkNode [2 0 0]
subviewer 2
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 5
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 4
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 2
linkNode [2 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 2
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 6
ssIdNumber 4
name "q"
linkNode [2 0 7]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 7
ssIdNumber 5
name "T44_fkine"
linkNode [2 6 8]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 8
ssIdNumber 6
name "qd"
linkNode [2 7 9]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 9
ssIdNumber 7
name "J0"
linkNode [2 8 10]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 10
ssIdNumber 8
name "Jn"
linkNode [2 9 11]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 11
ssIdNumber 9
name "Jdot0"
linkNode [2 10 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 12
name "botharms_get_torque/FRIDA arm acceleration NOT SI/frida_kin"
machine 1
chart 2
}
chart {
id 13
name "Left/calcGravityTorque/calcGravTrq"
windowPosition [390.75 188.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 14 0 0]
firstTransition 16
firstJunction 15
viewObj 13
machine 1
ssIdHighWaterMark 16
decomposition CLUSTER_CHART
type EML_CHART
firstData 17
chartFileNumber 2
disableImplicitCasting 1
eml {
name "fcn"
}
}
state {
id 14
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 13
treeNode [13 0 0 0]
superState SUBCHART
subviewer 13
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [gravTrq,gravTrqMotor,coulombWidthMotor, coulombWidth] = fcn(q_motor,x_hat,gearRatio,ba"
"seAngles)\n \n% gearRatio = [100;100;-100;100;-101;100;100]; \n \n q_arm_ABBorder = q_motor./gear"
"Ratio;\n \n\n \n gravTrqLogicalOrder = calcTrqFromParams( x_hat,q_arm_ABBorder,baseAngles);\n %logic"
"al ordering\n \n gravTrq = [gravTrqLogicalOrder(1:2) ; gravTrqLogicalOrder(4:7) ; gravTrqLogicalOrder(3)];"
"\n %ABB-order\n \n% gearRatio = [100;100;-100;100;-101;100;100];\n \n gravTrqMotor = gravTrq./ge"
"arRatio;\n \n coulombWidthLogicalOrder = x_hat(29:35);\n coulombWidth = [coulombWidthLogicalOrder(1:2) "
"; coulombWidthLogicalOrder(4:7) ; coulombWidthLogicalOrder(3)];\n coulombWidthMotor = coulombWidth./abs(gearR"
"atio);\n\nend\n\nfunction [ trq ] = calcTrqFromParams( x_hat,q,baseAngles)\n%CALCTRQFROMPARAMS Summary of this f"
"unction goes here\n% Detailed explanation goes here\n\n A = calcA_FRIDA(q,-9.82,baseAngles);\n \n \n "
" trq = A*x_hat(1:28,1);\n\nend\n\nfunction [A] = calcA_FRIDA(q,g,baseAngles)\n % q should be given in ABB-o"
"rder!!!\n % Offsets are already handled in the equations!!!\n\n c1 = cos(q(1));\n s1 = sin(q(1));\n "
"c2 = cos(q(2));\n s2 = sin(q(2));\n c3 = cos(q(7));\n s3 = sin(q(7));\n c4 = cos(q(3));\n s4 = si"
"n(q(3));\n c5 = cos(q(4));\n s5 = sin(q(4));\n c6 = cos(q(5));\n s6 = sin(q(5));\n c7 = cos(q(6))"
";\n s7 = sin(q(6));\n\n sb1 = sin(baseAngles(1));\n cb1 = cos(baseAngles(1));\n sb2 = sin(baseAngles"
"(2));\n cb2 = cos(baseAngles(2));\n sb3 = sin(baseAngles(3));\n cb3 = cos(baseAngles(3));\n\n should"
"erLen = 0.110;\n shoulderOffs = -0.030;\n elbowOffs = 0.0405;\n wristOffs = 0.0135;\n hand"
"Offs = -0.027;\n upArmLen = 0.2465;\n lowArmLen = 0.265;\n handLen = 0.032;\n\n A"
" = zeros(7,28);\n\n p0=g*cb1;\n p1=g*sb1;\n p2=sb2*p0;\n p3=s1*cb3;\n p4=c2*c3;\n p5=cb2*p0;\n"
" p6=c1*cb3;\n p7=c1*sb3;\n p8=s1*sb3;\n p9=s4*p1;\n p10=s4*p2;\n p11=c5*c6;\n p12=c4*p1;\n "
" p13=s1*sb3;\n p14=c4*p2;\n p15=s3*c2;\n p16=c7^2;\n p17=s7^2;\n p18=s5*c6;\n p19=s2*s6;\n "
" p20=s3*s6;\n p21=s2*c3;\n p22=c6^2;\n p23=s6*p4;\n p24=p3*p9;\n p25=p6*p10;\n p26=s4*p5;\n "
" p27=c3*p1;\n p28=p7*p9;\n p29=p8*p10;\n p30=p3*p12;\n p31=p6*p14;\n p32=c4*p5;\n p33=c3*p2;\n"
" p34=p1*p15;\n p35=s6^2;\n p36=p7*p12;\n p37=p8*p14;\n p38=c5*wristOffs;\n p39=p2*p15;\n p4"
"0=s2*s3;\n p41=p3*p10;\n p42=p6*p9;\n p43=p7*p10;\n p44=c7*p11;\n p45=s3*s5;\n p46=p3*p14;\n "
" p47=c5*p19;\n p48=p5*p40;\n p49=p9*p13;\n p50=s7*p11;\n p51=p6*p12;\n p52=p7*p14;\n p53=s5*s"
"6;\n p54=p1*p3;\n p55=c3*p19;\n p56=p8*p12;\n p57=handOffs*p11;\n p58=s5*p4;\n p59=s6*c2;\n "
" p60=p1*p7;\n p61=p2*p6;\n p62=p3*p33;\n p63=p2*p8;\n p64=s2*s5;\n p65=c5*c7;\n p66=elbowOffs*"
"p4;\n p67=p6*p27;\n p68=p7*p33;\n p69=c2*p32;\n p70=s7*c5;\n p71=c7*p18;\n p72=s2*c6;\n p73"
"=s3*elbowOffs;\n p74=p8*p27;\n p75=p1*p6;\n p76=p2*p3;\n p77=s5*wristOffs;\n p78=c2*shoulderOffs;"
"\n p79=s7*p18;\n p80=c6*p4;\n p81=p11*p17;\n p82=p1*p8;\n p83=p2*p7;\n p84=p3*p34;\n p85=p6"
"*p39;\n p86=c5*p23;\n p87=elbowOffs*p21;\n p88=s5*c7;\n p89=wristOffs*p45;\n p90=handOffs*p18;\n "
" p91=s6*c6;\n p92=p11*p16;\n p93=p7*p34;\n p94=p8*p39;\n p95=c5*p20;\n p96=s5*s7;\n p97=p21*"
"p26;\n p98=s2*elbowOffs;\n p99=wristOffs*p58;\n p100=p3*p27;\n p101=s3*c6;\n p102=s6*c5;\n p10"
"3=p16*p18;\n p104=c2*p26;\n p105=handOffs*p35;\n p106=s2*p30;\n p107=p21*p32;\n p108=p7*p27;\n "
" p109=p6*p34;\n p110=p3*p39;\n p111=p17*p18;\n p112=c5*p4;\n p113=s2*p36;\n p114=p8*p34;\n p11"
"5=p6*p33;\n p116=p7*p39;\n p117=p26*p40;\n p118=p32*p55;\n p119=s3*c5;\n p120=s2*p31;\n p121=p"
"4*p24;\n p122=p16*p20;\n p123=p8*p33;\n p124=p40*p54;\n p125=s2*p37;\n p126=p4*p25;\n p127=s3*"
"p42;\n p128=p17*p20;\n p129=p16*p19;\n p130=p15*p24;\n p131=c5*p59;\n p132=p40*p60;\n p133=s2*"
"upArmLen;\n p134=s3*p49;\n p135=p4*p28;\n p136=p17*p19;\n p137=c3*p42;\n p138=s5*p20;\n p139=p"
"40*p61;\n p140=p26*p59;\n p141=s2*shoulderOffs;\n p142=s3*p41;\n p143=p4*p29;\n p144=p17*p23;\n "
" p145=c3*p49;\n p146=p15*p28;\n p147=handOffs*p19;\n p148=c2*upArmLen;\n p149=c6*p11;\n p150=p40"
"*p63;\n p151=p26*p55;\n p152=s3*p43;\n p153=p16*p23;\n p154=s2*p38;\n p155=c3*p41;\n p156=p15*"
"p25;\n p157=c2*p30;\n p158=handOffs*p22;\n p159=c6*p57;\n p160=p17*p50;\n p161=c7*p64;\n p162="
"p4*p38;\n p163=c3*p43;\n p164=p15*p29;\n p165=s5*p23;\n p166=c2*p36;\n p167=p22*p92;\n p168=p1"
"6*p44;\n p169=s7*p64;\n p170=c7*p45;\n p171=s2*lowArmLen;\n p172=s3*p38;\n p173=c3*p47;\n p174"
"=c2*p31;\n p175=p22*p81;\n p176=p16*p50;\n p177=p17*p44;\n p178=handOffs*p23;\n p179=s7*p45;\n "
" p180=c7*p58;\n p181=lowArmLen*p4;\n p182=s6*c3;\n p183=c2*p37;\n p184=s7*p58;\n p185=handOffs*p2"
"0;\n p186=p19*p45;\n p187=s6*p15;\n p188=s7*p19;\n p189=p5*p15;\n p190=s6*p92;\n p191=c7*p19;\n"
" p192=p5*p21;\n p193=s6*p81;\n p194=p21*p24;\n p195=p21*p28;\n p196=p21*p25;\n p197=p21*p29;\n"
" A(1,1) = -shoulderOffs*p75+shoulderOffs*p82-shoulderOffs*p76-shoulderOffs*p83;\n A(1,2) = -shoulderOffs*p"
"75+shoulderOffs*p82+p78*p75-p78*p82-shoulderOffs*p76-shoulderOffs*p83+p78*p76+p78*p83;\n A(1,3) = -shoulderOf"
"fs*p75+shoulderOffs*p82+p78*p75+p54*p73+p60*p73-p75*p133-p78*p82+p82*p133-shoulderOffs*p76-shoulderOffs*p83+p66*"
"p82-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p133-p66*p75-p66*p76-p66*p83;\n A(1,4) = -shoulderOffs*p75+s"
"houlderOffs*p82+p78*p75+p54*p73+p60*p73-p75*p133-p78*p82+p82*p133-shoulderOffs*p76-shoulderOffs*p83+p66*p82+p24*"
"p73+p56*p98+p28*p73-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p133-p66*p75-p51*p98-p66*p76-p66*p83-p46*p98-p2"
"5*p73-p52*p98+p29*p73-p42*p66+p49*p66-p41*p66-p43*p66;\n A(1,5) = -shoulderOffs*p75+shoulderOffs*p82+p78*p75+"
"p54*p73+p60*p73-p75*p133-p78*p82+p82*p133-shoulderOffs*p76-shoulderOffs*p83+p66*p82+p42*p171+s3*lowArmLen*p30+s3"
"*lowArmLen*p36+p24*p73+p56*p98+p28*p73+p77*p100+p77*p108-p49*p171-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p"
"133-p66*p75-p51*p98-p24*p172-p56*p154-p28*p172-p77*p114-p66*p76-p66*p83-s3*lowArmLen*p31-p46*p98-p25*p73-p52*p98"
"-p77*p115+p41*p171+p43*p171+s3*lowArmLen*p37+p29*p73+p77*p123-p51*p181-p42*p66+p51*p154+p56*p181+p49*p66+p77*p10"
"9-p46*p181-p52*p181-p41*p66-p43*p66+p46*p154+p25*p172+p52*p154+p77*p110+p77*p116-p29*p172+p42*p162-p49*p162+p41*"
"p162+p43*p162;\n A(1,6) = -shoulderOffs*p75+shoulderOffs*p82+p78*p75+p54*p73+p60*p73-p75*p133-p78*p82+p82*p13"
"3-shoulderOffs*p76-shoulderOffs*p83+p66*p82+p42*p171+s3*lowArmLen*p30+s3*lowArmLen*p36+p24*p73+p56*p98+p28*p73+p"
"77*p100+p77*p108-p49*p171-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p133-p66*p75-p51*p98-p24*p172-p56*p154-p2"
"8*p172-p77*p114+p49*p147-p66*p76-p66*p83-s3*lowArmLen*p31-p46*p98-p25*p73-p52*p98-p77*p115+p41*p171+p43*p171+s3*"
"lowArmLen*p37+p29*p73+p77*p123-p51*p181-p42*p66+p51*p154+p100*p90+p90*p108+p56*p181+p49*p66+p77*p109-p42*p147-p3"
"0*p185-p36*p185-p90*p115-p46*p181-p52*p181-p41*p66-p43*p66+p31*p185+p46*p154+p25*p172+p52*p154+p90*p123+p77*p110"
"+p77*p116-p41*p147-p43*p147-p37*p185-p29*p172+p51*p178+s2*p51*p57+p42*p162+p90*p109-s3*p24*p57-p56*p178-s2*p57*p"
"56-s3*p28*p57-p49*p162-p90*p114+p46*p178+s2*p46*p57+s3*p25*p57+p52*p178+s2*p52*p57+p41*p162+p43*p162+p90*p110+p9"
"0*p116-s3*p29*p57+p4*p42*p57-p4*p49*p57+p4*p41*p57+p4*p43*p57;\n A(1,7) = -p75+p82-p76-p83;\n A(1,9) = +p5"
"4+p60-p61+p63;\n A(1,10) = -c2*p75+c2*p82-c2*p76-c2*p83;\n A(1,11) = +p54+p60-p61+p63;\n A(1,12) = -s2*"
"p75+s2*p82-s2*p76-s2*p83;\n A(1,13) = +s3*p54+s3*p60+p4*p82-s3*p61+s3*p63-p4*p75-p4*p76-p4*p83;\n A(1,14) "
"= +s2*p75-s2*p82+s2*p76+s2*p83;\n A(1,15) = +p100+p108+p109-p114-p115+p123+p110+p116;\n A(1,16) = +s2*p51-"
"s3*p24-s2*p56-s3*p28+s2*p46+s3*p25+s2*p52-s3*p29+p4*p42-p4*p49+p4*p41+p4*p43;\n A(1,17) = +p100+p108+p109-p11"
"4-p115+p123+p110+p116;\n A(1,18) = +s2*p42+s3*p30+s3*p36-s2*p49-s3*p31+s2*p41+s2*p43+s3*p37-p4*p51+p4*p56-p4*"
"p46-p4*p52;\n A(1,19) = +s5*p100+s5*p108-s5*p114-s5*p115+s5*p123+s2*c5*p51+s5*p109-p24*p119-s2*c5*p56-p28*p11"
"9+s2*c5*p46+p25*p119+s2*c5*p52+s5*p110+s5*p116-p29*p119+p42*p112-p49*p112+p41*p112+p43*p112;\n A(1,20) = -s2*"
"p42-s3*p30-s3*p36+s2*p49+s3*p31-s2*p41-s2*p43-s3*p37+p4*p51-p4*p56+p4*p46+p4*p52;\n A(1,21) = +c5*p100+c5*p10"
"8+p24*p45+p56*p64+p28*p45-c5*p115+c5*p123+c5*p109-p51*p64-c5*p114+c5*p110+c5*p116-p46*p64-p25*p45-p52*p64+p29*p4"
"5-p42*p58+p49*p58-p41*p58-p43*p58;\n A(1,22) = +p19*p49+p18*p100+p18*p108-p19*p42-p20*p30-p20*p36-p18*p115+p2"
"0*p31+p18*p123-p19*p41-p19*p43-p20*p37+p23*p51+s2*p11*p51+p18*p109-s3*p11*p24-p23*p56-s2*p11*p56-s3*p11*p28-p18*"
"p114+p23*p46+s2*p11*p46+s3*p11*p25+p23*p52+s2*p11*p52+p18*p110+p18*p116-s3*p11*p29+p4*p11*p42-p4*p11*p49+p4*p11*"
"p41+p4*p11*p43;\n A(1,23) = +c5*p100+c5*p108+p24*p45+p56*p64+p28*p45-c5*p115+c5*p123+c5*p109-p51*p64-c5*p114+"
"c5*p110+c5*p116-p46*p64-p25*p45-p52*p64+p29*p45-p42*p58+p49*p58-p41*p58-p43*p58;\n A(1,24) = -p49*p72+p42*p72"
"+p30*p101+p36*p101+p53*p100+p53*p108-p31*p101-p53*p115+p41*p72+p43*p72+p37*p101+p53*p123-p51*p80+p47*p51+p56*p80"
"+p53*p109-p24*p95-p47*p56-p28*p95-p53*p114-p46*p80-p52*p80+p47*p46+p25*p95+p47*p52+p53*p110+p53*p116-p29*p95+p42"
"*p86-p49*p86+p41*p86+p43*p86;\n A(1,25) = +p70*p100+p70*p108-p70*p115+p70*p123+p70*p109+p71*p100+p71*p108-p51"
"*p169-p42*p191-c7*p20*p30-c7*p20*p36-p70*p114+p24*p179+p56*p169+p28*p179+p49*p191-p71*p115+c7*p20*p31+p70*p110+p"
"70*p116+p71*p123-p46*p169-p25*p179-p41*p191-p52*p169-p43*p191-c7*p20*p37+p29*p179+c7*p23*p51+s2*p44*p51+p71*p109"
"-p42*p184-s3*p24*p44-c7*p23*p56-s2*p44*p56-s3*p28*p44-p71*p114+p49*p184+p71*p110+p71*p116-p41*p184-p43*p184-s3*p"
"29*p44+p4*p44*p42-p4*p44*p49+c7*p23*p46+s2*p46*p44+s3*p25*p44+c7*p23*p52+s2*p44*p52+p4*p41*p44+p4*p44*p43;\n "
"A(1,26) = +p65*p100+p65*p108-p49*p188-p65*p115+p65*p123+p65*p109-p51*p161-p79*p100-p65*p114-p79*p108+p42*p188+p2"
"4*p170+s7*p20*p30+p56*p161+p28*p170+s7*p20*p36+p65*p110+p79*p115+p65*p116-p46*p161-p25*p170-s7*p20*p31-p52*p161-"
"p79*p123+p41*p188+p43*p188+p29*p170+s7*p20*p37-p42*p180-s7*p23*p51-s2*p51*p50-p79*p109+s3*p24*p50+p49*p180+s7*p2"
"3*p56+s2*p50*p56+s3*p28*p50+p79*p114-p41*p180-s7*p23*p46-s2*p46*p50-s3*p25*p50-p43*p180-s7*p23*p52-s2*p52*p50-p7"
"9*p110-p79*p116+s3*p29*p50-p4*p42*p50+p4*p50*p49-p4*p41*p50-p4*p43*p50;\n A(1,27) = -p49*p72+p42*p72+p30*p101"
"+p36*p101+p53*p100+p53*p108-p31*p101-p53*p115+p41*p72+p43*p72+p37*p101+p53*p123-p51*p80+p47*p51+p56*p80+p53*p109"
"-p24*p95-p47*p56-p28*p95-p53*p114-p46*p80-p52*p80+p47*p46+p25*p95+p47*p52+p53*p110+p53*p116-p29*p95+p42*p86-p49*"
"p86+p41*p86+p43*p86;\n\n A(2,2) = -p5*p78-p54*p141-p60*p141+p61*p141-p63*p141;\n A(2,3) = -p5*p78+p5*p133+"
"p5*p66-p54*p148-p60*p148-p54*p141-p60*p141+p54*p87+p60*p87+p61*p148+p61*p141-p63*p148-p63*p141-p61*p87+p63*p87;\n"
" A(2,4) = -p5*p78+p5*p133+p5*p66+p32*p98-p54*p148-p60*p148-p54*p141-p60*p141+p54*p87+p60*p87+p61*p148+p61*p14"
"1-p63*p148-p63*p141+p26*p66-elbowOffs*p157-elbowOffs*p166+elbowOffs*p174-p61*p87-elbowOffs*p183+p63*p87+p24*p87+"
"p28*p87-p25*p87+p29*p87;\n A(2,5) = -p5*p78+p5*p133+p5*p66+p32*p98-p26*p171-p54*p148-p60*p148-p54*p141-p60*p1"
"41+p54*p87+p60*p87-p77*p189+c2*lowArmLen*p24+c2*lowArmLen*p28+p61*p148+p61*p141-p63*p148-p63*p141+p32*p181+p26*p"
"66-elbowOffs*p157-elbowOffs*p166-p32*p154-p77*p124-p77*p132+elbowOffs*p174-p61*p87-c2*lowArmLen*p25-elbowOffs*p1"
"83+p63*p87+c2*lowArmLen*p29-p26*p162+p38*p157+p38*p166+lowArmLen*p21*p30+lowArmLen*p21*p36+p24*p87+p28*p87-p38*p"
"174-lowArmLen*p21*p31-p25*p87+p38*p183+lowArmLen*p21*p37+p29*p87+p77*p139-p77*p150-p38*p194-p38*p195+p38*p196-p3"
"8*p197;\n A(2,6) = -p5*p78+p5*p133+p5*p66+p32*p98-p26*p171-p54*p148-p60*p148-p54*p141-p60*p141+p54*p87+p60*p8"
"7-p77*p189+p26*p147+c2*lowArmLen*p24+c2*lowArmLen*p28+p61*p148+p61*p141-p63*p148-p63*p141+p32*p181+p26*p66-elbow"
"Offs*p157-elbowOffs*p166-p32*p154-p77*p124-p77*p132+elbowOffs*p174-p61*p87-c2*lowArmLen*p25-elbowOffs*p183+p63*p"
"87+c2*lowArmLen*p29-p32*p178-s2*p32*p57-p26*p162-p90*p189+p38*p157+p38*p166+lowArmLen*p21*p30+lowArmLen*p21*p36+"
"p24*p87+p28*p87-handOffs*p24*p59-handOffs*p28*p59-p38*p174-lowArmLen*p21*p31-p25*p87+handOffs*p25*p59+p38*p183+l"
"owArmLen*p21*p37+p29*p87+p77*p139-handOffs*p29*p59-p77*p150-p4*p26*p57+p57*p157+p57*p166-handOffs*p30*p55-handOf"
"fs*p36*p55-p38*p194-p38*p195-p90*p124-p90*p132+handOffs*p31*p55+p57*p183+p38*p196+p90*p139-handOffs*p37*p55-p38*"
"p197-p90*p150-p57*p194-p57*p195-p57*p174-p57*p197+p57*p196;\n A(2,10) = +c2*p5+s2*p54+s2*p60-s2*p61+s2*p63;\n"
" A(2,12) = +s2*p5-c2*p54-c2*p60+c2*p61-c2*p63;\n A(2,13) = +p5*p4+p21*p54+p21*p60-p21*p61+p21*p63;\n A("
"2,14) = -s2*p5+c2*p54+c2*p60-c2*p61+c2*p63;\n A(2,15) = -p189-p124-p132+p139-p150;\n A(2,16) = -s2*p32+p15"
"7+p166-p4*p26-p174+p183-p194-p195+p196-p197;\n A(2,17) = -p189-p124-p132+p139-p150;\n A(2,18) = -s2*p26+c2"
"*p24+c2*p28+p4*p32-c2*p25+c2*p29+p21*p30+p21*p36-p21*p31+p21*p37;\n A(2,19) = -s5*p189-s2*c5*p32-s5*p124-s5*p"
"132-p26*p112+c5*p157+c5*p166-c5*p174+c5*p183+s5*p139-s5*p150-c5*p194-c5*p195+c5*p196-c5*p197;\n A(2,20) = +s2"
"*p26-c2*p24-c2*p28-p4*p32+c2*p25-c2*p29-p21*p30-p21*p36+p21*p31-p21*p37;\n A(2,21) = +p32*p64-c5*p189+p26*p58"
"-s5*p157-s5*p166-c5*p124-c5*p132+s5*p174+c5*p139-s5*p183-c5*p150+s5*p194+s5*p195-s5*p196+s5*p197;\n A(2,22) ="
" +p19*p26-p23*p32-s2*p11*p32-p18*p189-p24*p59-p28*p59+p25*p59-p29*p59-p4*p11*p26+p11*p157+p11*p166-p30*p55-p36*p"
"55-p18*p124-p18*p132-p11*p174+p31*p55+p11*p183+p18*p139-p37*p55-p18*p150-p11*p194-p11*p195-p11*p197+p11*p196;\n "
" A(2,23) = +p32*p64-c5*p189+p26*p58-s5*p157-s5*p166-c5*p124-c5*p132+s5*p174+c5*p139-s5*p183-c5*p150+s5*p194+s5"
"*p195-s5*p196+s5*p197;\n A(2,24) = -p26*p72+p32*p80-p32*p47-p53*p189+c2*c6*p24+c2*c6*p28-c2*c6*p25+c2*c6*p29-"
"p26*p86+c6*p21*p30+p30*p131+c6*p21*p36+p36*p131-p54*p186-p60*p186-c6*p21*p31-p31*p131+c6*p21*p37+p37*p131+p61*p1"
"86-p63*p186-p24*p173-p28*p173+p25*p173-p29*p173;\n A(2,25) = -p70*p189+p32*p169+p26*p191-c7*p23*p32-s2*p32*p4"
"4-p71*p189+p26*p184-p96*p157-c7*p24*p59-p96*p166-c7*p28*p59-p70*p124-p70*p132+p96*p174+c7*p25*p59+p70*p139-p96*p"
"183-c7*p29*p59-p70*p150-p4*p26*p44+p44*p157+p44*p166-c7*p30*p55-c7*p36*p55-p71*p124-p71*p132+p96*p194+p96*p195+p"
"71*p139-p96*p196-c7*p37*p55-p71*p150+p96*p197-p44*p194-p44*p195-p44*p174+c7*p31*p55+p44*p183+p44*p196-p44*p197;\n"
" A(2,26) = -p65*p189+p32*p161-p26*p188+p26*p180+s7*p23*p32+s2*p32*p50+p79*p189-p88*p157-p88*p166-p65*p124-p65"
"*p132+s7*p24*p59+s7*p28*p59+p88*p174+p65*p139-s7*p25*p59-p88*p183-p65*p150+s7*p29*p59+p4*p26*p50-p50*p157-p50*p1"
"66+p88*p194+s7*p30*p55+p88*p195+s7*p36*p55+p79*p124+p79*p132-p88*p196-s7*p31*p55-p50*p183-p79*p139+p88*p197+s7*p"
"37*p55+p79*p150+p50*p194+p50*p195+p50*p174-p50*p196+p50*p197;\n A(2,27) = -p26*p72+p32*p80-p32*p47-p53*p189+c"
"2*c6*p24+c2*c6*p28-c2*c6*p25+c2*c6*p29-p26*p86+c6*p21*p30+p30*p131+c6*p21*p36+p36*p131-p54*p186-p60*p186-c6*p21*"
"p31-p31*p131+c6*p21*p37+p37*p131+p61*p186-p63*p186-p24*p173-p28*p173+p25*p173-p29*p173;\n\n A(3,3) = -elbowOf"
"fs*p48-elbowOffs*p67+elbowOffs*p74+elbowOffs*p84+elbowOffs*p93-elbowOffs*p62-elbowOffs*p68-elbowOffs*p85+elbowOf"
"fs*p94;\n A(3,4) = -elbowOffs*p67-elbowOffs*p48+elbowOffs*p74-elbowOffs*p117+elbowOffs*p84+elbowOffs*p93+elbo"
"wOffs*p145-elbowOffs*p62-elbowOffs*p68-elbowOffs*p137-elbowOffs*p85-elbowOffs*p155-elbowOffs*p163+elbowOffs*p94+"
"elbowOffs*p130+elbowOffs*p146-elbowOffs*p156+elbowOffs*p164;\n A(3,5) = -elbowOffs*p67-elbowOffs*p48+elbowOff"
"s*p74-lowArmLen*p32*p40-elbowOffs*p117+elbowOffs*p84+elbowOffs*p93-p77*p192+c3*lowArmLen*p56+elbowOffs*p145+p75*"
"p89-p89*p82-elbowOffs*p62-elbowOffs*p68-c3*lowArmLen*p51-elbowOffs*p137-p38*p145-elbowOffs*p85-c3*lowArmLen*p46-"
"c3*lowArmLen*p52-elbowOffs*p155-elbowOffs*p163+elbowOffs*p94+p76*p89+p89*p83+p38*p137+lowArmLen*p15*p30+lowArmLe"
"n*p15*p36+elbowOffs*p130+elbowOffs*p146+p38*p117+p54*p99+p60*p99-lowArmLen*p15*p31-elbowOffs*p156-p61*p99+p38*p1"
"55+p38*p163+lowArmLen*p15*p37+elbowOffs*p164+p63*p99-p38*p130-p38*p146+p38*p156-p38*p164;\n A(3,6) = -elbowOf"
"fs*p67-elbowOffs*p48+elbowOffs*p74-lowArmLen*p32*p40-elbowOffs*p117+elbowOffs*p84+elbowOffs*p93-p77*p192+c3*lowA"
"rmLen*p56+elbowOffs*p145+p75*p89-p89*p82-elbowOffs*p62-elbowOffs*p68-c3*lowArmLen*p51-elbowOffs*p137-p38*p145-s3"
"*p82*p90-elbowOffs*p85-c3*lowArmLen*p46-c3*lowArmLen*p52-elbowOffs*p155-elbowOffs*p163+elbowOffs*p94+p76*p89+p89"
"*p83-p90*p192+handOffs*p51*p182+p38*p137+s3*p32*p147+s3*p75*p90+lowArmLen*p15*p30+lowArmLen*p15*p36+elbowOffs*p1"
"30+elbowOffs*p146+p38*p117+p54*p99+p60*p99-handOffs*p56*p182-lowArmLen*p15*p31-elbowOffs*p156-p61*p99+handOffs*p"
"46*p182+handOffs*p52*p182+p38*p155+p38*p163+s3*p76*p90+s3*p83*p90+lowArmLen*p15*p37+elbowOffs*p164+p63*p99+p57*p"
"137+p57*p117+p4*p54*p90+p4*p60*p90-handOffs*p30*p187-handOffs*p36*p187-p57*p145-p38*p130-p38*p146-p4*p61*p90+han"
"dOffs*p31*p187+p57*p155+p57*p163+p38*p156+p4*p63*p90-handOffs*p37*p187-p38*p164-p57*p130-p57*p146-p57*p164+p57*p"
"156;\n A(3,13) = -p67-p48+p74+p84+p93-p62-p68-p85+p94;\n A(3,15) = +s3*p75-p192-s3*p82+p4*p54+p4*p60+s3*p7"
"6+s3*p83-p4*p61+p4*p63;\n A(3,16) = +p137+p117-p145+p155+p163-p130-p146+p156-p164;\n A(3,17) = -p192+s3*p7"
"5-s3*p82+p4*p54+p4*p60+s3*p76+s3*p83-p4*p61+p4*p63;\n A(3,18) = -p32*p40+c3*p56-c3*p51-c3*p46-c3*p52+p15*p30+"
"p15*p36-p15*p31+p15*p37;\n A(3,19) = -s5*p192+p45*p75-p45*p82+p45*p76+p45*p83+c5*p137+c5*p117+p54*p58+p60*p58"
"-c5*p145-p61*p58+c5*p155+c5*p163+p58*p63-c5*p130-c5*p146+c5*p156-c5*p164;\n A(3,20) = +p32*p40-c3*p56+c3*p51+"
"c3*p46+c3*p52-p15*p30-p15*p36+p15*p31-p15*p37;\n A(3,21) = +p75*p119-p82*p119-c5*p192+s5*p145+p76*p119+p83*p1"
"19+p54*p112+p60*p112-s5*p137-s5*p117-p61*p112+p63*p112-s5*p155-s5*p163+s5*p130+s5*p146-s5*p156+s5*p164;\n A(3"
",22) = -s3*p18*p82-p18*p192+p51*p182+s3*p19*p32+s3*p18*p75-p56*p182+p46*p182+p52*p182+s3*p18*p76+s3*p18*p83+p11*"
"p137+p11*p117+p4*p18*p54+p4*p18*p60-p30*p187-p36*p187-p11*p145-p4*p18*p61+p31*p187+p11*p155+p11*p163+p4*p18*p63-"
"p37*p187-p11*p130-p11*p146-p11*p164+p11*p156;\n A(3,23) = +p75*p119-p82*p119-c5*p192+s5*p145+p76*p119+p83*p11"
"9+p54*p112+p60*p112-s5*p137-s5*p117-p61*p112+p63*p112-s5*p155-s5*p163+s5*p130+s5*p146-s5*p156+s5*p164;\n A(3,"
"24) = -p82*p138-c3*c6*p51-c6*p32*p40-s5*p5*p55+c3*c6*p56+p75*p138-c3*c6*p46-c3*c6*p52+p76*p138+p83*p138+c6*p15*p"
"30+p102*p137+c6*p15*p36+s3*p26*p47+p54*p165+p60*p165-p102*p145-c6*p15*p31-p61*p165+p102*p155+c6*p15*p37+p102*p16"
"3+p63*p165-p102*p130-p102*p146+p102*p156-p102*p164;\n A(3,25) = -s3*p70*p82-p70*p192+s3*p75*p70+s3*p76*p70+s3"
"*p70*p83-p71*p192+c7*p51*p182+s3*p32*p191+p4*p54*p70+s3*p75*p71+p4*p60*p70-p96*p137-c7*p56*p182-p96*p117-s3*p71*"
"p82+p96*p145-p4*p61*p70+c7*p46*p182+c7*p52*p182+s3*p76*p71+p4*p63*p70+s3*p71*p83-p96*p155-p96*p163+p44*p137+p44*"
"p117+p4*p54*p71+p4*p60*p71-c7*p30*p187-c7*p36*p187-p44*p145+p96*p130+p96*p146+p4*p63*p71-p96*p156-c7*p37*p187+p9"
"6*p164-p44*p130-p44*p146-p4*p61*p71+c7*p31*p187+p44*p155+p44*p163+p44*p156-p44*p164;\n A(3,26) = -p65*p192+s3"
"*p65*p75-s3*p65*p82+s3*p65*p76+s3*p65*p83+p4*p54*p65+p4*p60*p65+p79*p192-p88*p137-s7*p51*p182-p88*p117-s3*p32*p1"
"88-s3*p75*p79+p88*p145+s7*p56*p182+s3*p82*p79-p4*p61*p65+p4*p65*p63-p88*p155-s7*p46*p182-p88*p163-s7*p52*p182-s3"
"*p76*p79-s3*p83*p79-p50*p137-p50*p117-p4*p54*p79-p4*p60*p79+p88*p130+s7*p30*p187+p88*p146+s7*p36*p187+p50*p145-p"
"88*p156-s7*p31*p187-p50*p155-p50*p163-p4*p63*p79+p88*p164+s7*p37*p187+p50*p130+p50*p146+p4*p61*p79-p50*p156+p50*"
"p164;\n A(3,27) = -p82*p138-c3*c6*p51-c6*p32*p40-s5*p5*p55+c3*c6*p56+p75*p138-c3*c6*p46-c3*c6*p52+p76*p138+p8"
"3*p138+c6*p15*p30+p102*p137+c6*p15*p36+s3*p26*p47+p54*p165+p60*p165-p102*p145-c6*p15*p31-p61*p165+p102*p155+c6*p"
"15*p37+p102*p163+p63*p165-p102*p130-p102*p146+p102*p156-p102*p164;\n\n A(4,4) = +elbowOffs*p104+p24*p98+p28*p"
"98+p56*p73+p32*p87-p51*p73-p25*p98-p46*p73-p52*p73+p29*p98-p30*p66-p36*p66+p31*p66-p37*p66;\n A(4,5) = +lowAr"
"mLen*p69+elbowOffs*p104-lowArmLen*p97+lowArmLen*p106+lowArmLen*p127+lowArmLen*p113+p24*p98+p28*p98+p56*p73-lowAr"
"mLen*p134+p32*p87-p51*p73-p38*p104-p24*p154-p28*p154-p56*p172-lowArmLen*p120-p25*p98-p46*p73-p52*p73+lowArmLen*p"
"142+lowArmLen*p125+lowArmLen*p152+p29*p98-p30*p66-p36*p66-p38*p107+p51*p172+lowArmLen*p121+lowArmLen*p135+p31*p6"
"6-lowArmLen*p126-p37*p66+p25*p154+p46*p172+p52*p172+lowArmLen*p143-p29*p154+p30*p162+p36*p162-p31*p162+p37*p162;"
"\n A(4,6) = +lowArmLen*p69+elbowOffs*p104-lowArmLen*p97+lowArmLen*p106+lowArmLen*p127+lowArmLen*p113+p24*p98+"
"p28*p98+p56*p73-lowArmLen*p134+p32*p87-handOffs*p32*p59-p51*p73-p38*p104-p24*p154-p28*p154-p56*p172+p49*p185-low"
"ArmLen*p120-p25*p98-p46*p73-p52*p73+lowArmLen*p142+lowArmLen*p125+lowArmLen*p152+p29*p98-p57*p104-p30*p66-p36*p6"
"6-p38*p107+p51*p172+handOffs*p151+lowArmLen*p121+lowArmLen*p135-p30*p147-p42*p185-p36*p147+p31*p66-lowArmLen*p12"
"6-p37*p66+p31*p147+p25*p154+p46*p172+p52*p172+lowArmLen*p143-p41*p185-p37*p147-p43*p185-p29*p154-p57*p107+s3*p51"
"*p57+p30*p162+p36*p162-p24*p178-s2*p24*p57-p28*p178-s2*p28*p57-s3*p57*p56-p31*p162+p25*p178+s2*p25*p57+s3*p46*p5"
"7+s3*p52*p57+p37*p162-p29*p178-s2*p29*p57+p4*p30*p57+p4*p36*p57-p4*p31*p57+p4*p37*p57;\n A(4,16) = -p104+s3*p"
"51-s2*p24-s2*p28-s3*p56-p107+s2*p25+s3*p46+s3*p52-s2*p29+p4*p30+p4*p36-p4*p31+p4*p37;\n A(4,18) = +p69-p97+p1"
"06+p127+p113-p134-p120+p142+p125+p152+p121+p135-p126+p143;\n A(4,19) = -c5*p104-c5*p107+p51*p119-s2*c5*p24-s2"
"*c5*p28-p56*p119+s2*c5*p25+p46*p119+p52*p119-s2*c5*p29+p30*p112+p36*p112-p31*p112+p37*p112;\n A(4,20) = -p69+"
"p97-p106-p127-p113+p134+p120-p142-p125-p152-p121-p135+p126-p143;\n A(4,21) = +s5*p104+p24*p64+p28*p64+p45*p56"
"+s5*p107-p45*p51-p25*p64-p46*p45-p45*p52+p29*p64-p30*p58-p36*p58+p31*p58-p37*p58;\n A(4,22) = -p32*p59+p20*p4"
"9-p11*p104+p151-p19*p30-p20*p42-p19*p36+p19*p31-p20*p41-p19*p37-p20*p43-p11*p107+s3*p11*p51-p24*p23-s2*p11*p24-p"
"23*p28-s2*p11*p28-s3*p11*p56+p25*p23+s2*p11*p25+s3*p11*p46+s3*p11*p52-p23*p29-s2*p11*p29+p4*p11*p30+p4*p11*p36-p"
"4*p11*p31+p4*p11*p37;\n A(4,23) = +s5*p104+p24*p64+p28*p64+p45*p56+s5*p107-p45*p51-p25*p64-p46*p45-p45*p52+p2"
"9*p64-p30*p58-p36*p58+p31*p58-p37*p58;\n A(4,24) = +c6*p69-p49*p101-c6*p97-p26*p131+p30*p72+p42*p101+p36*p72-"
"p31*p72+p41*p101+p37*p72+p43*p101-p32*p173+p24*p80+p51*p95+p28*p80-p24*p47-p28*p47-p56*p95-p25*p80+p25*p47+p46*p"
"95+p29*p80+p52*p95-p29*p47+p30*p86+p36*p86+p37*p86-p31*p86;\n A(4,25) = -c7*p32*p59+p96*p104-p44*p104+p96*p10"
"7+c7*p151-p51*p179-p30*p191-c7*p20*p42-p36*p191+p24*p169+p28*p169+p56*p179+c7*p20*p49+p31*p191-p25*p169-p46*p179"
"-c7*p20*p41-p52*p179-p37*p191-c7*p20*p43+p29*p169-p44*p107+s3*p44*p51-p30*p184-c7*p24*p23-s2*p24*p44-p36*p184-c7"
"*p23*p28-s2*p28*p44-s3*p44*p56-p37*p184-c7*p23*p29-s2*p29*p44+p4*p30*p44+p4*p36*p44+p31*p184+c7*p25*p23+s2*p25*p"
"44+s3*p46*p44+s3*p44*p52-p4*p31*p44+p4*p37*p44;\n A(4,26) = +p88*p104+s7*p32*p59-s7*p20*p49+p88*p107+p50*p104"
"-p51*p170-s7*p151+p24*p161+p30*p188+s7*p20*p42+p28*p161+p36*p188+p56*p170-p25*p161-p31*p188-p46*p170-p52*p170+s7"
"*p20*p41+p29*p161+p37*p188+s7*p20*p43+p50*p107-p30*p180-s3*p51*p50-p36*p180+s7*p24*p23+s2*p24*p50+s7*p23*p28+s2*"
"p28*p50+s3*p50*p56-s7*p25*p23-s2*p25*p50-s3*p46*p50-p37*p180-s3*p52*p50+s7*p23*p29+s2*p29*p50-p4*p30*p50-p4*p36*"
"p50+p31*p180+p4*p31*p50-p4*p37*p50;\n A(4,27) = +c6*p69-p49*p101-c6*p97-p26*p131+p30*p72+p42*p101+p36*p72-p31"
"*p72+p41*p101+p37*p72+p43*p101-p32*p173+p24*p80+p51*p95+p28*p80-p24*p47-p28*p47-p56*p95-p25*p80+p25*p47+p46*p95+"
"p29*p80+p52*p95-p29*p47+p30*p86+p36*p86+p37*p86-p31*p86;\n\n A(5,5) = -p48*p38+p38*p74-p69*p77-p38*p67-wristO"
"ffs*p30*p64-p42*p89-wristOffs*p36*p64+p49*p89-p38*p62-p38*p68+p77*p97+p38*p84+p38*p93-p38*p85+wristOffs*p31*p64+"
"p38*p94-p41*p89-wristOffs*p37*p64-p43*p89-p24*p99-p28*p99+p25*p99-p29*p99;\n A(5,6) = -p48*p38+p38*p74-p69*p7"
"7-p38*p67-wristOffs*p30*p64-p42*p89-wristOffs*p36*p64+p49*p89-p38*p62-p38*p68-p69*p90-p57*p67-p48*p57+p57*p74+p7"
"7*p97+p38*p84+p38*p93-p62*p57-p57*p68-p38*p85+wristOffs*p31*p64+p38*p94-p41*p89-wristOffs*p37*p64-p43*p89+p97*p9"
"0+p57*p84+p57*p93-p90*p106-p90*p127-p90*p113-p24*p99-p28*p99+p90*p134-p57*p85+p90*p120+p25*p99+p57*p94-p90*p142-"
"p90*p125-p90*p152-p29*p99-p90*p121-p90*p135-p90*p143+p90*p126;\n A(5,19) = -c5*p48+c5*p74-s5*p69-c5*p67+p45*p"
"49-c5*p62-c5*p68+s5*p97+c5*p84+c5*p93-p30*p64-p45*p42-p36*p64-c5*p85+p31*p64+c5*p94-p41*p45-p37*p64-p45*p43-p24*"
"p58-p28*p58+p25*p58-p29*p58;\n A(5,20) = 0;\n A(5,21) = +s5*p67+s5*p48-s5*p74-c5*p69+p49*p119+s5*p62+s5*p6"
"8+c5*p97-c5*p106-p42*p119-c5*p113-s5*p84-s5*p93+c5*p120+s5*p85-p41*p119-c5*p125-p43*p119-s5*p94-p24*p112-p28*p11"
"2+p25*p112-p29*p112;\n A(5,22) = -p18*p69-p11*p67-p11*p48+p11*p74-p11*p62-p11*p68+p18*p97+p11*p84+p11*p93-p18"
"*p106-p18*p127-p18*p113+p18*p134-p11*p85+p18*p120+p11*p94-p18*p142-p18*p125-p18*p152-p18*p121-p18*p135-p18*p143+"
"p18*p126;\n A(5,23) = +s5*p67+s5*p48-s5*p74-c5*p69+p49*p119+s5*p62+s5*p68+c5*p97-c5*p106-p42*p119-c5*p113-s5*"
"p84-s5*p93+c5*p120+s5*p85-p41*p119-c5*p125-p43*p119-s5*p94-p24*p112-p28*p112+p25*p112-p29*p112;\n A(5,24) = -"
"p53*p69-p67*p102-s3*p5*p47+p74*p102+p49*p138-p62*p102-p68*p102+s5*p151+p84*p102+p93*p102-s5*p19*p30-p42*p138-s5*"
"p19*p36-p85*p102+s5*p19*p31+p94*p102-p41*p138-s5*p19*p37-p43*p138-p24*p165-p28*p165+p25*p165-p29*p165;\n A(5,"
"25) = -p74*p96-p69*p70+p67*p96+p48*p96+p62*p96+p68*p96-p69*p71-p44*p67-p48*p44+p70*p97+p44*p74-p70*p106-p70*p127"
"-p70*p113-p84*p96-p96*p93+p70*p134-p44*p62-p44*p68+p70*p120+p85*p96-p70*p142-p70*p125-p70*p152-p96*p94+p71*p97+p"
"44*p84+p44*p93-p70*p121-p71*p106-p71*p127-p70*p135-p71*p113+p71*p134+p44*p94-p71*p142-p70*p143-p71*p125-p71*p152"
"-p71*p121-p71*p135-p44*p85+p70*p126+p71*p120+p71*p126-p71*p143;\n A(5,26) = -p69*p65+p67*p88+p48*p88-p74*p88+"
"p62*p88+p68*p88+p65*p97+p69*p79+p50*p67+p48*p50-p65*p106-p65*p127-p65*p113-p88*p84-p88*p93-p50*p74+p65*p134+p65*"
"p120+p88*p85+p50*p62+p50*p68-p65*p142-p65*p125-p65*p152-p88*p94-p65*p121-p65*p135-p79*p97-p50*p84-p50*p93+p79*p1"
"06+p79*p127+p79*p113-p79*p134-p79*p120-p65*p143-p50*p94+p79*p142+p79*p125+p79*p152+p79*p121+p79*p135+p65*p126+p5"
"0*p85-p79*p126+p79*p143;\n A(5,27) = -p53*p69-p67*p102-s3*p5*p47+p74*p102+p49*p138-p62*p102-p68*p102+s5*p151+"
"p84*p102+p93*p102-s5*p19*p30-p42*p138-s5*p19*p36-p85*p102+s5*p19*p31+p94*p102-p41*p138-s5*p19*p37-p43*p138-p24*p"
"165-p28*p165+p25*p165-p29*p165;\n\n A(6,6) = -c6*p104*p158-c6*p107*p158-p32*p105*p131-c6*p105*p104+p51*p101*p"
"158+p53*p67*p105+p5*p20*p53*p147-p24*p72*p158-p28*p72*p158-p56*p101*p158-p53*p74*p105-handOffs*p91*p118+p30*p80*"
"p158+p36*p80*p158+p26*p105*p173+p51*p91*p185+p67*p91*p90+p5*p22*p45*p147-p30*p47*p105-p42*p95*p105-p36*p47*p105-"
"p53*p84*p105-p24*p91*p147-p53*p93*p105-p28*p91*p147-p56*p91*p185-p74*p91*p90+p49*p95*p105+p25*p72*p158+p46*p101*"
"p158+p52*p101*p158+p53*p62*p105+p53*p68*p105-p29*p72*p158-p32*p59*p159-p84*p91*p90-p91*p93*p90+p20*p49*p159-p31*"
"p80*p158+p31*p47*p105+p37*p80*p158+p53*p85*p105+p25*p91*p147+p46*p91*p185+p62*p91*p90+p52*p91*p185+p68*p91*p90-p"
"41*p95*p105-p37*p47*p105-p43*p95*p105-p53*p94*p105-p29*p91*p147+p151*p159+p30*p91*p178+p36*p91*p178-p24*p86*p105"
"-p19*p30*p159-p20*p42*p159-p28*p86*p105-p19*p36*p159-p31*p91*p178+p25*p86*p105+p19*p31*p159+p85*p91*p90+p37*p91*"
"p178-p20*p41*p159-p29*p86*p105-p19*p37*p159-p20*p43*p159-p91*p94*p90-p23*p24*p159-p23*p28*p159+p23*p25*p159-p23*"
"p29*p159;\n A(6,22) = +c6*p22*p104-c6*p104-c6*p104-p53*p74-p53*p74+c6*p22*p107+p32*p35*p131+c6*p35*p104-p22*p"
"51*p101-p35*p53*p67-p5*p19*p20*p53+p22*p24*p72+p22*p28*p72+p22*p56*p101+p35*p53*p74-c6*p107-c6*p107-p32*p131-p32"
"*p131+p51*p101+p51*p101+p53*p67+p53*p67+p5*p186+p5*p186-p24*p72-p24*p72-p28*p72-p56*p101-p28*p72-p56*p101+p32*p5"
"9*p149+p91*p118-p22*p30*p80-p22*p36*p80-p26*p35*p173-p20*p51*p91-p18*p67*p91-p5*p22*p186+p30*p35*p47+p35*p42*p95"
"+p35*p36*p47+p35*p53*p84+p19*p24*p91+p35*p53*p93+p19*p28*p91+p20*p56*p91+p18*p74*p91+p25*p72+p46*p101+p25*p72+p4"
"6*p101+p52*p101+p52*p101-p35*p49*p95+p53*p62+p53*p62+p53*p68+p53*p68-p29*p72-p29*p72-p22*p25*p72-p22*p46*p101-p2"
"2*p52*p101+p30*p80+p30*p80+p36*p80+p36*p80-p35*p53*p62-p35*p53*p68+p26*p173+p26*p173+p22*p29*p72-p30*p47-p42*p95"
"-p30*p47-p42*p95-p36*p47-p36*p47-p53*p84-p53*p84-p53*p93-p53*p93+p49*p95+p49*p95-p31*p80-p31*p80+p24*p35*p86+p19"
"*p30*p149+p20*p42*p149+p28*p35*p86+p19*p36*p149+p18*p84*p91+p18*p93*p91+p31*p47+p31*p47+p37*p80+p37*p80-p20*p49*"
"p149+p53*p85+p53*p85-p41*p95-p41*p95-p37*p47-p43*p95-p37*p47-p43*p95-p53*p94-p53*p94+p22*p31*p80-p31*p35*p47-p22"
"*p37*p80-p35*p53*p85-p19*p25*p91-p20*p46*p91-p18*p62*p91-p20*p52*p91-p18*p68*p91+p35*p41*p95+p35*p37*p47+p35*p43"
"*p95-p24*p86-p24*p86-p28*p86-p28*p86+p35*p53*p94+p19*p29*p91-p151*p149-p23*p30*p91-p23*p36*p91-p29*p86-p29*p86+p"
"23*p31*p91-p25*p35*p86-p19*p31*p149-p18*p85*p91-p23*p37*p91+p20*p41*p149+p29*p35*p86+p19*p37*p149+p20*p43*p149+p"
"18*p94*p91+p23*p24*p149+p23*p28*p149+p25*p86+p25*p86-p23*p25*p149+p23*p29*p149;\n A(6,24) = -p140-p140-p19*p2"
"4-p19*p24-p19*p28-p20*p56-p19*p28-p20*p56+p16*p35*p140+p17*p35*p140+p11*p69+p11*p69-p118-p118-p18*p67-p18*p67-p1"
"8*p48-p18*p48+p20*p51+p20*p51+p18*p74+p18*p74-p69*p167-p18*p62-p18*p62-p18*p68-p18*p68-p69*p175+p19*p25+p20*p46+"
"p19*p25+p20*p46+p20*p52+p20*p52+p16*p35*p118+p16*p22*p140+p22*p67*p103+p17*p35*p118+p22*p48*p103+p17*p22*p140-p3"
"5*p51*p122+p22*p67*p111-p19*p29-p19*p29+p22*p48*p111-p35*p51*p128-p22*p74*p103+p24*p35*p129+p28*p35*p129+p35*p56"
"*p122-p22*p74*p111+p24*p35*p136+p28*p35*p136+p35*p56*p128-p11*p97-p11*p97+p23*p30+p23*p30+p11*p106+p11*p127+p11*"
"p106+p11*p127+p23*p36+p23*p36+p11*p113+p11*p113+p18*p84+p18*p84+p18*p93+p18*p93-p11*p134-p11*p134-p25*p35*p136-p"
"35*p46*p128-p35*p52*p128+p29*p35*p129-p23*p31-p23*p31-p11*p120-p11*p120+p29*p35*p136-p18*p85-p18*p85-p35*p69*p92"
"+p11*p142+p11*p142+p23*p37+p23*p37+p11*p125+p11*p152+p11*p125+p11*p152+p16*p22*p118+p97*p167-p35*p69*p81+p18*p94"
"+p18*p94+p17*p22*p118+p97*p175-p30*p35*p153-p106*p167-p22*p51*p122-p127*p167+p35*p67*p103-p35*p36*p153-p113*p167"
"+p5*p19*p20*p103-p30*p35*p144-p22*p84*p103-p106*p175-p22*p51*p128-p127*p175+p35*p67*p111-p35*p36*p144-p22*p93*p1"
"03-p113*p175+p5*p19*p20*p111-p22*p84*p111+p22*p24*p129-p22*p93*p111+p22*p28*p129+p22*p56*p122+p134*p167-p35*p74*"
"p103+p22*p24*p136+p22*p28*p136+p22*p56*p128+p134*p175-p35*p74*p111+p11*p121+p11*p121+p11*p135+p11*p135+p22*p62*p"
"103+p22*p68*p103-p25*p35*p129-p35*p46*p122+p22*p62*p111-p35*p52*p122+p22*p68*p111+p11*p143+p11*p143+p151*p190-p2"
"2*p30*p153-p121*p167-p22*p36*p153-p135*p167+p151*p193-p22*p30*p144-p121*p175-p19*p30*p190-p20*p42*p190-p22*p36*p"
"144-p135*p175-p19*p36*p190-p20*p54*p59*p103-p19*p30*p193-p20*p42*p193-p20*p59*p60*p103-p19*p36*p193-p20*p54*p59*"
"p111-p20*p59*p60*p111+p20*p49*p190+p20*p49*p193+p31*p35*p153+p120*p167+p31*p35*p144+p22*p85*p103+p120*p175+p22*p"
"85*p111-p22*p25*p129-p22*p46*p122-p142*p167+p35*p62*p103-p35*p37*p153-p125*p167-p22*p52*p122-p152*p167+p35*p68*p"
"103-p22*p25*p136-p22*p46*p128-p142*p175+p35*p62*p111-p35*p37*p144-p22*p94*p103-p125*p175-p22*p52*p128-p152*p175+"
"p35*p68*p111-p22*p94*p111+p22*p29*p129-p11*p126-p11*p126+p22*p29*p136-p35*p92*p121-p23*p28*p190-p35*p81*p121-p23"
"*p28*p193+p22*p31*p153+p126*p167+p22*p31*p144+p126*p175+p19*p31*p190+p20*p59*p61*p103+p19*p31*p193-p22*p37*p153-"
"p143*p167+p20*p59*p61*p111-p20*p41*p190-p22*p37*p144-p143*p175-p19*p37*p190-p20*p43*p190-p20*p41*p193-p20*p59*p6"
"3*p103-p19*p37*p193-p20*p43*p193-p20*p59*p63*p111+p35*p92*p126+p35*p81*p126-p23*p29*p190-p23*p29*p193;\n A(6,"
"25) = -c6*c7*p104-c6*c7*p107-p32*p59*p65+c7*p51*p101+c7*p53*p67+p5*p19*p170-c7*p24*p72-c7*p28*p72-c7*p56*p101-c7"
"*p53*p74+c7*p25*p72+c7*p46*p101+c7*p52*p101+c7*p53*p62+c7*p53*p68-c7*p29*p72+c7*p30*p80+c7*p36*p80+c7*p26*p173-c"
"7*p30*p47-p20*p42*p65-c7*p36*p47-c7*p53*p84-c7*p53*p93+p20*p49*p65+c7*p53*p85-p20*p41*p65-c7*p37*p47-p20*p43*p65"
"-c7*p53*p94-p23*p24*p65-p23*p28*p65-c7*p31*p80+c7*p31*p47+c7*p37*p80+p23*p25*p65-p23*p29*p65;\n A(6,26) = +s7"
"*c6*p104+s7*c6*p107+p32*p59*p70-s7*p51*p101-s7*p53*p67-p5*p19*p179+s7*p24*p72+s7*p28*p72+s7*p56*p101+s7*p53*p74-"
"s7*p25*p72-s7*p46*p101-s7*p52*p101-s7*p53*p62-s7*p53*p68+s7*p29*p72-s7*p30*p80-s7*p36*p80-s7*p26*p173+s7*p30*p47"
"+p20*p42*p70+s7*p36*p47+s7*p53*p84+s7*p53*p93-p20*p49*p70-s7*p31*p47-s7*p37*p80-s7*p53*p85+p20*p41*p70+s7*p37*p4"
"7+p20*p43*p70+s7*p53*p94+p23*p24*p70+p23*p28*p70+s7*p31*p80-p23*p25*p70+p23*p29*p70;\n A(6,27) = -p16*p140-p1"
"7*p140+p69*p81-p16*p118-p67*p103-p17*p118-p48*p103+p51*p122-p67*p111-p48*p111+p51*p128+p74*p103-p24*p129-p28*p12"
"9-p56*p122+p74*p111-p24*p136-p28*p136-p56*p128+p69*p92+p30*p144+p84*p103+p81*p106+p81*p127+p36*p144+p93*p103+p81"
"*p113+p84*p111+p93*p111-p92*p134-p81*p134-p62*p103-p68*p103+p25*p129+p46*p122-p62*p111+p52*p122-p68*p111+p25*p13"
"6+p46*p128+p52*p128-p29*p129-p29*p136-p97*p92-p81*p97+p30*p153+p92*p106+p92*p127+p36*p153+p92*p113-p31*p153-p92*"
"p120-p31*p144-p85*p103-p81*p120-p85*p111+p92*p142+p37*p153+p92*p125+p92*p152+p81*p142+p37*p144+p94*p103+p81*p125"
"+p81*p152+p94*p111+p92*p121+p92*p135+p81*p121+p81*p135-p92*p126-p81*p126+p92*p143+p81*p143;\n\n A(7,25) = +s7"
"*p17*p140-p69*p88-p67*p65-p48*p65+p65*p74-p69*p160+s7*p17*p118+s7*p16*p140+p17*p67*p79+p17*p48*p79-s7*p51*p128-p"
"62*p65-p68*p65-p17*p74*p79+s7*p24*p136+s7*p28*p136+s7*p56*p128+p88*p97+p65*p84+p65*p93-p30*p161-p42*p170-p36*p16"
"1+p49*p170+p16*p48*p79-s7*p30*p144-p106*p160-p127*p160-s7*p51*p122-s7*p36*p144-p113*p160-p65*p85-p17*p84*p79-p17"
"*p79*p93-p16*p74*p79+p31*p161+s7*p24*p129+s7*p28*p129+p134*p160+s7*p56*p122+p65*p94-p41*p170-p37*p161-p43*p170+p"
"17*p62*p79+p17*p68*p79-s7*p25*p136-s7*p46*p128-s7*p52*p128-p24*p180-p28*p180+s7*p29*p136-p69*p176+p97*p160+s7*p1"
"6*p118+p16*p67*p79-p29*p180+s7*p31*p144+p120*p160+p17*p85*p79+p16*p62*p79+p16*p68*p79-s7*p25*p129-p142*p160-s7*p"
"46*p122-s7*p37*p144-p125*p160-p160*p152-s7*p52*p122-p17*p79*p94+s7*p29*p129+p97*p176-p121*p160-s7*p30*p153-p106*"
"p176-p127*p176-p135*p160-s7*p36*p153-p113*p176-p16*p84*p79-p16*p79*p93+p25*p180+p134*p176+p126*p160+s7*p31*p153+"
"p120*p176+p16*p85*p79-p142*p176-p143*p160-s7*p37*p153-p125*p176-p152*p176-p16*p79*p94-p121*p176-p135*p176+p126*p"
"176-p143*p176;\n A(7,26) = +c7*p16*p140+p69*p96+p67*p70+p48*p70-p74*p70-p49*p179+c7*p16*p118+p16*p67*p71+p16*"
"p48*p71+c7*p17*p140-c7*p51*p122-p16*p74*p71+c7*p24*p129+c7*p28*p129+c7*p56*p122+p62*p70+p68*p70-p97*p96-p70*p84-"
"p70*p93+p30*p169+p42*p179+p36*p169-p69*p168+p17*p48*p71-p16*p71*p84-c7*p51*p128-p16*p71*p93+p134*p168-p17*p74*p7"
"1+p70*p85+c7*p24*p136+c7*p28*p136+c7*p56*p128-p31*p169-p70*p94+p41*p179+p37*p169+p43*p179+p16*p62*p71+p16*p68*p7"
"1-c7*p25*p129-c7*p46*p122-c7*p52*p122+c7*p29*p129+p24*p184+p28*p184-p69*p177+p97*p168+c7*p17*p118-c7*p30*p153-p1"
"06*p168-p127*p168+p17*p67*p71-c7*p36*p153-p113*p168-p25*p184+p29*p184+c7*p31*p153+p120*p168+p16*p71*p85-p142*p16"
"8+p17*p62*p71-c7*p37*p153-p125*p168-p152*p168+p17*p68*p71-c7*p25*p136-c7*p46*p128-p16*p71*p94-c7*p52*p128+c7*p29"
"*p136+p97*p177-p121*p168-p135*p168-c7*p30*p144-p106*p177-p127*p177-c7*p36*p144-p113*p177-p17*p71*p84-p17*p71*p93"
"+p134*p177+p126*p168+c7*p31*p144+p120*p177+p17*p71*p85-p143*p168-p142*p177-c7*p37*p144-p125*p177-p152*p177-p17*p"
"71*p94-p121*p177-p135*p177+p126*p177-p143*p177;\n\nend"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 15
position [23.5747 49.5747 7]
chart 13
linkNode [13 0 0]
subviewer 13
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 16
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 15
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 13
linkNode [13 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 13
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 17
ssIdNumber 8
name "q_motor"
linkNode [13 0 18]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 18
ssIdNumber 9
name "x_hat"
linkNode [13 17 19]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 19
ssIdNumber 10
name "gearRatio"
linkNode [13 18 20]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 20
ssIdNumber 12
name "baseAngles"
linkNode [13 19 21]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 21
ssIdNumber 13
name "gravTrq"
linkNode [13 20 22]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 22
ssIdNumber 14
name "gravTrqMotor"
linkNode [13 21 23]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 23
ssIdNumber 15
name "coulombWidthMotor"
linkNode [13 22 24]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 24
ssIdNumber 16
name "coulombWidth"
linkNode [13 23 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 25
name "Left/calcGravityTorque/calcGravTrq"
machine 1
chart 13
}
chart {
id 26
name "botharms_get_torque/MATLAB Function2"
windowPosition [255.375 98.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 27 0 0]
firstTransition 29
firstJunction 28
viewObj 26
machine 1
toolbarMode LIBRARY_TOOLBAR
ssIdHighWaterMark 5
decomposition CLUSTER_CHART
type EML_CHART
firstData 30
chartFileNumber 3
disableImplicitCasting 1
eml {
name "abb2logical"
}
}
state {
id 27
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 26
treeNode [26 0 0 0]
superState SUBCHART
subviewer 26
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function logical = abb2logical(abb)\n%#codegen\n\nlogical = [abb(1:2); abb(7); abb(3:6)];"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 28
position [23.5747 49.5747 7]
chart 26
linkNode [26 0 0]
subviewer 26
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 29
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 28
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 26
linkNode [26 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 26
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 30
ssIdNumber 4
name "abb"
linkNode [26 0 31]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 31
ssIdNumber 5
name "logical"
linkNode [26 30 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 32
name "botharms_get_torque/MATLAB Function2"
machine 1
chart 26
}
chart {
id 33
name "botharms_get_torque/lp"
windowPosition [150.188 68.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 34 0 0]
firstTransition 36
firstJunction 35
viewObj 33
machine 1
toolbarMode LIBRARY_TOOLBAR
ssIdHighWaterMark 5
decomposition CLUSTER_CHART
type EML_CHART
firstData 37
chartFileNumber 4
disableImplicitCasting 1
eml {
name "lp"
}
}
state {
id 34
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 33
treeNode [33 0 0 0]
superState SUBCHART
subviewer 33
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function v_out = lp(v_in)\n%#codegen\n\n\npersistent v_flt\nif isempty(v_flt)\n v_flt = v_in;\n"
"end\nT = 0.016;\ndt = 0.004;\n\nv_dot = (v_in - v_flt)/T;\n\nv_flt = v_flt + dt*v_dot;\n\nv_out = v_flt;\n"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 35
position [23.5747 49.5747 7]
chart 33
linkNode [33 0 0]
subviewer 33
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 36
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 35
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 33
linkNode [33 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 33
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 37
ssIdNumber 4
name "v_in"
linkNode [33 0 38]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 38
ssIdNumber 5
name "v_out"
linkNode [33 37 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 39
name "botharms_get_torque/lp"
machine 1
chart 33
}
chart {
id 40
name "botharms_get_torque/MATLAB Function5"
windowPosition [375.375 -21.75 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 41 0 0]
firstTransition 43
firstJunction 42
viewObj 40
machine 1
ssIdHighWaterMark 28
decomposition CLUSTER_CHART
type EML_CHART
firstData 44
updateMethod DISCRETE
sampleTime "0.004"
chartFileNumber 5
disableImplicitCasting 1
eml {
name "nancheck"
}
}
state {
id 41
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 40
treeNode [40 0 0 0]
superState SUBCHART
subviewer 40
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function y = nancheck(u)\n\n\ny = u;\n\n\nif any(isnan(u)) || any(isinf(u))\n y = zeros(size"
"(u));\nend\n\nend"
editorLayout "100 M4x1[772 19 980 1190]"
}
}
junction {
id 42
position [23.5747 49.5747 7]
chart 40
linkNode [40 0 0]
subviewer 40
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 43
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 42
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 40
linkNode [40 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 40
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 44
ssIdNumber 28
name "y"
linkNode [40 0 45]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 45
ssIdNumber 4
name "u"
linkNode [40 44 0]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
instance {
id 46
name "botharms_get_torque/MATLAB Function5"
machine 1
chart 40
}
chart {
id 47
name "MATLAB Function3"
windowPosition [330.375 23.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 48 0 0]
firstTransition 50
firstJunction 49
viewObj 47
machine 1
ssIdHighWaterMark 28
decomposition CLUSTER_CHART
type EML_CHART
firstData 51
updateMethod DISCRETE
sampleTime "0.004"
chartFileNumber 6
disableImplicitCasting 1
eml {
name "nancheck"
}
}
state {
id 48
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 47
treeNode [47 0 0 0]
superState SUBCHART
subviewer 47
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function y = nancheck(u)\n\n\ny = u;\n\n\nif any(isnan(u)) || any(isinf(u))\n y = zeros(size"
"(u));\nend\n\nend"
editorLayout "100 M4x1[278 256 700 500]"
}
}
junction {
id 49
position [23.5747 49.5747 7]
chart 47
linkNode [47 0 0]
subviewer 47
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 50
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 49
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 47
linkNode [47 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 47
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 51
ssIdNumber 28
name "y"
linkNode [47 0 52]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 52
ssIdNumber 4
name "u"
linkNode [47 51 0]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
instance {
id 53
name "MATLAB Function3"
machine 1
chart 47
}
chart {
id 54
name "MATLAB Function1"
windowPosition [315.375 38.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 55 0 0]
firstTransition 57
firstJunction 56
viewObj 54
machine 1
ssIdHighWaterMark 28
decomposition CLUSTER_CHART
type EML_CHART
firstData 58
updateMethod DISCRETE
sampleTime "0.004"
chartFileNumber 7
disableImplicitCasting 1
eml {
name "nancheck"
}
}
state {
id 55
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 54
treeNode [54 0 0 0]
superState SUBCHART
subviewer 54
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function y = nancheck(u)\n\n\ny = u;\n\n\nif any(isnan(u)) || any(isinf(u))\n y = zeros(size"
"(u));\nend\n\nend"
editorLayout "100 M4x1[278 256 700 500]"
}
}
junction {
id 56
position [23.5747 49.5747 7]
chart 54
linkNode [54 0 0]
subviewer 54
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 57
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 56
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 54
linkNode [54 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 54
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 58
ssIdNumber 28
name "y"
linkNode [54 0 59]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 59
ssIdNumber 4
name "u"
linkNode [54 58 0]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
instance {
id 60
name "MATLAB Function1"
machine 1
chart 54
}
chart {
id 61
name "botharms_get_torque/FRIDA arm acceleration NOT SI/base_tool_transform"
windowPosition [270.375 98.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 62 0 0]
firstTransition 64
firstJunction 63
viewObj 61
machine 1
ssIdHighWaterMark 13
decomposition CLUSTER_CHART
type EML_CHART
firstData 65
chartFileNumber 8
disableImplicitCasting 1
eml {
name "base_tool_transform"
}
}
state {
id 62
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 61
treeNode [61 0 0 0]
superState SUBCHART
subviewer 61
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [T44_out, J0_out, Jn_out, Jdot0_out] = base_tool_transform(T44_world2base, T44_flange2to"
"ol, T44_in, J0_in, qd, Jdot0_in)\n\nT44_world2base_m = reshape(T44_world2base, 4, 4)';\nT44_flange2tool_m = resh"
"ape(T44_flange2tool, 4, 4)';\nT44_in_m = reshape(T44_in, 4, 4)';\nJ0_in_m = reshape(J0_in, [], 6)';\nJdot0_in_m "
"= reshape(Jdot0_in, [], 6)';\n\n% Forward kinematics\nT44_out_m = T44_world2base_m*T44_in_m*T44_flange2tool_m;\n"
"\n% Rotation of base\nRbase = T44_world2base_m(1:3,1:3);\nTbase = [Rbase zeros(3);zeros(3) Rbase];\nJ0_m = Tbase"
"*J0_in_m;\nJdot0_m = Tbase*Jdot0_in_m;\n\n% Handle tool transformation\nT44_baseToFlange = T44_world2base_m*T44_"
"in_m;\nT44_baseToTcp = T44_out_m;\nr = T44_baseToTcp(1:3,4) - T44_baseToFlange(1:3,4);\nomega = J0_m(4:6,:);\n\n"
"for i=1:size(Jdot0_m,2)\n jContribution = cross(omega(:,i),r);\n J0_m(1:3,i) = J0_m(1:3,i) + jContribution"
";\n for j=1:size(Jdot0_m,2)\n jdotContribution = cross(omega(:,i),cross(omega(:,j)*qd(j),r));\n "
" Jdot0_m(1:3,i) = Jdot0_m(1:3,i) + jdotContribution;\n end\nend\n\n% Jacobian in tool frame\nJn_m = [T44_out_"
"m(1:3,1:3)' zeros(3);zeros(3) T44_out_m(1:3,1:3)']*J0_m;\n\n \nT44_out = reshape(T44_out_m', 16, 1);\nJ0_o"
"ut = reshape(J0_m', [], 1);\nJn_out = reshape(Jn_m', [], 1);\nJdot0_out = reshape(Jdot0_m',[],1);\n\n\n"
editorLayout "100 M4x1[205 227 1080 733]"
}
}
junction {
id 63
position [23.5747 49.5747 7]
chart 61
linkNode [61 0 0]
subviewer 61
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 64
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 63
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 61
linkNode [61 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 61
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 65
ssIdNumber 4
name "T44_world2base"
linkNode [61 0 66]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 66
ssIdNumber 5
name "T44_out"
linkNode [61 65 67]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 67
ssIdNumber 6
name "T44_flange2tool"
linkNode [61 66 68]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 68
ssIdNumber 7
name "T44_in"
linkNode [61 67 69]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 69
ssIdNumber 8
name "J0_in"
linkNode [61 68 70]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 70
ssIdNumber 9
name "qd"
linkNode [61 69 71]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 71
ssIdNumber 10
name "Jdot0_in"
linkNode [61 70 72]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 72
ssIdNumber 11
name "J0_out"
linkNode [61 71 73]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 73
ssIdNumber 12
name "Jn_out"
linkNode [61 72 74]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 74
ssIdNumber 13
name "Jdot0_out"
linkNode [61 73 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 75
name "botharms_get_torque/FRIDA arm acceleration NOT SI/base_tool_transform"
machine 1
chart 61
}
chart {
id 76
name "MATLAB Function2"
windowPosition [315.375 38.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 77 0 0]
firstTransition 79
firstJunction 78
viewObj 76
machine 1
ssIdHighWaterMark 28
decomposition CLUSTER_CHART
type EML_CHART
firstData 80
updateMethod DISCRETE
sampleTime "0.004"
chartFileNumber 9
disableImplicitCasting 1
eml {
name "nancheck"
}
}
state {
id 77
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 76
treeNode [76 0 0 0]
superState SUBCHART
subviewer 76
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function y = nancheck(u)\n\n\ny = u;\n\n\nif any(isnan(u)) || any(isinf(u))\n y = zeros(size"
"(u));\nend\n\nend"
editorLayout "100 M4x1[772 19 980 1190]"
}
}
junction {
id 78
position [23.5747 49.5747 7]
chart 76
linkNode [76 0 0]
subviewer 76
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 79
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 78
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 76
linkNode [76 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 76
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 80
ssIdNumber 28
name "y"
linkNode [76 0 81]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 81
ssIdNumber 4
name "u"
linkNode [76 80 0]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
instance {
id 82
name "MATLAB Function2"
machine 1
chart 76
}
chart {
id 83
name "MATLAB Function4"
windowPosition [330.375 23.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 84 0 0]
firstTransition 86
firstJunction 85
viewObj 83
machine 1
ssIdHighWaterMark 28
decomposition CLUSTER_CHART
type EML_CHART
firstData 87
updateMethod DISCRETE
sampleTime "0.004"
chartFileNumber 10
disableImplicitCasting 1
eml {
name "nancheck"
}
}
state {
id 84
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 83
treeNode [83 0 0 0]
superState SUBCHART
subviewer 83
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function y = nancheck(u)\n\n\ny = u;\n\n\nif any(isnan(u)) || any(isinf(u))\n y = zeros(size"
"(u));\nend\n\nend"
editorLayout "100 M4x1[278 256 700 500]"
}
}
junction {
id 85
position [23.5747 49.5747 7]
chart 83
linkNode [83 0 0]
subviewer 83
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 86
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 85
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 83
linkNode [83 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 83
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 87
ssIdNumber 28
name "y"
linkNode [83 0 88]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 88
ssIdNumber 4
name "u"
linkNode [83 87 0]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
instance {
id 89
name "MATLAB Function4"
machine 1
chart 83
}
chart {
id 90
name "botharms_get_torque/MATLAB Function1"
windowPosition [270.375 98.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 91 0 0]
firstTransition 93
firstJunction 92
viewObj 90
machine 1
ssIdHighWaterMark 12
decomposition CLUSTER_CHART
type EML_CHART
firstData 94
chartFileNumber 11
disableImplicitCasting 1
eml {
name "fcn"
}
}
state {
id 91
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 90
treeNode [90 0 0 0]
superState SUBCHART
subviewer 90
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [e, scale] = fcn(T44_FK1, T44_FK2)\n%#codegen\n\n% Set scale:\nscale = 0.5;\n\n% Resha"
"ping:\nT441 = reshape(T44_FK1, 4,4)';\nT442 = reshape(T44_FK2, 4,4)';\n\nR1 = T441(1:3,1:3);\nR2 = T442(1:3,1:3)"
";\n\np1 = T441(1:3,4);\np2 = scale*T442(1:3,4);\n\n% Position:\npersistent delta_p\nif isempty(delta_p)\n del"
"ta_p = p2 - p1;\nend\nep = p2 - (p1 + delta_p);\n \n\n% Rotation:\npersistent delta_R\nif isempty(delta_R)\n"
" delta_R = R1'*R2;\nend\nR = R2*(R1*delta_R)';\n\n\nRxx = R(1,1); Rxy = R(1,2); Rxz = R(1,3);\nRyx = R(2,1); "
"Ryy = R(2,2); Ryz = R(2,3);\nRzx = R(3,1); Rzy = R(3,2); Rzz = R(3,3);\n\nw = sqrt( trace( R ) + 1 ) / 2;\n\n% c"
"heck if w is real. Otherwise, zero it.\nif( imag( w ) > 0 )\n w = 0;\nend\n\nx1 = ( Rzy - Ryz ) / (4*w);\ny1"
" = ( Rxz - Rzx ) / (4*w);\nz1 = ( Ryx - Rxy ) / (4*w);\nq1 = [x1; y1; z1];\n\nif any(isnan(q1)) || any(isinf(q1)"
")\n q1 = zeros(size(q1));\nend\n\npersistent q_old\nif isempty(q_old)\n q_old = q1;\nend\n\nq2 = -q1;\nif "
"(norm(q_old - q1) < norm(q_old-q2)) || norm(q1)<.1\n q_out = q1;\nelse\n q_out = q2;\nend\n\n\nq_old = q_o"
"ut;\n\ne = [ep ; q_out];\n\n"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 92
position [23.5747 49.5747 7]
chart 90
linkNode [90 0 0]
subviewer 90
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 93
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 92
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 90
linkNode [90 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 90
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 94
ssIdNumber 7
name "T44_FK1"
linkNode [90 0 95]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 95
ssIdNumber 4
name "e"
linkNode [90 94 96]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 96
ssIdNumber 11
name "T44_FK2"
linkNode [90 95 97]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 97
ssIdNumber 12
name "scale"
linkNode [90 96 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 98
name "botharms_get_torque/MATLAB Function1"
machine 1
chart 90
}
chart {
id 99
name "Left/leadthroug control/MATLAB Function"
windowPosition [390.75 188.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 100 0 0]
firstTransition 102
firstJunction 101
viewObj 99
machine 1
ssIdHighWaterMark 34
decomposition CLUSTER_CHART
type EML_CHART
firstData 103
chartFileNumber 12
disableImplicitCasting 1
eml {
name "fcn"
}
}
state {
id 100
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 99
treeNode [99 0 0 0]
superState SUBCHART
subviewer 99
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [Kp,Kv,Ki,trqFfw,posRef,test,sat] = fcn(Kp_in,Kv_in,Ki_in,gravTrqMotor,trqRaw,posRefIn,p"
"osRaw,f_switch,velFlt,highKiActive,keepStill,velFlt_noExtraFilt,coulomb_width,percentageTrqFfw,KiHighThres,slidi"
"ngThresWidth,maxRate,resetPosRef,jointLimitStartMotorRad,extTorque,sensorOn,fricHelp,qtorque)\n%#codegen\n% All "
"vectors are given in ABB-order\n\npersistent torqueFeedforward;\n % Used to account for the accumulate"
"d torqe feedforward (size 7x1)\npersistent posRefOffset;\n % The offset from the incoming position ref"
"erence, needs to be\n % updated to the current position to avoid the safety system of\n % th"
"e ABB controller to give errors\npersistent trqUpdateAddition;\n % The torque feedforward addition is "
"saved in this variable. The\n % first column is the total torque feedforward, the second\n %"
" column the \"feedforward\" part, i.e., the desired torque which\n % has to be corrected with a feedba"
"ck part to handle\n % disturbances. The last column contains a counter that is used\n % to a"
"void using feedback 3 samples after any feedforward change\n % (3 samples is the delay from the feedfo"
"rward signal is set\n % until a change can be seen in trqRaw)\npersistent trqRawFlt;\n % Fil"
"tered torque signal\npersistent keepKiClosed; \n % Variable of size 7x1 (one for each joint). If zero,"
" Ki should \n % be active for the respective joints. Also works as a counter, \n % it is set"
" to a value when a joint is moving, and then counted \n % down when the joint is still (below a thresh"
"old)\npersistent closeKiCounter; \n % Variable of size 7x2. It is used to make sure that a detected\n"
" % torque will have time to make an impact. When a torque is\n % detected with Ki active, cl"
"oseKiCounter is set to a value that\n % is then counted down. While closeKiCounter is non-zero, Ki\n "
" % will not be active and a torque feedforward will be used. The\n % first column is used if a"
" negative torque is detected, and the\n % second column if a positive torque is detected\n \n"
"persistent startOffset;\n % Variable used to handle the offset torque present for each\n % j"
"oint. The first column is used to save this torque, the\n % second column to tell if it has been set o"
"r not\npersistent extraTorqueOut;\n % Variable to handle ramping of the extra feedforward torque\npers"
"istent trqAddHistory\n % The history of added feedforward torques. It is used to\n % predict"
" trqRaw during the delay that is present.\n \npersistent saturated; \n\n\n\nlength_mean = [8"
"0;80;80;80;80;80;80];\ndelay_thres = [60;60;60;60;60;60;60];% length_mean has to be larger than delay thres\nrec"
"overyFactor = [1.5;1.5;1.5;1.5;1.5;1.5;1.5];\n% slidingThresWidth = [0.006;0.006;0.003;0.00045;0.0006;0.0004;0.0"
"03;];\n % These are parameters for detection of torque when Ki are\n % active\n \n"
"trqFiltConstant = [0.8;0.8;0.8;0.8;0.8;0.8;0.8];\n % The filter constant for the torque low pass filte"
"r\n\n% KiHighThres = [0.1 1;0.1 1;0.1 1;0.3 1;0.1 1;0.1 1;0.1 1]; The first\n% column is the threshold for the f"
"iltered version of velFlt, and the\n% second column the threshold for the non-filtered version of velFlt\nKiHigh"
"Delay = [30 ; 30 ; 30 ; 30 ; 30 ; 30 ; 30];\n % The standard delay for counting down keepKiClosed when"
" the\n % measured velocity is below KiHighThres\n \nhighVelThres = [5;5;5;5;5;5;5];\nlowVelT"
"hres = [2;2;2;2;2;2;2];\n % Thresholds for high and low velocities. Full torque\n % feedforw"
"ard is active when the velocity is above highVelThres,\n % and reduced feedforward when above lowVelTh"
"res (proportional\n % to velocity)\n\n \nuseKp = [0;0;0;0;0;0;0]; %Never use Kp\nuseKv = [1;"
"1;1;0;0;0;1]; %Kv needs to be active for the base joints (1,2,3, and 7)\n % When Ki is active, these v"
"ariables control if also Kp and Kv\n % should be active\n%TESTING:\npercentageTrqFfw = 0.9*ones(7,1);\n"
"percentageFfwPos = percentageTrqFfw;\npercentageFfwNeg = percentageTrqFfw;\nposFric = coulomb_width;\nnegFric = "
"coulomb_width;\n % Variables to control the amount of torque feedforward\n \nK = 1/3;\n %"
" The proportional feedback parameter for the feedback torque control\n % loop\n\ngearRatio = [100;100;-100;10"
"0;-101;100;100];\n\npersistent sampleIndex sampleIndexMod trqFltVec trqFltDelayVec;\nif isempty(sampleIndexMod) "
"|| f_switch==0\n sampleIndex = zeros(7,1);\n sampleIndexMod = zeros(7,1); % obs! zero indexed, add 1 when "
"used as index\n trqFltVec = repmat(trqRaw',max(length_mean),1);\n trqFltDelayVec = trqFltVec;\n trqRawF"
"lt = trqRaw;\nend\nfor k=1:7\n if abs(velFlt(k))>KiHighThres(k,1) || abs(velFlt_noExtraFilt(k))>KiHighThres(k"
",2)\n sampleIndex(k) = 0;\n sampleIndexMod(k) = 0; % obs! zero indexed, add 1 when used as index\n"
" trqFltVec(:,k) = ones(max(length_mean),1)*trqRaw(k);\n trqFltDelayVec(:,k) = trqFltVec(:,k);\n "
" trqRawFlt(k) = trqRaw(k);\n end\nend % Reset the variables for detection of torque when Ki is act"
"ive,\n % either when f_switch is zero or the velocity is above the\n % thresholds. Also used"
" for initialization\n\n\nif isempty(saturated)\n saturated = 0;\nend\n\nif isempty(trqAddHistory)\n trqAdd"
"History = zeros(7,3);\nend\n\nif isempty(extraTorqueOut)\n extraTorqueOut = zeros(7,1);\nend\n\nif isempty(st"
"artOffset)\n startOffset = zeros(7,2);\nend\n\nif isempty(closeKiCounter)\n closeKiCounter = zeros(7,2);\n"
"end\n\nif isempty(torqueFeedforward)\n torqueFeedforward = zeros(7,1);\nend\n\nif isempty(posRefOffset)\n "
"posRefOffset = zeros(7,1);\nend\n\nif isempty(trqUpdateAddition)\n trqUpdateAddition = zeros(7,3);\nend\n\nif"
" isempty(keepKiClosed)\n keepKiClosed = KiHighDelay; % Initialize to standard delay (the amount of\nend "
" % samples that needs to be counted down to\n % allow for s"
"etting Ki active\n\n \nif fricHelp==0\n highKiActive = 0;\nend\n\njointsActive"
" = [1;1;1;1;1;1;1]; %ABB-order\n % The joints where leadthrough should be activated\n\n% maxRate = 0.2"
"*[0.05 ; 0.05 ; 0.05 ; 0.005 ; 0.005 ; 0.005 ; 0.025];\n % The maximum rate with which the torque feed"
"forw\n\nKp = Kp_in;\nKv = Kv_in;\nKi = Ki_in;\n\njointLimitGain = [1.0;1.0;1.0;0.3;0.3;0.3;0.7].*coulomb_width;\n"
"\nif f_switch==1\n \n trqRawFlt = trqFiltConstant.*trqRawFlt + (1-trqFiltConstant).*trqRaw;\n % Filteri"
"ng of the raw torque signal\n \n extraTorque = zeros(7,1);\n % The extra torque feedforward (i.e., "
"all trqFfw except the gravity\n % torque)\n \n for k=1:7\n if jointsActive(k)\n \n"
" if startOffset(k,2)==0\n startOffset(k,1) = trqRawFlt(k);\n startOffse"
"t(k,2) = 1;\n end % If unset, save the torque offset\n \n% if (~(highKiActive)"
" && abs(velFlt_noExtraFilt(k))>KiHighThres(k,2)) || (highKiActive && keepKiClosed(k)==0 && abs(velFlt_noExtraFil"
"t(k))>KiHighThres(k,2))\n if (keepStill && abs(velFlt_noExtraFilt(k))>KiHighThres(k,2)) || (highKiAct"
"ive && keepKiClosed(k)==0 && abs(velFlt_noExtraFilt(k))>KiHighThres(k,2))\n keepKiClosed(k) = 100"
";\n end % Safety stop of active Ki if the velocity exceeds the threshold\n % ("
"setting keepKiClosed to 100 means that it will take\n % at least 100 samples until Ki will be"
" active again)\n \n if (keepStill==1 && keepKiClosed(k)==0) || (highKiActive==1 && abs"
"(velFlt(k))<KiHighThres(k,1) && keepKiClosed(k)==0 && max(closeKiCounter(k,:))==0)\n Kp(k) = useK"
"p(k)*Kp_in(k); % Activate Ki if either\n Kv(k) = useKv(k)*Kv_in(k); % 1) keepStill is active (for"
" tuning) and keepKiClosed==0\n Ki(k) = 100*Ki_in(k); % 2) highKiActive is active, the veloci"
"ty is close to zero, keepKiClosed is zero, and closeKiCounter is zero\n \n startOf"
"fset(k,2) = 0;\n % Reset the torque offset when the controller is\n % acti"
"vated\n else\n Kp(k) = 0;\n Kv(k) = 0; \n Ki(k) = 0; \n "
" end\n \n if abs(velFlt(k))>highVelThres(k)\n closeKiCounter(k,:)"
" = [0 0];\n end % Reset closeKiCounter if the velocity is high, as the detected force now has led to "
"a high velocity, it does not need any priority\n \n if fricHelp==1\n if vel"
"Flt(k)>highVelThres(k) || closeKiCounter(k,1)>0\n extraTorque(k) = percentageFfwPos(k)*posFri"
"c(k);\n elseif velFlt(k)>lowVelThres(k)\n extraTorque(k) = percentageFfwPos(k)"
"*posFric(k)*(velFlt(k)-lowVelThres(k))/(highVelThres(k)-lowVelThres(k));\n elseif velFlt(k)<-high"
"VelThres(k) || closeKiCounter(k,2)>0\n extraTorque(k) = -percentageFfwNeg(k)*negFric(k);\n "
" elseif velFlt(k)<-lowVelThres(k)\n extraTorque(k) = -percentageFfwNeg(k)*negFric"
"(k)*(-velFlt(k)-lowVelThres(k))/(highVelThres(k)-lowVelThres(k));\n end\n end\n "
" % If the velocity exceeds the thresholds or that a torque has\n % been detected when Ki was acti"
"ve (closeKiCounter>0), then add\n % an extra torque feedforward to make it easier to move the\n "
" % robot\n end\n end\n \n \n sliding_thres = zeros(7,2);\n trqFltDelay = zeros(7,1);\n"
" for k=1:7\n trqFltVec(sampleIndexMod(k)+1,k) = trqRaw(k);\n trqFlt = mean(trqFltVec(1:length_m"
"ean(k),k));\n trqFltDelayVec(sampleIndexMod(k)+1,k) = trqFlt;\n tmp = trqFltDelayVec(mod(sampleInd"
"exMod(k)-delay_thres(k),length_mean(k))+1,k);\n \n if sampleIndex(k)<=length_mean(k)\n "
"sliding_thres(k,1) = gravTrqMotor(k) + coulomb_width(k)*1;\n sliding_thres(k,2) = gravTrqMotor(k) + c"
"oulomb_width(k)*-1;\n trqFltDelay(k) = trqFlt;\n else\n tmpCount = min(sampleIndex("
"k)-length_mean(k),length_mean(k));\n trqFltDelay(k) = (tmpCount*tmp+(length_mean(k)-tmpCount)*trqFlt)"
"/length_mean(k);\n multFactor = max(1-1/(recoveryFactor(k)*length_mean(k))*(sampleIndex(k)-length_mea"
"n(k)),0);\n sliding_thres(k,:) = trqFltDelay(k) + [1 -1]*(slidingThresWidth(k)*(1-multFactor)+coulomb"
"_width(k)*multFactor);\n end\n \n sampleIndex(k) = sampleIndex(k) + 1;\n sampleIndex"
"Mod(k) = mod(sampleIndex(k),length_mean(k));\n end\n % Calculation of quantities that is used to detect to"
"rques when Ki is\n % active\n \n if highKiActive==1 && ~keepStill\n for k=1:7\n if jo"
"intsActive(k)\n if abs(velFlt(k))<KiHighThres(k,1) && keepKiClosed(k)==0 && max(closeKiCounter(k,"
":))==0\n if (trqRawFlt(k) > sliding_thres(k,1)) || (trqRawFlt(k)>(gravTrqMotor(k)+posFric(k))"
") \n extraTorque(k) = -percentageFfwNeg(k)*negFric(k);\n Ki(k) = 0"
"*Ki_in(k);\n closeKiCounter(k,2) = 30;\n elseif (trqRawFlt(k) < slidin"
"g_thres(k,2)) || (trqRawFlt(k)<(gravTrqMotor(k)-negFric(k))) \n extraTorque(k) = percenta"
"geFfwPos(k)*posFric(k);\n Ki(k) = 0*Ki_in(k);\n closeKiCounter(k,1"
") = 30;\n end\n \n end\n end\n end\n e"
"nd\n % Detection of torques when Ki is active. If a detection is made, Ki is\n % set to zero and closeKiCo"
"unter is set to 30, and Ki will not be\n % active before these 30 samples has been counted down\n \n if"
" sensorOn == 1\n deadZone = 0.0001;\n for k=1:7\n extMotorTorque = extTorque(k)/gearRat"
"io(k);\n if extMotorTorque>deadZone\n extraTorque(k) = percentageFfwPos(k)*posFric(k) "
"+ extMotorTorque-deadZone;\n closeKiCounter(k,1) = 30;\n Ki(k) = 0*Ki_in(k);\n "
" elseif extMotorTorque<(-deadZone)\n extraTorque(k) = -percentageFfwNeg(k)*negFric(k) + ex"
"tMotorTorque+deadZone;\n closeKiCounter(k,2) = 30;\n Ki(k) = 0*Ki_in(k);\n "
" end\n end\n end\n % Add extra torque due to forces measured by force sensor \n % TESTING:"
"\n% extraTorque = extraTorque + qtorque;\n maxExtraTorqueRate = maxRate/17;\n% maxExtraTorqueRate = m"
"axRate/4;\n % The rate with which the torque is allowed to change\n extraTorqueOut = extraTorqueOut + "
"max(-maxExtraTorqueRate,min(maxExtraTorqueRate,extraTorque-extraTorqueOut));\n % A rate limited version o"
"f extraTorque is saved in extraTorqueOut\n yMeas = trqRaw + sum(trqAddHistory,2);\n % A predicition of"
" the trq\n \n for k=1:7\n \n % Joint limit avoidance\n jointLimitTorque = 0;\n"
" if jointsActive(k) \n if posRaw(k)<jointLimitStartMotorRad(k,1)\n jointLi"
"mitTorque = -1*jointLimitGain(k)*(posRaw(k)-jointLimitStartMotorRad(k,1));\n elseif posRaw(k)>jointLi"
"mitStartMotorRad(k,2)\n jointLimitTorque = -1*jointLimitGain(k)*(posRaw(k)-jointLimitStartMotorRa"
"d(k,2));\n end\n end\n \n % TESTING TESTING\n if 1 || jointsActive(k) && "
"(abs(velFlt(k))>KiHighThres(k,1) || keepKiClosed(k)>0 || max(closeKiCounter(k,:))>0)\n \n "
" ffwAdd = (gravTrqMotor(k)+extraTorqueOut(k)+jointLimitTorque)-trqUpdateAddition(k,2) + qtorque(k);\n "
" % Update of the feedforward torque\n fbAdd = K*(gravTrqMotor(k)+extraTorqueOut(k)+jointLimitTorqu"
"e - yMeas(k)) + qtorque(k);\n % Update of the feedback torque\n \n ffwAdd = max"
"(-maxRate(k),min(maxRate(k),ffwAdd));\n % Limit the rate of the feedforward update\n \n "
" trqUpdateAddition(k,2) = trqUpdateAddition(k,2) + ffwAdd;\n % Update the accumulated feedfor"
"ward torque\n \n %if abs(ffwAdd)>0\n % trqUpdateAddition(k,3) = 3;\n "
" %else\n % trqUpdateAddition(k,3) = max(0,trqUpdateAddition(k,3)-1);\n %end % Set th"
"e counter to 3 if the feedforward torque addition\n % is nonzero, otherwise count it down\n "
" \n magFbAdd = maxRate(k); % Allowed magnitude for the feedback torque\n %if trqUpdateAdd"
"ition(k,3)>0\n % magFbAdd = 0;\n %end % Set the magnitude to zero if a feedforward torq"
"ue change\n % was made in the last three samples\n \n fbAdd = max(-magFbAdd,min"
"(magFbAdd,fbAdd));\n \n trqUpdateAddition(k,1) = ffwAdd+fbAdd;\n \n "
" else\n trqUpdateAddition(k,1:3) = [0 0 0];\n % Reset if joint is not active\n en"
"d\n end\n \n if ~keepStill\n torqueFeedforward = torqueFeedforward + trqUpdateAddition(:,1);\n "
" % add the torque feedforward addition\n end\n \n for k=1:7\n closeKiCounter(k,1) = max(clo"
"seKiCounter(k,1)-1,0);\n closeKiCounter(k,2) = max(closeKiCounter(k,2)-1,0);\n % Count down closeK"
"iCounter\n if abs(velFlt(k))<KiHighThres(k,1)\n keepKiClosed(k) = max(keepKiClosed(k)-1,0);\n "
" % If the velocity is close to zero, then count down the value\n % of keepKiClosed\n "
" else\n keepKiClosed(k) = max(keepKiClosed(k),KiHighDelay(k));\n % If the velocity exceeds"
" the threshold, then reset the counter\n % in keepKiClosed\n end\n end\n \n \n for"
" k=1:7\n if useKp(k)==0\n posRefOffset(k) = jointsActive(k)*(posRaw(k) - posRefIn(k));\n "
" % Make sure that the position reference follows the actual\n % position of the robot (to avoid "
"errors in the underlyiong\n % ABB-controller)\n end\n end\nelse\n startOffset(:,2) = zer"
"os(7,1);\n \n if resetPosRef\n posRefOffset = zeros(7,1);\n end\nend\n\n\njointLimitsDeg = [-170"
" 170\n -140 45\n -125 80\n -290 290\n -95 "
"130\n -180 180\n -170 170];\nzoneDeg = 10;\n\n\njointLimitsMotorrad = jointLim"
"itsDeg*pi/180.*[gearRatio gearRatio];\nzoneMotorrad = zoneDeg*pi/180*abs(gearRatio);\n\nvec1 = (posRaw-(min(join"
"tLimitsMotorrad,[],2))-zoneMotorrad)<0;\nvec2 = ((max(jointLimitsMotorrad,[],2))-posRaw-zoneMotorrad)<0;\n "
" \nif f_switch==1 && (saturated==1 || any(vec1) || any(vec2)) && 0%|| max(abs(torqueFeedforward))>max"
"Trq\n torqueFeedforward = zeros(7,1);\n \n posRawInside = min(max(posRaw,min(jointLimitsMotorrad,[],2)+"
"zoneMotorrad),max(jointLimitsMotorrad,[],2)-zoneMotorrad);\n posRefOffset = (posRawInside - posRefIn);\n s"
"aturated = 1;\n \n Kp = Kp_in;\n Kv = Kv_in;\n Ki = Ki_in;\nend\n\ntrqAddHistory(:,2:3) = trqAddHist"
"ory(:,1:2);\ntrqAddHistory(:,1) = trqUpdateAddition(:,1);\n\nposRef = posRefIn + posRefOffset;\n\ntrqFfw = torqu"
"eFeedforward;\ntest = posRefOffset;\nsat = saturated;\n"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 101
position [23.5747 49.5747 7]
chart 99
linkNode [99 0 0]
subviewer 99
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 102
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 101
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 99
linkNode [99 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 99
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 103
ssIdNumber 4
name "Kp_in"
linkNode [99 0 104]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 104
ssIdNumber 5
name "Kp"
linkNode [99 103 105]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 105
ssIdNumber 6
name "Kv_in"
linkNode [99 104 106]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 106
ssIdNumber 7
name "Ki_in"
linkNode [99 105 107]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 107
ssIdNumber 8
name "gravTrqMotor"
linkNode [99 106 108]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 108
ssIdNumber 9
name "trqRaw"
linkNode [99 107 109]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 109
ssIdNumber 10
name "posRefIn"
linkNode [99 108 110]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 110
ssIdNumber 11
name "posRaw"
linkNode [99 109 111]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 111
ssIdNumber 12
name "f_switch"
linkNode [99 110 112]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 112
ssIdNumber 13
name "velFlt"
linkNode [99 111 113]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 113
ssIdNumber 14
name "highKiActive"
linkNode [99 112 114]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 114
ssIdNumber 15
name "keepStill"
linkNode [99 113 115]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 115
ssIdNumber 16
name "velFlt_noExtraFilt"
linkNode [99 114 116]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 116
ssIdNumber 17
name "Kv"
linkNode [99 115 117]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 117
ssIdNumber 18
name "Ki"
linkNode [99 116 118]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 118
ssIdNumber 19
name "trqFfw"
linkNode [99 117 119]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 119
ssIdNumber 20
name "posRef"
linkNode [99 118 120]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 120
ssIdNumber 22
name "coulomb_width"
linkNode [99 119 121]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 121
ssIdNumber 23
name "percentageTrqFfw"
linkNode [99 120 122]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 122
ssIdNumber 24
name "KiHighThres"
linkNode [99 121 123]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 123
ssIdNumber 25
name "slidingThresWidth"
linkNode [99 122 124]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 124
ssIdNumber 26
name "maxRate"
linkNode [99 123 125]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 125
ssIdNumber 27
name "resetPosRef"
linkNode [99 124 126]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 126
ssIdNumber 28
name "jointLimitStartMotorRad"
linkNode [99 125 127]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 127
ssIdNumber 29
name "extTorque"
linkNode [99 126 128]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 128
ssIdNumber 30
name "sensorOn"
linkNode [99 127 129]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 129
ssIdNumber 31
name "fricHelp"
linkNode [99 128 130]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 130
ssIdNumber 32
name "test"
linkNode [99 129 131]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 131
ssIdNumber 33
name "sat"
linkNode [99 130 132]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 132
ssIdNumber 34
name "qtorque"
linkNode [99 131 0]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
instance {
id 133
name "Left/leadthroug control/MATLAB Function"
machine 1
chart 99
}
chart {
id 134
name "Right/leadthroug control/MATLAB Function"
windowPosition [390.75 188.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 135 0 0]
firstTransition 137
firstJunction 136
viewObj 134
machine 1
ssIdHighWaterMark 33
decomposition CLUSTER_CHART
type EML_CHART
firstData 138
chartFileNumber 13
disableImplicitCasting 1
eml {
name "fcn"
}
}
state {
id 135
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 134
treeNode [134 0 0 0]
superState SUBCHART
subviewer 134
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [Kp,Kv,Ki,trqFfw,posRef,rstill] = fcn(Kp_in,Kv_in,Ki_in,gravTrqMotor,trqRaw,posRefIn,pos"
"Raw,f_switch,velFlt,highKiActive,keepStill,velFlt_noExtraFilt,coulomb_width,percentageTrqFfw,KiHighThres,sliding"
"ThresWidth,maxRate,resetPosRef,jointLimitStartMotorRad,extTorque,sensorOn,fricHelp,qtorque)\n%#codegen\n% All ve"
"ctors are given in ABB-order\n\npersistent torqueFeedforward;\n % Used to account for the accumulated "
"torqe feedforward (size 7x1)\npersistent posRefOffset;\n % The offset from the incoming position refer"
"ence, needs to be\n % updated to the current position to avoid the safety system of\n % the "
"ABB controller to give errors\npersistent trqUpdateAddition;\n % The torque feedforward addition is sa"
"ved in this variable. The\n % first column is the total torque feedforward, the second\n % c"
"olumn the \"feedforward\" part, i.e., the desired torque which\n % has to be corrected with a feedback"
" part to handle\n % disturbances. The last column contains a counter that is used\n % to avo"
"id using feedback 3 samples after any feedforward change\n % (3 samples is the delay from the feedforw"
"ard signal is set\n % until a change can be seen in trqRaw)\npersistent trqRawFlt;\n % Filte"
"red torque signal\npersistent keepKiClosed; \n % Variable of size 7x1 (one for each joint). If zero, K"
"i should \n % be active for the respective joints. Also works as a counter, \n % it is set t"
"o a value when a joint is moving, and then counted \n % down when the joint is still (below a threshol"
"d)\npersistent closeKiCounter; \n % Variable of size 7x2. It is used to make sure that a detected\n "
" % torque will have time to make an impact. When a torque is\n % detected with Ki active, clos"
"eKiCounter is set to a value that\n % is then counted down. While closeKiCounter is non-zero, Ki\n "
" % will not be active and a torque feedforward will be used. The\n % first column is used if a n"
"egative torque is detected, and the\n % second column if a positive torque is detected\n \np"
"ersistent startOffset;\n % Variable used to handle the offset torque present for each\n % jo"
"int. The first column is used to save this torque, the\n % second column to tell if it has been set or"
" not\npersistent extraTorqueOut;\n % Variable to handle ramping of the extra feedforward torque\npersi"
"stent trqAddHistory\n % The history of added feedforward torques. It is used to\n % predict "
"trqRaw during the delay that is present.\n \npersistent saturated; \n\nrstill = zeros(7,1);\n"
"\nlength_mean = [80;80;80;80;80;80;80];\ndelay_thres = [60;60;60;60;60;60;60];% length_mean has to be larger tha"
"n delay thres\nrecoveryFactor = [1.5;1.5;1.5;1.5;1.5;1.5;1.5];\n% slidingThresWidth = [0.006;0.006;0.003;0.00045"
";0.0006;0.0004;0.003;];\n % These are parameters for detection of torque when Ki are\n % act"
"ive\n \ntrqFiltConstant = [0.8;0.8;0.8;0.8;0.8;0.8;0.8];\n % The filter constant for the tor"
"que low pass filter\n\n% KiHighThres = [0.1 1;0.1 1;0.1 1;0.3 1;0.1 1;0.1 1;0.1 1]; The first\n% column is the t"
"hreshold for the filtered version of velFlt, and the\n% second column the threshold for the non-filtered version"
" of velFlt\nKiHighDelay = [30 ; 30 ; 30 ; 30 ; 30 ; 30 ; 30];\n % The standard delay for counting down"
" keepKiClosed when the\n % measured velocity is below KiHighThres\n \nhighVelThres = [5;5;5;"
"5;5;5;5];\nlowVelThres = [2;2;2;2;2;2;2];\n % Thresholds for high and low velocities. Full torque\n "
" % feedforward is active when the velocity is above highVelThres,\n % and reduced feedforward w"
"hen above lowVelThres (proportional\n % to velocity)\n\n \nuseKp = [0;0;0;0;0;0;0]; %Never u"
"se Kp\nuseKv = [1;1;1;0;0;0;1]; %Kv needs to be active for the base joints (1,2,3, and 7)\n % When Ki "
"is active, these variables control if also Kp and Kv\n % should be active\n%TESTING:\npercentageTrqFfw"
" = 0.9*ones(7,1);\npercentageFfwPos = percentageTrqFfw;\npercentageFfwNeg = percentageTrqFfw;\nposFric = coulomb"
"_width;\nnegFric = coulomb_width;\n % Variables to control the amount of torque feedforward\n "
" \nK = 1/3;\n % The proportional feedback parameter for the feedback torque control\n % loop\n\ngearRatio"
" = [100;100;-100;100;-101;100;100];\n\npersistent sampleIndex sampleIndexMod trqFltVec trqFltDelayVec;\nif isemp"
"ty(sampleIndexMod) || f_switch==0\n sampleIndex = zeros(7,1);\n sampleIndexMod = zeros(7,1); % obs! zero i"
"ndexed, add 1 when used as index\n trqFltVec = repmat(trqRaw',max(length_mean),1);\n trqFltDelayVec = trqF"
"ltVec;\n trqRawFlt = trqRaw;\nend\nfor k=1:7\n if abs(velFlt(k))>KiHighThres(k,1) || abs(velFlt_noExtraFil"
"t(k))>KiHighThres(k,2)\n sampleIndex(k) = 0;\n sampleIndexMod(k) = 0; % obs! zero indexed, add 1 w"
"hen used as index\n trqFltVec(:,k) = ones(max(length_mean),1)*trqRaw(k);\n trqFltDelayVec(:,k) = t"
"rqFltVec(:,k);\n trqRawFlt(k) = trqRaw(k);\n end\nend % Reset the variables for detection of to"
"rque when Ki is active,\n % either when f_switch is zero or the velocity is above the\n % th"
"resholds. Also used for initialization\n\n\nif isempty(saturated)\n saturated = 0;\nend\n\nif isempty(trqAddH"
"istory)\n trqAddHistory = zeros(7,3);\nend\n\nif isempty(extraTorqueOut)\n extraTorqueOut = zeros(7,1);\ne"
"nd\n\nif isempty(startOffset)\n startOffset = zeros(7,2);\nend\n\nif isempty(closeKiCounter)\n closeKiCoun"
"ter = zeros(7,2);\nend\n\nif isempty(torqueFeedforward)\n torqueFeedforward = zeros(7,1);\nend\n\nif isempty("
"posRefOffset)\n posRefOffset = zeros(7,1);\nend\n\nif isempty(trqUpdateAddition)\n trqUpdateAddition = zer"
"os(7,3);\nend\n\nif isempty(keepKiClosed)\n keepKiClosed = KiHighDelay; % Initialize to standard delay (the a"
"mount of\nend % samples that needs to be counted down to\n "
" % allow for setting Ki active\n\n \nif fricHelp==0\n highKiActive = 0;\n"
"end\n\njointsActive = [1;1;1;1;1;1;1]; %ABB-order\n % The joints where leadthrough should be activated"
"\n\n% maxRate = 0.2*[0.05 ; 0.05 ; 0.05 ; 0.005 ; 0.005 ; 0.005 ; 0.025];\n % The maximum rate with wh"
"ich the torque feedforw\n\nKp = Kp_in;\nKv = Kv_in;\nKi = Ki_in;\n\njointLimitGain = [1.0;1.0;1.0;0.3;0.3;0.3;0."
"7].*coulomb_width;\n\nif f_switch==1\n \n trqRawFlt = trqFiltConstant.*trqRawFlt + (1-trqFiltConstant).*tr"
"qRaw;\n % Filtering of the raw torque signal\n \n extraTorque = zeros(7,1);\n % The extra torque"
" feedforward (i.e., all trqFfw except the gravity\n % torque)\n \n for k=1:7\n if jointsActi"
"ve(k)\n \n if startOffset(k,2)==0\n startOffset(k,1) = trqRawFlt(k);\n "
" startOffset(k,2) = 1;\n end % If unset, save the torque offset\n \n% "
" if (~(highKiActive) && abs(velFlt_noExtraFilt(k))>KiHighThres(k,2)) || (highKiActive && keepKiClosed(k)==0 && "
"abs(velFlt_noExtraFilt(k))>KiHighThres(k,2))\n if (keepStill && abs(velFlt_noExtraFilt(k))>KiHighThre"
"s(k,2)) || (highKiActive && keepKiClosed(k)==0 && abs(velFlt_noExtraFilt(k))>KiHighThres(k,2))\n "
"keepKiClosed(k) = 100;\n end % Safety stop of active Ki if the velocity exceeds the threshold\n "
" % (setting keepKiClosed to 100 means that it will take\n % at least 100 sam"
"ples until Ki will be active again)\n \n if (keepStill==1 && keepKiClosed(k)==0) || (h"
"ighKiActive==1 && abs(velFlt(k))<KiHighThres(k,1) && keepKiClosed(k)==0 && max(closeKiCounter(k,:))==0)\n "
" Kp(k) = useKp(k)*Kp_in(k); % Activate Ki if either\n Kv(k) = useKv(k)*Kv_in(k); % 1) kee"
"pStill is active (for tuning) and keepKiClosed==0\n Ki(k) = 100*Ki_in(k); % 2) highKiActive "
"is active, the velocity is close to zero, keepKiClosed is zero, and closeKiCounter is zero\n \n "
" startOffset(k,2) = 0;\n % Reset the torque offset when the controller is\n "
" % activated\n else\n Kp(k) = 0;\n Kv(k) = 0; \n "
" Ki(k) = 0; \n end\n \n if abs(velFlt(k))>highVelThres(k)\n "
" closeKiCounter(k,:) = [0 0];\n end % Reset closeKiCounter if the velocity is high, as the detected "
"force now has led to a high velocity, it does not need any priority\n \n if fricHelp==1\n "
" if velFlt(k)>highVelThres(k) || closeKiCounter(k,1)>0\n extraTorque(k) = perce"
"ntageFfwPos(k)*posFric(k);\n elseif velFlt(k)>lowVelThres(k)\n extraTorque(k) "
"= percentageFfwPos(k)*posFric(k)*(velFlt(k)-lowVelThres(k))/(highVelThres(k)-lowVelThres(k));\n e"
"lseif velFlt(k)<-highVelThres(k) || closeKiCounter(k,2)>0\n extraTorque(k) = -percentageFfwNe"
"g(k)*negFric(k);\n elseif velFlt(k)<-lowVelThres(k)\n extraTorque(k) = -percen"
"tageFfwNeg(k)*negFric(k)*(-velFlt(k)-lowVelThres(k))/(highVelThres(k)-lowVelThres(k));\n end\n "
" end\n % If the velocity exceeds the thresholds or that a torque has\n % been dete"
"cted when Ki was active (closeKiCounter>0), then add\n % an extra torque feedforward to make it easie"
"r to move the\n % robot\n end\n end\n \n \n sliding_thres = zeros(7,2);\n trqFl"
"tDelay = zeros(7,1);\n for k=1:7\n trqFltVec(sampleIndexMod(k)+1,k) = trqRaw(k);\n trqFlt = mea"
"n(trqFltVec(1:length_mean(k),k));\n trqFltDelayVec(sampleIndexMod(k)+1,k) = trqFlt;\n tmp = trqFlt"
"DelayVec(mod(sampleIndexMod(k)-delay_thres(k),length_mean(k))+1,k);\n \n if sampleIndex(k)<=length"
"_mean(k)\n sliding_thres(k,1) = gravTrqMotor(k) + coulomb_width(k)*1;\n sliding_thres(k,2)"
" = gravTrqMotor(k) + coulomb_width(k)*-1;\n trqFltDelay(k) = trqFlt;\n else\n tmpCo"
"unt = min(sampleIndex(k)-length_mean(k),length_mean(k));\n trqFltDelay(k) = (tmpCount*tmp+(length_mea"
"n(k)-tmpCount)*trqFlt)/length_mean(k);\n multFactor = max(1-1/(recoveryFactor(k)*length_mean(k))*(sam"
"pleIndex(k)-length_mean(k)),0);\n sliding_thres(k,:) = trqFltDelay(k) + [1 -1]*(slidingThresWidth(k)*"
"(1-multFactor)+coulomb_width(k)*multFactor);\n end\n \n sampleIndex(k) = sampleIndex(k) + 1"
";\n sampleIndexMod(k) = mod(sampleIndex(k),length_mean(k));\n end\n % Calculation of quantities tha"
"t is used to detect torques when Ki is\n % active\n \n if highKiActive==1 && ~keepStill\n for k="
"1:7\n if jointsActive(k)\n if abs(velFlt(k))<KiHighThres(k,1) && keepKiClosed(k)==0 &&"
" max(closeKiCounter(k,:))==0\n if (trqRawFlt(k) > sliding_thres(k,1)) || (trqRawFlt(k)>(gravT"
"rqMotor(k)+posFric(k))) \n extraTorque(k) = -percentageFfwNeg(k)*negFric(k);\n "
" Ki(k) = 0*Ki_in(k);\n closeKiCounter(k,2) = 30;\n elseif "
"(trqRawFlt(k) < sliding_thres(k,2)) || (trqRawFlt(k)<(gravTrqMotor(k)-negFric(k))) \n ext"
"raTorque(k) = percentageFfwPos(k)*posFric(k);\n Ki(k) = 0*Ki_in(k);\n "
" closeKiCounter(k,1) = 30;\n end\n \n end\n e"
"nd\n end\n end\n % Detection of torques when Ki is active. If a detection is made, Ki is\n % set"
" to zero and closeKiCounter is set to 30, and Ki will not be\n % active before these 30 samples has been coun"
"ted down\n \n if sensorOn == 1\n deadZone = 0.0001;\n for k=1:7\n extMotorTorque "
"= extTorque(k)/gearRatio(k);\n if extMotorTorque>deadZone\n extraTorque(k) = percentag"
"eFfwPos(k)*posFric(k) + extMotorTorque-deadZone;\n closeKiCounter(k,1) = 30;\n Ki("
"k) = 0*Ki_in(k);\n elseif extMotorTorque<(-deadZone)\n extraTorque(k) = -percentageFfw"
"Neg(k)*negFric(k) + extMotorTorque+deadZone;\n closeKiCounter(k,2) = 30;\n Ki(k) ="
" 0*Ki_in(k);\n end\n end\n end\n % Add extra torque due to forces measured by force sens"
"or \n % TESTING:\n% extraTorque = extraTorque + qtorque;\n maxExtraTorqueRate = maxRate/17;\n% "
"maxExtraTorqueRate = maxRate/4;\n % The rate with which the torque is allowed to change\n extraTorqueO"
"ut = extraTorqueOut + max(-maxExtraTorqueRate,min(maxExtraTorqueRate,extraTorque-extraTorqueOut));\n % A "
"rate limited version of extraTorque is saved in extraTorqueOut\n yMeas = trqRaw + sum(trqAddHistory,2);\n "
" % A predicition of the trq\n \n for k=1:7\n \n % Joint limit avoidance\n "
"jointLimitTorque = 0;\n if jointsActive(k) \n if posRaw(k)<jointLimitStartMotorRad(k,1)\n"
" jointLimitTorque = -1*jointLimitGain(k)*(posRaw(k)-jointLimitStartMotorRad(k,1));\n e"
"lseif posRaw(k)>jointLimitStartMotorRad(k,2)\n jointLimitTorque = -1*jointLimitGain(k)*(posRaw(k)"
"-jointLimitStartMotorRad(k,2));\n end\n end\n \n % TESTING TESTING:\n if "
"1 || jointsActive(k) && (abs(velFlt(k))>KiHighThres(k,1) || keepKiClosed(k)>0 || max(closeKiCounter(k,:))>0)\n "
" \n \n ffwAdd = (gravTrqMotor(k)+extraTorqueOut(k)+jointLimitTorque)-trqU"
"pdateAddition(k,2) + qtorque(k);\n % Update of the feedforward torque\n fbAdd = K*(gravTrq"
"Motor(k)+extraTorqueOut(k)+jointLimitTorque - yMeas(k)) + qtorque(k);\n % Update of the feedback torq"
"ue\n \n ffwAdd = max(-maxRate(k),min(maxRate(k),ffwAdd));\n % Limit the rate of"
" the feedforward update\n \n trqUpdateAddition(k,2) = trqUpdateAddition(k,2) + ffwAdd;\n "
" % Update the accumulated feedforward torque\n \n %if abs(ffwAdd)>0\n "
"% trqUpdateAddition(k,3) = 3;\n %else\n % trqUpdateAddition(k,3) = max(0,trqUpdateAd"
"dition(k,3)-1);\n %end % Set the counter to 3 if the feedforward torque addition\n % is no"
"nzero, otherwise count it down\n \n magFbAdd = maxRate(k); % Allowed magnitude for the fee"
"dback torque\n %if trqUpdateAddition(k,3)>0\n % magFbAdd = 0;\n %end % Set t"
"he magnitude to zero if a feedforward torque change\n % was made in the last three samples\n "
" \n fbAdd = max(-magFbAdd,min(magFbAdd,fbAdd));\n \n trqUpdateAddition(k,1) ="
" ffwAdd+fbAdd;\n \n else\n rstill(k) = 1;\n trqUpdateAddition(k,1"
":3) = [0 0 0];\n % Reset if joint is not active\n end\n end\n \n if ~keepStill\n "
" torqueFeedforward = torqueFeedforward + trqUpdateAddition(:,1);\n % add the torque feedforward additio"
"n\n end\n \n for k=1:7\n closeKiCounter(k,1) = max(closeKiCounter(k,1)-1,0);\n closeKiCou"
"nter(k,2) = max(closeKiCounter(k,2)-1,0);\n % Count down closeKiCounter\n if abs(velFlt(k))<KiHigh"
"Thres(k,1)\n keepKiClosed(k) = max(keepKiClosed(k)-1,0);\n % If the velocity is close to z"
"ero, then count down the value\n % of keepKiClosed\n else\n keepKiClosed(k) = max(k"
"eepKiClosed(k),KiHighDelay(k));\n % If the velocity exceeds the threshold, then reset the counter\n "
" % in keepKiClosed\n end\n end\n \n \n for k=1:7\n if useKp(k)==0\n "
" posRefOffset(k) = jointsActive(k)*(posRaw(k) - posRefIn(k));\n % Make sure that the position refere"
"nce follows the actual\n % position of the robot (to avoid errors in the underlyiong\n % A"
"BB-controller)\n end\n end\nelse\n startOffset(:,2) = zeros(7,1);\n \n if resetPosRef\n "
" posRefOffset = zeros(7,1);\n end\nend\n\n\njointLimitsDeg = [-170 170\n -140 45\n "
" -125 80\n -290 290\n -95 130\n -180 180\n "
" -170 170];\nzoneDeg = 10;\n\n\njointLimitsMotorrad = jointLimitsDeg*pi/180.*[gearRatio gearRatio];\nzon"
"eMotorrad = zoneDeg*pi/180*abs(gearRatio);\n\nvec1 = (posRaw-(min(jointLimitsMotorrad,[],2))-zoneMotorrad)<0;\nv"
"ec2 = ((max(jointLimitsMotorrad,[],2))-posRaw-zoneMotorrad)<0;\n \nif f_switch==1 && (saturated="
"=1 || any(vec1) || any(vec2)) %|| max(abs(torqueFeedforward))>maxTrq\n torqueFeedforward = zeros(7,1);\n \n"
" posRawInside = min(max(posRaw,min(jointLimitsMotorrad,[],2)+zoneMotorrad),max(jointLimitsMotorrad,[],2)-zone"
"Motorrad);\n posRefOffset = (posRawInside - posRefIn);\n saturated = 1;\n \n Kp = Kp_in;\n Kv = K"
"v_in;\n Ki = Ki_in;\nend\n\ntrqAddHistory(:,2:3) = trqAddHistory(:,1:2);\ntrqAddHistory(:,1) = trqUpdateAddit"
"ion(:,1);\n\nposRef = posRefIn + posRefOffset;\n\ntrqFfw = torqueFeedforward;"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 136
position [23.5747 49.5747 7]
chart 134
linkNode [134 0 0]
subviewer 134
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 137
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 136
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 134
linkNode [134 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 134
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 138
ssIdNumber 4
name "Kp_in"
linkNode [134 0 139]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 139
ssIdNumber 5
name "Kp"
linkNode [134 138 140]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 140
ssIdNumber 6
name "Kv_in"
linkNode [134 139 141]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 141
ssIdNumber 7
name "Ki_in"
linkNode [134 140 142]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 142
ssIdNumber 8
name "gravTrqMotor"
linkNode [134 141 143]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 143
ssIdNumber 9
name "trqRaw"
linkNode [134 142 144]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 144
ssIdNumber 10
name "posRefIn"
linkNode [134 143 145]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 145
ssIdNumber 11
name "posRaw"
linkNode [134 144 146]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 146
ssIdNumber 12
name "f_switch"
linkNode [134 145 147]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 147
ssIdNumber 13
name "velFlt"
linkNode [134 146 148]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 148
ssIdNumber 14
name "highKiActive"
linkNode [134 147 149]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 149
ssIdNumber 15
name "keepStill"
linkNode [134 148 150]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 150
ssIdNumber 16
name "velFlt_noExtraFilt"
linkNode [134 149 151]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 151
ssIdNumber 17
name "Kv"
linkNode [134 150 152]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 152
ssIdNumber 18
name "Ki"
linkNode [134 151 153]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 153
ssIdNumber 19
name "trqFfw"
linkNode [134 152 154]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 154
ssIdNumber 20
name "posRef"
linkNode [134 153 155]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 155
ssIdNumber 22
name "coulomb_width"
linkNode [134 154 156]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 156
ssIdNumber 23
name "percentageTrqFfw"
linkNode [134 155 157]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 157
ssIdNumber 24
name "KiHighThres"
linkNode [134 156 158]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 158
ssIdNumber 25
name "slidingThresWidth"
linkNode [134 157 159]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 159
ssIdNumber 26
name "maxRate"
linkNode [134 158 160]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 160
ssIdNumber 27
name "resetPosRef"
linkNode [134 159 161]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 161
ssIdNumber 28
name "jointLimitStartMotorRad"
linkNode [134 160 162]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 162
ssIdNumber 29
name "extTorque"
linkNode [134 161 163]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 163
ssIdNumber 30
name "sensorOn"
linkNode [134 162 164]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 164
ssIdNumber 31
name "fricHelp"
linkNode [134 163 165]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 165
ssIdNumber 32
name "rstill"
linkNode [134 164 166]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 166
ssIdNumber 33
name "qtorque"
linkNode [134 165 0]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
instance {
id 167
name "Right/leadthroug control/MATLAB Function"
machine 1
chart 134
}
chart {
id 168
name "botharms_get_torque/coulFric"
windowPosition [375.75 203.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 169 0 0]
firstTransition 171
firstJunction 170
viewObj 168
machine 1
toolbarMode LIBRARY_TOOLBAR
ssIdHighWaterMark 10
decomposition CLUSTER_CHART
type EML_CHART
firstData 172
chartFileNumber 14
disableImplicitCasting 1
eml {
name "coulFric"
}
}
state {
id 169
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 168
treeNode [168 0 0 0]
superState SUBCHART
subviewer 168
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [trqRight, trqLeft] = coulFric(coulRight, q_dot, coulLeft,dith)\n\n\ndithScale = 0.75;\n"
"\nfullThres = 0.1;\nslowThres = fullThres/2;\n\ntrqThres = 0.1;\n\nhelpRate = 0.30;\n\ntrqRight = zeros(7,1);\nt"
"rqLeft = zeros(7,1);\n\n\n% RIGHT:\nfor k = 1:7\n if abs(q_dot(k)) > fullThres\n trqRight(k) = coulRig"
"ht(k)*sign(q_dot(k))*helpRate;\n elseif abs(q_dot(k)) > slowThres\n trqRight(k) = coulRight(k)*(abs(q_"
"dot(k))-slowThres)/(fullThres-slowThres)*sign(q_dot(k))*helpRate;\n else\n trqRight(k) = dith(k)*dithS"
"cale;\n end\nend\n\n\n% LEFT:\nq_dot_l = q_dot(8:14);\nfor k = 1:7\n if abs(q_dot_l(k)) > fullThres\n "
" trqLeft(k) = coulLeft(k)*sign(q_dot_l(k))*helpRate;\n elseif abs(q_dot_l(k)) > slowThres\n trqLeft("
"k) = coulLeft(k)*(abs(q_dot_l(k))-slowThres)/(fullThres-slowThres)*sign(q_dot_l(k))*helpRate;\n else\n "
" trqLeft(k) = dith(k)*dithScale;\n end\nend\n\n"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 170
position [23.5747 49.5747 7]
chart 168
linkNode [168 0 0]
subviewer 168
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 171
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 170
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 168
linkNode [168 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 168
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 172
ssIdNumber 4
name "coulRight"
linkNode [168 0 173]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 173
ssIdNumber 9
name "q_dot"
linkNode [168 172 174]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 174
ssIdNumber 5
name "trqRight"
linkNode [168 173 175]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 175
ssIdNumber 7
name "coulLeft"
linkNode [168 174 176]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 176
ssIdNumber 8
name "trqLeft"
linkNode [168 175 177]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 177
ssIdNumber 10
name "dith"
linkNode [168 176 0]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
instance {
id 178
name "botharms_get_torque/coulFric"
machine 1
chart 168
}
chart {
id 179
name "Left/MATLAB Function3"
windowPosition [300.375 53.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 180 0 0]
firstTransition 182
firstJunction 181
viewObj 179
machine 1
ssIdHighWaterMark 32
decomposition CLUSTER_CHART
type EML_CHART
firstData 183
updateMethod DISCRETE
sampleTime "0.004"
chartFileNumber 15
disableImplicitCasting 1
eml {
name "fcn"
}
}
state {
id 180
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 179
treeNode [179 0 0 0]
superState SUBCHART
subviewer 179
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [runExtctrl , lockOutput, resetProgram, state] = fcn(controlActive, obtaining, switch_on"
")\n\n\nleadthrough_on = switch_on && obtaining;\n%obtaining = 1;\n% state variables\npersistent internalState;\n"
"\n% internal variables\npersistent runExtctrlInternal;\npersistent lockOutputInternal;\npersistent resetProgramI"
"nternal;\n\nif isempty(internalState)\n internalState = 1;\nend\n\nif isempty(runExtctrlInternal)\n runExt"
"ctrlInternal = 0;\nend\nif isempty(lockOutputInternal)\n lockOutputInternal = 0;\nend\nif isempty(resetProgra"
"mInternal)\n resetProgramInternal = 1;\nend\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n% number internalS"
"tate\n% 1 Extctrl_Idle\n% 2 Extctrl_Running\n% 3 Recovery_Phase\n\n%%%%%%%%%%%%"
"%%%%%%%%%%%%%%%%%%%%%%\n\n% State transitions + entry/exit-actions\nif internalState==1\n if (leadthrough_on)"
" && obtaining\n %Exit-actions\n \n %New state\n internalState = 2;\n \n "
" %Entry-actions\n runExtctrlInternal = 1;\n lockOutputInternal = 0;\n resetProgramInternal"
" = 0;\n end\nelseif internalState==2\n if ~controlActive\n %Exit-actions\n \n %New st"
"ate\n internalState = 1;\n \n %Entry-actions\n runExtctrlInternal = 0;\n lock"
"OutputInternal = 0;\n resetProgramInternal = 1;\n elseif (~leadthrough_on) && controlActive\n %"
"Exit-actions\n \n %New state\n internalState = 3;\n \n %Entry-actions\n "
" runExtctrlInternal = 0;\n lockOutputInternal = 1;\n resetProgramInternal = 0; \n end\nels"
"eif internalState==3\n if (~leadthrough_on) || ~controlActive\n %Exit-actions\n \n %New "
"state\n internalState = 1;\n \n %Entry-actions\n runExtctrlInternal = 0;\n lo"
"ckOutputInternal = 0;\n resetProgramInternal = 1; \n end\nend\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"
"%%%\n\n% Continous state actions\n\n%Set outputs\nrunExtctrl = runExtctrlInternal;\nlockOutput = lockOutputInter"
"nal;\nresetProgram = resetProgramInternal;\nstate = internalState;"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 181
position [23.5747 49.5747 7]
chart 179
linkNode [179 0 0]
subviewer 179
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 182
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 181
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 179
linkNode [179 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 179
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 183
ssIdNumber 28
name "runExtctrl"
linkNode [179 0 184]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 184
ssIdNumber 5
name "lockOutput"
linkNode [179 183 185]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 185
ssIdNumber 27
name "resetProgram"
linkNode [179 184 186]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 186
ssIdNumber 29
name "state"
linkNode [179 185 187]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 187
ssIdNumber 30
name "controlActive"
linkNode [179 186 188]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 188
ssIdNumber 31
name "obtaining"
linkNode [179 187 189]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 189
ssIdNumber 32
name "switch_on"
linkNode [179 188 0]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
instance {
id 190
name "Left/MATLAB Function3"
machine 1
chart 179
}
chart {
id 191
name "Right/calcGravityTorque/calcGravTrq"
windowPosition [390.75 188.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 192 0 0]
firstTransition 194
firstJunction 193
viewObj 191
machine 1
ssIdHighWaterMark 16
decomposition CLUSTER_CHART
type EML_CHART
firstData 195
chartFileNumber 16
disableImplicitCasting 1
eml {
name "fcn"
}
}
state {
id 192
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 191
treeNode [191 0 0 0]
superState SUBCHART
subviewer 191
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [gravTrq,gravTrqMotor,coulombWidthMotor, coulombWidth] = fcn(q_motor,x_hat,gearRatio,ba"
"seAngles)\n \n% gearRatio = [100;100;-100;100;-101;100;100]; \n \n q_arm_ABBorder = q_motor./gear"
"Ratio;\n \n\n \n gravTrqLogicalOrder = calcTrqFromParams( x_hat,q_arm_ABBorder,baseAngles);\n %logic"
"al ordering\n \n gravTrq = [gravTrqLogicalOrder(1:2) ; gravTrqLogicalOrder(4:7) ; gravTrqLogicalOrder(3)];"
"\n %ABB-order\n \n% gearRatio = [100;100;-100;100;-101;100;100];\n \n gravTrqMotor = gravTrq./ge"
"arRatio;\n \n coulombWidthLogicalOrder = x_hat(29:35);\n coulombWidth = [coulombWidthLogicalOrder(1:2) "
"; coulombWidthLogicalOrder(4:7) ; coulombWidthLogicalOrder(3)];\n coulombWidthMotor = coulombWidth./abs(gearR"
"atio);\n\nend\n\nfunction [ trq ] = calcTrqFromParams( x_hat,q,baseAngles)\n%CALCTRQFROMPARAMS Summary of this f"
"unction goes here\n% Detailed explanation goes here\n\n A = calcA_FRIDA(q,-9.82,baseAngles);\n \n \n "
" trq = A*x_hat(1:28,1);\n\nend\n\nfunction [A] = calcA_FRIDA(q,g,baseAngles)\n % q should be given in ABB-o"
"rder!!!\n % Offsets are already handled in the equations!!!\n\n c1 = cos(q(1));\n s1 = sin(q(1));\n "
"c2 = cos(q(2));\n s2 = sin(q(2));\n c3 = cos(q(7));\n s3 = sin(q(7));\n c4 = cos(q(3));\n s4 = si"
"n(q(3));\n c5 = cos(q(4));\n s5 = sin(q(4));\n c6 = cos(q(5));\n s6 = sin(q(5));\n c7 = cos(q(6))"
";\n s7 = sin(q(6));\n\n sb1 = sin(baseAngles(1));\n cb1 = cos(baseAngles(1));\n sb2 = sin(baseAngles"
"(2));\n cb2 = cos(baseAngles(2));\n sb3 = sin(baseAngles(3));\n cb3 = cos(baseAngles(3));\n\n should"
"erLen = 0.110;\n shoulderOffs = -0.030;\n elbowOffs = 0.0405;\n wristOffs = 0.0135;\n hand"
"Offs = -0.027;\n upArmLen = 0.2465;\n lowArmLen = 0.265;\n handLen = 0.032;\n\n A"
" = zeros(7,28);\n\n p0=g*cb1;\n p1=g*sb1;\n p2=sb2*p0;\n p3=s1*cb3;\n p4=c2*c3;\n p5=cb2*p0;\n"
" p6=c1*cb3;\n p7=c1*sb3;\n p8=s1*sb3;\n p9=s4*p1;\n p10=s4*p2;\n p11=c5*c6;\n p12=c4*p1;\n "
" p13=s1*sb3;\n p14=c4*p2;\n p15=s3*c2;\n p16=c7^2;\n p17=s7^2;\n p18=s5*c6;\n p19=s2*s6;\n "
" p20=s3*s6;\n p21=s2*c3;\n p22=c6^2;\n p23=s6*p4;\n p24=p3*p9;\n p25=p6*p10;\n p26=s4*p5;\n "
" p27=c3*p1;\n p28=p7*p9;\n p29=p8*p10;\n p30=p3*p12;\n p31=p6*p14;\n p32=c4*p5;\n p33=c3*p2;\n"
" p34=p1*p15;\n p35=s6^2;\n p36=p7*p12;\n p37=p8*p14;\n p38=c5*wristOffs;\n p39=p2*p15;\n p4"
"0=s2*s3;\n p41=p3*p10;\n p42=p6*p9;\n p43=p7*p10;\n p44=c7*p11;\n p45=s3*s5;\n p46=p3*p14;\n "
" p47=c5*p19;\n p48=p5*p40;\n p49=p9*p13;\n p50=s7*p11;\n p51=p6*p12;\n p52=p7*p14;\n p53=s5*s"
"6;\n p54=p1*p3;\n p55=c3*p19;\n p56=p8*p12;\n p57=handOffs*p11;\n p58=s5*p4;\n p59=s6*c2;\n "
" p60=p1*p7;\n p61=p2*p6;\n p62=p3*p33;\n p63=p2*p8;\n p64=s2*s5;\n p65=c5*c7;\n p66=elbowOffs*"
"p4;\n p67=p6*p27;\n p68=p7*p33;\n p69=c2*p32;\n p70=s7*c5;\n p71=c7*p18;\n p72=s2*c6;\n p73"
"=s3*elbowOffs;\n p74=p8*p27;\n p75=p1*p6;\n p76=p2*p3;\n p77=s5*wristOffs;\n p78=c2*shoulderOffs;"
"\n p79=s7*p18;\n p80=c6*p4;\n p81=p11*p17;\n p82=p1*p8;\n p83=p2*p7;\n p84=p3*p34;\n p85=p6"
"*p39;\n p86=c5*p23;\n p87=elbowOffs*p21;\n p88=s5*c7;\n p89=wristOffs*p45;\n p90=handOffs*p18;\n "
" p91=s6*c6;\n p92=p11*p16;\n p93=p7*p34;\n p94=p8*p39;\n p95=c5*p20;\n p96=s5*s7;\n p97=p21*"
"p26;\n p98=s2*elbowOffs;\n p99=wristOffs*p58;\n p100=p3*p27;\n p101=s3*c6;\n p102=s6*c5;\n p10"
"3=p16*p18;\n p104=c2*p26;\n p105=handOffs*p35;\n p106=s2*p30;\n p107=p21*p32;\n p108=p7*p27;\n "
" p109=p6*p34;\n p110=p3*p39;\n p111=p17*p18;\n p112=c5*p4;\n p113=s2*p36;\n p114=p8*p34;\n p11"
"5=p6*p33;\n p116=p7*p39;\n p117=p26*p40;\n p118=p32*p55;\n p119=s3*c5;\n p120=s2*p31;\n p121=p"
"4*p24;\n p122=p16*p20;\n p123=p8*p33;\n p124=p40*p54;\n p125=s2*p37;\n p126=p4*p25;\n p127=s3*"
"p42;\n p128=p17*p20;\n p129=p16*p19;\n p130=p15*p24;\n p131=c5*p59;\n p132=p40*p60;\n p133=s2*"
"upArmLen;\n p134=s3*p49;\n p135=p4*p28;\n p136=p17*p19;\n p137=c3*p42;\n p138=s5*p20;\n p139=p"
"40*p61;\n p140=p26*p59;\n p141=s2*shoulderOffs;\n p142=s3*p41;\n p143=p4*p29;\n p144=p17*p23;\n "
" p145=c3*p49;\n p146=p15*p28;\n p147=handOffs*p19;\n p148=c2*upArmLen;\n p149=c6*p11;\n p150=p40"
"*p63;\n p151=p26*p55;\n p152=s3*p43;\n p153=p16*p23;\n p154=s2*p38;\n p155=c3*p41;\n p156=p15*"
"p25;\n p157=c2*p30;\n p158=handOffs*p22;\n p159=c6*p57;\n p160=p17*p50;\n p161=c7*p64;\n p162="
"p4*p38;\n p163=c3*p43;\n p164=p15*p29;\n p165=s5*p23;\n p166=c2*p36;\n p167=p22*p92;\n p168=p1"
"6*p44;\n p169=s7*p64;\n p170=c7*p45;\n p171=s2*lowArmLen;\n p172=s3*p38;\n p173=c3*p47;\n p174"
"=c2*p31;\n p175=p22*p81;\n p176=p16*p50;\n p177=p17*p44;\n p178=handOffs*p23;\n p179=s7*p45;\n "
" p180=c7*p58;\n p181=lowArmLen*p4;\n p182=s6*c3;\n p183=c2*p37;\n p184=s7*p58;\n p185=handOffs*p2"
"0;\n p186=p19*p45;\n p187=s6*p15;\n p188=s7*p19;\n p189=p5*p15;\n p190=s6*p92;\n p191=c7*p19;\n"
" p192=p5*p21;\n p193=s6*p81;\n p194=p21*p24;\n p195=p21*p28;\n p196=p21*p25;\n p197=p21*p29;\n"
" A(1,1) = -shoulderOffs*p75+shoulderOffs*p82-shoulderOffs*p76-shoulderOffs*p83;\n A(1,2) = -shoulderOffs*p"
"75+shoulderOffs*p82+p78*p75-p78*p82-shoulderOffs*p76-shoulderOffs*p83+p78*p76+p78*p83;\n A(1,3) = -shoulderOf"
"fs*p75+shoulderOffs*p82+p78*p75+p54*p73+p60*p73-p75*p133-p78*p82+p82*p133-shoulderOffs*p76-shoulderOffs*p83+p66*"
"p82-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p133-p66*p75-p66*p76-p66*p83;\n A(1,4) = -shoulderOffs*p75+s"
"houlderOffs*p82+p78*p75+p54*p73+p60*p73-p75*p133-p78*p82+p82*p133-shoulderOffs*p76-shoulderOffs*p83+p66*p82+p24*"
"p73+p56*p98+p28*p73-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p133-p66*p75-p51*p98-p66*p76-p66*p83-p46*p98-p2"
"5*p73-p52*p98+p29*p73-p42*p66+p49*p66-p41*p66-p43*p66;\n A(1,5) = -shoulderOffs*p75+shoulderOffs*p82+p78*p75+"
"p54*p73+p60*p73-p75*p133-p78*p82+p82*p133-shoulderOffs*p76-shoulderOffs*p83+p66*p82+p42*p171+s3*lowArmLen*p30+s3"
"*lowArmLen*p36+p24*p73+p56*p98+p28*p73+p77*p100+p77*p108-p49*p171-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p"
"133-p66*p75-p51*p98-p24*p172-p56*p154-p28*p172-p77*p114-p66*p76-p66*p83-s3*lowArmLen*p31-p46*p98-p25*p73-p52*p98"
"-p77*p115+p41*p171+p43*p171+s3*lowArmLen*p37+p29*p73+p77*p123-p51*p181-p42*p66+p51*p154+p56*p181+p49*p66+p77*p10"
"9-p46*p181-p52*p181-p41*p66-p43*p66+p46*p154+p25*p172+p52*p154+p77*p110+p77*p116-p29*p172+p42*p162-p49*p162+p41*"
"p162+p43*p162;\n A(1,6) = -shoulderOffs*p75+shoulderOffs*p82+p78*p75+p54*p73+p60*p73-p75*p133-p78*p82+p82*p13"
"3-shoulderOffs*p76-shoulderOffs*p83+p66*p82+p42*p171+s3*lowArmLen*p30+s3*lowArmLen*p36+p24*p73+p56*p98+p28*p73+p"
"77*p100+p77*p108-p49*p171-p61*p73+p78*p76+p78*p83+p63*p73-p76*p133-p83*p133-p66*p75-p51*p98-p24*p172-p56*p154-p2"
"8*p172-p77*p114+p49*p147-p66*p76-p66*p83-s3*lowArmLen*p31-p46*p98-p25*p73-p52*p98-p77*p115+p41*p171+p43*p171+s3*"
"lowArmLen*p37+p29*p73+p77*p123-p51*p181-p42*p66+p51*p154+p100*p90+p90*p108+p56*p181+p49*p66+p77*p109-p42*p147-p3"
"0*p185-p36*p185-p90*p115-p46*p181-p52*p181-p41*p66-p43*p66+p31*p185+p46*p154+p25*p172+p52*p154+p90*p123+p77*p110"
"+p77*p116-p41*p147-p43*p147-p37*p185-p29*p172+p51*p178+s2*p51*p57+p42*p162+p90*p109-s3*p24*p57-p56*p178-s2*p57*p"
"56-s3*p28*p57-p49*p162-p90*p114+p46*p178+s2*p46*p57+s3*p25*p57+p52*p178+s2*p52*p57+p41*p162+p43*p162+p90*p110+p9"
"0*p116-s3*p29*p57+p4*p42*p57-p4*p49*p57+p4*p41*p57+p4*p43*p57;\n A(1,7) = -p75+p82-p76-p83;\n A(1,9) = +p5"
"4+p60-p61+p63;\n A(1,10) = -c2*p75+c2*p82-c2*p76-c2*p83;\n A(1,11) = +p54+p60-p61+p63;\n A(1,12) = -s2*"
"p75+s2*p82-s2*p76-s2*p83;\n A(1,13) = +s3*p54+s3*p60+p4*p82-s3*p61+s3*p63-p4*p75-p4*p76-p4*p83;\n A(1,14) "
"= +s2*p75-s2*p82+s2*p76+s2*p83;\n A(1,15) = +p100+p108+p109-p114-p115+p123+p110+p116;\n A(1,16) = +s2*p51-"
"s3*p24-s2*p56-s3*p28+s2*p46+s3*p25+s2*p52-s3*p29+p4*p42-p4*p49+p4*p41+p4*p43;\n A(1,17) = +p100+p108+p109-p11"
"4-p115+p123+p110+p116;\n A(1,18) = +s2*p42+s3*p30+s3*p36-s2*p49-s3*p31+s2*p41+s2*p43+s3*p37-p4*p51+p4*p56-p4*"
"p46-p4*p52;\n A(1,19) = +s5*p100+s5*p108-s5*p114-s5*p115+s5*p123+s2*c5*p51+s5*p109-p24*p119-s2*c5*p56-p28*p11"
"9+s2*c5*p46+p25*p119+s2*c5*p52+s5*p110+s5*p116-p29*p119+p42*p112-p49*p112+p41*p112+p43*p112;\n A(1,20) = -s2*"
"p42-s3*p30-s3*p36+s2*p49+s3*p31-s2*p41-s2*p43-s3*p37+p4*p51-p4*p56+p4*p46+p4*p52;\n A(1,21) = +c5*p100+c5*p10"
"8+p24*p45+p56*p64+p28*p45-c5*p115+c5*p123+c5*p109-p51*p64-c5*p114+c5*p110+c5*p116-p46*p64-p25*p45-p52*p64+p29*p4"
"5-p42*p58+p49*p58-p41*p58-p43*p58;\n A(1,22) = +p19*p49+p18*p100+p18*p108-p19*p42-p20*p30-p20*p36-p18*p115+p2"
"0*p31+p18*p123-p19*p41-p19*p43-p20*p37+p23*p51+s2*p11*p51+p18*p109-s3*p11*p24-p23*p56-s2*p11*p56-s3*p11*p28-p18*"
"p114+p23*p46+s2*p11*p46+s3*p11*p25+p23*p52+s2*p11*p52+p18*p110+p18*p116-s3*p11*p29+p4*p11*p42-p4*p11*p49+p4*p11*"
"p41+p4*p11*p43;\n A(1,23) = +c5*p100+c5*p108+p24*p45+p56*p64+p28*p45-c5*p115+c5*p123+c5*p109-p51*p64-c5*p114+"
"c5*p110+c5*p116-p46*p64-p25*p45-p52*p64+p29*p45-p42*p58+p49*p58-p41*p58-p43*p58;\n A(1,24) = -p49*p72+p42*p72"
"+p30*p101+p36*p101+p53*p100+p53*p108-p31*p101-p53*p115+p41*p72+p43*p72+p37*p101+p53*p123-p51*p80+p47*p51+p56*p80"
"+p53*p109-p24*p95-p47*p56-p28*p95-p53*p114-p46*p80-p52*p80+p47*p46+p25*p95+p47*p52+p53*p110+p53*p116-p29*p95+p42"
"*p86-p49*p86+p41*p86+p43*p86;\n A(1,25) = +p70*p100+p70*p108-p70*p115+p70*p123+p70*p109+p71*p100+p71*p108-p51"
"*p169-p42*p191-c7*p20*p30-c7*p20*p36-p70*p114+p24*p179+p56*p169+p28*p179+p49*p191-p71*p115+c7*p20*p31+p70*p110+p"
"70*p116+p71*p123-p46*p169-p25*p179-p41*p191-p52*p169-p43*p191-c7*p20*p37+p29*p179+c7*p23*p51+s2*p44*p51+p71*p109"
"-p42*p184-s3*p24*p44-c7*p23*p56-s2*p44*p56-s3*p28*p44-p71*p114+p49*p184+p71*p110+p71*p116-p41*p184-p43*p184-s3*p"
"29*p44+p4*p44*p42-p4*p44*p49+c7*p23*p46+s2*p46*p44+s3*p25*p44+c7*p23*p52+s2*p44*p52+p4*p41*p44+p4*p44*p43;\n "
"A(1,26) = +p65*p100+p65*p108-p49*p188-p65*p115+p65*p123+p65*p109-p51*p161-p79*p100-p65*p114-p79*p108+p42*p188+p2"
"4*p170+s7*p20*p30+p56*p161+p28*p170+s7*p20*p36+p65*p110+p79*p115+p65*p116-p46*p161-p25*p170-s7*p20*p31-p52*p161-"
"p79*p123+p41*p188+p43*p188+p29*p170+s7*p20*p37-p42*p180-s7*p23*p51-s2*p51*p50-p79*p109+s3*p24*p50+p49*p180+s7*p2"
"3*p56+s2*p50*p56+s3*p28*p50+p79*p114-p41*p180-s7*p23*p46-s2*p46*p50-s3*p25*p50-p43*p180-s7*p23*p52-s2*p52*p50-p7"
"9*p110-p79*p116+s3*p29*p50-p4*p42*p50+p4*p50*p49-p4*p41*p50-p4*p43*p50;\n A(1,27) = -p49*p72+p42*p72+p30*p101"
"+p36*p101+p53*p100+p53*p108-p31*p101-p53*p115+p41*p72+p43*p72+p37*p101+p53*p123-p51*p80+p47*p51+p56*p80+p53*p109"
"-p24*p95-p47*p56-p28*p95-p53*p114-p46*p80-p52*p80+p47*p46+p25*p95+p47*p52+p53*p110+p53*p116-p29*p95+p42*p86-p49*"
"p86+p41*p86+p43*p86;\n\n A(2,2) = -p5*p78-p54*p141-p60*p141+p61*p141-p63*p141;\n A(2,3) = -p5*p78+p5*p133+"
"p5*p66-p54*p148-p60*p148-p54*p141-p60*p141+p54*p87+p60*p87+p61*p148+p61*p141-p63*p148-p63*p141-p61*p87+p63*p87;\n"
" A(2,4) = -p5*p78+p5*p133+p5*p66+p32*p98-p54*p148-p60*p148-p54*p141-p60*p141+p54*p87+p60*p87+p61*p148+p61*p14"
"1-p63*p148-p63*p141+p26*p66-elbowOffs*p157-elbowOffs*p166+elbowOffs*p174-p61*p87-elbowOffs*p183+p63*p87+p24*p87+"
"p28*p87-p25*p87+p29*p87;\n A(2,5) = -p5*p78+p5*p133+p5*p66+p32*p98-p26*p171-p54*p148-p60*p148-p54*p141-p60*p1"
"41+p54*p87+p60*p87-p77*p189+c2*lowArmLen*p24+c2*lowArmLen*p28+p61*p148+p61*p141-p63*p148-p63*p141+p32*p181+p26*p"
"66-elbowOffs*p157-elbowOffs*p166-p32*p154-p77*p124-p77*p132+elbowOffs*p174-p61*p87-c2*lowArmLen*p25-elbowOffs*p1"
"83+p63*p87+c2*lowArmLen*p29-p26*p162+p38*p157+p38*p166+lowArmLen*p21*p30+lowArmLen*p21*p36+p24*p87+p28*p87-p38*p"
"174-lowArmLen*p21*p31-p25*p87+p38*p183+lowArmLen*p21*p37+p29*p87+p77*p139-p77*p150-p38*p194-p38*p195+p38*p196-p3"
"8*p197;\n A(2,6) = -p5*p78+p5*p133+p5*p66+p32*p98-p26*p171-p54*p148-p60*p148-p54*p141-p60*p141+p54*p87+p60*p8"
"7-p77*p189+p26*p147+c2*lowArmLen*p24+c2*lowArmLen*p28+p61*p148+p61*p141-p63*p148-p63*p141+p32*p181+p26*p66-elbow"
"Offs*p157-elbowOffs*p166-p32*p154-p77*p124-p77*p132+elbowOffs*p174-p61*p87-c2*lowArmLen*p25-elbowOffs*p183+p63*p"
"87+c2*lowArmLen*p29-p32*p178-s2*p32*p57-p26*p162-p90*p189+p38*p157+p38*p166+lowArmLen*p21*p30+lowArmLen*p21*p36+"
"p24*p87+p28*p87-handOffs*p24*p59-handOffs*p28*p59-p38*p174-lowArmLen*p21*p31-p25*p87+handOffs*p25*p59+p38*p183+l"
"owArmLen*p21*p37+p29*p87+p77*p139-handOffs*p29*p59-p77*p150-p4*p26*p57+p57*p157+p57*p166-handOffs*p30*p55-handOf"
"fs*p36*p55-p38*p194-p38*p195-p90*p124-p90*p132+handOffs*p31*p55+p57*p183+p38*p196+p90*p139-handOffs*p37*p55-p38*"
"p197-p90*p150-p57*p194-p57*p195-p57*p174-p57*p197+p57*p196;\n A(2,10) = +c2*p5+s2*p54+s2*p60-s2*p61+s2*p63;\n"
" A(2,12) = +s2*p5-c2*p54-c2*p60+c2*p61-c2*p63;\n A(2,13) = +p5*p4+p21*p54+p21*p60-p21*p61+p21*p63;\n A("
"2,14) = -s2*p5+c2*p54+c2*p60-c2*p61+c2*p63;\n A(2,15) = -p189-p124-p132+p139-p150;\n A(2,16) = -s2*p32+p15"
"7+p166-p4*p26-p174+p183-p194-p195+p196-p197;\n A(2,17) = -p189-p124-p132+p139-p150;\n A(2,18) = -s2*p26+c2"
"*p24+c2*p28+p4*p32-c2*p25+c2*p29+p21*p30+p21*p36-p21*p31+p21*p37;\n A(2,19) = -s5*p189-s2*c5*p32-s5*p124-s5*p"
"132-p26*p112+c5*p157+c5*p166-c5*p174+c5*p183+s5*p139-s5*p150-c5*p194-c5*p195+c5*p196-c5*p197;\n A(2,20) = +s2"
"*p26-c2*p24-c2*p28-p4*p32+c2*p25-c2*p29-p21*p30-p21*p36+p21*p31-p21*p37;\n A(2,21) = +p32*p64-c5*p189+p26*p58"
"-s5*p157-s5*p166-c5*p124-c5*p132+s5*p174+c5*p139-s5*p183-c5*p150+s5*p194+s5*p195-s5*p196+s5*p197;\n A(2,22) ="
" +p19*p26-p23*p32-s2*p11*p32-p18*p189-p24*p59-p28*p59+p25*p59-p29*p59-p4*p11*p26+p11*p157+p11*p166-p30*p55-p36*p"
"55-p18*p124-p18*p132-p11*p174+p31*p55+p11*p183+p18*p139-p37*p55-p18*p150-p11*p194-p11*p195-p11*p197+p11*p196;\n "
" A(2,23) = +p32*p64-c5*p189+p26*p58-s5*p157-s5*p166-c5*p124-c5*p132+s5*p174+c5*p139-s5*p183-c5*p150+s5*p194+s5"
"*p195-s5*p196+s5*p197;\n A(2,24) = -p26*p72+p32*p80-p32*p47-p53*p189+c2*c6*p24+c2*c6*p28-c2*c6*p25+c2*c6*p29-"
"p26*p86+c6*p21*p30+p30*p131+c6*p21*p36+p36*p131-p54*p186-p60*p186-c6*p21*p31-p31*p131+c6*p21*p37+p37*p131+p61*p1"
"86-p63*p186-p24*p173-p28*p173+p25*p173-p29*p173;\n A(2,25) = -p70*p189+p32*p169+p26*p191-c7*p23*p32-s2*p32*p4"
"4-p71*p189+p26*p184-p96*p157-c7*p24*p59-p96*p166-c7*p28*p59-p70*p124-p70*p132+p96*p174+c7*p25*p59+p70*p139-p96*p"
"183-c7*p29*p59-p70*p150-p4*p26*p44+p44*p157+p44*p166-c7*p30*p55-c7*p36*p55-p71*p124-p71*p132+p96*p194+p96*p195+p"
"71*p139-p96*p196-c7*p37*p55-p71*p150+p96*p197-p44*p194-p44*p195-p44*p174+c7*p31*p55+p44*p183+p44*p196-p44*p197;\n"
" A(2,26) = -p65*p189+p32*p161-p26*p188+p26*p180+s7*p23*p32+s2*p32*p50+p79*p189-p88*p157-p88*p166-p65*p124-p65"
"*p132+s7*p24*p59+s7*p28*p59+p88*p174+p65*p139-s7*p25*p59-p88*p183-p65*p150+s7*p29*p59+p4*p26*p50-p50*p157-p50*p1"
"66+p88*p194+s7*p30*p55+p88*p195+s7*p36*p55+p79*p124+p79*p132-p88*p196-s7*p31*p55-p50*p183-p79*p139+p88*p197+s7*p"
"37*p55+p79*p150+p50*p194+p50*p195+p50*p174-p50*p196+p50*p197;\n A(2,27) = -p26*p72+p32*p80-p32*p47-p53*p189+c"
"2*c6*p24+c2*c6*p28-c2*c6*p25+c2*c6*p29-p26*p86+c6*p21*p30+p30*p131+c6*p21*p36+p36*p131-p54*p186-p60*p186-c6*p21*"
"p31-p31*p131+c6*p21*p37+p37*p131+p61*p186-p63*p186-p24*p173-p28*p173+p25*p173-p29*p173;\n\n A(3,3) = -elbowOf"
"fs*p48-elbowOffs*p67+elbowOffs*p74+elbowOffs*p84+elbowOffs*p93-elbowOffs*p62-elbowOffs*p68-elbowOffs*p85+elbowOf"
"fs*p94;\n A(3,4) = -elbowOffs*p67-elbowOffs*p48+elbowOffs*p74-elbowOffs*p117+elbowOffs*p84+elbowOffs*p93+elbo"
"wOffs*p145-elbowOffs*p62-elbowOffs*p68-elbowOffs*p137-elbowOffs*p85-elbowOffs*p155-elbowOffs*p163+elbowOffs*p94+"
"elbowOffs*p130+elbowOffs*p146-elbowOffs*p156+elbowOffs*p164;\n A(3,5) = -elbowOffs*p67-elbowOffs*p48+elbowOff"
"s*p74-lowArmLen*p32*p40-elbowOffs*p117+elbowOffs*p84+elbowOffs*p93-p77*p192+c3*lowArmLen*p56+elbowOffs*p145+p75*"
"p89-p89*p82-elbowOffs*p62-elbowOffs*p68-c3*lowArmLen*p51-elbowOffs*p137-p38*p145-elbowOffs*p85-c3*lowArmLen*p46-"
"c3*lowArmLen*p52-elbowOffs*p155-elbowOffs*p163+elbowOffs*p94+p76*p89+p89*p83+p38*p137+lowArmLen*p15*p30+lowArmLe"
"n*p15*p36+elbowOffs*p130+elbowOffs*p146+p38*p117+p54*p99+p60*p99-lowArmLen*p15*p31-elbowOffs*p156-p61*p99+p38*p1"
"55+p38*p163+lowArmLen*p15*p37+elbowOffs*p164+p63*p99-p38*p130-p38*p146+p38*p156-p38*p164;\n A(3,6) = -elbowOf"
"fs*p67-elbowOffs*p48+elbowOffs*p74-lowArmLen*p32*p40-elbowOffs*p117+elbowOffs*p84+elbowOffs*p93-p77*p192+c3*lowA"
"rmLen*p56+elbowOffs*p145+p75*p89-p89*p82-elbowOffs*p62-elbowOffs*p68-c3*lowArmLen*p51-elbowOffs*p137-p38*p145-s3"
"*p82*p90-elbowOffs*p85-c3*lowArmLen*p46-c3*lowArmLen*p52-elbowOffs*p155-elbowOffs*p163+elbowOffs*p94+p76*p89+p89"
"*p83-p90*p192+handOffs*p51*p182+p38*p137+s3*p32*p147+s3*p75*p90+lowArmLen*p15*p30+lowArmLen*p15*p36+elbowOffs*p1"
"30+elbowOffs*p146+p38*p117+p54*p99+p60*p99-handOffs*p56*p182-lowArmLen*p15*p31-elbowOffs*p156-p61*p99+handOffs*p"
"46*p182+handOffs*p52*p182+p38*p155+p38*p163+s3*p76*p90+s3*p83*p90+lowArmLen*p15*p37+elbowOffs*p164+p63*p99+p57*p"
"137+p57*p117+p4*p54*p90+p4*p60*p90-handOffs*p30*p187-handOffs*p36*p187-p57*p145-p38*p130-p38*p146-p4*p61*p90+han"
"dOffs*p31*p187+p57*p155+p57*p163+p38*p156+p4*p63*p90-handOffs*p37*p187-p38*p164-p57*p130-p57*p146-p57*p164+p57*p"
"156;\n A(3,13) = -p67-p48+p74+p84+p93-p62-p68-p85+p94;\n A(3,15) = +s3*p75-p192-s3*p82+p4*p54+p4*p60+s3*p7"
"6+s3*p83-p4*p61+p4*p63;\n A(3,16) = +p137+p117-p145+p155+p163-p130-p146+p156-p164;\n A(3,17) = -p192+s3*p7"
"5-s3*p82+p4*p54+p4*p60+s3*p76+s3*p83-p4*p61+p4*p63;\n A(3,18) = -p32*p40+c3*p56-c3*p51-c3*p46-c3*p52+p15*p30+"
"p15*p36-p15*p31+p15*p37;\n A(3,19) = -s5*p192+p45*p75-p45*p82+p45*p76+p45*p83+c5*p137+c5*p117+p54*p58+p60*p58"
"-c5*p145-p61*p58+c5*p155+c5*p163+p58*p63-c5*p130-c5*p146+c5*p156-c5*p164;\n A(3,20) = +p32*p40-c3*p56+c3*p51+"
"c3*p46+c3*p52-p15*p30-p15*p36+p15*p31-p15*p37;\n A(3,21) = +p75*p119-p82*p119-c5*p192+s5*p145+p76*p119+p83*p1"
"19+p54*p112+p60*p112-s5*p137-s5*p117-p61*p112+p63*p112-s5*p155-s5*p163+s5*p130+s5*p146-s5*p156+s5*p164;\n A(3"
",22) = -s3*p18*p82-p18*p192+p51*p182+s3*p19*p32+s3*p18*p75-p56*p182+p46*p182+p52*p182+s3*p18*p76+s3*p18*p83+p11*"
"p137+p11*p117+p4*p18*p54+p4*p18*p60-p30*p187-p36*p187-p11*p145-p4*p18*p61+p31*p187+p11*p155+p11*p163+p4*p18*p63-"
"p37*p187-p11*p130-p11*p146-p11*p164+p11*p156;\n A(3,23) = +p75*p119-p82*p119-c5*p192+s5*p145+p76*p119+p83*p11"
"9+p54*p112+p60*p112-s5*p137-s5*p117-p61*p112+p63*p112-s5*p155-s5*p163+s5*p130+s5*p146-s5*p156+s5*p164;\n A(3,"
"24) = -p82*p138-c3*c6*p51-c6*p32*p40-s5*p5*p55+c3*c6*p56+p75*p138-c3*c6*p46-c3*c6*p52+p76*p138+p83*p138+c6*p15*p"
"30+p102*p137+c6*p15*p36+s3*p26*p47+p54*p165+p60*p165-p102*p145-c6*p15*p31-p61*p165+p102*p155+c6*p15*p37+p102*p16"
"3+p63*p165-p102*p130-p102*p146+p102*p156-p102*p164;\n A(3,25) = -s3*p70*p82-p70*p192+s3*p75*p70+s3*p76*p70+s3"
"*p70*p83-p71*p192+c7*p51*p182+s3*p32*p191+p4*p54*p70+s3*p75*p71+p4*p60*p70-p96*p137-c7*p56*p182-p96*p117-s3*p71*"
"p82+p96*p145-p4*p61*p70+c7*p46*p182+c7*p52*p182+s3*p76*p71+p4*p63*p70+s3*p71*p83-p96*p155-p96*p163+p44*p137+p44*"
"p117+p4*p54*p71+p4*p60*p71-c7*p30*p187-c7*p36*p187-p44*p145+p96*p130+p96*p146+p4*p63*p71-p96*p156-c7*p37*p187+p9"
"6*p164-p44*p130-p44*p146-p4*p61*p71+c7*p31*p187+p44*p155+p44*p163+p44*p156-p44*p164;\n A(3,26) = -p65*p192+s3"
"*p65*p75-s3*p65*p82+s3*p65*p76+s3*p65*p83+p4*p54*p65+p4*p60*p65+p79*p192-p88*p137-s7*p51*p182-p88*p117-s3*p32*p1"
"88-s3*p75*p79+p88*p145+s7*p56*p182+s3*p82*p79-p4*p61*p65+p4*p65*p63-p88*p155-s7*p46*p182-p88*p163-s7*p52*p182-s3"
"*p76*p79-s3*p83*p79-p50*p137-p50*p117-p4*p54*p79-p4*p60*p79+p88*p130+s7*p30*p187+p88*p146+s7*p36*p187+p50*p145-p"
"88*p156-s7*p31*p187-p50*p155-p50*p163-p4*p63*p79+p88*p164+s7*p37*p187+p50*p130+p50*p146+p4*p61*p79-p50*p156+p50*"
"p164;\n A(3,27) = -p82*p138-c3*c6*p51-c6*p32*p40-s5*p5*p55+c3*c6*p56+p75*p138-c3*c6*p46-c3*c6*p52+p76*p138+p8"
"3*p138+c6*p15*p30+p102*p137+c6*p15*p36+s3*p26*p47+p54*p165+p60*p165-p102*p145-c6*p15*p31-p61*p165+p102*p155+c6*p"
"15*p37+p102*p163+p63*p165-p102*p130-p102*p146+p102*p156-p102*p164;\n\n A(4,4) = +elbowOffs*p104+p24*p98+p28*p"
"98+p56*p73+p32*p87-p51*p73-p25*p98-p46*p73-p52*p73+p29*p98-p30*p66-p36*p66+p31*p66-p37*p66;\n A(4,5) = +lowAr"
"mLen*p69+elbowOffs*p104-lowArmLen*p97+lowArmLen*p106+lowArmLen*p127+lowArmLen*p113+p24*p98+p28*p98+p56*p73-lowAr"
"mLen*p134+p32*p87-p51*p73-p38*p104-p24*p154-p28*p154-p56*p172-lowArmLen*p120-p25*p98-p46*p73-p52*p73+lowArmLen*p"
"142+lowArmLen*p125+lowArmLen*p152+p29*p98-p30*p66-p36*p66-p38*p107+p51*p172+lowArmLen*p121+lowArmLen*p135+p31*p6"
"6-lowArmLen*p126-p37*p66+p25*p154+p46*p172+p52*p172+lowArmLen*p143-p29*p154+p30*p162+p36*p162-p31*p162+p37*p162;"
"\n A(4,6) = +lowArmLen*p69+elbowOffs*p104-lowArmLen*p97+lowArmLen*p106+lowArmLen*p127+lowArmLen*p113+p24*p98+"
"p28*p98+p56*p73-lowArmLen*p134+p32*p87-handOffs*p32*p59-p51*p73-p38*p104-p24*p154-p28*p154-p56*p172+p49*p185-low"
"ArmLen*p120-p25*p98-p46*p73-p52*p73+lowArmLen*p142+lowArmLen*p125+lowArmLen*p152+p29*p98-p57*p104-p30*p66-p36*p6"
"6-p38*p107+p51*p172+handOffs*p151+lowArmLen*p121+lowArmLen*p135-p30*p147-p42*p185-p36*p147+p31*p66-lowArmLen*p12"
"6-p37*p66+p31*p147+p25*p154+p46*p172+p52*p172+lowArmLen*p143-p41*p185-p37*p147-p43*p185-p29*p154-p57*p107+s3*p51"
"*p57+p30*p162+p36*p162-p24*p178-s2*p24*p57-p28*p178-s2*p28*p57-s3*p57*p56-p31*p162+p25*p178+s2*p25*p57+s3*p46*p5"
"7+s3*p52*p57+p37*p162-p29*p178-s2*p29*p57+p4*p30*p57+p4*p36*p57-p4*p31*p57+p4*p37*p57;\n A(4,16) = -p104+s3*p"
"51-s2*p24-s2*p28-s3*p56-p107+s2*p25+s3*p46+s3*p52-s2*p29+p4*p30+p4*p36-p4*p31+p4*p37;\n A(4,18) = +p69-p97+p1"
"06+p127+p113-p134-p120+p142+p125+p152+p121+p135-p126+p143;\n A(4,19) = -c5*p104-c5*p107+p51*p119-s2*c5*p24-s2"
"*c5*p28-p56*p119+s2*c5*p25+p46*p119+p52*p119-s2*c5*p29+p30*p112+p36*p112-p31*p112+p37*p112;\n A(4,20) = -p69+"
"p97-p106-p127-p113+p134+p120-p142-p125-p152-p121-p135+p126-p143;\n A(4,21) = +s5*p104+p24*p64+p28*p64+p45*p56"
"+s5*p107-p45*p51-p25*p64-p46*p45-p45*p52+p29*p64-p30*p58-p36*p58+p31*p58-p37*p58;\n A(4,22) = -p32*p59+p20*p4"
"9-p11*p104+p151-p19*p30-p20*p42-p19*p36+p19*p31-p20*p41-p19*p37-p20*p43-p11*p107+s3*p11*p51-p24*p23-s2*p11*p24-p"
"23*p28-s2*p11*p28-s3*p11*p56+p25*p23+s2*p11*p25+s3*p11*p46+s3*p11*p52-p23*p29-s2*p11*p29+p4*p11*p30+p4*p11*p36-p"
"4*p11*p31+p4*p11*p37;\n A(4,23) = +s5*p104+p24*p64+p28*p64+p45*p56+s5*p107-p45*p51-p25*p64-p46*p45-p45*p52+p2"
"9*p64-p30*p58-p36*p58+p31*p58-p37*p58;\n A(4,24) = +c6*p69-p49*p101-c6*p97-p26*p131+p30*p72+p42*p101+p36*p72-"
"p31*p72+p41*p101+p37*p72+p43*p101-p32*p173+p24*p80+p51*p95+p28*p80-p24*p47-p28*p47-p56*p95-p25*p80+p25*p47+p46*p"
"95+p29*p80+p52*p95-p29*p47+p30*p86+p36*p86+p37*p86-p31*p86;\n A(4,25) = -c7*p32*p59+p96*p104-p44*p104+p96*p10"
"7+c7*p151-p51*p179-p30*p191-c7*p20*p42-p36*p191+p24*p169+p28*p169+p56*p179+c7*p20*p49+p31*p191-p25*p169-p46*p179"
"-c7*p20*p41-p52*p179-p37*p191-c7*p20*p43+p29*p169-p44*p107+s3*p44*p51-p30*p184-c7*p24*p23-s2*p24*p44-p36*p184-c7"
"*p23*p28-s2*p28*p44-s3*p44*p56-p37*p184-c7*p23*p29-s2*p29*p44+p4*p30*p44+p4*p36*p44+p31*p184+c7*p25*p23+s2*p25*p"
"44+s3*p46*p44+s3*p44*p52-p4*p31*p44+p4*p37*p44;\n A(4,26) = +p88*p104+s7*p32*p59-s7*p20*p49+p88*p107+p50*p104"
"-p51*p170-s7*p151+p24*p161+p30*p188+s7*p20*p42+p28*p161+p36*p188+p56*p170-p25*p161-p31*p188-p46*p170-p52*p170+s7"
"*p20*p41+p29*p161+p37*p188+s7*p20*p43+p50*p107-p30*p180-s3*p51*p50-p36*p180+s7*p24*p23+s2*p24*p50+s7*p23*p28+s2*"
"p28*p50+s3*p50*p56-s7*p25*p23-s2*p25*p50-s3*p46*p50-p37*p180-s3*p52*p50+s7*p23*p29+s2*p29*p50-p4*p30*p50-p4*p36*"
"p50+p31*p180+p4*p31*p50-p4*p37*p50;\n A(4,27) = +c6*p69-p49*p101-c6*p97-p26*p131+p30*p72+p42*p101+p36*p72-p31"
"*p72+p41*p101+p37*p72+p43*p101-p32*p173+p24*p80+p51*p95+p28*p80-p24*p47-p28*p47-p56*p95-p25*p80+p25*p47+p46*p95+"
"p29*p80+p52*p95-p29*p47+p30*p86+p36*p86+p37*p86-p31*p86;\n\n A(5,5) = -p48*p38+p38*p74-p69*p77-p38*p67-wristO"
"ffs*p30*p64-p42*p89-wristOffs*p36*p64+p49*p89-p38*p62-p38*p68+p77*p97+p38*p84+p38*p93-p38*p85+wristOffs*p31*p64+"
"p38*p94-p41*p89-wristOffs*p37*p64-p43*p89-p24*p99-p28*p99+p25*p99-p29*p99;\n A(5,6) = -p48*p38+p38*p74-p69*p7"
"7-p38*p67-wristOffs*p30*p64-p42*p89-wristOffs*p36*p64+p49*p89-p38*p62-p38*p68-p69*p90-p57*p67-p48*p57+p57*p74+p7"
"7*p97+p38*p84+p38*p93-p62*p57-p57*p68-p38*p85+wristOffs*p31*p64+p38*p94-p41*p89-wristOffs*p37*p64-p43*p89+p97*p9"
"0+p57*p84+p57*p93-p90*p106-p90*p127-p90*p113-p24*p99-p28*p99+p90*p134-p57*p85+p90*p120+p25*p99+p57*p94-p90*p142-"
"p90*p125-p90*p152-p29*p99-p90*p121-p90*p135-p90*p143+p90*p126;\n A(5,19) = -c5*p48+c5*p74-s5*p69-c5*p67+p45*p"
"49-c5*p62-c5*p68+s5*p97+c5*p84+c5*p93-p30*p64-p45*p42-p36*p64-c5*p85+p31*p64+c5*p94-p41*p45-p37*p64-p45*p43-p24*"
"p58-p28*p58+p25*p58-p29*p58;\n A(5,20) = 0;\n A(5,21) = +s5*p67+s5*p48-s5*p74-c5*p69+p49*p119+s5*p62+s5*p6"
"8+c5*p97-c5*p106-p42*p119-c5*p113-s5*p84-s5*p93+c5*p120+s5*p85-p41*p119-c5*p125-p43*p119-s5*p94-p24*p112-p28*p11"
"2+p25*p112-p29*p112;\n A(5,22) = -p18*p69-p11*p67-p11*p48+p11*p74-p11*p62-p11*p68+p18*p97+p11*p84+p11*p93-p18"
"*p106-p18*p127-p18*p113+p18*p134-p11*p85+p18*p120+p11*p94-p18*p142-p18*p125-p18*p152-p18*p121-p18*p135-p18*p143+"
"p18*p126;\n A(5,23) = +s5*p67+s5*p48-s5*p74-c5*p69+p49*p119+s5*p62+s5*p68+c5*p97-c5*p106-p42*p119-c5*p113-s5*"
"p84-s5*p93+c5*p120+s5*p85-p41*p119-c5*p125-p43*p119-s5*p94-p24*p112-p28*p112+p25*p112-p29*p112;\n A(5,24) = -"
"p53*p69-p67*p102-s3*p5*p47+p74*p102+p49*p138-p62*p102-p68*p102+s5*p151+p84*p102+p93*p102-s5*p19*p30-p42*p138-s5*"
"p19*p36-p85*p102+s5*p19*p31+p94*p102-p41*p138-s5*p19*p37-p43*p138-p24*p165-p28*p165+p25*p165-p29*p165;\n A(5,"
"25) = -p74*p96-p69*p70+p67*p96+p48*p96+p62*p96+p68*p96-p69*p71-p44*p67-p48*p44+p70*p97+p44*p74-p70*p106-p70*p127"
"-p70*p113-p84*p96-p96*p93+p70*p134-p44*p62-p44*p68+p70*p120+p85*p96-p70*p142-p70*p125-p70*p152-p96*p94+p71*p97+p"
"44*p84+p44*p93-p70*p121-p71*p106-p71*p127-p70*p135-p71*p113+p71*p134+p44*p94-p71*p142-p70*p143-p71*p125-p71*p152"
"-p71*p121-p71*p135-p44*p85+p70*p126+p71*p120+p71*p126-p71*p143;\n A(5,26) = -p69*p65+p67*p88+p48*p88-p74*p88+"
"p62*p88+p68*p88+p65*p97+p69*p79+p50*p67+p48*p50-p65*p106-p65*p127-p65*p113-p88*p84-p88*p93-p50*p74+p65*p134+p65*"
"p120+p88*p85+p50*p62+p50*p68-p65*p142-p65*p125-p65*p152-p88*p94-p65*p121-p65*p135-p79*p97-p50*p84-p50*p93+p79*p1"
"06+p79*p127+p79*p113-p79*p134-p79*p120-p65*p143-p50*p94+p79*p142+p79*p125+p79*p152+p79*p121+p79*p135+p65*p126+p5"
"0*p85-p79*p126+p79*p143;\n A(5,27) = -p53*p69-p67*p102-s3*p5*p47+p74*p102+p49*p138-p62*p102-p68*p102+s5*p151+"
"p84*p102+p93*p102-s5*p19*p30-p42*p138-s5*p19*p36-p85*p102+s5*p19*p31+p94*p102-p41*p138-s5*p19*p37-p43*p138-p24*p"
"165-p28*p165+p25*p165-p29*p165;\n\n A(6,6) = -c6*p104*p158-c6*p107*p158-p32*p105*p131-c6*p105*p104+p51*p101*p"
"158+p53*p67*p105+p5*p20*p53*p147-p24*p72*p158-p28*p72*p158-p56*p101*p158-p53*p74*p105-handOffs*p91*p118+p30*p80*"
"p158+p36*p80*p158+p26*p105*p173+p51*p91*p185+p67*p91*p90+p5*p22*p45*p147-p30*p47*p105-p42*p95*p105-p36*p47*p105-"
"p53*p84*p105-p24*p91*p147-p53*p93*p105-p28*p91*p147-p56*p91*p185-p74*p91*p90+p49*p95*p105+p25*p72*p158+p46*p101*"
"p158+p52*p101*p158+p53*p62*p105+p53*p68*p105-p29*p72*p158-p32*p59*p159-p84*p91*p90-p91*p93*p90+p20*p49*p159-p31*"
"p80*p158+p31*p47*p105+p37*p80*p158+p53*p85*p105+p25*p91*p147+p46*p91*p185+p62*p91*p90+p52*p91*p185+p68*p91*p90-p"
"41*p95*p105-p37*p47*p105-p43*p95*p105-p53*p94*p105-p29*p91*p147+p151*p159+p30*p91*p178+p36*p91*p178-p24*p86*p105"
"-p19*p30*p159-p20*p42*p159-p28*p86*p105-p19*p36*p159-p31*p91*p178+p25*p86*p105+p19*p31*p159+p85*p91*p90+p37*p91*"
"p178-p20*p41*p159-p29*p86*p105-p19*p37*p159-p20*p43*p159-p91*p94*p90-p23*p24*p159-p23*p28*p159+p23*p25*p159-p23*"
"p29*p159;\n A(6,22) = +c6*p22*p104-c6*p104-c6*p104-p53*p74-p53*p74+c6*p22*p107+p32*p35*p131+c6*p35*p104-p22*p"
"51*p101-p35*p53*p67-p5*p19*p20*p53+p22*p24*p72+p22*p28*p72+p22*p56*p101+p35*p53*p74-c6*p107-c6*p107-p32*p131-p32"
"*p131+p51*p101+p51*p101+p53*p67+p53*p67+p5*p186+p5*p186-p24*p72-p24*p72-p28*p72-p56*p101-p28*p72-p56*p101+p32*p5"
"9*p149+p91*p118-p22*p30*p80-p22*p36*p80-p26*p35*p173-p20*p51*p91-p18*p67*p91-p5*p22*p186+p30*p35*p47+p35*p42*p95"
"+p35*p36*p47+p35*p53*p84+p19*p24*p91+p35*p53*p93+p19*p28*p91+p20*p56*p91+p18*p74*p91+p25*p72+p46*p101+p25*p72+p4"
"6*p101+p52*p101+p52*p101-p35*p49*p95+p53*p62+p53*p62+p53*p68+p53*p68-p29*p72-p29*p72-p22*p25*p72-p22*p46*p101-p2"
"2*p52*p101+p30*p80+p30*p80+p36*p80+p36*p80-p35*p53*p62-p35*p53*p68+p26*p173+p26*p173+p22*p29*p72-p30*p47-p42*p95"
"-p30*p47-p42*p95-p36*p47-p36*p47-p53*p84-p53*p84-p53*p93-p53*p93+p49*p95+p49*p95-p31*p80-p31*p80+p24*p35*p86+p19"
"*p30*p149+p20*p42*p149+p28*p35*p86+p19*p36*p149+p18*p84*p91+p18*p93*p91+p31*p47+p31*p47+p37*p80+p37*p80-p20*p49*"
"p149+p53*p85+p53*p85-p41*p95-p41*p95-p37*p47-p43*p95-p37*p47-p43*p95-p53*p94-p53*p94+p22*p31*p80-p31*p35*p47-p22"
"*p37*p80-p35*p53*p85-p19*p25*p91-p20*p46*p91-p18*p62*p91-p20*p52*p91-p18*p68*p91+p35*p41*p95+p35*p37*p47+p35*p43"
"*p95-p24*p86-p24*p86-p28*p86-p28*p86+p35*p53*p94+p19*p29*p91-p151*p149-p23*p30*p91-p23*p36*p91-p29*p86-p29*p86+p"
"23*p31*p91-p25*p35*p86-p19*p31*p149-p18*p85*p91-p23*p37*p91+p20*p41*p149+p29*p35*p86+p19*p37*p149+p20*p43*p149+p"
"18*p94*p91+p23*p24*p149+p23*p28*p149+p25*p86+p25*p86-p23*p25*p149+p23*p29*p149;\n A(6,24) = -p140-p140-p19*p2"
"4-p19*p24-p19*p28-p20*p56-p19*p28-p20*p56+p16*p35*p140+p17*p35*p140+p11*p69+p11*p69-p118-p118-p18*p67-p18*p67-p1"
"8*p48-p18*p48+p20*p51+p20*p51+p18*p74+p18*p74-p69*p167-p18*p62-p18*p62-p18*p68-p18*p68-p69*p175+p19*p25+p20*p46+"
"p19*p25+p20*p46+p20*p52+p20*p52+p16*p35*p118+p16*p22*p140+p22*p67*p103+p17*p35*p118+p22*p48*p103+p17*p22*p140-p3"
"5*p51*p122+p22*p67*p111-p19*p29-p19*p29+p22*p48*p111-p35*p51*p128-p22*p74*p103+p24*p35*p129+p28*p35*p129+p35*p56"
"*p122-p22*p74*p111+p24*p35*p136+p28*p35*p136+p35*p56*p128-p11*p97-p11*p97+p23*p30+p23*p30+p11*p106+p11*p127+p11*"
"p106+p11*p127+p23*p36+p23*p36+p11*p113+p11*p113+p18*p84+p18*p84+p18*p93+p18*p93-p11*p134-p11*p134-p25*p35*p136-p"
"35*p46*p128-p35*p52*p128+p29*p35*p129-p23*p31-p23*p31-p11*p120-p11*p120+p29*p35*p136-p18*p85-p18*p85-p35*p69*p92"
"+p11*p142+p11*p142+p23*p37+p23*p37+p11*p125+p11*p152+p11*p125+p11*p152+p16*p22*p118+p97*p167-p35*p69*p81+p18*p94"
"+p18*p94+p17*p22*p118+p97*p175-p30*p35*p153-p106*p167-p22*p51*p122-p127*p167+p35*p67*p103-p35*p36*p153-p113*p167"
"+p5*p19*p20*p103-p30*p35*p144-p22*p84*p103-p106*p175-p22*p51*p128-p127*p175+p35*p67*p111-p35*p36*p144-p22*p93*p1"
"03-p113*p175+p5*p19*p20*p111-p22*p84*p111+p22*p24*p129-p22*p93*p111+p22*p28*p129+p22*p56*p122+p134*p167-p35*p74*"
"p103+p22*p24*p136+p22*p28*p136+p22*p56*p128+p134*p175-p35*p74*p111+p11*p121+p11*p121+p11*p135+p11*p135+p22*p62*p"
"103+p22*p68*p103-p25*p35*p129-p35*p46*p122+p22*p62*p111-p35*p52*p122+p22*p68*p111+p11*p143+p11*p143+p151*p190-p2"
"2*p30*p153-p121*p167-p22*p36*p153-p135*p167+p151*p193-p22*p30*p144-p121*p175-p19*p30*p190-p20*p42*p190-p22*p36*p"
"144-p135*p175-p19*p36*p190-p20*p54*p59*p103-p19*p30*p193-p20*p42*p193-p20*p59*p60*p103-p19*p36*p193-p20*p54*p59*"
"p111-p20*p59*p60*p111+p20*p49*p190+p20*p49*p193+p31*p35*p153+p120*p167+p31*p35*p144+p22*p85*p103+p120*p175+p22*p"
"85*p111-p22*p25*p129-p22*p46*p122-p142*p167+p35*p62*p103-p35*p37*p153-p125*p167-p22*p52*p122-p152*p167+p35*p68*p"
"103-p22*p25*p136-p22*p46*p128-p142*p175+p35*p62*p111-p35*p37*p144-p22*p94*p103-p125*p175-p22*p52*p128-p152*p175+"
"p35*p68*p111-p22*p94*p111+p22*p29*p129-p11*p126-p11*p126+p22*p29*p136-p35*p92*p121-p23*p28*p190-p35*p81*p121-p23"
"*p28*p193+p22*p31*p153+p126*p167+p22*p31*p144+p126*p175+p19*p31*p190+p20*p59*p61*p103+p19*p31*p193-p22*p37*p153-"
"p143*p167+p20*p59*p61*p111-p20*p41*p190-p22*p37*p144-p143*p175-p19*p37*p190-p20*p43*p190-p20*p41*p193-p20*p59*p6"
"3*p103-p19*p37*p193-p20*p43*p193-p20*p59*p63*p111+p35*p92*p126+p35*p81*p126-p23*p29*p190-p23*p29*p193;\n A(6,"
"25) = -c6*c7*p104-c6*c7*p107-p32*p59*p65+c7*p51*p101+c7*p53*p67+p5*p19*p170-c7*p24*p72-c7*p28*p72-c7*p56*p101-c7"
"*p53*p74+c7*p25*p72+c7*p46*p101+c7*p52*p101+c7*p53*p62+c7*p53*p68-c7*p29*p72+c7*p30*p80+c7*p36*p80+c7*p26*p173-c"
"7*p30*p47-p20*p42*p65-c7*p36*p47-c7*p53*p84-c7*p53*p93+p20*p49*p65+c7*p53*p85-p20*p41*p65-c7*p37*p47-p20*p43*p65"
"-c7*p53*p94-p23*p24*p65-p23*p28*p65-c7*p31*p80+c7*p31*p47+c7*p37*p80+p23*p25*p65-p23*p29*p65;\n A(6,26) = +s7"
"*c6*p104+s7*c6*p107+p32*p59*p70-s7*p51*p101-s7*p53*p67-p5*p19*p179+s7*p24*p72+s7*p28*p72+s7*p56*p101+s7*p53*p74-"
"s7*p25*p72-s7*p46*p101-s7*p52*p101-s7*p53*p62-s7*p53*p68+s7*p29*p72-s7*p30*p80-s7*p36*p80-s7*p26*p173+s7*p30*p47"
"+p20*p42*p70+s7*p36*p47+s7*p53*p84+s7*p53*p93-p20*p49*p70-s7*p31*p47-s7*p37*p80-s7*p53*p85+p20*p41*p70+s7*p37*p4"
"7+p20*p43*p70+s7*p53*p94+p23*p24*p70+p23*p28*p70+s7*p31*p80-p23*p25*p70+p23*p29*p70;\n A(6,27) = -p16*p140-p1"
"7*p140+p69*p81-p16*p118-p67*p103-p17*p118-p48*p103+p51*p122-p67*p111-p48*p111+p51*p128+p74*p103-p24*p129-p28*p12"
"9-p56*p122+p74*p111-p24*p136-p28*p136-p56*p128+p69*p92+p30*p144+p84*p103+p81*p106+p81*p127+p36*p144+p93*p103+p81"
"*p113+p84*p111+p93*p111-p92*p134-p81*p134-p62*p103-p68*p103+p25*p129+p46*p122-p62*p111+p52*p122-p68*p111+p25*p13"
"6+p46*p128+p52*p128-p29*p129-p29*p136-p97*p92-p81*p97+p30*p153+p92*p106+p92*p127+p36*p153+p92*p113-p31*p153-p92*"
"p120-p31*p144-p85*p103-p81*p120-p85*p111+p92*p142+p37*p153+p92*p125+p92*p152+p81*p142+p37*p144+p94*p103+p81*p125"
"+p81*p152+p94*p111+p92*p121+p92*p135+p81*p121+p81*p135-p92*p126-p81*p126+p92*p143+p81*p143;\n\n A(7,25) = +s7"
"*p17*p140-p69*p88-p67*p65-p48*p65+p65*p74-p69*p160+s7*p17*p118+s7*p16*p140+p17*p67*p79+p17*p48*p79-s7*p51*p128-p"
"62*p65-p68*p65-p17*p74*p79+s7*p24*p136+s7*p28*p136+s7*p56*p128+p88*p97+p65*p84+p65*p93-p30*p161-p42*p170-p36*p16"
"1+p49*p170+p16*p48*p79-s7*p30*p144-p106*p160-p127*p160-s7*p51*p122-s7*p36*p144-p113*p160-p65*p85-p17*p84*p79-p17"
"*p79*p93-p16*p74*p79+p31*p161+s7*p24*p129+s7*p28*p129+p134*p160+s7*p56*p122+p65*p94-p41*p170-p37*p161-p43*p170+p"
"17*p62*p79+p17*p68*p79-s7*p25*p136-s7*p46*p128-s7*p52*p128-p24*p180-p28*p180+s7*p29*p136-p69*p176+p97*p160+s7*p1"
"6*p118+p16*p67*p79-p29*p180+s7*p31*p144+p120*p160+p17*p85*p79+p16*p62*p79+p16*p68*p79-s7*p25*p129-p142*p160-s7*p"
"46*p122-s7*p37*p144-p125*p160-p160*p152-s7*p52*p122-p17*p79*p94+s7*p29*p129+p97*p176-p121*p160-s7*p30*p153-p106*"
"p176-p127*p176-p135*p160-s7*p36*p153-p113*p176-p16*p84*p79-p16*p79*p93+p25*p180+p134*p176+p126*p160+s7*p31*p153+"
"p120*p176+p16*p85*p79-p142*p176-p143*p160-s7*p37*p153-p125*p176-p152*p176-p16*p79*p94-p121*p176-p135*p176+p126*p"
"176-p143*p176;\n A(7,26) = +c7*p16*p140+p69*p96+p67*p70+p48*p70-p74*p70-p49*p179+c7*p16*p118+p16*p67*p71+p16*"
"p48*p71+c7*p17*p140-c7*p51*p122-p16*p74*p71+c7*p24*p129+c7*p28*p129+c7*p56*p122+p62*p70+p68*p70-p97*p96-p70*p84-"
"p70*p93+p30*p169+p42*p179+p36*p169-p69*p168+p17*p48*p71-p16*p71*p84-c7*p51*p128-p16*p71*p93+p134*p168-p17*p74*p7"
"1+p70*p85+c7*p24*p136+c7*p28*p136+c7*p56*p128-p31*p169-p70*p94+p41*p179+p37*p169+p43*p179+p16*p62*p71+p16*p68*p7"
"1-c7*p25*p129-c7*p46*p122-c7*p52*p122+c7*p29*p129+p24*p184+p28*p184-p69*p177+p97*p168+c7*p17*p118-c7*p30*p153-p1"
"06*p168-p127*p168+p17*p67*p71-c7*p36*p153-p113*p168-p25*p184+p29*p184+c7*p31*p153+p120*p168+p16*p71*p85-p142*p16"
"8+p17*p62*p71-c7*p37*p153-p125*p168-p152*p168+p17*p68*p71-c7*p25*p136-c7*p46*p128-p16*p71*p94-c7*p52*p128+c7*p29"
"*p136+p97*p177-p121*p168-p135*p168-c7*p30*p144-p106*p177-p127*p177-c7*p36*p144-p113*p177-p17*p71*p84-p17*p71*p93"
"+p134*p177+p126*p168+c7*p31*p144+p120*p177+p17*p71*p85-p143*p168-p142*p177-c7*p37*p144-p125*p177-p152*p177-p17*p"
"71*p94-p121*p177-p135*p177+p126*p177-p143*p177;\n\nend"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 193
position [23.5747 49.5747 7]
chart 191
linkNode [191 0 0]
subviewer 191
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 194
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 193
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 191
linkNode [191 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 191
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 195
ssIdNumber 8
name "q_motor"
linkNode [191 0 196]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 196
ssIdNumber 9
name "x_hat"
linkNode [191 195 197]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 197
ssIdNumber 10
name "gearRatio"
linkNode [191 196 198]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 198
ssIdNumber 12
name "baseAngles"
linkNode [191 197 199]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 199
ssIdNumber 13
name "gravTrq"
linkNode [191 198 200]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 200
ssIdNumber 14
name "gravTrqMotor"
linkNode [191 199 201]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 201
ssIdNumber 15
name "coulombWidthMotor"
linkNode [191 200 202]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 202
ssIdNumber 16
name "coulombWidth"
linkNode [191 201 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 203
name "Right/calcGravityTorque/calcGravTrq"
machine 1
chart 191
}
chart {
id 204
name "botharms_get_torque/MATLAB Function3"
windowPosition [142.688 83.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 205 0 0]
firstTransition 207
firstJunction 206
viewObj 204
machine 1
ssIdHighWaterMark 18
decomposition CLUSTER_CHART
type EML_CHART
firstData 208
chartFileNumber 17
disableImplicitCasting 1
eml {
name "fcn"
}
}
state {
id 205
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 204
treeNode [204 0 0 0]
superState SUBCHART
subviewer 204
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function pose = fcn(T44_FK)\n%#codegen\n\n% Reshaping:\nT44 = reshape(T44_FK, 4,4)';\n\n\n\n\n"
"% Position:\np1 = T44(1:3,4)./1000;\n\n \n\n% Orientation\nR = T44(1:3,1:3);\n\nRxx = R(1,1); Rxy = R(1,2); Rxz "
"= R(1,3);\nRyx = R(2,1); Ryy = R(2,2); Ryz = R(2,3);\nRzx = R(3,1); Rzy = R(3,2); Rzz = R(3,3);\n\nw = sqrt( tra"
"ce( R ) + 1 ) / 2;\n\n% check if w is real. Otherwise, zero it.\nif( imag( w ) > 0 )\n w = 0;\nend\n\nx1 = ("
" Rzy - Ryz ) / (4*w);\ny1 = ( Rxz - Rzx ) / (4*w);\nz1 = ( Ryx - Rxy ) / (4*w);\nq1 = [x1; y1; z1];\n\nif any(is"
"nan(q1)) || any(isinf(q1))\n q1 = zeros(size(q1));\nend\n\npersistent q_old\nif isempty(q_old)\n q_old = q"
"1;\nend\n\nq2 = -q1;\nif (norm(q_old - q1) < norm(q_old-q2)) || norm(q1)<.1\n q_out = q1;\nelse\n q_out = "
"q2;\nend\n\n\nq_old = q_out;\n\npose = [p1 ; q_out];\n\n"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 206
position [23.5747 49.5747 7]
chart 204
linkNode [204 0 0]
subviewer 204
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 207
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 206
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 204
linkNode [204 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 204
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 208
ssIdNumber 7
name "T44_FK"
linkNode [204 0 209]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 209
ssIdNumber 4
name "pose"
linkNode [204 208 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 210
name "botharms_get_torque/MATLAB Function3"
machine 1
chart 204
}
chart {
id 211
name "botharms_get_torque/MATLAB Function4"
windowPosition [157.688 68.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 212 0 0]
firstTransition 214
firstJunction 213
viewObj 211
machine 1
ssIdHighWaterMark 21
decomposition CLUSTER_CHART
type EML_CHART
firstData 215
chartFileNumber 18
disableImplicitCasting 1
eml {
name "fcn"
}
}
state {
id 212
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 211
treeNode [211 0 0 0]
superState SUBCHART
subviewer 211
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [vel1,vel2] = fcn(J1,J2,q_dot)\n%#codegen\n\nJ1 = reshape(J1,7,6)';\nJ1(1:3,:) = J1(1"
":3,:)/1000; % Convert to m/s\n\nJ2 = reshape(J2,7,6)';\nJ2(1:3,:) = J2(1:3,:)/1000; % Convert to m/s\n\nqd1 = q_"
"dot(1:7);\nqd2 = q_dot(8:end);\n\n\nvel1 = J1*qd1;\nvel2 = J2*qd2;"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 213
position [23.5747 49.5747 7]
chart 211
linkNode [211 0 0]
subviewer 211
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 214
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 213
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 211
linkNode [211 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 211
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 215
ssIdNumber 7
name "J1"
linkNode [211 0 216]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 216
ssIdNumber 4
name "vel1"
linkNode [211 215 217]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 217
ssIdNumber 19
name "J2"
linkNode [211 216 218]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 218
ssIdNumber 20
name "q_dot"
linkNode [211 217 219]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 219
ssIdNumber 21
name "vel2"
linkNode [211 218 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 220
name "botharms_get_torque/MATLAB Function4"
machine 1
chart 211
}
chart {
id 221
name "botharms_get_torque/lp1"
windowPosition [165.188 53.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 222 0 0]
firstTransition 224
firstJunction 223
viewObj 221
machine 1
toolbarMode LIBRARY_TOOLBAR
ssIdHighWaterMark 5
decomposition CLUSTER_CHART
type EML_CHART
firstData 225
chartFileNumber 19
disableImplicitCasting 1
eml {
name "lp"
}
}
state {
id 222
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 221
treeNode [221 0 0 0]
superState SUBCHART
subviewer 221
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function v_out = lp(v_in)\n%#codegen\n\n\npersistent v_flt\nif isempty(v_flt)\n v_flt = v_in;\n"
"end\nT = 0.500;\ndt = 0.004;\n\nv_dot = (v_in - v_flt)/T;\n\nv_flt = v_flt + dt*v_dot;\n\nv_out = v_flt;\n"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 223
position [23.5747 49.5747 7]
chart 221
linkNode [221 0 0]
subviewer 221
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 224
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 223
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 221
linkNode [221 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 221
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 225
ssIdNumber 4
name "v_in"
linkNode [221 0 226]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 226
ssIdNumber 5
name "v_out"
linkNode [221 225 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 227
name "botharms_get_torque/lp1"
machine 1
chart 221
}
chart {
id 228
name "botharms_get_torque/dither/DITHERING"
windowPosition [495.75 83.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 229 0 0]
firstTransition 231
firstJunction 230
viewObj 228
machine 1
toolbarMode LIBRARY_TOOLBAR
ssIdHighWaterMark 7
decomposition CLUSTER_CHART
type EML_CHART
firstData 232
chartFileNumber 20
disableImplicitCasting 1
eml {
name "fcn"
}
}
state {
id 229
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 228
treeNode [228 0 0 0]
superState SUBCHART
subviewer 228
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function dithering = fcn(coulomb,Fdith,amp)\n\npersistent cont\nif isempty(cont)\n cont=0;\nen"
"d\n\npersistent out\nif isempty(out)\n out=0;\nend\n\n\ntc=1/250; % sempling period\nT=1/Fdith; % period of t"
"he dithering signal\nN=round(T/tc); % number of sample in a period of the dithering signal\n\n\n\nif cont>=0 && "
"cont<N/2 % first half period\n out=amp;\n cont=cont+1;\nend\nif cont >= N/2 && cont<=N % second half perio"
"d\n out=-amp;\n cont=cont+1;\nend\n\nif cont>=N % reset cont \n cont=0;\nend\n\ndithering=out."
"*coulomb;"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 230
position [23.5747 49.5747 7]
chart 228
linkNode [228 0 0]
subviewer 228
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 231
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 230
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 228
linkNode [228 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 228
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 232
ssIdNumber 7
name "coulomb"
linkNode [228 0 233]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 233
ssIdNumber 4
name "Fdith"
linkNode [228 232 234]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 234
ssIdNumber 5
name "dithering"
linkNode [228 233 235]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 235
ssIdNumber 6
name "amp"
linkNode [228 234 0]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
instance {
id 236
name "botharms_get_torque/dither/DITHERING"
machine 1
chart 228
}
chart {
id 237
name "botharms_get_torque/coulFric1"
windowPosition [195.375 188.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 238 0 0]
firstTransition 240
firstJunction 239
viewObj 237
machine 1
toolbarMode LIBRARY_TOOLBAR
ssIdHighWaterMark 13
decomposition CLUSTER_CHART
type EML_CHART
firstData 241
chartFileNumber 21
disableImplicitCasting 1
eml {
name "forcelog"
}
}
state {
id 238
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 237
treeNode [237 0 0 0]
superState SUBCHART
subviewer 237
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [wrench1_out, wrench2_out] = forcelog(T1,T2, wrench1, wrench2)\n\n% Rotations\nrotz = -p"
"i/4;\ntf2t = [cos(rotz) -sin(rotz) 0;\n sin(rotz) cos(rotz) 0;\n 0 0 1];\n\nrotz2 = -pi/2;\ntf2t2 = [cos(r"
"otz2) -sin(rotz2) 0;\n sin(rotz2) cos(rotz2) 0;\n 0 0 1];\n\npersistent w1offs\nif isempty(w1offs)\n w1"
"offs = wrench1;\nend\nif w1offs(1) == 0 && wrench1(1) ~= 0\n w1offs = wrench1;\nend\nwrench1 = wrench1 - w1of"
"fs;\n\n\n\n\npersistent w2offs\nif isempty(w2offs)\n w2offs = wrench2;\nend\nif w2offs(1) == 0 && wrench2(1) "
"~= 0\n w2offs = wrench2;\nend\nwrench2 = wrench2 - w2offs;\n\n\n\nT1 = reshape(T1, 4,4)';\nR1 = T1(1:3,1:3);\n"
"wrench1_out = [R1*tf2t*wrench1(1:3); R1*tf2t*wrench1(1:3)];\n\n\nT2 = reshape(T2, 4,4)';\nR2 = T2(1:3,1:3);\nwre"
"nch2_out = [R2*tf2t2*wrench2(1:3); R2*tf2t2*wrench2(1:3)];\n"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 239
position [23.5747 49.5747 7]
chart 237
linkNode [237 0 0]
subviewer 237
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 240
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 239
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 237
linkNode [237 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 237
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 241
ssIdNumber 4
name "T1"
linkNode [237 0 242]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 242
ssIdNumber 9
name "T2"
linkNode [237 241 243]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 243
ssIdNumber 5
name "wrench1_out"
linkNode [237 242 244]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 244
ssIdNumber 11
name "wrench1"
linkNode [237 243 245]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 245
ssIdNumber 12
name "wrench2"
linkNode [237 244 246]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 246
ssIdNumber 13
name "wrench2_out"
linkNode [237 245 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 247
name "botharms_get_torque/coulFric1"
machine 1
chart 237
}
chart {
id 248
name "botharms_get_torque/lp2"
windowPosition [195.188 23.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 249 0 0]
firstTransition 251
firstJunction 250
viewObj 248
machine 1
toolbarMode LIBRARY_TOOLBAR
ssIdHighWaterMark 5
decomposition CLUSTER_CHART
type EML_CHART
firstData 252
chartFileNumber 22
disableImplicitCasting 1
eml {
name "lp"
}
}
state {
id 249
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 248
treeNode [248 0 0 0]
superState SUBCHART
subviewer 248
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function v_out = lp(v_in)\n%#codegen\n\n\npersistent v_flt\nif isempty(v_flt)\n v_flt = v_in;\n"
"end\nT = 0.100;\ndt = 0.004;\n\nv_dot = (v_in - v_flt)/T;\n\nv_flt = v_flt + dt*v_dot;\n\nv_out = v_flt;\n"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 250
position [23.5747 49.5747 7]
chart 248
linkNode [248 0 0]
subviewer 248
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 251
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 250
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 248
linkNode [248 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 248
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 252
ssIdNumber 4
name "v_in"
linkNode [248 0 253]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 253
ssIdNumber 5
name "v_out"
linkNode [248 252 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 254
name "botharms_get_torque/lp2"
machine 1
chart 248
}
chart {
id 255
name "botharms_get_torque/lp3"
windowPosition [195.188 23.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 256 0 0]
firstTransition 258
firstJunction 257
viewObj 255
machine 1
toolbarMode LIBRARY_TOOLBAR
ssIdHighWaterMark 5
decomposition CLUSTER_CHART
type EML_CHART
firstData 259
chartFileNumber 23
disableImplicitCasting 1
eml {
name "lp"
}
}
state {
id 256
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 255
treeNode [255 0 0 0]
superState SUBCHART
subviewer 255
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function v_out = lp(v_in)\n%#codegen\n\n\npersistent v_flt\nif isempty(v_flt)\n v_flt = v_in;\n"
"end\nT = 0.100;\ndt = 0.004;\n\nv_dot = (v_in - v_flt)/T;\n\nv_flt = v_flt + dt*v_dot;\n\nv_out = v_flt;\n"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 257
position [23.5747 49.5747 7]
chart 255
linkNode [255 0 0]
subviewer 255
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 258
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 257
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 255
linkNode [255 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 255
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 259
ssIdNumber 4
name "v_in"
linkNode [255 0 260]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 260
ssIdNumber 5
name "v_out"
linkNode [255 259 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 261
name "botharms_get_torque/lp3"
machine 1
chart 255
}
chart {
id 262
name "botharms_get_torque/MATLAB Function6"
windowPosition [270.375 83.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 263 0 0]
firstTransition 265
firstJunction 264
viewObj 262
machine 1
toolbarMode LIBRARY_TOOLBAR
ssIdHighWaterMark 5
decomposition CLUSTER_CHART
type EML_CHART
firstData 266
chartFileNumber 24
disableImplicitCasting 1
eml {
name "abb2logical"
}
}
state {
id 263
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 262
treeNode [262 0 0 0]
superState SUBCHART
subviewer 262
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function logical = abb2logical(abb)\n%#codegen\n\nlogical = [abb(1:2); abb(7); abb(3:6)];"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 264
position [23.5747 49.5747 7]
chart 262
linkNode [262 0 0]
subviewer 262
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 265
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 264
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 262
linkNode [262 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 262
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 266
ssIdNumber 4
name "abb"
linkNode [262 0 267]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 267
ssIdNumber 5
name "logical"
linkNode [262 266 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 268
name "botharms_get_torque/MATLAB Function6"
machine 1
chart 262
}
chart {
id 269
name "Right/MATLAB Function3"
windowPosition [315.375 38.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 1920 1200 1.25]
treeNode [0 270 0 0]
firstTransition 272
firstJunction 271
viewObj 269
machine 1
ssIdHighWaterMark 32
decomposition CLUSTER_CHART
type EML_CHART
firstData 273
updateMethod DISCRETE
sampleTime "0.004"
chartFileNumber 27
disableImplicitCasting 1
eml {
name "fcn"
}
}
state {
id 270
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 269
treeNode [269 0 0 0]
superState SUBCHART
subviewer 269
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [runExtctrl , lockOutput, resetProgram, state] = fcn(controlActive, obtaining, switch_on"
")\n\n\nleadthrough_on =switch_on && obtaining;\n%obtaining = 1;\n% state variables\npersistent internalState;\n\n"
"% internal variables\npersistent runExtctrlInternal;\npersistent lockOutputInternal;\npersistent resetProgramInt"
"ernal;\n\nif isempty(internalState)\n internalState = 1;\nend\n\nif isempty(runExtctrlInternal)\n runExtct"
"rlInternal = 0;\nend\nif isempty(lockOutputInternal)\n lockOutputInternal = 0;\nend\nif isempty(resetProgramI"
"nternal)\n resetProgramInternal = 1;\nend\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n% number internalSta"
"te\n% 1 Extctrl_Idle\n% 2 Extctrl_Running\n% 3 Recovery_Phase\n\n%%%%%%%%%%%%%%"
"%%%%%%%%%%%%%%%%%%%%\n\n% State transitions + entry/exit-actions\nif internalState==1\n if (leadthrough_on) &"
"& obtaining\n %Exit-actions\n \n %New state\n internalState = 2;\n \n "
"%Entry-actions\n runExtctrlInternal = 1;\n lockOutputInternal = 0;\n resetProgramInternal ="
" 0;\n end\nelseif internalState==2\n if ~controlActive\n %Exit-actions\n \n %New stat"
"e\n internalState = 1;\n \n %Entry-actions\n runExtctrlInternal = 0;\n lockOu"
"tputInternal = 0;\n resetProgramInternal = 1;\n elseif (~leadthrough_on) && controlActive\n %Ex"
"it-actions\n \n %New state\n internalState = 3;\n \n %Entry-actions\n "
"runExtctrlInternal = 0;\n lockOutputInternal = 1;\n resetProgramInternal = 0; \n end\nelsei"
"f internalState==3\n if (~leadthrough_on) || ~controlActive\n %Exit-actions\n \n %New st"
"ate\n internalState = 1;\n \n %Entry-actions\n runExtctrlInternal = 0;\n lock"
"OutputInternal = 0;\n resetProgramInternal = 1; \n end\nend\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"
"%\n\n% Continous state actions\n\n%Set outputs\nrunExtctrl = runExtctrlInternal;\nlockOutput = lockOutputInterna"
"l;\nresetProgram = resetProgramInternal;\nstate = internalState;"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 271
position [23.5747 49.5747 7]
chart 269
linkNode [269 0 0]
subviewer 269
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 272
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 271
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 269
linkNode [269 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 269
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 273
ssIdNumber 28
name "runExtctrl"
linkNode [269 0 274]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 274
ssIdNumber 5
name "lockOutput"
linkNode [269 273 275]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 275
ssIdNumber 27
name "resetProgram"
linkNode [269 274 276]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 276
ssIdNumber 29
name "state"
linkNode [269 275 277]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 277
ssIdNumber 30
name "controlActive"
linkNode [269 276 278]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 278
ssIdNumber 31
name "obtaining"
linkNode [269 277 279]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 279
ssIdNumber 32
name "switch_on"
linkNode [269 278 0]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
instance {
id 280
name "Right/MATLAB Function3"
machine 1
chart 269
}
chart {
id 281
name "botharms_get_torque/MATLAB Function7"
windowPosition [390.375 -36.75 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 282 0 0]
firstTransition 284
firstJunction 283
viewObj 281
machine 1
ssIdHighWaterMark 28
decomposition CLUSTER_CHART
type EML_CHART
firstData 285
updateMethod DISCRETE
sampleTime "0.004"
chartFileNumber 28
disableImplicitCasting 1
eml {
name "nancheck"
}
}
state {
id 282
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 281
treeNode [281 0 0 0]
superState SUBCHART
subviewer 281
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function y = nancheck(u)\n\n\ny = u;\n\n\nif any(isnan(u)) || any(isinf(u))\n y = zeros(size"
"(u));\nend\n\nend"
editorLayout "100 M4x1[772 19 980 1190]"
}
}
junction {
id 283
position [23.5747 49.5747 7]
chart 281
linkNode [281 0 0]
subviewer 281
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 284
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 283
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 281
linkNode [281 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 281
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 285
ssIdNumber 28
name "y"
linkNode [281 0 286]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 286
ssIdNumber 4
name "u"
linkNode [281 285 0]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
instance {
id 287
name "botharms_get_torque/MATLAB Function7"
machine 1
chart 281
}
chart {
id 288
name "botharms_get_torque/FRIDA arm acceleration NOT SI1/base_tool_transform"
windowPosition [187.688 53.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 289 0 0]
firstTransition 291
firstJunction 290
viewObj 288
machine 1
ssIdHighWaterMark 13
decomposition CLUSTER_CHART
type EML_CHART
firstData 292
chartFileNumber 29
disableImplicitCasting 1
eml {
name "base_tool_transform"
}
}
state {
id 289
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 288
treeNode [288 0 0 0]
superState SUBCHART
subviewer 288
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [T44_out, J0_out, Jn_out, Jdot0_out] = base_tool_transform(T44_world2base, T44_flange2to"
"ol, T44_in, J0_in, qd, Jdot0_in)\n\nT44_world2base_m = reshape(T44_world2base, 4, 4)';\nT44_flange2tool_m = resh"
"ape(T44_flange2tool, 4, 4)';\nT44_in_m = reshape(T44_in, 4, 4)';\nJ0_in_m = reshape(J0_in, [], 6)';\nJdot0_in_m "
"= reshape(Jdot0_in, [], 6)';\n\n% Forward kinematics\nT44_out_m = T44_world2base_m*T44_in_m*T44_flange2tool_m;\n"
"\n% Rotation of base\nRbase = T44_world2base_m(1:3,1:3);\nTbase = [Rbase zeros(3);zeros(3) Rbase];\nJ0_m = Tbase"
"*J0_in_m;\nJdot0_m = Tbase*Jdot0_in_m;\n\n% Handle tool transformation\nT44_baseToFlange = T44_world2base_m*T44_"
"in_m;\nT44_baseToTcp = T44_out_m;\nr = T44_baseToTcp(1:3,4) - T44_baseToFlange(1:3,4);\nomega = J0_m(4:6,:);\n\n"
"for i=1:size(Jdot0_m,2)\n jContribution = cross(omega(:,i),r);\n J0_m(1:3,i) = J0_m(1:3,i) + jContribution"
";\n for j=1:size(Jdot0_m,2)\n jdotContribution = cross(omega(:,i),cross(omega(:,j)*qd(j),r));\n "
" Jdot0_m(1:3,i) = Jdot0_m(1:3,i) + jdotContribution;\n end\nend\n\n% Jacobian in tool frame\nJn_m = [T44_out_"
"m(1:3,1:3)' zeros(3);zeros(3) T44_out_m(1:3,1:3)']*J0_m;\n\n \nT44_out = reshape(T44_out_m', 16, 1);\nJ0_o"
"ut = reshape(J0_m', [], 1);\nJn_out = reshape(Jn_m', [], 1);\nJdot0_out = reshape(Jdot0_m',[],1);\n\n\n"
editorLayout "100 M4x1[205 227 1080 733]"
}
}
junction {
id 290
position [23.5747 49.5747 7]
chart 288
linkNode [288 0 0]
subviewer 288
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 291
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 290
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 288
linkNode [288 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 288
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 292
ssIdNumber 4
name "T44_world2base"
linkNode [288 0 293]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 293
ssIdNumber 5
name "T44_out"
linkNode [288 292 294]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 294
ssIdNumber 6
name "T44_flange2tool"
linkNode [288 293 295]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 295
ssIdNumber 7
name "T44_in"
linkNode [288 294 296]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 296
ssIdNumber 8
name "J0_in"
linkNode [288 295 297]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 297
ssIdNumber 9
name "qd"
linkNode [288 296 298]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 298
ssIdNumber 10
name "Jdot0_in"
linkNode [288 297 299]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 299
ssIdNumber 11
name "J0_out"
linkNode [288 298 300]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 300
ssIdNumber 12
name "Jn_out"
linkNode [288 299 301]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 301
ssIdNumber 13
name "Jdot0_out"
linkNode [288 300 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 302
name "botharms_get_torque/FRIDA arm acceleration NOT SI1/base_tool_transform"
machine 1
chart 288
}
chart {
id 303
name "botharms_get_torque/FRIDA arm acceleration NOT SI1/frida_kin"
windowPosition [187.688 53.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 304 0 0]
firstTransition 306
firstJunction 305
viewObj 303
machine 1
ssIdHighWaterMark 9
decomposition CLUSTER_CHART
type EML_CHART
firstData 307
chartFileNumber 30
disableImplicitCasting 1
eml {
name "frida_kinematics"
}
}
state {
id 304
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 303
treeNode [303 0 0 0]
superState SUBCHART
subviewer 303
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [T44_fkine, J0, Jn, Jdot0] = frida_kinematics(q,qd)\n% Calculation of forward kinematics"
", jacobian in base frame and flange\n% frame, and Jacobian time derivative (in base frame)\n\nn_joints = 7;\n\nq"
"_offs = [0;0;pi/2;0;0;0;0];\nq = q+q_offs;\n\nshoulderLen = 0.110*1000;\nshoulderOffs = -0.030*1000;\nelbowOffs "
"= 0.0405*1000;\nwristOffs = 0.0135*1000;\nhandOffs = -0.027*1000;\nupArmLen = 0.2465*1000;\nlowArmLen = 0.265*10"
"00;\nhandLen = 0.032*1000;\n\n\n% alpha a theta d Rev=0/Pris=1\ndhpar = [ -pi"
"/2 shoulderOffs q(1) shoulderLen 0\n pi/2 -shoulderOffs q(2) 0 0\n -"
"pi/2 elbowOffs q(7) upArmLen 0\n pi/2 -elbowOffs q(3) 0 0\n "
" -pi/2 wristOffs q(4) lowArmLen 0\n pi/2 handOffs q(5) 0 0\n "
" 0 0 q(6) handLen 0 ];\n \nbase = eye(4);\n \ntrans = zeros(4,4,n_joints"
");\n \nfor j=1:n_joints\n t = [ cos(dhpar(j,3)) -sin(dhpar(j,3))*cos(dhpar(j,1)) sin(dhpar(j,3))*"
"sin(dhpar(j,1)) dhpar(j,2)*cos(dhpar(j,3))\n sin(dhpar(j,3)) cos(dhpar(j,3))*cos(dhpar(j,1)) -"
"cos(dhpar(j,3))*sin(dhpar(j,1)) dhpar(j,2)*sin(dhpar(j,3))\n 0 sin(dhpar(j,1)) "
" cos(dhpar(j,1)) dhpar(j,4)\n 0 0 "
" 0 1 ];\n trans(:,:,j) = t;\nend\n\n%%%"
"%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% fkine\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\nT = base;\nfor i=1:n_joints,\n "
"T = T * trans(:,:,i);\nend\n\nT44_fkine = reshape(T',16,1);\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% tr2rot\n%%"
"%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\nR = T(1:3,1:3);\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% J0 + Jdot0\n%%%%%%"
"%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\nJ0_tmp = zeros(6,n_joints);\n\nJ_dot = zeros(6,n_joints);\nomega = zeros(3,n_joi"
"nts);\nnonZeroOmega = zeros(n_joints,1);\n\nT44_FK = eye(4);\n\nfor k=1:n_joints\n\n if dhpar(j,5) == 0\n % "
"revolute axis\n omega(:,k) = T44_FK(1:3,3);\n nonZeroOmega(k) = 1;\n J0_tmp(4:6,k) = omega("
":,k);\n end\n p_prev = T44_FK(1:3,4);\n T44_FK = T44_FK*trans(:,:,k);\n r_tilde = T44_FK(1:3,4)-p_pr"
"ev;\n \n for i=1:n_joints\n if nonZeroOmega(i)\n J0_tmp(1:3,i) = J0_tmp(1:3,i) + cross(o"
"mega(:,i),r_tilde);\n for j=1:n_joints\n if nonZeroOmega(j)\n contr"
"ibution = cross(omega(:,i),cross(omega(:,j)*qd(j),r_tilde));\n J_dot(1:3,i) = J_dot(1:3,i) + "
"contribution;\n end\n end\n end\n end\n\n if dhpar(j,5) == 1\n % prismat"
"ic axis\n prismAxis = T44_FK(1:3,3);\n J0_tmp(1:3,k) = prismAxis;\n for i=1:n_joints\n "
" contribution = cross(omega(:,i),prismAxis);\n J_dot(1:3,k) = J_dot(1:3,k) + contribution*qd(i)"
";\n J_dot(1:3,i) = J_dot(1:3,i) + contribution*qd(k);\n end\n end\nend\nJ0_tmp = J0_tmp(:,["
"1 2 4 5 6 7 3]); % Switch to wierd joint order\nJ0 = reshape(J0_tmp', 6*n_joints,1);\n\nJdot0 = reshape(J_dot',"
" 6*n_joints,1);\n\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n%% Jn\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n\nJn_tmp = ["
"R' zeros(3);zeros(3) R']*J0_tmp;\nJn = reshape(Jn_tmp', 6*n_joints,1);\n"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 305
position [23.5747 49.5747 7]
chart 303
linkNode [303 0 0]
subviewer 303
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 306
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 305
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 303
linkNode [303 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 303
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 307
ssIdNumber 4
name "q"
linkNode [303 0 308]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 308
ssIdNumber 5
name "T44_fkine"
linkNode [303 307 309]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 309
ssIdNumber 6
name "qd"
linkNode [303 308 310]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 310
ssIdNumber 7
name "J0"
linkNode [303 309 311]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 311
ssIdNumber 8
name "Jn"
linkNode [303 310 312]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 312
ssIdNumber 9
name "Jdot0"
linkNode [303 311 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 313
name "botharms_get_torque/FRIDA arm acceleration NOT SI1/frida_kin"
machine 1
chart 303
}
chart {
id 314
name "botharms_get_torque/MATLAB Function10"
windowPosition [292.875 53.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 315 0 0]
firstTransition 317
firstJunction 316
viewObj 314
machine 1
toolbarMode LIBRARY_TOOLBAR
ssIdHighWaterMark 5
decomposition CLUSTER_CHART
type EML_CHART
firstData 318
chartFileNumber 31
disableImplicitCasting 1
eml {
name "abb2logical"
}
}
state {
id 315
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 314
treeNode [314 0 0 0]
superState SUBCHART
subviewer 314
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function logical = abb2logical(abb)\n%#codegen\n\nlogical = [abb(1:2); abb(7); abb(3:6)];"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 316
position [23.5747 49.5747 7]
chart 314
linkNode [314 0 0]
subviewer 314
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 317
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 316
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 314
linkNode [314 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 314
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 318
ssIdNumber 4
name "abb"
linkNode [314 0 319]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 319
ssIdNumber 5
name "logical"
linkNode [314 318 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 320
name "botharms_get_torque/MATLAB Function10"
machine 1
chart 314
}
chart {
id 321
name "botharms_get_torque/haptic_controller"
windowPosition [183.938 68.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 322 0 0]
firstTransition 324
firstJunction 323
viewObj 321
machine 1
ssIdHighWaterMark 28
decomposition CLUSTER_CHART
type EML_CHART
firstData 325
chartFileNumber 35
disableImplicitCasting 1
eml {
name "fcn"
}
}
state {
id 322
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 321
treeNode [321 0 0 0]
superState SUBCHART
subviewer 321
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function [Q1,lambda, Q2,diagm] = fcn(B1,C1,J1,J1_dot, q1_dot, B2,C2,J2, J2_dot, q2_dot, e, scale)"
"\n%#codegen\n\n\n% Reshape before paper method:\n\nJ1 = reshape(J1,7,6)';\nJ1(1:3,:) = J1(1:3,:)/1000; % Convert"
" to m/s\nJ2 = reshape(J2,7,6)';\nJ2(1:3,:) = J2(1:3,:)/1000; % Convert to m/s\n\ne(1:3) = e(1:3)/1000;\n\nJ1_dot"
" = reshape(J1_dot,7,6)';\nJ1_dot(1:3,:) = J1_dot(1:3,:)/1000; % Convert to m/s\nJ2_dot = reshape(J2_dot,7,6)';\n"
"J2_dot(1:3,:) = J2_dot(1:3,:)/1000; % Convert to m/s\n\n % Convert B1, B2 to abb matrices\nB1 = reshape(B1,7,"
"7)';\nB1 = [B1(1:2,:); B1(4:7,:); B1(3,:)];\nB1 = [B1(:,1:2) B1(:,4:7) B1(:,3)];\n\nB2 = reshape(B2,7,7)';\nB2 ="
" [B2(1:2,:); B2(4:7,:); B2(3,:)];\nB2 = [B2(:,1:2) B2(:,4:7) B2(:,3)];\n\n\n % Convert C1, C2 to abb matrices"
"\nC1 = reshape(C1,7,7)';\nC1 = [C1(1:2,:); C1(4:7,:); C1(3,:)];\nC1 = [C1(:,1:2) C1(:,4:7) C1(:,3)];\n\nC2 = res"
"hape(C2,7,7)';\nC2 = [C2(1:2,:); C2(4:7,:); C2(3,:)];\nC2 = [C2(:,1:2) C2(:,4:7) C2(:,3)];\n\nq_dot = [q1_dot; q"
"2_dot];\n\n% Control gains:\n\n% kp_dist = 225; % Can be increased later\nkp_dist = 250;\nkd_dist = 2*sqrt(kp_di"
"st); % Nominal. Real negative double pole.\n% kp_quat = 160; % Can be increased later\nkp_quat = 250; % 200\nkd_"
"quat = 2*sqrt(kp_quat); % Derivative gain\n\nkp = [kp_dist*ones(3,1); kp_quat*ones(3,1)];\nkd = [kd_dist*ones(3,"
"1); kd_quat*ones(3,1)]; % Derivative gain\n\n\n% Scaling;\nJ2(1:3,:) = scale*J2(1:3,:);\nJ2_dot(1:3,:) = scale*J"
"2_dot(1:3,:);\n\n% Implementation according to paper:\nG = [-J1 J2];\n\n\ny_dot = G*q_dot;\nG_dot = [-J1_dot J2_"
"dot];\n\n\nM = blkdiag(B1,B2);\nM = M + 0.005*eye(14);\n\n\nC = blkdiag(C1,C2);\n\nGamma = G*pinv(M)*G';\n\n\n\n"
"\nustar = pinv(Gamma)*(G*pinv(M)*(C*q_dot)-G_dot*q_dot);\nlambda = ustar - pinv(Gamma)*(kd.*y_dot + kp.*e);\n\n\n"
"\nQ1 = -J1'*lambda;\n\n% Undo previous scaling, and scale acceleration.\nJ2(1:3,:) = J2(1:3,:)./scale^2;\nQ2 = J"
"2'*lambda;\n\n\n\n\ndiagm = diag(M);\n% % % Nice for debugging:\n% Q1(1:3)=0;\n% Q1(7) = 0;\n% Q2(1:3)=0;\n% Q2"
"(7) = 0;"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 323
position [23.5747 49.5747 7]
chart 321
linkNode [321 0 0]
subviewer 321
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 324
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 323
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 321
linkNode [321 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 321
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 325
ssIdNumber 12
name "B1"
linkNode [321 0 326]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 326
ssIdNumber 13
name "C1"
linkNode [321 325 327]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 327
ssIdNumber 7
name "J1"
linkNode [321 326 328]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 328
ssIdNumber 9
name "J1_dot"
linkNode [321 327 329]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 329
ssIdNumber 10
name "q1_dot"
linkNode [321 328 330]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 330
ssIdNumber 18
name "B2"
linkNode [321 329 331]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 331
ssIdNumber 19
name "C2"
linkNode [321 330 332]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 332
ssIdNumber 20
name "J2"
linkNode [321 331 333]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 333
ssIdNumber 21
name "J2_dot"
linkNode [321 332 334]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 334
ssIdNumber 22
name "q2_dot"
linkNode [321 333 335]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 335
ssIdNumber 4
name "e"
linkNode [321 334 336]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 336
ssIdNumber 5
name "Q1"
linkNode [321 335 337]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 337
ssIdNumber 24
name "lambda"
linkNode [321 336 338]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 338
ssIdNumber 8
name "Q2"
linkNode [321 337 339]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 339
ssIdNumber 27
name "diagm"
linkNode [321 338 340]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
data {
id 340
ssIdNumber 28
name "scale"
linkNode [321 339 0]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
isSigned 1
wordLength "16"
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_INHERITED
}
dataType "Inherit: Same as Simulink"
}
instance {
id 341
name "botharms_get_torque/haptic_controller"
machine 1
chart 321
}
chart {
id 342
name "botharms_get_torque/MATLAB Function9"
windowPosition [307.875 38.25 200.25 189.75]
viewLimits [0 156.75 0 153.75]
screen [1 1 3840 1200 1.25]
treeNode [0 343 0 0]
firstTransition 345
firstJunction 344
viewObj 342
machine 1
toolbarMode LIBRARY_TOOLBAR
ssIdHighWaterMark 5
decomposition CLUSTER_CHART
type EML_CHART
firstData 346
chartFileNumber 36
disableImplicitCasting 1
eml {
name "abb2logical"
}
}
state {
id 343
labelString "eML_blk_kernel()"
position [18 64.5 118 66]
fontSize 12
chart 342
treeNode [342 0 0 0]
superState SUBCHART
subviewer 342
ssIdNumber 1
type FUNC_STATE
decomposition CLUSTER_STATE
eml {
isEML 1
script "function logical = abb2logical(abb)\n%#codegen\n\nlogical = [abb(1:2); abb(7); abb(3:6)];"
editorLayout "100 M4x1[10 5 700 500]"
}
}
junction {
id 344
position [23.5747 49.5747 7]
chart 342
linkNode [342 0 0]
subviewer 342
ssIdNumber 3
type CONNECTIVE_JUNCTION
}
transition {
id 345
labelString "{eML_blk_kernel();}"
labelPosition [32.125 19.875 102.544 14.964]
fontSize 12
src {
intersection [0 0 1 0 23.5747 14.625 0 0]
}
dst {
id 344
intersection [7 0 -1 -1 23.5747 42.5747 0 0]
}
midPoint [23.5747 24.9468]
chart 342
linkNode [342 0 0]
dataLimits [23.575 23.575 14.625 34.575]
subviewer 342
drawStyle SMART
executionOrder 1
ssIdNumber 2
}
data {
id 346
ssIdNumber 4
name "abb"
linkNode [342 0 347]
scope INPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
}
dataType "Inherit: Same as Simulink"
}
data {
id 347
ssIdNumber 5
name "logical"
linkNode [342 346 0]
scope OUTPUT_DATA
machine 1
props {
array {
size "-1"
}
type {
method SF_INHERITED_TYPE
primitive SF_DOUBLE_TYPE
}
complexity SF_COMPLEX_INHERITED
frame SF_FRAME_NO
}
dataType "Inherit: Same as Simulink"
}
instance {
id 348
name "botharms_get_torque/MATLAB Function9"
machine 1
chart 342
}
target {
id 349
name "sfun"
description "Default Simulink S-Function Target."
machine 1
linkNode [1 0 350]
}
target {
id 350
name "rtw"
machine 1
linkNode [1 349 0]
}
}