Skip to content

Commit 48064fd

Browse files
baggepinnenbaggepinnen
authored
Merge pull request #6 from JuliaComputing/rsv_vboardinstruct
Info and debugging virtual interface
2 parents 22e1e7d + 36f3930 commit 48064fd

File tree

5 files changed

+202
-3
lines changed

5 files changed

+202
-3
lines changed

‎README.md

+12
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,18 @@ This repo contains a Julia interface to the Quanser hardware-in-the-loop (HIL) S
1010
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")`.
1111
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.
1212

13+
### Virtual environment
14+
15+
To install the virtual QLabs environment on MacOS, download and unzip this file: https://download.quanser.com/qlabs/latest/QLabs_Installer_maci64.zip
16+
17+
For installation on Windows, use : https://download.quanser.com/qlabs/latest/Install%20QLabs.exe
18+
19+
To carry out the actual installation on MacOS, please run`sudo ./install_QLabs.sh`
20+
21+
22+
Once installed, launch it and log in using Quanser credentials.
23+
To control the pendulum, the Quanser Interactive Labs `QUBE 2 – Pendulum > Pendulum workspace` session should be selected
24+
1325
### Setting preferences
1426
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
1527
```julia

‎examples/furuta_lqg.jl

+1-1
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@ Design al LQG controller for the Furuta pendulum in the upright position (xr[2]
33
=#
44

55
using HardwareAbstractions, QuanserInterface, ControlSystemsBase, RobustAndOptimalControl, LowLevelParticleFilters, StaticArrays, Plots
6-
6+
using LinearAlgebra
77
# ==============================================================================
88
## Pendulum
99
Ts = 0.005

‎examples/swingup.jl

+2-1
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@ using QuanserInterface
77
using HardwareAbstractions
88
using ControlSystemsBase
99
using QuanserInterface: energy, measure
10+
using StaticArrays
1011

1112

1213
const rr = Ref([0, pi, 0, 0])
@@ -77,7 +78,7 @@ function swingup(process; Tf = 10, verbose=true, stab=true, umax=2.0)
7778
if !(-deg2rad(110) <= y[1] <= deg2rad(110))
7879
u = SA[-0.5*y[1]]
7980
verbose && @warn "Correcting"
80-
control(process, u .+ u0)
81+
control(process, Vector(u .+ u0))
8182
oob += 20
8283
if oob > 1000
8384
verbose && @error "Out of bounds"

‎examples/virtual_swingup.jl

+164
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,164 @@
1+
#=
2+
This script performs swingup of the virtual pendulum using an energy-based controller, and stabilizes the pendulum at the top using an LQR controller.
3+
=#
4+
cd(@__DIR__)
5+
using Pkg; Pkg.activate("..")
6+
using QuanserInterface
7+
using HardwareAbstractions
8+
using ControlSystemsBase
9+
using QuanserInterface: energy, measure
10+
using StaticArrays
11+
12+
#=
13+
Before running script, be sure to have QLabs workspace loaded and run set_environment("virtual pendulum")
14+
=#
15+
const rr = Ref([0, pi, 0, 0])
16+
nu = 1 # number of controls
17+
nx = 4 # number of states
18+
Ts = 0.005 # sampling time
19+
20+
function plotD(D, th=0.2)
21+
tvec = D[1, :]
22+
y = D[2:3, :]'
23+
# y[:, 2] .-= pi
24+
# y[:, 2] .*= -1
25+
xh = D[4:7, :]'
26+
u = D[8, :]
27+
plot(tvec, xh, layout=6, lab=["arm" "pend" "arm ω" "pend ω"] .* " estimate", framestyle=:zerolines)
28+
plot!(tvec, y, sp=[1 2], lab = ["arm" "pend"] .* " meas", framestyle=:zerolines)
29+
hline!([-pi pi], lab="", sp=2)
30+
hline!([-pi-th -pi+th pi-th pi+th], lab="", l=(:black, :dash), sp=2)
31+
plot!(tvec, centraldiff(y) ./ median(diff(tvec)), sp=[3 4], lab="central diff")
32+
plot!(tvec, u, sp=5, lab = "u", framestyle=:zerolines)
33+
plot!(diff(D[1,:]), sp=6, lab="Δt"); hline!([process.Ts], sp=6, framestyle=:zerolines, lab="Ts")
34+
end
35+
normalize_angles(x::Number) = mod(x, 2pi)
36+
normalize_angles(x::AbstractVector) = SA[(x[1]), normalize_angles(x[2]), x[3], x[4]]
37+
38+
function swingup(process; Tf = 10, verbose=true, stab=true, umax=2.0)
39+
Ts = process.Ts
40+
N = round(Int, Tf/Ts)
41+
data = Vector{Vector{Float64}}(undef, 0)
42+
sizehint!(data, N)
43+
44+
simulation = processtype(process) isa SimulatedProcess
45+
46+
if simulation
47+
u0 = 0.0
48+
else
49+
u0 = 0.5QuanserInterface.go_home(process)
50+
@show u0
51+
end
52+
y = QuanserInterface.measure(process)
53+
if verbose && !simulation
54+
@info "Starting $(simulation ? "simulation" : "experiment") from y: $y, waiting for your input..."
55+
# readline()
56+
end
57+
yo = @SVector zeros(2)
58+
dyf = @SVector zeros(2)
59+
L = SA[-2.8515070942708687 -24.415803244034326 -0.9920297324372649 -1.9975963404759338]
60+
# L = [-7.410199310542298 -36.40730995983665 -2.0632501290782095 -3.149033572767301] # State-feedback gain Ts = 0.01
61+
62+
try
63+
# GC.gc()
64+
GC.enable(false)
65+
t_start = time()
66+
u = [0.0]
67+
oob = 0
68+
for i = 1:N
69+
@periodically Ts simulation begin
70+
t = simulation ? (i-1)*Ts : time() - t_start
71+
y = QuanserInterface.measure(process)
72+
dy = (y - yo) ./ Ts
73+
dyf = @. 0.5dyf + 0.5dy
74+
xh = [y; dyf]
75+
xhn = SA[xh[1], normalize_angles(xh[2]), xh[3], xh[4]]
76+
r = rr[]
77+
if !(-deg2rad(110) <= y[1] <= deg2rad(110))
78+
u = SA[-0.5*y[1]]
79+
verbose && @warn "Correcting"
80+
control(process, Vector(u .+ u0))
81+
oob += 20
82+
if oob > 600
83+
verbose && @error "Out of bounds"
84+
break
85+
end
86+
else
87+
oob = max(0, oob-1)
88+
if stab && abs(normalize_angles(y[2]) - pi) < 0.40
89+
verbose && @info "stabilizing"
90+
# if floor(Int, 2t) % 2 == 0
91+
# r[1] = -deg2rad(20)
92+
# else
93+
# r[1] = deg2rad(20)
94+
# end
95+
96+
u = clamp.(L*(r - xhn), -10, 10)
97+
else
98+
# xhn = (process.x) # Try with correct state if in simulation
99+
α = y[2] - pi
100+
αr = r[2] - pi
101+
α̇ = xh[4]
102+
E = energy(α, α̇)
103+
uE = 240*(E - energy(αr,0))*sign(α̇*cos(α))
104+
u = SA[clamp(uE - 0.2*y[1], -umax, umax)]
105+
end
106+
control(process, Vector(u))
107+
end
108+
verbose && @info "t = $(round(t, digits=3)), u = $(u[]), xh = $xh"
109+
log = [t; y; xh; u]
110+
push!(data, log)
111+
yo = y
112+
end
113+
end
114+
catch e
115+
@error "Terminating" e
116+
# rethrow()
117+
finally
118+
control(process, [0.0])
119+
GC.enable(true)
120+
# GC.gc()
121+
end
122+
123+
D = reduce(hcat, data)
124+
end
125+
##
126+
process = QuanserInterface.QubeServoPendulum(; Ts)
127+
# home!(process, 38)
128+
##
129+
function runplot(process; kwargs...)
130+
rr[][1] = deg2rad(0)
131+
rr[][2] = pi
132+
y = QuanserInterface.measure(process)
133+
if processtype(process) isa SimulatedProcess
134+
process.x = 0*process.x
135+
elseif abs(y[2]) > 0.8 || !(-2.5 < y[1] < 2.5)
136+
@info "Auto homing"
137+
autohome!(process)
138+
end
139+
global D
140+
D = swingup(process; kwargs...)
141+
plotD(D)
142+
end
143+
144+
runplot(process; Tf = 1500)
145+
146+
## Simulated process
147+
process = QuanserInterface.QubeServoPendulumSimulator(; Ts, p = QuanserInterface.pendulum_parameters(true));
148+
149+
@profview_allocs runplot(process; Tf = 5) sample_rate=0.1
150+
151+
##
152+
153+
task = @spawn runplot(process; Tf = 15)
154+
rr[][1] = deg2rad(-30)
155+
rr[][1] = deg2rad(-20)
156+
rr[][1] = deg2rad(-10)
157+
rr[][1] = deg2rad(0)
158+
rr[][1] = deg2rad(10)
159+
rr[][1] = deg2rad(20)
160+
rr[][1] = deg2rad(30)
161+
162+
163+
rr[][2] = pi
164+
rr[][2] = 0

‎src/backends.jl

+23-1
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,28 @@ function get_board()
1818
@load_preference("board", "qube_servo3_usb")
1919
end
2020

21+
"""
22+
set_environment(board_id)
23+
24+
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"
25+
"""
26+
function set_environment(board_id)
27+
if board_id == "physical"
28+
@set_preferences!("board_identifier" => "0")
29+
@info("New environment set to physical device")
30+
elseif board_id == "virtual pendulum"
31+
@set_preferences!("board_identifier" => "0@tcpip://localhost:18921")
32+
@info("New environment set to Virtual Pendulum")
33+
elseif board_id == "virtual DC motor"
34+
@set_preferences!("board_identifier" => "0@tcpip://localhost:18920?nagle='off")
35+
@info("New environment set to Virtual DC motor")
36+
end
37+
end
38+
39+
function get_environment()
40+
@load_preference("board_identifier", "0")
41+
end
42+
2143
"""
2244
set_default_backend(backend)
2345
@@ -115,7 +137,7 @@ function load_default_backend(::Type{CBackend};
115137
encoder_read_buffer::Vector{Int32},
116138
analog_read_buffer::Vector{Int32},
117139
board = get_board(),
118-
board_identifier = "0",
140+
board_identifier = get_environment(),
119141
)
120142

121143
if QuanserBindings.hil_is_valid(cardC[]) == Int8(1)

0 commit comments

Comments
 (0)