some updates for MTK v9

Manifest.toml

@@ -2,7 +2,7 @@
 
 julia_version = "1.10.2"
 manifest_format = "2.0"
-project_hash = "214cc8fea2909c2dc41d280131bd19bf33b2b5df"
+project_hash = "efeef262c86b4eafbecc9a61dea3d204d89a22d1"
 
 [[deps.ADTypes]]
 git-tree-sha1 = "016833eb52ba2d6bea9fcb50ca295980e728ee24"
@@ -87,6 +87,12 @@ git-tree-sha1 = "0c5f81f47bbbcf4aea7b2959135713459170798b"
 uuid = "62783981-4cbd-42fc-bca8-16325de8dc4b"
 version = "0.1.5"
 
+[[deps.Bumper]]
+deps = ["StrideArraysCore"]
+git-tree-sha1 = "aa2fc4ee0754a4ec23208961d4d40f154157f5a3"
+uuid = "8ce10254-0962-460f-a3d8-1f77fea1446e"
+version = "0.6.0"
+
 [[deps.CEnum]]
 git-tree-sha1 = "389ad5c84de1ae7cf0e28e381131c98ea87d54fc"
 uuid = "fa961155-64e5-5f13-b03f-caf6b980ea82"
@@ -126,12 +132,23 @@ weakdeps = ["SparseArrays"]
  [deps.ChainRulesCore.extensions]
  ChainRulesCoreSparseArraysExt = "SparseArrays"
 
+[[deps.Clang_jll]]
+deps = ["Artifacts", "JLLWrappers", "Libdl", "TOML", "Zlib_jll", "libLLVM_jll"]
+git-tree-sha1 = "de2204d98741f57e7ddb9a6a738db74ba8a608cb"
+uuid = "0ee61d77-7f21-5576-8119-9fcc46b10100"
+version = "15.0.7+10"
+
 [[deps.CloseOpenIntervals]]
 deps = ["Static", "StaticArrayInterface"]
 git-tree-sha1 = "70232f82ffaab9dc52585e0dd043b5e0c6b714f1"
 uuid = "fb6a15b2-703c-40df-9091-08a04967cfa9"
 version = "0.1.12"
 
+[[deps.CodeInfoTools]]
+git-tree-sha1 = "91018794af6e76d2d42b96b25f5479bca52598f5"
+uuid = "bc773b8a-8374-437a-b9f2-0e9785855863"
+version = "0.3.5"
+
 [[deps.Combinatorics]]
 git-tree-sha1 = "08c8b6831dc00bfea825826be0bc8336fc369860"
 uuid = "861a8166-3701-5b0c-9a16-15d98fcdc6aa"
@@ -336,10 +353,10 @@ uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
 version = "0.9.3"
 
 [[deps.DomainSets]]
-deps = ["CompositeTypes", "IntervalSets", "LinearAlgebra", "Random", "StaticArrays", "Statistics"]
-git-tree-sha1 = "51b4b84d33ec5e0955b55ff4b748b99ce2c3faa9"
+deps = ["CompositeTypes", "IntervalSets", "LinearAlgebra", "Random", "StaticArrays"]
+git-tree-sha1 = "9fd332fb3b276a080e3ebccf0dcd98f4a10bf6a6"
 uuid = "5b8099bc-c8ec-5219-889f-1d9e522a28bf"
-version = "0.6.7"
+version = "0.7.10"
 
 [[deps.Downloads]]
 deps = ["ArgTools", "FileWatching", "LibCURL", "NetworkOptions"]
@@ -358,6 +375,24 @@ git-tree-sha1 = "0bb0a6f812213ecc8fbbcf472f4a993036858971"
 uuid = "7c1d4256-1411-5781-91ec-d7bc3513ac07"
 version = "0.5.5"
 
