Implementation of 6DOF robot

Project.toml

@@ -4,17 +4,17 @@ authors = ["JuliaHub Inc."]
 version = "0.1.0"
 
 [deps]
+DataInterpolations = "82cc6244-b520-54b8-b5a6-8a565e85f1d0"
+JuliaSimCompiler = "8391cb6b-4921-5777-4e45-fd9aab8cb88d"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
 ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
-SymbolicIR = "f7e81a98-b176-4533-a916-1afd54a23a03"
 
 [compat]
 ModelingToolkit = "8.30"
-ModelingToolkitStandardLibrary = "1.8"
+ModelingToolkitStandardLibrary = "2"
 Rotations = "1.4"
-SymbolicIR = "0.1"
 julia = "1"
 
 [extras]

examples/robot/FullRobot.jl

@@ -0,0 +1,196 @@
+include("utilities/components.jl")
+include("utilities/path_planning.jl")
+
+function FullRobot(; name)
+    @parameters begin
+        # mLoad = 15, [description = "Mass of load"]
+        # rLoad[1:3] = [0.1, 0.25, 0.1],
+        #              [description = "Distance from last flange to load mass"]
+        # g = 9.81, [description = "Gravity acceleration"]
+        # refStartTime = 0, [description = "Start time of reference motion"]
+        # refSwingTime = 0.5,
+        #                [
+        #                    description = "Additional time after reference motion is in rest before simulation is stopped",
+        #                ]
+        # startAngle1 = -60, [description = "Start angle of axis 1"]
+        # startAngle2 = 20, [description = "Start angle of axis 2"]
+        # startAngle3 = 90, [description = "Start angle of axis 3"]
+        # startAngle4 = 0, [description = "Start angle of axis 4"]
+        # startAngle5 = -110, [description = "Start angle of axis 5"]
+        # startAngle6 = 0, [description = "Start angle of axis 6"]
+        # endAngle1 = 60, [description = "End angle of axis 1"]
+        # endAngle2 = -70, [description = "End angle of axis 2"]
+        # endAngle3 = -35, [description = "End angle of axis 3"]
+        # endAngle4 = 45, [description = "End angle of axis 4"]
+        # endAngle5 = 110, [description = "End angle of axis 5"]
+        # endAngle6 = 45, [description = "End angle of axis 6"]
+        # refSpeedMax[1:6] = [3, 1.5, 5, 3.1, 3.1, 4.1],
+        #                    [description = "Maximum reference speeds of all joints"]
+        # refAccMax[1:6] = [15, 15, 15, 60, 60, 60],
+        #                  [description = "Maximum reference accelerations of all joints"]
+        # kp1 = 5, [description = "Gain of position controller"]
+        # ks1 = 0.5, [description = "Gain of speed controller"]
+        # Ts1 = 0.05, [description = "Time constant of integrator of speed controller"]
+        # kp2 = 5, [description = "Gain of position controller"]
+        # ks2 = 0.5, [description = "Gain of speed controller"]
+        # Ts2 = 0.05, [description = "Time constant of integrator of speed controller"]
+        # kp3 = 5, [description = "Gain of position controller"]
+        # ks3 = 0.5, [description = "Gain of speed controller"]
+        # Ts3 = 0.05, [description = "Time constant of integrator of speed controller"]
+        # kp4 = 5, [description = "Gain of position controller"]
+        # ks4 = 0.5, [description = "Gain of speed controller"]
+        # Ts4 = 0.05, [description = "Time constant of integrator of speed controller"]
+        # kp5 = 5, [description = "Gain of position controller"]
+        # ks5 = 0.5, [description = "Gain of speed controller"]
+        # Ts5 = 0.05, [description = "Time constant of integrator of speed controller"]
+        # kp6 = 5, [description = "Gain of position controller"]
+        # ks6 = 0.5, [description = "Gain of speed controller"]
+        # Ts6 = 0.05, [description = "Time constant of integrator of speed controller"]
+    end
+
+    mLoad = 15 #, [description = "Mass of load"]
+    rLoad = [0.1, 0.25, 0.1] #,                   [description = "Distance from last flange to load mass"]
+    g = 9.81 #, [description = "Gravity acceleration"]
+    refStartTime = 0 #, [description = "Start time of reference motion"]
+    refSwingTime = 0.5 #, [description = "Additional time after reference motion is in rest before simulation is stopped",                   ]
+    refSpeedMax = [3, 1.5, 5, 3.1, 3.1, 4.1]    #  [description = "Maximum reference speeds of all joints"]
+    refAccMax = [15, 15, 15, 60, 60, 60]  #  [description = "Maximum reference accelerations of all joints"]
+    kp1 = 5 #, [description = "Gain of position controller"]
+    ks1 = 0.5 #, [description = "Gain of speed controller"]
+    Ts1 = 0.05 #, [description = "Time constant of integrator of speed controller"]
+    kp2 = 5 #, [description = "Gain of position controller"]
+    ks2 = 0.5 #, [description = "Gain of speed controller"]
+    Ts2 = 0.05 #, [description = "Time constant of integrator of speed controller"]
+    kp3 = 5 #, [description = "Gain of position controller"]
+    ks3 = 0.5 #, [description = "Gain of speed controller"]
+    Ts3 = 0.05 #, [description = "Time constant of integrator of speed controller"]
+    kp4 = 5 #, [description = "Gain of position controller"]
+    ks4 = 0.5 #, [description = "Gain of speed controller"]
+    Ts4 = 0.05 #, [description = "Time constant of integrator of speed controller"]
+    kp5 = 5 #, [description = "Gain of position controller"]
+    ks5 = 0.5 #, [description = "Gain of speed controller"]
+    Ts5 = 0.05 #, [description = "Time constant of integrator of speed controller"]
+    kp6 = 5 #, [description = "Gain of position controller"]
+    ks6 = 0.5 #, [description = "Gain of speed controller"]
+    Ts6 = 0.05 #, [description = "Time constant of integrator of speed controller"]
+
+    startAngle1 = -60 # Can't yet have these as parameters
+    startAngle2 = 20 # Can't yet have these as parameters
+    startAngle3 = 90 # Can't yet have these as parameters
+    startAngle4 = 0 # Can't yet have these as parameters
+    startAngle5 = -110 # Can't yet have these as parameters
+    startAngle6 = 0 # Can't yet have these as parameters
+    endAngle1 = 60 # Can't yet have these as parameters
+    endAngle2 = -70 # Can't yet have these as parameters
+    endAngle3 = -35 # Can't yet have these as parameters
+    endAngle4 = 45 # Can't yet have these as parameters
+    endAngle5 = 110 # Can't yet have these as parameters
+    endAngle6 = 45 # Can't yet have these as parameters
+
+    systems = @named begin
+        mechanics = MechanicalStructure(mLoad = (mLoad),
+                                        rLoad = (rLoad),
+                                        g = (g))
+        pathPlanning = PathPlanning6(naxis = 6,
+                                     angleBegDeg = [
+                                         startAngle1,
+                                         startAngle2,
+                                         startAngle3,
+                                         startAngle4,
+                                         startAngle5,
+                                         startAngle6,
+                                     ],
+                                     angleEndDeg = [
+                                         endAngle1,
+                                         endAngle2,
+                                         endAngle3,
+                                         endAngle4,
+                                         endAngle5,
+                                         endAngle6,
+                                     ],
+                                     speedMax = refSpeedMax,
+                                     accMax = refAccMax,
+                                     startTime = refStartTime,
+                                     swingTime = refSwingTime)
+
+        axis1 = AxisType1(w = 4590,
+                          ratio = -105,
+                          c = 43,
+                          cd = 0.005,
+                          Rv0 = 0.4,
+                          Rv1 = (0.13 / 160),
+                          kp = kp1,
+                          ks = ks1,
+                          Ts = Ts1)
+        axis2 = AxisType1(w = 5500,
+                          ratio = 210,
+                          c = 8,
+                          cd = 0.01,
+                          Rv1 = (0.1 / 130),
+                          Rv0 = 0.5,
+                          kp = kp2,
+                          ks = ks2,
+                          Ts = Ts2)
+
+        axis3 = AxisType1(w = 5500,
+                          ratio = 60,
+                          c = 58,
+                          cd = 0.04,
+                          Rv0 = 0.7,
+                          Rv1 = (0.2 / 130),
+                          kp = kp3,
+                          ks = ks3,
+                          Ts = Ts3)
+        axis4 = AxisType2(k = 0.2365,
+                          w = 6250,
+                          D = 0.55,
+                          J = 1.6e-4,
+                          ratio = -99,
+                          Rv0 = 21.8,
+                          Rv1 = 9.8,
+                          peak = 26.7 / 21.8,
+                          kp = kp4,
+                          ks = ks4,
+                          Ts = Ts4)
+        axis5 = AxisType2(k = 0.2608,
+                          w = 6250,
+                          D = 0.55,
+                          J = 1.8e-4,
+                          ratio = 79.2,
+                          Rv0 = 30.1,
+                          Rv1 = 0.03,
+                          peak = 39.6 / 30.1,
+                          kp = kp5,
+                          ks = ks5,
+                          Ts = Ts5)
+        axis6 = AxisType2(k = 0.0842,
+                          w = 7400,
+                          D = 0.27,
+                          J = 4.3e-5,
+                          ratio = -99,
+                          Rv0 = 10.9,
+                          Rv1 = 3.92,
+                          peak = 16.8 / 10.9,
+                          kp = kp6,
+                          ks = ks6,
+                          Ts = Ts6)
+
+        controlBus = ControlBus()
+    end
+
+    eqs = [connect(axis2.flange, mechanics.axis2)
+           connect(axis1.flange, mechanics.axis1)
+           connect(axis3.flange, mechanics.axis3)
+           connect(axis4.flange, mechanics.axis4)
+           connect(axis5.flange, mechanics.axis5)
+           connect(axis6.flange, mechanics.axis6)
+           connect(controlBus, pathPlanning.controlBus)
+           connect(controlBus.axisControlBus1, axis1.axisControlBus)
+           connect(controlBus.axisControlBus2, axis2.axisControlBus)
+           connect(controlBus.axisControlBus3, axis3.axisControlBus)
+           connect(controlBus.axisControlBus4, axis4.axisControlBus)
+           connect(controlBus.axisControlBus5, axis5.axisControlBus)
+           connect(controlBus.axisControlBus6, axis6.axisControlBus)]
+
+    ODESystem(eqs, t; systems, name)
+end

