add SynchJulia swingup script

Project.toml

@@ -1,12 +1,10 @@
 name = "QuanserInterface"
 uuid = "d7748c0a-89fb-413b-a9f0-29aba34b281f"
-authors = ["Fredrik Bagge Carlson"]
 version = "1.0.0-DEV"
+authors = ["Fredrik Bagge Carlson"]
 
 [deps]
-CBinding = "d43a6710-96b8-4a2d-833c-c424785e5374"
 CEnum = "fa961155-64e5-5f13-b03f-caf6b980ea82"
-Clang = "40e3b903-d033-50b4-a0cc-940c62c95e31"
 ControlSystemIdentification = "3abffc1c-5106-53b7-b354-a47bfc086282"
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
 DSP = "717857b8-e6f2-59f4-9121-6e50c889abd2"
@@ -18,19 +16,17 @@ Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
 Preferences = "21216c6a-2e73-6563-6e65-726566657250"
 RobustAndOptimalControl = "21fd56a4-db03-40ee-82ee-a87907bee541"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
-
+SynchCompiler = "5d9dccf6-a926-4748-b7e2-6521ccc431d1"
+SynchJulia = "a1b2c3d4-5e6f-7a8b-9c0d-e1f2a3b4c5d6"
 
 [weakdeps]
 PythonCall = "6099a3de-0909-46bc-b1f4-468b9a2dfc0d"
 
 [extensions]
 QuanserInterfacePythonCallExt = ["PythonCall"]
 
-
 [compat]
-CBinding = "1.0.10"
 CEnum = "0.4.2"
-Clang = "0.17.6"
 ControlSystemIdentification = "2.6"
 ControlSystemsBase = "1.7.0"
 DSP = "0.7.8"
@@ -44,8 +40,8 @@ StaticArrays = "1.5"
 julia = "1.9"
 
 [extras]
-Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 PythonCall = "6099a3de-0909-46bc-b1f4-468b9a2dfc0d"
+Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
 test = ["Test"]

examples/swingup_synch.jl

@@ -0,0 +1,146 @@
+#=
+Pendulum swingup controller reimplemented using the SynchJulia synchronous dataflow DSL.
+The controller is decomposed into separate @node functions:
+- velocity_estimator: finite-difference + exponential smoothing
+- energy_swingup: energy-based swingup controller
+- lqr_stabilizer: LQR state-feedback stabilization
+- swingup_node: top-level mode-switching orchestrator
+
+Runs on the simulated process only.
+=#
+cd(@__DIR__)
+using Pkg; Pkg.activate("..")
+using QuanserInterface
+using HardwareAbstractions
+using SynchCompiler, SynchJulia
+using StaticArrays
+
+# ==============================================================================
+## FFI helper functions (called from @node via FFI)
+# ==============================================================================
+
+function energy(α::Float64, α̇::Float64)::Float64
+    mp = 0.024
+    Lp = 0.129
+    g = 9.81
+    l = Lp / 2
+    Jp_cm = mp * Lp^2 / 12
+    return 0.5 * Jp_cm * α̇^2 + mp * g * l * (1 + cos(α))
+end
+
+
+# ==============================================================================
+## SynchJulia nodes
+# ==============================================================================
+
+# Velocity estimation: finite-difference differentiation + first-order exponential filter
+@node function velocityestimator(y::Float64, ts::Float64)::(dyfiltered::Float64)
+    yo = pre(y; init=0.0)
+    dy = (y - yo) / ts
+    prevdyf = pre(dyfiltered; init=0.0)
+    dyfiltered = 0.5 * prevdyf + 0.5 * dy
+end
+
+# Energy-based swingup controller
+@node function energyswingup(θ::Float64, α::Float64, α̇::Float64, umax::Float64)::(u::Float64)
+    αshifted = α - π
+    e = energy(αshifted, α̇)
+    eref = energy(0.0, 0.0)
+    ue = 80.0 * (e - eref) * sign(α̇ * cos(αshifted))
+    u = clamp(ue - 0.2 * θ, -umax, umax)
+end
+
+# LQR stabilization controller (gains designed for ts=0.01)
+@node function lqrstabilizer(θ::Float64, αnorm::Float64, dθ::Float64, dα::Float64)::(u::Float64)
+    e1 = 0.0 - θ
+    e2 = π - αnorm
+    e3 = 0.0 - dθ
+    e4 = 0.0 - dα
+    uraw = -7.410199310542298 * e1 + -36.40730995983665 * e2 + -2.0632501290782095 * e3 + -3.149033572767301 * e4
+    u = clamp(uraw, -10.0, 10.0)
+end
+
+# Top-level swingup controller: mode switching between OOB correction, LQR, and energy swingup
+@node function swingupnode(θ::Float64, α::Float64, ts::Float64, umax::Float64)::(u::Float64)
+    # Velocity estimation (each call site gets independent state)
+    dθ = velocityestimator(θ, ts)
+    dα = velocityestimator(α, ts)
+
+    # Normalize pendulum angle to [0, 2π)
+    αnorm = mod(α, 2pi)
+
+    # Mode conditions
+    ooblimit = deg2rad(110)
+    neartop = abs(αnorm - π) < 0.40
+    outofbounds = (θ > ooblimit) || (θ < -ooblimit)
+
+    # Compute control for each mode
+    uoob = -0.5 * θ
+    ulqr = lqrstabilizer(θ, αnorm, dθ, dα)
+    uswingup = energyswingup(θ, α, dα, umax)
+
+    # Select active mode (priority: OOB > LQR > swingup)
+    u = if outofbounds
+        uoob
+    else
+        if neartop
+            ulqr
+        else
+            uswingup
+        end
+    end
+end
+
+# ==============================================================================
+## Build and simulate
+# ==============================================================================
+
+exe = SynchExecutable(swingupnode, (Float64, Float64, Float64, Float64))
+
+Ts = 0.01
+process = QuanserInterface.QubeServoPendulumSimulator(; Ts, p = QuanserInterface.pendulum_parameters(true))
+
+Tf = 15.0
+N = round(Int, Tf / Ts)
+data = Vector{Vector{Float64}}(undef, 0)
+sizehint!(data, N)
+
+for i in 1:N
+    y = QuanserInterface.measure(process)
+    θ, α = y[1], y[2]
+
+    result = step!(exe, θ, α, Ts, 2.0)
+    u = result.u
+
+    control(process, [u])
+
+    t = (i - 1) * Ts
+    push!(data, [t, θ, α, u])
+end
+control(process, [0.0])
+
+D = reduce(hcat, data)
+
+# Verify: pendulum should be near [0, π]
+using LinearAlgebra
+final_state = D[2:3, end]
+@info "Final state: θ=$(final_state[1]), α=$(final_state[2]), target: [0, ±π]"
+
+# ==============================================================================
+## Plot results
+# ==============================================================================
+using Plots
+
+tvec = D[1, :]
+θvec = D[2, :]
+αvec = D[3, :]
+uvec = D[4, :]
+
+plot(
+    plot(tvec, θvec, lab="arm θ", ylabel="rad", framestyle=:zerolines),
+    plot(tvec, αvec, lab="pend α", ylabel="rad", framestyle=:zerolines,
+         legend=:right),
+    plot(tvec, uvec, lab="u", ylabel="V", xlabel="t [s]", framestyle=:zerolines),
+    layout=(3, 1), size=(800, 600), link=:x
+)
+hline!([π -π], sp=2, lab="", l=(:black, :dash))