+[[deps.DynamicQuantities]]
+deps = ["Compat", "PackageExtensionCompat", "Tricks"]
+git-tree-sha1 = "bcb4d4a81564700278852b730ced61b66ad338e6"
+uuid = "06fc5a27-2a28-4c7c-a15d-362465fb6821"
+version = "0.13.1"
+
+ [deps.DynamicQuantities.extensions]
+ DynamicQuantitiesLinearAlgebraExt = "LinearAlgebra"
+ DynamicQuantitiesMeasurementsExt = "Measurements"
+ DynamicQuantitiesScientificTypesExt = "ScientificTypes"
+ DynamicQuantitiesUnitfulExt = "Unitful"
+
+ [deps.DynamicQuantities.weakdeps]
+ LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
+ Measurements = "eff96d63-e80a-5855-80a2-b1b0885c5ab7"
+ ScientificTypes = "321657f4-b219-11e9-178b-2701a2544e81"
+ Unitful = "1986cc42-f94f-5a68-af5c-568840ba703d"
+
 [[deps.EnumX]]
 git-tree-sha1 = "bdb1942cd4c45e3c678fd11569d5cccd80976237"
 uuid = "4e289a0a-7415-4d19-859d-a7e5c4648b56"
@@ -421,6 +456,11 @@ weakdeps = ["PDMats", "SparseArrays", "Statistics"]
  FillArraysSparseArraysExt = "SparseArrays"
  FillArraysStatisticsExt = "Statistics"
 
+[[deps.FindFirstFunctions]]
+git-tree-sha1 = "e90fef90f7d75e6a5b435b0fd65609759f99717a"
+uuid = "64ca27bc-2ba2-4a57-88aa-44e436879224"
+version = "1.2.0"
+
 [[deps.FiniteDiff]]
 deps = ["ArrayInterface", "LinearAlgebra", "Requires", "Setfield", "SparseArrays"]
 git-tree-sha1 = "bc0c5092d6caaea112d3c8e3b238d61563c58d5f"
@@ -569,15 +609,15 @@ version = "0.21.4"
 
 [[deps.JuliaFormatter]]
 deps = ["CSTParser", "CommonMark", "DataStructures", "Glob", "Pkg", "PrecompileTools", "Tokenize"]
-git-tree-sha1 = "8f5295e46f594ad2d8652f1098488a77460080cd"
+git-tree-sha1 = "e07d6fd7db543b11cd90ed764efec53f39851f09"
 uuid = "98e50ef6-434e-11e9-1051-2b60c6c9e899"
-version = "1.0.45"
+version = "1.0.54"
 
 [[deps.JuliaSimCompiler]]
-deps = ["ChainRules", "ConstructionBase", "DataStructures", "DiffEqCallbacks", "DocStringExtensions", "Expronicon", "ForwardDiff", "GPUCompiler", "Graphs", "IfElse", "JuliaFormatter", "LLVM", "Libdl", "LinearAlgebra", "ModelingToolkit", "NaNMath", "OffsetArrays", "Preferences", "RecursiveFactorization", "RuntimeGeneratedFunctions", "SciMLBase", "Setfield", "SparseArrays", "SparseDiffTools", "SpecialFunctions", "StaticArrays", "StrideArraysCore", "SymbolicIndexingInterface", "Symbolics", "UnPack"]
-git-tree-sha1 = "a60ceef599c23a0e25255870296e0388ade0b448"
+deps = ["ChainRules", "ConstructionBase", "DataStructures", "DiffEqCallbacks", "DocStringExtensions", "Expronicon", "ForwardDiff", "Graphs", "IfElse", "LLVM", "Libdl", "LinearAlgebra", "ModelingToolkit", "OffsetArrays", "Preferences", "RecursiveFactorization", "RuntimeGeneratedFunctions", "SciMLBase", "Setfield", "SparseArrays", "SparseDiffTools", "SpecialFunctions", "StaticArrays", "StaticCompiler", "StrideArraysCore", "SymbolicIndexingInterface", "Symbolics", "UnPack"]
+git-tree-sha1 = "f30bd015524a66109844c55b6ddc14d7a2b12cb5"
 uuid = "8391cb6b-4921-5777-4e45-fd9aab8cb88d"
-version = "0.1.7"
+version = "0.1.6"
 
  [deps.JuliaSimCompiler.extensions]
  AcausalVisualizationExt = "CairoMakie"
@@ -611,6 +651,11 @@ git-tree-sha1 = "8a6837ec02fe5fb3def1abc907bb802ef11a0729"
 uuid = "ba0b0d4f-ebba-5204-a429-3ac8c609bfb7"
 version = "0.9.5"
 
