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

Commitcfe21de

Browse files
authored
Merge pull request#712 from murrayrm/optimal_18Mar2022
Optimal control enhancements
2 parents2f3ca18 +f3d46bc commitcfe21de

File tree

12 files changed

+321
-86
lines changed

12 files changed

+321
-86
lines changed

‎control/config.py‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -103,6 +103,9 @@ def reset_defaults():
103103
from .iosysimport_iosys_defaults
104104
defaults.update(_iosys_defaults)
105105

106+
from .optimalimport_optimal_defaults
107+
defaults.update(_optimal_defaults)
108+
106109

107110
def_get_param(module,param,argval=None,defval=None,pop=False,last=False):
108111
"""Return the default value for a configuration option.

‎control/iosys.py‎

Lines changed: 80 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -1571,7 +1571,7 @@ def __init__(self, io_sys, ss_sys=None):
15711571
definput_output_response(
15721572
sys,T,U=0.,X0=0,params={},
15731573
transpose=False,return_x=False,squeeze=None,
1574-
solve_ivp_kwargs={},**kwargs):
1574+
solve_ivp_kwargs={},t_eval='T',**kwargs):
15751575
"""Compute the output response of a system to a given input.
15761576
15771577
Simulate a dynamical system with a given input and return its output
@@ -1654,50 +1654,57 @@ def input_output_response(
16541654
ifkwargs.get('solve_ivp_method',None):
16551655
ifkwargs.get('method',None):
16561656
raiseValueError("ivp_method specified more than once")
1657-
solve_ivp_kwargs['method']=kwargs['solve_ivp_method']
1657+
solve_ivp_kwargs['method']=kwargs.pop('solve_ivp_method')
16581658

16591659
# Set the default method to 'RK45'
16601660
ifsolve_ivp_kwargs.get('method',None)isNone:
16611661
solve_ivp_kwargs['method']='RK45'
16621662

1663+
# Make sure all input arguments got parsed
1664+
ifkwargs:
1665+
raiseTypeError("unknown parameters %s"%kwargs)
1666+
16631667
# Sanity checking on the input
16641668
ifnotisinstance(sys,InputOutputSystem):
16651669
raiseTypeError("System of type ",type(sys)," not valid")
16661670

16671671
# Compute the time interval and number of steps
16681672
T0,Tf=T[0],T[-1]
1669-
n_steps=len(T)
1673+
ntimepts=len(T)
1674+
1675+
# Figure out simulation times (t_eval)
1676+
ifsolve_ivp_kwargs.get('t_eval'):
1677+
ift_eval=='T':
1678+
# Override the default with the solve_ivp keyword
1679+
t_eval=solve_ivp_kwargs.pop('t_eval')
1680+
else:
1681+
raiseValueError("t_eval specified more than once")
1682+
ifisinstance(t_eval,str)andt_eval=='T':
1683+
# Use the input time points as the output time points
1684+
t_eval=T
16701685

16711686
# Check and convert the input, if needed
16721687
# TODO: improve MIMO ninputs check (choose from U)
16731688
ifsys.ninputsisNoneorsys.ninputs==1:
1674-
legal_shapes= [(n_steps,), (1,n_steps)]
1689+
legal_shapes= [(ntimepts,), (1,ntimepts)]
16751690
else:
1676-
legal_shapes= [(sys.ninputs,n_steps)]
1691+
legal_shapes= [(sys.ninputs,ntimepts)]
16771692
U=_check_convert_array(U,legal_shapes,
16781693
'Parameter ``U``: ',squeeze=False)
1679-
U=U.reshape(-1,n_steps)
1680-
1681-
# Check to make sure this is not a static function
1682-
nstates=_find_size(sys.nstates,X0)
1683-
ifnstates==0:
1684-
# No states => map input to output
1685-
u=U[0]iflen(U.shape)==1elseU[:,0]
1686-
y=np.zeros((np.shape(sys._out(T[0],X0,u))[0],len(T)))
1687-
foriinrange(len(T)):
1688-
u=U[i]iflen(U.shape)==1elseU[:,i]
1689-
y[:,i]=sys._out(T[i], [],u)
1690-
returnTimeResponseData(
1691-
T,y,None,U,issiso=sys.issiso(),
1692-
output_labels=sys.output_index,input_labels=sys.input_index,
1693-
transpose=transpose,return_x=return_x,squeeze=squeeze)
1694+
U=U.reshape(-1,ntimepts)
1695+
ninputs=U.shape[0]
16941696

16951697
# create X0 if not given, test if X0 has correct shape
1698+
nstates=_find_size(sys.nstates,X0)
16961699
X0=_check_convert_array(X0, [(nstates,), (nstates,1)],
16971700
'Parameter ``X0``: ',squeeze=True)
16981701

1699-
# Update the parameter values
1700-
sys._update_params(params)
1702+
# Figure out the number of outputs
1703+
ifsys.noutputsisNone:
1704+
# Evaluate the output function to find number of outputs
1705+
noutputs=np.shape(sys._out(T[0],X0,U[:,0]))[0]
1706+
else:
1707+
noutputs=sys.noutputs
17011708

17021709
#
17031710
# Define a function to evaluate the input at an arbitrary time
@@ -1714,6 +1721,31 @@ def ufun(t):
17141721
dt= (t-T[idx-1])/ (T[idx]-T[idx-1])
17151722
returnU[...,idx-1]* (1.-dt)+U[...,idx]*dt
17161723

1724+
# Check to make sure this is not a static function
1725+
ifnstates==0:# No states => map input to output
1726+
# Make sure the user gave a time vector for evaluation (or 'T')
1727+
ift_evalisNone:
1728+
# User overrode t_eval with None, but didn't give us the times...
1729+
warn("t_eval set to None, but no dynamics; using T instead")
1730+
t_eval=T
1731+
1732+
# Allocate space for the inputs and outputs
1733+
u=np.zeros((ninputs,len(t_eval)))
1734+
y=np.zeros((noutputs,len(t_eval)))
1735+
1736+
# Compute the input and output at each point in time
1737+
fori,tinenumerate(t_eval):
1738+
u[:,i]=ufun(t)
1739+
y[:,i]=sys._out(t, [],u[:,i])
1740+
1741+
returnTimeResponseData(
1742+
t_eval,y,None,u,issiso=sys.issiso(),
1743+
output_labels=sys.output_index,input_labels=sys.input_index,
1744+
transpose=transpose,return_x=return_x,squeeze=squeeze)
1745+
1746+
# Update the parameter values
1747+
sys._update_params(params)
1748+
17171749
# Create a lambda function for the right hand side
17181750
defivp_rhs(t,x):
17191751
returnsys._rhs(t,x,ufun(t))
@@ -1724,27 +1756,27 @@ def ivp_rhs(t, x):
17241756
raiseNameError("scipy.integrate.solve_ivp not found; "
17251757
"use SciPy 1.0 or greater")
17261758
soln=sp.integrate.solve_ivp(
1727-
ivp_rhs, (T0,Tf),X0,t_eval=T,
1759+
ivp_rhs, (T0,Tf),X0,t_eval=t_eval,
17281760
vectorized=False,**solve_ivp_kwargs)
1761+
ifnotsoln.success:
1762+
raiseRuntimeError("solve_ivp failed: "+soln.message)
17291763

1730-
ifnotsoln.successorsoln.status!=0:
1731-
# Something went wrong
1732-
warn("sp.integrate.solve_ivp failed")
1733-
print("Return bunch:",soln)
1734-
1735-
# Compute the output associated with the state (and use sys.out to
1736-
# figure out the number of outputs just in case it wasn't specified)
1737-
u=U[0]iflen(U.shape)==1elseU[:,0]
1738-
y=np.zeros((np.shape(sys._out(T[0],X0,u))[0],len(T)))
1739-
foriinrange(len(T)):
1740-
u=U[i]iflen(U.shape)==1elseU[:,i]
1741-
y[:,i]=sys._out(T[i],soln.y[:,i],u)
1764+
# Compute inputs and outputs for each time point
1765+
u=np.zeros((ninputs,len(soln.t)))
1766+
y=np.zeros((noutputs,len(soln.t)))
1767+
fori,tinenumerate(soln.t):
1768+
u[:,i]=ufun(t)
1769+
y[:,i]=sys._out(t,soln.y[:,i],u[:,i])
17421770

17431771
elifisdtime(sys):
1772+
# If t_eval was not specified, use the sampling time
1773+
ift_evalisNone:
1774+
t_eval=np.arange(T[0],T[1]+sys.dt,sys.dt)
1775+
17441776
# Make sure the time vector is uniformly spaced
1745-
dt=T[1]-T[0]
1746-
ifnotnp.allclose(T[1:]-T[:-1],dt):
1747-
raiseValueError("Parameter ``T``: time values must be "
1777+
dt=t_eval[1]-t_eval[0]
1778+
ifnotnp.allclose(t_eval[1:]-t_eval[:-1],dt):
1779+
raiseValueError("Parameter ``t_eval``: time values must be "
17481780
"equally spaced.")
17491781

17501782
# Make sure the sample time matches the given time
@@ -1764,21 +1796,23 @@ def ivp_rhs(t, x):
17641796

17651797
# Compute the solution
17661798
soln=sp.optimize.OptimizeResult()
1767-
soln.t=T# Store the time vector directly
1768-
x=X0# Initilize state
1799+
soln.t=t_eval# Store the time vector directly
1800+
x=np.array(X0)# State vector (store as floats)
17691801
soln.y= []# Solution, following scipy convention
1770-
y= []# System output
1771-
foriinrange(len(T)):
1772-
# Store the current state and output
1802+
u,y= [], []# System input, output
1803+
fortint_eval:
1804+
# Store the currentinput,state, and output
17731805
soln.y.append(x)
1774-
y.append(sys._out(T[i],x,ufun(T[i])))
1806+
u.append(ufun(t))
1807+
y.append(sys._out(t,x,u[-1]))
17751808

17761809
# Update the state for the next iteration
1777-
x=sys._rhs(T[i],x,ufun(T[i]))
1810+
x=sys._rhs(t,x,u[-1])
17781811

17791812
# Convert output to numpy arrays
17801813
soln.y=np.transpose(np.array(soln.y))
17811814
y=np.transpose(np.array(y))
1815+
u=np.transpose(np.array(u))
17821816

17831817
# Mark solution as successful
17841818
soln.success=True# No way to fail
@@ -1787,7 +1821,7 @@ def ivp_rhs(t, x):
17871821
raiseTypeError("Can't determine system type")
17881822

17891823
returnTimeResponseData(
1790-
soln.t,y,soln.y,U,issiso=sys.issiso(),
1824+
soln.t,y,soln.y,u,issiso=sys.issiso(),
17911825
output_labels=sys.output_index,input_labels=sys.input_index,
17921826
state_labels=sys.state_index,
17931827
transpose=transpose,return_x=return_x,squeeze=squeeze)

‎control/optimal.py‎

Lines changed: 55 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -16,10 +16,21 @@
1616
importlogging
1717
importtime
1818

19+
from .importconfig
20+
from .exceptionimportControlNotImplemented
1921
from .timerespimportTimeResponseData
2022

2123
__all__= ['find_optimal_input']
2224

25+
# Define module default parameter values
26+
_optimal_defaults= {
27+
'optimal.minimize_method':None,
28+
'optimal.minimize_options': {},
29+
'optimal.minimize_kwargs': {},
30+
'optimal.solve_ivp_method':None,
31+
'optimal.solve_ivp_options': {},
32+
}
33+
2334

2435
classOptimalControlProblem():
2536
"""Description of a finite horizon, optimal control problem.
@@ -110,6 +121,10 @@ class OptimalControlProblem():
110121
values of the input at the specified times (using linear interpolation for
111122
continuous systems).
112123
124+
The default values for ``minimize_method``, ``minimize_options``,
125+
``minimize_kwargs``, ``solve_ivp_method``, and ``solve_ivp_options`` can
126+
be set using config.defaults['optimal.<keyword>'].
127+
113128
"""
114129
def__init__(
115130
self,sys,timepts,integral_cost,trajectory_constraints=[],
@@ -126,17 +141,22 @@ def __init__(
126141

127142
# Process keyword arguments
128143
self.solve_ivp_kwargs= {}
129-
self.solve_ivp_kwargs['method']=kwargs.pop('solve_ivp_method',None)
130-
self.solve_ivp_kwargs.update(kwargs.pop('solve_ivp_kwargs', {}))
144+
self.solve_ivp_kwargs['method']=kwargs.pop(
145+
'solve_ivp_method',config.defaults['optimal.solve_ivp_method'])
146+
self.solve_ivp_kwargs.update(kwargs.pop(
147+
'solve_ivp_kwargs',config.defaults['optimal.solve_ivp_options']))
131148

132149
self.minimize_kwargs= {}
133-
self.minimize_kwargs['method']=kwargs.pop('minimize_method',None)
134-
self.minimize_kwargs['options']=kwargs.pop('minimize_options', {})
135-
self.minimize_kwargs.update(kwargs.pop('minimize_kwargs', {}))
150+
self.minimize_kwargs['method']=kwargs.pop(
151+
'minimize_method',config.defaults['optimal.minimize_method'])
152+
self.minimize_kwargs['options']=kwargs.pop(
153+
'minimize_options',config.defaults['optimal.minimize_options'])
154+
self.minimize_kwargs.update(kwargs.pop(
155+
'minimize_kwargs',config.defaults['optimal.minimize_kwargs']))
136156

137-
iflen(kwargs)>0:
138-
raiseValueError(
139-
f'unrecognized keyword(s):{list(kwargs.keys())}')
157+
# Make sure all input arguments got parsed
158+
ifkwargs:
159+
raiseTypeError("unrecognized keyword(s):",str(kwargs))
140160

141161
# Process trajectory constraints
142162
ifisinstance(trajectory_constraints,tuple):
@@ -271,9 +291,10 @@ def _cost_function(self, coeffs):
271291
logging.debug("initial input[0:3] =\n"+str(inputs[:,0:3]))
272292

273293
# Simulate the system to get the state
294+
# TODO: try calling solve_ivp directly for better speed?
274295
_,_,states=ct.input_output_response(
275296
self.system,self.timepts,inputs,x,return_x=True,
276-
solve_ivp_kwargs=self.solve_ivp_kwargs)
297+
solve_ivp_kwargs=self.solve_ivp_kwargs,t_eval=self.timepts)
277298
self.system_simulations+=1
278299
self.last_x=x
279300
self.last_coeffs=coeffs
@@ -393,7 +414,7 @@ def _constraint_function(self, coeffs):
393414
# Simulate the system to get the state
394415
_,_,states=ct.input_output_response(
395416
self.system,self.timepts,inputs,x,return_x=True,
396-
solve_ivp_kwargs=self.solve_ivp_kwargs)
417+
solve_ivp_kwargs=self.solve_ivp_kwargs,t_eval=self.timepts)
397418
self.system_simulations+=1
398419
self.last_x=x
399420
self.last_coeffs=coeffs
@@ -475,7 +496,7 @@ def _eqconst_function(self, coeffs):
475496
# Simulate the system to get the state
476497
_,_,states=ct.input_output_response(
477498
self.system,self.timepts,inputs,x,return_x=True,
478-
solve_ivp_kwargs=self.solve_ivp_kwargs)
499+
solve_ivp_kwargs=self.solve_ivp_kwargs,t_eval=self.timepts)
479500
self.system_simulations+=1
480501
self.last_x=x
481502
self.last_coeffs=coeffs
@@ -548,7 +569,7 @@ def _process_initial_guess(self, initial_guess):
548569
initial_guess=np.atleast_1d(initial_guess)
549570

550571
# See whether we got entire guess or just first time point
551-
iflen(initial_guess.shape)==1:
572+
ifinitial_guess.ndim==1:
552573
# Broadcast inputs to entire time vector
553574
try:
554575
initial_guess=np.broadcast_to(
@@ -804,6 +825,15 @@ class OptimalControlResult(sp.optimize.OptimizeResult):
804825
Whether or not the optimizer exited successful.
805826
problem : OptimalControlProblem
806827
Optimal control problem that generated this solution.
828+
cost : float
829+
Final cost of the return solution.
830+
system_simulations, {cost, constraint, eqconst}_evaluations : int
831+
Number of system simulations and evaluations of the cost function,
832+
(inequality) constraint function, and equality constraint function
833+
performed during the optimzation.
834+
{cost, constraint, eqconst}_process_time : float
835+
If logging was enabled, the amount of time spent evaluating the cost
836+
and constraint functions.
807837
808838
"""
809839
def__init__(
@@ -833,15 +863,19 @@ def __init__(
833863
"unable to solve optimal control problem\n"
834864
"scipy.optimize.minimize returned "+res.message,UserWarning)
835865

866+
# Save the final cost
867+
self.cost=res.fun
868+
836869
# Optionally print summary information
837870
ifprint_summary:
838871
ocp._print_statistics()
872+
print("* Final cost:",self.cost)
839873

840874
ifreturn_statesandinputs.shape[1]==ocp.timepts.shape[0]:
841875
# Simulate the system if we need the state back
842876
_,_,states=ct.input_output_response(
843877
ocp.system,ocp.timepts,inputs,ocp.x,return_x=True,
844-
solve_ivp_kwargs=ocp.solve_ivp_kwargs)
878+
solve_ivp_kwargs=ocp.solve_ivp_kwargs,t_eval=ocp.timepts)
845879
ocp.system_simulations+=1
846880
else:
847881
states=None
@@ -858,7 +892,7 @@ def __init__(
858892

859893
# Compute the input for a nonlinear, (constrained) optimal control problem
860894
defsolve_ocp(
861-
sys,horizon,X0,cost,trajectory_constraints=[],terminal_cost=None,
895+
sys,horizon,X0,cost,trajectory_constraints=None,terminal_cost=None,
862896
terminal_constraints=[],initial_guess=None,basis=None,squeeze=None,
863897
transpose=None,return_states=False,log=False,**kwargs):
864898

@@ -960,12 +994,18 @@ def solve_ocp(
960994
# Process keyword arguments
961995
iftrajectory_constraintsisNone:
962996
# Backwards compatibility
963-
trajectory_constraints=kwargs.pop('constraints',None)
997+
trajectory_constraints=kwargs.pop('constraints',[])
964998

965999
# Allow 'return_x` as a synonym for 'return_states'
9661000
return_states=ct.config._get_param(
9671001
'optimal','return_x',kwargs,return_states,pop=True)
9681002

1003+
# Process (legacy) method keyword
1004+
ifkwargs.get('method'):
1005+
ifkwargs.get('minimize_method'):
1006+
raiseValueError("'minimize_method' specified more than once")
1007+
kwargs['minimize_method']=kwargs.pop('method')
1008+
9691009
# Set up the optimal control problem
9701010
ocp=OptimalControlProblem(
9711011
sys,horizon,cost,trajectory_constraints=trajectory_constraints,

‎control/statefbk.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -734,7 +734,7 @@ def dlqr(*args, **kwargs):
734734
The dlqr() function computes the optimal state feedback controller
735735
u[n] = - K x[n] that minimizes the quadratic cost
736736
737-
.. math:: J =\\Sum_0^\\infty (x[n]' Q x[n] + u[n]' R u[n] + 2 x[n]' N u[n])
737+
.. math:: J =\\sum_0^\\infty (x[n]' Q x[n] + u[n]' R u[n] + 2 x[n]' N u[n])
738738
739739
The function can be called with either 3, 4, or 5 arguments:
740740

0 commit comments

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