Skip to content

Commit 67a2169

Browse files
murrayrmmurrayrm
committed
updated docs, unit tests, code fixes
1 parent 8a9ab95 commit 67a2169

7 files changed

Lines changed: 138 additions & 144 deletions

File tree

‎control/flatsys/bezier.py‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,9 @@ class BezierFamily(BasisFamily):
4848
This class represents the family of polynomials of the form
4949
5050
.. math::
51-
\phi_i(t) = \sum_{i=0}^n {n \choose i} (T - t)^{n-i} t^i
51+
\phi_i(t) = \sum_{i=0}^n {n \choose i}
52+
\left( \frac{t}{T_\text{f}} - t \right)^{n-i}
53+
\left( \frac{t}{T_f} \right)^i
5254
5355
"""
5456
def __init__(self, N, T=1):

‎control/flatsys/flatsys.py‎

Lines changed: 45 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -247,9 +247,9 @@ def point_to_point(
247247
The initial time for the trajectory (corresponding to x0). If not
248248
specified, its value is taken to be zero.
249249
250-
basis : :class:`~control.flat.BasisFamily` object, optional
250+
basis : :class:`~control.flatsys.BasisFamily` object, optional
251251
The basis functions to use for generating the trajectory. If not
252-
specified, the :class:`~control.flat.PolyFamily` basis family will be
252+
specified, the :class:`~control.flatsys.PolyFamily` basis family will be
253253
used, with the minimal number of elements required to find a feasible
254254
trajectory (twice the number of system states)
255255
@@ -260,8 +260,8 @@ def point_to_point(
260260
constraints : list of tuples, optional
261261
List of constraints that should hold at each point in the time vector.
262262
Each element of the list should consist of a tuple with first element
263-
given by :meth:`scipy.optimize.LinearConstraint` or
264-
:meth:`scipy.optimize.NonlinearConstraint` and the remaining
263+
given by :class:`scipy.optimize.LinearConstraint` or
264+
:class:`scipy.optimize.NonlinearConstraint` and the remaining
265265
elements of the tuple are the arguments that would be passed to those
266266
functions. The following tuples are supported:
267267
@@ -280,7 +280,7 @@ def point_to_point(
280280
281281
Returns
282282
-------
283-
traj : :class:`~control.flat.SystemTrajectory` object
283+
traj : :class:`~control.flatsys.SystemTrajectory` object
284284
The system trajectory is returned as an object that implements the
285285
`eval()` function, we can be used to compute the value of the state
286286
and input and a given time t.
@@ -372,10 +372,13 @@ def point_to_point(
372372
#
373373
# At this point, we need to solve the equation M alpha = zflag, where M
374374
# is the matrix constrains for initial and final conditions and zflag =
375-
# [zflag_T0; zflag_tf]. Since everything is linear, just compute the
376-
# least squares solution for now.
375+
# [zflag_T0; zflag_tf].
376+
#
377+
# If there are no constraints, then we just need to solve a linear
378+
# system of equations => use least squares. Otherwise, we have a
379+
# nonlinear optimal control problem with equality constraints => use
380+
# scipy.optimize.minimize().
377381
#
378-
379382

380383
# Look to see if we have costs, constraints, or both
381384
if cost is None and constraints is None:
@@ -410,25 +413,26 @@ def traj_cost(coeffs):
410413
costval += cost(x, u)
411414
return costval
412415

413-
# If not cost given, override with magnitude of the coefficients
416+
# If no cost given, override with magnitude of the coefficients
414417
if cost is None:
415418
traj_cost = lambda coeffs: coeffs @ coeffs
416419

417420
# Process the constraints we were given
418421
if constraints is None:
419-
constraints = []
422+
traj_constraints = []
420423
elif isinstance(constraints, tuple):
421-
constraints = [constraints]
424+
traj_constraints = [constraints]
422425
elif not isinstance(constraints, list):
423426
raise TypeError("trajectory constraints must be a list")
424427

425428
# Process constraints
426-
if len(constraints) > 0:
429+
minimize_constraints = []
430+
if len(traj_constraints) > 0:
427431
# Set up a nonlinear function to evaluate the constraints
428432
def traj_const(coeffs):
429433
# Evaluate the constraints at the listed time points
430434
values = []
431-
for t in timepts:
435+
for i, t in enumerate(timepts):
432436
# Calculate the states and inputs for the flat output
433437
M_t = np.zeros((flag_tot, basis.N * sys.ninputs))
434438
flag_off = 0
@@ -439,44 +443,53 @@ def traj_const(coeffs):
439443
range(basis.N), range(flag_len)):
440444
M_t[flag_off + k, coeff_off + j] = \
441445
basis.eval_deriv(j, k, t)
442-
flag_off += flag_len
443-
coeff_off += basis.N
446+
flag_off += flag_len
447+
coeff_off += basis.N
444448

445449
# Compute flag at this time point
446450
zflag = (M_t @ coeffs).reshape(sys.ninputs, -1)
447451

448452
# Find states and inputs at the time points
449-
x, u = sys.reverse(zflag)
450-
451-
# Evaluate the constraints at this time point
452-
for constraint in constraints:
453-
values.append(constraint[0](x, u))
454-
lb.append(constraint[1])
455-
ub.append(constraint[2])
456-
return values
453+
states, inputs = sys.reverse(zflag)
454+
455+
# Evaluate the constraint function along the trajectory
456+
for type, fun, lb, ub in traj_constraints:
457+
if type == sp.optimize.LinearConstraint:
458+
# `fun` is A matrix associated with polytope...
459+
values.append(
460+
np.dot(fun, np.hstack([states, inputs])))
461+
elif type == sp.optimize.NonlinearConstraint:
462+
values.append(fun(states, inputs))
463+
else:
464+
raise TypeError("unknown constraint type %s" %
465+
constraint[0])
466+
return np.array(values).flatten()
457467

458468
# Store upper and lower bounds
459-
lb, ub = [], [], []
460-
for constraint in constraints:
461-
lb.append(constraint[1])
462-
ub.append(constraint[2])
469+
const_lb, const_ub = [], []
470+
for t in timepts:
471+
for type, fun, lb, ub in traj_constraints:
472+
const_lb.append(lb)
473+
const_ub.append(ub)
474+
const_lb = np.array(const_lb).flatten()
475+
const_ub = np.array(const_ub).flatten()
463476

464477
# Store the constraint as a nonlinear constraint
465-
constraints = [
466-
sp.optimize.NonlinearConstraint(traj_cost, lb, ub)]
478+
minimize_constraints = [sp.optimize.NonlinearConstraint(
479+
traj_const, const_lb, const_ub)]
467480

468481
# Add initial and terminal constraints
469-
constraints += [sp.optimize.LinearConstraint(M, Z, Z)]
482+
minimize_constraints += [sp.optimize.LinearConstraint(M, Z, Z)]
470483

471484
# Process the initial condition
472485
if initial_guess is None:
473486
initial_guess = np.zeros(basis.N * sys.ninputs)
474487
else:
475-
raise NotImplementedError("initial_guess not yet available")
488+
raise NotImplementedError("Initial guess not yet implemented.")
476489

477490
# Find the optimal solution
478491
res = sp.optimize.minimize(
479-
traj_cost, initial_guess, constraints=constraints,
492+
traj_cost, initial_guess, constraints=minimize_constraints,
480493
**minimize_kwargs)
481494
if res.success:
482495
alpha = res.x

‎control/iosys.py‎

Lines changed: 13 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1423,13 +1423,13 @@ def input_output_response(
14231423
14241424
Parameters
14251425
----------
1426-
sys: InputOutputSystem
1426+
sys : InputOutputSystem
14271427
Input/output system to simulate.
1428-
T: array-like
1428+
T : array-like
14291429
Time steps at which the input is defined; values must be evenly spaced.
1430-
U: array-like or number, optional
1430+
U : array-like or number, optional
14311431
Input array giving input at each time `T` (default = 0).
1432-
X0: array-like or number, optional
1432+
X0 : array-like or number, optional
14331433
Initial condition (default = 0).
14341434
return_x : bool, optional
14351435
If True, return the values of the state at each time (default = False).
@@ -1451,21 +1451,21 @@ def input_output_response(
14511451
xout : array
14521452
Time evolution of the state vector (if return_x=True).
14531453
1454-
Raises
1455-
------
1456-
TypeError
1457-
If the system is not an input/output system.
1458-
ValueError
1459-
If time step does not match sampling time (for discrete time systems)
1460-
1461-
Additional parameters
1462-
---------------------
1454+
Other parameters
1455+
----------------
14631456
solve_ivp_method : str, optional
14641457
Set the method used by :func:`scipy.integrate.solve_ivp`. Defaults
14651458
to 'RK45'.
14661459
solve_ivp_kwargs : str, optional
14671460
Pass additional keywords to :func:`scipy.integrate.solve_ivp`.
14681461
1462+
Raises
1463+
------
1464+
TypeError
1465+
If the system is not an input/output system.
1466+
ValueError
1467+
If time step does not match sampling time (for discrete time systems).
1468+
14691469
"""
14701470
#
14711471
# Process keyword arguments

