Skip to content

Commit 4ef15c4

Browse files
murrayrmmurrayrm
authored
Merge pull request #709 from murrayrm/cds112
Fixes to various optimization-based control functions
2 parents e10855e + f807b33 commit 4ef15c4

File tree

11 files changed

+283
-95
lines changed

11 files changed

+283
-95
lines changed

‎control/flatsys/bezier.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,7 @@ class BezierFamily(BasisFamily):
5555
"""
5656
def __init__(self, N, T=1):
5757
"""Create a polynomial basis of order N."""
58-
self.N = N # save number of basis functions
58+
super(BezierFamily, self).__init__(N)
5959
self.T = T # save end of time interval
6060

6161
# Compute the kth derivative of the ith basis function at time t

‎control/flatsys/flatsys.py‎

Lines changed: 30 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -108,15 +108,15 @@ class FlatSystem(NonlinearIOSystem):
108108
-----
109109
The class must implement two functions:
110110
111-
zflag = flatsys.foward(x, u)
111+
zflag = flatsys.foward(x, u, params)
112112
This function computes the flag (derivatives) of the flat output.
113113
The inputs to this function are the state 'x' and inputs 'u' (both
114114
1D arrays). The output should be a 2D array with the first
115115
dimension equal to the number of system inputs and the second
116116
dimension of the length required to represent the full system
117117
dynamics (typically the number of states)
118118
119-
x, u = flatsys.reverse(zflag)
119+
x, u = flatsys.reverse(zflag, params)
120120
This function system state and inputs give the the flag (derivatives)
121121
of the flat output. The input to this function is an 2D array whose
122122
first dimension is equal to the number of system inputs and whose
@@ -216,7 +216,7 @@ def _flat_updfcn(self, t, x, u, params={}):
216216
def _flat_outfcn(self, t, x, u, params={}):
217217
# Return the flat output
218218
zflag = self.forward(x, u, params)
219-
return np.array(zflag[:][0])
219+
return np.array([zflag[i][0] for i in range(len(zflag))])
220220

221221

222222
# Utility function to compute flag matrix given a basis
@@ -244,8 +244,8 @@ def _basis_flag_matrix(sys, basis, flag, t, params={}):
244244

245245
# Solve a point to point trajectory generation problem for a flat system
246246
def point_to_point(
247-
sys, timepts, x0=0, u0=0, xf=0, uf=0, T0=0, basis=None, cost=None,
248-
constraints=None, initial_guess=None, minimize_kwargs={}, **kwargs):
247+
sys, timepts, x0=0, u0=0, xf=0, uf=0, T0=0, cost=None, basis=None,
248+
trajectory_constraints=None, initial_guess=None, params=None, **kwargs):
249249
"""Compute trajectory between an initial and final conditions.
250250
251251
Compute a feasible trajectory for a differentially flat system between an
@@ -284,7 +284,7 @@ def point_to_point(
284284
Function that returns the integral cost given the current state
285285
and input. Called as `cost(x, u)`.
286286
287-
constraints : list of tuples, optional
287+
trajectory_constraints : list of tuples, optional
288288
List of constraints that should hold at each point in the time vector.
289289
Each element of the list should consist of a tuple with first element
290290
given by :class:`scipy.optimize.LinearConstraint` or
@@ -337,8 +337,15 @@ def point_to_point(
337337
T0 = timepts[0] if len(timepts) > 1 else T0
338338

339339
# Process keyword arguments
340+
if trajectory_constraints is None:
341+
# Backwards compatibility
342+
trajectory_constraints = kwargs.pop('constraints', None)
343+
344+
minimize_kwargs = {}
340345
minimize_kwargs['method'] = kwargs.pop('minimize_method', None)
341346
minimize_kwargs['options'] = kwargs.pop('minimize_options', {})
347+
minimize_kwargs.update(kwargs.pop('minimize_kwargs', {}))
348+
342349
if kwargs:
343350
raise TypeError("unrecognized keywords: ", str(kwargs))
344351

@@ -353,11 +360,14 @@ def point_to_point(
353360
# Make sure we have enough basis functions to solve the problem
354361
if basis.N * sys.ninputs < 2 * (sys.nstates + sys.ninputs):
355362
raise ValueError("basis set is too small")
356-
elif (cost is not None or constraints is not None) and \
363+
elif (cost is not None or trajectory_constraints is not None) and \
357364
basis.N * sys.ninputs == 2 * (sys.nstates + sys.ninputs):
358365
warnings.warn("minimal basis specified; optimization not possible")
359366
cost = None
360-
constraints = None
367+
trajectory_constraints = None
368+
369+
# Figure out the parameters to use, if any
370+
params = sys.params if params is None else params
361371

362372
#
363373
# Map the initial and final conditions to flat output conditions
@@ -366,8 +376,8 @@ def point_to_point(
366376
# and then evaluate this at the initial and final condition.
367377
#
368378

369-
zflag_T0 = sys.forward(x0, u0)
370-
zflag_Tf = sys.forward(xf, uf)
379+
zflag_T0 = sys.forward(x0, u0, params)
380+
zflag_Tf = sys.forward(xf, uf, params)
371381

372382
#
373383
# Compute the matrix constraints for initial and final conditions
@@ -400,7 +410,7 @@ def point_to_point(
400410
# Start by solving the least squares problem
401411
alpha, residuals, rank, s = np.linalg.lstsq(M, Z, rcond=None)
402412

403-
if cost is not None or constraints is not None:
413+
if cost is not None or trajectory_constraints is not None:
404414
# Search over the null space to minimize cost/satisfy constraints
405415
N = sp.linalg.null_space(M)
406416

@@ -418,7 +428,7 @@ def traj_cost(null_coeffs):
418428
zflag = (M_t @ coeffs).reshape(sys.ninputs, -1)
419429

420430
# Find states and inputs at the time points
421-
x, u = sys.reverse(zflag)
431+
x, u = sys.reverse(zflag, params)
422432

423433
# Evaluate the cost at this time point
424434
costval += cost(x, u)
@@ -429,13 +439,13 @@ def traj_cost(null_coeffs):
429439
traj_cost = lambda coeffs: coeffs @ coeffs
430440

431441
# Process the constraints we were given
432-
traj_constraints = constraints
433-
if constraints is None:
442+
traj_constraints = trajectory_constraints
443+
if traj_constraints is None:
434444
traj_constraints = []
435-
elif isinstance(constraints, tuple):
445+
elif isinstance(traj_constraints, tuple):
436446
# TODO: Check to make sure this is really a constraint
437-
traj_constraints = [constraints]
438-
elif not isinstance(constraints, list):
447+
traj_constraints = [traj_constraints]
448+
elif not isinstance(traj_constraints, list):
439449
raise TypeError("trajectory constraints must be a list")
440450

441451
# Process constraints
@@ -456,7 +466,7 @@ def traj_const(null_coeffs):
456466
zflag = (M_t @ coeffs).reshape(sys.ninputs, -1)
457467

458468
# Find states and inputs at the time points
459-
states, inputs = sys.reverse(zflag)
469+
states, inputs = sys.reverse(zflag, params)
460470

461471
# Evaluate the constraint function along the trajectory
462472
for type, fun, lb, ub in traj_constraints:
@@ -507,8 +517,8 @@ def traj_const(null_coeffs):
507517
# Transform the trajectory from flat outputs to states and inputs
508518
#
509519

510-
# Createa trajectory object to store the resul
511-
systraj = SystemTrajectory(sys, basis)
520+
# Create a trajectory object to store the result
521+
systraj = SystemTrajectory(sys, basis, params=params)
512522

513523
# Store the flag lengths and coefficients
514524
# TODO: make this more pythonic

‎control/flatsys/linflat.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -113,7 +113,7 @@ def __init__(self, linsys, inputs=None, outputs=None, states=None,
113113
self.Cf = Cfz @ Tr
114114

115115
# Compute the flat flag from the state (and input)
116-
def forward(self, x, u):
116+
def forward(self, x, u, params):
117117
"""Compute the flat flag given the states and input.
118118
119119
See :func:`control.flatsys.FlatSystem.forward` for more info.
@@ -130,7 +130,7 @@ def forward(self, x, u):
130130
return zflag
131131

132132
# Compute state and input from flat flag
133-
def reverse(self, zflag):
133+
def reverse(self, zflag, params):
134134
"""Compute the states and input given the flat flag.
135135
136136
See :func:`control.flatsys.FlatSystem.reverse` for more info.

‎control/flatsys/poly.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,7 @@ class PolyFamily(BasisFamily):
5252
"""
5353
def __init__(self, N):
5454
"""Create a polynomial basis of order N."""
55-
self.N = N # save number of basis functions
55+
super(PolyFamily, self).__init__(N)
5656

5757
# Compute the kth derivative of the ith basis function at time t
5858
def eval_deriv(self, i, k, t):

‎control/flatsys/systraj.py‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -62,14 +62,15 @@ class SystemTrajectory:
6262
6363
"""
6464

65-
def __init__(self, sys, basis, coeffs=[], flaglen=[]):
65+
def __init__(self, sys, basis, coeffs=[], flaglen=[], params=None):
6666
"""Initilize a system trajectory object."""
6767
self.nstates = sys.nstates
6868
self.ninputs = sys.ninputs
6969
self.system = sys
7070
self.basis = basis
7171
self.coeffs = list(coeffs)
7272
self.flaglen = list(flaglen)
73+
self.params = sys.params if params is None else params
7374

7475
# Evaluate the trajectory over a list of time points
7576
def eval(self, tlist):
@@ -112,6 +113,7 @@ def eval(self, tlist):
112113

113114
# Now copy the states and inputs
114115
# TODO: revisit order of list arguments
115-
xd[:,tind], ud[:,tind] = self.system.reverse(zflag)
116+
xd[:,tind], ud[:,tind] = \
117+
self.system.reverse(zflag, self.params)
116118

117119
return xd, ud

‎control/iosys.py‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1820,6 +1820,7 @@ def input_output_response(
18201820
legal_shapes = [(sys.ninputs, n_steps)]
18211821
U = _check_convert_array(U, legal_shapes,
18221822
'Parameter ``U``: ', squeeze=False)
1823+
U = U.reshape(-1, n_steps)
18231824

18241825
# Check to make sure this is not a static function
18251826
nstates = _find_size(sys.nstates, X0)
@@ -1870,6 +1871,11 @@ def ivp_rhs(t, x):
18701871
ivp_rhs, (T0, Tf), X0, t_eval=T,
18711872
vectorized=False, **solve_ivp_kwargs)
18721873

1874+
if not soln.success or soln.status != 0:
1875+
# Something went wrong
1876+
warn("sp.integrate.solve_ivp failed")
1877+
print("Return bunch:", soln)
1878+
18731879
# Compute the output associated with the state (and use sys.out to
18741880
# figure out the number of outputs just in case it wasn't specified)
18751881
u = U[0] if len(U.shape) == 1 else U[:, 0]
@@ -1886,7 +1892,7 @@ def ivp_rhs(t, x):
18861892
"equally spaced.")
18871893

18881894
# Make sure the sample time matches the given time
1889-
if (sys.dt is not True):
1895+
if sys.dt is not True:
18901896
# Make sure that the time increment is a multiple of sampling time
18911897

18921898
# TODO: add back functionality for undersampling
@@ -1903,7 +1909,7 @@ def ivp_rhs(t, x):
19031909
# Compute the solution
19041910
soln = sp.optimize.OptimizeResult()
19051911
soln.t = T # Store the time vector directly
1906-
x = [float(x0) for x0 in X0] # State vector (store as floats)
1912+
x = X0 # Initilize state
19071913
soln.y = [] # Solution, following scipy convention
19081914
y = [] # System output
19091915
for i in range(len(T)):

0 commit comments

Comments
 (0)