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

Commitc49ee90

Browse files
committed
updated benchmarks + performance tweaks
1 parent5838c2f commitc49ee90

File tree

4 files changed

+79
-54
lines changed

4 files changed

+79
-54
lines changed

‎benchmarks/optimal_bench.py

Lines changed: 52 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -15,21 +15,7 @@
1515
importtime
1616
importos
1717

18-
#
1918
# Vehicle steering dynamics
20-
#
21-
# The vehicle dynamics are given by a simple bicycle model. We take the state
22-
# of the system as (x, y, theta) where (x, y) is the position of the vehicle
23-
# in the plane and theta is the angle of the vehicle with respect to
24-
# horizontal. The vehicle input is given by (v, phi) where v is the forward
25-
# velocity of the vehicle and phi is the angle of the steering wheel. The
26-
# model includes saturation of the vehicle steering angle.
27-
#
28-
# System state: x, y, theta
29-
# System input: v, phi
30-
# System output: x, y
31-
# System parameters: wheelbase, maxsteer
32-
#
3319
defvehicle_update(t,x,u,params):
3420
# Get the parameters for the model
3521
l=params.get('wheelbase',3.)# vehicle wheelbase
@@ -45,14 +31,14 @@ def vehicle_update(t, x, u, params):
4531
(u[0]/l)*math.tan(phi)# thdot = v/l tan(phi)
4632
])
4733

48-
4934
defvehicle_output(t,x,u,params):
5035
returnx# return x, y, theta (full state)
5136

5237
vehicle=ct.NonlinearIOSystem(
5338
vehicle_update,vehicle_output,states=3,name='vehicle',
5439
inputs=('v','phi'),outputs=('x','y','theta'))
5540

41+
# Initial and final conditions
5642
x0= [0.,-2.,0.];u0= [10.,0.]
5743
xf= [100.,2.,0.];uf= [10.,0.]
5844
Tf=10
@@ -63,7 +49,7 @@ def vehicle_output(t, x, u, params):
6349
# Provide an intial guess (will be extended to entire horizon)
6450
bend_left= [10,0.01]# slight left veer
6551

66-
deftime_integrated_cost():
52+
deftime_steering_integrated_cost():
6753
# Set up the cost functions
6854
Q=np.diag([.1,10,.1])# keep lateral error low
6955
R=np.diag([.1,1])# minimize applied inputs
@@ -81,7 +67,7 @@ def time_integrated_cost():
8167
# Only count this as a benchmark if we converged
8268
assertres.success
8369

84-
deftime_terminal_cost():
70+
deftime_steering_terminal_cost():
8571
# Define cost and constraints
8672
traj_cost=opt.quadratic_cost(
8773
vehicle,None,np.diag([0.1,1]),u0=uf)
@@ -93,14 +79,35 @@ def time_terminal_cost():
9379
res=opt.solve_ocp(
9480
vehicle,horizon,x0,traj_cost,constraints,
9581
terminal_cost=term_cost,initial_guess=bend_left,print_summary=False,
96-
# solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2},
97-
minimize_method='SLSQP',minimize_options={'eps':0.01}
82+
solve_ivp_kwargs={'atol':1e-4,'rtol':1e-2},
83+
# minimize_method='SLSQP', minimize_options={'eps': 0.01}
84+
minimize_method='trust-constr',
85+
minimize_options={'finite_diff_rel_step':0.01},
9886
)
99-
10087
# Only count this as a benchmark if we converged
10188
assertres.success
10289

