def drawObjectPathCostMap(featureBrowser, obj_esdc, event_esdcs, annotation,
cf, xmin, xmax, ymin, ymax, step):
xstart, ystart = annotation.getGroundings(obj_esdc)[0].centroid2d
ax, ay = annotation.agent.centroid2d
ath = annotation.agent.path.theta[0]
print ax, ay, ath
#agent move to pick up object
aX, aY = sf.math2d_step_along_line(tp([(ax, ay), (xstart, ystart)]), .1)
aTh = ath * na.ones(len(aX))
costs = na.zeros((int((ymax - ymin) / step), int((xmax - xmin) / step)))
annotations = []
for i, x in enumerate(na.arange(xmin, xmax, step)):
for j, y in enumerate(na.arange(ymin, ymax, step)):
X, Y = sf.math2d_step_along_line(tp([(xstart, ystart), (x, y)]),
.1)
Z = na.ones(len(X))
th = na.zeros(len(X))
timestamps = range(len(X))
path = Path(timestamps, [X, Y, Z, th])
new_annotation = annotation_copy(annotation)
atimestamps = range(len(X) + len(aX))
axs = na.append(aX, X)
ays = na.append(aY, Y)
azs = na.zeros(len(X) + len(aX))
ath = na.append(aTh, th)
new_annotation.agent.path = Path(atimestamps, [axs, ays, azs, ath])
obj = new_annotation.getGroundings(obj_esdc)[0]
obj.path = path
assignPathGroundings(event_esdcs[0], new_annotation)
state, gggs = annotation_to_ggg_map(new_annotation)
ggg = ggg_from_esdc(new_annotation.esdcs[0])
factor = ggg.esdc_to_factor(event_esdcs[0])
cost, entries = cf.compute_costs([factor],
ggg,
state_sequence=None)
costs[j][i] = math.exp(-1.0 * cost)
annotations.append(((x, y), entries))
print i
featureBrowser.setCostImage(costs, annotations, xmin, xmax, ymin, ymax)
def is_visible_polygon(self, point, poly, max_dist=5.0):
x, y = point
mymap = self.get_map()
if isInteriorPoint([mymap.to_xy(p) for p in zip(poly.X, poly.Y)],
point):
return True
points_pts = []
for i in range(poly.num_segments()):
xi, yi = poly.get_segment(i)
xp, yp = mymap.to_xy([xi, yi])
points_pts.append((xp, yp))
pts = array(
math2d_step_along_line(tp(points_pts),
math2d_line_length(points_pts) / 10))
for i in range(len(pts[0])):
p = pts[:, i]
if self.debug:
print
print "p", p, point
idx = mymap.to_index(point)
print "indices", idx
print "resolution", mymap.to_xy([idx[0] + 1, idx[1] + 1])
print "prob", mymap.get_value_probability(
int(idx[0]), int(idx[1]))
if self.is_visible(point, p, max_dist):
return True
return False
def drawObjectStartCostMap(featureBrowser, obj_esdc, l2_esdc, event_esdcs,
annotation, beam_search, xmin, xmax, ymin, ymax,
step):
xend, yend = annotation.getGroundings(l2_esdc)[0].centroid2d
ax, ay = annotation.agent.centroid2d
ath_0 = annotation.agent.path.theta[0]
print ax, ay, ath_0
#agent move to pick up object
costs = na.zeros((int((ymax - ymin) / step), int((xmax - xmin) / step)))
annotations = []
for i, x in enumerate(na.arange(xmin, xmax, step)):
for j, y in enumerate(na.arange(ymin, ymax, step)):
aX, aY = sf.math2d_step_along_line(tp([(ax, ay), (x, y)]), .1)
aTh = ath_0 * na.ones(len(aX))
X, Y = sf.math2d_step_along_line(tp([(x, y), (xend, yend)]), .1)
Z = na.ones(len(X))
th = na.zeros(len(X))
timestamps = range(len(X))
path = Path(timestamps, [X, Y, Z, th])
new_annotation = annotation_copy(annotation)
atimestamps = range(len(X) + len(aX))
axs = na.append(aX, X)
ays = na.append(aY, Y)
azs = na.zeros(len(X) + len(aX))
ath = na.append(aTh, th)
new_annotation.agent.setPath(
Path(atimestamps, [axs, ays, azs, ath]))
obj = new_annotation.getGroundings(obj_esdc)[0]
obj.setPath(path)
assignPathGroundings(event_esdcs[0], new_annotation)
cost, entries = beam_search.cf_obj.costEntry(
event_esdcs, new_annotation)
costs[j][i] = math.exp(-1.0 * cost)
annotations.append(((x, y), entries))
print i
featureBrowser.setCostImage(costs, annotations, xmin, xmax, ymin, ymax)
def path_probabilities(cf, path_factor, path_node, ggg, xmin, xmax, ymin, ymax,
step):
"""
Compute probabilities of endpoints in the given range of a
top-level agent path given a path node and an associated factor
in a ggg, a weights vector, and a cost function.
"""
xstart, ystart = (xmin + 0.5, (ymax + ymin) / 2.0)
probs = na.zeros((int((ymax - ymin) / step), int((xmax - xmin) / step)))
prob_idx_to_xy = {}
for i, x in enumerate(na.arange(xmin, xmax, step)):
for j, y in enumerate(na.arange(ymin, ymax, step)):
X, Y = sf.math2d_step_along_line(
na.transpose([(xstart, ystart), (x, y)]), .1)
Z = na.ones(len(X))
fig_xy = na.array([X, Y])
Xst, Yst = fig_xy[:, :-1]
Xend, Yend = fig_xy[:, 1:]
Theta = na.arctan2(Yend - Yst, Xend - Xst)
Theta = list(Theta)
Theta.append(Theta[-1])
th = list(Theta)
timestamps = range(len(X))
path = Path.from_xyztheta(timestamps, [X, Y, Z, th])
pobj = PhysicalObject(Prism.from_point(X[0], Y[0], Z[0], Z[0] + 1),
tags=("forklift", ),
path=path)
ggg.set_evidence_for_node(path_node, [pobj])
new_evidences = Evidences.copy(ggg.evidences)
for phi in path_factor.nodes_with_type("phi"):
new_evidences[phi.id] = True
ggg = GGG.from_ggg_and_evidence(ggg, new_evidences)
ce = cf.compute_factor_cost_entry(path_factor, ggg, None)
probs[j][i] = ce.probability
prob_idx_to_xy[(j, i)] = (x, y)
print i
print 'min/max', min(probs.flatten()), max(probs.flatten())
print "max idx", na.argmax(probs)
max_idx = na.argmax(probs)
max_tuple = na.unravel_index(max_idx, probs.shape)
print "max x,y", prob_idx_to_xy[max_tuple]
return (probs, xstart, ystart)
def planToLcmLog(state, ggg, fname, rndf, app=None, realForklift=False):
if app == None:
app = action_executor.App(rndf_file=rndf)
sequence = state.getSequence()
log = lcm.EventLog(fname, 'w', overwrite=True)
sx, sy, sz, stheta = state.getGroundableById(
state.getAgentId()).path.points_ptsztheta[0]
#if actionMap != None:
# startId = actionMap.nearest_index((sx, sy))
# for chk_pt in rndf.checkpoints:
# if chk_pt.id_int == startId:
# lat = chk_pt.waypoint.lat
# lon = chk_pt.waypoint.lon
# break
#else:
lat, lon = ru.xy_to_latlon(sx, sy, rndf.origin)
if not realForklift:
transportEvent = app.send_transport_msg(lat,
lon,
stheta,
returnEvent=True)
log.write_event(transportEvent.timestamp, transportEvent.channel,
transportEvent.data)
for s, action in sequence:
print action
events = []
if isinstance(action, forkState.Move):
#event = app.send_checkpoint(a.to_loc, returnEvent=True)
sx, sy = action.from_location[0:2]
x, y = action.to_location[0:2]
#To keep forklift from trying to get to close to a pallet
line = sf.math2d_step_along_line([(x, sx), (y, sy)], .1)
line_pts = transpose(line)
worstCase = True # worst case if all points along line are too close to pallets
bestX = x
bestY = y
maxMinDist = 0 # distance for the point with greatest buffer (infinum)
for px, py in line_pts:
tooClose = False
held_pallet = s.get_held_pallet()
localMinDist = 100 # keeps track of the distance to closest pallet for this (px, py)
for obj in [
s.getGroundableById(gid) for gid in s.getObjectsSet()
]:
if held_pallet and obj.lcmId == held_pallet.lcmId:
continue
distFromPallet = sf.math2d_dist(
(px, py),
obj.prismAtT(-1).centroid2d())
if distFromPallet < localMinDist:
localMinDist = distFromPallet
if distFromPallet < 4.0:
tooClose = True
break
if not tooClose:
bestX = px
bestY = py
print 'changing move from', (x, y), 'to', (bestX, bestY)
worstCase = False
break
elif localMinDist > maxMinDist: # does this (px, py) offer a better worst case buffer?
maxMinDist = localMinDist
bestX = px
bestY = py
# If none of the points were sufficiently far from the pallet, choose the one that was the farthest
if worstCase == True:
print '(px, py) farthest from pallet was ', maxMinDist, ' away. Changing from', (
x, y), 'to', (bestX, bestY)
px = bestX
py = bestY
lat, lon = ru.xy_to_latlon(px, py, rndf.origin)
events.append(app.send_latlon(lat, lon, returnEvent=True))
elif isinstance(action, forkState.Place):
x, y, z = action.location
lat, lon = ru.xy_to_latlon(x, y, rndf.origin)
if realForklift:
print "**************************************"
print "WARNING--sending to forklift checkpoint 16"
print "this is a hack and only applies at waverly"
print "long term, agile should probably be changed to"
print "handle long distance \"place\" commands without"
print "having to go to a checkpoint first."
events.append(app.send_checkpoint(16, returnEvent=True))
events.append(
app.send_place_pallet_msg(lat, lon, z, returnEvent=True))
elif isinstance(action, forkState.Pickup):
print "pallet", action.pallet_id
if action.pallet_id == -1:
print "************************************"
print "warning, overwriting pallet id", action.pallet_id
print "remove me after we figure out why it is -1."
print "we are hardcoding it to the correct ID in one specific"
print "scenario, but this does not generalize."
pallet_id = 330225303595152896
else:
pallet_id = action.pallet_id
events.append(
app.send_pickup_pallet_msg(pallet_id, returnEvent=True))
elif action == None:
continue
else:
raise TypeError('Unrecognized action: ' + ` action ` + " class: " +
` action.__class__ `)
for event in events:
log.write_event(event.timestamp, event.channel, event.data)
log.close()
def drawAgentPathCostMap(featureBrowser, event_path_esdcs, annotation, cf,
xmin, xmax, ymin, ymax, step):
print "y", ymax, ymin
xstart, ystart = (xmin + 0.5, (ymax + ymin) / 2.0)
#xstart, ystart = 19.658, 14.900
ath = 0
print "start", xstart, ystart, ath
costs = na.zeros((int((ymax - ymin) / step), int((xmax - xmin) / step)))
state, gggs = annotation_to_ggg_map(annotation)
esdc = event_path_esdcs[0]
ggg = gggs[esdc]
factor = ggg.esdc_to_factor(esdc)
node = ggg.node_for_esdc(esdc)
print "esdc", esdc
print "node", node, node.__class__
for i, x in enumerate(na.arange(xmin, xmax, step)):
for j, y in enumerate(na.arange(ymin, ymax, step)):
X, Y = sf.math2d_step_along_line(tp([(xstart, ystart), (x, y)]),
.1)
Z = na.ones(len(X))
fig_xy = na.array([X, Y])
Xst, Yst = fig_xy[:, :-1]
Xend, Yend = fig_xy[:, 1:]
Theta = na.arctan2(Yend - Yst, Xend - Xst)
Theta = list(Theta)
Theta.append(Theta[-1])
th = list(Theta)
#th = ath*na.ones(len(X))
timestamps = range(len(X))
path = Path(timestamps, [X, Y, Z, th])
pobj = PhysicalObject(Prism.from_point(X[0], Y[0], Z[0], Z[0] + 1),
tags=("forklift", ),
path=path)
ggg.set_evidence_for_node(node, [pobj])
#new_annotation = annotation_copy(annotation)
#new_annotation.agent = new_annotation.agent.withPath(path)
#if esdc.type == "EVENT":
# assignPathGroundings(esdc, new_annotation)
#else: #type is path
# new_annotation.setGrounding(esdc, new_annotation.agent.path)
new_evidences = Evidences.copy(ggg.evidences)
for phi in factor.nodes_with_type("phi"):
new_evidences[phi.id] = True
ggg = GGG.from_ggg_and_evidence(ggg, new_evidences)
# print ggg.entry_for_factor(factor)
cost, entries = cf.compute_costs([factor],
ggg,
state_sequence=None)
costs[j][i] = math.exp(-1.0 * cost)
#annotations.append(((x,y), entries))
print i
# break
print 'min/max', min(costs.flatten()), max(costs.flatten())
featureBrowser.setCostImage(costs, xmin, xmax, ymin, ymax)
return ggg