LIBRARY
examples.CartWithSwingup
Cartpole with an energy-based swing-up controller and a stabilizing LQR controller that takes over when the pendulum is close to the upright position.
The swing-up controller pumps energy into the pendulum until its total energy slightly exceeds the energy at the upright equilibrium, at which point the pendulum swings up towards the top and passes it with a small velocity. The stabilizing state-feedback controller is activated when the pendulum angle is close enough to zero.
Usage
MultibodyComponents.examples.CartWithSwingup(L=[-10.0, 82.88765863287402, -15.311327687258002, 15.036803763996625], Er=1.9131576734573559, E_margin=0.4, k_swing=100, k_x=2, k_v=4, phi_switch=0.4)
Parameters:
| Name | Description | Units | Default value |
|---|---|---|---|
L | LQR feedback gain of the stabilizing controller, computed by dyad/examples/cartpole_lqr.jl | – | [-10.0, 82....3763996625] |
Er | Total energy of the pendulum at the upright equilibrium, computed by dyad/examples/cartpole_lqr.jl | – | 1.9131576734573559 |
E_margin | Energy pumped in excess of the upright equilibrium energy, makes the pendulum pass the upright position with a small velocity so that the stabilizing controller can catch it | – | 0.4 |
k_swing | Feedback gain of the energy-based swing-up controller | – | 100 |
k_x | Cart position feedback gain during swing-up, keeps the cart close to the origin | – | 2 |
k_v | Cart velocity feedback gain during swing-up | – | 4 |
phi_switch | Pendulum angle threshold below which the stabilizing controller is active | – | 0.4 |
Variables
| Name | Description | Units |
|---|---|---|
phi | Pendulum angle, normalized to be zero when the pendulum points up | rad |
w | Pendulum angular velocity | rad/s |
E | Total energy of the pendulum, expressed in the reference frame of the cart | – |
u_swing | Swing-up control signal | – |
u_stab | Stabilizing control signal | – |
switching_condition | Indicator that equals one when the stabilizing controller is active | – |
Behavior
Source
dyad
"""
Cartpole with an energy-based swing-up controller and a stabilizing LQR
controller that takes over when the pendulum is close to the upright position.
The swing-up controller pumps energy into the pendulum until its total energy
slightly exceeds the energy at the upright equilibrium, at which point the
pendulum swings up towards the top and passes it with a small velocity. The
stabilizing state-feedback controller is activated when the pendulum angle is
close enough to zero.
"""
example component CartWithSwingup
world = MultibodyComponents.World()
cartpole = Cartpole()
control_saturation = BlockComponents.Nonlinear.Limiter(y_max = 12)
"LQR feedback gain of the stabilizing controller, computed by dyad/examples/cartpole_lqr.jl"
parameter L::Real[4] = [-10.0, 82.88765863287402, -15.311327687258002, 15.036803763996625]
"Total energy of the pendulum at the upright equilibrium, computed by dyad/examples/cartpole_lqr.jl"
parameter Er::Real = 1.9131576734573559
"Energy pumped in excess of the upright equilibrium energy, makes the pendulum pass the upright position with a small velocity so that the stabilizing controller can catch it"
parameter E_margin::Real = 0.4
"Feedback gain of the energy-based swing-up controller"
parameter k_swing::Real = 100
"Cart position feedback gain during swing-up, keeps the cart close to the origin"
parameter k_x::Real = 2
"Cart velocity feedback gain during swing-up"
parameter k_v::Real = 4
"Pendulum angle threshold below which the stabilizing controller is active"
parameter phi_switch::Real = 0.4
"Pendulum angle, normalized to be zero when the pendulum points up"
variable phi::Angle
"Pendulum angular velocity"
variable w::AngularVelocity
"Total energy of the pendulum, expressed in the reference frame of the cart"
variable E::Real
"Swing-up control signal"
variable u_swing::Real
"Stabilizing control signal"
variable u_stab::Real
"Indicator that equals one when the stabilizing controller is active"
variable switching_condition::Real
relations
phi = mod(cartpole.phi + pi, 2 * pi) - pi
w = cartpole.w
# The energy of the pendulum is expressed in the reference frame of the cart by
# subtracting the contribution of the cart velocity (Galilean transformation)
E = cartpole.pendulum.body.KE + cartpole.pendulum.body.PE + cartpole.tip.KE + cartpole.tip.PE - cartpole.v * (cartpole.pendulum.body.m * cartpole.pendulum.body.v_cm_0[1] + cartpole.tip.m * cartpole.tip.v_cm_0[1]) + 0.5 * (cartpole.pendulum.body.m + cartpole.tip.m) * cartpole.v ^ 2
u_swing = k_swing * (E - (Er + E_margin)) * sign(w * cos(phi - pi)) - k_x * cartpole.x - k_v * cartpole.v
u_stab = -(L[1] * cartpole.x + L[2] * phi + L[3] * cartpole.v + L[4] * cartpole.w)
switching_condition = ifelse(abs(phi) < phi_switch, 1, 0)
control_saturation.u = ifelse(switching_condition > 0.5, u_stab, u_swing)
connect(control_saturation.y, cartpole.u)
endFlattened Source
dyad
"""
Cartpole with an energy-based swing-up controller and a stabilizing LQR
controller that takes over when the pendulum is close to the upright position.
The swing-up controller pumps energy into the pendulum until its total energy
slightly exceeds the energy at the upright equilibrium, at which point the
pendulum swings up towards the top and passes it with a small velocity. The
stabilizing state-feedback controller is activated when the pendulum angle is
close enough to zero.
"""
example component CartWithSwingup
world = MultibodyComponents.World()
cartpole = Cartpole()
control_saturation = BlockComponents.Nonlinear.Limiter(y_max = 12)
"LQR feedback gain of the stabilizing controller, computed by dyad/examples/cartpole_lqr.jl"
parameter L::Real[4] = [-10.0, 82.88765863287402, -15.311327687258002, 15.036803763996625]
"Total energy of the pendulum at the upright equilibrium, computed by dyad/examples/cartpole_lqr.jl"
parameter Er::Real = 1.9131576734573559
"Energy pumped in excess of the upright equilibrium energy, makes the pendulum pass the upright position with a small velocity so that the stabilizing controller can catch it"
parameter E_margin::Real = 0.4
"Feedback gain of the energy-based swing-up controller"
parameter k_swing::Real = 100
"Cart position feedback gain during swing-up, keeps the cart close to the origin"
parameter k_x::Real = 2
"Cart velocity feedback gain during swing-up"
parameter k_v::Real = 4
"Pendulum angle threshold below which the stabilizing controller is active"
parameter phi_switch::Real = 0.4
"Pendulum angle, normalized to be zero when the pendulum points up"
variable phi::Angle
"Pendulum angular velocity"
variable w::AngularVelocity
"Total energy of the pendulum, expressed in the reference frame of the cart"
variable E::Real
"Swing-up control signal"
variable u_swing::Real
"Stabilizing control signal"
variable u_stab::Real
"Indicator that equals one when the stabilizing controller is active"
variable switching_condition::Real
relations
phi = mod(cartpole.phi + pi, 2 * pi) - pi
w = cartpole.w
# The energy of the pendulum is expressed in the reference frame of the cart by
# subtracting the contribution of the cart velocity (Galilean transformation)
E = cartpole.pendulum.body.KE + cartpole.pendulum.body.PE + cartpole.tip.KE + cartpole.tip.PE - cartpole.v * (cartpole.pendulum.body.m * cartpole.pendulum.body.v_cm_0[1] + cartpole.tip.m * cartpole.tip.v_cm_0[1]) + 0.5 * (cartpole.pendulum.body.m + cartpole.tip.m) * cartpole.v ^ 2
u_swing = k_swing * (E - (Er + E_margin)) * sign(w * cos(phi - pi)) - k_x * cartpole.x - k_v * cartpole.v
u_stab = -(L[1] * cartpole.x + L[2] * phi + L[3] * cartpole.v + L[4] * cartpole.w)
switching_condition = ifelse(abs(phi) < phi_switch, 1, 0)
control_saturation.u = ifelse(switching_condition > 0.5, u_stab, u_swing)
connect(control_saturation.y, cartpole.u)
metadata {}
endTest Cases
No test cases defined.
Related
Examples
Experiments
Analyses