+[[deps.LLD_jll]]
+deps = ["Artifacts", "Libdl", "Zlib_jll", "libLLVM_jll"]
+uuid = "d55e3150-da41-5e91-b323-ecfd1eec6109"
+version = "15.0.7+10"
+
 [[deps.LLVM]]
 deps = ["CEnum", "LLVMExtra_jll", "Libdl", "Preferences", "Printf", "Requires", "Unicode"]
 git-tree-sha1 = "ab01dde107f21aa76144d0771dccc08f152ccac7"
@@ -834,10 +879,10 @@ version = "1.1.0"
 uuid = "a63ad114-7e13-5084-954f-fe012c677804"
 
 [[deps.ModelingToolkit]]
-deps = ["AbstractTrees", "ArrayInterface", "Combinatorics", "Compat", "ConstructionBase", "DataStructures", "DiffEqBase", "DiffEqCallbacks", "DiffRules", "Distributed", "Distributions", "DocStringExtensions", "DomainSets", "ForwardDiff", "FunctionWrappersWrappers", "Graphs", "IfElse", "InteractiveUtils", "JuliaFormatter", "JumpProcesses", "LabelledArrays", "Latexify", "Libdl", "LinearAlgebra", "MLStyle", "MacroTools", "NaNMath", "OrdinaryDiffEq", "PrecompileTools", "RecursiveArrayTools", "Reexport", "RuntimeGeneratedFunctions", "SciMLBase", "Serialization", "Setfield", "SimpleNonlinearSolve", "SparseArrays", "SpecialFunctions", "StaticArrays", "SymbolicIndexingInterface", "SymbolicUtils", "Symbolics", "URIs", "UnPack", "Unitful"]
-git-tree-sha1 = "11f35f9619c625c18454a94a9013e7d047501fbf"
+deps = ["AbstractTrees", "ArrayInterface", "Combinatorics", "Compat", "ConstructionBase", "DataStructures", "DiffEqBase", "DiffEqCallbacks", "DiffRules", "Distributed", "Distributions", "DocStringExtensions", "DomainSets", "DynamicQuantities", "ExprTools", "FindFirstFunctions", "ForwardDiff", "FunctionWrappersWrappers", "Graphs", "InteractiveUtils", "JuliaFormatter", "JumpProcesses", "LabelledArrays", "Latexify", "Libdl", "LinearAlgebra", "MLStyle", "NaNMath", "OrdinaryDiffEq", "PrecompileTools", "RecursiveArrayTools", "Reexport", "RuntimeGeneratedFunctions", "SciMLBase", "SciMLStructures", "Serialization", "Setfield", "SimpleNonlinearSolve", "SparseArrays", "SpecialFunctions", "StaticArrays", "SymbolicIndexingInterface", "SymbolicUtils", "Symbolics", "URIs", "UnPack", "Unitful"]
+git-tree-sha1 = "a61d4c58f49c0c0e8b6d48d864b1f24ef725232b"
 uuid = "961ee093-0014-501f-94e3-6117800e7a78"
-version = "8.75.0"
+version = "9.8.0"
 
  [deps.ModelingToolkit.extensions]
  MTKBifurcationKitExt = "BifurcationKit"
@@ -849,9 +894,9 @@ version = "8.75.0"
 
 [[deps.ModelingToolkitStandardLibrary]]
 deps = ["ChainRulesCore", "DiffEqBase", "IfElse", "LinearAlgebra", "ModelingToolkit", "Symbolics"]
-git-tree-sha1 = "8d3a68e1e3f95a7e61e10b7a563bc30c8b3c94cb"
+git-tree-sha1 = "50b27ae73c39ecba91bd3e48325b586bc970cd24"
 uuid = "16a59e39-deab-5bd0-87e4-056b12336739"
-version = "2.4.0"
+version = "2.6.0"
 
 [[deps.MozillaCACerts_jll]]
 uuid = "14a3606d-f60d-562e-9121-12d972cd8159"
@@ -1344,6 +1389,18 @@ git-tree-sha1 = "36b3d696ce6366023a0ea192b4cd442268995a0d"
 uuid = "1e83bf80-4336-4d27-bf5d-d5a4f845583c"
 version = "1.4.2"
 