‎control/optimal.py‎

Lines changed: 11 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -170,8 +170,7 @@ def __init__(
170170

171171
# Go through each time point and stack the bounds
172172
for t in self.timepts:
173-
for constraint in self.trajectory_constraints:
174-
type, fun, lb, ub = constraint
173+
for type, fun, lb, ub in self.trajectory_constraints:
175174
if np.all(lb == ub):
176175
# Equality constraint
177176
eqconst_value.append(lb)
@@ -181,8 +180,7 @@ def __init__(
181180
constraint_ub.append(ub)
182181

183182
# Add on the terminal constraints
184-
for constraint in self.terminal_constraints:
185-
type, fun, lb, ub = constraint
183+
for type, fun, lb, ub in self.terminal_constraints:
186184
if np.all(lb == ub):
187185
# Equality constraint
188186
eqconst_value.append(lb)
@@ -320,19 +318,19 @@ def _cost_function(self, coeffs):
320318
# constraints, which each take inputs [x, u] and evaluate the
321319
# constraint. How we handle these depends on the type of constraint:
322320
#
323-
# * For linear constraints (LinearConstraint), a combined vector of the
324-
# state and input is multiplied by the polytope A matrix for
325-
# comparison against the upper and lower bounds.
321+
# * For linear constraints (LinearConstraint), a combined (hstack'd)
322+
# vector of the state and input is multiplied by the polytope A matrix
323+
# for comparison against the upper and lower bounds.
326324
#
327325
# * For nonlinear constraints (NonlinearConstraint), a user-specific
328326
# constraint function having the form
329327
#
330-
# constraint_fun(x, u) TODO: convert from [x, u] to (x, u)
328+
# constraint_fun(x, u)
331329
#
332330
# is called at each point along the trajectory and compared against the
333331
# upper and lower bounds.
334332
#
335-
# * If the upper and lower bound for the constraint is identical, then we
333+
# * If the upper and lower bound for the constraint are identical, then we
336334
# separate out the evaluation into two different constraints, which
337335
# allows the SciPy optimizers to be more efficient (and stops them from
338336
# generating a warning about mixed constraints). This is handled
@@ -393,8 +391,7 @@ def _constraint_function(self, coeffs):
393391
# Evaluate the constraint function along the trajectory
394392
value = []
395393
for i, t in enumerate(self.timepts):
396-
for constraint in self.trajectory_constraints:
397-
type, fun, lb, ub = constraint
394+
for type, fun, lb, ub in self.trajectory_constraints:
398395
if np.all(lb == ub):
399396
# Skip equality constraints
400397
continue
@@ -409,8 +406,7 @@ def _constraint_function(self, coeffs):
409406
constraint[0])
410407

411408
# Evaluate the terminal constraint functions
412-
for constraint in self.terminal_constraints:
413-
type, fun, lb, ub = constraint
409+
for type, fun, lb, ub in self.terminal_constraints:
414410
if np.all(lb == ub):
415411
# Skip equality constraints
416412
continue
@@ -483,8 +479,7 @@ def _eqconst_function(self, coeffs):
483479
# Evaluate the constraint function along the trajectory
484480
value = []
485481
for i, t in enumerate(self.timepts):
486-
for constraint in self.trajectory_constraints:
487-
type, fun, lb, ub = constraint
482+
for type, fun, lb, ub in self.trajectory_constraints:
488483
if np.any(lb != ub):
489484
# Skip inequality constraints
490485
continue
@@ -499,8 +494,7 @@ def _eqconst_function(self, coeffs):
499494
constraint[0])
500495

501496
# Evaluate the terminal constraint functions
502-
for constraint in self.terminal_constraints:
503-
type, fun, lb, ub = constraint
497+
for type, fun, lb, ub in self.terminal_constraints:
504498
if np.any(lb != ub):
505499
# Skip inequality constraints
506500
continue

0 commit comments

Comments
 (0)