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 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 addPath(self):
annotation = self.annotationModel.selectedAnnotation()
esdc = self.esdcModel.selectedEsdc()
timestamps = [0 for p in self.currPath]
points_xyztheta = [tp(self.currPath)[0], tp(self.currPath)[1], [0 for p in self.currPath], [0 for p in self.currPath]]
path = Path(timestamps, points_xyztheta)
annotation.addGrounding(esdc, path)
self.groundingsModel.setData(annotation.getGroundings(esdc))
self.pathNodes = {}
self.currPath = []
self.drawForPath()
def addAgentPathSegments(self):
groundings = self.pathSegmentsModel.selectedGroundings()
timestamps = []
points_ptsztheta = []
for path in groundings:
timestamps.extend(path.timestamps)
points_ptsztheta.extend(path.points_ptsztheta)
path = Path(timestamps, tp(points_ptsztheta))
annotation = self.annotationModel.selectedAnnotation()
agent = annotation.agent
prism = agent.prismAtT(path.start_t)
pobj = PhysicalObject(prism, agent.tags, path, agent.id)
self.annotateGrounding(pobj)
def main():
import basewindow
app = basewindow.makeApp()
win = MainWindow()
obj1 = PhysicalObject(Prism(tp([(0, 0), (1, 0), (1, 1), (0, 1)]), 0, 3),
["tires"],
path=Path([0, 1, 2],
tp([(3, 3, 0, 0), (3, 3, 0, math.pi / 4),
(4, 4, 1, math.pi / 4)])))
obj2 = PhysicalObject(Prism(tp([(2, 2), (3, 2), (3, 3), (2, 3)]), 0, 3),
["boxes"],
path=Path([0, 1, 2],
tp([(3, 3, 0, 0), (3, 3, 0, math.pi / 4),
(4, 4, 1, math.pi / 4)])))
obj3 = PhysicalObject(
Prism(tp([(5, 5), (10, 5), (10, 10), (5, 10)]), 0, 3), ["truck"])
context = Context([obj1, obj2, obj3], [])
win.show()
win.setContext(context)
sys.exit(app.exec_())
def drawPlaceCostMap(featureBrowser, physicalObject, esdc, annotation,
beam_search, xmin, xmax, ymin, ymax, step):
obj = physicalObject
Xs = obj.pX - obj.pX[0]
Ys = obj.pY - obj.pY[0]
zStart = obj.zStart
zEnd = obj.zEnd
cx, cy = obj.centroid2d()
dx, dy = cx - obj.pX[0], cy - obj.pY[0]
costs = na.zeros((int((ymax - ymin) / step), int((xmax - xmin) / step)))
annotations = []
print dx, dy
path = Path([0], [[(xmax - xmin) * 0.5], [(ymax - ymin) * 0.5], [0], [0]])
for i, x in enumerate(na.arange(xmin, xmax, step)):
for j, y in enumerate(na.arange(ymin, ymax, step)):
prism = Prism([Xs + x - dy, Ys + y - dx], zStart, zEnd)
place = Place(prism)
new_annotation = annotation_copy(annotation)
new_annotation.setGrounding(esdc, place)
new_annotation.setGroundingIsCorrect(esdc, True)
#new_annotation.agent.setPath(path)
state, gggs = annotation_to_ggg_map(new_annotation)
#ggg = ggg_from_esdc(new_annotation.esdcs[0])
ggg = gggs[esdc]
factor = ggg.esdc_to_factor(esdc)
new_evidences = ggg.evidences
for phi in factor.nodes_with_type("phi"):
new_evidences = new_evidences.set_evidence(phi.id, True)
ggg = GGG.from_ggg_and_evidence(ggg, new_evidences)
cost, entries = beam_search.cf_obj.costEntry([factor], state, ggg)
costs[j][i] = math.exp(-1.0 * cost)
annotations.append(((x, y), entries))
#axes.imshow(costs, origin="lower",
# extent=(xmin, xmax, ymin, ymax))
featureBrowser.setCostImage(costs, annotations, xmin, xmax, ymin, ymax)
def initial_annotation(state, esdc):
esdcs = ExtendedSdcGroup([esdc])
ax, ay = state.getPosition()
agent_prism = prism_from_point(ax, ay, 0, 1)
agent_object = PhysicalObject(agent_prism, [],
Path([1],
[[ax], [ay], [0], [state.orientation]]),
lcmId=state.getAgentId())
context = state.to_context()
context.agent = agent_object
annotation = Annotation(0, esdcs, context=context, agent=agent_object)
#peg figure of event to agent
fig = esdc.f[0]
annotation.setGrounding(fig, agent_object)
return annotation
def parse_log(rndf_fname, log_fname, wait_until_teleport=False):
app = App(rndf_fname)
try:
log = EventLog(log_fname, "r")
except:
print "can't read", log_fname
raise
pallets = dict()
for e in log:
if e.channel == "PALLET_LIST":
msg = pallet_list_t.decode(e.data)
for p in msg.pallets:
if not p.id in pallets:
pallets[p.id] = p
if p.relative_to_id == 0:
p.pos = ru.robot_pose_to_rndf(p.pos, app.trans_xyz,
app.trans_theta,
app.trans_latlon, app.rndf)
if e.channel == "GPS_TO_LOCAL":
app.on_transform_msg(e.channel, e.data)
#should have all pallets in the first position they were seen
app.curr_pallets = pallets
#now that pallets are initialized, restart
log = EventLog(log_fname, "r")
agent_path = []
object_paths = collections.defaultdict(lambda: list())
obj_id_to_pobj = {}
agent_prism = None
sample_frequency_hz = 1
last_sample_micros = None
timestamps = []
teleport_ts = None
for e in log:
if e.channel == "SIM_TELEPORT":
teleport_ts = e.timestamp
if (teleport_ts == None or e.timestamp - teleport_ts <
(1 * 1000 * 1000)):
if wait_until_teleport:
continue
if e.channel == "POSE":
app.on_pose_msg(e.channel, e.data)
elif e.channel == "PALLET_LIST":
app.on_pallet_msg(e.channel, e.data)
elif e.channel == "GPS_TO_LOCAL":
app.on_transform_msg(e.channel, e.data)
elif e.channel == "OBJECT_LIST":
app.on_objects_msg(e.channel, e.data)
if last_sample_micros == None:
last_sample_micros = e.timestamp
if (e.timestamp - last_sample_micros >=
(1.0 / sample_frequency_hz) * 1000 * 1000):
state, new_am = app.get_current_state()
x, y, z = app.curr_location
if agent_prism == None:
agent_prism = app.curr_prism
agent_path.append((x, y, z, app.curr_orientation))
if state != None:
last_sample_micros = e.timestamp
timestamps.append(e.timestamp)
for pobj_id in state.getObjectsSet():
if pobj_id in app.curr_objects:
lcm_obj = app.curr_objects[pobj_id]
elif pobj_id in app.curr_pallets:
lcm_obj = app.curr_pallets[pobj_id]
else:
raise ValueError()
o_vec = app.bot_quat_rotate(lcm_obj.orientation, (1, 0, 0))
orientation = math.atan2(o_vec[1], o_vec[0])
pobj = state.getGroundableById(pobj_id)
object_paths[pobj.lcmId].append(pobj.centroid3d +
(orientation, ))
if not pobj.lcmId in obj_id_to_pobj:
obj_id_to_pobj[pobj.lcmId] = pobj
lcm_parse = LcmParse()
lcm_parse.agent_obj = PhysicalObject(agent_prism,
tags=["forklift"],
path=Path(
timestamps,
points_xyztheta=tp(agent_path)),
lcmId=FORKLIFT_ID)
for obj_id, path in object_paths.iteritems():
if obj_id == FORKLIFT_ID:
continue # forklift weirdly appears here, but we deal with it separately.
pobj = obj_id_to_pobj[obj_id]
path = Path(timestamps, points_xyztheta=tp(object_paths[pobj.lcmId]))
pobj.path = path
pobj.updateRep()
lcm_parse.pobjs.append(pobj)
lcm_parse.object_id_to_path[pobj.lcmId] = path
lcm_parse.places = []
for place_id in app.curr_state.getPlacesSet():
lcm_parse.places.append(app.curr_state.getGroundableById(place_id))
lcm_parse.object_id_to_path[FORKLIFT_ID] = lcm_parse.agent_obj.path
return lcm_parse
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