diff --git a/README.md b/README.md index fa93ae0..9fb4cab 100644 --- a/README.md +++ b/README.md @@ -10,6 +10,18 @@ This repo contains a Julia interface to the Quanser hardware-in-the-loop (HIL) S 2. To use the `PythonBackend` Install Quanser python packages as described [here](https://docs.quanser.com/quarc/documentation/python/installation.html) and manually install and load PythonCall (the python backend is an extension). Optionally, set the default backend using `QuanserInterface.set_default_backend("python")`. 3. To use the C backend (default), install the sdk, and if on Linux, possibly symlink `sudo ln -s /usr/lib/x86_64-linux-gnu/libquanser_communications.so.1 /lib/libquanser_communications.so` (or wherever the library is located on your system), I had issues with the `.1` suffix causing Libdl not to find the library. The easiest way to install all the required shared libraries is to follow the python install instructions, i.e., issue the `sudo apt install python3-quanser-apis` after having added their package server. +### Virtual environment + +To install the virtual QLabs environment on MacOS, download and unzip this file: https://download.quanser.com/qlabs/latest/QLabs_Installer_maci64.zip + +For installation on Windows, use : https://download.quanser.com/qlabs/latest/Install%20QLabs.exe + +To carry out the actual installation on MacOS, please run`sudo ./install_QLabs.sh` + + +Once installed, launch it and log in using Quanser credentials. +To control the pendulum, the Quanser Interactive Labs `QUBE 2 – Pendulum > Pendulum workspace` session should be selected + ### Setting preferences Preferences.jl is used to store the default backend choice, the path to the python HIL SDK as well as the default board type. You can set these preferences by running ```julia diff --git a/examples/furuta_lqg.jl b/examples/furuta_lqg.jl index c431f38..9fb1389 100644 --- a/examples/furuta_lqg.jl +++ b/examples/furuta_lqg.jl @@ -3,7 +3,7 @@ Design al LQG controller for the Furuta pendulum in the upright position (xr[2] =# using HardwareAbstractions, QuanserInterface, ControlSystemsBase, RobustAndOptimalControl, LowLevelParticleFilters, StaticArrays, Plots - +using LinearAlgebra # ============================================================================== ## Pendulum Ts = 0.005 diff --git a/examples/swingup.jl b/examples/swingup.jl index f0ef8d2..8f4772e 100644 --- a/examples/swingup.jl +++ b/examples/swingup.jl @@ -7,6 +7,7 @@ using QuanserInterface using HardwareAbstractions using ControlSystemsBase using QuanserInterface: energy, measure +using StaticArrays const rr = Ref([0, pi, 0, 0]) @@ -77,7 +78,7 @@ function swingup(process; Tf = 10, verbose=true, stab=true, umax=2.0) if !(-deg2rad(110) <= y[1] <= deg2rad(110)) u = SA[-0.5*y[1]] verbose && @warn "Correcting" - control(process, u .+ u0) + control(process, Vector(u .+ u0)) oob += 20 if oob > 1000 verbose && @error "Out of bounds" diff --git a/examples/virtual_swingup.jl b/examples/virtual_swingup.jl new file mode 100644 index 0000000..8f54a54 --- /dev/null +++ b/examples/virtual_swingup.jl @@ -0,0 +1,164 @@ +#= +This script performs swingup of the virtual pendulum using an energy-based controller, and stabilizes the pendulum at the top using an LQR controller. +=# +cd(@__DIR__) +using Pkg; Pkg.activate("..") +using QuanserInterface +using HardwareAbstractions +using ControlSystemsBase +using QuanserInterface: energy, measure +using StaticArrays + +#= +Before running script, be sure to have QLabs workspace loaded and run set_environment("virtual pendulum") +=# +const rr = Ref([0, pi, 0, 0]) +nu = 1 # number of controls +nx = 4 # number of states +Ts = 0.005 # sampling time + +function plotD(D, th=0.2) + tvec = D[1, :] + y = D[2:3, :]' + # y[:, 2] .-= pi + # y[:, 2] .*= -1 + xh = D[4:7, :]' + u = D[8, :] + plot(tvec, xh, layout=6, lab=["arm" "pend" "arm ω" "pend ω"] .* " estimate", framestyle=:zerolines) + plot!(tvec, y, sp=[1 2], lab = ["arm" "pend"] .* " meas", framestyle=:zerolines) + hline!([-pi pi], lab="", sp=2) + hline!([-pi-th -pi+th pi-th pi+th], lab="", l=(:black, :dash), sp=2) + plot!(tvec, centraldiff(y) ./ median(diff(tvec)), sp=[3 4], lab="central diff") + plot!(tvec, u, sp=5, lab = "u", framestyle=:zerolines) + plot!(diff(D[1,:]), sp=6, lab="Δt"); hline!([process.Ts], sp=6, framestyle=:zerolines, lab="Ts") +end +normalize_angles(x::Number) = mod(x, 2pi) +normalize_angles(x::AbstractVector) = SA[(x[1]), normalize_angles(x[2]), x[3], x[4]] + +function swingup(process; Tf = 10, verbose=true, stab=true, umax=2.0) + Ts = process.Ts + N = round(Int, Tf/Ts) + data = Vector{Vector{Float64}}(undef, 0) + sizehint!(data, N) + + simulation = processtype(process) isa SimulatedProcess + + if simulation + u0 = 0.0 + else + u0 = 0.5QuanserInterface.go_home(process) + @show u0 + end + y = QuanserInterface.measure(process) + if verbose && !simulation + @info "Starting $(simulation ? "simulation" : "experiment") from y: $y, waiting for your input..." + # readline() + end + yo = @SVector zeros(2) + dyf = @SVector zeros(2) + L = SA[-2.8515070942708687 -24.415803244034326 -0.9920297324372649 -1.9975963404759338] + # L = [-7.410199310542298 -36.40730995983665 -2.0632501290782095 -3.149033572767301] # State-feedback gain Ts = 0.01 + + try + # GC.gc() + GC.enable(false) + t_start = time() + u = [0.0] + oob = 0 + for i = 1:N + @periodically Ts simulation begin + t = simulation ? (i-1)*Ts : time() - t_start + y = QuanserInterface.measure(process) + dy = (y - yo) ./ Ts + dyf = @. 0.5dyf + 0.5dy + xh = [y; dyf] + xhn = SA[xh[1], normalize_angles(xh[2]), xh[3], xh[4]] + r = rr[] + if !(-deg2rad(110) <= y[1] <= deg2rad(110)) + u = SA[-0.5*y[1]] + verbose && @warn "Correcting" + control(process, Vector(u .+ u0)) + oob += 20 + if oob > 600 + verbose && @error "Out of bounds" + break + end + else + oob = max(0, oob-1) + if stab && abs(normalize_angles(y[2]) - pi) < 0.40 + verbose && @info "stabilizing" + # if floor(Int, 2t) % 2 == 0 + # r[1] = -deg2rad(20) + # else + # r[1] = deg2rad(20) + # end + + u = clamp.(L*(r - xhn), -10, 10) + else + # xhn = (process.x) # Try with correct state if in simulation + α = y[2] - pi + αr = r[2] - pi + α̇ = xh[4] + E = energy(α, α̇) + uE = 240*(E - energy(αr,0))*sign(α̇*cos(α)) + u = SA[clamp(uE - 0.2*y[1], -umax, umax)] + end + control(process, Vector(u)) + end + verbose && @info "t = $(round(t, digits=3)), u = $(u[]), xh = $xh" + log = [t; y; xh; u] + push!(data, log) + yo = y + end + end + catch e + @error "Terminating" e + # rethrow() + finally + control(process, [0.0]) + GC.enable(true) + # GC.gc() + end + + D = reduce(hcat, data) +end +## +process = QuanserInterface.QubeServoPendulum(; Ts) +# home!(process, 38) +## +function runplot(process; kwargs...) + rr[][1] = deg2rad(0) + rr[][2] = pi + y = QuanserInterface.measure(process) + if processtype(process) isa SimulatedProcess + process.x = 0*process.x + elseif abs(y[2]) > 0.8 || !(-2.5 < y[1] < 2.5) + @info "Auto homing" + autohome!(process) + end + global D + D = swingup(process; kwargs...) + plotD(D) +end + +runplot(process; Tf = 1500) + +## Simulated process +process = QuanserInterface.QubeServoPendulumSimulator(; Ts, p = QuanserInterface.pendulum_parameters(true)); + +@profview_allocs runplot(process; Tf = 5) sample_rate=0.1 + +## + +task = @spawn runplot(process; Tf = 15) +rr[][1] = deg2rad(-30) +rr[][1] = deg2rad(-20) +rr[][1] = deg2rad(-10) +rr[][1] = deg2rad(0) +rr[][1] = deg2rad(10) +rr[][1] = deg2rad(20) +rr[][1] = deg2rad(30) + + +rr[][2] = pi +rr[][2] = 0 \ No newline at end of file diff --git a/src/backends.jl b/src/backends.jl index b9679e1..5d52097 100644 --- a/src/backends.jl +++ b/src/backends.jl @@ -18,6 +18,28 @@ function get_board() @load_preference("board", "qube_servo3_usb") end +""" + set_environment(board_id) + +Set the environment to use, the default is `"physical"`. To use virtual pendulum, pass "virtual pendulum". To use virtual DC motor pass "virtual DC motor" +""" +function set_environment(board_id) + if board_id == "physical" + @set_preferences!("board_identifier" => "0") + @info("New environment set to physical device") + elseif board_id == "virtual pendulum" + @set_preferences!("board_identifier" => "0@tcpip://localhost:18921") + @info("New environment set to Virtual Pendulum") + elseif board_id == "virtual DC motor" + @set_preferences!("board_identifier" => "0@tcpip://localhost:18920?nagle='off") + @info("New environment set to Virtual DC motor") + end +end + +function get_environment() + @load_preference("board_identifier", "0") +end + """ set_default_backend(backend) @@ -115,7 +137,7 @@ function load_default_backend(::Type{CBackend}; encoder_read_buffer::Vector{Int32}, analog_read_buffer::Vector{Int32}, board = get_board(), - board_identifier = "0", + board_identifier = get_environment(), ) if QuanserBindings.hil_is_valid(cardC[]) == Int8(1)