Skip to content

LQR redesign for delay tolerance + velocity-aware catch condition (opt-in) #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
baggepinnen wants to merge 4 commits into
base: robust-swingup
Choose a base branch
from
Open

LQR redesign for delay tolerance + velocity-aware catch condition (opt-in) #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
455c2a1
Add velocity-aware ellipsoidal catch condition (opt-in)
baggepinnen Jul 10, 2026
de293a6
Redesign the LQR from the model linearization for delay tolerance
baggepinnen Jul 10, 2026
2190f7b
Update ellipsoid comparison numbers for the redesigned LQR
baggepinnen Jul 10, 2026
fd2cfbb
Merge branch 'robust-swingup' into velocity-aware-switch
baggepinnen Jul 11, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
107 changes: 89 additions & 18 deletions dyad/furuta_with_swingup.dyad
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -666,7 +666,7 @@ component ErefAdaptation@[input clk extends Discrete]
alpha = RealInput@[clk]() {^alpha}
"Estimated pendulum angular velocity"
dalpha = RealInput@[clk]() {^dalpha}
"Stabilization-active flag from NearTop"
"Stabilization-active flag from the catch condition"
neartop = RealInput@[clk]() {^neartop}
"Adaptive relative energy-reference margin"
eta = RealOutput@[clk]() {^eta}
Expand Down Expand Up @@ -785,11 +785,11 @@ component LQRstabilizer
shoulder_velocity = Dyad.RealInput() {^shoulder_velocity}
elbow_velocity = Dyad.RealInput() {^elbow_velocity}
u = Dyad.RealOutput() {^u}
"State-feedback gain vector applied to the error vector [shoulder_angle, elbow_angle, shoulder_velocity, elbow_velocity]"
parameter L1::Real = -9.625743176817387
parameter L2::Real = 394.43972658274106
parameter L3::Real = -7.461418005226849
parameter L4::Real = 84.42279971138271
"State-feedback gain vector applied to the error vector [shoulder_angle, elbow_angle, shoulder_velocity, elbow_velocity]. Discrete LQR from the plant linearization about upright with output weights diag([1000, 10, 1, 1]) and control weight 300, selected in robustness/08_lqr_redesign.jl for delay tolerance: catches with three samples of actuation delay where the previous gains failed with one, at equal catch capability."
parameter L1::Real = -1.7259853130770042
parameter L2::Real = 68.2810119491725
parameter L3::Real = -1.2317839406561077
parameter L4::Real = 7.7052139179986066
variable e1::Real
variable e2::Real
variable e3::Real
Expand Down Expand Up @@ -859,7 +859,7 @@ component Swingup
energyswingup = QuanserComponents.EnergySwingup() {^energyswingup}
anglenormalization = QuanserComponents.AngleNormalization() {^anglenormalization}
lqrstabilizer = QuanserComponents.LQRstabilizer() {^lqrstabilizer}
neartop = QuanserComponents.NearTop() {^neartop}
catchcondition = QuanserComponents.CatchCondition() {^catchcondition}
stabilizationswitch = QuanserComponents.StabilizationSwitch() {^stabilizationswitch}
erefadaptation = QuanserComponents.ErefAdaptation() {^erefadaptation}
alphabeta_shoulder = QuanserComponents.AlphaBetaEstimator() {^alphabeta_shoulder}
Expand All @@ -869,19 +869,19 @@ component Swingup
gain = BlockComponents.Math.Gain(k = 1.0) {^gain}
relations
connect(anglenormalization.y, lqrstabilizer.elbow_angle, energyswingup.elbow_angle) {^id12}
connect(lqrstabilizer.shoulder_velocity, estimatorswitch_shoulder.y, energyswingup.shoulder_velocity) {^id13}
connect(neartop.u, anglenormalization.y) {^id14}
connect(lqrstabilizer.elbow_velocity, estimatorswitch_elbow.y, energyswingup.elbow_velocity) {^id15}
connect(shoulder_angle, energyswingup.shoulder_angle, velocityestimator_shoulder.pos, alphabeta_shoulder.pos, lqrstabilizer.shoulder_angle) {^id16}
connect(neartop.y, stabilizationswitch.neartop) {^id17}
connect(lqrstabilizer.shoulder_velocity, estimatorswitch_shoulder.y, energyswingup.shoulder_velocity, catchcondition.shoulder_velocity) {^id13}
connect(catchcondition.elbow_angle, anglenormalization.y) {^id14}
connect(lqrstabilizer.elbow_velocity, estimatorswitch_elbow.y, energyswingup.elbow_velocity, catchcondition.elbow_velocity) {^id15}
connect(shoulder_angle, energyswingup.shoulder_angle, velocityestimator_shoulder.pos, alphabeta_shoulder.pos, lqrstabilizer.shoulder_angle, catchcondition.shoulder_angle) {^id16}
connect(catchcondition.y, stabilizationswitch.neartop) {^id17}
connect(energyswingup.realoutput, stabilizationswitch.u_swingup) {^id18}
connect(lqrstabilizer.u, stabilizationswitch.u_lqr) {^id19}
connect(velocityestimator_elbow.pos, elbow_angle, alphabeta_elbow.pos, anglenormalization.u) {^id20}
connect(stabilizationswitch.u, gain.u) {^id21}
connect(gain.y, u) {^id22}
connect(anglenormalization.y, erefadaptation.alpha) {^id23}
connect(estimatorswitch_elbow.y, erefadaptation.dalpha) {^id24}
connect(neartop.y, erefadaptation.neartop) {^id25}
connect(catchcondition.y, erefadaptation.neartop) {^id25}
connect(erefadaptation.eta, energyswingup.eta_adapt) {^id26}
connect(velocityestimator_shoulder.vel, estimatorswitch_shoulder.v_old) {^id27}
connect(alphabeta_shoulder.vel, estimatorswitch_shoulder.v_new) {^id28}
Expand Down Expand Up @@ -954,7 +954,7 @@ metadata {
"tags": []
}
},
"neartop": {
"catchcondition": {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 330, "y1": 20, "x2": 430, "y2": 120, "rot": 0}
Expand Down Expand Up @@ -1060,12 +1060,83 @@ metadata {
}
end

component NearTop
extends BlockComponents.Interfaces.SISO
"""
Velocity-aware stabilization-engage condition with hysteresis.

The stabilizer is engaged when the measured deviation from the upright
equilibrium, e = [θ, φ - π, θ̇, φ̇], lies inside the ellipsoidal sublevel
set V = e'Se ≤ 1 of a Lyapunov function of the implemented LQR loop, and
disengaged only when V > c_release. Compared to the previous angle-only
threshold this refuses handover at pendulum velocities the LQR cannot
catch, and the hysteresis prevents chattering between the controllers.

S is normalized so that the engage threshold is 1. Its default entries and
c_release are designed and calibrated against empirical catch-region grids
of perturbed plants in robustness/07_design_catch_condition.jl; the
ellipsoid contains no state that failed to be caught on any tested plant.

With `use_ellipsoid = false` (the default) the component reduces to the
original angle-only condition |φ - π| < th without hysteresis. The
ellipsoidal gate is opt-in: in the simulated mismatch campaign it lowers
the success rate (failed catch attempts are cheap in simulation and the
gate is computed from the nominal model, which is exactly what mismatch
invalidates), but it produces gentle, chatter-free handovers — mean arrival
speed less than half of the angle-only switch — which matters when failed
catch attempts stress real hardware. See robustness/README.md.
"""
component CatchCondition@[input clk extends Discrete]
"Shoulder angle"
shoulder_angle = RealInput@[clk]() {^shoulder_angle}
"Normalized elbow angle in [0, 2pi), pi at upright"
elbow_angle = RealInput@[clk]() {^elbow_angle}
"Estimated shoulder velocity"
shoulder_velocity = RealInput@[clk]() {^shoulder_velocity}
"Estimated elbow velocity"
elbow_velocity = RealInput@[clk]() {^elbow_velocity}
"True when the stabilizing controller should be active"
y = RealOutput@[clk]() {^y}
"If true, use the ellipsoidal condition with hysteresis, otherwise the angle-only threshold"
parameter use_ellipsoid::Boolean = false
"Angle threshold of the fallback angle-only condition"
parameter th::Real = 0.4
"Engage threshold: V = 1 is the largest sublevel set with no observed catch failure on any tested plant; larger values trade certain catches for earlier attempts"
parameter c_engage::Real = 1
"Release threshold of the hysteresis"
parameter c_release::Real = 4
parameter S11::Real = 0.156633
parameter S12::Real = -1.00123
parameter S13::Real = 0.0529337
parameter S14::Real = -0.119659
parameter S22::Real = 18.9434
parameter S23::Real = -0.562122
parameter S24::Real = 2.18534
parameter S33::Real = 0.026693
parameter S34::Real = -0.0670345
parameter S44::Real = 0.252618
"Deviation from the upright equilibrium"
variable e1::Real
variable e2::Real
variable e3::Real
variable e4::Real
"Lyapunov-function value, engage when at most 1"
variable V::Real
"Stabilizer-engaged state (hysteresis memory)"
variable engaged::Real
relations
y = abs(u - π) < th
metadata {"Dyad": {"icons": {"default": "dyad://DiscreteComponents/NearTop.svg"}}}
e1 = shoulder_angle
e2 = elbow_angle - pi
e3 = shoulder_velocity
e4 = elbow_velocity
V = S11 * e1 ^ 2 + S22 * e2 ^ 2 + S33 * e3 ^ 2 + S44 * e4 ^ 2 +
2 * (S12 * e1 * e2 + S13 * e1 * e3 + S14 * e1 * e4 +
S23 * e2 * e3 + S24 * e2 * e4 + S34 * e3 * e4)
engaged@clk = ifelse(use_ellipsoid == true,
ifelse(engaged@(clk-1) > 0.5,
ifelse(V <= c_release, 1, 0),
ifelse(V <= c_engage, 1, 0)),
ifelse(abs(elbow_angle - pi) < th, 1, 0))
y = engaged
initial engaged@(clk-1) = 0
end

component StabilizationSwitch
Expand Down
206 changes: 206 additions & 0 deletions generated/CatchCondition_definition.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,206 @@
### DO NOT EDIT THIS FILE
### This file is auto-generated by the Dyad command-line compiler.
### If you edit this code it is likely to get overwritten.
### Instead, update the Dyad source code and regenerate this file


import Moshi as __Ext__Moshi

@doc Markdown.doc"""
CatchCondition(; name, use_ellipsoid, th, c_engage, c_release, S11, S12, S13, S14, S22, S23, S24, S33, S34, S44)

Velocity-aware stabilization-engage condition with hysteresis.

The stabilizer is engaged when the measured deviation from the upright
equilibrium, e = [θ, φ - π, θ̇, φ̇], lies inside the ellipsoidal sublevel
set V = e'Se ≤ 1 of a Lyapunov function of the implemented LQR loop, and
disengaged only when V > c_release. Compared to the previous angle-only
threshold this refuses handover at pendulum velocities the LQR cannot
catch, and the hysteresis prevents chattering between the controllers.

S is normalized so that the engage threshold is 1. Its default entries and
c_release are designed and calibrated against empirical catch-region grids
of perturbed plants in robustness/07_design_catch_condition.jl; the
ellipsoid contains no state that failed to be caught on any tested plant.

With `use_ellipsoid = false` (the default) the component reduces to the
original angle-only condition |φ - π| < th without hysteresis. The
ellipsoidal gate is opt-in: in the simulated mismatch campaign it lowers
the success rate (failed catch attempts are cheap in simulation and the
gate is computed from the nominal model, which is exactly what mismatch
invalidates), but it produces gentle, chatter-free handovers — mean arrival
speed less than half of the angle-only switch — which matters when failed
catch attempts stress real hardware. See robustness/README.md.

## Parameters:

| Name | Description | Units | Default value |
| ------------ | ----------------------------------- | ------ | --------------- |
| `use_ellipsoid` | If true, use the ellipsoidal condition with hysteresis, otherwise the angle-only threshold | -- | false |
| `th` | Angle threshold of the fallback angle-only condition | -- | 0.4 |
| `c_engage` | Engage threshold: V = 1 is the largest sublevel set with no observed catch failure on any tested plant; larger values trade certain catches for earlier attempts | -- | 1 |
| `c_release` | Release threshold of the hysteresis | -- | 4 |
| `S11` | | -- | 0.156633 |
| `S12` | | -- | -1.00123 |
| `S13` | | -- | 0.0529337 |
| `S14` | | -- | -0.119659 |
| `S22` | | -- | 18.9434 |
| `S23` | | -- | -0.562122 |
| `S24` | | -- | 2.18534 |
| `S33` | | -- | 0.026693 |
| `S34` | | -- | -0.0670345 |
| `S44` | | -- | 0.252618 |

## Connectors

* `shoulder_angle` - This connector represents a real signal as an input to a component ([`RealInput`](@ref))
* `elbow_angle` - This connector represents a real signal as an input to a component ([`RealInput`](@ref))
* `shoulder_velocity` - This connector represents a real signal as an input to a component ([`RealInput`](@ref))
* `elbow_velocity` - This connector represents a real signal as an input to a component ([`RealInput`](@ref))
* `y` - This connector represents a real signal as an output from a component ([`RealOutput`](@ref))

## Variables

| Name | Description | Units |
| ------------ | ----------------------------------- | ------ |
| `e1` | Deviation from the upright equilibrium | -- |
| `e2` | | -- |
| `e3` | | -- |
| `e4` | | -- |
| `V` | Lyapunov-function value, engage when at most 1 | -- |
| `engaged` | Stabilizer-engaged state (hysteresis memory) | -- |
"""
@component function CatchCondition(; name = nothing, use_ellipsoid=false, th=0.4, c_engage=Float64(1), c_release=Float64(4), S11=0.156633, S12=-1.00123, S13=0.0529337, S14=-0.119659, S22=18.9434, S23=-0.562122, S24=2.18534, S33=0.026693, S34=-0.0670345, S44=0.252618, kwargs...)
isnothing(name) && throw(ArgumentError("""
The `name` keyword must be provided. Please consider using the `@named` macro,
like so:

@named model = CatchCondition()
"""))

__overrides = __build_overrides(kwargs)
__params = Symbolics.SymbolicT[]
__vars = Symbolics.SymbolicT[]
__systems = System[]
__guesses = Dict{Symbolics.SymbolicT, Symbolics.SymbolicT}()
__initial_conditions = Dict{Symbolics.SymbolicT, Symbolics.SymbolicT}()
__initialization_eqs = Equation[]
__eqs = Equation[]
__bindings = Dict{Symbolics.SymbolicT, Symbolics.SymbolicT}()

### Structural Parameters (functions)

### Structural Parameters (Final)

### Path Parameters (functions)

### Path Parameters (non-final)

### Final Parameters (declarations)

### Deferred assignment (default values that depend on final parameters)

### Symbolic Parameters
__local__use_ellipsoid = use_ellipsoid
append!(__params, @parameters (use_ellipsoid::Bool), [description = "If true, use the ellipsoidal condition with hysteresis, otherwise the angle-only threshold"])
__initial_conditions[use_ellipsoid] = __local__use_ellipsoid
__local__th = th
append!(__params, @parameters (th::Real), [description = "Angle threshold of the fallback angle-only condition"])
__initial_conditions[th] = __local__th
__local__c_engage = c_engage
append!(__params, @parameters (c_engage::Real), [description = "Engage threshold: V = 1 is the largest sublevel set with no observed catch failure on any tested plant; larger values trade certain catches for earlier attempts"])
__initial_conditions[c_engage] = __local__c_engage
__local__c_release = c_release
append!(__params, @parameters (c_release::Real), [description = "Release threshold of the hysteresis"])
__initial_conditions[c_release] = __local__c_release
__local__S11 = S11
append!(__params, @parameters (S11::Real))
__initial_conditions[S11] = __local__S11
__local__S12 = S12
append!(__params, @parameters (S12::Real))
__initial_conditions[S12] = __local__S12
__local__S13 = S13
append!(__params, @parameters (S13::Real))
__initial_conditions[S13] = __local__S13
__local__S14 = S14
append!(__params, @parameters (S14::Real))
__initial_conditions[S14] = __local__S14
__local__S22 = S22
append!(__params, @parameters (S22::Real))
__initial_conditions[S22] = __local__S22
__local__S23 = S23
append!(__params, @parameters (S23::Real))
__initial_conditions[S23] = __local__S23
__local__S24 = S24
append!(__params, @parameters (S24::Real))
__initial_conditions[S24] = __local__S24
__local__S33 = S33
append!(__params, @parameters (S33::Real))
__initial_conditions[S33] = __local__S33
__local__S34 = S34
append!(__params, @parameters (S34::Real))
__initial_conditions[S34] = __local__S34
__local__S44 = S44
append!(__params, @parameters (S44::Real))
__initial_conditions[S44] = __local__S44

### Final Parameters (assignments)

### Final Path Parameters
append!(__vars, @variables (shoulder_angle(t)::Real), [input = true])
append!(__vars, @variables (elbow_angle(t)::Real), [input = true])
append!(__vars, @variables (shoulder_velocity(t)::Real), [input = true])
append!(__vars, @variables (elbow_velocity(t)::Real), [input = true])
append!(__vars, @variables (y(t)::Real), [output = true])

### Variables (declarations)
append!(__vars, @variables (e1(t)::Real), [description = "Deviation from the upright equilibrium"])
append!(__vars, @variables (e2(t)::Real))
append!(__vars, @variables (e3(t)::Real))
append!(__vars, @variables (e4(t)::Real))
append!(__vars, @variables (V(t)::Real), [description = "Lyapunov-function value, engage when at most 1"])
append!(__vars, @variables (engaged(t)::Real), [description = "Stabilizer-engaged state (hysteresis memory)"])

### Variables (assignments)
__ovr_e1 = pop!(__overrides, "e1", nothing); isnothing(__ovr_e1) || push!(__eqs, e1 ~ __ovr_e1)
__ovr_e1__initial = pop!(__overrides, "e1__initial", nothing); isnothing(__ovr_e1__initial) || (__initial_conditions[e1] = __ovr_e1__initial)
__ovr_e2 = pop!(__overrides, "e2", nothing); isnothing(__ovr_e2) || push!(__eqs, e2 ~ __ovr_e2)
__ovr_e2__initial = pop!(__overrides, "e2__initial", nothing); isnothing(__ovr_e2__initial) || (__initial_conditions[e2] = __ovr_e2__initial)
__ovr_e3 = pop!(__overrides, "e3", nothing); isnothing(__ovr_e3) || push!(__eqs, e3 ~ __ovr_e3)
__ovr_e3__initial = pop!(__overrides, "e3__initial", nothing); isnothing(__ovr_e3__initial) || (__initial_conditions[e3] = __ovr_e3__initial)
__ovr_e4 = pop!(__overrides, "e4", nothing); isnothing(__ovr_e4) || push!(__eqs, e4 ~ __ovr_e4)
__ovr_e4__initial = pop!(__overrides, "e4__initial", nothing); isnothing(__ovr_e4__initial) || (__initial_conditions[e4] = __ovr_e4__initial)
__ovr_V = pop!(__overrides, "V", nothing); isnothing(__ovr_V) || push!(__eqs, V ~ __ovr_V)
__ovr_V__initial = pop!(__overrides, "V__initial", nothing); isnothing(__ovr_V__initial) || (__initial_conditions[V] = __ovr_V__initial)
__ovr_engaged = pop!(__overrides, "engaged", nothing); isnothing(__ovr_engaged) || push!(__eqs, engaged ~ __ovr_engaged)
__ovr_engaged__initial = pop!(__overrides, "engaged__initial", nothing); isnothing(__ovr_engaged__initial) || (__initial_conditions[engaged] = __ovr_engaged__initial)

### Constants
__constants = Any[]

### Components

### Check there are no unmatched overrides
isempty(__overrides) || throw(ArgumentError("overrides: [$(join(keys(__overrides), ", "))] don't match names found in model. These names may exist in the model but could have been conditionally excluded."))

### Guesses

### Initialization Equations
push!(__initialization_eqs, engaged(ShiftIndex() -1) ~ 0)

### Assertions
__assertions = []

### Equations
push!(__eqs, e1 ~ shoulder_angle)
push!(__eqs, e2 ~ elbow_angle - pi)
push!(__eqs, e3 ~ shoulder_velocity)
push!(__eqs, e4 ~ elbow_velocity)
push!(__eqs, V ~ S11 * e1 ^ 2 + S22 * e2 ^ 2 + S33 * e3 ^ 2 + S44 * e4 ^ 2 + 2 * (S12 * e1 * e2 + S13 * e1 * e3 + S14 * e1 * e4 + S23 * e2 * e3 + S24 * e2 * e4 + S34 * e3 * e4))
push!(__eqs, engaged ~ ifelse(use_ellipsoid == true, ifelse(engaged(ShiftIndex() -1) > 0.5, ifelse(V <= c_release, 1, 0), ifelse(V <= c_engage, 1, 0)), ifelse(abs(elbow_angle - pi) < th, 1, 0)))
push!(__eqs, y ~ engaged)

# Return completely constructed System
return System(__eqs, t, __vars, __params; systems=__systems, initial_conditions=__initial_conditions, guesses=__guesses, name, initialization_eqs=__initialization_eqs, bindings=__bindings, assertions=__assertions)
end
export CatchCondition
0 generated/NearTop_experiment.jl → generated/CatchCondition_experiment.jl
View file
Open in desktop
File renamed without changes.
0 generated/NearTop_test.jl → generated/CatchCondition_test.jl
View file
Open in desktop
File renamed without changes.
Loading