examples/robot/OneAxis.jl

@@ -0,0 +1,60 @@
+include("utilities/components.jl")
+
+function OneAxis(; name, mLoad = 15, kp = 5, ks = 0.5, Ts = 0.05, startAngle = 0,
+                 endAngle = 120, swingTime = 0.5, refSpeedMax = 3, refAccMax = 10)
+
+    # parameter SI.Mass mLoad(min=0)=15 "Mass of load";
+    # parameter Real kp=5 "Gain of position controller of axis";
+    # parameter Real ks=0.5 "Gain of speed controller of axis";
+    # parameter SI.Time Ts=0.05
+    #   "Time constant of integrator of speed controller of axis";
+    # parameter Modelica.Units.NonSI.Angle_deg startAngle = 0 "Start angle of axis";
+    # parameter Modelica.Units.NonSI.Angle_deg endAngle = 120 "End angle of axis";
+
+    # parameter SI.Time swingTime=0.5
+    #   "Additional time after reference motion is in rest before simulation is stopped";
+    # parameter SI.AngularVelocity refSpeedMax=3 "Maximum reference speed";
+    # parameter SI.AngularAcceleration refAccMax=10
+    #   "Maximum reference acceleration";
+
+    @parameters begin
+        mLoad = mLoad, [description = "Mass of load"]
+        kp = kp, [description = "Gain of position controller of axis"]
+        ks = ks, [description = "Gain of speed controller of axis"]
+        Ts = Ts, [description = "Time constant of integrator of speed controller of axis"]
+        # startAngle = startAngle, [description = "Start angle of axis"]
+        # endAngle = endAngle, [description = "End angle of axis"]
+        swingTime = swingTime,
+                    [
+                        description = "Additional time after reference motion is in rest before simulation is stopped",
+                    ]
+        # refSpeedMax = refSpeedMax, [description = "Maximum reference speed"]
+        # refAccMax = refAccMax, [description = "Maximum reference acceleration"]
+    end
+
+    systems = @named begin
+        axis = AxisType1(w = 5500,
+                         ratio = 210,
+                         c = 8,
+                         cd = 0.01,
+                         Rv0 = 0.5,
+                         Rv1 = (0.1 / 130),
+                         kp = kp,
+                         ks = ks,
+                         Ts = Ts)
+        load = Rotational.Inertia(J = 1.3 * mLoad)
+        pathPlanning = PathPlanning1(swingTime = swingTime,
+                                     angleBegDeg = startAngle,
+                                     angleEndDeg = endAngle
+                                     #  speedMax = refSpeedMax,
+                                     #  accMax = refAccMax
+                                     )
+        controlBus = ControlBus()
+    end
+    eqs = [
+        connect(axis.flange, load.flange_a),
+        connect(pathPlanning.controlBus, controlBus),
+        connect(controlBus.axisControlBus1, axis.axisControlBus),
+    ]
+    ODESystem(eqs, t; systems, name)
+end

