def test_problem_3_4(self):
"""Problem 3.4: LQR ROA, Prologue"""
from set_3_for_testing import lqr_controller
from set_3_for_testing import get_x0_does_not_converge
from set_3_for_testing import get_x0_does_converge
from inertial_wheel_pendulum import RunSimulation
# Run a forward sim from here
duration = 2.
eps = 1E-2 # pretty permissive, but should catch divergences
x0 = get_x0_does_converge()
input_log, state_log = RunSimulation(self.pendulum_plant,
lqr_controller,
x0 = x0,
duration = duration)
self.checkConvergenceOfStateLog(state_log, True)
x0 = get_x0_does_not_converge()
input_log, state_log = RunSimulation(self.pendulum_plant,
lqr_controller,
x0 = x0,
duration = duration)
self.checkConvergenceOfStateLog(state_log, False)
def test_problem_3_8(self):
"""Problem 3.8: Combined Swing-Up and Stabilization"""
# Assume from the run-through that vsamples, vdot samples are good...
from inertial_wheel_pendulum import RunSimulation
from set_3_for_testing import combined_controller
np.random.seed(0)
conditions_that_should_converge = [
np.array([0., 0., 0., 0.]),
np.array([math.pi, 0., 0., 0.]),
np.array([3*math.pi, 0., 0., 0.]),
np.array([0., -100., 0., 0.]),
np.array([0., 0., 0., 20.]),
np.random.random(4), # Three random initial conditions for fun
np.random.random(4),
np.random.random(4)
]
for x0 in conditions_that_should_converge:
# Run a forward sim from here
duration = 30.
input_log, state_log = RunSimulation(self.pendulum_plant,
combined_controller,
x0 = x0,
duration = duration)
self.checkConvergenceOfStateLog(state_log, True)
def test_problem_3_3(self):
"""Problem 3.3: LQR"""
from set_3_for_testing import create_reduced_lqr, lqr_controller
from inertial_wheel_pendulum import RunSimulation
A, B = self.pendulum_plant.GetLinearizedDynamics(np.array([0.]),
np.array([math.pi, 0., 0., 0.]))
K, S = create_reduced_lqr(A, B)
self.assertFalse(np.any(np.isnan(K)), "No elements of K should be NaN")
self.assertFalse(np.any(np.isnan(S)), "No elements of S should be NaN")
conditions_that_should_converge = [
np.array([math.pi, 1000.0, 0.0, 0.0]),
np.array([math.pi+0.05, 0.0, 0.0, 0.0]),
np.array([math.pi-0.05, 0.0, 0.1, 0.0]),
np.array([math.pi, 0.0, 0.0, 1.0]),
]
for x0 in conditions_that_should_converge:
# Run a forward sim from here
duration = 1.
input_log, state_log = RunSimulation(self.pendulum_plant,
lqr_controller,
x0 = x0,
duration = duration)
self.checkConvergenceOfStateLog(state_log, True)
def test_problem_3_6(self):
"""Problem 3.6: LQR ROA, Numerical estimate of the ROA"""
from inertial_wheel_pendulum import RunSimulation
# Assume from the run-through that vsamples, vdot samples are good...
from set_3_for_testing import (estimate_rho, V_samples, Vdot_samples,
calcV, calcVdot, lqr_controller)
# But regenerate rho to make sure that wasn't clobbered... more likely
# to have been, as it's a simpler name and just a scalar
rho = estimate_rho(V_samples, Vdot_samples)
self.assertGreater(rho, 0.0, "rho should be bigger than 0.")
self.assertLess(
rho, calcV(np.zeros(4)),
"rho can't include (0, 0, 0, 0) due to the input limits.")
np.random.seed(0)
# Sample at a few points in the ROA
# and sanity check that:
# Vdot is negative
# the LQR simulation converges from this position
for i in range(10):
# Rejection sample to find a rho
# (this might be a bad idea for small rho...)
sample_v = rho + 1.
while sample_v >= rho:
sample_x = np.random.random(4) * 10
sample_v = calcV(sample_x)
sample_vdot = calcVdot(sample_x)
self.assertLess(
sample_vdot, 0.0, "".join([
"Vdot sampled at x0=",
np.array_str(sample_x), " was positive (Vdot = ",
str(sample_vdot), ")"
]))
# Run a forward sim from here
duration = 10.
eps = 1E-2 # pretty permissive, but should catch divergences
input_log, state_log = RunSimulation(self.pendulum_plant,
lqr_controller,
x0=sample_x,
duration=duration)
self.checkConvergenceOfStateLog(state_log, True)