Movatterモバイル変換

[0]ホーム
URL:

Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly

 Sign up
Appearance settings

Commitd121102

Browse files
committed
updated examples + code cleanup
1 parent8256fa0 commitd121102

File tree

5 files changed

+190
-146
lines changed

5 files changed

+190
-146
lines changed

‎control/optimal.py

Lines changed: 76 additions & 72 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,6 @@
2323
# Define module default parameter values
2424
_optimal_trajectory_methods= {'shooting','collocation'}
2525
_optimal_defaults= {
26-
'optimal.trajectory_method':'shooting',
2726
'optimal.minimize_method':None,
2827
'optimal.minimize_options': {},
2928
'optimal.minimize_kwargs': {},
@@ -147,9 +146,8 @@ def __init__(
147146
self.terminal_constraints=terminal_constraints
148147
self.basis=basis
149148

150-
# Keep track of what type oftrajector method we are using
149+
# Keep track of what type oftrajectory method we are using
151150
iftrajectory_methodisNone:
152-
# TODO: change default
153151
trajectory_method='collocation'ifsys.isctime()else'shooting'
154152
eliftrajectory_methodnotin_optimal_trajectory_methods:
155153
raiseNotImplementedError(f"Unkown method{method}")
@@ -185,30 +183,23 @@ def __init__(
185183
raiseTypeError("unrecognized keyword(s): ",str(kwargs))
186184

187185
# Process trajectory constraints
188-
ifisinstance(trajectory_constraints,tuple):
189-
self.trajectory_constraints= [trajectory_constraints]
190-
elifnotisinstance(trajectory_constraints,list):
191-
raiseTypeError("trajectory constraints must be a list")
192-
else:
193-
self.trajectory_constraints=trajectory_constraints
186+
def_process_constraints(constraint_list,name):
187+
ifisinstance(constraint_list,tuple):
188+
constraint_list= [constraint_list]
189+
elifnotisinstance(constraint_list,list):
190+
raiseTypeError(f"{name} constraints must be a list")
194191

195-
# Make sure that we recognize all of the constraint types
196-
forctype,fun,lb,ubinself.trajectory_constraints:
197-
ifnotctypein [opt.LinearConstraint,opt.NonlinearConstraint]:
198-
raiseTypeError(f"unknown constraint type{ctype}")
192+
# Make sure that we recognize all of the constraint types
193+
forctype,fun,lb,ubinconstraint_list:
194+
ifnotctypein [opt.LinearConstraint,opt.NonlinearConstraint]:
195+
raiseTypeError(f"unknown{name} constraint type{ctype}")
199196

200-
# Process terminal constraints
201-
ifisinstance(terminal_constraints,tuple):
202-
self.terminal_constraints= [terminal_constraints]
203-
elifnotisinstance(terminal_constraints,list):
204-
raiseTypeError("terminal constraints must be a list")
205-
else:
206-
self.terminal_constraints=terminal_constraints
197+
returnconstraint_list
207198

208-
# Make sure that we recognize all of the constraint types
209-
forctype,fun,lb,ubinself.terminal_constraints:
210-
ifnotctypein [opt.LinearConstraint,opt.NonlinearConstraint]:
211-
raiseTypeError(f"unknown constraint type{ctype}")
199+
self.trajectory_constraints=_process_constraints(
200+
trajectory_constraints,"trajectory")
201+
self.terminal_constraints=_process_constraints(
202+
terminal_constraints,"terminal")
212203

213204
#
214205
# Compute and store constraints
@@ -223,10 +214,13 @@ def __init__(
223214
# is consistent with the `_constraint_function` that is used at
224215
# evaluation time.
225216
#
217+
# TODO: when using the collocation method, linear constraints on the
218+
# states and inputs can potentially maintain their linear structure
219+
# rather than being converted to nonlinear constraints.
220+
#
226221
constraint_lb,constraint_ub,eqconst_value= [], [], []
227222

228223
# Go through each time point and stack the bounds
229-
# TODO: for collocation method, keep track of linear vs nonlinear
230224
fortinself.timepts:
231225
fortype,fun,lb,ubinself.trajectory_constraints:
232226
ifnp.all(lb==ub):
@@ -253,15 +247,19 @@ def __init__(
253247
self.eqconst_value=np.hstack(eqconst_value)ifeqconst_valueelse []
254248

255249
# Create the constraints (inequality and equality)
250+
# TODO: for collocation method, keep track of linear vs nonlinear
256251
self.constraints= []
252+
257253
iflen(self.constraint_lb)!=0:
258254
self.constraints.append(sp.optimize.NonlinearConstraint(
259255
self._constraint_function,self.constraint_lb,
260256
self.constraint_ub))
257+
261258
iflen(self.eqconst_value)!=0:
262259
self.constraints.append(sp.optimize.NonlinearConstraint(
263260
self._eqconst_function,self.eqconst_value,
264261
self.eqconst_value))
262+
265263
ifself.collocation:
266264
# Add the collocation constraints
267265
colloc_zeros=np.zeros(sys.nstates*self.timepts.size)
@@ -275,7 +273,7 @@ def __init__(
275273
# Process the initial guess
276274
self.initial_guess=self._process_initial_guess(initial_guess)
277275

278-
# Store states, input,used later to minimize re-computation
276+
# Store states, input (used later to minimize re-computation)
279277
self.last_x=np.full(self.system.nstates,np.nan)
280278
self.last_coeffs=np.full(self.initial_guess.shape,np.nan)
281279

@@ -286,10 +284,14 @@ def __init__(
286284
#
287285
# Cost function
288286
#
289-
# Given the input U = [u[0], ... u[N]], we need to compute the cost of
290-
# the trajectory generated by that input. This means we have to
291-
# simulate the system to get the state trajectory X = [x[0], ...,
292-
# x[N]] and then compute the cost at each point:
287+
# For collocation methods we are given the states and inputs at each
288+
# time point and we use a trapezoidal approximation to compute the
289+
# integral cost, then add on the terminal cost.
290+
#
291+
# For shooting methods, given the input U = [u[0], ... u[N]] we need to
292+
# compute the cost of the trajectory generated by that input. This
293+
# means we have to simulate the system to get the state trajectory X =
294+
# [x[0], ..., x[N]] and then compute the cost at each point:
293295
#
294296
# cost = sum_k integral_cost(x[k], u[k]) + terminal_cost(x[N], u[N])
295297
#
@@ -362,11 +364,12 @@ def _cost_function(self, coeffs):
362364
# is called at each point along the trajectory and compared against the
363365
# upper and lower bounds.
364366
#
365-
# * If the upper and lower bound for the constraint are identical, then we
366-
# separate out the evaluation into two different constraints, which
367-
# allows the SciPy optimizers to be more efficient (and stops them from
368-
# generating a warning about mixed constraints). This is handled
369-
# through the use of the `_eqconst_function` and `eqconst_value` members.
367+
# * If the upper and lower bound for the constraint are identical, then
368+
# we separate out the evaluation into two different constraints, which
369+
# allows the SciPy optimizers to be more efficient (and stops them
370+
# from generating a warning about mixed constraints). This is handled
371+
# through the use of the `_eqconst_function` and `eqconst_value`
372+
# members.
370373
#
371374
# In both cases, the constraint is specified at a single point, but we
372375
# extend this to apply to each point in the trajectory. This means
@@ -378,16 +381,12 @@ def _cost_function(self, coeffs):
378381
# For collocation methods, we can directly evaluate the constraints at
379382
# the collocation points.
380383
#
381-
# For shooting methods, we do this by creating a function that
382-
# simulates the system dynamics and returns a vector of values
383-
# corresponding to the value of the function at each time. The
384-
# class initialization methods takes care of replicating the upper
385-
# and lower bounds for each point in time so that the SciPy
386-
# optimization algorithm can do the proper evaluation.
387-
#
388-
# In addition, since SciPy's optimization function does not allow us to
389-
# pass arguments to the constraint function, we have to store the initial
390-
# state prior to optimization and retrieve it here.
384+
# For shooting methods, we do this by creating a function that simulates
385+
# the system dynamics and returns a vector of values corresponding to
386+
# the value of the function at each time. The class initialization
387+
# methods takes care of replicating the upper and lower bounds for each
388+
# point in time so that the SciPy optimization algorithm can do the
389+
# proper evaluation.
391390
#
392391
def_constraint_function(self,coeffs):
393392
ifself.log:
@@ -437,8 +436,7 @@ def _constraint_function(self, coeffs):
437436
"_constraint_function elapsed time: %g",
438437
stop_time-start_time)
439438

440-
# Debugging information
441-
ifself.log:
439+
# Debugging information
442440
logging.debug(
443441
"constraint values\n"+str(value)+"\n"+
444442
"lb, ub =\n"+str(self.constraint_lb)+"\n"+
@@ -492,8 +490,7 @@ def _eqconst_function(self, coeffs):
492490
logging.info(
493491
"_eqconst_function elapsed time: %g",stop_time-start_time)
494492

495-
# Debugging information
496-
ifself.log:
493+
# Debugging information
497494
logging.debug(
498495
"eqconst values\n"+str(value)+"\n"+
499496
"desired =\n"+str(self.eqconst_value))
@@ -515,7 +512,7 @@ def _collocation_constraint(self, coeffs):
515512
self.timepts[0],states[:,0],inputs[:,0])
516513
continue
517514

518-
# M. Kelly, SIAM Review (2017), equation (3.2), i = k+1
515+
#FromM. Kelly, SIAM Review (2017), equation (3.2), i = k+1
519516
# x[k+1] - x[k] = 0.5 hk (f(x[k+1], u[k+1] + f(x[k], u[k]))
520517
fkp1=self.system._rhs(t,states[:,i],inputs[:,i])
521518
self.colloc_vals[:,i]=states[:,i]-states[:,i-1]- \
@@ -529,18 +526,20 @@ def _collocation_constraint(self, coeffs):
529526
returnself.colloc_vals.reshape(-1)
530527

531528
#
532-
# Initial guess
529+
# Initial guess processing
533530
#
534531
# We store an initial guess in case it is not specified later. Note
535532
# that create_mpc_iosystem() will reset the initial guess based on
536533
# the current state of the MPC controller.
537534
#
538-
# Note: the initial guess is passed as the inputs at the given time
535+
# The functions below are used to process the initial guess, which can
536+
# either consist of an input only (for shooting methods) or an input
537+
# and/or state trajectory (for collocaiton methods).
538+
#
539+
# Note: The initial input guess is passed as the inputs at the given time
539540
# vector. If a basis is specified, this is converted to coefficient
540541
# values (which are generally of smaller dimension).
541542
#
542-
# TODO: figure out how to modify this for collocation
543-
#
544543
def_process_initial_guess(self,initial_guess):
545544
# Sort out the input guess and the state guess
546545
ifself.collocationandinitial_guessisnotNoneand \
@@ -583,6 +582,7 @@ def _process_initial_guess(self, initial_guess):
583582
# Reshape for use by scipy.optimize.minimize()
584583
returninput_guess.reshape(-1)
585584

585+
# Utility function to broadcast an initial guess to the right shape
586586
def_broadcast_initial_guess(self,initial_guess,shape):
587587
# Convert to a 1D array (or higher)
588588
initial_guess=np.atleast_1d(initial_guess)
@@ -604,11 +604,11 @@ def _broadcast_initial_guess(self, initial_guess, shape):
604604
#
605605
# Utility function to convert input vector to coefficient vector
606606
#
607-
#Initially guesses from the user are passed as input vectors as a
607+
#Initial guesses from the user are passed as input vectors as a
608608
# function of time, but internally we store the guess in terms of the
609609
# basis coefficients. We do this by solving a least squares problem to
610-
# find coefficients that match the input functions at the time points (as
611-
# much as possible, if the problem is under-determined).
610+
# find coefficients that match the input functions at the time points
611+
#(asmuch as possible, if the problem is under-determined).
612612
#
613613
def_inputs_to_coeffs(self,inputs):
614614
# If there is no basis function, just return inputs as coeffs
@@ -638,6 +638,7 @@ def _inputs_to_coeffs(self, inputs):
638638
# Utility function to convert coefficient vector to input vector
639639
def_coeffs_to_inputs(self,coeffs):
640640
# TODO: vectorize
641+
# TODO: use BasisFamily eval() method (if more efficient)?
641642
inputs=np.zeros((self.system.ninputs,self.timepts.size))
642643
offset=0
643644
foriinrange(self.system.ninputs):
@@ -689,7 +690,7 @@ def _print_statistics(self, reset=True):
689690
# These internal functions return the states and inputs at the
690691
# collocation points given the ceofficient (optimizer state) vector.
691692
# They keep track of whether a shooting method is being used or not and
692-
# simulate thedynamis of needed.
693+
# simulate thedynamics if needed.
693694
#
694695

695696
# Compute the states and inputs from the coefficients
@@ -738,6 +739,7 @@ def _simulate_states(self, x0, inputs):
738739
ifself.log:
739740
logging.debug(
740741
"input_output_response returned states\n"+str(states))
742+
741743
returnstates
742744

743745
#
@@ -930,16 +932,6 @@ def __init__(
930932
ocp._print_statistics()
931933
print("* Final cost:",self.cost)
932934

933-
ifreturn_statesandinputs.shape[1]==ocp.timepts.shape[0]:
934-
# Simulate the system if we need the state back
935-
# TODO: this is probably not needed due to compute_states_inputs()
936-
_,_,states=ct.input_output_response(
937-
ocp.system,ocp.timepts,inputs,ocp.x,return_x=True,
938-
solve_ivp_kwargs=ocp.solve_ivp_kwargs,t_eval=ocp.timepts)
939-
ocp.system_simulations+=1
940-
else:
941-
states=None
942-
943935
# Process data as a time response (with "outputs" = inputs)
944936
response=TimeResponseData(
945937
ocp.timepts,inputs,states,issiso=ocp.system.issiso(),
@@ -1065,10 +1057,22 @@ def solve_ocp(
10651057
'optimal','return_x',kwargs,return_states,pop=True)
10661058

10671059
# Process (legacy) method keyword
1068-
ifkwargs.get('method')andmethodnotinoptimal_methods:
1069-
ifkwargs.get('minimize_method'):
1070-
raiseValueError("'minimize_method' specified more than once")
1071-
kwargs['minimize_method']=kwargs.pop('method')
1060+
ifkwargs.get('method'):
1061+
method=kwargs.pop('method')
1062+
ifmethodnotinoptimal_methods:
1063+
ifkwargs.get('minimize_method'):
1064+
raiseValueError("'minimize_method' specified more than once")
1065+
warnings.warn(
1066+
"'method' parameter is deprecated; assuming minimize_method",
1067+
DeprecationWarning)
1068+
kwargs['minimize_method']=method
1069+
else:
1070+
ifkwargs.get('trajectory_method'):
1071+
raiseValueError("'trajectory_method' specified more than once")
1072+
warnings.warn(
1073+
"'method' parameter is deprecated; assuming trajectory_method",
1074+
DeprecationWarning)
1075+
kwargs['trajectory_method']=method
10721076

10731077
# Set up the optimal control problem
10741078
ocp=OptimalControlProblem(

0 commit comments

Comments
 (0)
[8]ページ先頭
©2009-2025 Movatter.jp