Skip to content

Commit 329ae47

Browse files
murrayrmmurrayrm
committed
set default trajectory_methood for ctime systems to collocation
1 parent 704fdec commit 329ae47

3 files changed

Lines changed: 36 additions & 22 deletions

File tree

‎control/optimal.py‎

Lines changed: 10 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -147,11 +147,10 @@ def __init__(
147147
self.terminal_constraints = terminal_constraints
148148
self.basis = basis
149149

150-
# Keep track of what type of method we are using
150+
# Keep track of what type of trajector method we are using
151151
if trajectory_method is None:
152152
# TODO: change default
153-
# trajectory_method = 'collocation' if sys.isctime() else 'shooting'
154-
trajectory_method = 'shooting' if sys.isctime() else 'shooting'
153+
trajectory_method = 'collocation' if sys.isctime() else 'shooting'
155154
elif trajectory_method not in _optimal_trajectory_methods:
156155
raise NotImplementedError(f"Unkown method {method}")
157156

@@ -414,9 +413,9 @@ def _constraint_function(self, coeffs):
414413
# Skip equality constraints
415414
continue
416415
elif ctype == opt.LinearConstraint:
417-
value.append(fun @ np.hstack([states[:, i], inputs[:, i]]))
416+
value.append(fun @ np.hstack([states[:, -1], inputs[:, -1]]))
418417
elif ctype == opt.NonlinearConstraint:
419-
value.append(fun(states[:, i], inputs[:, i]))
418+
value.append(fun(states[:, -1], inputs[:, -1]))
420419
else: # pragma: no cover
421420
# Checked above => we should never get here
422421
raise TypeError(f"unknown constraint type {ctype}")
@@ -470,9 +469,9 @@ def _eqconst_function(self, coeffs):
470469
# Skip inequality constraints
471470
continue
472471
elif ctype == opt.LinearConstraint:
473-
value.append(fun @ np.hstack([states[:, i], inputs[:, i]]))
472+
value.append(fun @ np.hstack([states[:, -1], inputs[:, -1]]))
474473
elif ctype == opt.NonlinearConstraint:
475-
value.append(fun(states[:, i], inputs[:, i]))
474+
value.append(fun(states[:, -1], inputs[:, -1]))
476475
else: # pragma: no cover
477476
# Checked above => we should never get here
478477
raise TypeError("unknown constraint type {ctype}")
@@ -559,7 +558,10 @@ def _process_initial_guess(self, initial_guess):
559558
if self.collocation:
560559
if state_guess is None:
561560
# Run a simulation to get the initial guess
562-
inputs = input_guess.reshape(self.system.ninputs, -1)
561+
if self.basis:
562+
inputs = self._coeffs_to_inputs(input_guess)
563+
else:
564+
inputs = input_guess.reshape(self.system.ninputs, -1)
563565
state_guess = self._simulate_states(
564566
np.zeros(self.system.nstates), inputs)
565567
else:

‎control/tests/optimal_test.py‎

Lines changed: 13 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -367,7 +367,7 @@ def test_terminal_constraints(sys_args):
367367

368368
# Not all configurations are able to converge (?)
369369
if res.success:
370-
np.testing.assert_almost_equal(x2[:,-1], 0)
370+
np.testing.assert_almost_equal(x2[:,-1], 0, decimal=5)
371371

372372
# Make sure that it is *not* a straight line path
373373
assert np.any(np.abs(x2 - x1) > 0.1)
@@ -610,12 +610,16 @@ def final_point_eval(x, u):
610610
"method, npts, initial_guess", [
611611
# ('shooting', 3, None), # doesn't converge
612612
# ('shooting', 3, 'zero'), # doesn't converge
613-
('shooting', 3, 'input'), # github issue #782
614-
('shooting', 5, 'input'),
615-
# ('collocation', 5, 'zero'), # doesn't converge
613+
('shooting', 3, 'u0'), # github issue #782
614+
# ('shooting', 3, 'input'), # precision loss
615+
# ('shooting', 5, 'input'), # precision loss
616+
# ('collocation', 3, 'u0'), # doesn't converge
617+
('collocation', 5, 'u0'), # from documenentation
616618
('collocation', 5, 'input'),
617619
('collocation', 10, 'input'),
620+
('collocation', 10, 'u0'),
618621
('collocation', 10, 'state'),
622+
('collocation', 20, 'state'),
619623
])
620624
def test_optimal_doc(method, npts, initial_guess):
621625
"""Test optimal control problem from documentation"""
@@ -662,6 +666,9 @@ def vehicle_output(t, x, u, params):
662666
if initial_guess == 'zero':
663667
initial_guess = 0
664668

