Skip to content

Commit f02b1be

Browse files
murrayrmmurrayrm
committed
optimize via null space + updated testing
This commit changes the way that cost functions and constraints are handled for flat system to carry out optimization only in the null space of the flat system basis coefficients, eliminating the use of an equality constraint for the terminal condition.
1 parent 67a2169 commit f02b1be

5 files changed

Lines changed: 171 additions & 114 deletions

File tree

‎benchmarks/flatsys_bench.py‎

Lines changed: 2 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -94,7 +94,8 @@ def time_steering_cost():
9494
opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
9595

9696
traj = flat.point_to_point(
97-
vehicle, timepts, x0, u0, xf, uf, cost=traj_cost
97+
vehicle, timepts, x0, u0, xf, uf,
98+
cost=traj_cost, constraints=constraints, basis=flat.PolyFamily(8)
9899
)
99100

100101
# Verify that the trajectory computation is correct
@@ -104,39 +105,3 @@ def time_steering_cost():
104105
np.testing.assert_array_almost_equal(xf, x[:, 1])
105106
np.testing.assert_array_almost_equal(uf, u[:, 1])
106107

107-
def skip_steering_bezier_basis(nbasis, ntimes):
108-
# Set up costs and constriants
109-
Q = np.diag([.1, 10, .1]) # keep lateral error low
110-
R = np.diag([1, 1]) # minimize applied inputs
111-
cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
112-
constraints = [ opt.input_range_constraint(vehicle, [0, -0.1], [20, 0.1]) ]
113-
terminal = [ opt.state_range_constraint(vehicle, xf, xf) ]
114-
115-
# Set up horizon
116-
horizon = np.linspace(0, Tf, ntimes, endpoint=True)
117-
118-
# Set up the optimal control problem
119-
res = opt.solve_ocp(
120-
vehicle, horizon, x0, cost,
121-
constraints,
122-
terminal_constraints=terminal,
123-
initial_guess=bend_left,
124-
basis=flat.BezierFamily(nbasis, T=Tf),
125-
# solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2},
126-
minimize_method='trust-constr',
127-
minimize_options={'finite_diff_rel_step': 0.01},
128-
# minimize_method='SLSQP', minimize_options={'eps': 0.01},
129-
return_states=True, print_summary=False
130-
)
131-
t, u, x = res.time, res.inputs, res.states
132-
133-
# Make sure we found a valid solution
134-
assert res.success
135-
136-
# Reset the timeout value to allow for longer runs
137-
skip_steering_bezier_basis.timeout = 120
138-
139-
# Set the parameter values for the number of times and basis vectors
140-
skip_steering_bezier_basis.param_names = ['nbasis', 'ntimes']
141-
skip_steering_bezier_basis.params = ([2, 4, 6], [5, 10, 20])
142-

‎control/flatsys/flatsys.py‎

Lines changed: 39 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -42,9 +42,11 @@
4242
import numpy as np
4343
import scipy as sp
4444
import scipy.optimize
45+
import warnings
4546
from .poly import PolyFamily
4647
from .systraj import SystemTrajectory
4748
from ..iosys import NonlinearIOSystem
49+
from ..timeresp import _check_convert_array
4850

4951

5052
# Flat system class (for use as a base class)
@@ -217,7 +219,7 @@ def _flat_outfcn(self, t, x, u, params={}):
217219

