def test_warm_start(self):
params = Parameter()
params.set("wheel_base", 2.7)
params.set("dt", 0.2)
params.set("function_tolerance", 1e-6)
params.set("max_num_iterations", 1000)
initial_state = np.array([[0., 0., 0., 10.], [2., 0., 0., 10.],
[4., 0., 0., 10.], [6., 0., 0., 10.]])
opt_vec = np.array(
[[0., 0.], [-0.00581466, -0.02969236], [-0.0100573, -0.05063586],
[-0.01290205, -0.06336555], [-0.0145111, -0.06958468],
[-0.01504364, -0.07147759], [-0.01466189, -0.0712203],
[-0.01353391, -0.07066254], [-0.01183398, -0.0711591],
[-0.00974178, -0.0735261], [-0.00744058, -0.07806701],
[-0.00509113, -0.08413195], [-0.0028139, -0.09089476],
[-0.00069383, -0.0976187], [0.00121458, -0.10373539],
[0.00288213, -0.10887319], [0.00430011, -0.11284776],
[0.00547573, -0.11562908], [0.00642782, -0.11729807],
[0.00718276, -0.11800159], [0.0077706, -0.11791354],
[0.00821974, -0.11720583], [0.00855625, -0.11603267],
[0.0088038, -0.11452536], [0.0089836, -0.11278997],
[0.00911419, -0.11090794], [0.00921137, -0.10893774],
[0.00928801, -0.10691821], [0.00935393, -0.10487191],
[0., 0.00059502]])
ref_line = np.array([[0., 4.], [1000., 4.]])
obstacle_outline0 = np.array([[14., 1.7], [22., 1.7], [22., 4.7],
[14., 4.7], [14., 1.7]])
# optimizer
opt = Optimizer(params)
opt.SetOptimizationVector(opt_vec)
# costs
ref_cost = ReferenceLineCost(params, 100.)
ref_cost.SetReferenceLine(ref_line)
jerk_cost = JerkCost(params, 10000.)
outline = ObjectOutline()
obstacle_outline1 = obstacle_outline0 + np.array([[30.0, -2.]])
outline.Add(obstacle_outline0, 0.)
outline.Add(obstacle_outline1, 6.)
object_cost = StaticObjectCost(params, 2.5, 10000.)
object_cost.AddObjectOutline(outline)
input_cost = InputCost(params, 10.)
input_cost.SetLowerBound(np.array([[-0.2, -1.0]]))
input_cost.SetUpperBound(np.array([[0.2, 1.0]]))
speed_cost = SpeedCost(params, 10.)
speed_cost.SetDesiredSpeed(10.)
# optimization problem
functor = opt.AddFastSingleTrackFunctor(
initial_state, params,
[jerk_cost, ref_cost, input_cost, object_cost, speed_cost])
opt.FixOptimizationVector(0, 1)
opt.Solve()
opt.Report()
def test_single_track_optimizer(self):
params = Parameter()
params.set("wheel_base", 2.7)
params.set("dt", 0.2)
params.set("function_tolerance", 1e-8)
params.set("max_num_iterations", 1000)
initial_state = np.array([[0., 0., 0., 10.],
[2., 0., 0., 10.],
[4., 0., 0., 10.],
[6., 0., 0., 10.]])
opt_vec = np.array([[ 0. , 0. ],
[-0.00581466, -0.02969236],
[-0.0100573 , -0.05063586],
[-0.01290205, -0.06336555],
[-0.0145111 , -0.06958468],
[-0.01504364, -0.07147759],
[-0.01466189, -0.0712203 ],
[-0.01353391, -0.07066254],
[-0.01183398, -0.0711591 ],
[-0.00974178, -0.0735261 ],
[-0.00744058, -0.07806701],
[-0.00509113, -0.08413195],
[-0.0028139 , -0.09089476],
[-0.00069383, -0.0976187 ],
[ 0.00121458, -0.10373539],
[ 0.00288213, -0.10887319],
[ 0.00430011, -0.11284776],
[ 0.00547573, -0.11562908],
[ 0.00642782, -0.11729807],
[ 0.00718276, -0.11800159],
[ 0.0077706 , -0.11791354],
[ 0.00821974, -0.11720583],
[ 0.00855625, -0.11603267],
[ 0.0088038 , -0.11452536],
[ 0.0089836 , -0.11278997],
[ 0.00911419, -0.11090794],
[ 0.00921137, -0.10893774],
[ 0.00928801, -0.10691821],
[ 0.00935393, -0.10487191],
[ 0. , 0.00059502]])
ref_line = np.array([[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
1.00000000e+01],
[ 2.00000000e+00, 0.00000000e+00, 0.00000000e+00,
1.00000000e+01],
[ 4.00000000e+00, 0.00000000e+00, 0.00000000e+00,
1.00000000e+01],
[ 6.00000000e+00, 0.00000000e+00, 0.00000000e+00,
1.00000000e+01],
[ 8.00000000e+00, 0.00000000e+00, 0.00000000e+00,
1.00000000e+01],
[ 9.99939997e+00, -4.30463875e-03, -4.30592393e-03,
9.99406153e+00],
[ 1.19971306e+01, -2.03405218e-02, -1.17478308e-02,
9.98393436e+00],
[ 1.39923704e+01, -5.32967584e-02, -2.12840277e-02,
9.97126125e+00],
[ 1.59845144e+01, -1.06379431e-01, -3.19953707e-02,
9.95734431e+00],
[ 1.79731414e+01, -1.81067097e-01, -4.30841450e-02,
9.94304879e+00],
[ 1.99579824e+01, -2.77367780e-01, -5.38759931e-02,
9.92880473e+00],
[ 2.19388867e+01, -3.94078028e-01, -6.38232626e-02,
9.91467222e+00],
[ 2.39157899e+01, -5.29044532e-01, -7.25085470e-02,
9.90044040e+00],
[ 2.58886800e+01, -6.79428615e-01, -7.96477572e-02,
9.88573518e+00],
[ 2.78575651e+01, -8.41973335e-01, -8.50921200e-02,
9.87012178e+00],
[ 2.98224543e+01, -1.01325561e+00, -8.88112060e-02,
9.85329539e+00],
[ 3.17833555e+01, -1.18989285e+00, -9.08631109e-02,
9.83511644e+00],
[ 3.37402750e+01, -1.36869381e+00, -9.13680809e-02,
9.81559270e+00],
[ 3.56932177e+01, -1.54676003e+00, -9.04859136e-02,
9.79484562e+00],
[ 3.76421873e+01, -1.72154515e+00, -8.83971179e-02,
9.77307098e+00],
[ 3.95871864e+01, -1.89087947e+00, -8.52877092e-02,
9.75050143e+00],
[ 4.15282170e+01, -2.05296693e+00, -8.13374631e-02,
9.72737561e+00],
[ 4.34652803e+01, -2.20636175e+00, -7.67114427e-02,
9.70391600e+00],
[ 4.53983772e+01, -2.34993117e+00, -7.15546057e-02,
9.68031568e+00],
[ 4.73275086e+01, -2.48281085e+00, -6.59892879e-02,
9.65673297e+00],
[ 4.92526750e+01, -2.60435938e+00, -6.01165980e-02,
9.63329181e+00],
[ 5.11738771e+01, -2.71411662e+00, -5.40182610e-02,
9.61008527e+00],
[ 5.30911154e+01, -2.81176608e+00, -4.77585100e-02,
9.58718020e+00],
[ 5.50043903e+01, -2.89710162e+00, -4.13860399e-02,
9.56462221e+00],
[ 5.69137022e+01, -2.96999832e+00, -3.49360318e-02,
9.54244062e+00],
[ 5.88190514e+01, -3.03038789e+00, -2.84322483e-02,
9.52065307e+00],
[ 6.07204383e+01, -3.07823825e+00, -2.18892006e-02,
9.49926943e+00],
[ 6.26178630e+01, -3.11353783e+00, -1.53143860e-02,
9.47829505e+00],
[ 6.45133116e+01, -3.14256773e+00, -1.53143860e-02,
9.47841405e+00]])
obstacle_outline0 = np.array([[14., 1.7],
[22., 1.7],
[22., 4.7],
[14., 4.7],
[14., 1.7]])
# optimizer
opt = Optimizer(params)
opt.SetOptimizationVector(opt_vec)
# costs
ref_cost = ReferenceCost(params, 100.)
ref_cost.SetReference(ref_line)
jerk_cost = JerkCost(params, 10000.)
outline = ObjectOutline()
obstacle_outline1 = obstacle_outline0 + np.array([[30.0, -3.]])
outline.Add(obstacle_outline0, 0.)
outline.Add(obstacle_outline1, 6.)
object_cost = StaticObjectCost(params, 2.5, 10000.)
object_cost.AddObjectOutline(outline)
input_cost = InputCost(params, 10.)
input_cost.SetLowerBound(np.array([[-0.2, -1.0]]))
input_cost.SetUpperBound(np.array([[0.2, 1.0]]))
# optimization problem
functor = opt.AddFastSingleTrackFunctor(initial_state,
params,
[jerk_cost,
object_cost,
input_cost,
ref_cost])
opt.FixOptimizationVector(0, 1)
opt.Solve()
opt.Report()
inputs = opt.Result()
trajectory = GenerateTrajectorySingleTrack(initial_state, inputs, params)
cmap, norm = GetColorMap(0., 6.)
fig, ax = plt.subplots(nrows=1, ncols=2)
for t in np.arange(0, 6, 0.5):
poly = outline.Query(t)
ax[0].plot(poly[:, 0], poly[:, 1], color=cmap(norm(t)))
colorbar_axis = ax[0].plot(trajectory[:, 0], trajectory[:, 1])
for i, pt in enumerate(trajectory[:]):
ax[0].plot(pt[0], pt[1], color=cmap(norm(params.set("dt", 0.2)*i)), marker='o')
ax[0].axis("equal")
ax[1].plot(inputs[:-1, 0], label="Steering angle", marker='o')
ax[1].plot(inputs[:-1, 1], label="Acceleration", marker='o', color="green")
ax[1].legend()
plt.show()
print(repr(inputs))
print(repr(trajectory))
def test_triple_int_optimizer(self):
params = Parameter()
params.set("dt", 0.2)
params.set("function_tolerance", 1e-6)
params.set("max_num_iterations", 1000)
initial_state = np.array([[0., 10., 0., 0., 10., 0., 0., 1., 0.],
[2., 10., 0., 2., 10., 0., 0.2, 1., 0.],
[4., 10., 0., 4., 10., 0., 0.4, 1., 0.],
[6., 10., 0., 6., 10., 0., 0.6, 1., 0.]])
opt_vec = np.zeros(shape=(30, 3))
ref_line = np.array([[10., -100.], [50.1, 100.1]])
# optimizer
opt = Optimizer(params)
opt.SetOptimizationVector(opt_vec)
# costs
ref_cost = ReferenceLineCost(params, 10.)
ref_cost.SetReferenceLine(ref_line)
jerk_cost = JerkCost(params, 100.)
speed_cost = SpeedCost(params, 10.)
speed_cost.SetDesiredSpeed(20.)
# optimization problem
functor = opt.AddTripleIntFunctor(initial_state, params,
[ref_cost, jerk_cost, speed_cost])
opt.FixOptimizationVector(0, 1)
opt.Solve()
opt.Report()
inputs = opt.Result()
trajectory = GenerateTrajectoryTripleInt(initial_state, inputs, params)
fig = plt.figure()
plt.subplot(144)
ax = fig.add_subplot(144, projection='3d')
plt.plot(trajectory[:, 0],
trajectory[:, 3],
trajectory[:, 6],
marker='o')
plt.plot([10, 50.1], [-100, 100], [0., 0.], marker='o')
plt.axis("equal")
plt.subplot(141)
plt.plot(trajectory[:-3, 1], label="vx", marker='o')
plt.plot(trajectory[:-3, 4], label="vy", marker='o', color="green")
plt.plot(trajectory[:-3, 7], label="vz", marker='o', color="red")
plt.xlabel("v [m/s]")
plt.legend()
plt.subplot(142)
plt.plot(trajectory[:, 0], trajectory[:, 3], marker='o')
plt.plot([10, 50.1], [-100, 100], marker='o')
plt.axis("equal")
plt.xlabel("x [m]")
plt.ylabel("y [m]")
plt.subplot(143)
plt.plot(trajectory[:, 3], trajectory[:, 6], marker='o')
plt.axis("equal")
plt.xlabel("y [m]")
plt.ylabel("z [m]")
# fig, ax = plt.subplots(nrows=1, ncols=2)
# colorbar_axis = ax[0].plot(trajectory[:, 0], trajectory[:, 3])
# ax[0].axis("equal")
# ax[1].plot(inputs[:-1, 0], label="ax", marker='o')
# ax[1].plot(inputs[:-1, 1], label="ay", marker='o', color="green")
# ax[1].plot(inputs[:-1, 2], label="az", marker='o', color="red")
# ax[1].legend()
plt.show()
print(repr(trajectory))
print(repr(inputs))
def test_single_track_optimizer(self):
params = Parameter()
params.set("wheel_base", 2.7)
params.set("dt", 0.2)
params.set("function_tolerance", 1e-6)
params.set("max_num_iterations", 1000)
params.set("num_threads", 1)
initial_state = np.array([[0., 0., 0., 10.], [2., 0., 0., 10.],
[4., 0., 0., 10.], [6., 0., 0., 10.]])
opt_vec = np.zeros(shape=(30, 2))
ref_line = np.array([[0., 4.], [1000., 4.]])
obstacle_outline0 = np.array([[14., 1.7], [22., 1.7], [22., 4.7],
[14., 4.7], [14., 1.7]])
# optimizer
opt = Optimizer(params)
opt.SetOptimizationVector(opt_vec)
# costs
ref_cost = ReferenceLineCost(params, 100.)
ref_cost.SetReferenceLine(ref_line)
jerk_cost = JerkCost(params, 10000.)
outline = ObjectOutline()
obstacle_outline1 = obstacle_outline0 + np.array([[30.0, -2.]])
outline.Add(obstacle_outline0, 0.)
outline.Add(obstacle_outline1, 6.)
object_cost = StaticObjectCost(params, 2.5, 10000.)
object_cost.AddObjectOutline(outline)
input_cost = InputCost(params, 10.)
input_cost.SetLowerBound(np.array([[-0.2, -1.0]]))
input_cost.SetUpperBound(np.array([[0.2, 1.0]]))
speed_cost = SpeedCost(params, 10.)
speed_cost.SetDesiredSpeed(10.)
# optimization problem
functor = opt.AddFastSingleTrackFunctor(
initial_state, params,
[jerk_cost, ref_cost, input_cost, object_cost, speed_cost])
opt.FixOptimizationVector(0, 1)
opt.Solve()
opt.Report()
inputs = opt.Result()
trajectory = GenerateTrajectorySingleTrack(initial_state, inputs,
params)
cmap, norm = GetColorMap(0., 6.)
fig, ax = plt.subplots(nrows=1, ncols=2)
for t in np.arange(0, 6, 0.5):
poly = outline.Query(t)
ax[0].plot(poly[:, 0], poly[:, 1], color=cmap(norm(t)))
colorbar_axis = ax[0].plot(trajectory[:, 0], trajectory[:, 1])
for i, pt in enumerate(trajectory[:]):
ax[0].plot(pt[0],
pt[1],
color=cmap(norm(params.set("dt", 0.2) * i)),
marker='o')
ax[0].axis("equal")
ax[1].plot(inputs[:-1, 0], label="Steering angle", marker='o')
ax[1].plot(inputs[:-1, 1],
label="Acceleration",
marker='o',
color="green")
ax[1].legend()
plt.show()
print(repr(trajectory))
print(repr(inputs))