trail_utraj = utraj[trail_indices]
    trail_utraj[:,0] = trail_utraj[:,0]/max_horz_thrust
    trail_utraj[:,1] = trail_utraj[:,1]/max_vert_thrust
    
    trail_traj = traj[trail_indices,2:4] #use position coordinates
    
    Ship_Sprite.make_trail_plot(trail_plot,trail_utraj,trail_traj)
        
    trail_plot.set_xlim(np.min(traj[:,2])-10,np.max(traj[:,2])+10)
    trail_plot.set_ylim(np.min(traj[:,3]-10),np.max(traj[:,3])+10)
    trail_plot.set_aspect('equal')

    trail_plot.set_xlim(-10,110)
    trail_plot.set_ylim(-10,110)

    pc = ship_field.get_patch_collection()
    pc.set_color('gray')      
    trail_plot.add_collection(pc)

    if False: #plot explored states
        s = shelve.open('kin_rrt.shelve')
        tree = s['tree']
        states = [node['state'] for node in tree.node.values()]
        states = np.array(states).T
        trail_plot.plot(states[0], states[1], 'r.', zorder=0,alpha=.4)
        s.close()

    #assert False    

    ani_fig = plt.figure(None)
    ani_ax = ani_fig.gca()
示例#2
0
    for j in range(len(a[i])):
        #i ranges over different runs of RRT and j ranges over different solution improvements in i.
        iteration = a[i][j][0]
        cost = a[i][j][1]
        solution_costs.append(cost)
        best_solution = a[i][j][2]
        u_path = best_solution[2]
        start_state = best_solution[1][0]
        x_path = run_forward(start_state,u_path)
        solutions.append( (cost,x_path,u_path) )
        if plot_all:
            normalized_cost = (cost-cost_min)/(cost_max-cost_min)
            ax.plot(x_path[:,2],x_path[:,3],alpha=.2,color=colormap(np.log(normalized_cost+1))[0:3],lw=3)

from ship_field import get_patch_collection
ax.add_collection(get_patch_collection())

ax.set_title('Solutions color-coded by cost')

solutions.sort(key=lambda x: x[0])
solutions = solutions[::-1] #most expensive first

best_cost,best_x_traj,best_u_traj = solutions[-1]

best_shelve = shelve.open('rrt_best.shelve')
best_shelve['traj'] = best_x_traj
best_shelve['utraj'] = best_x_traj
best_shelve.close()
print 'made best shelve'

if False:
示例#3
0
        solution_costs.append(cost)
        best_solution = a[i][j][2]
        u_path = best_solution[2]
        start_state = best_solution[1][0]
        x_path = run_forward(start_state, u_path)
        solutions.append((cost, x_path, u_path))
        if plot_all:
            normalized_cost = (cost - cost_min) / (cost_max - cost_min)
            ax.plot(x_path[:, 2],
                    x_path[:, 3],
                    alpha=.2,
                    color=colormap(np.log(normalized_cost + 1))[0:3],
                    lw=3)

from ship_field import get_patch_collection
ax.add_collection(get_patch_collection())

ax.set_title('Solutions color-coded by cost')

solutions.sort(key=lambda x: x[0])
solutions = solutions[::-1]  #most expensive first

best_cost, best_x_traj, best_u_traj = solutions[-1]

best_shelve = shelve.open('rrt_best.shelve')
best_shelve['traj'] = best_x_traj
best_shelve['utraj'] = best_x_traj
best_shelve.close()
print 'made best shelve'

if False: