2020
2121__all__ = ['find_optimal_input' ]
2222
23+
2324class OptimalControlProblem ():
2425 """Description of a finite horizon, optimal control problem
2526
@@ -124,7 +125,6 @@ def __init__(
124125 else :
125126 self .terminal_constraints = terminal_constraints
126127
127-
128128 #
129129 # Compute and store constraints
130130 #
@@ -197,7 +197,7 @@ def __init__(
197197 initial_guess = np .broadcast_to (
198198 initial_guess .reshape (- 1 , 1 ),
199199 (self .system .ninputs , self .time_vector .size ))
200- except :
200+ except ValueError :
201201 raise ValueError ("initial guess is the wrong shape" )
202202
203203 elif initial_guess .shape != \
@@ -222,7 +222,6 @@ def __init__(
222222 if log :
223223 logging .info ("New optimal control problem initailized" )
224224
225-
226225 #
227226 # Cost function
228227 #
@@ -253,7 +252,7 @@ def _cost_function(self, inputs):
253252 else :
254253 if self .log :
255254 logging .debug ("calling input_output_response from state\n "
256- + str (x ))
255+ + str (x ))
257256 logging .debug ("initial input[0:3] =\n " + str (inputs [:, 0 :3 ]))
258257
259258 # Simulate the system to get the state
@@ -266,7 +265,7 @@ def _cost_function(self, inputs):
266265
267266 if self .log :
268267 logging .debug ("input_output_response returned states\n "
269- + str (states ))
268+ + str (states ))
270269
271270 # Trajectory cost
272271 # TODO: vectorize
@@ -293,7 +292,7 @@ def _cost_function(self, inputs):
293292
294293 # Terminal cost
295294 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 ])
297296
298297 # Update statistics
299298 self .cost_evaluations += 1
@@ -307,7 +306,6 @@ def _cost_function(self, inputs):
307306 # Return the total cost for this input sequence
308307 return cost
309308
310-
311309 #
312310 # Constraints
313311 #
@@ -368,7 +366,7 @@ def _constraint_function(self, inputs):
368366 else :
369367 if self .log :
370368 logging .debug ("calling input_output_response from state\n "
371- + str (x ))
369+ + str (x ))
372370 logging .debug ("initial input[0:3] =\n " + str (inputs [:, 0 :3 ]))
373371
374372 # Simulate the system to get the state
@@ -390,9 +388,9 @@ def _constraint_function(self, inputs):
390388 elif type == opt .LinearConstraint :
391389 # `fun` is the A matrix associated with the polytope...
392390 value .append (
393- np .dot (fun , np .hstack ([states [:,i ], inputs [:,i ]])))
391+ np .dot (fun , np .hstack ([states [:, i ], inputs [:, i ]])))
394392 elif type == opt .NonlinearConstraint :
395- value .append (fun (states [:,i ], inputs [:,i ]))
393+ value .append (fun (states [:, i ], inputs [:, i ]))
396394 else :
397395 raise TypeError ("unknown constraint type %s" %
398396 constraint [0 ])
@@ -405,9 +403,9 @@ def _constraint_function(self, inputs):
405403 continue
406404 elif type == opt .LinearConstraint :
407405 value .append (
408- np .dot (fun , np .hstack ([states [:,i ], inputs [:,i ]])))
406+ np .dot (fun , np .hstack ([states [:, i ], inputs [:, i ]])))
409407 elif type == opt .NonlinearConstraint :
410- value .append (fun (states [:,i ], inputs [:,i ]))
408+ value .append (fun (states [:, i ], inputs [:, i ]))
411409 else :
412410 raise TypeError ("unknown constraint type %s" %
413411 constraint [0 ])
@@ -448,7 +446,7 @@ def _eqconst_function(self, inputs):
448446 else :
449447 if self .log :
450448 logging .debug ("calling input_output_response from state\n "
451- + str (x ))
449+ + str (x ))
452450 logging .debug ("initial input[0:3] =\n " + str (inputs [:, 0 :3 ]))
453451
454452 # Simulate the system to get the state
@@ -461,7 +459,7 @@ def _eqconst_function(self, inputs):
461459
462460 if self .log :
463461 logging .debug ("input_output_response returned states\n "
464- + str (states ))
462+ + str (states ))
465463
466464 # Evaluate the constraint function along the trajectory
467465 value = []
@@ -474,9 +472,9 @@ def _eqconst_function(self, inputs):
474472 elif type == opt .LinearConstraint :
475473 # `fun` is the A matrix associated with the polytope...
476474 value .append (
477- np .dot (fun , np .hstack ([states [:,i ], inputs [:,i ]])))
475+ np .dot (fun , np .hstack ([states [:, i ], inputs [:, i ]])))
478476 elif type == opt .NonlinearConstraint :
479- value .append (fun (states [:,i ], inputs [:,i ]))
477+ value .append (fun (states [:, i ], inputs [:, i ]))
480478 else :
481479 raise TypeError ("unknown constraint type %s" %
482480 constraint [0 ])
@@ -489,9 +487,9 @@ def _eqconst_function(self, inputs):
489487 continue
490488 elif type == opt .LinearConstraint :
491489 value .append (
492- np .dot (fun , np .hstack ([states [:,i ], inputs [:,i ]])))
490+ np .dot (fun , np .hstack ([states [:, i ], inputs [:, i ]])))
493491 elif type == opt .NonlinearConstraint :
494- value .append (fun (states [:,i ], inputs [:,i ]))
492+ value .append (fun (states [:, i ], inputs [:, i ]))
495493 else :
496494 raise TypeError ("unknown constraint type %s" %
497495 constraint [0 ])
@@ -523,7 +521,7 @@ def _eqconst_function(self, inputs):
523521 #
524522 def _reset_statistics (self , log = False ):
525523 """Reset counters for keeping track of statistics"""
526- self .log = log
524+ self .log = log
527525 self .cost_evaluations , self .cost_process_time = 0 , 0
528526 self .constraint_evaluations , self .constraint_process_time = 0 , 0
529527 self .eqconst_evaluations , self .eqconst_process_time = 0 , 0
@@ -555,13 +553,13 @@ def _create_mpc_iosystem(self, dt=True):
555553 def _update (t , x , u , params = {}):
556554 inputs = x .reshape ((self .system .ninputs , self .time_vector .size ))
557555 self .initial_guess = np .hstack (
558- [inputs [:,1 :], inputs [:,- 1 :]]).reshape (- 1 )
556+ [inputs [:, 1 :], inputs [:, - 1 :]]).reshape (- 1 )
559557 res = self .compute_trajectory (u , print_summary = False )
560558 return res .inputs .reshape (- 1 )
561559
562560 def _output (t , x , u , params = {}):
563561 inputs = x .reshape ((self .system .ninputs , self .time_vector .size ))
564- return inputs [:,0 ]
562+ return inputs [:, 0 ]
565563
566564 return ct .NonlinearIOSystem (
567565 _update , _output , dt = dt ,
0 commit comments