def test_problem_3_5(self):
"""Problem 3.5: LQR ROA, Evaluating F, V, and Vdot"""
from set_3_for_testing import calcF, calcV, calcVdot
# Sample at a couple of points and sanity-check some
# basic things.
# Not super-involved testing -- visual checking of the
# plots is easier and more informative.
np.random.seed(0)
for i in range(100):
sample_x = np.random.random(4)*100 - 50.
# Make sure they don't return a nan for any of these conditions
self.assertFalse(np.any(np.isnan(calcF(sample_x))),
"".join([
"calcF(",
np.array_str(sample_x),
") returned a NaN"
]))
if np.sum(np.abs(sample_x)) > 0.0:
self.assertGreater(calcV(sample_x), 0.0,
"".join([
"V(",
np.array_str(sample_x),
") was nonpositive."
]))
vAtFP = calcV(np.array([math.pi, 0., 0., 0.]))
self.assertAlmostEqual(vAtFP, 0.0,
msg="V(pi,0,0,0) = %f != 0.0" % vAtFP)
vdotAtFP = calcVdot(np.array([math.pi, 0., 0., 0.]))
self.assertAlmostEqual(vdotAtFP, 0.0,
msg="Vdot(pi,0,0,0) = %f != 0.0" % vdotAtFP)
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)