def random_robot_problem(methods):
print 'Random Robot Problem'
robot = Robot([ \
Polygon([Point(-2, -1), Point(2, -1), Point(2, 1), Point(-2, 1)]),
Polygon([Point(-1, 1), Point(1, 1), Point(0, 2)])
])
start = (5, 5, 0)
goal = (15, 15, pi / 2)
cspace = ConfigurationSpace([ \
Range(0, 20), \
Range(0, 20), \
Range(-pi, pi, wrap_around = True)
], start, robot.distance, 3*[0.25])
obstacles = []
problem = Problem(20,
20,
robot,
obstacles,
start,
goal,
cspace,
display_tree=True)
for i in range(3):
problem.generate_random_poly((3, 15), (1, 4))
for method in methods:
print 'Method', method
problem.run_and_display(method, DISPLAY)
def robot_problem(methods):
print 'Robot Problem'
robot = Robot([ \
Polygon([Point(-2, -1), Point(2, -1), Point(2, 1), Point(-2, 1)]),
Polygon([Point(-1, 1), Point(1, 1), Point(0, 2)])
])
start = (5.0, 5.0, 0)
goal = (15.0, 15.0, pi / 2)
cspace = ConfigurationSpace([ \
Range(0, 20), \
Range(0, 20), \
Range(-pi, pi, wrap_around = True)
], start, robot.distance, 3*[0.25])
obstacles = [
Object(
Point(10, 10),
[Polygon([Point(0, -2),
Point(2, 0),
Point(0, 2),
Point(-2, 0)])])
]
problem = Problem(20,
20,
robot,
obstacles,
start,
goal,
cspace,
display_tree=True)
for method in methods:
print 'Method', method
problem.run_and_display(method, DISPLAY)
def test_from_normal_and_point():
corners1 = [
Point(0.0, 0.0, 0.0),
Point(1.0, 0.0, 0.0),
Point(1.0, 1.0, 0.0),
Point(0.0, 1.0, 0.0)
]
center1 = Point(0.5, 0.5, 0.0)
polygon1 = Polygon(corners1, center1)
plane1 = Plane.from_normal_and_point(polygon1.plane().normal(), center1)
assert polygon1.plane(
) == plane1 # tests the reduced normal form coefficients.
corners2 = [
Point(0.0, 0.0, 0.0),
Point(0.0, 1.0, 0.0),
Point(0.0, 1.0, 1.0),
Point(0.0, 0.0, 1.0)
]
center2 = Point(0.0, 0.5, 0.5)
polygon2 = Polygon(corners2, center2)
plane2 = Plane.from_normal_and_point(polygon2.plane().normal(), center2)
assert polygon2.plane() == plane2
corners4 = [
Point(1.0, 0.0, 0.0),
Point(1.0, 1.0, 0.0),
Point(1.0, 1.0, 1.0),
Point(1.0, 0.0, 1.0)
]
center4 = Point(1.0, 0.5, 0.5)
polygon4 = Polygon(corners4, center4)
plane4 = Plane.from_normal_and_point(polygon4.plane().normal(), center4)
assert polygon4.plane() == plane4
def example_2(resolution: int = 100, save: bool = False):
obstacle1 = Polygon(np.array([[20, 20], [60, 20], [20, 60]]))
obstacle2 = Polygon(np.array([[80, 80], [60, 80], [80, 60]]))
grid = BrushfireGrid(resolution, [obstacle1, obstacle2],
animate_calculation=save,
results_name='Example2')
grid.plot_distances(save=save)
grid.plot_voronoi_boundary(save=save)
grid.plot_voronoi_diagram(save=save)
plt.show()
def test_point_is_in_square_polygon(self):
square = Polygon(make_square_vertices(side_length=2, center=(0, 0)))
# Assert
expected = True
actual = square.point_in_convex_polygon((0, 0))
self.assertEqual(expected, actual)
def get_polygon_data(G):
"""Extract polygon data from OGR geometry
:param G: OGR polygon geometry
:return: List of InaSAFE polygon instances
"""
# Get outer ring, then inner rings
# http://osgeo-org.1560.n6.nabble.com/
# gdal-dev-Polygon-topology-td3745761.html
number_of_rings = G.GetGeometryCount()
# Get outer ring
outer_ring = get_ring_data(G.GetGeometryRef(0))
# Get inner rings if any
inner_rings = []
if number_of_rings > 1:
for i in range(1, number_of_rings):
inner_ring = get_ring_data(G.GetGeometryRef(i))
inner_rings.append(inner_ring)
# Return Polygon instance
return Polygon(outer_ring=outer_ring,
inner_rings=inner_rings)
def calc_all_vertex(vertex, faces, tr_matrix):
#new_vertex_faces will be something like [polygon1,polygon2,polygon3,...,polygonN]
new_vertex_faces = []
size = len(faces)
#faces will be something like [[0,1,2,3],[5,4,7,6]]
for f in faces:
#f something like [0,1,2,3]
fp = []
f_index = faces.index(f)
for v in f:
tr_matrix_i = tr_matrix[f_index]
#it's necessary transpose the matrix
tr_matrix_i = tr_matrix_i.transpose()
tr_matrix_i = tr_matrix_i.tolist()
#new_vertex is new vertex coordinates with transformations
new_vertex = calc_vertex(tr_matrix_i, vertex[v])
p = Point(new_vertex[0], new_vertex[1], new_vertex[2])
fp.append(p)
face = Polygon(fp)
new_vertex_faces.append(face)
return new_vertex_faces
def load_test_polygon(filename: str) -> Tuple[Polygon, List[Tuple[int, int]]]:
"""Load a polygon and start/end point data from a file."""
polygon_data = []
point_data = []
with open(filename, 'r') as f:
polygon = True
for line in f:
if line.startswith('%'):
polygon = False
continue
if polygon:
polygon_data.append(line.strip().split(' '))
else:
point, rest = line.strip().split('<')
edge, visible = rest.split(':')
point = int(point)
edge = int(edge)
visible = int(visible) == 1
point_data.append((point, edge, visible))
if not polygon_data:
raise ValueError('No data read from file.')
polygon_data = list(
map(lambda tup: Point(float(tup[0]), float(tup[1])), polygon_data))
return Polygon(polygon_data), point_data
def generate_random_poly(self, num_verts, radius):
'''Generates a random polygon that does not collide with other
obstacles or the robot at start and goal. This polygon is added
to self.obstacles.
Args:
num_verts: int. the number of vertices of the polygon >= 3
radius: float. a reference distance between the origin and some
vertex of the polygon > 0
'''
(min_verts, max_verts) = num_verts
(min_radius, max_radius) = radius
assert not (min_verts < 3 or min_verts > max_verts or \
min_radius <= 0 or min_radius > max_radius)
reference = Point(random() * self.x, random() * self.y)
verts = randint(min_verts, max_verts)
points = [Point(0, 0)]
for i in range(verts):
angle = 2 * pi * random()
points.append(((max_radius - min_radius) * random() + min_radius) *
Point(cos(angle), sin(angle)))
obj = Object(reference, [Polygon(convex_hull(points))])
if any([obj.collides(current) for current in self.obstacles]) or \
obj.collides(self.robot.configuration(self.start)) or \
obj.collides(self.robot.configuration(self.goal)) or \
not self.region.contains(obj):
self.generate_random_poly(num_verts, radius)
else:
self.obstacles.append(obj)
def polygon(self):
e = self.polygon_element
if isinstance(e, svgelements.Rect):
svg_points = [
Point(e.x, e.y),
Point(e.x + e.width, e.y),
Point(e.x + e.width, e.y + e.height),
Point(e.x, e.y + e.height)
]
elif isinstance(e, svgelements.Circle):
svg_points = [Point(e.cx, e.cy)]
elif isinstance(self.polygon_element, svgelements.Path):
svg_points = list(self.polygon_element.as_points())
else:
svg_points = self.polygon_element.points
# Make sure we don't have any duplicate points
unique_points = []
previous_point = svg_points[-1]
for point in svg_points:
if point != previous_point:
unique_points.append(point)
previous_point = point
return Polygon(unique_points)
def generate_random_regular_poly(self,
num_verts,
radius,
angle=uniform(-pi, pi)):
'''Generates a regular polygon that does not collide with other
obstacles or the robot at start and goal. This polygon is added
to self.obstacles. To make it random, keep the default angle
argument.
Args:
num_verts: int. the number of vertices of the polygon >= 3
radius: float. the distance from the center of the polygon
to any vertex > 0
angle: float. the angle in radians between the origin and
the first vertex. the default is a random value between
-pi and pi.
'''
(min_verts, max_verts) = num_verts
(min_radius, max_radius) = radius
assert not (min_verts < 3 or min_verts > max_verts or \
min_radius <= 0 or min_radius > max_radius)
reference = Point(random() * self.x, random() * self.y)
distance = uniform(min_radius, max_radius)
sides = randint(min_verts, max_verts)
obj = Object(reference,
[Polygon([Point(distance*cos(angle + 2*n*pi/sides),
distance*sin(angle + 2*n*pi/sides)) \
for n in range(sides)])])
if any([obj.collides(current) for current in self.obstacles]) or \
obj.collides(self.robot.configuration(self.start)) or \
obj.collides(self.robot.configuration(self.goal)) or \
not self.region.contains(obj):
self.generate_random_regular_poly(num_verts, radius, angle=angle)
else:
self.obstacles.append(obj)
def polygon(self, attrs, end=False):
if end: return
self._form(Polygon(*self.drawing_attrs(attrs)))
points = self.polyline(attrs)
# close
x, y = eval("(" + points[0] + ")")
self._lin(x * unit, y * unit)
def create_link(length, width):
return Polygon([
Point(0, -width / 2.0),
Point(length, -width / 2.0),
Point(length, width / 2.0),
Point(0, width / 2.0)
])
def test_nonconvex_polygon_convexity(self):
vertices = make_square_vertices(side_length=2,
center=(0, 0)) + [[0, 0]]
nonconvex = Polygon(vertices)
expected = False
actual = nonconvex.convex
self.assertEqual(expected, actual)
def setUp(self):
points = [
Point(0.0, 0.0, 0.0),
Point(1.0, 0.0, 0.0),
Point(0.0, 1.0, 0.0)
]
self.polygon = Polygon(points, Point(0.25, 0.25, 0.0))
def circle(self, attrs, end=False):
if end: return
self._form(Polygon(*self.drawing_attrs(attrs)))
r = float(attrs["r"]) * unit
x = float(attrs["cx"]) * unit
y = float(attrs["cy"]) * unit
attrs["rx"] = attrs["ry"] = attrs["r"]
return self.ellipse(attrs)
def test_insert_convex_polygon_with_square_obstacle(self):
vertices = make_square_vertices(side_length=0.5, center=(1.25, 1.25))
square = Polygon(vertices)
self.occupancy_grid.insert_convex_polygon(square, 1.0)
expected = [0] * 15 + [1]
actual = self.occupancy_grid.occupancy.flatten(order='F')
np.testing.assert_array_equal(expected, actual)
def test_point_not_in_square_polygon(self):
# Arrange
square = Polygon(make_square_vertices(side_length=2, center=(0, 0)))
# Act
expected = False
actual = square.point_in_convex_polygon((3, 3))
# Assert
self.assertEqual(expected, actual)
def robot_arm_problem(methods):
print 'Robot Arm Problem'
robot = RobotArm(Point(10, 10), [ \
(Point(-.5, 0), create_link(4, 1)), \
(Point(3.5, 0), create_link(3, 1)), \
(Point(2.5, 0), create_link(2, 1))
])
start = (pi / 2, pi / 2, -pi / 4)
goal = (pi / 4, -pi / 6, -pi / 3)
cspace = ConfigurationSpace([ \
Range(-pi/2, pi/2), \
Range(-pi/2, pi/2), \
Range(-pi/2, pi/2)
], start, robot.distance, 3*[0.25])
"""
cspace = ConfigurationSpace([ \
Range(-pi, pi, wrap_around = True), \
Range(-pi, pi, wrap_around = True), \
Range(-pi, pi, wrap_around = True)
], start, robot.distance, 3*[0.25])
"""
obstacles = [
Object(
Point(12, 17.5),
[Polygon([Point(0, -2),
Point(2, 0),
Point(0, 2),
Point(-2, 0)])]),
Object(
Point(16, 16),
[Polygon([Point(0, -2),
Point(2, 0),
Point(0, 2),
Point(-2, 0)])])
]
problem = Problem(20, 20, robot, obstacles, start, goal, cspace)
for method in methods:
print 'Method', method
return problem.run_and_display(method, DISPLAY)
def load_world(world_filename=WORLD_FILENAME, width=WIDTH, length=LENGTH, height=HEIGHT):
mat = loadmat(__seville_2009__ + world_filename)
polygons = PolygonList()
for xs, ys, zs, col in zip(mat["X"], mat["Y"], mat["Z"], mat["colp"]):
col[0] = col[2] = 0
polygons.append(Polygon(xs, ys, zs, col))
observer = ephem.Observer()
observer.lat = '37.392509'
observer.lon = '-5.983877'
return World(observer=observer, polygons=polygons, width=width, length=length, height=height)
def set_value(self, field_name, value):
""" sets an attribute value for a given field name """
if field_name in self.fields:
if not value is None:
self._dict['attributes'][field_name] = _unicode_convert(value)
self._json = json.dumps(self._dict, default=_date_handler)
else:
pass
elif field_name.upper() in ['SHAPE', 'SHAPE@', "GEOMETRY"]:
if isinstance(value, AbstractGeometry):
if isinstance(value, Point):
self._dict['geometry'] = {
"x": value.asDictionary['x'],
"y": value.asDictionary['y']
}
elif isinstance(value, MultiPoint):
self._dict['geometry'] = {
"points": value.asDictionary['points']
}
elif isinstance(value, Polyline):
self._dict['geometry'] = {
"paths": value.asDictionary['paths']
}
elif isinstance(value, Polygon):
self._dict['geometry'] = {
"rings": value.asDictionary['rings']
}
else:
return False
self._json = json.dumps(self._dict, default=_date_handler)
elif isinstance(value, arcpy.Geometry):
if isinstance(value, arcpy.PointGeometry):
self.set_value(
field_name,
Point(value, value.spatialReference.factoryCode))
elif isinstance(value, arcpy.Multipoint):
self.set_value(
field_name,
MultiPoint(value, value.spatialReference.factoryCode))
elif isinstance(value, arcpy.Polyline):
self.set_value(
field_name,
Polyline(value, value.spatialReference.factoryCode))
elif isinstance(value, arcpy.Polygon):
self.set_value(
field_name,
Polygon(value, value.spatialReference.factoryCode))
else:
return False
return True
def test_insert_convex_polygon_with_circular_obstacle(self):
circle = Polygon(
make_circular_vertices(radius=1, center=(0, 0), num_pts=8))
self.occupancy_grid.insert_convex_polygon(circle, 1.0)
expected = [0] * 16
expected[5] = 1
expected[6] = 1
expected[9] = 1
expected[10] = 1
actual = self.occupancy_grid.occupancy.flatten(order='F')
np.testing.assert_array_equal(expected, actual)
def rect(self, attrs, end=False):
if end: return
#print attrs
self._form(Polygon(*self.drawing_attrs(attrs)))
x = self.curX = float(attrs["x"]) * unit
y = self.curY = float(attrs["y"]) * unit
w = float(attrs["width"]) * unit
h = float(attrs["height"]) * unit
self._mov(x, y)
self._hor(x + w)
self._ver(y + h)
self._hor(x)
self._ver(y)
pass
def start_1():
"""Generate a start configuration."""
polygon = Polygon([
Point(x, y) for x, y in ((0, 2), (10, 0), (3, 2), (9, 1), (8, 8),
(5, 3), (4.5, 5), (4, 6))
])
t = Point(4.25, 5.25)
t_trapezoid = polygon.trapezoid(t)
p = PolygonPoint(7.5, 4)
q1 = polygon.point(5)
q2 = hit_polygon_boundary(p, q1, polygon)
assert isinstance(q2, EdgePoint) and q2.index == 0
return polygon, q1, q2, t, t_trapezoid
def ellipse(self, attrs, end=False):
if end: return
self._form(Polygon(*self.drawing_attrs(attrs)))
rx = float(attrs["rx"]) * unit
ry = float(attrs["ry"]) * unit
x = float(attrs["cx"]) * unit
y = float(attrs["cy"]) * unit
steps = max(3, int((math.pi * ((rx + ry) * 0.5) * 2) / self.precision))
angstep = 2 * math.pi / steps
self._mov(x + rx, y)
for i in range(steps):
phi = i * angstep
self._lin(x + math.cos(phi) * rx, y + math.sin(phi) * ry)
self._lin(x + rx, y) # close form
def test_intersection():
corners = [
Point(0.0, 0.0, 0.0),
Point(1.0, 0.0, 0.0),
Point(1.0, 1.0, 0.0),
Point(0.0, 1.0, 0.0)
]
polygon = Polygon(corners, Point(0.5, 0.5, 0.0))
a = Point(0.5, 0.5, 1.0)
b = Point(0.5, 0.5, -1.0)
intersection = polygon.plane().intersection(a, b)
assert intersection == Point(0.5, 0.5, 0.0)
def test_intersection(self):
corners = [
Point(0.0, 0.0, 0.0),
Point(1.0, 0.0, 0.0),
Point(1.0, 1.0, 0.0),
Point(0.0, 1.0, 0.0)
]
polygon = Polygon(corners, Point(0.5, 0.5, 0.0))
a = Point(0.5, 0.5, 1.0)
b = Point(0.5, 0.5, -1.0)
intersection = polygon.plane().intersection(a, b)
self.assertEqual(intersection, Point(0.5, 0.5, 0.0))
def setup(self, width, height):
self.width = width
self.height = height
self.color = {
'BLACK': (0, 0, 0),
'WHITE': (255, 255, 255),
'BLUE': (0, 0, 255),
'GREEN': (0, 255, 0),
'RED': (255, 0, 0)
}
self.polygon = Polygon()
self.screen = pygame.display.set_mode([self.width, self.height])
self.exit = False
pygame.init()
pygame.display.set_caption("Convex hull")
def main():
p = RandomPolygon(8, ordered=False) # random quadrilateral
p = Polygon([(-1, 1), (1, 1), (0, 1), (0, -1), (1, -1)])
print "POINTS:"
print p.points
print
# p = Polygon([(0, 0), (1, 0), (1, 3), (0, 1), (0, 1.001)])
plot_poly(p, size=2)
Xs, Ys, p = sim_poly(p, T=1000, random=False, i0=0)
plt.figure(figsize=(5, 5))
plt.scatter(Xs, Ys)
x, y = p.scatter()
plt.scatter(x, y, color='red')
# find_radii(p, iters=5000, plot=True)
# track_point(p, 0)
plt.show()
def setUp(self):
self.config = Mock()
self.config.alliance = "Blue"
self.config.blue_goal_region = Polygon(
make_square_vertices(side_length=0.5, center=(-2.5, -3.5)))
self.config.blue_chute_pos = np.array([10, 10])
self.config.occupancy_grid_num_cols = 6
self.config.occupancy_grid_num_rows = 6
self.config.occupancy_grid_cell_resolution = 1
self.config.occupancy_grid_origin = (0, 0)
self.config.occupancy_grid_dilation_kernel_size = 3
self.config.ball_probability_decay_factor = 0
self.config.ball_probability_growth_factor = 1
self.config.ball_probability_threshold = 1
self.config.obstacle_probability_decay_factor = 0
self.config.obstacle_probability_growth_factor = 1
self.config.obstacle_probability_threshold = 1
self.config.lidar_deadzone_radius = 0.85
self.planning = Planning(self.config)