103-
deftime_terminal_constraint():
90+
# Define integrator and minimizer methods and options/keywords
91+
integrator_table= {
92+
'RK23_default': ('RK23', {'atol':1e-4,'rtol':1e-2}),
93+
'RK23_sloppy': ('RK23', {}),
94+
'RK45_default': ('RK45', {}),
95+
'RK45_sloppy': ('RK45', {'atol':1e-4,'rtol':1e-2}),
96+
}
97+
98+
minimizer_table= {
99+
'trust_default': ('trust-constr', {}),
100+
'trust_bigstep': ('trust-constr', {'finite_diff_rel_step':0.01}),
101+
'SLSQP_default': ('SLSQP', {}),
102+
'SLSQP_bigstep': ('SLSQP', {'eps':0.01}),
103+
}
104+
105+
106+
deftime_steering_terminal_constraint(integrator_name,minimizer_name):
107+
# Get the integrator and minimizer parameters to use
108+
integrator=integrator_table[integrator_name]
109+
minimizer=minimizer_table[minimizer_name]
110+
104111
# Input cost and terminal constraints
105112
R=np.diag([1,1])# minimize applied inputs
106113
cost=opt.quadratic_cost(vehicle,np.zeros((3,3)),R,u0=uf)
@@ -111,58 +118,59 @@ def time_terminal_constraint():
111118
res=opt.solve_ocp(
112119
vehicle,horizon,x0,cost,constraints,
113120
terminal_constraints=terminal,initial_guess=bend_left,log=False,
114-
solve_ivp_kwargs={'atol':1e-3,'rtol':1e-2},
115-
# solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2},
116-
minimize_method='trust-constr',
117-
# minimize_method='SLSQP', minimize_options={'eps': 0.01}
121+
solve_ivp_method=integrator[0],solve_ivp_kwargs=integrator[1],
122+
minimize_method=minimizer[0],minimize_options=minimizer[1],
118123
)
119-
120124
# Only count this as a benchmark if we converged
121125
assertres.success
122126

123127
# Reset the timeout value to allow for longer runs
124-
time_terminal_constraint.timeout=120
128+
time_steering_terminal_constraint.timeout=120
129+
130+
# Parameterize the test against different choices of integrator and minimizer
131+
time_steering_terminal_constraint.param_names= ['integrator','minimizer']
132+
time_steering_terminal_constraint.params= (
133+
['RK23_default','RK23_sloppy','RK45_default','RK45_sloppy'],
134+
['trust_default','trust_bigstep','SLSQP_default','SLSQP_bigstep']
135+
)
125136

126-
deftime_optimal_basis_vehicle():
137+
deftime_steering_bezier_basis(nbasis,ntimes):
127138
# Set up costs and constriants
128139
Q=np.diag([.1,10,.1])# keep lateral error low
129140
R=np.diag([1,1])# minimize applied inputs
130141
cost=opt.quadratic_cost(vehicle,Q,R,x0=xf,u0=uf)
131142
constraints= [opt.input_range_constraint(vehicle, [0,-0.1], [20,0.1]) ]
132143
terminal= [opt.state_range_constraint(vehicle,xf,xf) ]
133-
bend_left= [10,0.05]# slight left veer
134-
near_optimal= [
135-
[1.15073736e+01,1.16838616e+01,1.15413395e+01,
136-
1.11585544e+01,1.06142537e+01,9.98718468e+00,
137-
9.35609454e+00,8.79973057e+00,8.39684004e+00,
138-
8.22617023e+00],
139-
[-9.99830506e-02,8.98139594e-03,5.26385615e-02,
140-
4.96635954e-02,1.87316470e-02,-2.14821345e-02,
141-
-5.23025996e-02,-5.50545990e-02,-1.10629834e-02,
142-
9.83473965e-02] ]
143144

144145
# Set up horizon
145-
horizon=np.linspace(0,Tf,10,endpoint=True)
146+
horizon=np.linspace(0,Tf,ntimes,endpoint=True)
146147

147148
# Set up the optimal control problem
148149
res=opt.solve_ocp(
149150
vehicle,horizon,x0,cost,
150151
constraints,
151152
terminal_constraints=terminal,
152-
initial_guess=near_optimal,
153-
basis=flat.BezierFamily(4,T=Tf),
153+
initial_guess=bend_left,
154+
basis=flat.BezierFamily(nbasis,T=Tf),
155+
# solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2},
154156
minimize_method='trust-constr',
157+
minimize_options={'finite_diff_rel_step':0.01},
155158
# minimize_method='SLSQP', minimize_options={'eps': 0.01},
156-
solve_ivp_kwargs={'atol':1e-4,'rtol':1e-2},
157159
return_states=True,print_summary=False
158160
)
159161
t,u,x=res.time,res.inputs,res.states
160162

