02 lqg
In [ ]:
Copied!
import os
import sys
from time import sleep
import os
import sys
from time import sleep
In [ ]:
Copied!
import numpy as np
import numpy as np
In [ ]:
Copied!
import collimator
from collimator import library
import collimator
from collimator import library
In [ ]:
Copied!
from controllers import * # noqa: F403
from controllers import * # noqa: F403
In [ ]:
Copied!
HOME = os.path.expanduser("~")
sys.path.append(f"{HOME}/Quanser/libraries/python")
HOME = os.path.expanduser("~")
sys.path.append(f"{HOME}/Quanser/libraries/python")
In [ ]:
Copied!
# Linearize the system and discretize in time
x_eq = np.array([0.0, 0.0, 0.0, 0.0])
y_eq = x_eq[:2]
u_eq = np.array([0.0])
# Linearize the system and discretize in time
x_eq = np.array([0.0, 0.0, 0.0, 0.0])
y_eq = x_eq[:2]
u_eq = np.array([0.0])
In [ ]:
Copied!
plant = library.QubeServoModel(full_state_output=False, name="qube")
u0 = np.array([0.0])
plant.input_ports[0].fix_value(u0)
context = plant.create_context()
plant = library.QubeServoModel(full_state_output=False, name="qube")
u0 = np.array([0.0])
plant.input_ports[0].fix_value(u0)
context = plant.create_context()
In [ ]:
Copied!
base_context = context.with_continuous_state(x_eq)
base_context = context.with_continuous_state(x_eq)
In [ ]:
Copied!
# Linearize and discretize in time
lin_sys = library.linearize(plant, base_context)
# Linearize and discretize in time
lin_sys = library.linearize(plant, base_context)
In [ ]:
Copied!
dt = 0.01 # 100 Hz sampling rate
dt_sys = lin_sys.sample(dt)
dt = 0.01 # 100 Hz sampling rate
dt_sys = lin_sys.sample(dt)
In [ ]:
Copied!
n = 4 # State dimension
m = 1 # Input dimension
p = 2 # Output dimension
n = 4 # State dimension
m = 1 # Input dimension
p = 2 # Output dimension
In [ ]:
Copied!
# Covariance matrices for LQR
Q = np.eye(n)
R = 1e-2 * np.eye(m)
# Covariance matrices for LQR
Q = np.eye(n)
R = 1e-2 * np.eye(m)
In [ ]:
Copied!
# Covariance matrices for Kalman filter
noise_scale = 1e2
QN = noise_scale * np.eye(m)
RN = np.eye(p)
# Covariance matrices for Kalman filter
noise_scale = 1e2
QN = noise_scale * np.eye(m)
RN = np.eye(p)
In [ ]:
Copied!
def make_lqg_diagram(plant, controller, pulse_amplitude=1.0, name="root"):
# Add initial impulse
pulse = make_pulse(amplitude=pulse_amplitude, start_time=2.0, width=0.1) # noqa: F405
adder = library.Adder(2, name="adder")
builder = collimator.DiagramBuilder()
builder.add(plant, controller, pulse, adder)
# Connect the plant to the LQG controller
builder.connect(plant.output_ports[0], controller.input_ports[0])
builder.connect(controller.output_ports[0], adder.input_ports[0])
builder.connect(pulse.output_ports[0], adder.input_ports[1])
builder.connect(adder.output_ports[0], plant.input_ports[0])
return builder.build(name=name)
def make_lqg_diagram(plant, controller, pulse_amplitude=1.0, name="root"):
# Add initial impulse
pulse = make_pulse(amplitude=pulse_amplitude, start_time=2.0, width=0.1) # noqa: F405
adder = library.Adder(2, name="adder")
builder = collimator.DiagramBuilder()
builder.add(plant, controller, pulse, adder)
# Connect the plant to the LQG controller
builder.connect(plant.output_ports[0], controller.input_ports[0])
builder.connect(controller.output_ports[0], adder.input_ports[0])
builder.connect(pulse.output_ports[0], adder.input_ports[1])
builder.connect(adder.output_ports[0], plant.input_ports[0])
return builder.build(name=name)
In [ ]:
Copied!
vhil_plant = library.QuanserHAL(dt=dt, hardware=True, version=3, name="Qube")
balance_controller = make_lqg(dt_sys, dt=dt, Q=Q, R=R, QN=QN, RN=RN, x0=None) # noqa: F405
system = make_lqg_diagram(vhil_plant, balance_controller, pulse_amplitude=0.0)
system.pprint()
context = system.create_context()
vhil_plant = library.QuanserHAL(dt=dt, hardware=True, version=3, name="Qube")
balance_controller = make_lqg(dt_sys, dt=dt, Q=Q, R=R, QN=QN, RN=RN, x0=None) # noqa: F405
system = make_lqg_diagram(vhil_plant, balance_controller, pulse_amplitude=0.0)
system.pprint()
context = system.create_context()
In [ ]:
Copied!
tf = 10.0
tf = 10.0
In [ ]:
Copied!
print("Sleeping for 1 second to allow the interface to start...")
sleep(1)
print("Sleeping for 1 second to allow the interface to start...")
sleep(1)
In [ ]:
Copied!
results = collimator.simulate(
system,
context,
(0.0, tf),
)
results = collimator.simulate(
system,
context,
(0.0, tf),
)
In [ ]:
Copied!
vhil_plant.terminate()
vhil_plant.terminate()