def is_in_triangle(self, p1, p2, p3):
o1 = GeoTools.orientation(self, p2, p1)
o2 = GeoTools.orientation(self, p3, p2)
o3 = GeoTools.orientation(self, p1, p3)
all_CCW = o1 == 2 and o2 == 2 and o3 == 2
all_CW = o1 == 1 and o2 == 1 and o3 == 1
return all_CCW or all_CW
def get_observation_matrices(lines_seen_now, env, n, actual_err=False):
C = np.empty((0, n))
v = np.zeros(n)
b_half = []
b_actual = []
w = []
for rectangle_idx, lines in lines_seen_now.items():
rectangle = env.rectangles[rectangle_idx]
for line_idx in lines:
line = rectangle.edges["full"][line_idx]
v[:2] = GeoTools.get_normal_vect(line)
mid_point = GeoTools.get_mid_point(line).as_array()
center_point = rectangle.get_center_gen()[0]
# Make sure v is pointed outward wrt rectangle surface
if (mid_point - center_point).dot(v) < 0:
v = -v
C = np.vstack((C, v))
half_wdth = rectangle.half_widths[line_idx]
if actual_err:
b_actual.append(
-(mid_point + rectangle.actual_errors[line_idx] * v[:2]).dot(
v[:2]
)
)
b_half.append(
-(mid_point + rectangle.center_offsets[line_idx] * v[:2]).dot(v[:2])
)
w.append(half_wdth)
if actual_err:
return C, np.array(b_actual), np.array(b_half), np.array(w)
else:
return C, np.array(w)
def is_valid_rectangle(self, rectangle, verbose=True):
"""Return True if rectangle is a valid region in the environment, assuming
by the "worst" (i.e. most inflated) line configurations.
Args:
rectangle (Rectangle)
"""
return GeoTools.is_valid_region(rectangle.as_poly["worst"], self, verbose)
def add_obstacles(self, num_obs, lgth, wdth, ang=None, max_iter=1e3):
"""Add num_obs obstacles of certain length and width, within
the limits of the current environment.
If ang is not specified, will pick a random rectangle angle.
"""
max_id = max(self.rectangles)
added = 0
it = 0
while added < num_obs:
it += 1
if it > max_iter:
print("Could fit {} such rectangles in the environment.".format(added))
break
x, y = GeoTools.sample_in_range(self.x_range)
if ang is None:
rectangle = Rectangle(
max_id + added + 1, x, y, lgth, wdth, np.random.uniform(0, 360)
)
else:
rectangle = Rectangle(max_id + added + 1, x, y, lgth, wdth, ang)
try:
assert rectangle.as_poly <= self.as_rectangle().as_poly
self.add_rectangle(rectangle, verbose=False)
except AssertionError:
continue
added += 1
def is_valid_goal(self, config="worst", verbose=True):
try:
return GeoTools.is_valid_region(self.goal_region,
self.graph.env,
verbose,
config=config)
except AttributeError:
print("Please set the goal region using .set_goal(goal_region).")
def is_valid(self):
for i in range(len(self.edges) - 2):
for j in range(i + 2, len(self.edges) - (i == 0)):
edge_i = self.edges[i]
edge_j = self.edges[j]
if GeoTools.doIntersect(edge_i[0], edge_i[1], edge_j[0],
edge_j[1]):
return False
return True
def is_intersected(self, line, config):
if config is not None:
edges = self.edges[config]
else:
edges = self.edges
for edge in edges:
if GeoTools.doIntersect(edge[0], edge[1], line[0], line[1]):
return True
return False
def is_intersected(self, line, config="worst"):
"""Return True if self is intersected by a line.
Args:
line (list of Point): line to consider for intersection with self.
config (str): rectangle configuration to check line intersection for,
defaults to "worst", but other options are "best" or "full".
"""
for edge in self.edges[config]:
if GeoTools.doIntersect(edge[0], edge[1], line[0], line[1]):
return True
return False
def plot(self,
show_id=True,
show_edges_id=True,
as_goal=False,
mask=False):
if not as_goal:
if mask:
plt.plot(
self.x_list + [self.x_list[0]],
self.y_list + [self.y_list[0]],
color="r",
alpha=0.75,
)
else:
plt.fill(
self.x_list,
self.y_list,
alpha=0.5,
edgecolor="r",
linestyle="-",
label=self.id,
)
if show_edges_id:
if isinstance(self.edges, dict):
edges = self.edges["original"]
else:
edges = self.edges
for edge_idx in range(len(edges)):
edge = edges[edge_idx]
mid_point = GeoTools.get_mid_point(edge)
plt.annotate(
str(edge_idx),
[mid_point.x, mid_point.y],
fontsize=10,
alpha=0.75,
)
annot = str(self.id)
color = "k"
else:
plt.fill(
self.x_list,
self.y_list,
alpha=0.5,
facecolor="lightsalmon",
linestyle="-",
edgecolor="red",
)
annot = "G"
color = "r"
plt.annotate(annot,
self.get_center_gen()[0],
fontsize=12,
weight="bold",
color=color)
def __init__(self, id_, vertices, plot_sty="ggplot"):
try:
assert isinstance(id_, int)
self.id = id_
self.vertices = vertices
self.edges = GeoTools.vertices2edges(vertices)
assert self.is_valid()
self.x_list, self.y_list = map(
list, zip(*[(v.x, v.y) for v in self.vertices]))
mpl.style.use(plot_sty)
except AssertionError:
print("Polygon is not valid.")
raise
def update_stored_configs(self):
"""Update the vertices, edges, zonotopes and polytopes corresponding to five
configurations:
"original": as per the very first instantiation of the object.
"full": considering centered errors.
"worst": considering most inflated (i.e. positive extreme) errors.
"best": considering least inflated (i.e. negative extreme) errors.
"actual": considering actual errors.
"""
self.vertices = {
"original": self.vertices_original,
"full": self.get_vertices("full"),
"worst": self.get_vertices("worst"),
"best": self.get_vertices("best"),
"actual": self.get_vertices("actual"),
}
self.edges = {
"original": self.edges_original,
"full": GeoTools.vertices2edges(self.vertices["full"]),
"worst": GeoTools.vertices2edges(self.vertices["worst"]),
"best": GeoTools.vertices2edges(self.vertices["best"]),
"actual": GeoTools.vertices2edges(self.vertices["actual"]),
}
self.as_zono = {}
self.as_zono = {
"original": self.to_zonotope(),
"full": self.to_zonotope("full"),
"worst": self.to_zonotope("worst"),
"best": self.to_zonotope("best"),
"actual": self.to_zonotope("actual"),
}
self.as_poly = {}
self.as_poly = {
"original": self.to_poly(),
"full": self.to_poly("full"),
"worst": self.to_poly("worst"),
"best": self.to_poly("best"),
"actual": self.to_poly("actual"),
}
def get_vertices(self, config, as_array=False):
"""Return the four vertices given a config.
Args:
config (str or bitarray):
None corresponds to the non-centered rectangle.
"full" corresponds to the centered rectangle.
"actual" corresponds to the actual rectangle.
bitarray(b1,b2,b3,b4) where the b's are binary values corresponds to
the rectangle with the left (0) or right (1) extremes for each line.
"worst" is mapped to bitarray('1111').
"best" is mapped to bitarray('0000').
"""
vertices_config = self.vertices_original[:]
if config is not None:
for edge_idx in range(len(self.edges_original)):
edge = self.edges_original[edge_idx]
p1, p2 = edge
v = GeoTools.get_normal_vect(edge)
v = Point(v[0], v[1])
p3, p4 = self.edges_original[(edge_idx + 1) % 4]
# Make sure v is pointed outward wrt rectangle surface
if (p2 - p4).dot(v) < 0:
v = -v
if config == "full":
w = self.center_offsets[edge_idx]
elif config == "actual":
w = self.actual_errors[edge_idx]
else:
if not isinstance(config, frozenbitarray):
config = CONFIG2BIT[config]
w = self.error_bounds[edge_idx, int(config[edge_idx])]
vertices_config[edge_idx] += w * v
vertices_config[(edge_idx + 1) % 4] += w * v
if as_array:
return GeoTools.vertices2array(vertices_config)
return vertices_config
def sample_free(self, n, config, goal_region=None):
"""Sample n nodes in the free space."""
num_added = 0
new_samples = []
while num_added < n:
try:
sample = GeoTools.sample_in_range(self.x_range)
if num_added == 0 and goal_region is not None:
if not goal_region.contains(sample):
raise NotInGoalError
if not self.env.contains(self.fromarray(sample), config):
new_samples.append(sample)
num_added += 1
except NotInGoalError:
pass
self.add_sample(new_samples)
self.update_kdtree()
def get_vertices(self, as_array=True):
if not as_array:
return self.vertices
else:
return GeoTools.vertices2array(self.vertices)