20
20
21
21
__all__ = ['find_optimal_input' ]
22
22
23
+
23
24
class OptimalControlProblem ():
24
25
"""Description of a finite horizon, optimal control problem
25
26
@@ -124,7 +125,6 @@ def __init__(
124
125
else :
125
126
self .terminal_constraints = terminal_constraints
126
127
127
-
128
128
#
129
129
# Compute and store constraints
130
130
#
@@ -197,7 +197,7 @@ def __init__(
197
197
initial_guess = np .broadcast_to (
198
198
initial_guess .reshape (- 1 , 1 ),
199
199
(self .system .ninputs , self .time_vector .size ))
200
- except :
200
+ except ValueError :
201
201
raise ValueError ("initial guess is the wrong shape" )
202
202
203
203
elif initial_guess .shape != \
@@ -222,7 +222,6 @@ def __init__(
222
222
if log :
223
223
logging .info ("New optimal control problem initailized" )
224
224
225
-
226
225
#
227
226
# Cost function
228
227
#
@@ -253,7 +252,7 @@ def _cost_function(self, inputs):
253
252
else :
254
253
if self .log :
255
254
logging .debug ("calling input_output_response from state\n "
256
- + str (x ))
255
+ + str (x ))
257
256
logging .debug ("initial input[0:3] =\n " + str (inputs [:, 0 :3 ]))
258
257
259
258
# Simulate the system to get the state
@@ -266,7 +265,7 @@ def _cost_function(self, inputs):
266
265
267
266
if self .log :
268
267
logging .debug ("input_output_response returned states\n "
269
- + str (states ))
268
+ + str (states ))
270
269
271
270
# Trajectory cost
272
271
# TODO: vectorize
@@ -293,7 +292,7 @@ def _cost_function(self, inputs):
293
292
294
293
# Terminal cost
295
294
if self .terminal_cost is not None :
296
- cost += self .terminal_cost (states [:,- 1 ], inputs [:,- 1 ])
295
+ cost += self .terminal_cost (states [:, - 1 ], inputs [:, - 1 ])
297
296
298
297
# Update statistics
299
298
self .cost_evaluations += 1
@@ -307,7 +306,6 @@ def _cost_function(self, inputs):
307
306
# Return the total cost for this input sequence
308
307
return cost
309
308
310
-
311
309
#
312
310
# Constraints
313
311
#
@@ -368,7 +366,7 @@ def _constraint_function(self, inputs):
368
366
else :
369
367
if self .log :
370
368
logging .debug ("calling input_output_response from state\n "
371
- + str (x ))
369
+ + str (x ))
372
370
logging .debug ("initial input[0:3] =\n " + str (inputs [:, 0 :3 ]))
373
371
374
372
# Simulate the system to get the state
@@ -390,9 +388,9 @@ def _constraint_function(self, inputs):
390
388
elif type == opt .LinearConstraint :
391
389
# `fun` is the A matrix associated with the polytope...
392
390
value .append (
393
- np .dot (fun , np .hstack ([states [:,i ], inputs [:,i ]])))
391
+ np .dot (fun , np .hstack ([states [:, i ], inputs [:, i ]])))
394
392
elif type == opt .NonlinearConstraint :
395
- value .append (fun (states [:,i ], inputs [:,i ]))
393
+ value .append (fun (states [:, i ], inputs [:, i ]))
396
394
else :
397
395
raise TypeError ("unknown constraint type %s" %
398
396
constraint [0 ])
@@ -405,9 +403,9 @@ def _constraint_function(self, inputs):
405
403
continue
406
404
elif type == opt .LinearConstraint :
407
405
value .append (
408
- np .dot (fun , np .hstack ([states [:,i ], inputs [:,i ]])))
406
+ np .dot (fun , np .hstack ([states [:, i ], inputs [:, i ]])))
409
407
elif type == opt .NonlinearConstraint :
410
- value .append (fun (states [:,i ], inputs [:,i ]))
408
+ value .append (fun (states [:, i ], inputs [:, i ]))
411
409
else :
412
410
raise TypeError ("unknown constraint type %s" %
413
411
constraint [0 ])
@@ -448,7 +446,7 @@ def _eqconst_function(self, inputs):
448
446
else :
449
447
if self .log :
450
448
logging .debug ("calling input_output_response from state\n "
451
- + str (x ))
449
+ + str (x ))
452
450
logging .debug ("initial input[0:3] =\n " + str (inputs [:, 0 :3 ]))
453
451
454
452
# Simulate the system to get the state
@@ -461,7 +459,7 @@ def _eqconst_function(self, inputs):
461
459
462
460
if self .log :
463
461
logging .debug ("input_output_response returned states\n "
464
- + str (states ))
462
+ + str (states ))
465
463
466
464
# Evaluate the constraint function along the trajectory
467
465
value = []
@@ -474,9 +472,9 @@ def _eqconst_function(self, inputs):
474
472
elif type == opt .LinearConstraint :
475
473
# `fun` is the A matrix associated with the polytope...
476
474
value .append (
477
- np .dot (fun , np .hstack ([states [:,i ], inputs [:,i ]])))
475
+ np .dot (fun , np .hstack ([states [:, i ], inputs [:, i ]])))
478
476
elif type == opt .NonlinearConstraint :
479
- value .append (fun (states [:,i ], inputs [:,i ]))
477
+ value .append (fun (states [:, i ], inputs [:, i ]))
480
478
else :
481
479
raise TypeError ("unknown constraint type %s" %
482
480
constraint [0 ])
@@ -489,9 +487,9 @@ def _eqconst_function(self, inputs):
489
487
continue
490
488
elif type == opt .LinearConstraint :
491
489
value .append (
492
- np .dot (fun , np .hstack ([states [:,i ], inputs [:,i ]])))
490
+ np .dot (fun , np .hstack ([states [:, i ], inputs [:, i ]])))
493
491
elif type == opt .NonlinearConstraint :
494
- value .append (fun (states [:,i ], inputs [:,i ]))
492
+ value .append (fun (states [:, i ], inputs [:, i ]))
495
493
else :
496
494
raise TypeError ("unknown constraint type %s" %
497
495
constraint [0 ])
@@ -523,7 +521,7 @@ def _eqconst_function(self, inputs):
523
521
#
524
522
def _reset_statistics (self , log = False ):
525
523
"""Reset counters for keeping track of statistics"""
526
- self .log = log
524
+ self .log = log
527
525
self .cost_evaluations , self .cost_process_time = 0 , 0
528
526
self .constraint_evaluations , self .constraint_process_time = 0 , 0
529
527
self .eqconst_evaluations , self .eqconst_process_time = 0 , 0
@@ -555,13 +553,13 @@ def _create_mpc_iosystem(self, dt=True):
555
553
def _update (t , x , u , params = {}):
556
554
inputs = x .reshape ((self .system .ninputs , self .time_vector .size ))
557
555
self .initial_guess = np .hstack (
558
- [inputs [:,1 :], inputs [:,- 1 :]]).reshape (- 1 )
556
+ [inputs [:, 1 :], inputs [:, - 1 :]]).reshape (- 1 )
559
557
res = self .compute_trajectory (u , print_summary = False )
560
558
return res .inputs .reshape (- 1 )
561
559
562
560
def _output (t , x , u , params = {}):
563
561
inputs = x .reshape ((self .system .ninputs , self .time_vector .size ))
564
- return inputs [:,0 ]
562
+ return inputs [:, 0 ]
565
563
566
564
return ct .NonlinearIOSystem (
567
565
_update , _output , dt = dt ,
0 commit comments