def execute(self, in_state, tstep=1):
modified_groundings = []
next_state = ForkState.copy(in_state)
next_state.currentNode = next_state.actionMap.nearest_index(self.location)
placed_pallet_id = in_state.has_pallet_id
next_state.has_pallet_id = None
#propogate trajectories using rrt
agent = next_state.agent
sx,sy,sz,stheta = transpose(agent.path.points_xyztheta)[-1]
start_t = agent.path.timestamps[-1]+tstep
fx,fy,z = self.location
ftheta = arctan2(fy - sy, fx - sx)
path = [[fx, fy, ftheta]]
pX, pY, pTheta = transpose(path)
end_t = tstep*len(pX)+start_t
pTime = arange(start_t, end_t, tstep)
apath = Path.from_xyztheta(pTime, [pX, pY, zeros(len(pX)), pTheta])
new_agent = next_state.agent.withExtendedPath(apath)
next_state.groundableDict[new_agent.id] = new_agent
modified_groundings.append(new_agent)
cx,cy,cz = self.location
ppath = Path.from_xyztheta([end_t], [[cx],[cy],[cz],[0]])
new_pallet = next_state.getGroundableById(placed_pallet_id).withExtendedPath(ppath)
next_state.groundableDict[new_pallet.id] = new_pallet
modified_groundings.append(new_pallet)
next_state.action_sequence.append((in_state, self))
return next_state, modified_groundings
def execute(self, in_state, tstep=1):
next_state = ForkState.copy(in_state)
next_state.has_pallet_id = self.pallet_id
modified_groundings = []
agent = next_state.agent
sx,sy,sz,stheta = transpose(agent.path.points_xyztheta)[-1]
start_t = agent.path.timestamps[-1]+tstep
fx,fy,z,th = transpose(next_state.getGroundableById(self.pallet_id).path.points_xyztheta)[-1]
ftheta = arctan2(fy - sy, fx - sx)
path = [[fx, fy, ftheta]]
pX, pY, pTheta = transpose(path)
end_t = tstep*len(pX)+start_t
pTime = arange(start_t, end_t, tstep)
apath = Path.from_xyztheta(pTime, [pX, pY, zeros(len(pX)), pTheta])
new_agent = next_state.agent.withExtendedPath(apath)
next_state.groundableDict[new_agent.id] = new_agent
modified_groundings.append(new_agent)
ppath = Path.from_xyztheta([end_t], [[fx],[fy],[1],[0]])
new_pallet = next_state.getGroundableById(self.pallet_id).withExtendedPath(ppath)
next_state.groundableDict[new_pallet.id] = new_pallet
modified_groundings.append(new_pallet)
next_state.currentNode = next_state.actionMap.nearest_index((fx,fy,z))
next_state.action_sequence.append((in_state, self))
return next_state, modified_groundings
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 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 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 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 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 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 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 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 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 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 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 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