"
+ Name "DescTextVar"
+ }
+ Name "DescGroupVar"
+ }
+ Object {
+ $ObjectID 32
+ Prompt "Robot Parameters"
+ Array {
+ Type "Simulink.dialog.parameter.Edit"
+ Dimension 5
+ Object {
+ $ObjectID 33
+ Name "q_t0"
+ }
+ Object {
+ $ObjectID 34
+ Name "qD_t0"
+ }
+ Object {
+ $ObjectID 35
+ Name "g_base"
+ }
+ Object {
+ $ObjectID 36
+ Name "pkin"
+ }
+ Object {
+ $ObjectID 37
+ Name "DynPar1"
+ }
+ PropName "DialogControls"
+ }
+ Name "ParameterGroupVar"
+ }
+ PropName "DialogControls"
+ }
+ }
+ Port {
+ PortNumber 1
+ Name "q_meas"
+ PropagatedSignals "q"
+ }
+ Port {
+ PortNumber 2
+ Name "qd_meas"
+ PropagatedSignals "qD"
+ }
+ System {
+ Name "forearmbot_fordyn_fixb_vp1"
+ Location [47, 27, 1680, 1050]
+ Open off
+ PortBlocksUseCompactNotation off
+ SetExecutionDomain off
+ ExecutionDomainType "Deduce"
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ 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 "tau_ext\n"
+ SID "58"
+ Position [70, 233, 100, 247]
+ ZOrder 61
+ IconDisplay "Port number"
+ PortDimensions "NQJ"
+ }
+ Block {
+ BlockType Constant
+ Name "Constant"
+ SID "59"
+ Position [685, 340, 740, 370]
+ ZOrder 236
+ BlockMirror on
+ NamePlacement "alternate"
+ Value "g_base"
+ }
+ Block {
+ BlockType DiscreteIntegrator
+ Name "Discrete-Time\nIntegrator"
+ SID "93"
+ Ports [1, 1]
+ Position [335, 82, 370, 118]
+ ZOrder 237
+ InitialCondition "qD_t0"
+ InitialConditionSetting "Auto"
+ SampleTime "-1"
+ ICPrevOutput "DiscIntNeverNeededParam"
+ ICPrevScaledInput "DiscIntNeverNeededParam"
+ Port {
+ PortNumber 1
+ Name "qD"
+ }
+ }
+ Block {
+ BlockType DiscreteIntegrator
+ Name "Discrete-Time\nIntegrator1"
+ SID "94"
+ Ports [1, 1]
+ Position [450, 82, 485, 118]
+ ZOrder 238
+ InitialCondition "q_t0"
+ InitialConditionSetting "Auto"
+ SampleTime "-1"
+ ICPrevOutput "DiscIntNeverNeededParam"
+ ICPrevScaledInput "DiscIntNeverNeededParam"
+ Port {
+ PortNumber 1
+ Name "q"
+ }
+ }
+ Block {
+ BlockType Sum
+ Name "Sum3"
+ SID "62"
+ Ports [3, 1]
+ Position [180, 230, 200, 250]
+ ZOrder 46
+ BlockRotation 270
+ ShowName off
+ IconShape "round"
+ Inputs "+--"
+ InputSameDT off
+ OutDataTypeStr "Inherit: Inherit via internal rule"
+ SaturateOnIntegerOverflow off
+ Port {
+ PortNumber 1
+ Name "tau_acc"
+ }
+ }
+ Block {
+ BlockType SubSystem
+ Name "forearmbot_coriolisvec_fixb"
+ SID "63"
+ Ports [2, 1]
+ Position [305, 210, 370, 270]
+ ZOrder 234
+ BlockMirror on
+ NamePlacement "alternate"
+ RequestExecContextInheritance off
+ Object {
+ $PropName "MaskObject"
+ $ObjectID 38
+ $ClassName "Simulink.Mask"
+ Initialization "NQJ=2;\nm = DynPar1.m;\nr_S = DynPar1.r_S;\nI_S = DynPar1.I_S;"
+ Array {
+ Type "Simulink.MaskParameter"
+ Dimension 2
+ Object {
+ $ObjectID 39
+ Type "edit"
+ Name "pkin"
+ Prompt "Kinematik-Parameter"
+ Value "pkin"
+ }
+ Object {
+ $ObjectID 40
+ Type "edit"
+ Name "DynPar1"
+ Prompt "Dynamik-Parameter (Parametersatz 1)"
+ Value "DynPar1"
+ }
+ PropName "Parameters"
+ }
+ }
+ Port {
+ PortNumber 1
+ Name "tau_c"
+ PropagatedSignals "tauc"
+ }
+ System {
+ Name "forearmbot_coriolisvec_fixb"
+ Location [330, 79, 1543, 1076]
+ Open off
+ PortBlocksUseCompactNotation off
+ SetExecutionDomain off
+ ExecutionDomainType "Deduce"
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ 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 "qJ"
+ SID "77"
+ Position [20, 28, 50, 42]
+ ZOrder 11
+ IconDisplay "Port number"
+ PortDimensions "NQJ"
+ }
+ Block {
+ BlockType Inport
+ Name "qJD"
+ SID "78"
+ Position [20, 63, 50, 77]
+ ZOrder 12
+ Port "2"
+ IconDisplay "Port number"
+ PortDimensions "NQJ"
+ }
+ Block {
+ BlockType Constant
+ Name "Constant5"
+ SID "79"
+ Position [40, 377, 105, 393]
+ ZOrder 17
+ Value "I_S"
+ }
+ Block {
+ BlockType Constant
+ Name "Constant6"
+ SID "80"
+ Position [40, 152, 90, 168]
+ ZOrder 10
+ Value "pkin"
+ }
+ Block {
+ BlockType Constant
+ Name "Constant7"
+ SID "81"
+ Position [35, 242, 100, 258]
+ ZOrder 15
+ Value "m"
+ }
+ Block {
+ BlockType Constant
+ Name "Constant8"
+ SID "82"
+ Position [35, 317, 100, 333]
+ ZOrder 16
+ Value "r_S"
+ }
+ Block {
+ BlockType SubSystem
+ Name "coriolisvec_fcn"
+ SID "83"
+ Ports [6, 1]
+ Position [200, 17, 320, 418]
+ ZOrder 1
+ LibraryVersion "1.32"
+ ErrorFcn "Stateflow.Translate.translate"
+ PermitHierarchicalResolution "ExplicitOnly"
+ TreatAsAtomicUnit on
+ RequestExecContextInheritance off
+ SFBlockType "MATLAB Function"
+ System {
+ Name "coriolisvec_fcn"
+ Location [223, 338, 826, 833]
+ Open off
+ PortBlocksUseCompactNotation off
+ SetExecutionDomain off
+ ExecutionDomainType "Deduce"
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ 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 "80"
+ Block {
+ BlockType Inport
+ Name "q"
+ SID "83::1"
+ Position [20, 101, 40, 119]
+ ZOrder -1
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "qD"
+ SID "83::19"
+ Position [20, 136, 40, 154]
+ ZOrder 10
+ Port "2"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "pkin"
+ SID "83::20"
+ Position [20, 171, 40, 189]
+ ZOrder 11
+ Port "3"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "m"
+ SID "83::26"
+ Position [20, 206, 40, 224]
+ ZOrder 17
+ Port "4"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "r_S"
+ SID "83::72"
+ Position [20, 246, 40, 264]
+ ZOrder 24
+ Port "5"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "I_S"
+ SID "83::73"
+ Position [20, 281, 40, 299]
+ ZOrder 25
+ Port "6"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Demux
+ Name " Demux "
+ SID "83::79"
+ Ports [1, 1]
+ Position [270, 230, 320, 270]
+ ZOrder 30
+ Outputs "1"
+ }
+ Block {
+ BlockType S-Function
+ Name " SFunction "
+ SID "83::78"
+ Tag "Stateflow S-Function appint_ros_example 7"
+ Ports [6, 2]
+ Position [180, 102, 230, 243]
+ ZOrder 29
+ FunctionName "sf_sfun"
+ PortCounts "[6 2]"
+ SFunctionDeploymentMode off
+ EnableBusSupport on
+ SFcnIsStateOwnerBlock off
+ Port {
+ PortNumber 2
+ Name "tauc"
+ }
+ }
+ Block {
+ BlockType Terminator
+ Name " Terminator "
+ SID "83::80"
+ Position [460, 241, 480, 259]
+ ZOrder 31
+ }
+ Block {
+ BlockType Outport
+ Name "tauc"
+ SID "83::5"
+ Position [460, 101, 480, 119]
+ ZOrder -5
+ IconDisplay "Port number"
+ }
+ Line {
+ ZOrder 10
+ SrcBlock "q"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 1
+ }
+ Line {
+ ZOrder 11
+ SrcBlock "qD"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 2
+ }
+ Line {
+ ZOrder 12
+ SrcBlock "pkin"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 3
+ }
+ Line {
+ ZOrder 13
+ SrcBlock "m"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 4
+ }
+ Line {
+ ZOrder 14
+ SrcBlock "r_S"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 5
+ }
+ Line {
+ ZOrder 15
+ SrcBlock "I_S"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 6
+ }
+ Line {
+ Name "tauc"
+ ZOrder 16
+ Labels [0, 0]
+ SrcBlock " SFunction "
+ SrcPort 2
+ DstBlock "tauc"
+ DstPort 1
+ }
+ Line {
+ ZOrder 17
+ SrcBlock " Demux "
+ SrcPort 1
+ DstBlock " Terminator "
+ DstPort 1
+ }
+ Line {
+ ZOrder 18
+ SrcBlock " SFunction "
+ SrcPort 1
+ DstBlock " Demux "
+ DstPort 1
+ }
+ }
+ }
+ Block {
+ BlockType Outport
+ Name "tauJ_c"
+ SID "84"
+ Position [515, 195, 530, 225]
+ ZOrder 13
+ NamePlacement "alternate"
+ IconDisplay "Port number"
+ PortDimensions "NQJ"
+ }
+ Line {
+ ZOrder 1
+ SrcBlock "Constant5"
+ SrcPort 1
+ Points [75, 0]
+ DstBlock "coriolisvec_fcn"
+ DstPort 6
+ }
+ Line {
+ ZOrder 2
+ SrcBlock "Constant8"
+ SrcPort 1
+ Points [80, 0]
+ DstBlock "coriolisvec_fcn"
+ DstPort 5
+ }
+ Line {
+ ZOrder 3
+ SrcBlock "Constant7"
+ SrcPort 1
+ DstBlock "coriolisvec_fcn"
+ DstPort 4
+ }
+ Line {
+ ZOrder 4
+ SrcBlock "qJ"
+ SrcPort 1
+ Points [130, 0]
+ DstBlock "coriolisvec_fcn"
+ DstPort 1
+ }
+ Line {
+ ZOrder 5
+ SrcBlock "qJD"
+ SrcPort 1
+ Points [130, 0]
+ DstBlock "coriolisvec_fcn"
+ DstPort 2
+ }
+ Line {
+ ZOrder 6
+ SrcBlock "Constant6"
+ SrcPort 1
+ Points [90, 0]
+ DstBlock "coriolisvec_fcn"
+ DstPort 3
+ }
+ Line {
+ ZOrder 7
+ SrcBlock "coriolisvec_fcn"
+ SrcPort 1
+ Points [0, -10]
+ DstBlock "tauJ_c"
+ DstPort 1
+ }
+ }
+ }
+ Block {
+ BlockType SubSystem
+ Name "forearmbot_gravload"
+ SID "64"
+ Ports [2, 1]
+ Position [315, 310, 355, 370]
+ ZOrder 235
+ BlockMirror on
+ NamePlacement "alternate"
+ RequestExecContextInheritance off
+ Object {
+ $PropName "MaskObject"
+ $ObjectID 41
+ $ClassName "Simulink.Mask"
+ Initialization "NQJ=2;\nm = DynPar1.m;\nr_S = DynPar1.r_S;\nI_S = DynPar1.I_S;\n"
+ Array {
+ Type "Simulink.MaskParameter"
+ Dimension 2
+ Object {
+ $ObjectID 42
+ Type "edit"
+ Name "pkin"
+ Prompt "Kinematik-Parameter"
+ Value "pkin"
+ }
+ Object {
+ $ObjectID 43
+ Type "edit"
+ Name "DynPar1"
+ Prompt "Dynamik-Parameter (Parametersatz 1)"
+ Value "DynPar1"
+ }
+ PropName "Parameters"
+ }
+ }
+ Port {
+ PortNumber 1
+ Name "tau_g"
+ PropagatedSignals "taug"
+ }
+ System {
+ Name "forearmbot_gravload"
+ Location [330, 79, 1543, 1076]
+ Open off
+ PortBlocksUseCompactNotation off
+ SetExecutionDomain off
+ ExecutionDomainType "Deduce"
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ 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 "qJ"
+ SID "85"
+ Position [45, 58, 75, 72]
+ ZOrder 11
+ IconDisplay "Port number"
+ PortDimensions "NQJ"
+ }
+ Block {
+ BlockType Inport
+ Name "g_base"
+ SID "86"
+ Position [45, 128, 75, 142]
+ ZOrder 12
+ Port "2"
+ IconDisplay "Port number"
+ PortDimensions "3"
+ }
+ Block {
+ BlockType Constant
+ Name "Constant5"
+ SID "87"
+ Position [25, 267, 90, 283]
+ ZOrder 7
+ Value "m"
+ }
+ Block {
+ BlockType Constant
+ Name "Constant6"
+ SID "88"
+ Position [35, 197, 85, 213]
+ ZOrder 10
+ Value "pkin"
+ }
+ Block {
+ BlockType Constant
+ Name "Constant7"
+ SID "89"
+ Position [25, 337, 90, 353]
+ ZOrder 14
+ Value "r_S"
+ }
+ Block {
+ BlockType SubSystem
+ Name "MATLAB Function"
+ SID "90"
+ Ports [5, 1]
+ Position [190, 31, 300, 379]
+ ZOrder 1
+ LibraryVersion "1.32"
+ ErrorFcn "Stateflow.Translate.translate"
+ PermitHierarchicalResolution "ExplicitOnly"
+ TreatAsAtomicUnit on
+ RequestExecContextInheritance off
+ SFBlockType "MATLAB Function"
+ System {
+ Name "MATLAB Function"
+ Location [223, 338, 826, 833]
+ Open off
+ PortBlocksUseCompactNotation off
+ SetExecutionDomain off
+ ExecutionDomainType "Deduce"
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ 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 "63"
+ Block {
+ BlockType Inport
+ Name "q"
+ SID "90::1"
+ Position [20, 101, 40, 119]
+ ZOrder -1
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "g_base"
+ SID "90::19"
+ Position [20, 136, 40, 154]
+ ZOrder 10
+ Port "2"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "pkin"
+ SID "90::20"
+ Position [20, 171, 40, 189]
+ ZOrder 11
+ Port "3"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "m"
+ SID "90::26"
+ Position [20, 206, 40, 224]
+ ZOrder 17
+ Port "4"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "r_S"
+ SID "90::56"
+ Position [20, 246, 40, 264]
+ ZOrder 21
+ Port "5"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Demux
+ Name " Demux "
+ SID "90::62"
+ Ports [1, 1]
+ Position [270, 230, 320, 270]
+ ZOrder 26
+ Outputs "1"
+ }
+ Block {
+ BlockType S-Function
+ Name " SFunction "
+ SID "90::61"
+ Tag "Stateflow S-Function appint_ros_example 6"
+ Ports [5, 2]
+ Position [180, 100, 230, 220]
+ ZOrder 25
+ FunctionName "sf_sfun"
+ PortCounts "[5 2]"
+ SFunctionDeploymentMode off
+ EnableBusSupport on
+ SFcnIsStateOwnerBlock off
+ Port {
+ PortNumber 2
+ Name "taug"
+ }
+ }
+ Block {
+ BlockType Terminator
+ Name " Terminator "
+ SID "90::63"
+ Position [460, 241, 480, 259]
+ ZOrder 27
+ }
+ Block {
+ BlockType Outport
+ Name "taug"
+ SID "90::5"
+ Position [460, 101, 480, 119]
+ ZOrder -5
+ IconDisplay "Port number"
+ }
+ Line {
+ ZOrder 9
+ SrcBlock "q"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 1
+ }
+ Line {
+ ZOrder 10
+ SrcBlock "g_base"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 2
+ }
+ Line {
+ ZOrder 11
+ SrcBlock "pkin"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 3
+ }
+ Line {
+ ZOrder 12
+ SrcBlock "m"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 4
+ }
+ Line {
+ ZOrder 13
+ SrcBlock "r_S"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 5
+ }
+ Line {
+ Name "taug"
+ ZOrder 14
+ Labels [0, 0]
+ SrcBlock " SFunction "
+ SrcPort 2
+ DstBlock "taug"
+ DstPort 1
+ }
+ Line {
+ ZOrder 15
+ SrcBlock " Demux "
+ SrcPort 1
+ DstBlock " Terminator "
+ DstPort 1
+ }
+ Line {
+ ZOrder 16
+ SrcBlock " SFunction "
+ SrcPort 1
+ DstBlock " Demux "
+ DstPort 1
+ }
+ }
+ }
+ Block {
+ BlockType Outport
+ Name "tau_g"
+ SID "91"
+ Position [495, 180, 510, 210]
+ ZOrder 13
+ NamePlacement "alternate"
+ IconDisplay "Port number"
+ PortDimensions "NQJ"
+ }
+ Line {
+ ZOrder 1
+ SrcBlock "Constant7"
+ SrcPort 1
+ DstBlock "MATLAB Function"
+ DstPort 5
+ }
+ Line {
+ ZOrder 2
+ SrcBlock "Constant5"
+ SrcPort 1
+ DstBlock "MATLAB Function"
+ DstPort 4
+ }
+ Line {
+ ZOrder 3
+ SrcBlock "qJ"
+ SrcPort 1
+ DstBlock "MATLAB Function"
+ DstPort 1
+ }
+ Line {
+ ZOrder 4
+ SrcBlock "g_base"
+ SrcPort 1
+ DstBlock "MATLAB Function"
+ DstPort 2
+ }
+ Line {
+ ZOrder 5
+ SrcBlock "Constant6"
+ SrcPort 1
+ DstBlock "MATLAB Function"
+ DstPort 3
+ }
+ Line {
+ ZOrder 6
+ SrcBlock "MATLAB Function"
+ SrcPort 1
+ Points [0, -10]
+ DstBlock "tau_g"
+ DstPort 1
+ }
+ }
+ }
+ Block {
+ BlockType SubSystem
+ Name "forearmbot_inertiaJ"
+ SID "65"
+ Ports [1, 1]
+ Position [95, 67, 155, 103]
+ ZOrder 233
+ RequestExecContextInheritance off
+ Object {
+ $PropName "MaskObject"
+ $ObjectID 44
+ $ClassName "Simulink.Mask"
+ Initialization "NQJ=2;\nm = DynPar1.m;\nr_S = DynPar1.r_S;\nI_S = DynPar1.I_S;"
+ Array {
+ Type "Simulink.MaskParameter"
+ Dimension 2
+ Object {
+ $ObjectID 45
+ Type "edit"
+ Name "pkin"
+ Prompt "Kinematik-Parameter"
+ Value "pkin"
+ }
+ Object {
+ $ObjectID 46
+ Type "edit"
+ Name "DynPar1"
+ Prompt "Dynamik-Parameter (Parametersatz 1)"
+ Value "DynPar1"
+ }
+ PropName "Parameters"
+ }
+ }
+ System {
+ Name "forearmbot_inertiaJ"
+ Location [330, 79, 1543, 1076]
+ Open off
+ PortBlocksUseCompactNotation off
+ SetExecutionDomain off
+ ExecutionDomainType "Deduce"
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ 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 "qJ"
+ SID "70"
+ Position [50, 83, 80, 97]
+ ZOrder 11
+ IconDisplay "Port number"
+ PortDimensions "NQJ"
+ }
+ Block {
+ BlockType Constant
+ Name "Constant6"
+ SID "71"
+ Position [40, 172, 90, 188]
+ ZOrder 10
+ Value "pkin"
+ }
+ Block {
+ BlockType Constant
+ Name "Constant7"
+ SID "72"
+ Position [35, 262, 100, 278]
+ ZOrder 15
+ Value "m"
+ }
+ Block {
+ BlockType Constant
+ Name "Constant8"
+ SID "73"
+ Position [35, 352, 100, 368]
+ ZOrder 16
+ Value "r_S"
+ }
+ Block {
+ BlockType Constant
+ Name "Constant9"
+ SID "74"
+ Position [40, 442, 105, 458]
+ ZOrder 18
+ Value "I_S"
+ }
+ Block {
+ BlockType SubSystem
+ Name "inertiajoint_fcn"
+ SID "75"
+ Ports [5, 1]
+ Position [235, 47, 380, 493]
+ ZOrder 1
+ LibraryVersion "1.32"
+ ErrorFcn "Stateflow.Translate.translate"
+ PermitHierarchicalResolution "ExplicitOnly"
+ TreatAsAtomicUnit on
+ RequestExecContextInheritance off
+ SFBlockType "MATLAB Function"
+ System {
+ Name "inertiajoint_fcn"
+ Location [223, 338, 826, 833]
+ Open off
+ PortBlocksUseCompactNotation off
+ SetExecutionDomain off
+ ExecutionDomainType "Deduce"
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ 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 "q"
+ SID "75::1"
+ Position [20, 101, 40, 119]
+ ZOrder -1
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "pkin"
+ SID "75::20"
+ Position [20, 136, 40, 154]
+ ZOrder 11
+ Port "2"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "m"
+ SID "75::26"
+ Position [20, 171, 40, 189]
+ ZOrder 17
+ Port "3"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "r_S"
+ SID "75::60"
+ Position [20, 206, 40, 224]
+ ZOrder 27
+ Port "4"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "I_S"
+ SID "75::61"
+ Position [20, 246, 40, 264]
+ ZOrder 28
+ Port "5"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Demux
+ Name " Demux "
+ SID "75::67"
+ Ports [1, 1]
+ Position [270, 230, 320, 270]
+ ZOrder 33
+ Outputs "1"
+ }
+ Block {
+ BlockType S-Function
+ Name " SFunction "
+ SID "75::66"
+ Tag "Stateflow S-Function appint_ros_example 8"
+ Ports [5, 2]
+ Position [180, 100, 230, 220]
+ ZOrder 32
+ FunctionName "sf_sfun"
+ PortCounts "[5 2]"
+ SFunctionDeploymentMode off
+ EnableBusSupport on
+ SFcnIsStateOwnerBlock off
+ Port {
+ PortNumber 2
+ Name "Mq"
+ }
+ }
+ Block {
+ BlockType Terminator
+ Name " Terminator "
+ SID "75::68"
+ Position [460, 241, 480, 259]
+ ZOrder 34
+ }
+ Block {
+ BlockType Outport
+ Name "Mq"
+ SID "75::5"
+ Position [460, 101, 480, 119]
+ ZOrder -5
+ IconDisplay "Port number"
+ }
+ Line {
+ ZOrder 9
+ SrcBlock "q"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 1
+ }
+ Line {
+ ZOrder 10
+ SrcBlock "pkin"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 2
+ }
+ Line {
+ ZOrder 11
+ SrcBlock "m"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 3
+ }
+ Line {
+ ZOrder 12
+ SrcBlock "r_S"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 4
+ }
+ Line {
+ ZOrder 13
+ SrcBlock "I_S"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 5
+ }
+ Line {
+ Name "Mq"
+ ZOrder 14
+ Labels [0, 0]
+ SrcBlock " SFunction "
+ SrcPort 2
+ DstBlock "Mq"
+ DstPort 1
+ }
+ Line {
+ ZOrder 15
+ SrcBlock " Demux "
+ SrcPort 1
+ DstBlock " Terminator "
+ DstPort 1
+ }
+ Line {
+ ZOrder 16
+ SrcBlock " SFunction "
+ SrcPort 1
+ DstBlock " Demux "
+ DstPort 1
+ }
+ }
+ }
+ Block {
+ BlockType Outport
+ Name "M_JJ"
+ SID "76"
+ Position [505, 240, 520, 270]
+ ZOrder 13
+ NamePlacement "alternate"
+ IconDisplay "Port number"
+ PortDimensions "[NQJ NQJ]"
+ }
+ Line {
+ ZOrder 1
+ SrcBlock "Constant9"
+ SrcPort 1
+ DstBlock "inertiajoint_fcn"
+ DstPort 5
+ }
+ Line {
+ ZOrder 2
+ SrcBlock "Constant8"
+ SrcPort 1
+ DstBlock "inertiajoint_fcn"
+ DstPort 4
+ }
+ Line {
+ ZOrder 3
+ SrcBlock "Constant7"
+ SrcPort 1
+ DstBlock "inertiajoint_fcn"
+ DstPort 3
+ }
+ Line {
+ ZOrder 4
+ SrcBlock "qJ"
+ SrcPort 1
+ DstBlock "inertiajoint_fcn"
+ DstPort 1
+ }
+ Line {
+ ZOrder 5
+ SrcBlock "Constant6"
+ SrcPort 1
+ DstBlock "inertiajoint_fcn"
+ DstPort 2
+ }
+ Line {
+ ZOrder 6
+ SrcBlock "inertiajoint_fcn"
+ SrcPort 1
+ Points [0, -15]
+ DstBlock "M_JJ"
+ DstPort 1
+ }
+ }
+ }
+ Block {
+ BlockType SubSystem
+ Name "mFcn_Minv"
+ SID "66"
+ Ports [2, 1]
+ Position [205, 71, 290, 124]
+ ZOrder 144
+ LibraryVersion "1.190"
+ ErrorFcn "Stateflow.Translate.translate"
+ PermitHierarchicalResolution "ExplicitOnly"
+ TreatAsAtomicUnit on
+ RequestExecContextInheritance off
+ SFBlockType "MATLAB Function"
+ Port {
+ PortNumber 1
+ Name "qDD"
+ PropagatedSignals "q_ddot"
+ }
+ System {
+ Name "mFcn_Minv"
+ Location [223, 338, 826, 833]
+ Open off
+ PortBlocksUseCompactNotation off
+ SetExecutionDomain off
+ ExecutionDomainType "Deduce"
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ ScreenColor "white"
+ PaperOrientation "landscape"
+ PaperPositionMode "auto"
+ PaperType "usletter"
+ PaperUnits "inches"
+ TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
+ TiledPageScale 1
+ ShowPageBoundaries off
+ ZoomFactor "100"
+ SIDHighWatermark "96"
+ Block {
+ BlockType Inport
+ Name "M"
+ SID "66::1"
+ Position [20, 101, 40, 119]
+ ZOrder -1
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "tau"
+ SID "66::21"
+ Position [20, 136, 40, 154]
+ ZOrder 7
+ Port "2"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Demux
+ Name " Demux "
+ SID "66::95"
+ Ports [1, 1]
+ Position [270, 230, 320, 270]
+ ZOrder 33
+ Outputs "1"
+ }
+ Block {
+ BlockType S-Function
+ Name " SFunction "
+ SID "66::94"
+ Tag "Stateflow S-Function appint_ros_example 11"
+ Ports [2, 2]
+ Position [180, 100, 230, 160]
+ ZOrder 32
+ FunctionName "sf_sfun"
+ PortCounts "[2 2]"
+ SFunctionDeploymentMode off
+ EnableBusSupport on
+ SFcnIsStateOwnerBlock off
+ Port {
+ PortNumber 2
+ Name "q_ddot"
+ }
+ }
+ Block {
+ BlockType Terminator
+ Name " Terminator "
+ SID "66::96"
+ Position [460, 241, 480, 259]
+ ZOrder 34
+ }
+ Block {
+ BlockType Outport
+ Name "q_ddot"
+ SID "66::5"
+ Position [460, 101, 480, 119]
+ ZOrder -5
+ IconDisplay "Port number"
+ }
+ Line {
+ ZOrder 6
+ SrcBlock "M"
+ SrcPort 1
+ Points [120, 0]
+ DstBlock " SFunction "
+ DstPort 1
+ }
+ Line {
+ ZOrder 7
+ SrcBlock "tau"
+ SrcPort 1
+ DstBlock " SFunction "
+ DstPort 2
+ }
+ Line {
+ Name "q_ddot"
+ ZOrder 8
+ Labels [0, 0]
+ SrcBlock " SFunction "
+ SrcPort 2
+ DstBlock "q_ddot"
+ DstPort 1
+ }
+ Line {
+ ZOrder 9
+ SrcBlock " Demux "
+ SrcPort 1
+ DstBlock " Terminator "
+ DstPort 1
+ }
+ Line {
+ ZOrder 10
+ SrcBlock " SFunction "
+ SrcPort 1
+ DstBlock " Demux "
+ DstPort 1
+ }
+ }
+ }
+ Block {
+ BlockType Outport
+ Name "qJ"
+ SID "67"
+ Position [695, 93, 725, 107]
+ ZOrder 63
+ IconDisplay "Port number"
+ PortDimensions "NQJ"
+ }
+ Block {
+ BlockType Outport
+ Name "qJD"
+ SID "68"
+ Position [695, 148, 725, 162]
+ ZOrder 64
+ Port "2"
+ IconDisplay "Port number"
+ PortDimensions "NQJ"
+ }
+ Block {
+ BlockType Outport
+ Name "qJDD"
+ SID "69"
+ Position [695, 48, 725, 62]
+ ZOrder 65
+ Port "3"
+ IconDisplay "Port number"
+ PortDimensions "NQJ"
+ }
+ Line {
+ Name "tau_g"
+ ZOrder 1
+ Labels [0, 0]
+ SrcBlock "forearmbot_gravload"
+ SrcPort 1
+ Points [-120, 0]
+ DstBlock "Sum3"
+ DstPort 2
+ }
+ Line {
+ Name "q"
+ ZOrder 24
+ Labels [-1, 0]
+ SrcBlock "Discrete-Time\nIntegrator1"
+ SrcPort 1
+ Points [55, 0]
+ Branch {
+ ZOrder 3
+ Labels [1, 1]
+ Points [0, 125]
+ DstBlock "forearmbot_coriolisvec_fixb"
+ DstPort 1
+ }
+ Branch {
+ ZOrder 4
+ Labels [2, 1]
+ Points [0, -64; -465, 0]
+ DstBlock "forearmbot_inertiaJ"
+ DstPort 1
+ }
+ Branch {
+ ZOrder 5
+ Labels [1, 1]
+ Points [67, 0]
+ Branch {
+ ZOrder 6
+ Labels [1, 1]
+ Points [0, 225]
+ DstBlock "forearmbot_gravload"
+ DstPort 1
+ }
+ Branch {
+ ZOrder 7
+ Labels [1, 1]
+ DstBlock "qJ"
+ DstPort 1
+ }
+ }
+ }
+ Line {
+ Name "qDD"
+ ZOrder 8
+ Labels [0, 0]
+ SrcBlock "mFcn_Minv"
+ SrcPort 1
+ Points [18, 0]
+ Branch {
+ ZOrder 21
+ DstBlock "Discrete-Time\nIntegrator"
+ DstPort 1
+ }
+ Branch {
+ ZOrder 9
+ Points [0, -45]
+ DstBlock "qJDD"
+ DstPort 1
+ }
+ }
+ Line {
+ Name "tau_c"
+ ZOrder 11
+ Labels [-1, 0]
+ SrcBlock "forearmbot_coriolisvec_fixb"
+ SrcPort 1
+ DstBlock "Sum3"
+ DstPort 3
+ }
+ Line {
+ ZOrder 12
+ SrcBlock "tau_ext\n"
+ SrcPort 1
+ DstBlock "Sum3"
+ DstPort 1
+ }
+ Line {
+ Name "qD"
+ ZOrder 22
+ Labels [1, 1]
+ SrcBlock "Discrete-Time\nIntegrator"
+ SrcPort 1
+ Points [27, 0]
+ Branch {
+ ZOrder 23
+ Labels [1, 1]
+ DstBlock "Discrete-Time\nIntegrator1"
+ DstPort 1
+ }
+ Branch {
+ ZOrder 14
+ Points [0, 55; 15, 0]
+ Branch {
+ ZOrder 15
+ Labels [1, 0; -1, 0]
+ Points [0, 100]
+ DstBlock "forearmbot_coriolisvec_fixb"
+ DstPort 2
+ }
+ Branch {
+ ZOrder 16
+ DstBlock "qJD"
+ DstPort 1
+ }
+ }
+ }
+ Line {
+ Name "tau_acc"
+ ZOrder 18
+ Labels [0, 0]
+ SrcBlock "Sum3"
+ SrcPort 1
+ DstBlock "mFcn_Minv"
+ DstPort 2
+ }
+ Line {
+ ZOrder 19
+ SrcBlock "forearmbot_inertiaJ"
+ SrcPort 1
+ DstBlock "mFcn_Minv"
+ DstPort 1
+ }
+ Line {
+ ZOrder 20
+ SrcBlock "Constant"
+ SrcPort 1
+ DstBlock "forearmbot_gravload"
+ DstPort 2
+ }
+ }
+ }
+ Block {
+ BlockType S-Function
+ Name "ros_rt_interface_pcu"
+ SID "116"
+ Ports [1, 1]
+ Position [80, 140, 365, 170]
+ ZOrder 22534
+ FunctionName "ros_rt_interface_pcu"
+ SFunctionDeploymentMode off
+ EnableBusSupport off
+ SFcnIsStateOwnerBlock off
+ Object {
+ $PropName "MaskObject"
+ $ObjectID 47
+ $ClassName "Simulink.Mask"
+ Type "Legacy Function"
+ Description "This block allows you to call the legacy functions specified by the following Legacy Code Tool definiti"
+ "on:\ndef.SFunctionName = 'ros_rt_interface_pcu';\ndef.OutputFcnSpec = 'void SL_io_func(SL_OUT_type u1[1], SL_IN_type "
+ "y1[1])';\ndef.StartFcnSpec = 'void SL_start_func()';\ndef.TerminateFcnSpec = 'void SL_terminate_func()';\ndef.HeaderF"
+ "iles = {'SL_func.h'};\ndef.SourceFiles = {'../ros_rt_interface/ros_rt_core/SL_func_dummy.cpp'};\ndef.TargetLibFiles ="
+ " {'libros_sl_interface.so'};\ndef.IncPaths = {'../ros_rt_interface/ros_rt_core'};\n"
+ Help "This block was generated automatically by the Legacy Code Tool. For detailed help on this tool, call legacy_code('help').
"
+ SelfModifiable "on"
+ Display "fprintf('void SL_io_func(SL_OUT_type u1[1], SL_IN_type y1[1])');"
+ IconRotate "port"
+ IconUnits "normalized"
+ Array {
+ Type "Simulink.MaskParameter"
+ Dimension 2
+ Object {
+ $ObjectID 48
+ Type "edit"
+ Name "SFunctionSpec"
+ Prompt "SFunctionSpec"
+ Value "void SL_io_func(SL_OUT_type u1[1], SL_IN_type y1[1])"
+ Evaluate "off"
+ Tunable "off"
+ Visible "off"
+ }
+ Object {
+ $ObjectID 49
+ Type "checkbox"
+ Name "ShowSpec"
+ Prompt "Display function specification"
+ Value "on"
+ Callback "if strcmp(get_param(gcbh, 'ShowSpec'), 'on')\n set_param(gcbh, 'MaskDisplay', sprintf('fprintf(''"
+ "%s'');', get_param(gcbh, 'SFunctionSpec')));\nelse\n set_param(gcbh, 'MaskDisplay', sprintf('fprintf(''%s'');', "
+ "get_param(gcbh, 'FunctionName')));\nend"
+ }
+ PropName "Parameters"
+ }
+ }
+ }
+ Block {
+ BlockType SubSystem
+ Name "state_manager_dummy"
+ SID "100"
+ Ports [1, 1]
+ Position [310, 739, 485, 811]
+ ZOrder 22531
+ BlockMirror on
+ NamePlacement "alternate"
+ RequestExecContextInheritance off
+ ContentPreviewEnabled on
+ Port {
+ PortNumber 1
+ Name "sl_state"
+ }
+ System {
+ Name "state_manager_dummy"
+ Location [1680, 0, 2704, 768]
+ Open off
+ PortBlocksUseCompactNotation off
+ SetExecutionDomain off
+ ExecutionDomainType "Deduce"
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ ScreenColor "white"
+ PaperOrientation "landscape"
+ PaperPositionMode "auto"
+ PaperType "usletter"
+ PaperUnits "inches"
+ TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
+ TiledPageScale 1
+ ShowPageBoundaries off
+ ZoomFactor "150"
+ Block {
+ BlockType Inport
+ Name "state_request"
+ SID "101"
+ Position [-50, 658, -20, 672]
+ ZOrder 22531
+ IconDisplay "Port number"
+ OutDataTypeStr "uint8"
+ Port {
+ PortNumber 1
+ PropagatedSignals "state"
+ ShowPropagatedSignals "on"
+ }
+ }
+ Block {
+ BlockType Constant
+ Name "Constant"
+ SID "99"
+ Position [-80, 820, -50, 850]
+ ZOrder 22530
+ Value "42"
+ OutDataTypeStr "uint8"
+ }
+ Block {
+ BlockType Constant
+ Name "Constant1"
+ SID "109"
+ Position [90, 850, 120, 880]
+ ZOrder 22539
+ Value "5"
+ OutDataTypeStr "uint8"
+ }
+ Block {
+ BlockType RelationalOperator
+ Name "Relational\nOperator1"
+ SID "98"
+ Ports [2, 1]
+ Position [80, 757, 110, 788]
+ ZOrder 22529
+ Operator "=="
+ InputSameDT off
+ OutDataTypeStr "boolean"
+ RndMeth "Simplest"
+ }
+ Block {
+ BlockType Switch
+ Name "Switch2"
+ SID "97"
+ Position [255, 755, 305, 795]
+ ZOrder 22528
+ NamePlacement "alternate"
+ Criteria "u2 > Threshold"
+ InputSameDT off
+ SaturateOnIntegerOverflow off
+ }
+ Block {
+ BlockType Outport
+ Name "state"
+ SID "106"
+ Position [400, 768, 430, 782]
+ ZOrder 22536
+ IconDisplay "Port number"
+ OutDataTypeStr "uint8"
+ }
+ Line {
+ ZOrder 173
+ SrcBlock "Constant1"
+ SrcPort 1
+ Points [63, 0; 0, -75]
+ DstBlock "Switch2"
+ DstPort 3
+ }
+ Line {
+ ZOrder 170
+ SrcBlock "state_request"
+ SrcPort 1
+ Points [51, 0; 0, 27]
+ Branch {
+ ZOrder 175
+ Points [147, 0; 0, 68]
+ DstBlock "Switch2"
+ DstPort 1
+ }
+ Branch {
+ ZOrder 174
+ Points [0, 73]
+ DstBlock "Relational\nOperator1"
+ DstPort 1
+ }
+ }
+ Line {
+ ZOrder 167
+ SrcBlock "Switch2"
+ SrcPort 1
+ DstBlock "state"
+ DstPort 1
+ }
+ Line {
+ ZOrder 171
+ SrcBlock "Constant"
+ SrcPort 1
+ Points [79, 0; 0, -55]
+ DstBlock "Relational\nOperator1"
+ DstPort 2
+ }
+ Line {
+ ZOrder 172
+ SrcBlock "Relational\nOperator1"
+ SrcPort 1
+ DstBlock "Switch2"
+ DstPort 2
+ }
+ Annotation {
+ SID "115"
+ Name "Dieser Block ist aktuell nur ein Dummy-Block. Hier könnten bspw verschiedene Regler-Modi umgeschaltet we"
+ "rden\n(z.B. zwischen verschiedenen Reglern wie GravKomp, PD, ... umschalten). Dafür sollte man eine gewisse Logik "
+ "einbauen."
+ Position [-37, 598, 505, 626]
+ InternalMargins [0, 0, 0, 0]
+ ZOrder -1
+ }
+ }
+ }
+ Line {
+ ZOrder 191
+ SrcBlock "Delay"
+ SrcPort 1
+ DstBlock "ros_rt_interface_pcu"
+ DstPort 1
+ }
+ Line {
+ ZOrder 12
+ SrcBlock "Bus_Creator"
+ SrcPort 1
+ DstBlock "Delay"
+ DstPort 1
+ }
+ Line {
+ ZOrder 192
+ SrcBlock "ros_rt_interface_pcu"
+ SrcPort 1
+ DstBlock "Bus\nSelector1"
+ DstPort 1
+ }
+ Line {
+ ZOrder 119
+ SrcBlock "PD_Regler"
+ SrcPort 1
+ Points [-88, 0; 0, 25; -2, 0]
+ Branch {
+ ZOrder 225
+ Points [0, 165; -305, 0; 0, -100]
+ DstBlock "SL_OUT"
+ DstPort 4
+ }
+ Branch {
+ ZOrder 224
+ DstBlock "forearmbot_fordyn_fixb_vp1"
+ DstPort 1
+ }
+ }
+ Line {
+ Name "q_meas"
+ ZOrder 120
+ SrcBlock "forearmbot_fordyn_fixb_vp1"
+ SrcPort 1
+ Points [-52, 0]
+ Branch {
+ ZOrder 140
+ Points [0, 105]
+ DstBlock "Delay1"
+ DstPort 1
+ }
+ Branch {
+ ZOrder 139
+ Points [-145, 0]
+ Branch {
+ ZOrder 218
+ DstBlock "SL_OUT"
+ DstPort 1
+ }
+ Branch {
+ ZOrder 217
+ Labels [2, 1]
+ Points [0, -340]
+ DstBlock "Bus_Creator"
+ DstPort 2
+ }
+ }
+ }
+ Line {
+ Name "qd_meas"
+ ZOrder 121
+ SrcBlock "forearmbot_fordyn_fixb_vp1"
+ SrcPort 2
+ Points [-68, 0]
+ Branch {
+ ZOrder 143
+ Points [0, 125]
+ DstBlock "Delay2"
+ DstPort 1
+ }
+ Branch {
+ ZOrder 142
+ Points [-74, 0]
+ Branch {
+ ZOrder 222
+ Labels [2, 1]
+ Points [0, -320]
+ DstBlock "Bus_Creator"
+ DstPort 3
+ }
+ Branch {
+ ZOrder 221
+ Points [0, 5]
+ DstBlock "SL_OUT"
+ DstPort 2
+ }
+ }
+ }
+ Line {
+ ZOrder 141
+ SrcBlock "Delay1"
+ SrcPort 1
+ Points [315, 0; 0, -170]
+ DstBlock "PD_Regler"
+ DstPort 1
+ }
+ Line {
+ ZOrder 144
+ SrcBlock "Delay2"
+ SrcPort 1
+ Points [329, 0; 0, -165]
+ DstBlock "PD_Regler"
+ DstPort 3
+ }
+ Line {
+ Name ""
+ ZOrder 145
+ Labels [0, 0]
+ SrcBlock "Bus\nSelector1"
+ SrcPort 4
+ Points [153, 0]
+ Branch {
+ ZOrder 194
+ Points [0, -45]
+ DstBlock "SL_IN_Scope"
+ DstPort 3
+ }
+ Branch {
+ ZOrder 171
+ Points [0, 255]
+ DstBlock "PD_Regler"
+ DstPort 2
+ }
+ }
+ Line {
+ Name ""
+ ZOrder 146
+ Labels [0, 0]
+ SrcBlock "Bus\nSelector1"
+ SrcPort 5
+ Points [139, 0]
+ Branch {
+ ZOrder 196
+ Points [0, -60]
+ DstBlock "SL_IN_Scope"
+ DstPort 4
+ }
+ Branch {
+ ZOrder 173
+ Points [0, 260]
+ DstBlock "PD_Regler"
+ DstPort 4
+ }
+ }
+ Line {
+ Name ""
+ ZOrder 147
+ Labels [0, 0]
+ SrcBlock "Bus\nSelector1"
+ SrcPort 2
+ Points [120, 0]
+ Branch {
+ ZOrder 193
+ Points [0, -15]
+ DstBlock "SL_IN_Scope"
+ DstPort 1
+ }
+ Branch {
+ ZOrder 168
+ Points [0, 445]
+ DstBlock "PD_Regler"
+ DstPort 6
+ }
+ }
+ Line {
+ Name ""
+ ZOrder 148
+ Labels [0, 0]
+ SrcBlock "Bus\nSelector1"
+ SrcPort 3
+ Points [106, 0]
+ Branch {
+ ZOrder 195
+ Points [0, -30]
+ DstBlock "SL_IN_Scope"
+ DstPort 2
+ }
+ Branch {
+ ZOrder 169
+ Points [0, 375]
+ DstBlock "PD_Regler"
+ DstPort 5
+ }
+ }
+ Line {
+ Name ""
+ ZOrder 162
+ Labels [0, 0]
+ SrcBlock "Bus\nSelector1"
+ SrcPort 1
+ Points [48, 0; 0, 710]
+ DstBlock "state_manager_dummy"
+ DstPort 1
+ }
+ Line {
+ Name "sl_state"
+ ZOrder 163
+ Labels [3, 1]
+ SrcBlock "state_manager_dummy"
+ SrcPort 1
+ Points [-411, 0; 0, -665]
+ DstBlock "Bus_Creator"
+ DstPort 1
+ }
+ Line {
+ ZOrder 223
+ SrcBlock "forearmbot_fordyn_fixb_vp1"
+ SrcPort 3
+ Points [-23, 0; 0, 10]
+ DstBlock "SL_OUT"
+ DstPort 3
+ }
+ Annotation {
+ SID "112"
+ Name "Regler"
+ Position [320, 371, 455, 586]
+ InternalMargins [0, 0, 0, 0]
+ FixedHeight on
+ FixedWidth on
+ ForegroundColor "[0.901961, 0.901961, 1.000000]"
+ BackgroundColor "[0.901961, 0.901961, 1.000000]"
+ DropShadow on
+ AnnotationType "area_annotation"
+ ZOrder 257
+ FontSize 12
+ }
+ Annotation {
+ SID "4"
+ Name "Interface ROS - Simulink"
+ Position [71, 120, 376, 200]
+ InternalMargins [0, 0, 0, 0]
+ FixedHeight on
+ FixedWidth on
+ ForegroundColor "[0.901961, 0.901961, 1.000000]"
+ BackgroundColor "[0.901961, 0.901961, 1.000000]"
+ DropShadow on
+ AnnotationType "area_annotation"
+ ZOrder 256
+ FontSize 12
+ }
+ Annotation {
+ SID "113"
+ Name "Strecke (Simulation)"
+ Position [35, 434, 260, 574]
+ InternalMargins [0, 0, 0, 0]
+ FixedHeight on
+ FixedWidth on
+ ForegroundColor "[0.901961, 0.901961, 1.000000]"
+ BackgroundColor "[0.901961, 0.901961, 1.000000]"
+ DropShadow on
+ AnnotationType "area_annotation"
+ ZOrder 258
+ FontSize 12
+ }
+ Annotation {
+ SID "114"
+ Name "Regler Zustandsmaschine"
+ Position [255, 704, 520, 824]
+ InternalMargins [0, 0, 0, 0]
+ FixedHeight on
+ FixedWidth on
+ ForegroundColor "[0.901961, 0.901961, 1.000000]"
+ BackgroundColor "[0.901961, 0.901961, 1.000000]"
+ DropShadow on
+ AnnotationType "area_annotation"
+ ZOrder 259
+ FontSize 12
+ }
+ }
+}
+#Finite State Machines
+#
+# Stateflow 80000014
+#
+#
+Stateflow {
+ machine {
+ id 1
+ name "appint_ros_example"
+ created "27-Mar-2020 09:50:40"
+ isLibrary 0
+ sfVersion 80000014
+ firstTarget 44
+ }
+ chart {
+ id 2
+ machine 1
+ name "forearmbot_fordyn_fixb_vp1/forearmbot_gravload/MATLAB Function"
+ windowPosition [422 539.941 189 413]
+ viewLimits [0 156.75 0 153.75]
+ screen [1 1 3600 1200 1.180555555555556]
+ treeNode [0 3 0 0]
+ viewObj 2
+ ssIdHighWaterMark 15
+ decomposition CLUSTER_CHART
+ type EML_CHART
+ chartFileNumber 6
+ disableImplicitCasting 1
+ eml {
+ name "gravload"
+ }
+ firstData 4
+ firstTransition 11
+ firstJunction 10
+ }
+ 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 taug = gravload(q, g_base, pkin, m, r_S)\n%#codegen\ntaug = forearmbot_gravloadJ_floatb_"
+ "twist_slag_vp1(q, g_base, pkin, m, r_S);\n\nend\n\nfunction taug = forearmbot_gravloadJ_floatb_twist_slag_vp1(qJ"
+ ", g, ...\n pkin, m, rSges)\n\n%% Symbolic Calculation\n% From gravload_joint_floatb_twist_par1_matlab.m\n% Opti"
+ "mizationMode: 2\n% StartTime: 2020-03-31 10:54:15\n% EndTime: 2020-03-31 10:54:15\n% DurationCPUTime: 0.11s\n% C"
+ "omputational Cost: add. (1->1), mult. (6->5), div. (0->0), fcn. (2->2), ass. (0->1)\nt1 = [(m(2) * sin(qJ(1)) * "
+ "rSges(2,1) + m(3) * cos(qJ(1)) * pkin(1)) * g(3), 0];\ntaug = t1(:);\n\nend"
+ editorLayout "100 M4x1[10 5 700 500]"
+ }
+ }
+ data {
+ id 4
+ ssIdNumber 4
+ name "q"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ }
+ complexity SF_COMPLEX_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [2 0 5]
+ }
+ data {
+ id 5
+ ssIdNumber 5
+ name "taug"
+ scope OUTPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_NO
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [2 4 6]
+ }
+ data {
+ id 6
+ ssIdNumber 6
+ name "g_base"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [2 5 7]
+ }
+ data {
+ id 7
+ ssIdNumber 7
+ name "pkin"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [2 6 8]
+ }
+ data {
+ id 8
+ ssIdNumber 13
+ name "m"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [2 7 9]
+ }
+ data {
+ id 9
+ ssIdNumber 14
+ name "r_S"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [2 8 0]
+ }
+ junction {
+ id 10
+ position [23.5747 49.5747 7]
+ chart 2
+ subviewer 2
+ ssIdNumber 3
+ type CONNECTIVE_JUNCTION
+ linkNode [2 0 0]
+ }
+ transition {
+ id 11
+ labelString "{eML_blk_kernel();}"
+ labelPosition [28.125 13.875 102.544 14.964]
+ fontSize 12
+ src {
+ intersection [0 0 1 0 23.5747 14.625 0 0]
+ }
+ dst {
+ id 10
+ intersection [1 0 -1 0 23.5747 42.5747 0 0]
+ }
+ midPoint [23.5747 24.9468]
+ chart 2
+ dataLimits [21.175 25.975 14.625 42.575]
+ subviewer 2
+ drawStyle SMART
+ slide {
+ sticky BOTH_STICK
+ }
+ executionOrder 1
+ ssIdNumber 2
+ linkNode [2 0 0]
+ }
+ instance {
+ id 12
+ machine 1
+ name "forearmbot_fordyn_fixb_vp1/forearmbot_gravload/MATLAB Function"
+ chart 2
+ }
+ chart {
+ id 13
+ machine 1
+ name "forearmbot_fordyn_fixb_vp1/forearmbot_coriolisvec_fixb/coriolisvec_fcn"
+ windowPosition [422 539.941 189 413]
+ viewLimits [0 156.75 0 153.75]
+ screen [1 1 3600 1200 1.180555555555556]
+ treeNode [0 14 0 0]
+ viewObj 13
+ ssIdHighWaterMark 16
+ decomposition CLUSTER_CHART
+ type EML_CHART
+ chartFileNumber 7
+ disableImplicitCasting 1
+ eml {
+ name "coriolis"
+ }
+ firstData 15
+ firstTransition 23
+ firstJunction 22
+ }
+ 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 tauc = coriolis(q, qD, pkin, m, r_S, I_S)\n%#codegen\ntauc = forearmbot_coriolisvecJ_fix"
+ "b_slag_vp1(q, qD, pkin, m, r_S, I_S);\n\nend\n\nfunction tauc = forearmbot_coriolisvecJ_fixb_slag_vp1(qJ, qJD, ."
+ "..\n pkin, m, rSges, Icges)\n\n%% Symbolic Calculation\n% From coriolisvec_joint_fixb_par1_matlab.m\n% Optimiza"
+ "tionMode: 2\n% StartTime: 2020-03-31 10:54:15\n% EndTime: 2020-03-31 10:54:16\n% DurationCPUTime: 0.75s\n% Compu"
+ "tational Cost: add. (228->55), mult. (1048->101), div. (0->0), fcn. (610->4), ass. (0->47)\nt27 = sin(qJ(1));\nt"
+ "26 = sin(qJ(2));\nt28 = cos(qJ(2));\nt46 = Icges(3,1) - Icges(3,2);\nt65 = t46 * t28 * t26;\nt15 = t27 * t65;\nt"
+ "22 = t26 ^ 2;\nt24 = t28 ^ 2;\nt39 = Icges(3,1) * t24 + Icges(3,2) * t22;\nt64 = 2 * qJD(2);\nt36 = -Icges(3,3) "
+ "+ t39;\nt66 = t27 * t36;\nt23 = t27 ^ 2;\nt29 = cos(qJ(1));\nt25 = t29 ^ 2;\nt16 = Icges(3,3) * t25 + t39 * t23;"
+ "\nt62 = t27 / 0.2e1;\nt61 = -t29 / 0.2e1;\nt60 = t29 / 0.2e1;\nt11 = t29 * t66;\nt59 = t11 * t27;\nt58 = t11 * t"
+ "29;\nt14 = t29 * t65;\nt57 = t14 * t29;\nt56 = t15 * t27;\nt55 = t16 * t27;\nt17 = t23 * Icges(3,3) + t25 * t39;"
+ "\nt54 = t17 * t29;\nt47 = qJD(1) * t29;\nt13 = t14 * qJD(1);\nt33 = t46 * (t22 - t24);\nt32 = qJD(2) * t33;\nt4 "
+ "= t27 * t32 - t13;\nt37 = qJD(1) * t65;\nt5 = t27 * t37 + t29 * t32;\nt40 = -t5 * t27 + t4 * t29;\nt38 = qJD(2) "
+ "* t65;\nt12 = t27 * t14;\nt35 = (t16 * t29 - t59) * qJD(2);\nt34 = (-t17 * t27 + t58) * qJD(2);\nt10 = t33 * t29"
+ ";\nt9 = t33 * t27;\nt8 = -t16 + t17;\nt3 = -t12 * t64 + (-t23 + t25) * qJD(1) * t36;\nt2 = -t15 * qJD(1) + t35;\n"
+ "t1 = -t13 + t34;\nt6 = [(t2 * t62 - t13 * t61 + t1 * t60 + ((-t27 * t8 - t55 + t58) * t61 + (t29 * t8 - t54 + t5"
+ "9) * t62) * qJD(2) + (0.3e1 / 0.2e1 * t56 + t57 + 0.2e1 * t65) * qJD(1) + t40) * qJD(2); qJD(1) * ((t56 + t57) *"
+ " qJD(1) + t40) / 0.2e1 - (t5 * qJD(1) + (-0.2e1 * t27 * (-t25 * t38 - t47 * t66) + t3 * t29 + (-t54 - t59) * qJD"
+ "(1)) * t64) * t27 / 0.2e1 + (t4 * qJD(1) + (0.2e1 * t29 * (qJD(1) * t11 - t23 * t38) - t3 * t27 + (-t55 - t58) *"
+ " qJD(1)) * t64) * t60 - (t1 + t34) * t47 / 0.2e1 + ((t9 * qJD(1) + (t12 * t27 - t23 * t14) * t64) * t61 + (t10 *"
+ " qJD(1) + (-t12 * t29 + t15 * t25) * t64) * t62) * qJD(2) - (0.2e1 * t37 + (t2 + t35) * t27 + (-t10 * t27 + t9 *"
+ " t29) * qJD(2)) * qJD(1) / 0.2e1;];\ntauc = t6(:);\nend"
+ editorLayout "100 M4x1[10 5 700 500]"
+ }
+ }
+ data {
+ id 15
+ ssIdNumber 4
+ name "q"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ }
+ complexity SF_COMPLEX_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [13 0 16]
+ }
+ data {
+ id 16
+ ssIdNumber 5
+ name "tauc"
+ scope OUTPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_NO
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [13 15 17]
+ }
+ data {
+ id 17
+ ssIdNumber 6
+ name "qD"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [13 16 18]
+ }
+ data {
+ id 18
+ ssIdNumber 7
+ name "pkin"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [13 17 19]
+ }
+ data {
+ id 19
+ ssIdNumber 13
+ name "m"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [13 18 20]
+ }
+ data {
+ id 20
+ ssIdNumber 14
+ name "r_S"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [13 19 21]
+ }
+ data {
+ id 21
+ ssIdNumber 15
+ name "I_S"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [13 20 0]
+ }
+ junction {
+ id 22
+ position [23.5747 49.5747 7]
+ chart 13
+ subviewer 13
+ ssIdNumber 3
+ type CONNECTIVE_JUNCTION
+ linkNode [13 0 0]
+ }
+ transition {
+ id 23
+ labelString "{eML_blk_kernel();}"
+ labelPosition [28.125 13.875 102.544 14.964]
+ fontSize 12
+ src {
+ intersection [0 0 1 0 23.5747 14.625 0 0]
+ }
+ dst {
+ id 22
+ intersection [1 0 -1 0 23.5747 42.5747 0 0]
+ }
+ midPoint [23.5747 24.9468]
+ chart 13
+ dataLimits [21.175 25.975 14.625 42.575]
+ subviewer 13
+ drawStyle SMART
+ slide {
+ sticky BOTH_STICK
+ }
+ executionOrder 1
+ ssIdNumber 2
+ linkNode [13 0 0]
+ }
+ instance {
+ id 24
+ machine 1
+ name "forearmbot_fordyn_fixb_vp1/forearmbot_coriolisvec_fixb/coriolisvec_fcn"
+ chart 13
+ }
+ chart {
+ id 25
+ machine 1
+ name "forearmbot_fordyn_fixb_vp1/forearmbot_inertiaJ/inertiajoint_fcn"
+ windowPosition [422 539.941 189 413]
+ viewLimits [0 156.75 0 153.75]
+ screen [1 1 3600 1200 1.180555555555556]
+ treeNode [0 26 0 0]
+ viewObj 25
+ ssIdHighWaterMark 16
+ decomposition CLUSTER_CHART
+ type EML_CHART
+ chartFileNumber 8
+ disableImplicitCasting 1
+ eml {
+ name "inertia"
+ }
+ firstData 27
+ firstTransition 34
+ firstJunction 33
+ }
+ state {
+ id 26
+ labelString "eML_blk_kernel()"
+ position [18 64.5 118 66]
+ fontSize 12
+ chart 25
+ treeNode [25 0 0 0]
+ superState SUBCHART
+ subviewer 25
+ ssIdNumber 1
+ type FUNC_STATE
+ decomposition CLUSTER_STATE
+ eml {
+ isEML 1
+ script "function Mq = inertia(q, pkin, m, r_S, I_S)\n%#codegen\nMq = forearmbot_inertiaJ_slag_vp1(q, pkin"
+ ", m, r_S, I_S);\n\nend\n\nfunction Mq = forearmbot_inertiaJ_slag_vp1(qJ, ...\n pkin, m, rSges, Icges)\n\n%% Sym"
+ "bolic Calculation\n% From inertia_joint_joint_floatb_twist_par1_matlab.m\n% OptimizationMode: 2\n% StartTime: 20"
+ "20-03-31 10:54:15\n% EndTime: 2020-03-31 10:54:15\n% DurationCPUTime: 0.17s\n% Computational Cost: add. (20->7),"
+ " mult. (84->15), div. (0->0), fcn. (52->4), ass. (0->7)\nt7 = sin(qJ(1));\nt3 = t7 ^ 2;\nt9 = cos(qJ(1));\nt5 = "
+ "t9 ^ 2;\nt8 = cos(qJ(2));\nt6 = sin(qJ(2));\nt1 = [t6 ^ 2 * Icges(3,1) + t8 ^ 2 * Icges(3,2) + Icges(2,3) + (m(2"
+ ") * rSges(2,1) ^ 2 + m(3) * pkin(1) ^ 2) * (t3 + t5); 0; (t5 ^ 2 + (0.2e1 * t5 + t3) * t3) * Icges(3,3);];\n%% P"
+ "ostprocessing: Reshape Output\n% From vec2symmat_2_matlab.m\nres = [t1(1), t1(2); t1(2), t1(3);];\nMq = res;\n\n"
+ "end"
+ editorLayout "100 M4x1[10 5 700 500]"
+ }
+ }
+ data {
+ id 27
+ ssIdNumber 4
+ name "q"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ }
+ complexity SF_COMPLEX_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [25 0 28]
+ }
+ data {
+ id 28
+ ssIdNumber 5
+ name "Mq"
+ scope OUTPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_NO
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [25 27 29]
+ }
+ data {
+ id 29
+ ssIdNumber 7
+ name "pkin"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [25 28 30]
+ }
+ data {
+ id 30
+ ssIdNumber 13
+ name "m"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [25 29 31]
+ }
+ data {
+ id 31
+ ssIdNumber 14
+ name "r_S"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [25 30 32]
+ }
+ data {
+ id 32
+ ssIdNumber 15
+ name "I_S"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [25 31 0]
+ }
+ junction {
+ id 33
+ position [23.5747 49.5747 7]
+ chart 25
+ subviewer 25
+ ssIdNumber 3
+ type CONNECTIVE_JUNCTION
+ linkNode [25 0 0]
+ }
+ transition {
+ id 34
+ labelString "{eML_blk_kernel();}"
+ labelPosition [28.125 13.875 102.544 14.964]
+ fontSize 12
+ src {
+ intersection [0 0 1 0 23.5747 14.625 0 0]
+ }
+ dst {
+ id 33
+ intersection [1 0 -1 0 23.5747 42.5747 0 0]
+ }
+ midPoint [23.5747 24.9468]
+ chart 25
+ dataLimits [21.175 25.975 14.625 42.575]
+ subviewer 25
+ drawStyle SMART
+ slide {
+ sticky BOTH_STICK
+ }
+ executionOrder 1
+ ssIdNumber 2
+ linkNode [25 0 0]
+ }
+ instance {
+ id 35
+ machine 1
+ name "forearmbot_fordyn_fixb_vp1/forearmbot_inertiaJ/inertiajoint_fcn"
+ chart 25
+ }
+ chart {
+ id 36
+ machine 1
+ name "forearmbot_fordyn_fixb_vp1/mFcn_Minv"
+ windowPosition [357.331 483.407 167 391]
+ viewLimits [0 156.75 0 153.75]
+ screen [1 1 1280 1024 1.041050272174191]
+ treeNode [0 37 0 0]
+ viewObj 36
+ ssIdHighWaterMark 6
+ decomposition CLUSTER_CHART
+ type EML_CHART
+ chartFileNumber 11
+ disableImplicitCasting 1
+ eml {
+ name "fcn"
+ }
+ firstData 38
+ firstTransition 42
+ firstJunction 41
+ }
+ state {
+ id 37
+ labelString "eML_blk_kernel()"
+ position [18 64.5 118 66]
+ fontSize 12
+ chart 36
+ treeNode [36 0 0 0]
+ superState SUBCHART
+ subviewer 36
+ ssIdNumber 1
+ type FUNC_STATE
+ decomposition CLUSTER_STATE
+ eml {
+ isEML 1
+ script "function q_ddot = fcn(M,tau)\n%#codegen\nq_ddot=M\\tau;"
+ editorLayout "100 M4x1[10 5 700 500]"
+ }
+ }
+ data {
+ id 38
+ ssIdNumber 4
+ name "M"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ }
+ complexity SF_COMPLEX_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [36 0 39]
+ }
+ data {
+ id 39
+ ssIdNumber 5
+ name "q_ddot"
+ scope OUTPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_NO
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [36 38 40]
+ }
+ data {
+ id 40
+ ssIdNumber 6
+ name "tau"
+ scope INPUT_DATA
+ machine 1
+ props {
+ array {
+ size "-1"
+ }
+ type {
+ method SF_INHERITED_TYPE
+ primitive SF_DOUBLE_TYPE
+ isSigned 1
+ wordLength "16"
+ }
+ complexity SF_COMPLEX_INHERITED
+ frame SF_FRAME_INHERITED
+ unit {
+ name "inherit"
+ }
+ }
+ dataType "Inherit: Same as Simulink"
+ linkNode [36 39 0]
+ }
+ junction {
+ id 41
+ position [23.5747 49.5747 7]
+ chart 36
+ subviewer 36
+ ssIdNumber 3
+ type CONNECTIVE_JUNCTION
+ linkNode [36 0 0]
+ }
+ transition {
+ id 42
+ labelString "{eML_blk_kernel();}"
+ labelPosition [32.125 19.875 102.544 14.964]
+ fontSize 12
+ src {
+ intersection [0 0 1 0 23.5747 14.625 0 0]
+ }
+ dst {
+ id 41
+ intersection [1 0 -1 0 23.5747 42.5747 0 0]
+ }
+ midPoint [23.5747 24.9468]
+ chart 36
+ dataLimits [21.175 25.975 14.625 42.575]
+ subviewer 36
+ drawStyle SMART
+ slide {
+ sticky BOTH_STICK
+ }
+ executionOrder 1
+ ssIdNumber 2
+ linkNode [36 0 0]
+ }
+ instance {
+ id 43
+ machine 1
+ name "forearmbot_fordyn_fixb_vp1/mFcn_Minv"
+ chart 36
+ }
+ target {
+ id 44
+ machine 1
+ name "sfun"
+ description "Default Simulink S-Function Target."
+ linkNode [1 0 45]
+ }
+ target {
+ id 45
+ machine 1
+ name "rtw"
+ linkNode [1 44 0]
+ }
+}
diff --git a/appinterface_ros/appint_ros_example_open.m b/appinterface_ros/appint_ros_example_open.m
new file mode 100644
index 0000000..b536711
--- /dev/null
+++ b/appinterface_ros/appint_ros_example_open.m
@@ -0,0 +1,29 @@
+% Öffne Beispielmodell für Schnittstelle ROS-Simulink
+% Modell basiert auf der Simulation eines zweiachsigen Roboters.
+% Das Dynamikmodell wird mit der Definition aus example_data/robot_env
+% generiert (HybrDyn-Toolbox).
+
+% Moritz Schappler, moritz.schappler@imes.uni-hannover.de, 2020-03
+% (C) Institut für Mechatronische Systeme, Leibniz Universität Hannover
+
+clear
+clc
+
+% Pfade für Modell initialisieren
+this_path = fileparts(which(mfilename));
+cd(this_path);
+addpath(fullfile(this_path, 'ros_rt_interface'));
+addpath(fullfile(this_path, 'ros_rt_interface', 'build'));
+% Bus-Definitionen für Simulink laden
+run(fullfile(this_path, 'ros_rt_interface', 'pcu_ros_load_buses.m'));
+% Definitionen für das Beispielmodell laden
+DynPar1 = struct( ...
+ 'm', [0;1;2], ... % Masse eines zweiachsigen Unterarm-Roboters
+ 'r_S', [[0,0,0]; [0.2,0,0]; [0,0,0]], ... % Schwerpunkte der Segmente
+ 'I_S', [[0,0,0,0,0,0]; [0,0.1,0.1,0,0,0]; [0.2,0.2,0.2,0,0,0]]); % Trägheitsmomente
+pkin = 0.4; % Länge des Beispiel-Roboters
+g_base = [0;0;-9.81]; % G-Vektor
+q_t0 = zeros(2,1); % Anfangswerte der Integratoren der simulierten Strecke
+qD_t0 = zeros(2,1);
+% Simulink-Modell öffnen
+open_system('./appint_ros_example.mdl')
diff --git a/appinterface_ros/build.sh b/appinterface_ros/build.sh
new file mode 100755
index 0000000..c5166f4
--- /dev/null
+++ b/appinterface_ros/build.sh
@@ -0,0 +1,10 @@
+#!/bin/bash -e
+# ROS-Workspace kompilieren
+
+# Moritz Schappler, moritz.schappler@imes.uni-hannover.de, 2020-03
+# (C) Institut für Mechatronische Systeme, Leibniz Universität Hannover
+
+source /opt/ros/melodic/setup.bash
+source catkin_ws/devel/setup.bash
+cd catkin_ws
+catkin_make install
diff --git a/appinterface_ros/build_dep_simulink.sh b/appinterface_ros/build_dep_simulink.sh
new file mode 100755
index 0000000..b9bc5f3
--- /dev/null
+++ b/appinterface_ros/build_dep_simulink.sh
@@ -0,0 +1,12 @@
+#!/bin/bash -e
+# ROS-Echtzeit-Interface für Simulink bauen
+# Dieses Skript muss aus dem Verzeichnis aufgerufen werden, in dem es liegt.
+
+# Moritz Schappler, moritz.schappler@imes.uni-hannover.de, 2020-03
+# (C) Institut für Mechatronische Systeme, Leibniz Universität Hannover
+
+cd ros_rt_interface
+rm -rf build
+mkdir -p build
+make
+cd ..
diff --git a/appinterface_ros/catkin_ws/.gitignore b/appinterface_ros/catkin_ws/.gitignore
new file mode 100644
index 0000000..f216102
--- /dev/null
+++ b/appinterface_ros/catkin_ws/.gitignore
@@ -0,0 +1,5 @@
+build
+devel
+install
+.catkin_workspace
+src/CMakeLists.txt
diff --git a/appinterface_ros/catkin_ws/scripts/autostart.sh b/appinterface_ros/catkin_ws/scripts/autostart.sh
new file mode 100755
index 0000000..b221243
--- /dev/null
+++ b/appinterface_ros/catkin_ws/scripts/autostart.sh
@@ -0,0 +1,35 @@
+#!/bin/bash
+
+# Initialisierung aller Programme für das Beispiel zur Kommunikation
+# zwischen ROS und Simulink
+# Starte alles in einer tmux-Konsole
+
+# Lucas Jürgens, BA bei Moritz Schappler, 2017-04
+# (C) Institut für Regelungstechnik, Leibniz Universität Hannover
+# moritz.schappler@imes.uni-hannover.de, Überarbeitung 2020-03
+
+# ROS initialisieren
+INIT1="/opt/ros/melodic/setup.bash"
+# Hierhin muss der ROS-Workspace installiert worden sein
+INIT2="/home/ec/app_interface/ros_install/scripts/source_ros_install.sh"
+# Init-Befehl
+INITCMD="source $INIT1 && source $INIT2"
+
+# Neue tmux-Session starten
+tmux new-session -d -s app
+
+# Programm 1: Konsolen-Task Manager unter Linux: Prozessorauslastung der Programme
+tmux send-keys "top" C-m
+
+tmux split-window -h # nächstes Fenster ist rechts
+# Programm 2: Kernel-Log anzeigen
+tmux send-keys "watch -n1 \"dmesg | tail -n60\"" C-m
+
+tmux split-window -v # nächstes Fenster ist unten
+# Programm 3: ROS Core starten (entfällt, falls roslaunch oder rosrun gemacht wird)
+tmux send-keys "$INITCMD" C-m "roscore" C-m
+
+tmux select-pane -t 0 # wechsele wieder in erste Spalte
+tmux split-window -v # nächstes Fenster in erster Spalte unten
+# Programm 4: Simulink-Modell starten
+tmux send-keys "$INITCMD" C-m "/home/ec/rtmdl/appint_ros_example" C-m
diff --git a/appinterface_ros/catkin_ws/scripts/source_ros_install.sh b/appinterface_ros/catkin_ws/scripts/source_ros_install.sh
new file mode 100755
index 0000000..1a39ad7
--- /dev/null
+++ b/appinterface_ros/catkin_ws/scripts/source_ros_install.sh
@@ -0,0 +1,25 @@
+#!/bin/bash
+
+# Initialisierung des installierten ROS-Workspace
+# (durch Kopieren des Ordners install aus einem vollen catkin-Workspace)
+
+# Lucas Jürgens, BA bei Moritz Schappler, 2017-04
+# (C) Institut für Regelungstechnik, Leibniz Universität Hannover
+# moritz.schappler@imes.uni-hannover.de, Überarbeitung 2020-03
+
+
+# Verzeichnis des ROS-Workspace
+prefix=/home/ec/app_interface/ros_install/install
+
+# Befehl zum Hinzufügen einer Umgebungsvariablen definieren
+addenv () {
+ if ! eval "echo \$$1" | /bin/grep -Eq "(^|:)$2($|:)" ; then
+ export $1="$2:$(eval "echo \$$1")"
+ fi
+}
+# Umgebungsvariablen für den installierten ROS-Workspace einrichten (ohne src, devel und build)
+addenv ROS_PACKAGE_PATH $prefix/share
+addenv LD_LIBRARY_PATH $prefix/lib
+addenv PYTHONPATH $prefix/lib/python2.7/site-packages
+addenv PKG_CONFIG_PATH $prefix/lib/pkgconfig
+addenv CMAKE_PREFIX_PATH $prefix
diff --git a/appinterface_ros/catkin_ws/src/pcu_common/CMakeLists.txt b/appinterface_ros/catkin_ws/src/pcu_common/CMakeLists.txt
new file mode 100644
index 0000000..1f2f5eb
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_common/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(pcu_common)
+
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ rospy
+ std_msgs
+ message_generation
+ genmsg actionlib_msgs actionlib
+)
+
+
+
+add_service_files(
+ FILES
+ SetInt.srv
+ SetFloat.srv
+ SetString.srv
+ GetBond.srv
+)
+
+add_action_files(DIRECTORY action FILES AcquireRessource.action)
+
+catkin_python_setup()
+
+generate_messages(
+ DEPENDENCIES
+ std_msgs
+ actionlib
+ actionlib_msgs
+)
+
+catkin_package(
+ CATKIN_DEPENDS message_runtime actionlib actionlib_msgs
+)
+
+
+include_directories(
+ ${catkin_INCLUDE_DIRS}
+)
diff --git a/appinterface_ros/catkin_ws/src/pcu_common/action/AcquireRessource.action b/appinterface_ros/catkin_ws/src/pcu_common/action/AcquireRessource.action
new file mode 100644
index 0000000..017b123
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_common/action/AcquireRessource.action
@@ -0,0 +1,3 @@
+
+---
+---
diff --git a/appinterface_ros/catkin_ws/src/pcu_common/package.xml b/appinterface_ros/catkin_ws/src/pcu_common/package.xml
new file mode 100644
index 0000000..e849c36
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_common/package.xml
@@ -0,0 +1,25 @@
+
+
+ pcu_common
+ 1.0.0
+ Common dependencies for the PCU environment
+ user
+ MIT
+ message_generation
+ message_runtime
+ catkin
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+
+ actionlib
+ actionlib_msgs
+ actionlib
+ actionlib_msgs
+
+
+
+
diff --git a/appinterface_ros/catkin_ws/src/pcu_common/setup.py b/appinterface_ros/catkin_ws/src/pcu_common/setup.py
new file mode 100644
index 0000000..bcb9a5a
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_common/setup.py
@@ -0,0 +1,15 @@
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+ packages=['pcu_common_utils'],
+ package_dir={'': 'src'},
+)
+
+
+setup(**setup_args)
+
diff --git a/appinterface_ros/catkin_ws/src/pcu_common/src/pcu_common_utils/AcquireRessource.py b/appinterface_ros/catkin_ws/src/pcu_common/src/pcu_common_utils/AcquireRessource.py
new file mode 100644
index 0000000..e319803
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_common/src/pcu_common_utils/AcquireRessource.py
@@ -0,0 +1,117 @@
+#!/usr/bin/env python
+# Action Server zur Organisation der Client-Verbindung zum Simulink-Modell
+
+# Lucas Jürgens (BA), lucas.juergens@zubox.de, 2017-02
+# Moritz Schappler, schappler@irt.uni-hannover.de
+# (C) Institut für Regelungstechnik, Leibniz Universität Hannover
+
+import rospy
+import actionlib
+
+import threading
+from Queue import Queue
+
+from pcu_common.msg import AcquireRessourceAction, AcquireRessourceGoal
+
+class AcquirableRessource():
+ def __init__(self, action_topic):
+
+ self.acquired = False
+
+ self._as = actionlib.SimpleActionServer(action_topic, AcquireRessourceAction, auto_start=False)
+
+ self._as.register_goal_callback(self._goal_cb)
+ self._as.register_preempt_callback(self._preempt_cb)
+
+ self._as.start()
+
+ self._aborted_cb = None
+ self._acquired_cb = None
+
+ t = threading.Thread(target=self._check_for_abort)
+ t.start()
+
+ def register_acquired_cb(self, cb):
+ self._acquired_cb = cb
+
+ def register_aborted_cb(self, cb):
+ self._aborted_cb = cb
+
+ def _check_for_abort(self):
+ rate = rospy.Rate(50)
+
+ while not rospy.is_shutdown():
+ if self.acquired:
+ if not self._as.is_active():
+ self.acquired = False
+ if self._aborted_cb:
+ self._aborted_cb()
+ rate.sleep()
+
+ def break_acquirement(self):
+ if self.acquired:
+ self._as.set_aborted()
+ acquired = False
+
+ def _goal_cb(self):
+ self._as.accept_new_goal()
+ self.acquired = True
+ if self._acquired_cb:
+ self._acquired_cb()
+
+
+ def _preempt_cb(self):
+ self._as.set_aborted()
+
+class AcquireRessource():
+ def __init__(self, action_topic):
+ self._ac = actionlib.SimpleActionClient(action_topic, AcquireRessourceAction)
+ self._queue = Queue() # Using a queue here, because python 2 doesnt know a timeout for semaphore.wait
+ self._lost_cb = None
+
+ self._timed_out = False
+ self.is_acquired = False
+
+ def register_lost_callback(self, cb):
+ self._lost_cb = cb
+
+ def release(self):
+ if self.is_acquired:
+ self._ac.cancel_all_goals()
+ self.is_acquired = False
+
+ def acquire(self, timeout):
+ if self.is_acquired:
+ return False
+
+ self._timed_out = False
+ if not self._ac.wait_for_server(rospy.Duration(timeout)):
+ self._timed_out = True
+ return False
+
+ goal = AcquireRessourceGoal()
+ self._ac.send_goal(goal, self._done_cb, self._active_cb)
+
+ try:
+ self._queue.get(True, timeout)
+ except:
+ self._timed_out = True
+ return False
+
+ return True
+
+
+ def _done_cb(self, a, b):
+ if self.is_acquired:
+ self.is_acquired = False
+ if self._lost_cb:
+ t = threading.Thread(target=self._lost_cb)
+ t.start()
+
+ def _active_cb(self):
+ if not self._timed_out:
+ self.is_acquired = True
+ self._queue.put(None)
+ else:
+ self._ac.cancel_goal()
+
diff --git a/appinterface_ros/catkin_ws/src/pcu_common/src/pcu_common_utils/__init__.py b/appinterface_ros/catkin_ws/src/pcu_common/src/pcu_common_utils/__init__.py
new file mode 100644
index 0000000..ac690b1
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_common/src/pcu_common_utils/__init__.py
@@ -0,0 +1,4 @@
+import random, string
+
+def genId():
+ return ''.join(random.SystemRandom().choice(string.ascii_uppercase + string.digits) for _ in range(20))
diff --git a/appinterface_ros/catkin_ws/src/pcu_common/srv/GetBond.srv b/appinterface_ros/catkin_ws/src/pcu_common/srv/GetBond.srv
new file mode 100644
index 0000000..689ed77
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_common/srv/GetBond.srv
@@ -0,0 +1,3 @@
+string id
+---
+bool success
diff --git a/appinterface_ros/catkin_ws/src/pcu_common/srv/SetFloat.srv b/appinterface_ros/catkin_ws/src/pcu_common/srv/SetFloat.srv
new file mode 100644
index 0000000..e7b83e8
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_common/srv/SetFloat.srv
@@ -0,0 +1,4 @@
+float32 data
+---
+bool success
+string message
diff --git a/appinterface_ros/catkin_ws/src/pcu_common/srv/SetInt.srv b/appinterface_ros/catkin_ws/src/pcu_common/srv/SetInt.srv
new file mode 100644
index 0000000..ead7b28
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_common/srv/SetInt.srv
@@ -0,0 +1,4 @@
+int32 data
+---
+bool success
+string message
diff --git a/appinterface_ros/catkin_ws/src/pcu_common/srv/SetString.srv b/appinterface_ros/catkin_ws/src/pcu_common/srv/SetString.srv
new file mode 100644
index 0000000..b0de4d8
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_common/srv/SetString.srv
@@ -0,0 +1,4 @@
+string data
+---
+bool success
+string message
diff --git a/appinterface_ros/catkin_ws/src/pcu_sl_interface/.gitignore b/appinterface_ros/catkin_ws/src/pcu_sl_interface/.gitignore
new file mode 100644
index 0000000..924a8eb
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_sl_interface/.gitignore
@@ -0,0 +1,3 @@
+build
+bin
+lib
diff --git a/appinterface_ros/catkin_ws/src/pcu_sl_interface/CMakeLists.txt b/appinterface_ros/catkin_ws/src/pcu_sl_interface/CMakeLists.txt
new file mode 100644
index 0000000..1edf546
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_sl_interface/CMakeLists.txt
@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(pcu_sl_interface)
+
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ rospy
+ std_msgs
+ message_generation
+ dynamic_reconfigure
+ pcu_common
+)
+
+generate_dynamic_reconfigure_options(cfg/model.cfg)
+
+#add_action_files(DIRECTORY action FILES CollectFeatureData.action)
+#add_action_files(DIRECTORY action FILES AquireRessource.action)
+add_message_files(FILES ControllerState.msg JointsVector.msg)
+add_service_files(FILES SetState.srv)
+
+
+# make sure configure headers are built before any node using them
+# add_dependencies(example_node ${PROJECT_NAME}_gencfg)
+
+generate_messages(DEPENDENCIES std_msgs)
+
+catkin_package(CATKIN_DEPENDS message_runtime pcu_common)
+
+include_directories(
+ ${catkin_INCLUDE_DIRS}
+)
+
+add_library(ros_sl_interface src/node.cpp)
+target_link_libraries(ros_sl_interface ${catkin_LIBRARIES})
+add_dependencies(ros_sl_interface pcu_common_gencpp)
+
+add_executable(test_interface src/test_interface.cpp)
+target_link_libraries(test_interface ros_sl_interface ${catkin_LIBRARIES})
+
+#add_executable(test2 src/test.cpp)
+#target_link_libraries(test2 ${catkin_LIBRARIES})
+
+install(TARGETS ros_sl_interface test_interface
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+catkin_install_python(PROGRAMS scripts/client_test.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
diff --git a/appinterface_ros/catkin_ws/src/pcu_sl_interface/cfg/model.cfg b/appinterface_ros/catkin_ws/src/pcu_sl_interface/cfg/model.cfg
new file mode 100755
index 0000000..19e647c
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_sl_interface/cfg/model.cfg
@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+PACKAGE = "pcu_sl_interface"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("D_j1",double_t,0,"Attenuation Joint 1",0.7, 0,1)
+gen.add("D_j2",double_t,0,"Attenuation Joint 2",0.7, 0,1)
+gen.add("K_j1",double_t,0,"Stiffness Joint 1",140, 0,500)
+gen.add("K_j2",double_t,0,"Stiffness Joint 2",20, 0,50)
+
+exit(gen.generate(PACKAGE, "pcu_sl_interface", "model"))
diff --git a/appinterface_ros/catkin_ws/src/pcu_sl_interface/msg/ControllerState.msg b/appinterface_ros/catkin_ws/src/pcu_sl_interface/msg/ControllerState.msg
new file mode 100644
index 0000000..c939c11
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_sl_interface/msg/ControllerState.msg
@@ -0,0 +1,6 @@
+uint8 state
+uint8 OFF=0
+uint8 STANDBY=1
+uint8 GRAVCOM=2
+uint8 TRA_VELOCITY=3
+uint8 TRA_POSITION=4
diff --git a/appinterface_ros/catkin_ws/src/pcu_sl_interface/msg/JointsVector.msg b/appinterface_ros/catkin_ws/src/pcu_sl_interface/msg/JointsVector.msg
new file mode 100644
index 0000000..877236c
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_sl_interface/msg/JointsVector.msg
@@ -0,0 +1 @@
+float64[2] joint_data
diff --git a/appinterface_ros/catkin_ws/src/pcu_sl_interface/package.xml b/appinterface_ros/catkin_ws/src/pcu_sl_interface/package.xml
new file mode 100644
index 0000000..85cb84d
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_sl_interface/package.xml
@@ -0,0 +1,30 @@
+
+
+ pcu_sl_interface
+ 1.0.0
+ ROS-Interface for Simulink
+
+
+ user
+ MIT
+
+ catkin
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+
+ message_generation
+ message_runtime
+
+ pcu_common
+ pcu_common
+
+ dynamic_reconfigure
+ dynamic_reconfigure
+
+
+
+
diff --git a/appinterface_ros/catkin_ws/src/pcu_sl_interface/scripts/client_test.py b/appinterface_ros/catkin_ws/src/pcu_sl_interface/scripts/client_test.py
new file mode 100755
index 0000000..57c86f8
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_sl_interface/scripts/client_test.py
@@ -0,0 +1,106 @@
+#!/usr/bin/env python
+
+import sys
+import time
+
+import rospy
+
+import threading
+
+from pcu_common_utils.AcquireRessource import AcquireRessource
+from pcu_sl_interface.srv import SetState
+from pcu_sl_interface.msg import ControllerState, JointsVector
+
+
+class Node:
+ def __init__(self):
+ self.sema = threading.Semaphore(0)
+
+ self.ar = AcquireRessource("/SL_RT_CORE/acquire")
+
+ self.srv_set_state = rospy.ServiceProxy("/SL_RT_CORE/set_state", SetState)
+
+ print("waiting for service...")
+ self.srv_set_state.wait_for_service()
+ print("ok!")
+
+
+ self.qd_pub = rospy.Publisher("/SL_RT_CORE/qd_set", JointsVector, queue_size=1)
+
+ def aq(self):
+ print("Aquiring Core")
+ if self.ar.acquire(1.0):
+ print("Aquired!")
+ else:
+ print("timeout!")
+
+
+ def run(self):
+
+
+ #self.done = false
+ self.setState(ControllerState.STANDBY)
+
+ self.aq()
+
+ self.setState(ControllerState.TRA_VELOCITY)
+
+ rate = rospy.Rate(10)
+ c = 0
+
+ #time.sleep(2)
+
+
+
+ while self.ar.is_acquired and not rospy.is_shutdown():
+ c += 0.1
+ jv = JointsVector()
+ jv.joint_data = [c, c*3.3]
+
+ self.qd_pub.publish(jv)
+ rate.sleep()
+
+
+ self.ar.release()
+
+ print("Broken")
+
+ def setState(self, nr):
+ cs = ControllerState()
+ cs.state = nr
+ if not self.srv_set_state(cs).success:
+ print("Failed to set State.")
+ else:
+ print("Setting state ok.")
+
+rospy.init_node('client_test', anonymous=True)
+node = Node()
+
+t = threading.Thread(target=node.run)
+t.start()
+
+#node.run()
+rospy.spin()
+
+
+
+
+
+
+
+
+
+
+"""
+
+
+id = generate_unique_id()
+# Sends id to B using an action or a service
+bond = bondpy.Bond("example_bond_topic", id)
+bond.start()
+if not bond.wait_until_formed(rospy.Duration(1.0)):
+ raise Exception('Bond could not be formed')
+# ... do things with B ...
+bond.wait_until_broken()
+print "B has broken the bond"
+"""
diff --git a/appinterface_ros/catkin_ws/src/pcu_sl_interface/src/SL_func.h b/appinterface_ros/catkin_ws/src/pcu_sl_interface/src/SL_func.h
new file mode 100644
index 0000000..62f3f61
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_sl_interface/src/SL_func.h
@@ -0,0 +1,45 @@
+/*
+Definition der Ein- und Ausgabedaten für die Echtzeitschnittstelle zu Simulink
+Diese Datei gilt für die Anwendungsseite und muss identisch auf der Simulink-Seite existieren
+
+* Lucas Jürgens, BA bei Moritz Schappler, 2017-04
+* (C) Institut für Regelungstechnik, Leibniz Universität Hannover
+* moritz.schappler@imes.uni-hannover.de
+*/
+
+#ifndef SL_FUNC_H
+#define SL_FUNC_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Eingaben in Simulink-Modell (=Ausgabe aus dem SFcn-Block)
+typedef struct{
+ double q_set[2];
+ double qd_set[2];
+
+ double param_D[2];
+ double param_K[2];
+
+ unsigned char state;
+
+} SL_IN_type;
+
+// Ausgaben aus Simulink-Modell (=Eingabe in den SFcn-Block)
+typedef struct{
+ double q_meas[2];
+ double qd_meas[2];
+ unsigned char sl_state;
+
+} SL_OUT_type;
+
+void SL_io_func(SL_OUT_type* sl_out, SL_IN_type* sl_in);
+void SL_start_func();
+void SL_terminate_func();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/appinterface_ros/catkin_ws/src/pcu_sl_interface/src/node.cpp b/appinterface_ros/catkin_ws/src/pcu_sl_interface/src/node.cpp
new file mode 100644
index 0000000..c420e8c
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_sl_interface/src/node.cpp
@@ -0,0 +1,381 @@
+/*
+Mit dieser Klasse ist eine Echtzeit-Kommunikation mit Simulink-Modellen möglich.
+Details, siehe BA von Lucas Jürgens (Kap. 6.3.2) und Dokumentation des Beispiels.
+Diese Datei stellt ein Minimalbeispiel ohne die Abhängigkeit zu ROS dar.
+
+Allgemein:
+* Die mit SL_ beginnenden Funktionen werden durch das Simulink-Modell aufgerufen
+
+* Lucas Jürgens, BA bei Moritz Schappler, 2017-04
+* (C) Institut für Regelungstechnik, Leibniz Universität Hannover
+* moritz.schappler@imes.uni-hannover.de, Überarbeitung 2020-03
+*/
+
+#include
+#include
+#include
+
+#include "ros/ros.h"
+
+#include
+
+#include "std_msgs/Float64.h"
+#include "std_msgs/String.h"
+#include "pcu_common/AcquireRessourceAction.h"
+#include "pcu_sl_interface/JointsVector.h"
+#include "pcu_sl_interface/SetState.h"
+#include "pcu_sl_interface/ControllerState.h"
+
+#include
+#include
+
+#include "SL_func.h"
+
+class SLNode
+{
+protected:
+
+ sem_t sem_out_newdata_; // Semaphore für Synchronisation der Ausgabedaten von Simulink
+ pthread_mutex_t mut_out_; // Schutz der gemeinsamen Variable sl_out_buffer_
+ pthread_mutex_t mut_in_; // Schutz der gemeinsamen Variable sl_in_buffer_
+ pthread_mutexattr_t mutattr_prioinherit_; // Eigenschafts-Objekt für mutex-Variablen
+ pthread_mutex_t mut_state_; // TODO: Braucht man das?
+
+ // Structures holding Simulink Signals , corresponding to Simulink Busses
+ SL_OUT_type sl_out_buffer_;
+ SL_IN_type sl_in_buffer_;
+
+ int current_state_;
+ int state_requested_;
+
+
+ bool was_acquired_; // gibt an, ob eine Verbindung erstellt ist und Sollwerte geschrieben werden dürfen
+
+ int publish_decimation_value_; // Reduktion der an den nicht-Echtzeitbereich zu sendenden Daten
+ bool is_stopped_;
+
+ // Deklaration von ROS-bezogenen Handles
+ ros::NodeHandle nh_;
+ ros::AsyncSpinner spinner_;
+ // Deklaration des Action-Servers (für Bond-Primitive)
+ actionlib::SimpleActionServer as_acquire_;
+ // Deklaration der Services
+ ros::ServiceServer srvServer_setState_;
+ // Deklaration von ROS Subscribern und Publisher
+ ros::Subscriber sub_q_set_;
+ ros::Subscriber sub_qd_set_;
+ ros::Publisher pub_q_meas_;
+ ros::Publisher pub_qd_meas_;
+ ros::Publisher pub_controller_state_;
+ // Deklaration von Dynamic Reconfigure
+ dynamic_reconfigure::Server dyn_server_;
+ dynamic_reconfigure::Server::CallbackType dynCB_;
+
+
+public:
+
+ SLNode():
+ as_acquire_(nh_, ros::this_node::getName() + "/acquire", false),
+ spinner_(1),
+ current_state_(0),
+ is_stopped_(false),
+ state_requested_(0),
+ was_acquired_(false) // Start ohne Verbindung zu Client
+{
+ // Initialisiere Synchronisationsmittel (Mutex, Semaphore)
+ //ROS_ASSERT(!sem_init(&sem_init_finished, 0, 0));
+ ROS_ASSERT(!sem_init(&sem_out_newdata_, 0, 0));
+ ROS_ASSERT(!pthread_mutexattr_init(&mutattr_prioinherit_));
+ // Eigenschaft PRIO_INHERIT setzen, um Prioritätsinversion zu verhindern (siehe BA Jürgens, S. 70)
+ ROS_ASSERT(!pthread_mutexattr_setprotocol(&mutattr_prioinherit_, PTHREAD_PRIO_INHERIT));
+ ROS_ASSERT(!pthread_mutex_init(&mut_in_, &mutattr_prioinherit_));
+ ROS_ASSERT(!pthread_mutex_init(&mut_out_, &mutattr_prioinherit_));
+ ROS_ASSERT(!pthread_mutex_init(&mut_state_, NULL)); // TODO: Braucht man das?
+
+ nh_ = ros::NodeHandle("~");
+ // Initialisiere Action Server
+ as_acquire_.registerGoalCallback(boost::bind(&SLNode::as_acquire_goal_cb_, this));
+ as_acquire_.registerPreemptCallback(boost::bind(&SLNode::as_acquire_preempted_cb_, this));
+ // Initialisiere Service
+ srvServer_setState_ = nh_.advertiseService("set_state", &SLNode::srv_setState_cb, this);
+ // Initialisiere ROS-Subscriber
+ sub_q_set_ = nh_.subscribe("q_set", 1, &SLNode::sub_q_set_cb, this);
+ sub_qd_set_ = nh_.subscribe("qd_set", 1, &SLNode::sub_qd_set_cb, this);
+ // Initialisiere ROS-Publisher
+ pub_q_meas_ = nh_.advertise("q_meas", 1);
+ pub_qd_meas_ = nh_.advertise("qd_meas", 1);
+ pub_controller_state_ = nh_.advertise("controller_state", 1, true);
+ // Parametriere Publisher
+ nh_.param("publish_decimation", publish_decimation_value_, 1);
+ // Initialisiere Dynamic Reconfigure
+ dynCB_ = boost::bind(&SLNode::dynamic_reconfigure_cb, this, _1, _2);
+ dyn_server_.setCallback(dynCB_);
+ // Weitere Initialisierung
+ as_acquire_.start();
+
+ spinner_.start();
+
+ changedState(pcu_sl_interface::ControllerState::OFF);
+
+}
+
+ ~SLNode(void)
+ {
+ if(as_acquire_.isActive())
+ as_acquire_.setAborted(); // Unterbreche Verbindung zu Client über Action Server, falls existent
+ }
+
+
+
+ void SL_io_func(SL_OUT_type* sl_out, SL_IN_type* sl_in){
+ // Diese Funktion wird vom Simulink-Modell im Echtzeit-Teil aufgerufen
+ int val = 1;
+
+ // Eingabedaten für Simulinkmodell ("ROS to Simulink")
+ pthread_mutex_lock(&mut_in_); // Sperre Zugriff auf sl_in_buffer_
+ memcpy((void *)sl_in, (void *)&sl_in_buffer_, sizeof(SL_IN_type)); // Daten kopieren (sl_in sind die Daten aus Simulink)
+ pthread_mutex_unlock(&mut_in_); // Freigabe von sl_in_buffer_
+
+ // Ausgabedaten aus dem Simulinkmodell ("Simulink to ROS")
+ sem_getvalue(&sem_out_newdata_, &val); // Semaphore für Daten-Ein-/Ausgabe zu Simulink
+ if(val == 0){ // Zugriff auf Daten ist möglich
+ pthread_mutex_lock(&mut_out_); // Sperre Zugriff auf sl_out_buffer_
+ memcpy((void *)&sl_out_buffer_, (void *)sl_out, sizeof(SL_OUT_type)); // Daten kopieren (sl_out wird nach Simulink geschrieben)
+ pthread_mutex_unlock(&mut_out_); // Freigabe von sl_out_buffer_
+ sem_post(&sem_out_newdata_); // Increments value of sem_out_newdata
+ }
+ }
+
+ void run(){
+
+ int decimation_counter = 0;
+
+ int sl_state = 0; // Hilfsvariable für Eingabevariable sl_state in den SFcn-Block
+ int sl_state_last = 0; // Altwert dazu
+
+ sem_wait(&sem_out_newdata_);
+ while (ros::ok() && !is_stopped_)
+ {
+
+ // Definitionen für Daten aus Simulink-Modell
+ pcu_sl_interface::JointsVector q_meas;
+ pcu_sl_interface::JointsVector qd_meas;
+
+ pthread_mutex_lock(&mut_out_); // Warte auf Freigabe des Simulink-Buffers (erteilt durch SL_io_func)
+ // Daten aus Simulink-Buffer holen (in lokale Variablen schreiben)
+ q_meas.joint_data[0] = sl_out_buffer_.q_meas[0];
+ q_meas.joint_data[1] = sl_out_buffer_.q_meas[1];
+ qd_meas.joint_data[0] = sl_out_buffer_.qd_meas[0];
+ qd_meas.joint_data[1] = sl_out_buffer_.qd_meas[1];
+ sl_state_last = sl_state;
+ sl_state = sl_out_buffer_.sl_state;
+
+ pthread_mutex_unlock(&mut_out_); // Gebe Simulink-Buffer wieder frei
+
+
+ decimation_counter = (decimation_counter+1)%publish_decimation_value_;
+ // von Simulink erhaltene Daten publishen
+ if(decimation_counter==0){
+ pub_q_meas_.publish(q_meas);
+ pub_qd_meas_.publish(qd_meas);
+ }
+
+ sem_wait(&sem_out_newdata_);
+
+ if(sl_state != sl_state_last){
+ // Der Controller-State wurde vom Simulink-Modell geändert.
+ // Ursache z.B. aktivierte Sicherheitsfunktion (z.B. Kollisionserkennung)
+ if(sl_state != state_requested_){
+ ROS_INFO("ControllerState Override!");
+ if(as_acquire_.isActive()){
+ // Unterbreche Verbindung zum Client
+ breakActiveAcquirement();
+ }
+ requestState(sl_state);
+ }
+ changedState(sl_state);
+ }
+
+ ros::spinOnce();
+
+ //When a client acquired control and released it, set current_state to STANDBY, of not OFF.
+ if(was_acquired_){
+ if(!as_acquire_.isActive()){
+ was_acquired_ = false;
+ ROS_INFO("Client released");
+ if(sl_state != pcu_sl_interface::ControllerState::OFF)
+ requestState(pcu_sl_interface::ControllerState::STANDBY);
+ }
+ }
+ }
+ }
+
+ void stop(){
+ is_stopped_ = true;
+ sem_post(&sem_out_newdata_);
+ }
+
+
+private:
+
+ void as_acquire_goal_cb_()
+ {
+ // Callback für Goal beim Action Server
+ // Von einem Client wird eine Verbindung hergestellt
+ ROS_INFO("New Client Acquiring!");
+ as_acquire_.acceptNewGoal();
+ was_acquired_ = true;
+ }
+
+ void as_acquire_preempted_cb_()
+ {
+ // Callback, falls über Action-Server der Zugriff als unterbrochen angezeigt wird.
+ as_acquire_.setAborted();
+ }
+
+ bool srv_setState_cb(pcu_sl_interface::SetState::Request &req,
+ pcu_sl_interface::SetState::Response &res )
+ {
+ // Verlange Wechsel des Controller-States. Anfrage wird nur an das
+ // Simulink-Modell weitergegeben, wenn ein Verbindung über den Action
+ // Server hergestellt wurde. Ansonsten verweigert der Service die Anfrage.
+ res.success = false;
+
+ if(as_acquire_.isActive())
+ res.success = true;
+ else{
+ if(current_state_ == pcu_sl_interface::ControllerState::OFF && req.state.state == pcu_sl_interface::ControllerState::STANDBY)
+ res.success = true; // Gehe von OFF in STANDBY
+ else if(current_state_ == pcu_sl_interface::ControllerState::STANDBY && req.state.state == pcu_sl_interface::ControllerState::OFF)
+ res.success = true; // Gehe von STANDBY in OFF
+ }
+
+ if(!res.success == true){
+ ROS_WARN("Client failed to set ControllerState!");
+ }
+ else{
+ requestState(req.state.state);
+ }
+
+ return true;
+ }
+
+ void breakActiveAcquirement(){
+ // Verbindung zu vorhandem Client über den Action Server unterbrechen
+ if(as_acquire_.isActive())
+ as_acquire_.setAborted();
+ was_acquired_ = false;
+ }
+
+ void changedState(int new_state){
+ // Sendet eine ROS-Message bei Wechsel des Controller-States
+ current_state_ = new_state;
+ ROS_INFO("New ControllerState: %d", current_state_);
+
+ pcu_sl_interface::ControllerState cs;
+ cs.state = current_state_;
+ pub_controller_state_.publish(cs);
+ }
+
+ void requestState(int req_state){
+ // Funktion zum Definieren eines neuen Zustands für den SL-Eingang state
+ if(req_state != current_state_){
+ pthread_mutex_lock(&mut_in_);
+ sl_in_buffer_.state = req_state;
+ pthread_mutex_unlock(&mut_in_);
+ }
+ state_requested_ = req_state;
+ }
+
+
+ void sub_q_set_cb(const pcu_sl_interface::JointsVector::ConstPtr& msg)
+ {
+ // Callback-Funktion für Subscriber von q_set
+ // Wird aufgerufen, wenn neue ROS-Message dafür ankommt
+ pthread_mutex_lock(&mut_in_); // Sperre Zugriff auf sl_in_buffer_
+ sl_in_buffer_.q_set[0] = msg->joint_data[0]; // Daten von ROS-Message in Buffer schreiben
+ sl_in_buffer_.q_set[1] = msg->joint_data[1];
+ pthread_mutex_unlock(&mut_in_); // Freigabe von sl_in_buffer_
+ }
+
+ void sub_qd_set_cb(const pcu_sl_interface::JointsVector::ConstPtr& msg)
+ {
+ // Callback-Funktion für Subscriber von qd_set
+ pthread_mutex_lock(&mut_in_);
+ sl_in_buffer_.qd_set[0] = msg->joint_data[0];
+ sl_in_buffer_.qd_set[1] = msg->joint_data[1];
+ pthread_mutex_unlock(&mut_in_);
+ }
+
+ void dynamic_reconfigure_cb(pcu_sl_interface::modelConfig &config, uint32_t level){
+ // Callback-Funktion für Dynamic Reconfigure.
+ ROS_INFO("dynamic reconfigure");
+
+ pthread_mutex_lock(&mut_in_); // Sperre Zugriff auf sl_in_buffer_
+ sl_in_buffer_.param_D[0] = config.D_j1;
+ sl_in_buffer_.param_D[1] = config.D_j2;
+ sl_in_buffer_.param_K[0] = config.K_j1;
+ sl_in_buffer_.param_K[1] = config.K_j2;
+ pthread_mutex_unlock(&mut_in_); // Freigabe von sl_in_buffer_
+ }
+
+};
+
+pthread_t ros_thread; // definiert Thread, in dem der ROS-Teil läuft
+void *ros_thread_fn(void* arg); // Startet ROS-Thread unten
+
+sem_t sem_init_finished; // Semaphore zur Synchronisation der Initialisierung
+
+SLNode* slNode;
+
+void SL_io_func(SL_OUT_type* sl_out, SL_IN_type* sl_in){
+ // Wird bei Aufrufen des Simulink-Blocks zu jedem Zeitschritt ausgeführt
+ // (Einstellung in Matlab über legacy_code, OutputFcnSpec in generate_block.m)
+ slNode->SL_io_func(sl_out, sl_in);
+}
+
+void SL_start_func(){
+ // Wird bei Modell-Initialisierung aufgerufen
+ // (Einstellung in Matlab über legacy_code, StartFcnSpec in generate_block.m)
+ std::cout << "Creating ROS-Node Thread!" << std::endl;
+
+ ROS_ASSERT(!sem_init(&sem_init_finished, 0, 0)); // Initialisierung der Semaphore
+ pthread_create(&ros_thread, NULL, &ros_thread_fn, NULL);
+ sem_wait(&sem_init_finished); //Wait for Initialization of the ROS-Node in main-thread. Wait, because dynamic-reconfigure presets values
+}
+
+void SL_terminate_func(){
+ // Wird bei Beenden des Modells ausgeführt
+ // (Einstellung in Matlab über legacy_code, TerminateFcnSpec in generate_block.m)
+ std::cout << "Terminating ROS-Node Thread!" << std::endl;
+
+ slNode->stop();
+ pthread_join(ros_thread, NULL);
+}
+
+void *ros_thread_fn(void* arg)
+{
+ int argc = 0;
+ char **argv = NULL;
+
+ // Set Threads Priority FIXME: Priority adjustable
+ // set it prior to the ROS-initialisation. ROS-threads inherit the priority
+ struct sched_param param = {};
+ param.sched_priority = 90;
+ if (sched_setscheduler (0, SCHED_RR, ¶m) == -1){/*empty*/};
+ // ROS-Node initialisieren
+ ros::init(argc, argv, "SL_RT_CORE", ros::init_options::NoSigintHandler);
+
+ slNode = new SLNode();
+
+ sem_post(&sem_init_finished); // Gebe Semaphore für Initialisierung frei. Vorher wird die SL_start_func zurückgehalten.
+
+ slNode->run(); // Läuft so lange als Endlosschleife, bis das Programm abbricht.
+
+ delete(slNode);
+ ros::shutdown();
+
+ ROS_INFO("OUT!");
+
+ return 0;
+}
diff --git a/appinterface_ros/catkin_ws/src/pcu_sl_interface/src/test_interface.cpp b/appinterface_ros/catkin_ws/src/pcu_sl_interface/src/test_interface.cpp
new file mode 100644
index 0000000..079aa9d
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_sl_interface/src/test_interface.cpp
@@ -0,0 +1,76 @@
+/*
+Dieses Testskript dient nur zur Prüfung, ob die Simulink-Schnittstelle kompiliert.
+Hiermit können einzelne Details der Klasse SLNode aus node.cpp getestet werden.
+Eine Kommunikation mit dem Simulink-Modell ist nicht möglich!
+
+* Lucas Jürgens, BA bei Moritz Schappler, 2017-04
+* (C) Institut für Regelungstechnik, Leibniz Universität Hannover
+* moritz.schappler@imes.uni-hannover.de, Überarbeitung 2020-03
+*/
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "SL_func.h"
+
+
+bool do_shutdown = false;
+
+SL_OUT_type sl_out;
+SL_IN_type sl_in;
+
+void my_siginthandler(int s){
+ do_shutdown = true;
+}
+
+int main(){
+ struct sigaction sigIntHandler;
+ sigIntHandler.sa_handler = my_siginthandler;
+ sigemptyset(&sigIntHandler.sa_mask);
+ sigIntHandler.sa_flags = 0;
+ sigaction(SIGINT, &sigIntHandler, NULL);
+
+ SL_start_func();
+
+ int testcnt = 0;
+
+ double test_double = 0;
+
+ while(!do_shutdown){
+ SL_io_func(&sl_out, &sl_in);
+ //printf("i:%d, qd1: %f; state: %d\n", testcnt, sl_in.qd_set[0], sl_in.state);
+
+
+ test_double += 0.01;
+ if(test_double > 1.0)
+ test_double = 0.0;
+
+ sl_out.q_meas[0] = 1.0 - test_double*0.5;
+ sl_out.q_meas[1] = test_double;
+
+
+
+ /*switch(testcnt++){
+ case 100:
+ sl_out.state_override = 2;
+ printf("\n\nOVERRIDE-Test!\n");
+ break;
+ case 110:
+ sl_out.state_override = 0;
+ break;
+ }
+ */
+
+ sl_out.sl_state = sl_in.state;
+
+ usleep(100000); // 0.1 Sekunde Pause
+ }
+
+ SL_terminate_func();
+
+ return 0;
+}
diff --git a/appinterface_ros/catkin_ws/src/pcu_sl_interface/srv/SetState.srv b/appinterface_ros/catkin_ws/src/pcu_sl_interface/srv/SetState.srv
new file mode 100644
index 0000000..5144fd1
--- /dev/null
+++ b/appinterface_ros/catkin_ws/src/pcu_sl_interface/srv/SetState.srv
@@ -0,0 +1,3 @@
+pcu_sl_interface/ControllerState state
+---
+bool success
diff --git a/appinterface_ros/example_data/robot_env_forearmbot b/appinterface_ros/example_data/robot_env_forearmbot
new file mode 100644
index 0000000..c327591
--- /dev/null
+++ b/appinterface_ros/example_data/robot_env_forearmbot
@@ -0,0 +1,27 @@
+# Einfaches zweiachsiges System
+# Prothese 2 DOF
+
+robot_name := "forearmbot":
+
+NJ := 2:
+NQJ := 2:
+
+qoffset := Matrix(2,1,[-(1/2)*Pi, 0]):
+alpha := Matrix(2,1,[-(1/2)*Pi, -(1/2)*Pi]):
+d := Matrix(2,1,[ 0, d2]):
+a := Matrix(2,1,[0, 0]):
+v := Matrix(2,1,[0, 1]):
+b := Matrix(2,1,[0, 0]):
+beta := Matrix(2,1,[0, 0]):
+theta := qJ_t+qoffset:
+
+# Tischaufbau: Schwerkraft in nur eine Richtung
+g_world := <0;0;g3>:
+
+# Setze Massen (Basis masselos. Ist aber sowieso egal).
+user_M := <0;M1;M2>:
+# Setze Schwerpunkt (nur auf Segment)
+user_CoM := <<0;0;0> | | <0;0;0>>:
+# Setze Trägheitsmomente: Reihenfolge: XX;XY,XZ,YY,YZ,ZZ
+# Annahme: Dünner Stab (in x-Richtung des Körper-KS). Dann Kugel für zweites Segment am Ende
+user_inertia := <<0;0;0;0;0;0>| <0;0;0;YYC1;0;ZZC1>| >:
diff --git a/appinterface_ros/ros_rt_interface/.gitignore b/appinterface_ros/ros_rt_interface/.gitignore
new file mode 100644
index 0000000..567609b
--- /dev/null
+++ b/appinterface_ros/ros_rt_interface/.gitignore
@@ -0,0 +1 @@
+build/
diff --git a/appinterface_ros/ros_rt_interface/Makefile b/appinterface_ros/ros_rt_interface/Makefile
new file mode 100644
index 0000000..e7a3833
--- /dev/null
+++ b/appinterface_ros/ros_rt_interface/Makefile
@@ -0,0 +1,18 @@
+# Make-File für das ROS-Simulink-Interface
+
+# Lucas Jürgens (BA), lucas.juergens@zubox.de, 2017-02
+# Moritz Schappler, schappler@irt.uni-hannover.de
+# (c) Institut für Regelungstechnik, Universität Hannover
+
+
+BUILDDIR=build
+ROS_NODE_DIR=ros_rt_core
+# ROS_PATH=$OECORE_TARGET_SYSROOT/opt/ros/indigo
+ROS_NODE_FILE=SL_func_dummy
+LIB_NAME=libros_sl_interface.so
+
+all: roslib
+
+roslib: $(BUILDDIR)/$(roslib.a)
+ ${CXX} -fPIC -c -DDUMMY $(ROS_NODE_DIR)/$(ROS_NODE_FILE).cpp -o $(BUILDDIR)/$(ROS_NODE_FILE).o -Wall -shared
+ ${CXX} -shared -fPIC -Wl,-soname,$(LIB_NAME) -o $(BUILDDIR)/$(LIB_NAME) $(BUILDDIR)/$(ROS_NODE_FILE).o -lc
diff --git a/appinterface_ros/ros_rt_interface/generate_block.m b/appinterface_ros/ros_rt_interface/generate_block.m
new file mode 100644
index 0000000..4ca5e1f
--- /dev/null
+++ b/appinterface_ros/ros_rt_interface/generate_block.m
@@ -0,0 +1,42 @@
+% Erstelle den Block zur Kommunikation mit der ROS-Seite des App-Interface
+% Dieser Block muss jedes Mal neu generiert werden, wenn sich der Bus SL_IN
+% oder SL_out ändert.
+% Der Ordner ros_rt_interface muss im Matlab-Pfad sein.
+% Ein Ersetzen des Blocks AppInterface in der Library ist nicht notwendig.
+% Im Simulink-Modell wird nur der hier erzeugte Code aufgerufen (libros_sl_interface.so, SL_func.h,SL_func_dummy.cpp).
+
+% Lucas Jürgens (BA), lucas.juergens@zubox.de, 2017-02
+% Moritz Schappler, schappler@irt.uni-hannover.de
+% (c) Institut für Regelungstechnik, Universität Hannover
+
+[folder, name, ext] = fileparts(which(mfilename));
+builddir = fullfile(folder, 'build'); % build-Ordner im selben Verzeichnis wie dieses Skript
+mkdir(builddir);
+cd(builddir);
+
+try
+ def = legacy_code('initialize')
+ def.SourceFiles = {'../ros_rt_interface/ros_rt_core/SL_func_dummy.cpp'}; % Die Angabe dieser Datei ist trotz der shared Library nötig, da diese ja nur mit ARM kompatibel ist.
+ def.HeaderFiles = {'SL_func.h'};
+ def.IncPaths = {'../ros_rt_interface/ros_rt_core'};
+ def.TargetLibFiles = {'libros_sl_interface.so'};
+ def.LibPaths = {'../build'};
+
+ def.SFunctionName = 'ros_rt_interface_pcu';
+
+ def.StartFcnSpec = 'void SL_start_func()';
+ def.OutputFcnSpec = 'void SL_io_func(SL_OUT_type u1[1], SL_IN_type y1[1])';
+ def.TerminateFcnSpec = 'void SL_terminate_func()';
+
+ legacy_code('sfcn_cmex_generate', def);
+ legacy_code('compile', def, '-DDUMMY'); % Erstellt MEX-File. Ist leider nötig
+ legacy_code('sfcn_tlc_generate', def);
+ legacy_code('rtwmakecfg_generate', def);
+ legacy_code('slblock_generate', def);
+
+catch ME
+ cd(folder);
+ rethrow(ME)
+end
+
+cd(folder);
diff --git a/appinterface_ros/ros_rt_interface/pcu_ros_load_buses.m b/appinterface_ros/ros_rt_interface/pcu_ros_load_buses.m
new file mode 100644
index 0000000..9d1c25c
--- /dev/null
+++ b/appinterface_ros/ros_rt_interface/pcu_ros_load_buses.m
@@ -0,0 +1,14 @@
+% Erstelle Bus-Objekte zum Kompilieren des PCU-App-Interface im Matlab-Workspace
+% Wird zum Kompilieren der PCU-Simulinkmodelle benötigt
+%
+% Führe dieses Skript direkt aus und nicht zeilenweise (damit der Pfad bestimmt werden kann)
+
+% Lucas Jürgens (BA), lucas.juergens@zubox.de, 2017-02
+% Moritz Schappler, schappler@irt.uni-hannover.de
+% (c) Institut für Regelungstechnik, Universität Hannover
+
+[folder, ~, ~] = fileparts(which(mfilename));
+filedir = fullfile(folder, 'ros_rt_core');
+
+run(fullfile(filedir, 'bus_SL_IN.m'))
+run(fullfile(filedir, 'bus_SL_OUT.m'))
diff --git a/appinterface_ros/ros_rt_interface/ros_rt_core/SL_func.h b/appinterface_ros/ros_rt_interface/ros_rt_core/SL_func.h
new file mode 100644
index 0000000..2894106
--- /dev/null
+++ b/appinterface_ros/ros_rt_interface/ros_rt_core/SL_func.h
@@ -0,0 +1,43 @@
+/*
+Definition der Ein- und Ausgabedaten für die S-Function der Simulink-Schnittstelle
+Muss identisch auf der Anwendungsseite existieren
+
+* Lucas Jürgens, BA bei Moritz Schappler, 2017-04
+* (C) Institut für Regelungstechnik, Leibniz Universität Hannover
+* moritz.schappler@imes.uni-hannover.de
+*/
+
+#ifndef SL_FUNC_H
+#define SL_FUNC_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct{
+ double q_set[2];
+ double qd_set[2];
+
+ double param_D[2];
+ double param_K[2];
+
+ unsigned char state;
+
+} SL_IN_type;
+
+typedef struct{
+ double q_meas[2];
+ double qd_meas[2];
+ unsigned char sl_state;
+
+} SL_OUT_type;
+
+void SL_io_func(SL_OUT_type* sl_out, SL_IN_type* sl_in);
+void SL_start_func();
+void SL_terminate_func();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/appinterface_ros/ros_rt_interface/ros_rt_core/SL_func_dummy.cpp b/appinterface_ros/ros_rt_interface/ros_rt_core/SL_func_dummy.cpp
new file mode 100644
index 0000000..f574044
--- /dev/null
+++ b/appinterface_ros/ros_rt_interface/ros_rt_core/SL_func_dummy.cpp
@@ -0,0 +1,20 @@
+/*
+Dummy-Code zur Definition der Funktionen für die Simulink-S-Function.
+Zum Kompilierzeitpunkt muss der Inhalt der Funktionen nicht bekannt sein.
+Die Funktionen werden später über eine Programmbibliothek eingebunden.
+
+* Lucas Jürgens, BA bei Moritz Schappler, 2017-04
+* (C) Institut für Regelungstechnik, Leibniz Universität Hannover
+* moritz.schappler@imes.uni-hannover.de
+*/
+
+#ifdef DUMMY
+
+#include "SL_func.h"
+
+
+void SL_io_func(SL_OUT_type* sl_out, SL_IN_type* sl_in){}
+void SL_start_func(){}
+void SL_terminate_func(){}
+
+#endif
diff --git a/appinterface_ros/ros_rt_interface/ros_rt_core/bus_SL_IN.m b/appinterface_ros/ros_rt_interface/ros_rt_core/bus_SL_IN.m
new file mode 100644
index 0000000..0589849
--- /dev/null
+++ b/appinterface_ros/ros_rt_interface/ros_rt_core/bus_SL_IN.m
@@ -0,0 +1,39 @@
+function cellInfo = bus_SL_IN(varargin)
+% BUS_SL_IN returns a cell array containing bus object information
+%
+% Optional Input: 'false' will suppress a call to Simulink.Bus.cellToObject
+% when the MATLAB file is executed.
+% The order of bus element attributes is as follows:
+% ElementName, Dimensions, DataType, SampleTime, Complexity, SamplingMode, DimensionsMode, Min, Max, DocUnits, Description
+
+% Lucas Jürgens (BA), lucas.juergens@zubox.de, 2017-02
+% Moritz Schappler, schappler@irt.uni-hannover.de
+% (C) Institut für Regelungstechnik, Universität Hannover
+
+suppressObject = false;
+if nargin == 1 && islogical(varargin{1}) && varargin{1} == false
+ suppressObject = true;
+elseif nargin > 1
+ error('Invalid input argument(s) encountered');
+end
+
+cellInfo = { ...
+ { ...
+ 'SL_IN_type', ...
+ 'SL_func.h', ...
+ '', ...
+ 'Auto', ...
+ '-1', {...
+{'state', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ...
+{'param_D', 2, 'double', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ...
+{'param_K', 2, 'double', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ...
+{'q_set', 2, 'double', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ...
+{'qd_set', 2, 'double', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ...
+} ...
+ } ...
+}';
+
+if ~suppressObject
+ % Create bus objects in the MATLAB base workspace
+ Simulink.Bus.cellToObject(cellInfo)
+end
diff --git a/appinterface_ros/ros_rt_interface/ros_rt_core/bus_SL_OUT.m b/appinterface_ros/ros_rt_interface/ros_rt_core/bus_SL_OUT.m
new file mode 100644
index 0000000..f7a652d
--- /dev/null
+++ b/appinterface_ros/ros_rt_interface/ros_rt_core/bus_SL_OUT.m
@@ -0,0 +1,37 @@
+function cellInfo = bus_SL_OUT(varargin)
+% BUS_SL_OUT returns a cell array containing bus object information
+%
+% Optional Input: 'false' will suppress a call to Simulink.Bus.cellToObject
+% when the MATLAB file is executed.
+% The order of bus element attributes is as follows:
+% ElementName, Dimensions, DataType, SampleTime, Complexity, SamplingMode, DimensionsMode, Min, Max, DocUnits, Description
+
+% Lucas Jürgens (BA), lucas.juergens@zubox.de, 2017-02
+% Moritz Schappler, schappler@irt.uni-hannover.de
+% (C) Institut für Regelungstechnik, Universität Hannover
+
+suppressObject = false;
+if nargin == 1 && islogical(varargin{1}) && varargin{1} == false
+ suppressObject = true;
+elseif nargin > 1
+ error('Invalid input argument(s) encountered');
+end
+
+cellInfo = { ...
+ { ...
+ 'SL_OUT_type', ...
+ 'SL_func.h', ...
+ '', ...
+ 'Auto', ...
+ '-1', {...
+{'sl_state', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ...
+{'q_meas', 2, 'double', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ...
+{'qd_meas', 2, 'double', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ...
+ } ...
+ } ...
+}';
+
+if ~suppressObject
+ % Create bus objects in the MATLAB base workspace
+ Simulink.Bus.cellToObject(cellInfo)
+end
diff --git a/appinterface_ros/sync.sh b/appinterface_ros/sync.sh
new file mode 100755
index 0000000..4bfa115
--- /dev/null
+++ b/appinterface_ros/sync.sh
@@ -0,0 +1,14 @@
+#!/bin/bash -e
+# Beispiel auf Testrechner kopieren.
+# Der Rechner imessmartrtpc1_ec ist in der ~/.ssh/config eingetragen
+# Dieses Skript muss aus dem Verzeichnis aufgerufen werden, in dem es liegt.
+
+# Moritz Schappler, moritz.schappler@imes.uni-hannover.de, 2020-03
+# (C) Institut für Mechatronische Systeme, Leibniz Universität Hannover
+
+# Simulink-Modell kopieren
+scp appint_ros_example imessmartrtpc1_ec:rtmdl/
+# ROS-Workspace (Installationsordner) kopieren
+ssh imessmartrtpc1_ec 'mkdir -p ~/app_interface/ros_install'
+cd catkin_ws
+rsync -rltv --delete install scripts imessmartrtpc1_ec:~/app_interface/ros_install