669+
elif initial_guess == 'u0':
670+
initial_guess = u0
671+
665672
elif initial_guess == 'input':
666673
# Velocity = constant that gets us from start to end
667674
initial_guess = np.zeros((vehicle.ninputs, timepts.size))
@@ -701,6 +708,6 @@ def vehicle_output(t, x, u, params):
701708
assert abs((y[1, 0] - x0[1]) / x0[1]) < 0.01
702709
assert abs(y[2, 0]) < 0.01
703710

704-
assert abs((y[0, -1] - xf[0]) / xf[0]) < 0.2 # TODO: reset to 0.1
705-
assert abs((y[1, -1] - xf[1]) / xf[1]) < 0.2 # TODO: reset to 0.1
711+
assert abs((y[0, -1] - xf[0]) / xf[0]) < 0.12
712+
assert abs((y[1, -1] - xf[1]) / xf[1]) < 0.12
706713
assert abs(y[2, -1]) < 0.1

‎doc/optimal.rst‎

Lines changed: 13 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -112,15 +112,15 @@ problem into a standard optimization problem that can be solved by
112112
:func:`scipy.optimize.minimize`. The optimal control problem can be solved
113113
by using the :func:`~control.obc.solve_ocp` function::
114114

115-
res = obc.solve_ocp(sys, horizon, X0, cost, constraints)
115+
res = obc.solve_ocp(sys, timepts, X0, cost, constraints)
116116

117117
The `sys` parameter should be an :class:`~control.InputOutputSystem` and the
118-
`horizon` parameter should represent a time vector that gives the list of
118+
`timepts` parameter should represent a time vector that gives the list of
119119
times at which the cost and constraints should be evaluated.
120120

121121
The `cost` function has call signature `cost(t, x, u)` and should return the
122122
(incremental) cost at the given time, state, and input. It will be
123-
evaluated at each point in the `horizon` vector. The `terminal_cost`
123+
evaluated at each point in the `timepts` vector. The `terminal_cost`
124124
parameter can be used to specify a cost function for the final point in the
125125
trajectory.
126126

@@ -157,7 +157,7 @@ that has the following elements:
157157
* `res.success`: `True` if the optimization was successfully solved
158158
* `res.inputs`: optimal input
159159
* `res.states`: state trajectory (if `return_x` was `True`)
160-
* `res.time`: copy of the time horizon vector
160+
* `res.time`: copy of the time timepts vector
161161

162162
In addition, the results from :func:`scipy.optimize.minimize` are also
163163
available.
@@ -235,16 +235,16 @@ and constrain the velocity to be in the range of 9 m/s to 11 m/s::
235235

236236
Finally, we solve for the optimal inputs::
237237

238-
horizon = np.linspace(0, Tf, 3, endpoint=True)
238+
timepts = np.linspace(0, Tf, 10, endpoint=True)
239239
result = opt.solve_ocp(
240-
vehicle, horizon, x0, traj_cost, constraints,
240+
vehicle, timepts, x0, traj_cost, constraints,
241241
terminal_cost=term_cost, initial_guess=u0)
242242

243243
Plotting the results::
244244

245245
# Simulate the system dynamics (open loop)
246246
resp = ct.input_output_response(
247-
vehicle, horizon, result.inputs, x0,
247+
vehicle, timepts, result.inputs, x0,
248248
t_eval=np.linspace(0, Tf, 100))
249249
t, y, u = resp.time, resp.outputs, resp.inputs
250250

@@ -262,7 +262,7 @@ Plotting the results::
262262

263263
plt.subplot(3, 1, 3)
264264
plt.plot(t, u[1])
265-
plt.axis([0, 10, -0.01, 0.01])
265+
plt.axis([0, 10, -0.015, 0.015])
266266
plt.xlabel("t [sec]")
267267
plt.ylabel("u2 [rad/s]")
268268

@@ -282,6 +282,11 @@ toolbox and it can sometimes be tricky to get the optimization to converge.
282282
If you are getting errors when solving optimal control problems or your
283283
solutions do not seem close to optimal, here are a few things to try:
284284

285+
* The initial guess matters: providing a reasonable initial guess is often
286+
needed in order for the optimizer to find a good answer. For an optimal
287+
control problem that uses a larger terminal cost to get to a neighborhood
288+
of a final point, a straight line in the state space often works well.
289+
285290
* Less is more: try using a smaller number of time points in your
286291
optimiation. The default optimal control problem formulation uses the
287292
value of the inputs at each time point as a free variable and this can

0 commit comments

Comments
 (0)