def test_optimal_basis_simple():
sys = ct.ss2io(ct.ss([[1, 1], [0, 1]], [[1], [0.5]], np.eye(2), 0, 1))
# State and input constraints
constraints = [
(sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -1], [5, 5, 1]),
]
# Quadratic state and input penalty
Q = [[1, 0], [0, 1]]
R = [[1]]
cost = opt.quadratic_cost(sys, Q, R)
# Set up the optimal control problem
Tf = 5
time = np.arange(0, Tf, 1)
x0 = [4, 0]
# Basic optimal control problem
res1 = opt.solve_ocp(sys,
time,
x0,
cost,
constraints,
basis=flat.BezierFamily(4, Tf),
return_x=True)
assert res1.success
# Make sure the constraints were satisfied
np.testing.assert_array_less(np.abs(res1.states[0]), 5 + 1e-6)
np.testing.assert_array_less(np.abs(res1.states[1]), 5 + 1e-6)
np.testing.assert_array_less(np.abs(res1.inputs[0]), 1 + 1e-6)
# Pass an initial guess and rerun
res2 = opt.solve_ocp(sys,
time,
x0,
cost,
constraints,
initial_guess=0.99 * res1.inputs,
basis=flat.BezierFamily(4, Tf),
return_x=True)
assert res2.success
np.testing.assert_allclose(res2.inputs, res1.inputs, atol=0.01, rtol=0.01)
# Run with logging turned on for code coverage
res3 = opt.solve_ocp(sys,
time,
x0,
cost,
constraints,
basis=flat.BezierFamily(4, Tf),
return_x=True,
log=True)
assert res3.success
np.testing.assert_almost_equal(res3.inputs, res1.inputs, decimal=3)
def time_steering_bezier_basis(nbasis, ntimes):
# Set up costs and constriants
Q = np.diag([.1, 10, .1]) # keep lateral error low
R = np.diag([1, 1]) # minimize applied inputs
cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
constraints = [opt.input_range_constraint(vehicle, [0, -0.1], [20, 0.1])]
terminal = [opt.state_range_constraint(vehicle, xf, xf)]
# Set up horizon
horizon = np.linspace(0, Tf, ntimes, endpoint=True)
# Set up the optimal control problem
res = opt.solve_ocp(
vehicle,
horizon,
x0,
cost,
constraints,
terminal_constraints=terminal,
initial_guess=bend_left,
basis=flat.BezierFamily(nbasis, T=Tf),
# solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2},
minimize_method='trust-constr',
minimize_options={'finite_diff_rel_step': 0.01},
# minimize_method='SLSQP', minimize_options={'eps': 0.01},
return_states=True,
print_summary=False)
t, u, x = res.time, res.inputs, res.states
# Make sure we found a valid solution
assert res.success
def test_bezier_basis(self):
bezier = fs.BezierFamily(4)
time = np.linspace(0, 1, 100)
# Sum of the Bezier curves should be one
np.testing.assert_almost_equal(
1, sum([bezier(i, time) for i in range(4)]))
# Sum of derivatives should be zero
for k in range(1, 5):
np.testing.assert_almost_equal(
0, sum([bezier.eval_deriv(i, k, time) for i in range(4)]))
# Compare derivatives to formulas
np.testing.assert_almost_equal(bezier.eval_deriv(1, 0, time),
3 * time - 6 * time**2 + 3 * time**3)
np.testing.assert_almost_equal(bezier.eval_deriv(1, 1, time),
3 - 12 * time + 9 * time**2)
np.testing.assert_almost_equal(bezier.eval_deriv(1, 2, time),
-12 + 18 * time)
# Make sure that the second derivative integrates to the first
time = np.linspace(0, 1, 1000)
dt = np.diff(time)
for N in range(5):
bezier = fs.BezierFamily(N)
for i in range(N):
for j in range(1, N + 1):
np.testing.assert_allclose(
np.diff(bezier.eval_deriv(i, j - 1, time)) / dt,
bezier.eval_deriv(i, j, time)[0:-1],
atol=0.01,
rtol=0.01)
# Exception check
with pytest.raises(ValueError, match="index too high"):
bezier.eval_deriv(4, 0, time)
def time_steering_point_to_point(basis_name, basis_size):
if basis_name == 'poly':
basis = flat.PolyFamily(basis_size)
elif basis_name == 'bezier':
basis = flat.BezierFamily(basis_size)
# Find trajectory between initial and final conditions
traj = flat.point_to_point(vehicle, Tf, x0, u0, xf, uf, basis=basis)
# Verify that the trajectory computation is correct
x, u = traj.eval([0, Tf])
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, 1])
np.testing.assert_array_almost_equal(uf, u[:, 1])
class TestFlatSys:
"""Test differential flat systems"""
@pytest.mark.parametrize("xf, uf, Tf", [([1, 0], [0], 2), ([0, 1], [0], 3),
([1, 1], [1], 4)])
def test_double_integrator(self, xf, uf, Tf):
# Define a second order integrator
sys = ct.StateSpace([[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], 0)
flatsys = fs.LinearFlatSystem(sys)
# Define the basis set
poly = fs.PolyFamily(6)
x1, u1, = [0, 0], [0]
traj = fs.point_to_point(flatsys, Tf, x1, u1, xf, uf, basis=poly)
# Verify that the trajectory computation is correct
x, u = traj.eval([0, Tf])
np.testing.assert_array_almost_equal(x1, x[:, 0])
np.testing.assert_array_almost_equal(u1, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, 1])
np.testing.assert_array_almost_equal(uf, u[:, 1])
# Simulate the system and make sure we stay close to desired traj
T = np.linspace(0, Tf, 100)
xd, ud = traj.eval(T)
t, y, x = ct.forced_response(sys, T, ud, x1, return_x=True)
np.testing.assert_array_almost_equal(x, xd, decimal=3)
@pytest.fixture
def vehicle_flat(self):
"""Differential flatness for a kinematic car"""
def vehicle_flat_forward(x, u, params={}):
b = params.get('wheelbase', 3.) # get parameter values
zflag = [np.zeros(3), np.zeros(3)] # list for flag arrays
zflag[0][0] = x[0] # flat outputs
zflag[1][0] = x[1]
zflag[0][1] = u[0] * np.cos(x[2]) # first derivatives
zflag[1][1] = u[0] * np.sin(x[2])
thdot = (u[0] / b) * np.tan(u[1]) # dtheta/dt
zflag[0][2] = -u[0] * thdot * np.sin(x[2]) # second derivatives
zflag[1][2] = u[0] * thdot * np.cos(x[2])
return zflag
def vehicle_flat_reverse(zflag, params={}):
b = params.get('wheelbase', 3.) # get parameter values
x = np.zeros(3)
u = np.zeros(2) # vectors to store x, u
x[0] = zflag[0][0] # x position
x[1] = zflag[1][0] # y position
x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # angle
u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])
thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])
u[1] = np.arctan2(thdot_v, u[0]**2 / b)
return x, u
def vehicle_update(t, x, u, params):
b = params.get('wheelbase', 3.) # get parameter values
dx = np.array([
np.cos(x[2]) * u[0],
np.sin(x[2]) * u[0], (u[0] / b) * np.tan(u[1])
])
return dx
def vehicle_output(t, x, u, params):
return x
# Create differentially flat input/output system
return fs.FlatSystem(vehicle_flat_forward,
vehicle_flat_reverse,
vehicle_update,
vehicle_output,
inputs=('v', 'delta'),
outputs=('x', 'y', 'theta'),
states=('x', 'y', 'theta'))
@pytest.mark.parametrize(
"poly", [fs.PolyFamily(6),
fs.PolyFamily(8),
fs.BezierFamily(6)])
def test_kinematic_car(self, vehicle_flat, poly):
# Define the endpoints of the trajectory
x0 = [0., -2., 0.]
u0 = [10., 0.]
xf = [100., 2., 0.]
uf = [10., 0.]
Tf = 10
# Find trajectory between initial and final conditions
traj = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly)
# Verify that the trajectory computation is correct
x, u = traj.eval([0, Tf])
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, 1])
np.testing.assert_array_almost_equal(uf, u[:, 1])
# Simulate the system and make sure we stay close to desired traj
T = np.linspace(0, Tf, 500)
xd, ud = traj.eval(T)
# For SciPy 1.0+, integrate equations and compare to desired
if StrictVersion(sp.__version__) >= "1.0":
t, y, x = ct.input_output_response(vehicle_flat,
T,
ud,
x0,
return_x=True)
np.testing.assert_allclose(x, xd, atol=0.01, rtol=0.01)
def test_flat_cost_constr(self):
# Double integrator system
sys = ct.ss([[0, 1], [0, 0]], [[0], [1]], [[1, 0]], 0)
flat_sys = fs.LinearFlatSystem(sys)
# Define the endpoints of the trajectory
x0 = [1, 0]
u0 = [0]
xf = [0, 0]
uf = [0]
Tf = 10
T = np.linspace(0, Tf, 500)
# Find trajectory between initial and final conditions
traj = fs.point_to_point(flat_sys,
Tf,
x0,
u0,
xf,
uf,
basis=fs.PolyFamily(8))
x, u = traj.eval(T)
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, -1])
np.testing.assert_array_almost_equal(uf, u[:, -1])
# Solve with a cost function
timepts = np.linspace(0, Tf, 10)
cost_fcn = opt.quadratic_cost(flat_sys,
np.diag([0, 0]),
1,
x0=xf,
u0=uf)
traj_cost = fs.point_to_point(
flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
basis=fs.PolyFamily(8),
# initial_guess='lstsq',
# minimize_kwargs={'method': 'trust-constr'}
)
# Verify that the trajectory computation is correct
x_cost, u_cost = traj_cost.eval(T)
np.testing.assert_array_almost_equal(x0, x_cost[:, 0])
np.testing.assert_array_almost_equal(u0, u_cost[:, 0])
np.testing.assert_array_almost_equal(xf, x_cost[:, -1])
np.testing.assert_array_almost_equal(uf, u_cost[:, -1])
# Make sure that we got a different answer than before
assert np.any(np.abs(x - x_cost) > 0.1)
# Re-solve with constraint on the y deviation
lb, ub = [-2, -0.1], [2, 0]
lb, ub = [-2, np.min(x_cost[1]) * 0.95], [2, 1]
constraints = [opt.state_range_constraint(flat_sys, lb, ub)]
# Make sure that the previous solution violated at least one constraint
assert np.any(x_cost[0, :] < lb[0]) or np.any(x_cost[0, :] > ub[0]) \
or np.any(x_cost[1, :] < lb[1]) or np.any(x_cost[1, :] > ub[1])
traj_const = fs.point_to_point(
flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
constraints=constraints,
basis=fs.PolyFamily(8),
)
# Verify that the trajectory computation is correct
x_const, u_const = traj_const.eval(T)
np.testing.assert_array_almost_equal(x0, x_const[:, 0])
np.testing.assert_array_almost_equal(u0, u_const[:, 0])
np.testing.assert_array_almost_equal(xf, x_const[:, -1])
np.testing.assert_array_almost_equal(uf, u_const[:, -1])
# Make sure that the solution respects the bounds (with some slop)
for i in range(x_const.shape[0]):
assert np.all(x_const[i] >= lb[i] * 1.02)
assert np.all(x_const[i] <= ub[i] * 1.02)
# Solve the same problem with a nonlinear constraint type
nl_constraints = [(sp.optimize.NonlinearConstraint, lambda x, u: x, lb,
ub)]
traj_nlconst = fs.point_to_point(
flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
constraints=nl_constraints,
basis=fs.PolyFamily(8),
)
x_nlconst, u_nlconst = traj_nlconst.eval(T)
np.testing.assert_almost_equal(x_const, x_nlconst)
np.testing.assert_almost_equal(u_const, u_nlconst)
def test_bezier_basis(self):
bezier = fs.BezierFamily(4)
time = np.linspace(0, 1, 100)
# Sum of the Bezier curves should be one
np.testing.assert_almost_equal(
1, sum([bezier(i, time) for i in range(4)]))
# Sum of derivatives should be zero
for k in range(1, 5):
np.testing.assert_almost_equal(
0, sum([bezier.eval_deriv(i, k, time) for i in range(4)]))
# Compare derivatives to formulas
np.testing.assert_almost_equal(bezier.eval_deriv(1, 0, time),
3 * time - 6 * time**2 + 3 * time**3)
np.testing.assert_almost_equal(bezier.eval_deriv(1, 1, time),
3 - 12 * time + 9 * time**2)
np.testing.assert_almost_equal(bezier.eval_deriv(1, 2, time),
-12 + 18 * time)
# Make sure that the second derivative integrates to the first
time = np.linspace(0, 1, 1000)
dt = np.diff(time)
for N in range(5):
bezier = fs.BezierFamily(N)
for i in range(N):
for j in range(1, N + 1):
np.testing.assert_allclose(
np.diff(bezier.eval_deriv(i, j - 1, time)) / dt,
bezier.eval_deriv(i, j, time)[0:-1],
atol=0.01,
rtol=0.01)
# Exception check
with pytest.raises(ValueError, match="index too high"):
bezier.eval_deriv(4, 0, time)
def test_point_to_point_errors(self):
"""Test error and warning conditions in point_to_point()"""
# Double integrator system
sys = ct.ss([[0, 1], [0, 0]], [[0], [1]], [[1, 0]], 0)
flat_sys = fs.LinearFlatSystem(sys)
# Define the endpoints of the trajectory
x0 = [1, 0]
u0 = [0]
xf = [0, 0]
uf = [0]
Tf = 10
T = np.linspace(0, Tf, 500)
# Cost function
timepts = np.linspace(0, Tf, 10)
cost_fcn = opt.quadratic_cost(flat_sys,
np.diag([1, 1]),
1,
x0=xf,
u0=uf)
# Solving without basis specified should be OK
traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf, uf)
x, u = traj.eval(timepts)
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, -1])
np.testing.assert_array_almost_equal(uf, u[:, -1])
# Adding a cost function generates a warning
with pytest.warns(UserWarning, match="optimization not possible"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn)
# Make sure we still solved the problem
x, u = traj.eval(timepts)
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, -1])
np.testing.assert_array_almost_equal(uf, u[:, -1])
# Try to optimize with insufficient degrees of freedom
with pytest.warns(UserWarning, match="optimization not possible"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
basis=fs.PolyFamily(6))
# Make sure we still solved the problem
x, u = traj.eval(timepts)
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, -1])
np.testing.assert_array_almost_equal(uf, u[:, -1])
# Solve with the errors in the various input arguments
with pytest.raises(ValueError, match="Initial state: Wrong shape"):
traj = fs.point_to_point(flat_sys, timepts, np.zeros(3), u0, xf,
uf)
with pytest.raises(ValueError, match="Initial input: Wrong shape"):
traj = fs.point_to_point(flat_sys, timepts, x0, np.zeros(3), xf,
uf)
with pytest.raises(ValueError, match="Final state: Wrong shape"):
traj = fs.point_to_point(flat_sys, timepts, x0, u0, np.zeros(3),
uf)
with pytest.raises(ValueError, match="Final input: Wrong shape"):
traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf,
np.zeros(3))
# Different ways of describing constraints
constraint = opt.input_range_constraint(flat_sys, -100, 100)
with pytest.warns(UserWarning, match="optimization not possible"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
constraints=constraint,
basis=fs.PolyFamily(6))
x, u = traj.eval(timepts)
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, -1])
np.testing.assert_array_almost_equal(uf, u[:, -1])
# Constraint that isn't a constraint
with pytest.raises(TypeError, match="must be a list"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
constraints=np.eye(2),
basis=fs.PolyFamily(8))
# Unknown constraint type
with pytest.raises(TypeError, match="unknown constraint type"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
constraints=[(None, 0, 0, 0)],
basis=fs.PolyFamily(8))
# Unsolvable optimization
constraint = [opt.input_range_constraint(flat_sys, -0.01, 0.01)]
with pytest.raises(RuntimeError, match="Unable to solve optimal"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
constraints=constraint,
basis=fs.PolyFamily(8))
# Method arguments, parameters
traj_method = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
basis=fs.PolyFamily(8),
minimize_method='slsqp')
traj_kwarg = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
basis=fs.PolyFamily(8),
minimize_kwargs={'method': 'slsqp'})
np.testing.assert_allclose(traj_method.eval(timepts)[0],
traj_kwarg.eval(timepts)[0],
atol=1e-5)
# Unrecognized keywords
with pytest.raises(TypeError, match="unrecognized keyword"):
traj_method = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
solve_ivp_method=None)
xf = [75, -2., 0.]
uf = [15, 0.]
Tf = xf[0] / uf[0]
# Define a set of basis functions to use for the trajectories
poly = fs.PolyFamily(8)
# Find a trajectory between the initial condition and the final condition
traj1 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly)
plot_vehicle_lanechange(traj1)
# ### Change of basis function
# In[ ]:
bezier = fs.BezierFamily(8)
traj2 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=bezier)
plot_vehicle_lanechange(traj2)
# ### Added cost function
# In[ ]:
timepts = np.linspace(0, Tf, 12)
poly = fs.PolyFamily(8)
traj_cost = opt.quadratic_cost(vehicle_flat,
np.diag([0, 0.1, 0]),
np.diag([0.1, 10]),
x0=xf,
u0=uf)
constraints = [opt.input_range_constraint(vehicle_flat, [8, -0.1], [12, 0.1])]
def test_terminal_constraints(sys_args):
"""Test out the ability to handle terminal constraints"""
# Create the system
sys = ct.ss2io(ct.ss(*sys_args))
# Shortest path to a point is a line
Q = np.zeros((2, 2))
R = np.eye(2)
cost = opt.quadratic_cost(sys, Q, R)
# Set up the terminal constraint to be the origin
final_point = [opt.state_range_constraint(sys, [0, 0], [0, 0])]
# Create the optimal control problem
time = np.arange(0, 3, 1)
optctrl = opt.OptimalControlProblem(
sys, time, cost, terminal_constraints=final_point)
# Find a path to the origin
x0 = np.array([4, 3])
res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
t, u1, x1 = res.time, res.inputs, res.states
# Bug prior to SciPy 1.6 will result in incorrect results
if NumpyVersion(sp.__version__) < '1.6.0':
pytest.xfail("SciPy 1.6 or higher required")
np.testing.assert_almost_equal(x1[:,-1], 0, decimal=4)
# Make sure it is a straight line
Tf = time[-1]
if ct.isctime(sys):
# Continuous time is not that accurate on the input, so just skip test
pass
else:
# Final point doesn't affect cost => don't need to test
np.testing.assert_almost_equal(
u1[:, 0:-1],
np.kron((-x0/Tf).reshape((2, 1)), np.ones(time.shape))[:, 0:-1])
np.testing.assert_allclose(
x1, np.kron(x0.reshape((2, 1)), time[::-1]/Tf), atol=0.1, rtol=0.01)
# Re-run using initial guess = optional and make sure nothing changes
res = optctrl.compute_trajectory(x0, initial_guess=u1)
np.testing.assert_almost_equal(res.inputs, u1)
# Re-run using a basis function and see if we get the same answer
res = opt.solve_ocp(sys, time, x0, cost, terminal_constraints=final_point,
basis=flat.BezierFamily(4, Tf))
np.testing.assert_almost_equal(res.inputs, u1, decimal=2)
# Impose some cost on the state, which should change the path
Q = np.eye(2)
R = np.eye(2) * 0.1
cost = opt.quadratic_cost(sys, Q, R)
optctrl = opt.OptimalControlProblem(
sys, time, cost, terminal_constraints=final_point)
# Turn off warning messages, since we sometimes don't get convergence
with warnings.catch_warnings():
warnings.filterwarnings(
"ignore", message="unable to solve", category=UserWarning)
# Find a path to the origin
res = optctrl.compute_trajectory(
x0, squeeze=True, return_x=True, initial_guess=u1)
t, u2, x2 = res.time, res.inputs, res.states
# Not all configurations are able to converge (?)
if res.success:
np.testing.assert_almost_equal(x2[:,-1], 0)
# Make sure that it is *not* a straight line path
assert np.any(np.abs(x2 - x1) > 0.1)
assert np.any(np.abs(u2) > 1) # Make sure next test is useful
# Add some bounds on the inputs
constraints = [opt.input_range_constraint(sys, [-1, -1], [1, 1])]
optctrl = opt.OptimalControlProblem(
sys, time, cost, constraints, terminal_constraints=final_point)
res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
t, u3, x3 = res.time, res.inputs, res.states
# Check the answers only if we converged
if res.success:
np.testing.assert_almost_equal(x3[:,-1], 0, decimal=4)
# Make sure we got a new path and didn't violate the constraints
assert np.any(np.abs(x3 - x1) > 0.1)
np.testing.assert_array_less(np.abs(u3), 1 + 1e-6)
# Make sure that infeasible problems are handled sensibly
x0 = np.array([10, 3])
with pytest.warns(UserWarning, match="unable to solve"):
res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
assert not res.success
# for a much more time resolved set of inputs.
print("Approach 4: Bezier basis")
import control.flatsys as flat
# Compute the optimal control
start_time = time.process_time()
result4 = opt.solve_ocp(
vehicle,
horizon,
x0,
quad_cost,
constraints,
terminal_constraints=terminal,
initial_guess=bend_left,
basis=flat.BezierFamily(4, T=Tf),
# solve_ivp_kwargs={'method': 'RK45', 'atol': 1e-2, 'rtol': 1e-2},
solve_ivp_kwargs={
'atol': 1e-3,
'rtol': 1e-2
},
minimize_method='trust-constr',
minimize_options={'disp': True},
log=False)
print("* Total time = %5g seconds\n" % (time.process_time() - start_time))
# If we are running CI tests, make sure we succeeded
if 'PYCONTROL_TEST_EXAMPLES' in os.environ:
assert result4.success
# Extract and plot the results (+ state trajectory)
x0 = [0., 2., 0.]
u0 = [15, 0.]
xf = [75, -2., 0.]
uf = [15, 0.]
Tf = xf[0] / uf[0]
# Define a set of basis functions to use for the trajectories
poly = fs.PolyFamily(10)
# Find a trajectory between the initial condition and the final condition
traj1 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly)
plot_vehicle_lanechange(traj1)
# ## Change of basis function
# ##
bezier = fs.BezierFamily(8) # 创建阶数8的多项式基础
# Find a trajectory between the initial condition and the final condition
traj2 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=bezier)
plot_vehicle_lanechange(traj2)
# ### Added cost function
# ###
timepts = np.linspace(0, Tf, 12)
poly = fs.PolyFamily(8)
# Create quadratic cost function
# Returns a quadratic cost function that can be used for an optimal control problem.
traj_cost = opt.quadratic_cost(vehicle_flat,
np.diag([0, 0.1, 0]),
np.diag([0.1, 10]),
x0=xf,
