10000 add processing for linear, cubic interpolation · python-control/python-control@5fcf2b5 · GitHub
[go: up one dir, main page]
Skip to content

Commit 5fcf2b5

Browse files
murrayrmmurrayrm
committed
add processing for linear, cubic interpolation
1 parent 8710ee2 commit 5fcf2b5

File tree

2 files changed

+43
-31
lines changed

2 files changed

+43
-31
lines changed

‎control/statefbk.py

Lines changed: 21 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -802,6 +802,26 @@ def create_statefbk_iosystem(
802802

803803
# TODO: Process scheduling variables
804804

805+
# Create interpolating function
806+
if method == 'nearest':
807+
_interp = sp.interpolate.NearestNDInterpolator(points, gains)
808+
_nearest = _interp
809+
elif method == 'linear':
810+
_interp = sp.interpolate.LinearNDInterpolator(points, gains)
811+
_nearest = sp.interpolate.NearestNDInterpolator(points, gains)
812+
elif method == 'cubic':
813+
_interp = sp.interpolate.CloughTocher2DInterpolator(points, gains)
814+
_nearest = sp.interpolate.NearestNDInterpolator(points, gains)
815+
else:
816+
raise ControlArgument(
817+
f"unknown gain scheduling method '{method}'")
818+
819+
def _compute_gain(mu):
820+
K = _interp(mu)
821+
if np.isnan(K).any():
822+
K = _nearest(mu)
823+
return K
824+
805825
# Define the controller system
806826
if type == 'nonlinear':
807827
# Create an I/O system for the state feedback gains
@@ -813,19 +833,10 @@ def _control_update(t, states, inputs, params):
813833
# Compute the integral error in the xy coordinates
814834
return C @ (x_vec - xd_vec)
815835

816-
def _compute_gain(mu, gains_, points_):
817-
K = np.array([
818-
[sp.interpolate.griddata(
819-
points_, gains_[:, i, j], mu, method=method).item()
820-
for j in range(gains_.shape[2])]
821-
for i in range(gains_.shape[1])
822-
])
823-
return K
824-
825836
def _control_output(t, states, inputs, params):
826837
if gainsched:
827838
mu = inputs[gainsched_indices]
828-
K_ = _compute_gain(mu, gains, points)
839+
K_ = _compute_gain(mu)
829840
else:
830841
K_ = params.get('K')
831842

‎control/tests/statefbk_test.py

Lines changed: 22 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -799,18 +799,18 @@ def unicycle_output(t, x, u, params):
799799

800800
from math import pi
801801

802-
@pytest.mark.parametrize("method", [None, 'nearest'])
802+
@pytest.mark.parametrize("method", [None, 'nearest', 'linear', 'cubic'])
803803
def test_gainsched_unicycle(unicycle, method):
804804
# Speeds and angles at which to compute the gains
805805
speeds = [1, 5, 10]
806-
angles = -pi + np.linspace(0, 2*pi, 10)
806+
angles = np.linspace(0, pi/2, 4)
807807
points = list(itertools.product(speeds, angles))
808808

809809
# Gains for each speed (using LQR controller)
810810
Q = np.identity(unicycle.nstates)
811811
R = np.identity(unicycle.ninputs)
812-
gains = [ct.lqr(unicycle.linearize(
813-
[0, 0, angle], [speed, 0]), Q, R)[0] for speed, angle in points]
812+
gains = [np.array(ct.lqr(unicycle.linearize(
813+
[0, 0, angle], [speed, 0]), Q, R)[0]) for speed, angle in points]
814814

815815
#
816816
# Schedule on desired speed and angle
@@ -836,13 +836,28 @@ def test_gainsched_unicycle(unicycle, method):
836836

837837
# Check the closed loop system at the scheduling points
838838
clsys_lin = clsys.linearize(xe, [xd, ud])
839-
np.testing.assert_allclose(np.sort(
840-
clsys_lin.poles()), np.sort(E), rtol=1e-2)
839+
np.testing.assert_allclose(
840+
np.sort(clsys_lin.poles()), np.sort(E), rtol=1e-2)
841+
842+
# Check the gain at an intermediate point and confirm stability
843+
speed, angle = 2, pi/3
844+
xe, ue = np.array([0, 0, angle]), np.array([speed, 0])
845+
xd, ud = np.array([0, 0, angle]), np.array([speed, 0])
846+
clsys_lin = clsys.linearize(xe, [xd, ud])
847+
assert np.all(np.real(clsys_lin.poles()) < 0)
848+
849+
# Make sure that gains are different from 'nearest'
850+
if method is not None and method != 'nearest':
851+
ctrl_nearest, clsys_nearest = ct.create_statefbk_iosystem(
852+
unicycle, (gains, points, 'nearest'), gainsched_indices=[3, 2])
853+
nearest_lin = clsys_nearest.linearize(xe, [xd, ud])
854+
assert not np.allclose(
855+
np.sort(clsys_lin.poles()), np.sort(nearest_lin.poles()), rtol=1e-2)
841856

842857
# Run a simulation following a curved path
843858
T = 10 # length of the trajectory [sec]
844859
r = 10 # radius of the circle [m]
845-
timepts = np.linspace(0, T, 100)
860+
timepts = np.linspace(0, T, 50)
846861
Xd = np.vstack([
847862
r * np.cos(timepts/T * pi/2 + 3*pi/2),
848863
r * np.sin(timepts/T * pi/2 + 3*pi/2) + r,
@@ -885,20 +900,6 @@ def test_gainsched_unicycle(unicycle, method):
885900
clsys_lin.poles()), np.sort(E), rtol=1e-2)
886901

887902
# Run a simulation following a curved path
888-
T = 10 # length of the trajectory [sec]
889-
r = 10 # radius of the circle [m]
890-
timepts = np.linspace(0, T, 100)
891-
Xd = np.vstack([
892-
r * np.cos(timepts/T * pi/2 + 3*pi/2),
893-
r * np.sin(timepts/T * pi/2 + 3*pi/2) + r,
894-
timepts/T * pi/2
895-
])
896-
Ud = np.vstack([
897-
np.ones_like(timepts) * (r * pi/2) / T,
898-
np.ones_like(timepts) * (pi / 2) / T
899-
])
900-
X0 = Xd[:, 0] + np.array([-0.1, -0.1, -0.1])
901-
902903
resp = ct.input_output_response(clsys, timepts, [Xd, Ud], X0)
903904
np.testing.assert_allclose(
904905
resp.states[:, -1], Xd[:, -1], atol=1e-2, rtol=1e-2)

0 commit comments

Comments
 (0)
0