def one_plan():
model = new_model()
plan, lam_x, lam_g = Planner.create_plan(model)
plan, lam_x, lam_g = Planner.create_belief_plan(
model, warm_start=True,
x0=plan, lam_x0=lam_x, lam_g0=lam_g
)
x_all = plan.prefix['X']
u_all = plan.prefix['U']
eb_all = Simulator.simulate_eb_trajectory(model, u_all)
# Plot
fig, ax = plt.subplots()
fig.tight_layout()
handles = Plotter.plot_plan(ax, eb_all)
ax.legend(handles=handles, loc='upper left')
ax.set_aspect('equal')
plt.show()
# ============================================================================
# Plan trajectory
# ============================================================================
# Find optimal controls
plan, lam_x, lam_g = Planner.create_plan(model)
# plan, lam_x, lam_g = Planner.create_plan(model, warm_start=True,
# x0=plan, lam_x0=lam_x, lam_g0=lam_g)
x_all = plan.prefix['X']
u_all = plan.prefix['U']
# Simulate ebelief trajectory
eb_all = Simulator.simulate_eb_trajectory(model, u_all)
# Plot 2D
fig, ax = plt.subplots()
handles = Plotter.plot_plan(ax, eb_all)
ax.legend(handles=handles, loc='upper left')
ax.set_aspect('equal')
fig.tight_layout()
# Plot 3D
# fig_3D = plt.figure(figsize=(10, 10))
# ax_3D = fig_3D.add_subplot(111, projection='3d')
# Plotter.plot_trajectory_3D(ax_3D, x_all)
# ============================================================================
# Belief space planning
# ============================================================================
# Find optimal controls
plan, lam_x, lam_g = Planner.create_belief_plan(model, warm_start=True,