+[[deps.StaticCompiler]]
+deps = ["Clang_jll", "CodeInfoTools", "GPUCompiler", "InteractiveUtils", "LLD_jll", "LLVM", "Libdl", "MacroTools", "Serialization", "StaticTools"]
+git-tree-sha1 = "545c528c77eb51ebd65502acf20956ea35171a41"
+uuid = "81625895-6c0f-48fc-b932-11a18313743c"
+version = "0.6.3"
+
+[[deps.StaticTools]]
+deps = ["Bumper", "LinearAlgebra", "LoopVectorization", "ManualMemory", "Random"]
+git-tree-sha1 = "80f14502d07ca392387ac8a9963585046b92a8c6"
+uuid = "86c06d3c-3f03-46de-9781-57580aa96d0a"
+version = "0.8.9"
+
 [[deps.Statistics]]
 deps = ["LinearAlgebra", "SparseArrays"]
 uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"
@@ -1559,6 +1616,11 @@ deps = ["Libdl"]
 uuid = "83775a58-1f1d-513f-b197-d71354ab007a"
 version = "1.2.13+1"
 
+[[deps.libLLVM_jll]]
+deps = ["Artifacts", "Libdl"]
+uuid = "8f36deef-c2a5-5394-99ed-8e07531fb29a"
+version = "15.0.7+10"
+
 [[deps.libblastrampoline_jll]]
 deps = ["Artifacts", "Libdl"]
 uuid = "8e850b90-86db-534c-a0d3-1478176c7d93"

Project.toml

@@ -19,7 +19,7 @@ Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a"
 Render = ["Makie"]
 
 [compat]
-ModelingToolkit = "8.30"
+ModelingToolkit = "9"
 ModelingToolkitStandardLibrary = "2"
 Rotations = "1.4"
 CoordinateTransformations = "0.6"

src/components.jl

@@ -100,20 +100,21 @@ Can be though of as a massless rod. For a massive rod, see [`BodyShape`](@ref) o
 @component function FixedTranslation(; name, r, radius=0.08f0)
  @named frame_a = Frame()
  @named frame_b = Frame()