218220
# Solve a point to point trajectory generation problem for a flat system
219221
def point_to_point(
220-
sys, timepts, x0, u0, xf, uf, T0=0, basis=None, cost=None,
222+
sys, timepts, x0=0, u0=0, xf=0, uf=0, T0=0, basis=None, cost=None,
221223
constraints=None, initial_guess=None, minimize_kwargs={}):
222224
"""Compute trajectory between an initial and final conditions.
223225
@@ -289,12 +291,14 @@ def point_to_point(
289291
#
290292
# Make sure the problem is one that we can handle
291293
#
292-
# TODO: put in tests for flat system input
293-
# TODO: process initial and final conditions to allow x0 or (x0, u0)
294-
if x0 is None: x0 = np.zeros(sys.nstates)
295-
if u0 is None: u0 = np.zeros(sys.ninputs)
296-
if xf is None: xf = np.zeros(sys.nstates)
297-
if uf is None: uf = np.zeros(sys.ninputs)
294+
x0 = _check_convert_array(x0, [(sys.nstates,), (sys.nstates, 1)],
295+
'Initial state: ', squeeze=True)
296+
u0 = _check_convert_array(u0, [(sys.ninputs,), (sys.ninputs, 1)],
297+
'Initial input: ', squeeze=True)
298+
xf = _check_convert_array(xf, [(sys.nstates,), (sys.nstates, 1)],
299+
'Final state: ' , squeeze=True)
300+
uf = _check_convert_array(uf, [(sys.ninputs,), (sys.ninputs, 1)],
301+
'Final input: ', squeeze=True)
298302

299303
# Process final time
300304
timepts = np.atleast_1d(timepts)
@@ -307,11 +311,16 @@ def point_to_point(
307311

308312
# If no basis set was specified, use a polynomial basis (poor choice...)
309313
if basis is None:
310-
basis = PolyFamily(2*sys.nstates)
314+
basis = PolyFamily(2 * (sys.nstates + sys.ninputs))
311315

312316
# Make sure we have enough basis functions to solve the problem
313317
if basis.N * sys.ninputs < 2 * (sys.nstates + sys.ninputs):
314318
raise ValueError("basis set is too small")
319+
elif (cost is not None or constraints is not None) and \
320+
basis.N * sys.ninputs == 2 * (sys.nstates + sys.ninputs):
321+
warnings.warn("minimal basis specified; optimization not possible")
322+
cost = None
323+
constraints = None
315324

316325
#
317326
# Map the initial and final conditions to flat output conditions
@@ -380,14 +389,18 @@ def point_to_point(
380389
# scipy.optimize.minimize().
381390
#
382391

383-
# Look to see if we have costs, constraints, or both
384-
if cost is None and constraints is None:
385-
# Unconstrained => solve a least squares problem
386-
alpha, residuals, rank, s = np.linalg.lstsq(M, Z, rcond=None)
392+
# Start by solving the least squares problem
393+
alpha, residuals, rank, s = np.linalg.lstsq(M, Z, rcond=None)
394+
395+
if cost is not None or constraints is not None:
396+
# Search over the null space to minimize cost/satisfy constraints
397+
N = sp.linalg.null_space(M)
387398

388-
else:
389399
# Define a function to evaluate the cost along a trajectory
390-
def traj_cost(coeffs):
400+
def traj_cost(null_coeffs):
401+
# Add this to the existing solution
402+
coeffs = alpha + N @ null_coeffs
403+
391404
# Evaluate the costs at the listed time points
392405
costval = 0
393406
for t in timepts:
@@ -418,9 +431,11 @@ def traj_cost(coeffs):
418431
traj_cost = lambda coeffs: coeffs @ coeffs
419432

420433
# Process the constraints we were given
434+
traj_constraints = constraints
421435
if constraints is None:
422436
traj_constraints = []
423437
elif isinstance(constraints, tuple):
438+
# TODO: Check to make sure this is really a constraint
424439
traj_constraints = [constraints]
425440
elif not isinstance(constraints, list):
426441
raise TypeError("trajectory constraints must be a list")
@@ -429,7 +444,10 @@ def traj_cost(coeffs):
429444
minimize_constraints = []
430445
if len(traj_constraints) > 0:
431446
# Set up a nonlinear function to evaluate the constraints
432-
def traj_const(coeffs):
447+
def traj_const(null_coeffs):
448+
# Add this to the existing solution
449+
coeffs = alpha + N @ null_coeffs
450+
433451
# Evaluate the constraints at the listed time points
434452
values = []
435453
for i, t in enumerate(timepts):
@@ -479,11 +497,11 @@ def traj_const(coeffs):
479497
traj_const, const_lb, const_ub)]
480498

481499
# Add initial and terminal constraints
482-
minimize_constraints += [sp.optimize.LinearConstraint(M, Z, Z)]
500+
# minimize_constraints += [sp.optimize.LinearConstraint(M, Z, Z)]
483501

484502
# Process the initial condition
485503
if initial_guess is None:
486-
initial_guess = np.zeros(basis.N * sys.ninputs)
504+
initial_guess = np.zeros(basis.N * sys.ninputs - 2 * flag_tot)
487505
else:
488506
raise NotImplementedError("Initial guess not yet implemented.")
489507

@@ -492,9 +510,11 @@ def traj_const(coeffs):
492510
traj_cost, initial_guess, constraints=minimize_constraints,
493511
**minimize_kwargs)
494512
if res.success:
495-
alpha = res.x
513+
alpha += N @ res.x
496514
else:
497-
raise RuntimeError("Can't solve optimization problem")
515+
raise RuntimeError(
516+
"Unable to solve optimal control problem\n" +
517+
"scipy.optimize.minimize returned " + res.message)
498518

499519
#
500520
# Transform the trajectory from flat outputs to states and inputs

‎control/tests/flatsys_test.py‎

Lines changed: 66 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -122,16 +122,20 @@ def test_kinematic_car(self, vehicle_flat, poly):
122122
vehicle_flat, T, ud, x0, return_x=True)
123123
np.testing.assert_allclose(x, xd, atol=0.01, rtol=0.01)
124124

125-
def test_kinematic_car_cost_constr(self, vehicle_flat):
125+
def test_flat_cost_constr(self):
126+
# Double integrator system
127+
sys = ct.ss([[0, 1], [0, 0]], [[0], [1]], [[1, 0]], 0)
128+
flat_sys = fs.LinearFlatSystem(sys)
129+
126130
# Define the endpoints of the trajectory
127-
x0 = [0., -2., 0.]; u0 = [10., 0.]
128-
xf = [100., 2., 0.]; uf = [10., 0.]
131+
x0 = [1, 0]; u0 = [0]
132+
xf = [0, 0]; uf = [0]
129133
Tf = 10
130134
T = np.linspace(0, Tf, 500)
131135

132136
# Find trajectory between initial and final conditions
133137
traj = fs.point_to_point(
134-
vehicle_flat, Tf, x0, u0, xf, uf, basis=fs.PolyFamily(6))
138+
flat_sys, Tf, x0, u0, xf, uf, basis=fs.PolyFamily(8))
135139
x, u = traj.eval(T)
136140

137141
np.testing.assert_array_almost_equal(x0, x[:, 0])
@@ -142,11 +146,13 @@ def test_kinematic_car_cost_constr(self, vehicle_flat):
142146
# Solve with a cost function
143147
timepts = np.linspace(0, Tf, 10)
144148
cost_fcn = opt.quadratic_cost(
145-
vehicle_flat, np.diag([0, 0.1, 0]), np.diag([0.1, 1]), x0=xf, u0=uf)
149+
flat_sys, np.diag([0, 0]), 1, x0=xf, u0=uf)
146150

147151
traj_cost = fs.point_to_point(
148-
vehicle_flat, timepts, x0, u0, xf, uf, cost=cost_fcn,
149-
basis=fs.PolyFamily(8)
152+
flat_sys, timepts, x0, u0, xf, uf, cost=cost_fcn,
153+
basis=fs.PolyFamily(8),
154+
# initial_guess='lstsq',
155+
# minimize_kwargs={'method': 'trust-constr'}
150156
)
151157

152158
# Verify that the trajectory computation is correct
@@ -159,22 +165,18 @@ def test_kinematic_car_cost_constr(self, vehicle_flat):
159165
# Make sure that we got a different answer than before
160166
assert np.any(np.abs(x - x_cost) > 0.1)
161167

162-
# Make sure that the previous computation had large y deviation
163-
assert np.any(x_cost[1, :] > 2.6)
164-
165168
# Re-solve with constraint on the y deviation
166-
# timepts = np.array([0, 0.5*Tf, 0.8*Tf, Tf])
167-
constraints = (
168-
sp.optimize.LinearConstraint,
169-
np.array([[0, 1, 0, 0, 0]]), -2.6, 2.6)
169+
lb, ub = [-2, -0.1], [2, 0]
170+
lb, ub = [-2, np.min(x_cost[1])*0.95], [2, 1]
171+
constraints = [opt.state_range_constraint(flat_sys, lb, ub)]
172+
173+
# Make sure that the previous solution violated at least one constraint
174+
assert np.any(x_cost[0, :] < lb[0]) or np.any(x_cost[0, :] > ub[0]) \
175+
or np.any(x_cost[1, :] < lb[1]) or np.any(x_cost[1, :] > ub[1])
176+
170177
traj_const = fs.point_to_point(
171-
vehicle_flat, timepts, x0, u0, xf, uf, cost=cost_fcn,
178+
flat_sys, timepts, x0, u0, xf, uf, cost=cost_fcn,
172179
constraints=constraints, basis=fs.PolyFamily(8),
173-
# minimize_kwargs={
174-
# 'method': 'trust-constr',
175-
# 'options': {'finite_diff_rel_step': 0.01},
176-
# # 'hess': lambda x: np.zeros((x.size, x.size))
177-
# }
178180
)
179181

180182
# Verify that the trajectory computation is correct
@@ -184,8 +186,10 @@ def test_kinematic_car_cost_constr(self, vehicle_flat):
184186
np.testing.assert_array_almost_equal(xf, x_const[:, -1])
185187
np.testing.assert_array_almost_equal(uf, u_const[:, -1])
186188

187-
# Make sure that the solution respects the bounds
188-
assert np.any(x_const[:, 1] < 2.6)
189+
# Make sure that the solution respects the bounds (with some slop)
190+
for i in range(x_const.shape[0]):
191+
assert np.all(x_const[i] >= lb[i] * 1.02)
192+
assert np.all(x_const[i] <= ub[i] * 1.02)
189193

190194
def test_bezier_basis(self):
191195
bezier = fs.BezierFamily(4)
@@ -223,3 +227,43 @@ def test_bezier_basis(self):
223227
# Exception check
224228
with pytest.raises(ValueError, match="index too high"):
225229
bezier.eval_deriv(4, 0, time)
230+
231+
def test_point_to_point_errors(self):
232+
"""Test error and warning conditions in point_to_point()"""
233+
# Double integrator system
234+
sys = ct.ss([[0, 1], [0, 0]], [[0], [1]], [[1, 0]], 0)
235+
flat_sys = fs.LinearFlatSystem(sys)
236+
237+
# Define the endpoints of the trajectory
238+
x0 = [1, 0]; u0 = [0]
239+
xf = [0, 0]; uf = [0]
240+
Tf = 10
241+
T = np.linspace(0, Tf, 500)
242+
243+
# Cost function
244+
timepts = np.linspace(0, Tf, 10)
245+
cost_fcn = opt.quadratic_cost(
246+
flat_sys, np.diag([1, 1]), 1, x0=xf, u0=uf)
247+
248+
# Try to optimize with insufficient degrees of freedom
249+
with pytest.warns(UserWarning, match="optimization not possible"):
250+
traj = fs.point_to_point(
251+
flat_sys, timepts, x0, u0, xf, uf, cost=cost_fcn,
252+
basis=fs.PolyFamily(6))
253+
254+
# Make sure we still solved the problem
255+
x, u = traj.eval(timepts)
256+
np.testing.assert_array_almost_equal(x0, x[:, 0])
257+
np.testing.assert_array_almost_equal(u0, u[:, 0])
258+
np.testing.assert_array_almost_equal(xf, x[:, -1])
259+
np.testing.assert_array_almost_equal(uf, u[:, -1])
260+
261+
# Solve with the errors in the various input arguments
262+
with pytest.raises(ValueError, match="Initial state: Wrong shape"):
263+
traj = fs.point_to_point(flat_sys, timepts, np.zeros(3), u0, xf, uf)
264+
with pytest.raises(ValueError, match="Initial input: Wrong shape"):
265+
traj = fs.point_to_point(flat_sys, timepts, x0, np.zeros(3), xf, uf)
266+
with pytest.raises(ValueError, match="Final state: Wrong shape"):
267+
traj = fs.point_to_point(flat_sys, timepts, x0, u0, np.zeros(3), uf)
268+
with pytest.raises(ValueError, match="Final input: Wrong shape"):
269+
traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf, np.zeros(3))

‎control/tests/optimal_test.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -459,7 +459,7 @@ def test_optimal_basis_simple():
459459
sys, time, x0, cost, constraints, initial_guess=0.99*res1.inputs,
460460
basis=flat.BezierFamily(4, Tf), return_x=True)
461461
assert res2.success
462-
np.testing.assert_almost_equal(res2.inputs, res1.inputs, decimal=3)
462+
np.testing.assert_allclose(res2.inputs, res1.inputs, atol=0.01, rtol=0.01)
463463

464464
# Run with logging turned on for code coverage
465465
res3 = opt.solve_ocp(

0 commit comments

Comments
 (0)