diff --git a/docs/Manifest.toml b/docs/Manifest.toml index 3c6792ae..4ee023d2 100644 --- a/docs/Manifest.toml +++ b/docs/Manifest.toml @@ -2,12 +2,12 @@ julia_version = "1.10.5" manifest_format = "2.0" -project_hash = "06f3e6980a6e6c68713c93d9095c9cb5de3db7cf" +project_hash = "08003e42b5c1441ed313cb0f744985213930fbba" [[deps.ADTypes]] -git-tree-sha1 = "5a5eafb8344b81b8c2237f8a6f6b3602b3f6180e" +git-tree-sha1 = "eea5d80188827b35333801ef97a40c2ed653b081" uuid = "47edcb42-4c32-4615-8424-f2b9edc5f35b" -version = "1.8.1" +version = "1.9.0" weakdeps = ["ChainRulesCore", "EnzymeCore"] [deps.ADTypes.extensions] @@ -331,6 +331,36 @@ deps = ["Artifacts", "Libdl"] uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" version = "1.1.1+0" +[[deps.ComponentArrays]] +deps = ["ArrayInterface", "ChainRulesCore", "ForwardDiff", "Functors", "LinearAlgebra", "PackageExtensionCompat", "StaticArrayInterface", "StaticArraysCore"] +git-tree-sha1 = "bc391f0c19fa242fb6f71794b949e256cfa3772c" +uuid = "b0b7db55-cfe3-40fc-9ded-d10e2dbeff66" +version = "0.15.17" + + [deps.ComponentArrays.extensions] + ComponentArraysAdaptExt = "Adapt" + ComponentArraysConstructionBaseExt = "ConstructionBase" + ComponentArraysGPUArraysExt = "GPUArrays" + ComponentArraysOptimisersExt = "Optimisers" + ComponentArraysRecursiveArrayToolsExt = "RecursiveArrayTools" + ComponentArraysReverseDiffExt = "ReverseDiff" + ComponentArraysSciMLBaseExt = "SciMLBase" + ComponentArraysTrackerExt = "Tracker" + ComponentArraysTruncatedStacktracesExt = "TruncatedStacktraces" + ComponentArraysZygoteExt = "Zygote" + + [deps.ComponentArrays.weakdeps] + Adapt = "79e6a3ab-5dfb-504d-930d-738a2a938a0e" + ConstructionBase = "187b0558-2788-49d3-abe0-74a17ed4e7c9" + GPUArrays = "0c68f7d7-f131-5f86-a1c3-88cf8149b2d7" + Optimisers = "3bd65402-5787-11e9-1adc-39752487f4e2" + RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" + ReverseDiff = "37e2e3b7-166d-5795-8a7a-e32c996b4267" + SciMLBase = "0bca4576-84f4-4d90-8ffe-ffa030f20462" + Tracker = "9f7883ad-71c0-57eb-9f7f-b5c9e6d3789c" + TruncatedStacktraces = "781d530d-4396-4725-bb49-402e4bee1e77" + Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f" + [[deps.CompositeTypes]] git-tree-sha1 = "bce26c3dab336582805503bed209faab1c279768" uuid = "b152e2b5-7a66-4b01-a709-34e65c35f657" @@ -372,6 +402,25 @@ git-tree-sha1 = "439e35b0b36e2e5881738abc8857bd92ad6ff9a8" uuid = "d38c429a-6771-53c6-b99e-75d170b6e991" version = "0.6.3" +[[deps.ControlSystemsBase]] +deps = ["DSP", "ForwardDiff", "IterTools", "LaTeXStrings", "LinearAlgebra", "MacroTools", "MatrixEquations", "MatrixPencils", "Polyester", "Polynomials", "PrecompileTools", "Printf", "Random", "RecipesBase", "SparseArrays", "StaticArraysCore", "UUIDs"] +git-tree-sha1 = "163d11b7aee72d66c3e2e91f2aa48bb709f8439f" +uuid = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" +version = "1.10.3" + + [deps.ControlSystemsBase.extensions] + ControlSystemsBaseImplicitDifferentiationExt = ["ImplicitDifferentiation", "ComponentArrays"] + + [deps.ControlSystemsBase.weakdeps] + ComponentArrays = "b0b7db55-cfe3-40fc-9ded-d10e2dbeff66" + ImplicitDifferentiation = "57b37032-215b-411a-8a7c-41a003a55207" + +[[deps.ControlSystemsMTK]] +deps = ["ControlSystemsBase", "DataInterpolations", "LinearAlgebra", "ModelingToolkit", "ModelingToolkitStandardLibrary", "MonteCarloMeasurements", "RobustAndOptimalControl", "Symbolics", "UnPack"] +git-tree-sha1 = "0a2169c781c3aadca3e38c424401f9b838c778c7" +uuid = "687d7614-c7e5-45fc-bfc3-9ee385575c88" +version = "2.2.1" + [[deps.CoordinateTransformations]] deps = ["LinearAlgebra", "StaticArrays"] git-tree-sha1 = "f9d7112bfff8a19a3a4ea4e03a8e6a91fe8456bf" @@ -389,6 +438,12 @@ git-tree-sha1 = "249fe38abf76d48563e2f4556bebd215aa317e15" uuid = "a8cc5b0e-0ffa-5ad4-8c14-923d3ee1735f" version = "4.1.1" +[[deps.DSP]] +deps = ["Compat", "FFTW", "IterTools", "LinearAlgebra", "Polynomials", "Random", "Reexport", "SpecialFunctions", "Statistics"] +git-tree-sha1 = "f7f4319567fe769debfcf7f8c03d8da1dd4e2fb0" +uuid = "717857b8-e6f2-59f4-9121-6e50c889abd2" +version = "0.7.9" + [[deps.DataAPI]] git-tree-sha1 = "abe83f3a2f1b857aac70ef8b269080af17764bbe" uuid = "9a962f9c-6df0-11e9-0e5d-c546b8b5ee8a" @@ -446,6 +501,12 @@ git-tree-sha1 = "9e2f36d3c96a820c678f2f1f1782582fcf685bae" uuid = "8bb1440f-4735-579b-a4ab-409b98df4dab" version = "1.9.1" +[[deps.DescriptorSystems]] +deps = ["LinearAlgebra", "MatrixEquations", "MatrixPencils", "Polynomials", "Random"] +git-tree-sha1 = "1e32ab7adb1aae200e3f9c18decb3508af3dc33c" +uuid = "a81e2ce2-54d1-11eb-2c75-db236b00f339" +version = "1.4.4" + [[deps.DiffEqBase]] deps = ["ArrayInterface", "ConcreteStructs", "DataStructures", "DocStringExtensions", "EnumX", "EnzymeCore", "FastBroadcast", "FastClosures", "ForwardDiff", "FunctionWrappers", "FunctionWrappersWrappers", "LinearAlgebra", "Logging", "Markdown", "MuladdMacro", "Parameters", "PreallocationTools", "PrecompileTools", "Printf", "RecursiveArrayTools", "Reexport", "SciMLBase", "SciMLOperators", "SciMLStructures", "Setfield", "Static", "StaticArraysCore", "Statistics", "Tricks", "TruncatedStacktraces"] git-tree-sha1 = "fa7d580038451a8df4434ef5b079ac9b2d486194" @@ -568,9 +629,9 @@ uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b" [[deps.Distributions]] deps = ["AliasTables", "FillArrays", "LinearAlgebra", "PDMats", "Printf", "QuadGK", "Random", "SpecialFunctions", "Statistics", "StatsAPI", "StatsBase", "StatsFuns"] -git-tree-sha1 = "e6c693a0e4394f8fda0e51a5bdf5aef26f8235e9" +git-tree-sha1 = "d7477ecdafb813ddee2ae727afa94e9dcb5f3fb0" uuid = "31c24e10-a181-5473-b8eb-7969acd0382f" -version = "0.25.111" +version = "0.25.112" [deps.Distributions.extensions] DistributionsChainRulesCoreExt = "ChainRulesCore" @@ -1176,9 +1237,9 @@ version = "0.1.5" [[deps.JpegTurbo_jll]] deps = ["Artifacts", "JLLWrappers", "Libdl"] -git-tree-sha1 = "c84a835e1a09b289ffcd2271bf2a337bbdda6637" +git-tree-sha1 = "25ee0be4d43d0269027024d75a24c24d6c6e590c" uuid = "aacddb02-875f-59d6-b918-886e6ef4fbf8" -version = "3.0.3+0" +version = "3.0.4+0" [[deps.JuliaFormatter]] deps = ["CSTParser", "CommonMark", "DataStructures", "Glob", "PrecompileTools", "TOML", "Tokenize"] @@ -1428,6 +1489,18 @@ version = "7.3.0" deps = ["Libdl", "OpenBLAS_jll", "libblastrampoline_jll"] uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +[[deps.LinearMaps]] +deps = ["LinearAlgebra"] +git-tree-sha1 = "ee79c3208e55786de58f8dcccca098ced79f743f" +uuid = "7a12625a-238d-50fd-b39a-03d52299707e" +version = "3.11.3" +weakdeps = ["ChainRulesCore", "SparseArrays", "Statistics"] + + [deps.LinearMaps.extensions] + LinearMapsChainRulesCoreExt = "ChainRulesCore" + LinearMapsSparseArraysExt = "SparseArrays" + LinearMapsStatisticsExt = "Statistics" + [[deps.LinearSolve]] deps = ["ArrayInterface", "ChainRulesCore", "ConcreteStructs", "DocStringExtensions", "EnumX", "FastLapackInterface", "GPUArraysCore", "InteractiveUtils", "KLU", "Krylov", "LazyArrays", "Libdl", "LinearAlgebra", "MKL_jll", "Markdown", "PrecompileTools", "Preferences", "RecursiveFactorization", "Reexport", "SciMLBase", "SciMLOperators", "Setfield", "SparseArrays", "Sparspak", "StaticArraysCore", "UnPack"] git-tree-sha1 = "6c5e4555ac2bc449a28604e184f759d18fc08420" @@ -1556,6 +1629,18 @@ git-tree-sha1 = "e1641f32ae592e415e3dbae7f4a188b5316d4b62" uuid = "0a4f8689-d25c-4efe-a92b-7142dfc1aa53" version = "0.6.1" +[[deps.MatrixEquations]] +deps = ["LinearAlgebra", "LinearMaps"] +git-tree-sha1 = "f765b4eda3ea9be8e644b9127809ca5151f3d9ea" +uuid = "99c1a7ee-ab34-5fd5-8076-27c950a045f4" +version = "2.4.2" + +[[deps.MatrixPencils]] +deps = ["LinearAlgebra", "Polynomials", "Random"] +git-tree-sha1 = "c00a086f4f1df792c77dc1bd674357044aa08d74" +uuid = "48965c70-4690-11ea-1f13-43a2532b2fa8" +version = "1.8.0" + [[deps.MaybeInplace]] deps = ["ArrayInterface", "LinearAlgebra", "MacroTools"] git-tree-sha1 = "54e2fdc38130c05b42be423e90da3bade29b74bd" @@ -1618,8 +1703,6 @@ version = "9.41.0" [[deps.ModelingToolkitStandardLibrary]] deps = ["ChainRulesCore", "DiffEqBase", "IfElse", "LinearAlgebra", "ModelingToolkit", "Symbolics"] git-tree-sha1 = "21a278835793f475f8e9ccc0756bb38515473cf1" -repo-rev = "main" -repo-url = "https://github.com/SciML/ModelingToolkitStandardLibrary.jl.git" uuid = "16a59e39-deab-5bd0-87e4-056b12336739" version = "2.14.0" @@ -1629,6 +1712,16 @@ git-tree-sha1 = "b76ea40b5c0f45790ae09492712dd326208c28b2" uuid = "66fc600b-dfda-50eb-8b99-91cfa97b1301" version = "1.1.7" +[[deps.MonteCarloMeasurements]] +deps = ["Distributed", "Distributions", "ForwardDiff", "GenericSchur", "LinearAlgebra", "MacroTools", "Random", "RecipesBase", "Requires", "SLEEFPirates", "StaticArrays", "Statistics", "StatsBase", "Test"] +git-tree-sha1 = "36ccc5e09dbba9aea61d78cd7bc46c5113e6ad84" +uuid = "0987c9cc-fe09-11e8-30f0-b96dd679fdca" +version = "1.2.1" +weakdeps = ["Makie"] + + [deps.MonteCarloMeasurements.extensions] + MakieExt = "Makie" + [[deps.MosaicViews]] deps = ["MappedArrays", "OffsetArrays", "PaddedViews", "StackViews"] git-tree-sha1 = "7b86a5d4d70a9f5cdf2dacb3cbe6d251d1a61dbe" @@ -1646,7 +1739,7 @@ version = "0.2.4" [[deps.Multibody]] deps = ["CoordinateTransformations", "DataInterpolations", "FileIO", "JuliaSimCompiler", "LinearAlgebra", "MeshIO", "ModelingToolkit", "ModelingToolkitStandardLibrary", "Rotations", "SparseArrays", "StaticArrays"] -path = ".." +git-tree-sha1 = "3d0ca08b6fb8ef5190bf112746dba6becc6cda5f" uuid = "e1cad5d1-98ef-44f9-a79a-9ca4547f95b9" version = "0.3.0" @@ -2130,6 +2223,19 @@ git-tree-sha1 = "77b3d3605fc1cd0b42d95eba87dfcd2bf67d5ff6" uuid = "647866c9-e3ac-4575-94e7-e3d426903924" version = "0.1.2" +[[deps.Polynomials]] +deps = ["LinearAlgebra", "RecipesBase", "Requires", "Setfield", "SparseArrays"] +git-tree-sha1 = "1a9cfb2dc2c2f1bd63f1906d72af39a79b49b736" +uuid = "f27b6e38-b328-58d1-80ce-0feddd5e7a45" +version = "4.0.11" +weakdeps = ["ChainRulesCore", "FFTW", "MakieCore", "MutableArithmetics"] + + [deps.Polynomials.extensions] + PolynomialsChainRulesCoreExt = "ChainRulesCore" + PolynomialsFFTWExt = "FFTW" + PolynomialsMakieCoreExt = "MakieCore" + PolynomialsMutableArithmeticsExt = "MutableArithmetics" + [[deps.PositiveFactorizations]] deps = ["LinearAlgebra"] git-tree-sha1 = "17275485f373e6673f7e7f97051f703ed5b15b20" @@ -2361,6 +2467,12 @@ git-tree-sha1 = "58cdd8fb2201a6267e1db87ff148dd6c1dbd8ad8" uuid = "f50d1b31-88e8-58de-be2c-1cc44531875f" version = "0.5.1+0" +[[deps.RobustAndOptimalControl]] +deps = ["ChainRulesCore", "ComponentArrays", "ControlSystemsBase", "DescriptorSystems", "Distributions", "GenericSchur", "LinearAlgebra", "MatrixEquations", "MatrixPencils", "MonteCarloMeasurements", "Optim", "Printf", "Random", "RecipesBase", "Statistics", "UUIDs", "UnPack"] +git-tree-sha1 = "5ae8bb37ef2d7b9357b5112a43e03bb0365d5cf4" +uuid = "21fd56a4-db03-40ee-82ee-a87907bee541" +version = "0.4.31" + [[deps.Rotations]] deps = ["LinearAlgebra", "Quaternions", "Random", "StaticArrays"] git-tree-sha1 = "5680a9276685d392c87407df00d57c9924d9f11e" @@ -2693,9 +2805,9 @@ version = "7.2.1+1" [[deps.SymbolicIndexingInterface]] deps = ["Accessors", "ArrayInterface", "RuntimeGeneratedFunctions", "StaticArraysCore"] -git-tree-sha1 = "988e04b34a4c3b824fb656f542473df99a4f610d" +git-tree-sha1 = "0225f7c62f5f78db35aae6abb2e5cabe38ce578f" uuid = "2efcf032-c050-4f8e-a9bb-153293bab1f5" -version = "0.3.30" +version = "0.3.31" [[deps.SymbolicLimits]] deps = ["SymbolicUtils"] @@ -3087,9 +3199,9 @@ version = "1.2.13+1" [[deps.Zstd_jll]] deps = ["Artifacts", "JLLWrappers", "Libdl"] -git-tree-sha1 = "e678132f07ddb5bfa46857f0d7620fb9be675d3b" +git-tree-sha1 = "555d1076590a6cc2fdee2ef1469451f872d8b41b" uuid = "3161d3a3-bdf6-5164-811a-617609db77b4" -version = "1.5.6+0" +version = "1.5.6+1" [[deps.eudev_jll]] deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg", "gperf_jll"] diff --git a/docs/Project.toml b/docs/Project.toml index 2e868b94..d9bfc821 100644 --- a/docs/Project.toml +++ b/docs/Project.toml @@ -1,4 +1,6 @@ [deps] +ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" +ControlSystemsMTK = "687d7614-c7e5-45fc-bfc3-9ee385575c88" Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" GLMakie = "e9467ef8-e4e7-5192-8a1a-b1aee30e663a" JuliaSimCompiler = "8391cb6b-4921-5777-4e45-fd9aab8cb88d" @@ -8,3 +10,4 @@ ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739" Multibody = "e1cad5d1-98ef-44f9-a79a-9ca4547f95b9" OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed" Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" +RobustAndOptimalControl = "21fd56a4-db03-40ee-82ee-a87907bee541" diff --git a/docs/src/examples/pendulum.md b/docs/src/examples/pendulum.md index 6d89e879..a35c1379 100644 --- a/docs/src/examples/pendulum.md +++ b/docs/src/examples/pendulum.md @@ -310,3 +310,218 @@ r_A = [r_A; 1] # Homogeneous coordinates get_frame(sol, model.lower_arm.frame_a, 12)*r_A ``` the vector is now coinciding with `get_trans(sol, model.lower_arm.frame_b, 12)`. + + +## Control-design example: Pendulum on cart +We will now demonstrate a complete workflow including +- Modeling +- Linearizaiton +- Control design + +We will continue the pendulum theme and design an inverted pendulum on cart. The cart is modeled as [`BodyShape`](@ref) with specified mass, and `shape = "box"` to render it as a box in animations. The cart is moving along the $x$-axis by means of a [`Prismatic`](@ref) joint, and the pendulum is attached to the cart by means of a [`Revolute`](@ref) joint. The pendulum is a [`BodyCylinder`](@ref) with a diameter of `0.015`, the mass and inertia properties are automatically computed using the geometrical dimensions and the density (which defaults to that of steel). A force is applied to the cart by means of a `TranslationalModelica.Force` component. + +We start by putting the model together and control it in open loop using a simple periodic input signal: + +```@example pendulum +import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica +import ModelingToolkitStandardLibrary.Blocks +using Plots +W(args...; kwargs...) = Multibody.world +gray = [0.5, 0.5, 0.5, 1] +@mtkmodel Cartpole begin + @components begin + world = W() + cart = BodyShape(m = 1, r = [0.2, 0, 0], color=[0.2, 0.2, 0.2, 1], shape="box") + mounting_point = FixedTranslation(r = [0.1, 0, 0]) + prismatic = Prismatic(n = [1, 0, 0], axisflange = true, color=gray, state_priority=100) + revolute = Revolute(n = [0, 0, 1], axisflange = false, state_priority=100) + pendulum = BodyCylinder(r = [0, 0.5, 0], diameter = 0.015, color=gray) + motor = TranslationalModelica.Force(use_support = true) + tip = Body(m = 0.05) + end + @variables begin + u(t) = 0 + x(t) + v(t) + phi(t) + w(t) + end + @equations begin + connect(world.frame_b, prismatic.frame_a) + connect(prismatic.frame_b, cart.frame_a, mounting_point.frame_a) + connect(mounting_point.frame_b, revolute.frame_a) + connect(revolute.frame_b, pendulum.frame_a) + connect(pendulum.frame_b, tip.frame_a) + connect(motor.flange, prismatic.axis) + connect(prismatic.support, motor.support) + u ~ motor.f.u + x ~ prismatic.s + v ~ prismatic.v + phi ~ revolute.phi + w ~ revolute.w + end +end +@mtkmodel CartWithInput begin + @components begin + cartpole = Cartpole() + input = Blocks.Cosine(frequency=1, amplitude=1) + end + @equations begin + connect(input.output, :u, cartpole.motor.f) + end +end +@named model = CartWithInput() +model = complete(model) +ssys = structural_simplify(IRSystem(model)) +prob = ODEProblem(ssys, [model.cartpole.prismatic.s => 0.0, model.cartpole.revolute.phi => 0.1], (0, 10)) +sol = solve(prob, Tsit5()) +plot(sol, layout=4) +``` +As usual, we render the simulation in 3D to get a better feel for the system: +```@example pendulum +import GLMakie +Multibody.render(model, sol, filename = "cartpole.gif", traces=[model.cartpole.pendulum.frame_b]) +nothing # hide +``` +![cartpole](cartpole.gif) + +### Adding feedback + +We will attempt to stabilize the pendulum in the upright position by using feedback control. To design the contorller, we linearize the model in the upward equilibrium position and design an infinite-horizon LQR controller using ControlSystems.jl. We then connect the controller to the motor on the cart. See also [RobustAndOptimalControl.jl: Control design for a pendulum on a cart](https://juliacontrol.github.io/RobustAndOptimalControl.jl/dev/cartpole/) for a similar example with more detail on the control design. + +### Linearization +We start by linearizing the model in the upward equilibrium position using the function `ModelingToolkit.linearize`. +```@example pendulum +import ModelingToolkit: D_nounits as D +using LinearAlgebra +@named cp = Cartpole() +cp = complete(cp) +inputs = [cp.u] # Input to the linearized system +outputs = [cp.x, cp.phi, cp.v, cp.w] # These are the outputs of the linearized system +op = Dict([ # Operating point to linearize in + cp.u => 0 + cp.revolute.phi => 0 # Pendulum pointing upwards +] +) +matrices, simplified_sys = linearize(IRSystem(cp), inputs, outputs; op) +matrices +``` +This gives us the matrices $A,B,C,D$ in a linearized statespace representation of the system. To make these easier to work with, we load the control packages and call `named_ss` instead of `linearize` to get a named statespace object instead: +```@example pendulum +using ControlSystemsMTK +lsys = named_ss(IRSystem(cp), inputs, outputs; op) # identical to linearize, but packages the resulting matrices in a named statespace object for convenience +``` + +### LQR Control design +With a linear statespace object in hand, we can proceed to design an LQR controller. Since the function `lqr` operates on the state vector, and we have access to the specified output vector, we make use of the system ``C`` matrix to reformulate the problem in terms of the outputs. This relies on the ``C`` matrix being full rank, which is the case here since our outputs include a complete state realization of the system. + +To make the simulation interesting, we make a change in the reference position of the cart after a few seconds. +```@example pendulum +using ControlSystemsBase +C = lsys.C +Q = Diagonal([1, 1, 1, 1]) +R = Diagonal([0.1]) +Lmat = lqr(lsys, C'Q*C, R)/C # Compute LQR feedback gain. The multiplication by the C matrix is to handle the difference between state and output + +@mtkmodel CartWithFeedback begin + @components begin + cartpole = Cartpole() + L = Blocks.MatrixGain(K = Lmat) + reference = Blocks.Step(start_time = 5, height=0.5) + control_saturation = Blocks.Limiter(y_max = 10) # To limit the control signal magnitude + end + begin + namespaced_outputs = ModelingToolkit.renamespace.(:cartpole, outputs) # Give outputs correct namespace, they are variables in the cartpole system + end + @equations begin + L.input.u[1] ~ reference.output.u - namespaced_outputs[1] # reference cart position - cartpole.x + L.input.u[2] ~ 0 - namespaced_outputs[2] # cartpole.phi + L.input.u[3] ~ 0 - namespaced_outputs[3] # cartpole.v + L.input.u[4] ~ 0 - namespaced_outputs[4] # cartpole.w + connect(L.output, control_saturation.input) + connect(control_saturation.output, cartpole.motor.f) + end +end +@named model = CartWithFeedback() +model = complete(model) +ssys = structural_simplify(IRSystem(model)) +prob = ODEProblem(ssys, [model.cartpole.prismatic.s => 0.1, model.cartpole.revolute.phi => 0.35], (0, 10)) +sol = solve(prob, Tsit5()) +cp = model.cartpole +plot(sol, idxs=[cp.prismatic.s, cp.revolute.phi, cp.motor.f.u], layout=3) +plot!(sol, idxs=model.reference.output.u, sp=1, l=(:black, :dash), legend=:bottomright) +``` + +```@example pendulum +Multibody.render(model, sol, filename = "inverted_cartpole.gif", x=1, z=1) +nothing # hide +``` +![inverted cartpole](inverted_cartpole.gif) + + +### Swing up +Below, we add also an energy-based swing-up controller. For more details this kind of swing-up controller, see [Part 7: Control of rotary pendulum using Julia: Swing up control (YouTube)](https://www.youtube.com/watch?v=RhF2NMCYoiw) +```@example pendulum +"Compute total energy, kinetic + potential, for a body rotating around the z-axis of the world" +function energy(body, w) + g = world.g + m = body.m + d2 = body.r_cm[1]^2 + body.r_cm[2]^2 # Squared distance from + I = body.I_33 + m*d2 # Parallel axis theorem + r_cm_worldframe = Multibody.resolve1(ori(body.frame_a), body.r_cm)[2] # Rotate the distance from frame_a to the center of mass to the world frame + 1/2*I*w^2 + 2m*g*(body.frame_a.r_0[2] + r_cm_worldframe) # Assuming rotation around the z-axis +end + +normalize_angle(x::Number) = mod(x+3.1415, 2pi)-3.1415 + +@mtkmodel CartWithSwingup begin + @components begin + cartpole = Cartpole() + L = Blocks.MatrixGain(K = Lmat) + control_saturation = Blocks.Limiter(y_max = 12) # To limit the control signal magnitude + end + @variables begin + phi(t) + w(t) + E(t), [description = "Total energy of the pendulum"] + u_swing(t), [description = "Swing-up control signal"] + switching_condition(t)::Bool, [description = "Switching condition that indicates when stabilizing controller is active"] + end + @parameters begin + Er = 3.825676486352941 # Total energy of the cartpole at the top equilibrium position + end + begin + namespaced_outputs = ModelingToolkit.renamespace.(:cartpole, outputs) # Give outputs correct namespace, they are variables in the cartpole system + end + @equations begin + phi ~ normalize_angle(cartpole.phi) + w ~ cartpole.w + E ~ energy(cartpole.pendulum.body, w) + energy(cartpole.tip, w) + u_swing ~ 100*(E - Er)*sign(w*cos(phi-3.1415)) + + L.input.u[1] ~ 0 - namespaced_outputs[1] # - cartpole.x + L.input.u[2] ~ 0 - phi # cartpole.phi but normalized + L.input.u[3] ~ 0 - namespaced_outputs[3] # cartpole.v + L.input.u[4] ~ 0 - namespaced_outputs[4] # cartpole.w + switching_condition ~ abs(phi) < 0.4 + control_saturation.input.u ~ ifelse(switching_condition, L.output.u, u_swing) + connect(control_saturation.output, cartpole.motor.f) + end +end +@named model = CartWithSwingup() +model = complete(model) +ssys = structural_simplify(IRSystem(model)) +cp = model.cartpole +prob = ODEProblem(ssys, [cp.prismatic.s => 0.0, cp.revolute.phi => 0.99pi], (0, 5)) +sol = solve(prob, Tsit5(), dt = 1e-2, adaptive=false) +plot(sol, idxs=[cp.prismatic.s, cp.revolute.phi, cp.motor.f.u, model.E], layout=4) +hline!([0, 2pi], sp=2, l=(:black, :dash), primary=false) +plot!(sol, idxs=[model.switching_condition], sp=2) +``` + + +```@example pendulum +Multibody.render(model, sol, filename = "swingup.gif", x=2, z=2) +nothing # hide +``` +![inverted cartpole](swingup.gif) diff --git a/docs/src/examples/quad.md b/docs/src/examples/quad.md index 55e6aa78..c5b8d969 100644 --- a/docs/src/examples/quad.md +++ b/docs/src/examples/quad.md @@ -33,38 +33,51 @@ cable_mass = 5 # Mass of the cable. cable_diameter = 0.01 # Diameter of the cable. number_of_links = 5 # Number of links in the cable. -# Controller parameters -kalt = 1 +# PID Controller parameters +kalt = 2.7 Tialt = 3 -Tdalt = 3 +Tdalt = 0.5 -kroll = 0.02 +kroll = 0.2 Tiroll = 100 Tdroll = 1 -kpitch = 0.02 +kpitch = 0.2 Tipitch = 100 Tdpitch = 1 @mtkmodel Thruster begin + @structural_parameters begin + clockwise = true + end @components begin frame_b = Frame() thrust3d = WorldForce(resolve_frame = :frame_b, scale=0.1, radius=0.02) # The thrust force is resolved in the local frame of the thruster. + torque3d = WorldTorque(resolve_frame = :frame_b, scale=0.1, radius=0.02) # Using the thruster also causes a torque around the force axis. thrust = RealInput() end + @parameters begin + torque_constant = 1, [description="Thrust force to torque conversion factor [Nm/N]"] + end @variables begin u(t), [state_priority=1000] + ut(t), [state_priority=1000] end @equations begin thrust3d.force.u[1] ~ 0 thrust3d.force.u[2] ~ thrust.u thrust3d.force.u[3] ~ 0 + torque3d.torque.u[1] ~ 0 + torque3d.torque.u[2] ~ (clockwise ? ut : -ut) + torque3d.torque.u[3] ~ 0 thrust.u ~ u + ut ~ torque_constant*u connect(frame_b, thrust3d.frame_b) + connect(frame_b, torque3d.frame_b) end end -function RotorCraft(; closed_loop = true, addload=true) +function RotorCraft(; closed_loop = true, addload=true, L=nothing, outputs = nothing, pid=false) arms = [ BodyCylinder( r = [arm_length*cos(angle_between_arms*(i-1)), 0, arm_length*sin(angle_between_arms*(i-1))], @@ -76,18 +89,43 @@ function RotorCraft(; closed_loop = true, addload=true) ] @variables begin - y_alt(t) - y_roll(t) - y_pitch(t) + y_alt(t), [state_priority=2] + y_roll(t), [state_priority=2] + y_pitch(t), [state_priority=2] + y_yaw(t), [state_priority=2] + y_forward(t), [state_priority=2] + y_sideways(t), [state_priority=2] + v_alt(t)=0, [state_priority=2] + v_roll(t)=0, [state_priority=2] + v_pitch(t)=0, [state_priority=2] + v_yaw(t)=0, [state_priority=2] + v_forward(t)=0, [state_priority=2] + v_sideways(t)=0, [state_priority=2] + (Ie_alt(t)=0), [description="Integral of altitude error"] + yIe_alt(t) end - thrusters = [Thruster(name = Symbol("thruster$i")) for i = 1:num_arms] - @named body = Body(m = body_mass, state_priority = 0, I_11=0.01, I_22=0.01, I_33=0.01, air_resistance=1, isroot=true) + thrusters = [Thruster(name = Symbol("thruster$i"), clockwise = (i-1) % 2 == 0) for i = 1:num_arms] + @named body = Body(m = body_mass, state_priority = 100, I_11=0.01, I_22=0.01, I_33=0.01, air_resistance=1, isroot=true) + # @named freemotion = FreeMotion(state=true, isroot=true, quat=false) # We use Euler angles to describe the orientation of the rotorcraft. connections = [ + # connect(world.frame_b, freemotion.frame_a) + # connect(freemotion.frame_b, body.frame_a) y_alt ~ body.r_0[2] y_roll ~ body.phi[3] y_pitch ~ body.phi[1] + y_yaw ~ body.phi[2] + y_forward ~ body.r_0[1] + y_sideways ~ body.r_0[3] + v_alt ~ D(body.r_0[2]) + v_roll ~ D(body.phi[3]) + v_pitch ~ D(body.phi[1]) + v_yaw ~ D(body.phi[2]) + v_forward ~ D(body.r_0[1]) + v_sideways ~ D(body.r_0[3]) + D(Ie_alt) ~ y_alt + yIe_alt ~ Ie_alt [connect(body.frame_a, arms[i].frame_a) for i = 1:num_arms] [connect(arms[i].frame_b, thrusters[i].frame_b) for i = 1:num_arms] ] @@ -114,50 +152,50 @@ function RotorCraft(; closed_loop = true, addload=true) end if closed_loop # add controllers - # Mixing matrices for the control signals - @parameters Galt[1:4] = ones(4) # The altitude controller affects all thrusters equally - @parameters Groll[1:4] = [-1,0,1,0] - @parameters Gpitch[1:4] = [0,1,0,-1] - - @named Calt = PID(; k=kalt, Ti=Tialt, Td=Tdalt) - @named Croll = PID(; k=kroll, Ti=Tiroll, Td=Tdroll) - @named Cpitch = PID(; k=kpitch, Ti=Tipitch, Td=Tdpitch) - - uc = Galt*Calt.ctr_output.u + Groll*Croll.ctr_output.u + Gpitch*Cpitch.ctr_output.u - uc = collect(uc) - append!(connections, [thrusters[i].u ~ uc[i] for i = 1:num_arms]) - - append!(connections, [ - Calt.err_input.u ~ -y_alt - Croll.err_input.u ~ -y_roll - Cpitch.err_input.u ~ -y_pitch - ]) - append!(systems, [Calt; Croll; Cpitch]) - - #= - # append!(connections, [thrusters[i].thrust.u ~ feedback_gain.output.u[i] for i = 1:num_arms]) - # append!(connections, [feedback_gain.input.u[i] ~ arms[i].frame_b.r_0[2] for i = 1:num_arms ]) # Connect positions to controller - # append!(connections, [feedback_gain.input.u[i+num_arms] ~ D(arms[i].frame_b.r_0[2]) for i = 1:num_arms]) # Connect velocities to controller - # append!(connections, [feedback_gain.input.u[i+2num_arms] ~ Ie[i] for i = 1:num_arms]) # - # append!(connections, [feedback_gain.input.u[i] ~ body.phi[[1,3][i]] for i = 1:2 ]) # Connect positions to controller - # append!(connections, [feedback_gain.input.u[i+2] ~ body.phid[[1,3][i]] for i = 1:2]) # Connect velocities to controller - # push!(systems, feedback_gain) - =# + if pid + # Mixing matrices for the control signals + @parameters Galt[1:4] = ones(4) # The altitude controller affects all thrusters equally + @parameters Groll[1:4] = [-1,0,1,0] + @parameters Gpitch[1:4] = [0,1,0,-1] + + @named Calt = PID(; k=kalt, Ti=Tialt, Td=Tdalt) + @named Croll = PID(; k=kroll, Ti=Tiroll, Td=Tdroll) + @named Cpitch = PID(; k=kpitch, Ti=Tipitch, Td=Tdpitch) + + uc = Galt*Calt.ctr_output.u + Groll*Croll.ctr_output.u + Gpitch*Cpitch.ctr_output.u + uc = collect(uc) + append!(connections, [thrusters[i].u ~ uc[i] for i = 1:num_arms]) + + append!(connections, [ + Calt.err_input.u ~ -y_alt + Croll.err_input.u ~ -y_roll + Cpitch.err_input.u ~ -y_pitch + ]) + append!(systems, [Calt; Croll; Cpitch]) + else # LQR + @named feedback_gain = Blocks.MatrixGain(K = L) + @named system_outputs = RealOutput(nout=length(outputs)) + @named system_inputs = RealInput(nin=num_arms) + append!(connections, [system_outputs.u[i] ~ outputs[i] for i = 1:length(outputs)]) + append!(connections, [thrusters[i].thrust.u ~ system_inputs.u[i] for i = 1:num_arms]) + push!(connections, connect(system_outputs, :y, feedback_gain.input)) # Connect outputs to controller + push!(connections, connect(feedback_gain.output, :u, system_inputs)) # Connect controller to inputs + push!(systems, feedback_gain) + push!(systems, system_outputs) + push!(systems, system_inputs) + end + end @named model = ODESystem(connections, t; systems) complete(model) end -model = RotorCraft(closed_loop=true, addload=true) +model = RotorCraft(closed_loop=true, addload=true, pid=true) model = complete(model) ssys = structural_simplify(IRSystem(model)) - +# display(unknowns(ssys)) op = [ model.body.v_0[1] => 0; collect(model.cable.joint_2.phi) .=> 0.03; - model.world.g => 2; - # model.body.frame_a.render => true - # model.body.frame_a.radius => 0.01 - # model.body.frame_a.length => 0.1 ] prob = ODEProblem(ssys, op, (0, 20)) @@ -177,3 +215,123 @@ nothing # hide ![quadrotor animation](quadrotor.gif) The green arrows in the animation indicate the force applied by the thrusters. + + +## LQR control design +Below, we demonstrate a workflow where the model is linearized and an LQR controller is designed to stabilize the quadrotor. We linearize the model using the function `named_ss` from [ControlSystemsMTK](https://github.com/JuliaControl/ControlSystemsMTK.jl), this gives us a linear statespace model with named inputs and outputs. We then design an LQR controller using the `lqr` function from [ControlSystems.jl](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/synthesis/#ControlSystemsBase.lqr-Tuple{Union{Continuous,%20Type{Continuous}},%20Any,%20Any,%20Any,%20Any,%20Vararg{Any}}). Since `lqr` operates on the state realization of the system, but ModelingToolkit gives no guaratees about what the state realization will be, we specify the LQR penalty matrix in terms of the outputs using the system output matrix ``C``. + +```@example QUAD +using ControlSystemsBase, RobustAndOptimalControl, ControlSystemsMTK +quad = RotorCraft(closed_loop=false, addload=false) +quad = complete(quad) +inputs = [quad.thruster1.u; quad.thruster2.u; quad.thruster3.u; quad.thruster4.u] +outputs = [quad.y_alt, quad.y_roll, quad.y_pitch, quad.y_yaw, quad.y_forward, quad.y_sideways, quad.v_alt, quad.v_roll, quad.v_pitch, quad.v_yaw, quad.v_forward, quad.v_sideways, quad.yIe_alt] + +op = [ + quad.body.r_0[2] => 1e-32 + quad.v_alt => 1e-32 # To avoid singularity in linearization + quad.world.g => 9.81 + inputs .=> 1; +] |> Dict + +@time lsys = named_ss(IRSystem(quad), inputs, outputs; op) +rsys = minreal(sminreal(lsys)) +C = rsys.C +rank(C) >= rsys.nx || @warn "The output matrix C is not full rank" +Q = Diagonal([ # Output penalty matrix + 30 # Altitude + 1 # Roll + 1 # Pitch + 1 # Yaw + 1 # Forward + 1 # Sideways + 20 # Altitude velocity + 1 # Roll velocity + 1 # Pitch velocity + 1 # Yaw velocity + 10 # Forward velocity + 10 # Sideways velocity + 5 # Altitude integral error +]) +R = I(4) +L = lqr(rsys, Symmetric(C'Q*C), R)/C +trunc_zero!(A) = A[abs.(A) .< 1e4eps(maximum(abs, A))] .= 0 +trunc_zero!(L) +L +``` + + +```@example QUAD +ModelingToolkit.get_iv(i::IRSystem) = i.t +model = RotorCraft(; closed_loop=true, addload=true, L=-L, outputs) # Negate L for negative feedback +model = complete(model) +ssys = structural_simplify(IRSystem(model)) + +op = [ + model.body.r_0[2] => 1e-3 + model.body.r_0[3] => 1e-3 + model.body.r_0[1] => 1e-3 + collect(model.cable.joint_2.phi) .=> 0.1 # Usa a larger initial cable bend since this controller is more robust + model.world.g => 9.81; + collect(model.body.phid) .=> 0; + collect(D.(model.body.phi)) .=> 0; + model.feedback_gain.input.u[9] => 0; + model.feedback_gain.input.u[12] => 0; + model.Ie_alt => -10; # Initialize the integrator state to avoid a very large initial transient. This pre-compensates for gravity +] |> Dict +prob = ODEProblem(ssys, op, (0, 20)) +sol = solve(prob, FBDF(autodiff=false)) +@test SciMLBase.successful_retcode(sol) +plot(sol, idxs=[model.arm1.frame_b.r_0[2], model.arm2.frame_b.r_0[2], model.arm3.frame_b.r_0[2], model.arm4.frame_b.r_0[2]], layout=4, framestyle=:zerolines) +``` + +```@example QUAD +render(model, sol, 0:0.1:sol.t[end], x=-2, z=-2, y=-1, lookat=[0,-1,0], show_axis=false, filename="quadrotor_lqr.gif", framerate=25) +nothing # hide +``` +![quadrotor with LQR animation](quadrotor_lqr.gif) + +The observant reader may have noticed that we linearized the quadrotor without the cable-suspended load applied, but we simulated the closed-loop system with the load. Thankfully, the LQR controller is robust enough to stabilize the system despite this large model error. Before being satisfied with the controller, we should perform robustness analysis. Below, we compute sensitivity functions at the plant output and input and plot their sigma plots, as well as simultaneous diskmargins at the plant output and input. + +```@example QUAD +# NOTE: this section is temporarily disabled waiting for improved performance in linearization + +# linop = merge(op, Dict([ +# collect(model.system_outputs.u) .=> 0 +# collect(model.body.r_0) .=> 1e-32 +# collect(model.load.v_0) .=> 1e-32 # To avoid singularity in linearization +# collect(model.system_outputs.u) .=> 1e-32 +# collect(model.feedback_gain.input.u) .=> 1e-32 +# ])) +# @time "Sensitivity function" S = get_named_sensitivity(model, :y; system_modifier=IRSystem, op=linop) +# S = minreal(S, 1e-6) +# isstable(S) || @error "Sensitivity function S is not stable" +# T = I(S.ny) - S +# T = minreal(T, 1e-6) +# isstable(T) || @error "Sensitivity function T is not stable" +# LT = feedback(T, -I(T.ny))#get_named_looptransfer(model, :y; system_modifier=IRSystem, op) + +# @time "Comp Sensitivity function" Ti = get_named_comp_sensitivity(model, :u; system_modifier=IRSystem, op=linop) +# Ti = minreal(Ti, 1e-6) +# isstable(Ti) || @error "Sensitivity function Ti is not stable" +# LTi = feedback(Ti, -I(Ti.ny)) # Input loop-transfer function + +# CS = named_ss(model, :y, :u; op=linop, system_modifier=IRSystem) # Closed-loop system from measurement noise to control signal + +# w = 2pi.*exp10.(LinRange(-2, 2, 200)) +# fig_dm = plot(diskmargin(LT, 0.5), label="Plant output") # Compute diskmargin with a positive skew of 0.5 to account for a likely gain increase when the load is dropped +# plot!(diskmargin(LTi, 0.5), label="Plant input", titlefontsize=8) # Note, simultaneous diskmargins are somewhat conservative + +# plot( +# sigmaplot(S, w, hz=true, label="", title="S", legend=false), +# sigmaplot(T, w, hz=true, label="", title="T", legend=false), +# sigmaplot(LT, w, hz=true, label="", title="L", legend=false), +# bodeplot(CS, w, hz=true, label="", title="CS", legend=false, plotphase=false, layout=1), +# fig_dm, +# layout=(2,3), size=(800,500), legend=:bottomright, ylims=(1e-4, Inf), +# ) +``` + +While gain and phase margins appear to be reasonable, we have a large high-frequency gain in the transfer functions from measurement noise to control signal, ``C(s)S(s)``. For a rotor craft where the control signal manipulates the current through motor windings, this may lead to excessive heat generation in the motors if the sensor measurements are noisy. + +The diskmargin at the plant output is small, luckily, the gain variation appears at the plant input where the diskmargin is significantly larger. The diskmargins are visualized in the figure titled "Stable region for combined gain and phase variation". See [Diskmargin example](https://juliacontrol.github.io/RobustAndOptimalControl.jl/dev/#Diskmargin-example) to learn more about diskmargins. diff --git a/docs/src/examples/robot.md b/docs/src/examples/robot.md index cbbaf42d..eae0affd 100644 --- a/docs/src/examples/robot.md +++ b/docs/src/examples/robot.md @@ -131,7 +131,8 @@ fkine(prob.u0, prob.p, 0) The function `fkine` above takes the full state of the robot model, as opposed to only the joint angles. ```@setup -# ### Jacobian # Temporarily deactivated +# Temporarily removed due to https://github.com/JuliaComputing/JuliaSimCompiler.jl/issues/366 +### Jacobian # We can compute the Jacobian ``J`` of the forward-kinematics function using the package ForwardDiff (this Jacobian is often referred to as the _analytical Jacobian_, which in the 6DOF case is different from the _geometrical Jacobian_ that is used in the relation ``v = J\dot{q}``). The Jacobian of the end-effector positional coordinates will be a 3×36 matrix, since we have 36-dimensional state of the robot after simplification. Since the end-effector coordinates do not depend on all the state variables, we may ask which variables it depends on by finding non-zero columns of ``J`` # ```@example robot # using ModelingToolkit.ForwardDiff diff --git a/ext/Render.jl b/ext/Render.jl index 9857c627..c2587891 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -224,7 +224,7 @@ function get_color(sys, sol, default, var_name = :color) end end -function get_shape(sys, sol)::String +function get_shapefile(sys, sol)::String try sf = sol(sol.t[1], idxs=collect(sys.shapefile)) decode(sf) @@ -233,6 +233,15 @@ function get_shape(sys, sol)::String end end +function get_shape(sys, sol)::String + try + sf = sol(sol.t[1], idxs=collect(sys.shape)) + decode(sf) + catch + "" + end +end + function default_scene(x,y,z; lookat=Vec3f(0,0,0),up=Vec3f(0,1,0),show_axis=false) # if string(Makie.current_backend()) == "CairoMakie" @@ -588,19 +597,56 @@ end function render!(scene, ::typeof(BodyShape), sys, sol, t) color = get_color(sys, sol, :purple) - shapepath = get_shape(sys, sol) + shapepath = get_shapefile(sys, sol) + shape = get_shape(sys, sol) + Tshape = reshape(sol(sol.t[1], idxs=sys.shape_transform), 4, 4) + scale = Vec3f(Float32(sol(sol.t[1], idxs=sys.shape_scale))*ones(Float32, 3)) if isempty(shapepath) - radius = Float32(sol(sol.t[1], idxs=sys.radius)) r_0a = get_fun(sol, collect(sys.frame_a.r_0)) r_0b = get_fun(sol, collect(sys.frame_b.r_0)) - thing = @lift begin - r1 = Point3f(r_0a($t)) - r2 = Point3f(r_0b($t)) - origin = r1 - extremity = r2 - Makie.GeometryBasics.Cylinder(origin, extremity, radius) + shape = get_shape(sys, sol) + if shape == "cylinder" + radius = Float32(sol(sol.t[1], idxs=sys.radius)) + thing = @lift begin + r1 = Point3f(r_0a($t)) + r2 = Point3f(r_0b($t)) + origin = r1 + extremity = r2 + Makie.GeometryBasics.Cylinder(origin, extremity, radius) + end + mesh!(scene, thing; color, specular = Vec3f(1.5), transparency=true) + elseif shape == "box" + Rfun = get_rot_fun(sol, sys.frame_a) + + width = 2*Float32(sol(sol.t[1], idxs=sys.radius)) + height = width + r = sol(sol.t[1], idxs=collect(sys.r)) + length = norm(r) + length_dir = normalize(r) + + width_dir = [1,0,0]'length_dir > 0.9 ? [0,1,0] : [1,0,0] + height_dir = normalize(cross(length_dir, width_dir)) + width_dir = normalize(cross(height_dir, length_dir)) + + r_0a = get_fun(sol, collect(sys.frame_a.r_0)) # Origin is translated by r_shape + + R0 = [length_dir width_dir height_dir] + @assert isapprox(det(R0), 1.0, atol=1e-6) + + origin = Vec3f(0, -width/2, -height/2) + extent = Vec3f(length, width, height) + thing = Makie.Rect3f(origin, extent) + m = mesh!(scene, thing; color, specular = Vec3f(1.5), transparency=true) + on(t) do t + r1 = Point3f(r_0a(t)) + R = Rfun(t) + q = Rotations.QuatRotation(R*R0).q + Q = Makie.Quaternionf(q.v1, q.v2, q.v3, q.s) + Makie.transform!(m, translation=r1, rotation=Q) + end + else + error("Shape $shape not supported") end - mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1), transparency=true) else T = get_frame_fun(sol, sys.frame_a) scale = Vec3f(Float32(sol(sol.t[1], idxs=sys.shape_scale))*ones(Float32, 3)) diff --git a/src/PlanarMechanics/utils.jl b/src/PlanarMechanics/utils.jl index 25f307a9..df4db2c0 100644 --- a/src/PlanarMechanics/utils.jl +++ b/src/PlanarMechanics/utils.jl @@ -39,8 +39,6 @@ function ori_2d(frame) return [cos(phi) -sin(phi); sin(phi) cos(phi)] end -# extends Frame with just styling -# https://github.com/dzimmer/PlanarMechanics/blob/master/PlanarMechanics/Interfaces/Frame_resolve.mo FrameResolve = Frame @mtkmodel PartialTwoFrames begin @@ -55,8 +53,8 @@ Base.@doc """ Partial model with two frames # Connectors: - - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque - - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque +- `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque +- `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque """ PartialTwoFrames """ @@ -64,7 +62,7 @@ Partial model with two frames Set zero position vector and orientation object of frame_resolve # Connectors: - - `frame_resolve` [FrameResolve](@ref) Coordinate system fixed to the component with one cut-force and cut-torque +- `frame_resolve` [FrameResolve](@ref) Coordinate system fixed to the component with one cut-force and cut-torque """ @mtkmodel ZeroPosition begin @components begin diff --git a/src/components.jl b/src/components.jl index d6f4eb90..678f882f 100644 --- a/src/components.jl +++ b/src/components.jl @@ -481,7 +481,9 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames See also [`BodyCylinder`](@ref) and [`BodyBox`](@ref) for body components with predefined shapes and automatically computed inertial properties based on geometry and density. """ -@component function BodyShape(; name, m = 1, r = [0, 0, 0], r_cm = 0.5*r, r_0 = 0, radius = 0.08, color=purple, shapefile="", shape_transform = I(4), shape_scale = 1, kwargs...) +@component function BodyShape(; name, m = 1, r = [0, 0, 0], r_cm = 0.5*r, r_0 = 0, radius = 0.08, color=purple, shapefile="", shape_transform = I(4), shape_scale = 1, + height = 0.1_norm(r), width = height, shape = "cylinder", + kwargs...) systems = @named begin translation = FixedTranslation(r = r, render=false) translation_cm = FixedTranslation(r = r_cm, render=false) @@ -504,7 +506,8 @@ See also [`BodyCylinder`](@ref) and [`BodyBox`](@ref) for body components with p ] shapecode = encode(shapefile) - @parameters begin + shape = encode(shape) + pars = @parameters begin r[1:3]=r, [ description = "Vector from frame_a to frame_b resolved in frame_a", ] @@ -513,10 +516,12 @@ See also [`BodyCylinder`](@ref) and [`BodyBox`](@ref) for body components with p shapefile[1:length(shapecode)] = shapecode shape_transform[1:16] = vec(shape_transform) shape_scale = shape_scale + width = width, [description = """Width of the body in animations (if shape = "box")"""] + height = height, [description = """Height of the body in animations (if shape = "box")"""] + shape[1:length(shape)] = shape end - - pars = [r; radius; color; shapefile; shape_transform; shape_scale] + pars = collect_all(pars) r_0, v_0, a_0 = collect.((r_0, v_0, a_0))