___________________ test_optimal_doc[shooting-3-None-xfail] ____________________
method = 'shooting', npts = 3, initial_guess = None, fail = 'xfail'
@pytest.mark.slow
@pytest.mark.parametrize(
"method, npts, initial_guess, fail", [
('shooting', 3, None, 'xfail'), # doesn't converge
('shooting', 3, 'zero', 'xfail'), # doesn't converge
# ('shooting', 3, 'u0', None), # github issue #782
('shooting', 3, 'input', 'endpoint'), # doesn't converge to optimal
('shooting', 5, 'input', 'endpoint'), # doesn't converge to optimal
('collocation', 3, 'u0', 'endpoint'), # doesn't converge to optimal
('collocation', 5, 'u0', 'endpoint'),
('collocation', 5, 'input', 'openloop'),# open loop sim fails
('collocation', 10, 'input', None),
('collocation', 10, 'u0', None), # from documentation
('collocation', 10, 'state', None),
('collocation', 20, 'state', None),
])
def test_optimal_doc(method, npts, initial_guess, fail):
"""Test optimal control problem from documentation"""
def vehicle_update(t, x, u, params):
# Get the parameters for the model
l = params.get('wheelbase', 3.) # vehicle wheelbase
phimax = params.get('maxsteer', 0.5) # max steering angle (rad)
# Saturate the steering input
phi = np.clip(u[1], -phimax, phimax)
# Return the derivative of the state
return np.array([
np.cos(x[2]) * u[0], # xdot = cos(theta) v
np.sin(x[2]) * u[0], # ydot = sin(theta) v
(u[0] / l) * np.tan(phi) # thdot = v/l tan(phi)
])
def vehicle_output(t, x, u, params):
return x # return x, y, theta (full state)
# Define the vehicle steering dynamics as an input/output system
vehicle = ct.NonlinearIOSystem(
vehicle_update, vehicle_output, states=3, name='vehicle',
inputs=('v', 'phi'), outputs=('x', 'y', 'theta'))
# Define the initial and final points and time interval
x0 = np.array([0., -2., 0.]); u0 = np.array([10., 0.])
xf = np.array([100., 2., 0.]); uf = np.array([10., 0.])
Tf = 10
# Define the cost functions
Q = np.diag([0, 0, 0.1]) # don't turn too sharply
R = np.diag([1, 1]) # keep inputs small
P = np.diag([1000, 1000, 1000]) # get close to final point
traj_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
term_cost = opt.quadratic_cost(vehicle, P, 0, x0=xf)
# Define the constraints
constraints = [ opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
# Define an initial guess at the trajectory
timepts = np.linspace(0, Tf, npts, endpoint=True)
if initial_guess == 'zero':
initial_guess = 0
elif initial_guess == 'u0':
initial_guess = u0
elif initial_guess == 'input':
# Velocity = constant that gets us from start to end
initial_guess = np.zeros((vehicle.ninputs, timepts.size))
initial_guess[0, :] = (xf[0] - x0[0]) / Tf
# Steering = rate required to turn to proper slope in first segment
approximate_angle = math.atan2(xf[1] - x0[1], xf[0] - x0[0])
initial_guess[1, 0] = approximate_angle / (timepts[1] - timepts[0])
initial_guess[1, -1] = -approximate_angle / (timepts[-1] - timepts[-2])
elif initial_guess == 'state':
input_guess = np.outer(u0, np.ones((1, npts)))
state_guess = np.array([
x0 + (xf - x0) * time/Tf for time in timepts]).transpose()
initial_guess = (state_guess, input_guess)
# Solve the optimal control problem
with warnings.catch_warnings():
warnings.filterwarnings(
'ignore', message="unable to solve", category=UserWarning)
result = opt.solve_optimal_trajectory(
vehicle, timepts, x0, traj_cost, constraints,
terminal_cost=term_cost, initial_guess=initial_guess,
trajectory_method=method,
# minimize_method='COBYLA', # SLSQP',
)
if fail == 'xfail':
> assert not result.success
E assert not True
E + where True = message: Optimization terminated successfully\n success: True\n status: 0\n fun: 2.4136235872518714\n x: [ 9.480e+00 1.025e+01 1.006e+01 3.297e-03 2.840e-03\n -4.371e-03]\n nit: 75\n jac: [-2.905e+01 -5.013e+01 -2.577e+01 1.936e+07 -1.171e+03\n 1.935e+07]\n nfev: 678\n njev: 74\n multipliers: [ 0.000e+00 0.000e+00 0.000e+00 0.000e+00 0.000e+00\n 0.000e+00 0.000e+00 0.000e+00 0.000e+00 0.000e+00\n 0.000e+00 0.000e+00]\n problem: <control.optimal.OptimalControlProblem object at 0x7f4fb7b074d0>\n cost: 2.4136235872518714\n time: [ 0.000e+00 5.000e+00 1.000e+01]\n inputs: [[ 9.480e+00 1.025e+01 1.006e+01]\n [ 3.297e-03 2.840e-03 -4.371e-03]]\n states: [[ 0.000e+00 4.929e+01 9.999e+01]\n [-2.000e+00 -7.256e-01 1.998e+00]\n [ 0.000e+00 5.001e-02 3.711e-02]].success
control/tests/optimal_test.py:756: AssertionError
----------------------------- Captured stdout call -----------------------------
Summary statistics:
* Cost function calls: 678
* Constraint calls: 760
* System simulations: 1204
* Final cost: 2.4136235872518714
___________________ test_optimal_doc[shooting-3-zero-xfail] ____________________
method = 'shooting', npts = 3, initial_guess = 0, fail = 'xfail'
@pytest.mark.slow
@pytest.mark.parametrize(
"method, npts, initial_guess, fail", [
('shooting', 3, None, 'xfail'), # doesn't converge
('shooting', 3, 'zero', 'xfail'), # doesn't converge
# ('shooting', 3, 'u0', None), # github issue #782
('shooting', 3, 'input', 'endpoint'), # doesn't converge to optimal
('shooting', 5, 'input', 'endpoint'), # doesn't converge to optimal
('collocation', 3, 'u0', 'endpoint'), # doesn't converge to optimal
('collocation', 5, 'u0', 'endpoint'),
('collocation', 5, 'input', 'openloop'),# open loop sim fails
('collocation', 10, 'input', None),
('collocation', 10, 'u0', None), # from documentation
('collocation', 10, 'state', None),
('collocation', 20, 'state', None),
])
def test_optimal_doc(method, npts, initial_guess, fail):
"""Test optimal control problem from documentation"""
def vehicle_update(t, x, u, params):
# Get the parameters for the model
l = params.get('wheelbase', 3.) # vehicle wheelbase
phimax = params.get('maxsteer', 0.5) # max steering angle (rad)
# Saturate the steering input
phi = np.clip(u[1], -phimax, phimax)
# Return the derivative of the state
return np.array([
np.cos(x[2]) * u[0], # xdot = cos(theta) v
np.sin(x[2]) * u[0], # ydot = sin(theta) v
(u[0] / l) * np.tan(phi) # thdot = v/l tan(phi)
])
def vehicle_output(t, x, u, params):
return x # return x, y, theta (full state)
# Define the vehicle steering dynamics as an input/output system
vehicle = ct.NonlinearIOSystem(
vehicle_update, vehicle_output, states=3, name='vehicle',
inputs=('v', 'phi'), outputs=('x', 'y', 'theta'))
# Define the initial and final points and time interval
x0 = np.array([0., -2., 0.]); u0 = np.array([10., 0.])
xf = np.array([100., 2., 0.]); uf = np.array([10., 0.])
Tf = 10
# Define the cost functions
Q = np.diag([0, 0, 0.1]) # don't turn too sharply
R = np.diag([1, 1]) # keep inputs small
P = np.diag([1000, 1000, 1000]) # get close to final point
traj_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
term_cost = opt.quadratic_cost(vehicle, P, 0, x0=xf)
# Define the constraints
constraints = [ opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
# Define an initial guess at the trajectory
timepts = np.linspace(0, Tf, npts, endpoint=True)
if initial_guess == 'zero':
initial_guess = 0
elif initial_guess == 'u0':
initial_guess = u0
elif initial_guess == 'input':
# Velocity = constant that gets us from start to end
initial_guess = np.zeros((vehicle.ninputs, timepts.size))
initial_guess[0, :] = (xf[0] - x0[0]) / Tf
# Steering = rate required to turn to proper slope in first segment
approximate_angle = math.atan2(xf[1] - x0[1], xf[0] - x0[0])
initial_guess[1, 0] = approximate_angle / (timepts[1] - timepts[0])
initial_guess[1, -1] = -approximate_angle / (timepts[-1] - timepts[-2])
elif initial_guess == 'state':
input_guess = np.outer(u0, np.ones((1, npts)))
state_guess = np.array([
x0 + (xf - x0) * time/Tf for time in timepts]).transpose()
initial_guess = (state_guess, input_guess)
# Solve the optimal control problem
with warnings.catch_warnings():
warnings.filterwarnings(
'ignore', message="unable to solve", category=UserWarning)
result = opt.solve_optimal_trajectory(
vehicle, timepts, x0, traj_cost, constraints,
terminal_cost=term_cost, initial_guess=initial_guess,
trajectory_method=method,
# minimize_method='COBYLA', # SLSQP',
)
if fail == 'xfail':
> assert not result.success
E assert not True
E + where True = message: Optimization terminated successfully\n success: True\n status: 0\n fun: 2.4136235872518714\n x: [ 9.480e+00 1.025e+01 1.006e+01 3.297e-03 2.840e-03\n -4.371e-03]\n nit: 75\n jac: [-2.905e+01 -5.013e+01 -2.577e+01 1.936e+07 -1.171e+03\n 1.935e+07]\n nfev: 678\n njev: 74\n multipliers: [ 0.000e+00 0.000e+00 0.000e+00 0.000e+00 0.000e+00\n 0.000e+00 0.000e+00 0.000e+00 0.000e+00 0.000e+00\n 0.000e+00 0.000e+00]\n problem: <control.optimal.OptimalControlProblem object at 0x7f4fb7b07750>\n cost: 2.4136235872518714\n time: [ 0.000e+00 5.000e+00 1.000e+01]\n inputs: [[ 9.480e+00 1.025e+01 1.006e+01]\n [ 3.297e-03 2.840e-03 -4.371e-03]]\n states: [[ 0.000e+00 4.929e+01 9.999e+01]\n [-2.000e+00 -7.256e-01 1.998e+00]\n [ 0.000e+00 5.001e-02 3.711e-02]].success
control/tests/optimal_test.py:756: AssertionError
----------------------------- Captured stdout call -----------------------------
Summary statistics:
* Cost function calls: 678
* Constraint calls: 760
* System simulations: 1204
* Final cost: 2.4136235872518714