示例#1
0
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()
示例#2
0
文件: main.py 项目: b4be1/easy_catch
# ============================================================================
#                             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,