diff --git a/dyad/furuta_with_swingup.dyad b/dyad/furuta_with_swingup.dyad index 0d0f482..55db3e2 100644 --- a/dyad/furuta_with_swingup.dyad +++ b/dyad/furuta_with_swingup.dyad @@ -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} @@ -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 @@ -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} @@ -869,11 +869,11 @@ 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} @@ -881,7 +881,7 @@ relations 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} @@ -954,7 +954,7 @@ metadata { "tags": [] } }, - "neartop": { + "catchcondition": { "Dyad": { "placement": { "diagram": {"iconName": "default", "x1": 330, "y1": 20, "x2": 430, "y2": 120, "rot": 0} @@ -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 diff --git a/generated/CatchCondition_definition.jl b/generated/CatchCondition_definition.jl new file mode 100644 index 0000000..16beee0 --- /dev/null +++ b/generated/CatchCondition_definition.jl @@ -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 diff --git a/generated/NearTop_experiment.jl b/generated/CatchCondition_experiment.jl similarity index 100% rename from generated/NearTop_experiment.jl rename to generated/CatchCondition_experiment.jl diff --git a/generated/NearTop_test.jl b/generated/CatchCondition_test.jl similarity index 100% rename from generated/NearTop_test.jl rename to generated/CatchCondition_test.jl diff --git a/generated/LQRstabilizer_definition.jl b/generated/LQRstabilizer_definition.jl index 62201d7..f39db91 100644 --- a/generated/LQRstabilizer_definition.jl +++ b/generated/LQRstabilizer_definition.jl @@ -13,10 +13,10 @@ import Moshi as __Ext__Moshi | Name | Description | Units | Default value | | ------------ | ----------------------------------- | ------ | --------------- | -| `L1` | State-feedback gain vector applied to the error vector [shoulder_angle, elbow_angle, shoulder_velocity, elbow_velocity] | -- | -9.625743176817387 | -| `L2` | | -- | 394.43972658274106 | -| `L3` | | -- | -7.461418005226849 | -| `L4` | | -- | 84.42279971138271 | +| `L1` | 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. | -- | -1.7259853130770042 | +| `L2` | | -- | 68.2810119491725 | +| `L3` | | -- | -1.2317839406561077 | +| `L4` | | -- | 7.7052139179986066 | | `umax` | | -- | 10.0 | ## Connectors @@ -37,7 +37,7 @@ import Moshi as __Ext__Moshi | `e4` | | -- | | `uraw` | | -- | """ -@component function LQRstabilizer(; name = nothing, L1=-9.625743176817387, L2=394.43972658274106, L3=-7.461418005226849, L4=84.42279971138271, umax=Float64(10.0), kwargs...) +@component function LQRstabilizer(; name = nothing, L1=-1.7259853130770042, L2=68.2810119491725, L3=-1.2317839406561077, L4=7.7052139179986066, umax=Float64(10.0), kwargs...) isnothing(name) && throw(ArgumentError(""" The `name` keyword must be provided. Please consider using the `@named` macro, like so: @@ -69,7 +69,7 @@ import Moshi as __Ext__Moshi ### Symbolic Parameters __local__L1 = L1 - append!(__params, @parameters (L1::Real), [description = "State-feedback gain vector applied to the error vector [shoulder_angle, elbow_angle, shoulder_velocity, elbow_velocity]"]) + append!(__params, @parameters (L1::Real), [description = "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."]) __initial_conditions[L1] = __local__L1 __local__L2 = L2 append!(__params, @parameters (L2::Real)) diff --git a/generated/NearTop_definition.jl b/generated/NearTop_definition.jl deleted file mode 100644 index c395a05..0000000 --- a/generated/NearTop_definition.jl +++ /dev/null @@ -1,89 +0,0 @@ -### 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""" - NearTop(; name, th) - -## Parameters: - -| Name | Description | Units | Default value | -| ------------ | ----------------------------------- | ------ | --------------- | -| `th` | | -- | 0.4 | - -## Connectors - - * `u` - 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)) -""" -@component function NearTop(; name = nothing, th=0.4, kwargs...) - isnothing(name) && throw(ArgumentError(""" - The `name` keyword must be provided. Please consider using the `@named` macro, - like so: - - @named model = NearTop() - """)) - - __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__th = th - append!(__params, @parameters (th::Real)) - __initial_conditions[th] = __local__th - - ### Final Parameters (assignments) - - ### Final Path Parameters - append!(__vars, @variables (u(t)::Real), [input = true]) - append!(__vars, @variables (y(t)::Real), [output = true]) - - ### Variables (declarations) - - ### Variables (assignments) - - ### 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 - - ### Assertions - __assertions = [] - - ### Equations - push!(__eqs, y ~ abs(u - π) < th) - - # 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 NearTop diff --git a/generated/Swingup_definition.jl b/generated/Swingup_definition.jl index 9011b35..fd99aba 100644 --- a/generated/Swingup_definition.jl +++ b/generated/Swingup_definition.jl @@ -77,9 +77,9 @@ import Moshi as __Ext__Moshi # Subcomponent lqrstabilizer of type QuanserComponents.LQRstabilizer lqrstabilizer_overrides = __pop_subcomponent_overrides!(__overrides, "lqrstabilizer") push!(__systems, @named lqrstabilizer = QuanserComponents.LQRstabilizer(; lqrstabilizer_overrides...)) - # Subcomponent neartop of type QuanserComponents.NearTop - neartop_overrides = __pop_subcomponent_overrides!(__overrides, "neartop") - push!(__systems, @named neartop = QuanserComponents.NearTop(; neartop_overrides...)) + # Subcomponent catchcondition of type QuanserComponents.CatchCondition + catchcondition_overrides = __pop_subcomponent_overrides!(__overrides, "catchcondition") + push!(__systems, @named catchcondition = QuanserComponents.CatchCondition(; catchcondition_overrides...)) # Subcomponent stabilizationswitch of type QuanserComponents.StabilizationSwitch stabilizationswitch_overrides = __pop_subcomponent_overrides!(__overrides, "stabilizationswitch") push!(__systems, @named stabilizationswitch = QuanserComponents.StabilizationSwitch(; stabilizationswitch_overrides...)) @@ -114,11 +114,11 @@ import Moshi as __Ext__Moshi ### Equations push!(__eqs, connect(anglenormalization.y, lqrstabilizer.elbow_angle, energyswingup.elbow_angle)) - push!(__eqs, connect(estimatorswitch_shoulder.y, lqrstabilizer.shoulder_velocity, energyswingup.shoulder_velocity)) - push!(__eqs, connect(anglenormalization.y, neartop.u)) - push!(__eqs, connect(estimatorswitch_elbow.y, lqrstabilizer.elbow_velocity, energyswingup.elbow_velocity)) - push!(__eqs, connect(shoulder_angle, energyswingup.shoulder_angle, velocityestimator_shoulder.pos, alphabeta_shoulder.pos, lqrstabilizer.shoulder_angle)) - push!(__eqs, connect(neartop.y, stabilizationswitch.neartop)) + push!(__eqs, connect(estimatorswitch_shoulder.y, lqrstabilizer.shoulder_velocity, energyswingup.shoulder_velocity, catchcondition.shoulder_velocity)) + push!(__eqs, connect(anglenormalization.y, catchcondition.elbow_angle)) + push!(__eqs, connect(estimatorswitch_elbow.y, lqrstabilizer.elbow_velocity, energyswingup.elbow_velocity, catchcondition.elbow_velocity)) + push!(__eqs, connect(shoulder_angle, energyswingup.shoulder_angle, velocityestimator_shoulder.pos, alphabeta_shoulder.pos, lqrstabilizer.shoulder_angle, catchcondition.shoulder_angle)) + push!(__eqs, connect(catchcondition.y, stabilizationswitch.neartop)) push!(__eqs, connect(energyswingup.realoutput, stabilizationswitch.u_swingup)) push!(__eqs, connect(lqrstabilizer.u, stabilizationswitch.u_lqr)) push!(__eqs, connect(velocityestimator_elbow.pos, elbow_angle, alphabeta_elbow.pos, anglenormalization.u)) @@ -126,7 +126,7 @@ import Moshi as __Ext__Moshi push!(__eqs, connect(gain.y, u)) push!(__eqs, connect(anglenormalization.y, erefadaptation.alpha)) push!(__eqs, connect(estimatorswitch_elbow.y, erefadaptation.dalpha)) - push!(__eqs, connect(neartop.y, erefadaptation.neartop)) + push!(__eqs, connect(catchcondition.y, erefadaptation.neartop)) push!(__eqs, connect(erefadaptation.eta, energyswingup.eta_adapt)) push!(__eqs, connect(velocityestimator_shoulder.vel, estimatorswitch_shoulder.v_old)) push!(__eqs, connect(alphabeta_shoulder.vel, estimatorswitch_shoulder.v_new)) diff --git a/generated/definitions.jl b/generated/definitions.jl index 8ea1e32..f4f0fbf 100644 --- a/generated/definitions.jl +++ b/generated/definitions.jl @@ -206,6 +206,7 @@ end include("AlphaBetaEstimator_definition.jl") include("AngleNormalization_definition.jl") +include("CatchCondition_definition.jl") include("Cos_definition.jl") include("EnergySwingup_definition.jl") include("Energy_definition.jl") @@ -215,7 +216,6 @@ include("EstimatorSwitch_definition.jl") include("FurutaSwingupDelayed_definition.jl") include("FurutaSwingup_definition.jl") include("LQRstabilizer_definition.jl") -include("NearTop_definition.jl") include("QubePendulum_definition.jl") include("Sign_definition.jl") include("SmoothCoulombFriction_definition.jl") diff --git a/generated/experiments.jl b/generated/experiments.jl index b4e274e..668e993 100644 --- a/generated/experiments.jl +++ b/generated/experiments.jl @@ -11,6 +11,7 @@ end include("AlphaBetaEstimator_experiment.jl") include("AngleNormalization_experiment.jl") +include("CatchCondition_experiment.jl") include("Cos_experiment.jl") include("EnergySwingup_experiment.jl") include("Energy_experiment.jl") @@ -20,7 +21,6 @@ include("EstimatorSwitch_experiment.jl") include("FurutaSwingupDelayed_experiment.jl") include("FurutaSwingup_experiment.jl") include("LQRstabilizer_experiment.jl") -include("NearTop_experiment.jl") include("QubePendulum_experiment.jl") include("Sign_experiment.jl") include("SmoothCoulombFriction_experiment.jl") diff --git a/generated/tests.jl b/generated/tests.jl index 87d3810..c38bbcb 100644 --- a/generated/tests.jl +++ b/generated/tests.jl @@ -20,6 +20,7 @@ include("test_internals.jl") @testset "`QuanserComponents`" begin include("AlphaBetaEstimator_test.jl") include("AngleNormalization_test.jl") +include("CatchCondition_test.jl") include("Cos_test.jl") include("EnergySwingup_test.jl") include("Energy_test.jl") @@ -29,7 +30,6 @@ include("EstimatorSwitch_test.jl") include("FurutaSwingupDelayed_test.jl") include("FurutaSwingup_test.jl") include("LQRstabilizer_test.jl") -include("NearTop_test.jl") include("QubePendulum_test.jl") include("Sign_test.jl") include("SmoothCoulombFriction_test.jl") diff --git a/robustness/02_montecarlo.jl b/robustness/02_montecarlo.jl index 9ae424d..56cba00 100644 --- a/robustness/02_montecarlo.jl +++ b/robustness/02_montecarlo.jl @@ -8,8 +8,10 @@ using Random, Statistics @isdefined(controller_config) || (controller_config = "baseline") @isdefined(N_mc) || (N_mc = 300) @isdefined(tf_mc) || (tf_mc = 15.0) +@isdefined(use_delayed) || (use_delayed = false) -model, ssys = build_system() +model, ssys = build_system(delayed = use_delayed) +mc_label = string(controller_config, use_delayed ? "_delay1" : "") ctrl = controller_overrides(ssys, controller_config) has_friction = supports_friction(ssys) @@ -46,13 +48,13 @@ for i in 1:N_mc end df = DataFrame(rows) -CSV.write(resultpath("mc_$(controller_config).csv"), df) +CSV.write(resultpath("mc_$(mc_label).csv"), df) rate = mean(df.success) n = nrow(df) ci = 1.96 * sqrt(rate * (1 - rate) / n) @printf("\nMC success rate (%s): %.1f%% ± %.1f%% (N = %d)\n", - controller_config, 100rate, 100ci, n) + mc_label, 100rate, 100ci, n) # Conditional failure rates: split each parameter at its median, compare # failure rate in the upper vs lower half to rank influence. diff --git a/robustness/04_catch_region.jl b/robustness/04_catch_region.jl index 0820d00..46bd8fd 100644 --- a/robustness/04_catch_region.jl +++ b/robustness/04_catch_region.jl @@ -6,6 +6,11 @@ include("common.jl") +# lqr_config = "old" pins the original gains; "new" uses the redesigned +# defaults and suffixes the output files with _newlqr. +@isdefined(lqr_config) || (lqr_config = "old") +suffix = lqr_config == "new" ? "_newlqr" : "" + model, ssys = build_system() alpha_grid = range(-1.2, 1.2, length = 25) # elbow angle offset from upright @@ -14,10 +19,14 @@ omega_grid = range(-8.0, 8.0, length = 25) # elbow angular velocity "Success = settled upright at tf, starting near the top with LQR always active." function catch_success(ssys, plant_kwargs, dphi, w; tf = 3.0) ov = vcat( - controller_settings(ssys), + with_overrides(controller_settings(ssys; pin_lqr = lqr_config == "old"), + Pair[ + # force the switch to always select the LQR + ssys.swingup.catchcondition.use_ellipsoid => false + ssys.swingup.catchcondition.th => 1e6 + ]), supports_friction(ssys) ? perturbation_overrides(ssys; plant_kwargs...) : Pair[], Pair[ - ssys.swingup.neartop.th => 1e6 ssys.qubependulum.elbow_joint.phi => pi + dphi ssys.qubependulum.elbow_joint.w => w ssys.qubependulum.shoulder_joint.phi => 0.0 @@ -49,13 +58,13 @@ for (name, kwargs) in plants S[i, j] = catch_success(ssys, kwargs, a, w) @printf("%s: α−π=%6.3f ω=%6.2f -> %s\n", name, a, w, S[i, j]) end - CSV.write(resultpath("catch_region_$(name).csv"), + CSV.write(resultpath("catch_region_$(name)$(suffix).csv"), DataFrame(S, string.(round.(omega_grid, sigdigits = 3)))) hm = heatmap(omega_grid, alpha_grid, S, xlabel = "elbow velocity [rad/s]", ylabel = "α − π [rad]", color = [:red, :green], clims = (0, 1), colorbar = false, title = "LQR catch region: $name") hline!(hm, [-0.4, 0.4], l = (:dash, :black), label = "NearTop.th") - savefig(hm, figurepath("catch_region_$(name).png")) + savefig(hm, figurepath("catch_region_$(name)$(suffix).png")) end # Delay variant @@ -64,10 +73,10 @@ S = falses(length(alpha_grid), length(omega_grid)) for (i, a) in enumerate(alpha_grid), (j, w) in enumerate(omega_grid) S[i, j] = catch_success(ssys_d, (;), a, w) end -CSV.write(resultpath("catch_region_delay1.csv"), +CSV.write(resultpath("catch_region_delay1$(suffix).csv"), DataFrame(S, string.(round.(omega_grid, sigdigits = 3)))) hm = heatmap(omega_grid, alpha_grid, S, xlabel = "elbow velocity [rad/s]", ylabel = "α − π [rad]", color = [:red, :green], clims = (0, 1), colorbar = false, title = "LQR catch region: delay 1 sample") hline!(hm, [-0.4, 0.4], l = (:dash, :black), label = "NearTop.th") -savefig(hm, figurepath("catch_region_delay1.png")) +savefig(hm, figurepath("catch_region_delay1$(suffix).png")) diff --git a/robustness/07_design_catch_condition.jl b/robustness/07_design_catch_condition.jl index 91b16ec..22b1dba 100644 --- a/robustness/07_design_catch_condition.jl +++ b/robustness/07_design_catch_condition.jl @@ -14,6 +14,12 @@ include("common.jl") using ControlSystemsMTK, ControlSystemsBase using LinearAlgebra +# lqr_config = "old" calibrates for the original gains against the original +# catch grids; "new" uses the redesigned gains (08_lqr_redesign.jl) and the +# _newlqr grids from 04, suffixing the outputs accordingly. +@isdefined(lqr_config) || (lqr_config = "old") +suffix = lqr_config == "new" ? "_newlqr" : "" + model, ssys = build_system() op = Dict( @@ -40,13 +46,19 @@ P = named_ss(model, [ssys.u_plant], outputs; ) Pd = c2d(ss(P), Ts) -# Same weights as the LQR design in test/runtests.jl +# Same state weights as the LQR design in test/runtests.jl; the control +# weight matches the respective design (the redesign uses 300, see +# 08_lqr_redesign.jl) Q1 = P.C' * Diagonal([1000.0, 10.0, 1.0, 1.0]) * P.C -Q2 = 10.0 * I(1) +Q2 = (lqr_config == "new" ? 300.0 : 10.0) * I(1) S, _, L_dare = ControlSystemsBase.MatrixEquations.ared(Pd.A, Pd.B, Q2, Q1) L_fresh = vec(L_dare * pinv(P.C)) -L_baked = [-9.625743176817387, 394.43972658274106, -7.461418005226849, 84.42279971138271] +L_baked = if lqr_config == "new" + CSV.read(resultpath("lqr_redesign_q2_300.csv"), DataFrame).value +else + L_ORIGINAL +end println("L fresh LQR design: ", round.(L_fresh, digits = 4)) println("L in component: ", round.(L_baked, digits = 4)) if !isapprox(L_fresh, L_baked, rtol = 1e-2) @@ -78,7 +90,7 @@ V(a, w) = [0.0, a, 0.0, w]' * Sy * [0.0, a, 0.0, w] plants = ["nominal", "mp_plus20", "coulomb", "quantized"] Vfail = Inf # smallest V among failed grid points, over all plants for name in plants - f = resultpath("catch_region_$name.csv") + f = resultpath("catch_region_$(name)$(suffix).csv") isfile(f) || (println("missing $f – run 04_catch_region.jl first"); continue) S_grid = Matrix(CSV.read(f, DataFrame)) for (i, a) in enumerate(alpha_grid), (j, w) in enumerate(omega_grid) @@ -100,7 +112,7 @@ for i in 1:4, j in i:4 push!(rows, (; entry = "S$(i)$(j)", value = Sn[i, j])) end push!(rows, (; entry = "c_release", value = c_out / c_in)) -CSV.write(resultpath("catch_condition_design.csv"), DataFrame(rows)) +CSV.write(resultpath("catch_condition_design$(suffix).csv"), DataFrame(rows)) println("\nnormalized ellipsoid entries (engage at V = 1):") for r in rows @printf(" parameter %s::Real = %.6g\n", r.entry, r.value) @@ -108,7 +120,7 @@ end # Coverage: how much of the empirically catchable set the ellipsoid retains for name in plants - f = resultpath("catch_region_$name.csv") + f = resultpath("catch_region_$(name)$(suffix).csv") isfile(f) || continue S_grid = Matrix(CSV.read(f, DataFrame)) caught = [S_grid[i, j] for (i, a) in enumerate(alpha_grid), (j, w) in enumerate(omega_grid)] diff --git a/robustness/08_lqr_redesign.jl b/robustness/08_lqr_redesign.jl new file mode 100644 index 0000000..cde0df3 --- /dev/null +++ b/robustness/08_lqr_redesign.jl @@ -0,0 +1,104 @@ +# LQR redesign from the actual model linearization. +# +# The gains baked into LQRstabilizer do not correspond to an LQR design from +# this model's linearization with the weights documented in test/runtests.jl +# (they are far more aggressive), and the campaign showed the resulting loop +# has essentially no delay margin. This script designs fresh gains, sweeping +# the control weight to buy input margins, and validates each candidate in +# closed-loop simulation before selecting. +# +# Selection criteria: +# - delay margin of the state-feedback loop at least two samples +# - nominal swingup (robust defaults) still catches promptly +# - catches with one sample of actuation delay +# - catches a hard perturbed plant from the MC campaign + +include("common.jl") +using ControlSystemsMTK, ControlSystemsBase +using LinearAlgebra +using Printf + +model, ssys = build_system() +model_d, ssys_d = build_system(delayed = true, delay_n = 1) + +op = Dict( + ssys.qubependulum.elbow_joint.phi => π, + ssys.qubependulum.shoulder_joint.phi => 0.0, + ssys.qubependulum.elbow_joint.w => 0.0, + ssys.qubependulum.shoulder_joint.w => 0.0, + ssys.qubependulum.voltage => 0.0, + ssys.elbow_sampler.u => 0.0, + ssys.shoulder_sampler.u => 0.0, +) +outputs = [ + ssys.qubependulum.shoulder_angle + ssys.qubependulum.elbow_angle + ssys.qubependulum.shoulder_joint.w + ssys.qubependulum.elbow_joint.w +] +P = named_ss(model, [ssys.u_plant], outputs; + op, + loop_openings = [ssys.u_plant, ssys.shoulder_y, ssys.elbow_y], + warn_empty_op = true, + additional_passes = [SynchToolkit.compile_lustre], + MultibodyComponents.linsys..., +) +Pd = c2d(ss(P), Ts) +Ci = pinv(P.C) + +"Loop-transfer margins of discrete state feedback u = -L*x at the plant input." +function loop_margins(L_y) + Lx = reshape(L_y, 1, 4) * P.C + Lt = ss(Pd.A, Pd.B, Lx, 0, Ts) # loop transfer L(z) + wgm, gm, wpm, pm = margin(Lt) + delay_margin_s = deg2rad(only(pm)) / only(wpm) + (; gm = only(gm), pm = only(pm), delay_samples = delay_margin_s / Ts) +end + +L_baked = [-9.625743176817387, 394.43972658274106, -7.461418005226849, 84.42279971138271] +m0 = loop_margins(L_baked) +@printf("implemented gains: gm %.2f, pm %.1f deg, delay margin %.2f samples\n", + m0.gm, m0.pm, m0.delay_samples) + +designs = Dict{Float64, Vector{Float64}}() +for q2 in (10.0, 30.0, 100.0, 300.0) + Q1 = P.C' * Diagonal([1000.0, 10.0, 1.0, 1.0]) * P.C + Q1 = Matrix(Symmetric(Q1)) + _, _, F = ControlSystemsBase.MatrixEquations.ared(Pd.A, Pd.B, q2 * I(1), Q1) + L = vec(F * Ci) + designs[q2] = L + m = loop_margins(L) + @printf("Q2 = %6.1f: L = [%8.3f %8.3f %8.3f %8.3f] gm %.2f pm %.1f deg delay %.2f samples\n", + q2, L..., m.gm, m.pm, m.delay_samples) +end + +# Simulation validation of each candidate (robust swingup defaults). +# Hard plant: the mildest failing MC draw of the original campaign. +hard = (; mp = 0.0244341173211006, Lp = 0.12963014151022853, + mr = 0.08900340167288685, kt = 0.044360644011252365, + km = 0.03913861399268383, Rm = 7.97187181423963, + br = 0.0005855064113230941, bp = 4.786994165706217e-5, + tau_c_sh = 0.0040382876985633346, tau_c_el = 0.0003770799004443717, + quantized = true) + +function lqr_overrides(sys, L) + Pair[ + sys.swingup.lqrstabilizer.L1 => L[1], + sys.swingup.lqrstabilizer.L2 => L[2], + sys.swingup.lqrstabilizer.L3 => L[3], + sys.swingup.lqrstabilizer.L4 => L[4], + sys.qubependulum.shoulder_joint.render => false, + ] +end + +println() +for q2 in sort(collect(keys(designs))) + L = designs[q2] + m_nom = metrics(simulate(ssys, vcat(lqr_overrides(ssys, L), swingup_ics(ssys)); tf = 10.0), ssys) + m_del = metrics(simulate(ssys_d, vcat(lqr_overrides(ssys_d, L), swingup_ics(ssys_d)); tf = 15.0), ssys_d) + m_hard = metrics(simulate(ssys, + vcat(lqr_overrides(ssys, L), perturbation_overrides(ssys; hard...), swingup_ics(ssys)); tf = 20.0), ssys) + @printf("Q2 = %6.1f: nominal %s (catch %.2f) delay1 %s (catch %.2f) hard %s (catch %.2f)\n", + q2, m_nom.success, m_nom.catch_time, m_del.success, m_del.catch_time, + m_hard.success, m_hard.catch_time) +end diff --git a/robustness/08b_lqr_candidates.jl b/robustness/08b_lqr_candidates.jl new file mode 100644 index 0000000..4c5083b --- /dev/null +++ b/robustness/08b_lqr_candidates.jl @@ -0,0 +1,64 @@ +# Discriminate the LQR redesign candidates empirically: +# - swingup success with 2 and 3 samples of actuation delay +# - LQR-only catch capability at the top: largest |ω| caught at α = π, +# nominal and with +20% pendulum mass +# The implemented (old) gains are included for reference. + +include("common.jl") +using Printf + +candidates = [ + ("old", [-9.625743176817387, 394.43972658274106, -7.461418005226849, 84.42279971138271]), + ("Q2=10", [-9.307, 119.591, -4.032, 13.836]), + ("Q2=30", [-5.409, 95.615, -2.686, 10.971]), + ("Q2=100", [-2.979, 78.561, -1.765, 8.934]), + ("Q2=300", [-1.726, 68.281, -1.232, 7.705]), +] + +function lqr_overrides(sys, L) + Pair[ + sys.swingup.lqrstabilizer.L1 => L[1], + sys.swingup.lqrstabilizer.L2 => L[2], + sys.swingup.lqrstabilizer.L3 => L[3], + sys.swingup.lqrstabilizer.L4 => L[4], + sys.qubependulum.shoulder_joint.render => false, + ] +end + +model, ssys = build_system() + +"Largest |elbow velocity| caught at alpha = pi with the LQR always engaged." +function catch_capability(L; mp_scale = 1.0) + mp = 0.024 * mp_scale + wmax = 0.0 + for w in 0.5:0.5:8.0 + ov = vcat(lqr_overrides(ssys, L), + perturbation_overrides(ssys; mp), + Pair[ + ssys.swingup.catchcondition.use_ellipsoid => false + ssys.swingup.catchcondition.th => 1e6 + ssys.qubependulum.elbow_joint.phi => pi + ssys.qubependulum.elbow_joint.w => w + ssys.qubependulum.shoulder_joint.phi => 0.0 + ]) + m = metrics(simulate(ssys, ov; tf = 3.0), ssys; window = 0.5, th = 1e6) + m.success ? (wmax = w) : break + end + wmax +end + +results = Dict() +for (name, L) in candidates + c1 = catch_capability(L) + c2 = catch_capability(L; mp_scale = 1.2) + results[name] = (; c1, c2) + @printf("%-7s catch capability at top: nominal %.1f rad/s, mp+20%% %.1f rad/s\n", name, c1, c2) +end + +for n in (2, 3) + model_d, ssys_d = build_system(delayed = true, delay_n = n) + for (name, L) in candidates + m = metrics(simulate(ssys_d, vcat(lqr_overrides(ssys_d, L), swingup_ics(ssys_d)); tf = 15.0), ssys_d) + @printf("%-7s delay %d: success %s (catch %.2f)\n", name, n, m.success, m.catch_time) + end +end diff --git a/robustness/README.md b/robustness/README.md index d9a73bc..f58bf56 100644 --- a/robustness/README.md +++ b/robustness/README.md @@ -115,11 +115,53 @@ Interpretation: `LQRstabilizer.umax` defaults to the value every actual use overrode it to; with its old default the out-of-the-box model could not hold the catch at all. -2. Keep `NearTop.th = 0.4` only together with a velocity condition: the - catch-region slices show the band is far from invariant. An ellipsoidal - condition from the LQR Riccati solution is the natural refinement. -3. If the real rig can exhibit even one sample of IO latency, redesign the - LQR with a delay state; the current design has no delay margin. +2. **(Implemented, opt-in, negative result in simulation)** `CatchCondition` + replaces the angle-only `NearTop`: it can gate the handover on an + ellipsoidal sublevel set V = e'Se ≤ c_engage of a Lyapunov function of + the implemented LQR loop (S from `07_design_catch_condition.jl`, + calibrated for zero false positives on the empirical catch grids of four + perturbed plants) with hysteresis release at `c_release`. Findings from + the paired MC: + - angle-only switch (default): 93.0% (original LQR) / 93.7% (redesigned) + - ellipsoid, zero-false-positive calibration (`c_engage = 1`): 79.7% + with the original LQR, 85.7% recalibrated for the redesigned LQR + (whose larger catch region makes the gate less restrictive) + - ellipsoid, loosened (`c_engage = 2, c_release = 8`): 76.3% (threshold + tuning on the broken/rescued subset did not generalize — selection + bias; draws that were fine under both other configs broke) + + Interpretation: the gate is computed from the nominal model, which is + exactly what mismatch invalidates, and in simulation failed catch + attempts are cheap, so refusing marginal attempts costs success. The + gate's real benefits are qualitative: single chatter-free engagement and + mean arrival speed under half of the angle-only switch (1.5 vs 3.2 rad/s + among successes) — relevant on hardware where failed high-speed catch + attempts cause voltage spikes and mechanical stress. Hence opt-in via + `catchcondition.use_ellipsoid`, default off. A handover that is both + gentle and mismatch-robust likely requires redesigning the LQR itself + (see item 3 and the stale-gain observation in + `07_design_catch_condition.jl`). +3. **(Implemented)** LQR redesigned from the actual model linearization + (`08_lqr_redesign.jl`). The original gains did not correspond to any LQR + design from this model with the documented weights and had no delay + margin. The redesign keeps the documented output weights and raises the + control weight until the loop tolerates delay (selected: control weight + 300), validated in simulation rather than by classical margins (the + plant is unstable, making single-crossover margin numbers misleading): + + | metric | original gains | redesigned | + |---|---|---| + | swingup MC (robust config, tf = 20 s) | 93.0% | **93.7%** | + | swingup MC with 1-sample actuation delay | 57.0% | **92.0%** | + | swingup with 3-sample delay (nominal) | fails at 1 | catches at 5.8 s | + | LQR catch region, nominal | 142/625 | 179/625 | + | LQR catch region, 1-sample delay | 8/625 | 191/625 | + | catch capability at top (nominal / mp+20%) | 2.5 / 2.5 rad/s | 2.5 / 2.5 rad/s | + + The ellipsoidal catch condition was recalibrated for the new loop + (`07_design_catch_condition.jl` with `lqr_config = "new"`): the safe + ellipsoid now retains 63-68% of the catchable set (was 40-44%), still + with zero false positives across the perturbed plants. ## Reproducing diff --git a/robustness/common.jl b/robustness/common.jl index a27bb10..570fdfa 100644 --- a/robustness/common.jl +++ b/robustness/common.jl @@ -31,14 +31,19 @@ function build_system(; delayed = false, delay_n = 1) model, ssys end +"LQR gains of the original controller (before the redesign in 08_lqr_redesign.jl)." +const L_ORIGINAL = [-9.625743176817387, 394.43972658274106, -7.461418005226849, 84.42279971138271] + """ Controller settings pinning the ORIGINAL (pre-robustification) controller, regardless of the current component defaults: unnormalized energy error, -no adaptation, original velocity estimator. This is the campaign "baseline" -and the reference for the behavior-neutrality regression. +no adaptation, original velocity estimator, and (with `pin_lqr = true`) the +original LQR gains. This is the campaign "baseline" and the reference for +the behavior-neutrality regression. Pass `pin_lqr = false` to use the +current default (redesigned) LQR gains instead. """ -function controller_settings(ssys) - Pair[ +function controller_settings(ssys; pin_lqr = true) + ov = Pair[ ssys.qubependulum.shoulder_joint.render => false ssys.gain.k => 1.0 ssys.swingup.lqrstabilizer.umax => 10 @@ -50,7 +55,17 @@ function controller_settings(ssys) ssys.swingup.erefadaptation.gamma => 0.0 ssys.swingup.estimatorswitch_shoulder.use_new => false ssys.swingup.estimatorswitch_elbow.use_new => false + ssys.swingup.catchcondition.use_ellipsoid => false ] + if pin_lqr + append!(ov, Pair[ + ssys.swingup.lqrstabilizer.L1 => L_ORIGINAL[1], + ssys.swingup.lqrstabilizer.L2 => L_ORIGINAL[2], + ssys.swingup.lqrstabilizer.L3 => L_ORIGINAL[3], + ssys.swingup.lqrstabilizer.L4 => L_ORIGINAL[4], + ]) + end + ov end "Initial conditions for a swingup experiment: pendulum hanging near down." @@ -240,6 +255,34 @@ function controller_overrides(ssys, config::AbstractString) ssys.swingup.alphabeta_elbow.alpha => ab_alpha, ssys.swingup.alphabeta_elbow.beta => ab_beta, ]) + elseif config == "adaptive_ab_newlqr" + # adaptive_ab with the redesigned LQR gains (component defaults) + drop = Set(Any[ssys.swingup.lqrstabilizer.L1, ssys.swingup.lqrstabilizer.L2, + ssys.swingup.lqrstabilizer.L3, ssys.swingup.lqrstabilizer.L4]) + return filter(p -> !any(k -> isequal(p.first, k), drop), + controller_overrides(ssys, "adaptive_ab")) + elseif config == "adaptive_ab_vswitch_newlqr" + # redesigned LQR + ellipsoidal catch condition recalibrated for it + return with_overrides(controller_overrides(ssys, "adaptive_ab_newlqr"), Pair[ + ssys.swingup.catchcondition.use_ellipsoid => true, + ]) + elseif config == "adaptive_ab_vswitch" + # adaptive_ab + the velocity-aware ellipsoidal catch condition at the + # zero-false-positive calibration, which performed best among the + # ellipsoid tunings on the full ensemble (rec2_tune_engage.jl showed + # that subset-based tuning of the thresholds does not generalize). + # The S entries are pinned to the calibration for the ORIGINAL LQR + # gains; the component defaults are recalibrated for the redesign. + cc = ssys.swingup.catchcondition + return with_overrides(controller_overrides(ssys, "adaptive_ab"), Pair[ + cc.use_ellipsoid => true, + cc.c_engage => 1.0, + cc.c_release => 4.0, + cc.S11 => 0.144396, cc.S12 => -1.04444, cc.S13 => 0.0497274, + cc.S14 => -0.0910319, cc.S22 => 15.837, cc.S23 => -0.588979, + cc.S24 => 1.3186, cc.S33 => 0.0244188, cc.S34 => -0.0471712, + cc.S44 => 0.151749, + ]) end error("unknown controller config: $config") end diff --git a/robustness/rec1_defaults_smoke.jl b/robustness/rec1_defaults_smoke.jl index e8714ba..8651d46 100644 --- a/robustness/rec1_defaults_smoke.jl +++ b/robustness/rec1_defaults_smoke.jl @@ -15,8 +15,8 @@ m_default = metrics(simulate(ssys, ov; tf = 10.0), ssys) println("pure defaults: success=$(m_default.success) catch=$(m_default.catch_time)") @assert m_default.success "out-of-the-box model failed to swing up" -m2 = metrics(simulate(ssys, controller_overrides(ssys, "adaptive_ab"); tf = 10.0), ssys) -println("defaults: catch=$(m_default.catch_time) adaptive_ab config: catch=$(m2.catch_time)") -@assert m_default.catch_time == m2.catch_time "defaults do not reproduce the adaptive_ab campaign config" +m2 = metrics(simulate(ssys, controller_overrides(ssys, "adaptive_ab_newlqr"); tf = 10.0), ssys) +println("defaults: catch=$(m_default.catch_time) adaptive_ab_newlqr config: catch=$(m2.catch_time)") +@assert m_default.catch_time == m2.catch_time "defaults do not reproduce the adaptive_ab_newlqr campaign config" println("rec 1 defaults smoke passed") diff --git a/robustness/rec2_tune_engage.jl b/robustness/rec2_tune_engage.jl new file mode 100644 index 0000000..a8b3d61 --- /dev/null +++ b/robustness/rec2_tune_engage.jl @@ -0,0 +1,33 @@ +# Calibrate the CatchCondition engage threshold on the MC draws that the +# strict (c_engage = 1) ellipsoid broke relative to the angle-only switch, +# plus the draws it rescued (to check they are not lost again). + +include("common.jl") +using Random, Statistics + +model, ssys = build_system() + +ab = sort(CSV.read(resultpath("mc_adaptive_ab.csv"), DataFrame), :sample) +v1 = sort(CSV.read(resultpath("mc_adaptive_ab_vswitch.csv"), DataFrame), :sample) +broken = ab.sample[ab.success .& .!v1.success] +rescued = ab.sample[.!ab.success .& v1.success] +samples = vcat(broken, rescued) +println("tuning on $(length(broken)) broken + $(length(rescued)) rescued draws") + +params = [:mp, :Lp, :mr, :kt, :km, :Rm, :br, :bp, :tau_c_sh, :tau_c_el, :quantized] + +function run_sample(row, c_engage, c_release) + kwargs = NamedTuple(p => row[p] for p in params) + ov = vcat( + with_overrides(controller_overrides(ssys, "adaptive_ab_vswitch"), + Pair[ssys.swingup.catchcondition.c_engage => c_engage, + ssys.swingup.catchcondition.c_release => c_release]), + perturbation_overrides(ssys; kwargs...)) + metrics(simulate(ssys, ov; tf = 20.0), ssys).success +end + +for (c_engage, c_release) in ((2.0, 4.0), (2.0, 8.0), (3.0, 6.0), (1.5, 6.0)) + nb = count(run_sample(ab[ab.sample .== s, :][1, :], c_engage, c_release) for s in broken) + nr = count(run_sample(ab[ab.sample .== s, :][1, :], c_engage, c_release) for s in rescued) + println("c_engage=$c_engage c_release=$c_release: recovers $nb/$(length(broken)) broken, keeps $nr/$(length(rescued)) rescued") +end diff --git a/robustness/rec2_vswitch_smoke.jl b/robustness/rec2_vswitch_smoke.jl new file mode 100644 index 0000000..e723d6e --- /dev/null +++ b/robustness/rec2_vswitch_smoke.jl @@ -0,0 +1,28 @@ +# Recommendation 2 verification (velocity-aware catch condition): +# 1. Nominal swingup with the ellipsoidal switch succeeds and engages exactly +# once (no chattering). +# 2. Angle-only fallback (use_ellipsoid = false) reproduces the previous +# behavior (checked via 00_baseline.jl separately; here we check the +# adaptive_ab config, which pins the angle-only switch, still matches its +# saved catch time). + +include("common.jl") + +model, ssys = build_system() + +# robust swingup + ellipsoidal switch (opt-in config) +ov = controller_overrides(ssys, "adaptive_ab_vswitch") +sol = simulate(ssys, ov; tf = 10.0) +m = metrics(sol, ssys) +engaged = sol[ssys.swingup.catchcondition.y] +n_engage = count(i -> engaged[i] > 0.5 && engaged[i-1] <= 0.5, 2:length(engaged)) +println("defaults+vswitch: success=$(m.success) catch=$(m.catch_time) engagements=$n_engage arrival=$(round(m.arrival_speed, digits=2))") +@assert m.success "swingup with ellipsoidal switch failed nominally" +@assert n_engage == 1 "switch chattered: $n_engage engagements" + +# angle-only configs must be unaffected by the new component +m_ab = metrics(simulate(ssys, controller_overrides(ssys, "adaptive_ab"); tf = 10.0), ssys) +println("adaptive_ab (angle-only switch): success=$(m_ab.success) catch=$(m_ab.catch_time)") +@assert m_ab.success + +println("rec 2 smoke tests passed") diff --git a/test/runtests.jl b/test/runtests.jl index 1b57b17..80a0d13 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -69,7 +69,7 @@ render(model, sol, 0.0; lights=qube_lights)[1] ## using Plots -f1 = plot(sol, idxs=[ssys.qubependulum.elbow_joint.phi, 0.1*ssys.swingup.u, ssys.swingup.neartop.y]) +f1 = plot(sol, idxs=[ssys.qubependulum.elbow_joint.phi, 0.1*ssys.swingup.u, ssys.swingup.catchcondition.y]) hline!([pi], l=(:dash, :black), primary=false) f2 = plot(sol, idxs=ssys.qubependulum.shoulder_joint.phi) plot(f1, f2) |> display