def object_landmark_features(self, agent, f_prism, r_words, l_prism):
result_dict = {}
assert not isnan(l_prism.points_xy[0][0])
assert not isnan(f_prism.points_xy[0][0])
prism_dict = compute_fdict(sf.sfe_f_prism_l_prism_names(),
sf.sfe_f_prism_l_prism(f_prism, l_prism, normalize=True))
result_dict = merge(result_dict, prism_dict)
ax, ay, agent_theta = agent.path.points_xytheta
for name, theta in [("avs_theta_start", agent_theta[0]), ("avs_theta_end", agent_theta[-1]),
("avs_theta_avg", na.mean(agent_theta))]:
avs_dict = compute_fdict(sf.spatial_features_names_avs_polygon_polygon(),
sf.spatial_features_avs_polygon_polygon(f_prism.points_xy,
l_prism.points_xy,
theta))
result_dict = merge(result_dict, add_prefix(name, avs_dict))
#print "******************"
#theta = agent_theta[0]
theta = agent_theta[0]
#print "agent theta", degrees(theta)
#print "f_centroid", f_prism.centroid2d()
#print "l_centroid", l_prism.centroid2d()
if not array_equal(f_prism.centroid2d(), l_prism.centroid2d()):
angle_btwn_points = sf.math2d_angle(na.array(f_prism.centroid2d()) - na.array(l_prism.centroid2d()))
#print "angle between points", degrees(angle_btwn_points)
angle = theta - angle_btwn_points - math.pi/4
#print "angle", degrees(angle)
quadrant = sf.math2d_angle_to_quadrant(angle)
octant = sf.math2d_angle_to_octant(angle + math.pi/8)
#print "quadrant", quadrant
#print "******************"
else:
quadrant = -1
octant = -1
#result_dict["f_in_l_quadrant_%d" % quadrant] = 1
result_dict["f_in_l_quadrant"] = quadrant
for i in range(-1, 8):
result_dict["f_in_l_octant_%d" % i] = 0
result_dict["f_in_l_octant_%d" % octant] = 1
result_dict = dict((f, v) for f, v in result_dict.iteritems() if (("avsg" not in f) and
("avsHeightExp" not in f) and
("avsResult" not in f)))
result_dict = add_word_features(result_dict, r_words)
return result_dict
def getSuccessors(self, groundings=[]):
states = []
obstacles_2d = [self.getGroundableById(obj).prismAtT(-1) for obj in self.getObjectsSet()]
# null action
a = None
s = ForkState.copy(self)
s.action_sequence.append((self, None))
states.append((s, a, [s.agent]))
# motions
here = self.actionMap.index_to_location(self.currentNode)
for loc in self.actionMap.neighbors_by_index(self.currentNode):
x,y = self.actionMap.index_to_location(loc)
if not array_equal((x, y), here):
if not any(sf.math2d_is_interior_point((x, y), o.points_xy)
for o in obstacles_2d):
a = Move(here, (x,y))
s, mg = a.execute(self)
states.append((s, a, mg))
# manipulations
if self.has_pallet_id:
places = sorted(self.getPlacesSet())
for p in places:
x,y,z = self.getGroundableById(p).prismAtT(-1).centroid3d()
a = PutDown((x,y,z+1), self.has_pallet_id)
s, mg = a.execute(self)
states.append((s, a, mg))
else:
for pid in sorted(self.pallet_ids):
# check if pallet has grounding
#if not pid in [g.id for g in groundings]:
# continue
a = PickUp(pid)
s, mg = a.execute(self)
states.append((s, a, mg))
return states
def __eq__(self, other):
if not isinstance(other, Quaternion):
return False
return array_equal(self.q, other.q)
def __eq__(self, other):
if not isinstance(other, Path):
return False
return (array_equal(self.points_xyztheta, other.points_xyztheta)
and array_equal(self.timestamps, other.timestamps))
def __eq__(self, other):
if not isinstance(other, Prism):
return False
return (array_equal(self.lower_points_xyz, other.lower_points_xyz)
and array_equal(self.upper_points_xyz, other.upper_points_xyz))