Skip to content

failure to access unknown from solution #3065

@baggepinnen

Description

@baggepinnen

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

Labels

bugSomething isn't working

Type

No type

Projects

No projects

Milestone

No milestone

Relationships

None yet

Development

No branches or pull requests

Issue actions