def tester(fname_classifier, Jth):
COST = save_obj.load_object('COST_obs.pkl')
classifier = save_obj.load_object(fname_classifier)
COSTkeys = list(COST.keys())
tot = 0
ptot = 0
wrongs = 0
for k in COSTkeys[1000000:1200000]:
truth = COST[k][0] < Jth
pred = classifier.predict([k[0] + k[1]])
if pred != truth:
wrongs += 1
tot += truth
ptot += pred
print("tot: {}, ptot: {}, wrongs: {}".format(tot, ptot, wrongs))
print("percentage correct: {}".format(1 - wrongs / 200000.))
def precompute_reachable_sets(fname_V, fname_COST, fname_classifier, Jth):
print("Memoizing reachable sets...")
sys.setrecursionlimit(200000)
if isinstance(fname_V, str):
V = load_object(fname_V)
else:
V = fname_V
if isinstance(fname_COST, str):
COST = load_object(fname_COST)
else:
COST = fname_COST
if isinstance(fname_classifier, str):
classifier = load_object(fname_classifier)
else:
classifier = fname_classifier
N = len(V)
i=1
for v in V:
print(" Updating node {} / {}".format(i, N))
v.N_out = reachable_set(v, V, Jth, COST, classifier)
v.N_in = reachable_set(v, V, Jth, COST, classifier, forward=False)
i+=1
return V
def trainer(count, n, Jth):
COST = save_obj.load_object('COST_full_{}.pkl'.format(n))
COSTkeys = list(COST.keys())
print("total number of entries in COST dict:", len(COSTkeys))
subdict = {k: COST[k] for k in COSTkeys[:count]}
classifier = trainsvm(subdict, 0.05, Jth)
tot = 0
ptot = 0
wrongs = 0
for k in COSTkeys[100000:200000]:
truth = COST[k][0] < Jth
pred = classifier.predict([k[0] + k[1]])
if pred != truth:
wrongs += 1
# print("Wrong. ", k, ":", COST[k][0])
tot += truth # number of actual true values
ptot += pred # number of predicted true values
print("Out of 100,000 experiments -- tot: {}, ptot: {}, wrongs: {}".format(
tot, ptot, wrongs))
print("percentage correct: {}".format(1 - wrongs / 100000.))
save_obj.save_object(classifier,
'classifier{}node_J{}.pkl'.format(n, int(Jth)))
NUM = 50
sys.setrecursionlimit(200000)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlabel('X')
ax.set_xlim3d(0, 10)
ax.set_ylabel('Y')
ax.set_ylim3d(0, 10)
ax.set_zlabel('Z')
ax.set_zlim3d(0, 10)
P = save_obj.load_object('P.pkl')
M = 3
N = 9
T = [1.7608794149125462, 1.5615551748592407, 1.1402720224000513]
for m in range(M):
tt = np.linspace(0, T[m], num=NUM)
xs = polyval(tt, P['x'][m*(N+1):(m+1)*(N+1)]).reshape(NUM,)
ys = polyval(tt, P['y'][m*(N+1):(m+1)*(N+1)]).reshape(NUM,)
zs = polyval(tt, P['z'][m*(N+1):(m+1)*(N+1)]).reshape(NUM,)
yaws = polyval(tt, P['yaw'][m*(N+1):(m+1)*(N+1)]).reshape(NUM,)
ax.plot(xs, ys, zs, lw=2, color='g', zorder=2)
plt.show()
from save_obj import load_object
from kripke import kripke
from modelchecker import modelcheck
# MG has 2 regions, each of their best paths has an endnode in the other region
MG = load_object('MG.pkl')
MG.graphs = load_object('MGdoubleint2.pkl')
endnodes = load_object('endnodes_doubleint2.pkl')
Props = ['a', 'b', 'c']
K = kripke(MG, endnodes, Props)
print modelcheck(
K,
'nuZ.((a && muX.(((b || c) && Z) || <>X)) || (b && muY.(((a || c) && Z) || <>Y)) || (c && muY.(((a || b) && Z) || <>Y)))',
Props, MG.graphs[0].V[0][0])
#nuZ.((a && muX.(((b || c) && Z) || <>X)) || (b && muY.(((a || c) && Z) || <>Y)) || (c && muY.(((a || b) && Z) || <>Y)))
def planner(n, Jth, xinit, Xgoal, ax,
plot_bool=False, all_lines=False,
plot_waypts=True, plot_smooth=True, no_smoothing=False):
# --- setup environment --- #
sys.setrecursionlimit(10*n*(n-1))
Xgoal_pos = Xgoal[0]
Xgoal_width = Xgoal[1]
Xgoal_depth = Xgoal[2]
Xgoal_height = Xgoal[3]
g_intervals = goal_intervals(Xgoal)
for o in OBS:
draw_3d(*o, ax, color=(0.8, 0.2, 0.2, 1.))
# --------------------------- #
# kinoFMT algorithm #
# --------------------------- #
# --- initialize graph ---------------------------------------------------- #
# reinitialize xinit to ensure it starts "fresh"
xinit = Node(xinit.state)
# ax.scatter(*xinit.state[:3], s=64, marker='*') # plot initial state
# load the presampled set of states for this (n, Jth) configuration, and
# add xinit to this set.
V = save_obj.load_object('V_n{}_J{}.pkl'.format(int(n),int(Jth))) | {xinit}
# COST roadmap (dictionary)
COST = save_obj.load_object('COST_full_n{}_J{}.pkl'.format(int(n), int(Jth)))
# classifier is a svm.SVC object which classifies a pair of states with Jth
classifier = \
save_obj.load_object('classifier{}node_J{}.pkl'.format(n, int(Jth)))
#--------------------------------------------------------------------------
# Remove any nodes that happen to lie in the goal region -- we only
# want to deal with goal nodes we sample from goal explicitly.
#--------------------------------------------------------------------------
# goal_set = set()
# for v in V - {xinit}:
# within_goal = within_boundary(v, g_intervals)
# if within_goal:
# goal_set |= {v}
# print("goal state:", v.state)
# V -= goal_set
# print("Total removed from goal:", len(goal_set))
#
# # FOR DEBUGGING
# tot_pred = 0
# for q in goal_set:
# for v in V - {q}:
# p = np.append(v.state, q.state)
# pred = classifier.predict([p])
# if pred: tot_pred += 1
# oldgoal = goal_set.pop()
# goal_set |= {oldgoal}
# print("Total predicted in N_in for removed goal state:", tot_pred)
# print("N_in actual:", len(oldgoal.N_in))
# print("iself?",oldgoal in oldgoal.N_in)
# print("cost:", oldgoal.cost)
# for q in goal_set:
# print("Removed:", q.state)
# for v in V - {xinit}:
# if q in v.N_out:
# v.N_out -= {q}
# if q in v.N_in:
# v.N_in -= {q}
#--------------------------------------------------------------------------
E = set([])
H = {xinit} # frontier of exploration
W = V - {xinit} # copy of V setminus {xinit} (unvisited)
z = xinit # point in the frontier, H
stime = time.time()
# find xinit reachable set
V |= {xinit}
xinit.N_out = reachable_set(xinit, V, Jth, COST, classifier)
for v in xinit.N_out:
# cost(xinit, v, COST) # add cost to COST dictionary
v.N_in |= {xinit}
# find Xgoal reachable sets
Vgoal = sample(n_goal, Xgoal_pos, Xgoal_width, Xgoal_depth, Xgoal_height,
COST, ax=None, zero_vel=True)
# # FOR DEBUGGING
# for q in goal_set:
# replacement = Node(q.state)
# Vgoal |= {replacement}
# print("Added replacement:", replacement.state)
for vg in Vgoal:
vg.N_in = reachable_set(vg, V, Jth, COST, classifier, forward=False)
print("goal state N_in contains", len(vg.N_in))
# add vg to the outgoing nbhd of all nodes that can reach it
for q in vg.N_in:
# cost(q, vg, COST) # add cost to COST dictionary
q.N_out |= {vg}
V |= Vgoal
print("Running kinoFMT...")
kinotimer = time.time()
coltimer = 0. # accumulates collision-check time
i = 0
while not(within_boundary(z, g_intervals)):
N_zout = reachable_set(z, V, Jth, COST, classifier)
Xnear = N_zout & W # set of as yet unreached nodes in reachable set
for x in Xnear:
# find backward reachable set of x
N_xin = reachable_set(x, V, Jth, COST, classifier, forward=False)
Ynear = N_xin & H # set of frontier nodes that can reach x
# find ymin if it exists, otherwise proceed to next x.
if len(Ynear) == 0:
continue
ymin = random.choice(tuple(Ynear)) # initialize with random y
ymin_x_cost = ymin.cost + cost(ymin, x, COST)
for y in Ynear:
y_x_cost = y.cost + cost(y, x, COST)
if y_x_cost < ymin_x_cost:
ymin = y
ymin_x_cost = y_x_cost
colcurtime = time.time()
no_collision = collision_check_3d(ymin, x, OBS)
coltimer += time.time() - colcurtime
if no_collision:
# ymin.children |= {x}
x.parent = ymin
x.cost = ymin_x_cost
E |= set([Edge(ymin, x)])
H |= set([x])
W -= set([x])
xs = (ymin.state[0], x.state[0])
ys = (ymin.state[1], x.state[1])
zs = (ymin.state[2], x.state[2])
if all_lines:
ax.plot(xs, ys, zs, lw=1, zorder=1)
H -= {z}
if len(H) == 0:
print("Failure")
return -1, [], -1 # so as to not break when returning P,T,z
# find argmin h in H of cost-to-come
z = random.choice(tuple(H))
for h in H:
if h.cost < z.cost:
z = h
i += 1
kinotime = time.time() - kinotimer
# plt.axis('scaled')
draw_3d(*Xgoal, ax)
waypoints = draw_path(xinit, z, ax, plot_waypoints=plot_waypts) # list of nodes from start to finish
# create list T of optimal times for each segment/edge
M = len(waypoints)-1
T = []
for m in range(M):
n1 = waypoints[m]
n2 = waypoints[m+1]
p = (n1.state, n2.state)
t = COST[p][1]
T.append(t)
print("M =", M)
print("T =", T)
if no_smoothing:
if plot_bool:
plt.show()
return waypoints, T, z
# --- trajectory smoothing --- #
smoothtimer = time.time()
print("")
P, all_xs, all_ys, all_zs = smooth_traj(waypoints, T, N, beta,
OBS, screen_intervals, NUM)
print("")
print("Time spent smoothing:", time.time() - smoothtimer)
print("Time spent running kinoFMT:", kinotime)
print("Total collision_check_3d computation time: {}\n".format(coltimer))
print("Actual execution time: {}".format(time.time() - stime))
print("-----"*4)
if plot_smooth:
ax.plot(all_xs, all_ys, all_zs, lw=4, color='g', zorder=1)#, linestyle='--')
if plot_bool:
# plt.axis('scaled')
plt.show()
return P, T, z