examples/robot/test_components.jl

@@ -0,0 +1,41 @@
+using ModelingToolkit
+using Multibody
+using Test
+using SymbolicIR
+t = Multibody.t
+
+cd(@__DIR__)
+world = Multibody.world
+include("OneAxis.jl")
+include("FullRobot.jl")
+@named structure = MechanicalStructure()
+@named motor = Motor()
+@named controller = Controller()
+@named axis2 = AxisType2()
+@named gear2 = GearType2()
+# @named axis1 = AxisType1()
+@named gear1 = GearType1()
+
+@named pp = PathPlanning1(;)
+@named pp6 = PathPlanning6(;)
+
+@named oneaxis = OneAxis()
+
+ssys = structural_simplify(IRSystem(oneaxis))
+ssys = structural_simplify(oneaxis, allow_parameters = false)
+
+@named robot = FullRobot()
+
+ssys = structural_simplify(robot, allow_parameters = false)
+ssys = structural_simplify(IRSystem(robot))
+
+
+
+dummyder = setdiff(states(ssys), states(oneaxis))
+op = merge(ModelingToolkit.defaults(oneaxis), Dict(x => 0.0 for x in dummyder))
+prob = ODEProblem(ssys, op, (0.0, 10.0))
+
+using OrdinaryDiffEq
+sol = solve(prob, Rodas4(), u0=prob.u0 .+ 0.01.*randn.())
+
+