161163
# Make sure we found a valid solution
162164
assertres.success
163-
np.testing.assert_almost_equal(x[:,-1],xf,decimal=4)
164165

165-
deftime_mpc_iosystem():
166+
# Reset the timeout value to allow for longer runs
167+
time_steering_bezier_basis.timeout=120
168+
169+
# Set the parameter values for the number of times and basis vectors
170+
time_steering_bezier_basis.param_names= ['nbasis','ntimes']
171+
time_steering_bezier_basis.params= ([2,4,6], [5,10,20])
172+
173+
deftime_aircraft_mpc():
166174
# model of an aircraft discretized with 0.2s sampling time
167175
# Source: https://www.mpt3.org/UI/RegulationProblem
168176
A= [[0.99,0.01,0.18,-0.09,0],

‎control/flatsys/bezier.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -15,16 +15,16 @@
1515
#
1616
# 1. Redistributions of source code must retain the above copyright
1717
# notice, this list of conditions and the following disclaimer.
18-
#
18+
#
1919
# 2. Redistributions in binary form must reproduce the above copyright
2020
# notice, this list of conditions and the following disclaimer in the
2121
# documentation and/or other materials provided with the distribution.
22-
#
22+
#
2323
# 3. Neither the name of the California Institute of Technology nor
2424
# the names of its contributors may be used to endorse or promote
2525
# products derived from this software without specific prior
2626
# written permission.
27-
#
27+
#
2828
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
2929
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
3030
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
@@ -48,7 +48,7 @@ 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} (T - t)^{n-i} t^i
5252
5353
"""
5454
def__init__(self,N,T=1):

‎control/iosys.py

Lines changed: 22 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1522,9 +1522,27 @@ def input_output_response(
15221522
# Update the parameter values
15231523
sys._update_params(params)
15241524

1525+
#
1526+
# Define a function to evaluate the input at an arbitrary time
1527+
#
1528+
# This is equivalent to the function
1529+
#
1530+
# ufun = sp.interpolate.interp1d(T, U, fill_value='extrapolate')
1531+
#
1532+
# but has a lot less overhead => simulation runs much faster
1533+
defufun(t):
1534+
# Find the value of the index using linear interpolation
1535+
idx=np.searchsorted(T,t,side='left')
1536+
ifidx==0:
1537+
# For consistency in return type, multiple by a float
1538+
returnU[...,0]*1.
1539+
else:
1540+
dt= (t-T[idx-1])/ (T[idx]-T[idx-1])
1541+
returnU[...,idx-1]* (1.-dt)+U[...,idx]*dt
1542+
15251543
# Create a lambda function for the right hand side
1526-
u=sp.interpolate.interp1d(T,U,fill_value="extrapolate")
1527-
defivp_rhs(t,x):returnsys._rhs(t,x,u(t))
1544+
defivp_rhs(t,x):
1545+
returnsys._rhs(t,x,ufun(t))
15281546

15291547
# Perform the simulation
15301548
ifisctime(sys):
@@ -1574,10 +1592,10 @@ def ivp_rhs(t, x): return sys._rhs(t, x, u(t))
15741592
foriinrange(len(T)):
15751593
# Store the current state and output
15761594
soln.y.append(x)
1577-
y.append(sys._out(T[i],x,u(T[i])))
1595+
y.append(sys._out(T[i],x,ufun(T[i])))
15781596

15791597
# Update the state for the next iteration
1580-
x=sys._rhs(T[i],x,u(T[i]))
1598+
x=sys._rhs(T[i],x,ufun(T[i]))
15811599

15821600
# Convert output to numpy arrays
15831601
soln.y=np.transpose(np.array(soln.y))

‎examples/steering-optimal.py

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -145,8 +145,7 @@ def plot_results(t, y, u, figure=None, yf=None):
145145
# inputs). Instead, we can penalize the final state and impose a higher
146146
# cost on the inputs, resuling in a more graduate lane change.
147147
#
148-
# We also set the solver explicitly (its actually the default one, but shows
149-
# how to do this).
148+
# We also set the solver explicitly.
150149
#
151150
print("Approach 2: input cost and constraints plus terminal cost")
152151

0 commit comments

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