def testPath(self):
pobj = PhysicalObject(
Prism.from_points_xy(tp([(0, 0), (1, 0), (1, 1), (0, 1)]), 0,
3), ["tires"],
path=Path.from_xyztheta([0, 1, 2],
tp([(3, 3, 0, 0), (3, 3, 0, math.pi / 4),
(4, 4, 1, math.pi / 4)])))
self.assertEqual(pobj.prismAtT(0), pobj.prism)
aeq(pobj.path.locationAtT(1), (3, 3, 0, math.pi / 4))
self.assertEqual(
pobj.prismAtT(1),
Prism.from_points_xy(
array([[0.5, 1.20710678, 0.5, -0.20710678],
[-0.20710678, 0.5, 1.20710678, 0.5]]), 0.0, 3.0))
aeq(pobj.path.locationAtT(2), (4, 4, 1, math.pi / 4))
self.assertEqual(
pobj.prismAtT(2),
Prism.from_points_xy(
array([[1.5, 2.20710678, 1.5, 0.79289322],
[0.79289322, 1.5, 2.20710678, 1.5]]), 1.0, 4.0))
aeq(pobj.path.locationAtT(-1),
pobj.path.locationAtT(len(pobj.path.timestamps)))
def drawGroundingCostMap(featureBrowser, physicalObject, esdc, annotation,
beam_search, xmin, xmax, ymin, ymax, step):
obj = physicalObject
Xs = obj.X - obj.X[0]
Ys = obj.Y - obj.Y[0]
zStart = obj.prism.zStart
zEnd = obj.prism.zEnd
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)):
prism = Prism([Xs + x, Ys + y], zStart, zEnd)
new_object = PhysicalObject(prism, obj.tags)
new_annotation = annotation_copy(annotation)
new_annotation.setGrounding(esdc, new_object)
cost, entries = beam_search.cf_obj.costEntry([esdc],
new_annotation)
# print j, i, len(costs), len(costs[j])
costs[j][i] = math.exp(-1.0 * cost)
annotations.append(((x, y), entries))
featureBrowser.setCostImage(costs, annotations, xmin, xmax, ymin, ymax)
def annotation_candidate(esdc_structure, esdc_field_to_texts, groundings,
test_grounding):
esdc_candidate = make_esdc_candidate(esdc_structure, esdc_field_to_texts)
annotation = Annotation("test", esdc_candidate)
esdc_candidate = esdc_candidate[0]
annotation.setGrounding(esdc_candidate, test_grounding)
for field in ExtendedSdc.fieldNames:
if not esdc_candidate.childIsEmpty(field):
child = esdc_candidate.children(field)[0]
while True:
grounding = random.choice(groundings)
if not isinstance(grounding, PhysicalObject):
continue
else:
break
annotation.setGrounding(child, grounding)
if isinstance(test_grounding, PhysicalObject):
annotation.agent = test_grounding
else:
annotation.agent = PhysicalObject(Prism(
tp([(0, 0), (1, 0), (1, 1), (0, 1)]), 0, 1),
tags=["agent"],
path=test_grounding)
return annotation, esdc_candidate
def testGroundings(self):
corpus = annotationIo.load(SOURCE_FILE)
annotation = corpus[0]
esdc = annotation.flattenedEsdcs[0]
annotation.addGrounding(
esdc,
PhysicalObject(
Prism.from_points_xy(tp([(0, 0), (1, 0), (1, 1), (0, 1)]), 3,
4), ["tire", "pallet"]))
annotation.addGrounding(
esdc,
Place(
Prism.from_points_xy(tp([(0, 0), (1, 0), (1, 1), (0, 1)]), 3,
4)))
annotation.addGrounding(
esdc,
Path.from_xyztheta(timestamps=[0, 1],
points_xyztheta=pts_to_xyzTheta([(0, 0),
(1, 1)])))
yamlCorpus = annotationIo.toYaml(corpus)
print "yaml", yamlCorpus
newCorpus = annotationIo.fromYaml(yamlCorpus)
esdc1 = corpus[0].flattenedEsdcs[0]
esdc2 = newCorpus[0].flattenedEsdcs[0]
null_ids(esdc1)
null_ids(esdc2)
self.assertEqual(esdc1, esdc2)
def testRotateAwayFromOrigin(self):
pobj = PhysicalObject(
Prism.from_points_xy(tp([(0, 0), (1, 0), (1, 1), (0, 1)]), 0,
3), ["tires"],
path=Path.from_xyztheta([0, 1],
tp([(3, 3, 0, 0),
(3, 3, 0, math.pi / 4)])))
aeq(pobj.centroid2d, (0.5, 0.5))
newp = pobj.prismAtT(1)
self.assertEqual(
newp,
Prism.from_points_xy(
array([[0.5, 1.20710678, 0.5, -0.20710678],
[-0.20710678, 0.5, 1.20710678, 0.5]]), 0.0, 3.0))
aeq(newp.centroid2d(), (0.5, 0.5))
def generate_path_heat_map(esdcs, cf, output_fname=None, title=None):
path_esdc = esdcs[0]
landmark_esdc = path_esdc.l[0]
agent = PhysicalObject(Prism.from_points_xy(
tp([(-1, -1), (1, -1), (1, 1), (-1, 1)]), 0, 2),
tags=("robot", ),
lcmId=-1)
landmark = PhysicalObject(Prism.from_points_xy(
tp([(9, -1), (10, -1), (10, 1), (9, 1)]), 0, 2),
tags=("square", ),
lcmId=3)
context = Context.from_groundings([agent, landmark])
print 'path', path_esdc
gggs = gggs_from_esdc_group(esdcs)
ggg = gggs[0]
context.agent = agent
ggg.context = context
path_factor = ggg.esdc_to_factor(path_esdc)
path_node = ggg.node_for_esdc(path_esdc)
landmark_node = ggg.node_for_esdc(landmark_esdc)
ggg.set_evidence_for_node(landmark_node, [landmark])
probs, xstart, ystart = path_probabilities(cf, path_factor, path_node, ggg,
-5, 20, -10, 10, 1)
print "starting draw"
draw(probs,
-5,
20,
-10,
10,
xstart,
ystart,
agent,
landmark,
output_fname=output_fname,
title=esdcs.entireText)
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 annotation_copy(annotation):
objectGroundings = []
for esdc in annotation.flattenedEsdcs:
groundings = annotation.getGroundings(esdc)
objectGroundings.append(copy.deepcopy(groundings))
newAnnotation = Annotation(0, annotation.esdcs, objectGroundings)
a = annotation.agent
newAnnotation.agent = PhysicalObject(a.prism, a.tags, a.path, a.lcmId)
newAnnotation.priorAnnotation = annotation.priorAnnotation
newAnnotation.context = annotation.context
return newAnnotation
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 testRotateAroundOrigin(self):
pobj = PhysicalObject(Prism.from_points_xy(
tp([(-0.5, -0.5), (0.5, -0.5), (0.5, 0.5), (-0.5, 0.5)]), 0, 3),
["tires"],
path=Path.from_xyztheta([0, 1],
tp([(3, 3, 0, 0),
(3, 3, 0,
math.pi / 4)])))
self.assertEqual(pobj.centroid2d, [0, 0])
print "prism"
self.assertEqual(
pobj.prismAtT(1),
Prism.from_points_xy(
array([[
-5.55111512e-17, 7.07106781e-01, 5.55111512e-17,
-7.07106781e-01
],
[
-7.07106781e-01, -5.55111512e-17, 7.07106781e-01,
5.55111512e-17
]]), 0.0, 3.0))
def initialize_ggg_context(state, ggg, state_object=None):
"""
Set the context and agent for a ggg.
"""
ax, ay = state.getPosition()
agent_prism = prism_from_point(ax, ay, 0, 1)
agent_object = PhysicalObject(
agent_prism, [],
Path.from_xyztheta([1], [[ax], [ay], [0], [state.orientation]]),
lcmId=state.getAgentId())
context = state.to_context()
context.agent = agent_object
ggg.context = context
return ggg
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 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 from_lcm(agent, orientation, pallets, has_pallet, objects, actionMap):
from load_from_lcm import getLabel, physicalObject
new_state = ForkState()
new_state.currentNode = agent
new_state.orientation = orientation
new_state.pallet_ids = [p.id for p in pallets]
new_state.object_ids = [o.id for o in objects if not 'wheel' in getLabel(o)]
if has_pallet:
new_state.has_pallet_id = has_pallet.id
else:
new_state.has_pallet_id = None
new_state.actionMap = actionMap
for obj in pallets+objects:
new_state.groundableDict[obj.id] = physicalObject(obj)
obj_places = [new_state.getGroundableById(oid).centroid3d for oid in new_state.object_ids]
loc_places = [(l[0], l[1], 0) for l in new_state.actionMap.list_locations()]
id_counter = max([0]+new_state.getObjectsSet()) + 1
new_state.place_ids = []
for x,y,z in obj_places+loc_places:
new_state.place_ids.append(id_counter)
new_state.groundableDict[id_counter] = Place(prism_from_point(x,y,
z+.2,
z+.5))
id_counter += 1
ax, ay = new_state.getPosition()
new_state.groundableDict[State.AGENT_ID] = \
PhysicalObject(prism_from_point(ax, ay, 0, 1),
path=Path.from_xyztheta([1], [[ax],[ay],[0],[new_state.orientation]]),
lcmId = State.AGENT_ID, tags=['forklift'])
#Legacy fields, should not be used other than in repr
new_state.has_pallet = has_pallet
return new_state
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
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 physicalObject(obj, path=None):
prism = fromLcmObject(obj)
obj = PhysicalObject(prism, getLabel(obj).split(), path, lcmId=obj.id)
return obj