def testSupports1(self):
f_prism = Prism.from_points_xy(
array([[16.26972295, 17.58320551, 16.85879986, 15.5453173],
[47.84974226, 48.57414791, 49.88763048, 49.16322482]]), 0.0,
2.0)
l_prism = Prism.from_points_xy(
array([[17.28494274, 16.78494273, 17.82417321, 18.32417321],
[48.70261888, 49.56864429, 50.16864428, 49.30261887]]),
0.27844635713715404, 0.98183174233715431)
mpl.gca().clear()
mpl.gca().set_aspect("equal")
#mpl.show()
features = compute_fdict(
sfe_f_prism_l_prism_names(),
sfe_f_prism_l_prism(f_prism, l_prism, normalize=True))
self.assertEqual(features["F_3dSupportsLandmarkFigure"], 0)
self.assertEqual(features["F_3dSupportsFigureLandmark"], 1)
self.assertEqual(features["F_3dIntersectsFigureLandmark"], 1)
self.assertEqual(features["F_3dEndsHigherThanLandmarkFigure"], 0)
self.assertEqual(features["F_3dEndsHigherThanFigureLandmark"], 1)
self.assertEqual(features["F_3dStartsHigherThanLandmarkFigure"], 1)
self.assertEqual(features["F_3dStartsHigherThanFigureLandmark"], 0)
def testPrism(self):
prism1 = Prism.from_points_xy(tp([(0, 0), (1, 0), (1, 1), (0, 1)]),
zStart=0,
zEnd=4)
prism2 = Prism.from_points_xy(tp([(0, 0), (1, 0), (1, 1), (0, 1)]),
zStart=4.1,
zEnd=5)
self.assertEqual(math3d_higher_than(prism2, prism2), False)
self.assertEqual(math3d_higher_than(prism1, prism1), False)
self.assertEqual(math3d_higher_than(prism1, prism2), False)
self.assertEqual(math3d_higher_than(prism2, prism1), True)
self.assertEqual(math2d_overlaps(prism1.points_xy, prism2.points_xy),
True)
self.assertEqual(math2d_overlaps(prism2.points_xy, prism1.points_xy),
True)
#print "points", prism1.points_xy
#print "points", prism1.points_xy[0]
features = sfe_f_prism_l_prism(prism1, prism2, normalize=True)
fnames = list(sfe_f_prism_l_prism_names())
self.assertEqual(len(fnames), len(features))
print fnames
print features
fmap = dict(zip(fnames, features))
self.assertEqual(fmap['F_3dEndsHigherThanFigureLandmark'], 0)
self.assertEqual(fmap['F_3dEndsHigherThanLandmarkFigure'], 1)
self.assertEqual(fmap['F_3dSupportsFigureLandmark'], 1)
self.assertEqual(fmap['F_3dSupportsLandmarkFigure'], 0)
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 testSupports2(self):
f_prism = Prism.from_points_xy(
array([[12.40321594, 11.89190306, 10.48174026, 10.99305313],
[50.60192082, 52.01208363, 51.50077075, 50.09060794]]), 0.0,
2.0)
l_prism = Prism.from_points_xy(
array([[10.77335704, 10.26535702, 11.3045877, 11.81258772],
[51.37659439, 52.25647628, 52.85647637, 51.97659448]]),
0.30952800960131738, 1.3382280096013166)
mpl.gca().clear()
mpl.gca().set_aspect("equal")
#mpl.show()
features = compute_fdict(
sfe_f_prism_l_prism_names(),
sfe_f_prism_l_prism(f_prism, l_prism, normalize=True))
self.assertEqual(features["F_3dSupportsLandmarkFigure"], 0)
self.assertEqual(features["F_3dSupportsFigureLandmark"], 1)
self.assertEqual(features["F_3dIntersectsFigureLandmark"], 1)
self.assertEqual(features["F_3dEndsHigherThanLandmarkFigure"], 0)
self.assertEqual(features["F_3dEndsHigherThanFigureLandmark"], 1)
self.assertEqual(features["F_3dStartsHigherThanLandmarkFigure"], 1)
self.assertEqual(features["F_3dStartsHigherThanFigureLandmark"], 0)
def testPolygonInsideAnother(self):
figure = Prism.from_points_xy(
array([[
7.69945323, 7.69945323, 10.97806236, 11.42204982, 8.47089291
], [
54.13234479, 54.13234479, 55.86027265, 54.38893454, 52.75711988
]]), 0.85000741680000036, 1.8500074168000005)
landmark = Prism.from_points_xy(
array([[8.20976285, 7.35040178, 12.13996339, 12.99932446],
[52.3373722, 54.04890265, 56.4537461, 54.74221565]]),
0.44360731519999963, 0.8500074167999998)
self.assertEqual(math2d_overlaps(figure.points_xy, landmark.points_xy),
True)
self.assertEqual(math2d_overlaps(landmark.points_xy, figure.points_xy),
True)
self.assertEqual(math3d_higher_than(figure, landmark), True)
self.assertEqual(math3d_higher_than(landmark, figure), False)
self.assertEqual(math3d_supports(figure, landmark), False)
self.assertEqual(math3d_supports(landmark, figure), True)
names = sfe_f_prism_l_prism_names()
feats_obs = sfe_f_prism_l_prism(figure, landmark, normalize=True)
features = dict(zip(names, feats_obs))
self.assertEqual(features["F_3dSupportsLandmarkFigure"], 1)
self.assertEqual(features["F_3dSupportsFigureLandmark"], 0)
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 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 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 testSupports(self):
f_prism = Prism.from_points_xy(
array([[12.19116674, 11.65687213, 10.8115738, 11.34586841],
[49.87727769, 50.72257602, 50.18828141, 49.34298308]]), 0.0,
2.0)
l_prism = Prism.from_points_xy(
array([[10.61577541, 10.11577543, 11.15500591, 11.6550059],
[50.24525845, 51.11128387, 51.71128384, 50.84525842]]),
0.30936596999170651, 1.0127513551917064)
mpl.gca().clear()
mpl.gca().set_aspect("equal")
#mpl.show()
features = compute_fdict(
sfe_f_prism_l_prism_names(),
sfe_f_prism_l_prism(f_prism, l_prism, normalize=True))
self.assertEqual(features["F_3dSupportsLandmarkFigure"], 0)
self.assertEqual(features["F_3dSupportsFigureLandmark"], 0)
self.assertEqual(features["F_3dIntersectsFigureLandmark"], 0)
def testSameFigureAndLandmark(self):
f_prism = Prism.from_points_xy(
array([[21.72099827, 21.22099828, 22.26022877, 22.76022875],
[40.789814, 41.65583942, 42.25583939, 41.38981397]]),
3.3273999996559184e-06, 0.70338871259999969)
l_prism = Prism.from_points_xy(
array([[21.72099827, 21.22099828, 22.26022877, 22.76022875],
[40.789814, 41.65583942, 42.25583939, 41.38981397]]),
3.3273999996559184e-06, 0.70338871259999969)
features = compute_fdict(
sfe_f_prism_l_prism_names(),
sfe_f_prism_l_prism(f_prism, l_prism, normalize=True))
#mpl.show()
self.assertEqual(features["F_3dSupportsLandmarkFigure"], 0)
self.assertEqual(features["F_3dSupportsFigureLandmark"], 0)
self.assertEqual(features["F_3dIntersectsFigureLandmark"], 1)
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 example_state():
robot = PhysicalObject(Prism.from_points_xy(tp([(0, 0), (1, 0), (1, 1), (0, 1)]),
0, 2),
tags=("robot",),
lcmId=DiaperState.AGENT_ID + 2)
caregiver = PhysicalObject(Prism.from_points_xy(tp([(3, 2), (4, 2), (4, 3), (3, 3)]),
0, 2),
tags=("caregiver",),
lcmId=DiaperState.AGENT_ID + 3)
child = PhysicalObject(Prism.from_points_xy(tp([(4, 4), (5, 4), (5, 5), (4, 5)]),
0, 0.5),
tags=("child",),
lcmId=DiaperState.AGENT_ID + 3)
objects = [
PhysicalObject(Prism.from_points_xy(tp([(-1, 1.1), (2, 1.1), (2, 3),
(-1, 3)]),
1, 1.25), tags=("table",), lcmId=3),
PhysicalObject(Prism.from_points_xy(tp([(0.5, 1.5), (0.75, 1.5),
(0.75, 1.75), (0.5, 1.75)]),
1.25, 1.3), tags=("wipes",),
lcmId=4),
PhysicalObject(Prism.from_points_xy(tp([(1, 2), (1.25, 2),
(1.25, 2.25), (1, 2.25)]),
1.25, 1.3), tags=("diaper",),
lcmId=5)]
return DiaperState(robot, caregiver, child, objects)
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 testNotOverlapping(self):
fend = Prism.from_points_xy(
array([[11.38086607, 11.11520079, 10.6916185, 10.95728378],
[50.18213065, 50.60571294, 50.34004767, 49.91646537]]), 0.0,
2.0)
lend = Prism.from_points_xy(
array([[8.81022941, 8.31022943, 9.34945991, 9.8494599],
[52.6054371, 53.47146252, 54.07146249, 53.20543707]]),
0.81463427695147927, 1.5180196621514792)
features = compute_fdict(
sfe_f_prism_l_prism_names(),
sfe_f_prism_l_prism(fend, lend, normalize=True))
self.assertEqual(math3d_higher_than(fend, lend), True)
self.assertEqual(math3d_higher_than(lend, fend), False)
self.assertEqual(math2d_overlaps(fend.points_xy, lend.points_xy),
False)
self.assertEqual(math3d_intersect_prisms(fend, fend), True)
self.assertEqual(features["F_3dSupportsLandmarkFigure"], 0)
self.assertEqual(features["F_3dSupportsFigureLandmark"], 0)
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 makePlace(self):
zStart = None
points_pts = []
for x, y, z in self.place_footprint:
if zStart == None:
zStart = z
else:
assert z == zStart, (z, zStart)
points_pts.append((x, y))
if zStart == None:
zStart = 0
place = Place(
Prism.from_points_xy(tp(points_pts),
zStart=zStart,
zEnd=zStart + self.placeHeight))
return place
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 fromLcmObject(obj):
#x0,y0,z0 = [obj.pos[i] + obj.bbox_min[i] for i in range(3)]
#x1,y1,z1 = [obj.pos[i] + obj.bbox_max[i] for i in range(3)]
x0, y0, z0 = [obj.bbox_min[i] for i in range(3)]
x1, y1, z1 = [obj.bbox_max[i] for i in range(3)]
if len(obj.orientation) != 0:
m = quat_pos_to_matrix(obj.orientation, obj.pos)
points = na.array([(x0, y0, z0, 1), (x0, y1, z0, 1), (x1, y1, z0, 1),
(x1, y0, z1, 1)])
points_xyz = na.transpose([na.dot(m, p) for p in points])[0:3]
X, Y, Z = points_xyz
else:
xs, ys, zs = obj.pos
points_xyz = na.transpose([(xs + x0, ys + y0, zs + z0),
(xs + x0, ys + y1, zs + z0),
(xs + x1, ys + y1, zs + z0),
(xs + x1, ys + y0, zs + z0)])
#(x1, y1, z1)])
Z = [z0, z1]
return Prism.from_points_xy(points_xyz[0:2], min(Z), max(Z))
def on_pose_msg(self, channel, data):
msg = pose_t.decode(data)
x, y, z = msg.pos[0:3]
self.curr_location = self.pos_to_location(msg.pos[0:3])
#p = self.curr_location
#print p[0], p[1] ,self.closest_map_location(self.curr_location)
#compute orientation in rndf frame
o_vec = self.bot_quat_rotate(msg.orientation, (1, 0, 0))
robot_orientation = math.atan2(o_vec[1], o_vec[0])
self.curr_orientation = robot_orientation - self.trans_theta
x, y, z = self.curr_location
bounding_polygon = [(x - 0.75, y - 0.75), (x + 0.75, y - 0.75),
(x + 0.75, y + 0.75), (x - 0.75, y + 0.75)]
cx, cy = sf.math2d_centroid(tp(bounding_polygon))
af = AffineMatrix()
af.translate(cx, cy)
af.rotate(self.curr_orientation)
af.translate(-cx, -cy)
self.curr_prism = Prism(
tp([af.transformPt(p) for p in bounding_polygon]), 0, 2)
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 prism_from_point(x, y, z1, z2):
return Prism.from_points_xy([(x - 1, x + 1, x + 1, x - 1),
(y - 1, y - 1, y + 1, y + 1)], z1, z2)