- @parameters r(t)[1:3]=r [
+ @parameters r[1:3]=r [
  description = "position vector from frame_a to frame_b, resolved in frame_a",
  ]
+ r = collect(r)
  @parameters radius=radius [
  description = "Radius of the body in animations",
  ]
  fa = frame_a.f |> collect
  fb = frame_b.f |> collect
  taua = frame_a.tau |> collect
  taub = frame_b.tau |> collect
- eqs = Equation[collect(frame_b.r_0) .~ collect(frame_a.r_0) + resolve1(ori(frame_a), r)
- ori(frame_b) ~ ori(frame_a)
- 0 .~ fa + fb
- 0 .~ taua + taub + cross(r, fb)]
+ eqs = Equation[(collect(frame_b.r_0) .~ collect(frame_a.r_0) + resolve1(ori(frame_a), r))
+ (ori(frame_b) ~ ori(frame_a))
+ (0 .~ fa + fb)
+ (0 .~ taua + taub + cross(r, fb))]
  pars = [r; radius]
  vars = []
  compose(ODESystem(eqs, t, vars, pars; name), frame_a, frame_b)
@@ -150,6 +151,9 @@ Fixed translation followed by a fixed rotation of `frame_b` with respect to `fra
  @parameters angle(t)=angle [
  description = "angle of rotation in radians",
  ]
+
+ pars = [r; n; angle]
+
  fa = frame_a.f |> collect
  fb = frame_b.f |> collect
  taua = frame_a.tau |> collect
@@ -160,20 +164,20 @@ Fixed translation followed by a fixed rotation of `frame_b` with respect to `fra
  if isroot
  R_rel = planar_rotation(n, angle, 0)
  eqs = [ori(frame_b) ~ absoluteRotation(frame_a, R_rel);
- zeros(3) .~ fa + resolve1(R_rel, fb);
- zeros(3) .~ taua + resolve1(R_rel, taub) - cross(r,
+ zeros(3) ~ fa + resolve1(R_rel, fb);
+ zeros(3) ~ taua + resolve1(R_rel, taub) - cross(r,
  fa)]
  else
  R_rel_inv = planar_rotation(n, -angle, 0)
  eqs = [ori(frame_a) ~ absoluteRotation(frame_b, R_rel_inv);
- zeros(3) .~ fb + resolve1(R_rel_inv, fa);
- zeros(3) .~ taub + resolve1(R_rel_inv, taua) +
+ zeros(3) ~ fb + resolve1(R_rel_inv, fa);
+ zeros(3) ~ taub + resolve1(R_rel_inv, taua) +
  cross(resolve1(R_rel_inv, r), fb)]
  end
  eqs = collect(eqs)
  append!(eqs, collect(frame_b.r_0) .~ collect(frame_a.r_0) + resolve1(frame_a, r))
 
- compose(ODESystem(eqs, t; name), frame_a, frame_b)
+ compose(ODESystem(eqs, t, [], pars; name), frame_a, frame_b)
 end
 
 """
@@ -211,20 +215,20 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
  state_priority = 2,
  description = "Position vector from origin of world frame to origin of frame_a",
  ]
- @variables v_0(t)[1:3]=0 [
+ @variables v_0(t)[1:3] [guess = 0, 
  state_priority = 2,
  description = "Absolute velocity of frame_a, resolved in world frame (= D(r_0))",
  ]
- @variables a_0(t)[1:3]=0 [
+ @variables a_0(t)[1:3] [guess = 0, 
  state_priority = 2,
  description = "Absolute acceleration of frame_a resolved in world frame (= D(v_0))",
  ]
- @variables g_0(t)[1:3]=0 [description = "gravity acceleration"]
- @variables w_a(t)[1:3]=0 [
+ @variables g_0(t)[1:3] [guess = 0, description = "gravity acceleration"]
+ @variables w_a(t)[1:3] [guess = 0, 
  state_priority = 2,
  description = "Absolute angular velocity of frame_a resolved in frame_a",
  ]
- @variables z_a(t)[1:3]=0 [
+ @variables z_a(t)[1:3] [guess = 0, 
  description = "Absolute angular acceleration of frame_a resolved in frame_a",
  ]
  # 6*3 potential variables + Frame: 2*3 flow + 3 potential + 3 residual = 24 equations + 2*3 flow
@@ -335,11 +339,11 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames
  state_priority = 2,
  description = "Position vector from origin of world frame to origin of frame_a",
  ]
- @variables v_0(t)[1:3]=0 [
+ @variables v_0(t)[1:3] [ guess=0,
  state_priority = 2,
  description = "Absolute velocity of frame_a, resolved in world frame (= D(r_0))",
  ]
- @variables a_0(t)[1:3]=0 [
+ @variables a_0(t)[1:3] [ guess=0,
  description = "Absolute acceleration of frame_a resolved in world frame (= D(v_0))",
  ]
  @parameters begin

src/forces.jl

@@ -139,6 +139,7 @@ function Force(; name, resolveInFrame = :frame_b)
  extend(ODESystem(eqs, t, name = name, systems = [force, basicForce]), ptf)
 end
 
+_norm(x) = sqrt(sum(x^2 for x in x)) # Workaround for buggy symbolic arrays
 function LineForceBase(; name, length = 0, s_small = 1e-10, fixedRotationAtFrame_a = false,
  fixedRotationAtFrame_b = false, r_rel_0 = 0, s0 = 0)
  @named frame_a = Frame()
@@ -157,10 +158,10 @@ function LineForceBase(; name, length = 0, s_small = 1e-10, fixedRotationAtFrame
  description = "Unit vector in direction from frame_a to frame_b, resolved in world frame",
  ]
 
- eqs = [r_rel_0 .~ frame_b.r_0 .- frame_a.r_0;
- length ~ norm(r_rel_0)
+ eqs = [collect(r_rel_0 .~ frame_b.r_0 .- frame_a.r_0);
+ length ~ _norm(r_rel_0)
  s ~ max(length, s_small)
- e_rel_0 .~ r_rel_0 ./ s]
+ collect(e_rel_0 .~ r_rel_0 ./ s)]
 
  # Modelica stdlib has the option to inser special equations when two line forces are connected, this option does not yet exisst here https://github.com/modelica/ModelicaStandardLibrary/blob/10238e9927e2078571e41b53cda128c5207f69f7/Modelica/Mechanics/MultiBody/Interfaces/LineForceBase.mo#L49
 
@@ -214,7 +215,7 @@ function LineForceWithMass(; name, length = 0, m = 1.0, lengthFraction = 0.5, kw
  # NOTE, both frames are assumed to be connected, while modelica has special handling if they aren't
  if m0 > 0
  eqs = [eqs
- r_CM_0 .~ frame_a.r_0 + r_rel_0 * lengthFraction
+ collect(r_CM_0 .~ frame_a.r_0 + r_rel_0 * lengthFraction)
  v_CM_0 .~ D.(r_CM_0)
  ag_CM_0 .~ D.(v_CM_0) - gravity_acceleration(r_CM_0)
  frame_a.f .~ resolve2(ori(frame_a),

src/frames.jl

@@ -1,12 +1,12 @@
-@connector function Frame(; name, varw = false)
- @variables r_0(t)[1:3]=zeros(3) [
+@connector function Frame(; name, varw = false, r_0 = zeros(3))
+ @variables r_0(t)[1:3]=r_0 [
  description = "Position vector directed from the origin of the world frame to the connector frame origin, resolved in world frame",
  ]
- @variables f(t)[1:3]=zeros(3) [
+ @variables f(t)[1:3] [
  connect = Flow,
  description = "Cut force resolved in connector frame",
  ]
- @variables tau(t)[1:3]=zeros(3) [
+ @variables tau(t)[1:3] [
  connect = Flow,
  description = "Cut torque resolved in connector frame",
  ]

src/robot/robot_components.jl

@@ -21,17 +21,17 @@ D = Differential(t)
 """
 @connector function AxisControlBus(; name)
  vars = @variables begin
- (motion_ref(t) = 0), [description = "= true, if reference motion is not in rest"]
- (angle_ref(t) = 0), [description = "Reference angle of axis flange"]
- (angle(t) = 0), [description = "Angle of axis flange"]
- (speed_ref(t) = 0), [description = "Reference speed of axis flange"]
- (speed(t) = 0), [description = "Speed of axis flange"]
- (acceleration_ref(t) = 0), [description = "Reference acceleration of axis flange"]
- (acceleration(t) = 0), [description = "Acceleration of axis flange"]
- (current_ref(t) = 0), [description = "Reference current of motor"]
- (current(t) = 0), [description = "Current of motor"]
- (motorAngle(t) = 0), [description = "Angle of motor flange"]
- (motorSpeed(t) = 0), [description = "Speed of motor flange"]
+ (motion_ref(t)), [guess=0.0,input = true, description = "= true, if reference motion is not in rest"]
+ (angle_ref(t)), [guess=0.0,input = true, description = "Reference angle of axis flange"]
+ (angle(t)), [guess=0.0,output = true, description = "Angle of axis flange"]
+ (speed_ref(t)), [guess=0.0,input = true, description = "Reference speed of axis flange"]
+ (speed(t)), [guess=0.0,output = true, description = "Speed of axis flange"]
+ (acceleration_ref(t)), [guess=0.0,input = true, description = "Reference acceleration of axis flange"]
+ (acceleration(t)), [guess=0.0,output = true, description = "Acceleration of axis flange"]
+ (current_ref(t)), [guess=0.0,input = true, description = "Reference current of motor"]
+ (current(t)), [guess=0.0,output = true, description = "Current of motor"]
+ (motorAngle(t)), [guess=0.0,output = true, description = "Angle of motor flange"]
+ (motorSpeed(t)), [guess=0.0,output = true, description = "Speed of motor flange"]
  end
  ODESystem(Equation[], t, vars, []; name)
 end
@@ -433,12 +433,12 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81)
  # end
 
  @variables begin
- (q(t)[1:6] = 0), [state_priority = typemax(Int), description = "Joint angles"]
- (qd(t)[1:6] = 0), [state_priority = typemax(Int), description = "Joint speeds"]
- (qdd(t)[1:6] = 0),
- [state_priority = typemax(Int), description = "Joint accelerations"]
- (tau(t)[1:6] = 0),
- [state_priority = typemax(Int), description = "Joint driving torques"]
+ (q(t)[1:6]), [guess = 0, state_priority = typemax(Int), description = "Joint angles"]
+ (qd(t)[1:6]), [guess = 0, state_priority = typemax(Int), description = "Joint speeds"]
+ (qdd(t)[1:6]),
+ [guess = 0, state_priority = typemax(Int), description = "Joint accelerations"]
+ (tau(t)[1:6]),
+ [guess = 0, state_priority = typemax(Int), description = "Joint driving torques"]
  end
 
  systems = @named begin