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()
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:
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: