Add robot components to lib

Manifest.toml

@@ -689,9 +689,9 @@ uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 
 [[deps.LinearSolve]]
 deps = ["ArrayInterface", "ConcreteStructs", "DocStringExtensions", "EnumX", "EnzymeCore", "FastLapackInterface", "GPUArraysCore", "InteractiveUtils", "KLU", "Krylov", "Libdl", "LinearAlgebra", "MKL", "MKL_jll", "PrecompileTools", "Preferences", "RecursiveFactorization", "Reexport", "Requires", "SciMLBase", "SciMLOperators", "Setfield", "SparseArrays", "Sparspak", "SuiteSparse", "UnPack"]
-git-tree-sha1 = "84bdad5fb1fe03a6637ad413e0e4b7e48ac22be5"
+git-tree-sha1 = "34082ca62d07e9d4ae04659f616b3674a321f740"
 uuid = "7ed4a6bd-45f5-4d41-b270-4a48e9bafcae"
-version = "2.16.2"
+version = "2.17.1"
 
     [deps.LinearSolve.extensions]
     LinearSolveBandedMatricesExt = "BandedMatrices"

docs/make.jl

@@ -28,6 +28,7 @@ makedocs(;
                  "Spherical pendulum" => "examples/spherical_pendulum.md",
                  "Gearbox" => "examples/gearbox.md",
                  "Free motions" => "examples/free_motion.md",
+                 "Industrial robot" => "examples/robot.md",
              ],
              "3D rendering" => "rendering.md",
          ])

docs/src/examples/robot.md

@@ -0,0 +1,79 @@
+# Industrial robot
+
+![animation](robot.gif)
+
+```@example robot
+using Multibody
+using ModelingToolkit
+using Plots
+using JuliaSimCompiler
+using OrdinaryDiffEq
+using Test
+
+t = Multibody.t
+D = Differential(t)
+@named robot = Multibody.Robot6DOF(trivial=true)
+robot = complete(robot)
+show(stdout, MIME"text/plain"(), robot)
+```
+The robot is a medium sized system with some 3000 variables before simplification.
+
+After simplification, the following states are chosen:
+```@example robot
+ssys = structural_simplify(IRSystem(robot))
+states(ssys)
+```
+
+```@example robot
+prob = ODEProblem(ssys, [
+    robot.mechanics.r1.phi => deg2rad(-60)
+    robot.mechanics.r2.phi => deg2rad(20)
+    robot.mechanics.r3.phi => deg2rad(90)
+    robot.mechanics.r4.phi => deg2rad(0)
+    robot.mechanics.r5.phi => deg2rad(-110)
+    robot.mechanics.r6.phi => deg2rad(0)
+], (0.0, 4.0))
+sol = solve(prob, Rodas5P(autodiff=false));
+@test SciMLBase.successful_retcode(sol)
+
+plot(sol, idxs = [
+    robot.pathPlanning.controlBus.axisControlBus1.angle_ref
+    robot.pathPlanning.controlBus.axisControlBus2.angle_ref
+    robot.pathPlanning.controlBus.axisControlBus3.angle_ref
+    robot.pathPlanning.controlBus.axisControlBus4.angle_ref
+    robot.pathPlanning.controlBus.axisControlBus5.angle_ref
+    robot.pathPlanning.controlBus.axisControlBus6.angle_ref
+], layout=(4,3), size=(800,800), l=(:black, :dash), legend=:outertop, legendfontsize=6)
+plot!(sol, idxs = [
+    robot.pathPlanning.controlBus.axisControlBus1.angle
+    robot.pathPlanning.controlBus.axisControlBus2.angle
+    robot.pathPlanning.controlBus.axisControlBus3.angle
+    robot.pathPlanning.controlBus.axisControlBus4.angle
+    robot.pathPlanning.controlBus.axisControlBus5.angle
+    robot.pathPlanning.controlBus.axisControlBus6.angle
+], sp=1:6)
+
+plot!(sol, idxs = [
+    robot.axis1.controller.feedback1.output.u
+    robot.axis2.controller.feedback1.output.u
+    robot.axis3.controller.feedback1.output.u
+    robot.axis4.controller.feedback1.output.u
+    robot.axis5.controller.feedback1.output.u
+    robot.axis6.controller.feedback1.output.u
+], sp=7:12, lab="Position error", link=:x)
+plot!(xlabel=[fill("", 1, 9) fill("Time [s]", 1, 3)])
+```
+We see that after an initial transient, the robot controller converges to tracking the reference trajectory well.
+
+
+
+## 3D animation
+Multibody.jl supports automatic 3D rendering of mechanisms, we use this feature to illustrate the result of the simulation below:
+
+```@example robot
+import CairoMakie
+Multibody.render(robot, sol; z = -5, filename = "robot.gif")
+nothing # hide
+```
+
+![animation](robot.gif)

src/Multibody.jl

@@ -135,4 +135,10 @@ include("forces.jl")
 export PartialCutForceBaseSensor, BasicCutForce, BasicCutTorque, CutTorque, CutForce
 include("sensors.jl")
 
+include("robot/path_planning.jl")
+include("robot/robot_components.jl")
+include("robot/FullRobot.jl")
+
+
+
 end

examples/robot/FullRobot.jl → src/robot/FullRobot.jl

@@ -1,7 +1,82 @@
-include("utilities/components.jl")
-include("utilities/path_planning.jl")
+"""
+    RobotAxis(; name, mLoad = 15, kp = 5, ks = 0.5, Ts = 0.05, startAngle = 0, endAngle = 120, swingTime = 0.5, refSpeedMax = 3, refAccMax = 10, kwargs)
 
-function FullRobot(; name, kwargs...)
+A single robot axis.
+
+- `mLoad`: Mass of load
+- `kp`: Proportional gain of position controller
+- `ks`: Proportional gain of velocity controller
+- `Ts`: Time constant of integrator of velocity controller
+- `startAngle`: 
+- `endAngle`: 
+"""
+function RobotAxis(; name, mLoad = 15, kp = 5.0, ks = 0.5, Ts = 0.05, startAngle = 0,
+    endAngle = 120, swingTime = 0.5, refSpeedMax = 3, refAccMax = 10, kwargs...)
+
+    # 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
+                            kwargs...
+                            )
+    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
+
+
+"""
+    Robot6DOF(; name, kwargs)
+
+A model of a 6DOF serial industrial robot with revolute joints and cascade P/PI position/velocity controllers.
+"""
+function Robot6DOF(; name, kwargs...)
     @parameters begin
         # mLoad = 15, [description = "Mass of load"]
         # rLoad[1:3] = [0.1, 0.25, 0.1],