Skip to content

Commit ea2884d

Browse files
murrayrmmurrayrm
committed
slight refactoring of cost functions + example tweaks
1 parent 769eaa5 commit ea2884d

2 files changed

Lines changed: 123 additions & 54 deletions

File tree

‎control/obc.py‎

Lines changed: 44 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,7 @@ class OptimalControlProblem():
5555
def __init__(
5656
self, sys, time_vector, integral_cost, trajectory_constraints=[],
5757
terminal_cost=None, terminal_constraints=[], initial_guess=None,
58-
log=False, options={}):
58+
log=False, **kwargs):
5959
"""Set up an optimal control problem
6060
6161
To describe an optimal control problem we need an input/output system,
@@ -90,8 +90,8 @@ def __init__(
9090
extension of the time axis.
9191
log : bool, optional
9292
If `True`, turn on logging messages (using Python logging module).
93-
options : dict, optional
94-
Solver options (passed to :func:`scipy.optimal.minimize`).
93+
kwargs : dict, optional
94+
Additional parameters (passed to :func:`scipy.optimal.minimize`).
9595
9696
Returns
9797
-------
@@ -107,7 +107,7 @@ def __init__(
107107
self.trajectory_constraints = trajectory_constraints
108108
self.terminal_cost = terminal_cost
109109
self.terminal_constraints = terminal_constraints
110-
self.options = options
110+
self.kwargs = kwargs
111111

112112
#
113113
# Compute and store constraints
@@ -251,7 +251,15 @@ def _cost_function(self, inputs):
251251
# TODO: vectorize
252252
cost = 0
253253
for i, t in enumerate(self.time_vector):
254-
cost += self.integral_cost(states[:,i], inputs[:,i])
254+
if ct.isctime(self.system):
255+
# Approximate the integral using trapezoidal rule
256+
if i > 0:
257+
cost += 0.5 * (
258+
self.integral_cost(states[:, i-1], inputs[:, i-1]) +
259+
self.integral_cost(states[:, i], inputs[:, i])) * (
260+
self.time_vector[i] - self.time_vector[i-1])
261+
else:
262+
cost += self.integral_cost(states[:,i], inputs[:,i])
255263

256264
# Terminal cost
257265
if self.terminal_cost is not None:
@@ -573,7 +581,7 @@ def compute_trajectory(
573581
# Call ScipPy optimizer
574582
res = sp.optimize.minimize(
575583
self._cost_function, self.initial_guess,
576-
constraints=self.constraints, options=self.options)
584+
constraints=self.constraints, **self.kwargs)
577585

578586
# Process and return the results
579587
return OptimalControlResult(
@@ -676,7 +684,7 @@ def __init__(
676684
def compute_optimal_input(
677685
sys, horizon, X0, cost, constraints=[], terminal_cost=None,
678686
terminal_constraints=[], initial_guess=None, squeeze=None,
679-
transpose=None, return_x=None, log=False, options={}):
687+
transpose=None, return_x=None, log=False, **kwargs):
680688

681689
"""Compute the solution to an optimal control problem
682690
@@ -743,8 +751,8 @@ def compute_optimal_input(
743751
If True, assume that 2D input arrays are transposed from the standard
744752
format. Used to convert MATLAB-style inputs to our format.
745753
746-
options : dict, optional
747-
Solver options (passed to :func:`scipy.optimal.minimize`).
754+
kwargs : dict, optional
755+
Additional parameters (passed to :func:`scipy.optimal.minimize`).
748756
749757
Returns
750758
-------
@@ -763,7 +771,7 @@ def compute_optimal_input(
763771
ocp = OptimalControlProblem(
764772
sys, horizon, cost, trajectory_constraints=constraints,
765773
terminal_cost=terminal_cost, terminal_constraints=terminal_constraints,
766-
initial_guess=initial_guess, log=log, options=options)
774+
initial_guess=initial_guess, log=log, **kwargs)
767775

768776
# Solve for the optimal input from the current state
769777
return ocp.compute_trajectory(
@@ -773,7 +781,7 @@ def compute_optimal_input(
773781
# Create a model predictive controller for an optimal control problem
774782
def create_mpc_iosystem(
775783
sys, horizon, cost, constraints=[], terminal_cost=None,
776-
terminal_constraints=[], dt=True, log=False, options={}):
784+
terminal_constraints=[], dt=True, log=False, **kwargs):
777785
"""Create a model predictive I/O control system
778786
779787
This function creates an input/output system that implements a model
@@ -805,8 +813,8 @@ def create_mpc_iosystem(
805813
List of constraints that should hold at the end of the trajectory.
806814
Same format as `constraints`.
807815
808-
options : dict, optional
809-
Solver options (passed to :func:`scipy.optimal.minimize`).
816+
kwargs : dict, optional
817+
Additional parameters (passed to :func:`scipy.optimal.minimize`).
810818
811819
Returns
812820
-------
@@ -821,7 +829,7 @@ def create_mpc_iosystem(
821829
ocp = OptimalControlProblem(
822830
sys, horizon, cost, trajectory_constraints=constraints,
823831
terminal_cost=terminal_cost, terminal_constraints=terminal_constraints,
824-
log=log, options=options)
832+
log=log, **kwargs)
825833

826834
# Return an I/O system implementing the model predictive controller
827835
return ocp._create_mpc_iosystem(dt=dt)
@@ -863,8 +871,28 @@ def quadratic_cost(sys, Q, R, x0=0, u0=0):
863871
input. The call signature of the function is cost_fun(x, u).
864872
865873
"""
866-
Q = np.atleast_2d(Q)
867-
R = np.atleast_2d(R)
874+
# Process the input arguments
875+
if Q is not None:
876+
Q = np.atleast_2d(Q)
877+
if Q.size == 1: # allow scalar weights
878+
Q = np.eye(sys.nstates) * Q.item()
879+
elif Q.shape != (sys.nstates, sys.nstates):
880+
raise ValueError("Q matrix is the wrong shape")
881+
882+
if R is not None:
883+
R = np.atleast_2d(R)
884+
if R.size == 1: # allow scalar weights
885+
R = np.eye(sys.ninputs) * R.item()
886+
elif R.shape != (sys.ninputs, sys.ninputs):
887+
raise ValueError("R matrix is the wrong shape")
888+
889+
if Q is None:
890+
return lambda x, u: ((u-u0) @ R @ (u-u0)).item()
891+
892+
if R is None:
893+
return lambda x, u: ((x-x0) @ Q @ (x-x0)).item()
894+
895+
# Received both Q and R matrices
868896
return lambda x, u: ((x-x0) @ Q @ (x-x0) + (u-u0) @ R @ (u-u0)).item()
869897

870898

‎examples/steering-optimal.py‎

Lines changed: 79 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -92,60 +92,101 @@ def plot_results(t, y, u, figure=None, yf=None):
9292
xf = [100., 2., 0.]; uf = [10., 0.]
9393
Tf = 10
9494

95-
# Set up the cost functions
96-
Q = np.diag([0.1, 1, 0.1]) # keep lateral error low
97-
R = np.eye(2) # minimize applied inputs
98-
cost = obc.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
99-
10095
#
101-
# Set up different types of constraints to demonstrate
96+
# Approach 1: standard quadratic cost
97+
#
98+
# We can set up the optimal control problem as trying to minimize the
99+
# distance form the desired final point while at the same time as not
100+
# exerting too much control effort to achieve our goal.
101+
#
102+
# Note: depending on what version of SciPy you are using, you might get a
103+
# warning message about precision loss, but the solution is pretty good.
102104
#
103105

104-
# Input constraints
105-
constraints = [ obc.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
106-
107-
# Terminal constraints (optional)
108-
terminal = [ obc.state_range_constraint(vehicle, xf, xf) ]
106+
# Set up the cost functions
107+
Q = np.diag([1, 10, 1]) # keep lateral error low
108+
R = np.diag([1, 1]) # minimize applied inputs
109+
cost1 = obc.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
109110

110-
# Time horizon and possible initial guessses
111+
# Define the time horizon (and spacing) for the optimization
111112
horizon = np.linspace(0, Tf, 10, endpoint=True)
112-
straight = [10, 0] # straight trajectory
113-
bend_left = [10, 0.01] # slight left veer
114113

115-
#
116-
# Solve the optimal control problem in dififerent ways
117-
#
114+
# Provide an intial guess (will be extended to entire horizon)
115+
bend_left = [10, 0.01] # slight left veer
118116

119-
# Basic setup: quadratic cost, no terminal constraint, straight initial path
117+
# Turn on debug level logging so that we can see what the optimizer is doing
120118
logging.basicConfig(
121-
level=logging.DEBUG, filename="steering-straight.log",
119+
level=logging.DEBUG, filename="steering-integral_cost.log",
122120
filemode='w', force=True)
123-
result = obc.compute_optimal_input(
124-
vehicle, horizon, x0, cost, initial_guess=straight,
125-
log=True, options={'eps': 0.01})
126-
t1, u1 = result.time, result.inputs
121+
122+
# Compute the optimal control, setting step size for gradient calculation (eps)
123+
result1 = obc.compute_optimal_input(
124+
vehicle, horizon, x0, cost1, initial_guess=bend_left, log=True,
125+
options={'eps': 0.01})
126+
127+
# Extract and plot the results (+ state trajectory)
128+
t1, u1 = result1.time, result1.inputs
127129
t1, y1 = ct.input_output_response(vehicle, horizon, u1, x0)
128130
plot_results(t1, y1, u1, figure=1, yf=xf[0:2])
129131

130-
# Add constraint on the input to avoid high steering angles
132+
#
133+
# Approach 2: input cost, input constraints, terminal cost
134+
#
135+
# The previous solution integrates the position error for the entire
136+
# horizon, and so the car changes lanes very quickly (at the cost of larger
137+
# inputs). Instead, we can penalize the final state and impose a higher
138+
# cost on the inputs, resuling in a more graduate lane change.
139+
#
140+
# We also set the solver explicitly (its actually the default one, but shows
141+
# how to do this).
142+
#
143+
144+
# Add input constraint, input cost, terminal cost
145+
constraints = [ obc.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
146+
traj_cost = obc.quadratic_cost(vehicle, None, np.diag([0.1, 1]), u0=uf)
147+
term_cost = obc.quadratic_cost(vehicle, np.diag([1, 10, 10]), None, x0=xf)
148+
149+
# Change logging to keep less information
131150
logging.basicConfig(
132-
level=logging.INFO, filename="./steering-bendleft.log",
151+
level=logging.INFO, filename="./steering-terminal_cost.log",
133152
filemode='w', force=True)
134-
result = obc.compute_optimal_input(
135-
vehicle, horizon, x0, cost, constraints, initial_guess=bend_left,
136-
log=True, options={'eps': 0.01})
137-
t2, u2 = result.time, result.inputs
153+
154+
# Compute the optimal control
155+
result2 = obc.compute_optimal_input(
156+
vehicle, horizon, x0, traj_cost, constraints, terminal_cost=term_cost,
157+
initial_guess=bend_left, log=True,
158+
method='SLSQP', options={'eps': 0.01})
159+
160+
# Extract and plot the results (+ state trajectory)
161+
t2, u2 = result2.time, result2.inputs
138162
t2, y2 = ct.input_output_response(vehicle, horizon, u2, x0)
139163
plot_results(t2, y2, u2, figure=2, yf=xf[0:2])
140164

141-
# Resolve with a terminal constraint (starting with previous result)
142-
logging.basicConfig(
143-
level=logging.WARN, filename="./steering-terminal.log",
144-
filemode='w', force=True)
145-
result = obc.compute_optimal_input(
146-
vehicle, horizon, x0, cost, constraints,
147-
terminal_constraints=terminal, initial_guess=u2,
148-
log=True, options={'eps': 0.01})
149-
t3, u3 = result.time, result.inputs
165+
#
166+
# Approach 3: terminal constraints and new solver
167+
#
168+
# As a final example, we can remove the cost function on the state and
169+
# replace it with a terminal *constraint* on the state. If a solution is
170+
# found, it guarantees we get to exactly the final state.
171+
#
172+
# To speeds things up a bit, we initalize the problem using the previous
173+
# optimal controller (which didn't quite hit the final value).
174+
#
175+
176+
# Input cost and terminal constraints
177+
cost3 = obc.quadratic_cost(vehicle, np.zeros((3,3)), R, u0=uf)
178+
terminal = [ obc.state_range_constraint(vehicle, xf, xf) ]
179+
180+
# Reset logging to its default values
181+
logging.basicConfig(level=logging.WARN, force=True)
182+
183+
# Compute the optimal control
184+
result3 = obc.compute_optimal_input(
185+
vehicle, horizon, x0, cost3, constraints,
186+
terminal_constraints=terminal, initial_guess=u2, log=True,
187+
options={'eps': 0.01})
188+
189+
# Extract and plot the results (+ state trajectory)
190+
t3, u3 = result3.time, result3.inputs
150191
t3, y3 = ct.input_output_response(vehicle, horizon, u3, x0)
151192
plot_results(t3, y3, u3, figure=3, yf=xf[0:2])

0 commit comments

Comments
 (0)