-
-
Notifications
You must be signed in to change notification settings - Fork 233
Closed
Labels
bugSomething isn't workingSomething isn't working
Description
With the Multibody model below, I cannot access a variable, despite it being indicated as one of the unknowns of the model
julia> model.freemotion.Rrel_f.r_0[1] in Set(unknowns(model))
true
julia> sol(0.0, idxs=model.freemotion.Rrel_f.r_0[1])
ERROR: Invalid symbol (freemotion₊Rrel_f₊r_0(t))[1] for `getu`
Stacktrace:
[1] error(s::String)
@ Base ./error.jl:35
[2] _getu(sys::ODESolution{…}, ::SymbolicIndexingInterface.ScalarSymbolic, ::SymbolicIndexingInterface.ScalarSymbolic, sym::Num)
@ SymbolicIndexingInterface ~/.julia/packages/SymbolicIndexingInterface/rBPDg/src/state_indexing.jl:145
[3] getu
@ ~/.julia/packages/SymbolicIndexingInterface/rBPDg/src/state_indexing.jl:31 [inlined]
[4] (::ODESolution{…})(t::Float64, ::Type{…}, idxs::Num, continuity::Symbol)
@ SciMLBase ~/.julia/packages/SciMLBase/j8jQg/src/solutions/ode_solutions.jl:289
[5] #_#478
@ ~/.julia/packages/SciMLBase/j8jQg/src/solutions/ode_solutions.jl:220 [inlined]
[6] AbstractODESolution
@ ~/.julia/packages/SciMLBase/j8jQg/src/solutions/ode_solutions.jl:215 [inlined]
[7] top-level scope
@ REPL[91]:1
Some type information was truncated. Use `show(err)` to see complete types.using Multibody
using ModelingToolkit
using ModelingToolkitStandardLibrary.Blocks
using LinearAlgebra
using Plots
using JuliaSimCompiler
using OrdinaryDiffEq
using Test
t = Multibody.t
D = Differential(t)
world = Multibody.world
num_arms = 4 # Number of arms of the rotor craft.
angle_between_arms = 2*pi/num_arms # Angle between the arms, assuming they are evenly spaced.
arm_length = 0.2 # Length of each arm.
arm_outer_diameter = 0.03
arm_inner_diameter = 0.02
arm_density = 800 # Density of the arm [kg/m³].
body_mass = 0.2 # Mass of the body.
load_mass = 0.1 # Mass of the load.
cable_length = 1 # Length of the cable.
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
Tialt = 3
Tdalt = 3
kroll = 0.02
Tiroll = 100
Tdroll = 1
kpitch = 0.02
Tipitch = 100
Tdpitch = 1
@mtkmodel Thruster begin
@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.
thrust = RealInput()
end
@variables begin
u(t), [state_priority=1000]
end
@equations begin
thrust3d.force.u[1] ~ 0
thrust3d.force.u[2] ~ thrust.u
thrust3d.force.u[3] ~ 0
thrust.u ~ u
connect(frame_b, thrust3d.frame_b)
end
end
function RotorCraft(; closed_loop = true, addload=true)
arms = [
BodyCylinder(
r = [arm_length*cos(angle_between_arms*(i-1)), 0, arm_length*sin(angle_between_arms*(i-1))],
diameter = arm_outer_diameter,
inner_diameter = arm_inner_diameter,
density = arm_density,
name=Symbol("arm$i"),
) for i = 1:num_arms
]
@variables begin
y_alt(t)
y_roll(t)
y_pitch(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)
@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 ~ freemotion.phi[3]
y_pitch ~ freemotion.phi[1]
[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]
]
systems = [world; arms; body; thrusters; freemotion]
if addload
@named load = Body(m = load_mass, air_resistance=0.1)
@named cable = Rope(
l = cable_length,
m = cable_mass,
n = number_of_links,
c = 0,
d = 0,
air_resistance = 0.1,
d_joint = 0.1,
radius = cable_diameter/2,
color = [0.5, 0.4, 0.4, 1],
dir = [0.0, -1, 0]
)
push!(systems, load)
push!(systems, cable)
push!(connections, connect(body.frame_a, cable.frame_a))
push!(connections, connect(cable.frame_b, load.frame_a))
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])
end
@named model = ODESystem(connections, t; systems)
complete(model)
end
model = RotorCraft(closed_loop=true, addload=true)
model = complete(model)
ssys = structural_simplify(IRSystem(model))
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))
sol = solve(prob, FBDF(autodiff=false), reltol=1e-8, abstol=1e-8)
@test SciMLBase.successful_retcode(sol)Metadata
Metadata
Assignees
Labels
bugSomething isn't workingSomething isn't working